专利名称：自动引导车辆无线定位、导航与控制系统的制作方法
技术领域：
本发明涉及一种自动引导车辆无线定位、导航与控制系统。
背景技术：
所谓自动引导车辆AGV(Automatic Guide Vehicle)，是通过各种传感器的融合提取车辆的位置，同时自动控制执行机构实现AGV的自动导航；所谓自动引导车辆AGV无线定位是通过无线的方式确定自动引导车辆AGV在运行范围内的绝对坐标；所谓自动引导车辆AGV导航是自动引导车辆对自动引导车辆AGV所要行进的路径进行规划，并根据路径跟踪算法保证自动引导车辆AGV沿着事先定义好的路径前进，当自动引导车辆AGV与预定路径有了偏差能够根据路径跟踪算法自动调整；所谓自动引导车辆AGV控制是根据路径跟踪算法得到的控制参数对自动引导车辆AGV运行速度，方向进行控制。
通过阅读相关资料和有关专利，主要引证以下专利国名 专利号
公开日期1、 美国 4,593,239 1986.7.32、 美国 4,700,427 1987.10.203、 美国 6,128,574 2000.10.34、 美国 6,690,134 2004.2.105、 美国 6,711,501 2004.3.236、 美国 6,721,638 2004.4.137、 美国 6,772,525 2004.8.108、 美国 6,781,338 2004.8.24公开号9、 中国 CN1438138A 2003.8.2710、 中国 CN1450338A 2003.10.22
以上所述专利都是目前自动引导车辆AGV方面宝贵的科研成果，与本发明有一定的相关性，都是体现自动引导车辆“柔性化”的特点。专利1、2、6代表国外AGV发展水平，3、4、5、7、8与之有一定的相关性，专利9、10体现国内AGV发展现状。具体分类如下定位方式方面1、专利1定位系统采用光学系统，增加硬件复杂度，抗干扰能力弱。2、专利9中提出视觉定位方法，是通过设置在自动引导车上的CCD摄像机摄取在敷设地面的运行路径标线、工位地址编码标记符和加速、减速、停车、倒车的控制标记符。与摄像机相连的计算机根据摄取的图像信息，通过图像识别算法对车辆进行智能引导。其突出缺点是成本太高，图像处理过程复杂，处理速度实时性差。3、同样，专利10采用卫星定位，定位精度不高。
路径规划方面专利1和专利2路径的定义是在控制区域内设定几个点或者已知的存储起来的几条简单曲线，车在各个点之间行走或沿着简单曲线行走。
路径跟踪方面专利6中提出的路径跟踪实现方法是基于复杂的车载控制执行系统实现的。
无线通信方面专利10中所提及的通信�？槭褂玫墓ぷ髌刀挝狦SM移动通信用的900MHz/1800MHz。
以上就是当前自动引导车辆的现状以及面临的问题。
发明内容
本发明提供一种自动引导车辆无线定位、导航与控制系统，以解决上述自动引导车辆所面临的问题。本发明采取的技术方案是结构由两部分组成车载系统[5]、导航控制系统[6]，该车载系统[5]包括车载控制执行�？閇1]、无线定位�？�1[2.1]、蓝牙无线通信�？�1[3.1]，导航控制系统[6]包括无线定位�？�2[2.2]、蓝牙无线通信�？�2[3.2]、PC机[4]，电源管理�？閇1.1]作为一个相对独立的部分，分别为蓝牙无线通信�？�1[3.1]、无线定位�？�1[2.1]，车载控制执行�？閇1]提供合适的电源电压，其中
车载控制执行�？閇1]属于车载系统[5]，用于根据控制数据来完成对车的协调控制，使车完成前进、后退、加速、减速、停车、左转、右转等动作，它主要包括控制车前进、后退的主电机电路[1.2]、控制车转向的舵机电路[1.3]及微处理器1[1.4]，其中主电机电路[1.2]包括电子调速器[1.5]、主电机[1.6]；舵机电路[1.3]包括伺服控制电路[1.7]、H桥电机驱动电路[1.8]、舵机[1.9]，微处理器1[1.4]连接伺服控制电路[1.7]，该伺服控制电路[1.7]连接H桥电机驱动电路[1.8]，该H桥电机驱动电路[1.8]连接舵机[1.9]，微处理器1[1.4]还连接电子调速器[1.5]，该电子调速器[1.5]连接主电机[1.6]；车载控制执行�？閇1]电路中微处理器使用的是Mirochip公司生产的Pic16f873单片机，该芯片能够输出PWM脉冲。
控制车速的驱动信号及控制转向的驱动信号由微处理器1[1.4]输出，微处理器1[1.4]根据蓝牙无线通信模块1[3.1]接收到的PC机的控制命令，改变输出脉冲的周期及占空比，输出控制运动速度的PWM脉冲给主电机电路[1.2]及控制转向的PWM脉冲给舵机电路[1.3]。
无线定位�？�1[2.1]、2[2.2]采用基于超声传感器的主动信标定位方法，以红外发射时间作为基准，用于提取车上发射超声信号到接收端接收到超声信号的时延信息，对车进行精度达毫米级的精确定位。
无线定位�？�1[2.1]属于车载系统[5]，无线定位�？�2[2.2]属于导航控制系统[6]。
无线定位�？�1[2.1]的电路主要有红外接收电路[2.3]、微处理器2[2.4]、车头超声发射电路[2.5]、车尾超声发射电路[2.6]，红外接收电路[2.3]接收红外信号作为微处理器2[2.4]发射超声驱动信号的时间基准，红外接收电路[2.3]与微处理器2[2.4]连接，该微处理器2[2.4]分别连接车头超声发射电路[2.5]和车尾超声发射电路[2.6]。
红外接收电路[2.3]接收到的红外信号作为微处理器2[2.4]发射超声驱动信号的时间基准。微处理器2[2.4]在接收到红外信号后立即输出车头超声发射电路[2.5]所需要的驱动信号，25ms后发射车尾超声发射电路[2.6]所需要的驱动信号。车头超声发射电路[2.5]、车尾超声发射电路[2.6]接收到微处理器2[2.4]的驱动信号后发射超声信号。使用的是红外接收�？椋苯雍玫缭春偷厥毙藕哦丝梢约觳夂焱庑藕诺挠形薏⒉嘤Φ穆龀�。通过变压器升压驱动超声发射传感器，超声发射器选用的是频率为40kHz的全向超声发射器，型号为US40KT-01，是由美国精量公司生产的，超声发射电路中的微处理器使用的是Mirochip公司生产的Pic12c508，单片机，该芯片具有体积�。δ芮康挠诺�。
无线定位�？�2[2.2]上的电路主要有红外信号发射电路[2.7]、超声信号放大电路[2.8]、电压比较电路[2.9][2.10]、时延提取电路[2.11]、微处理器3[2.12]、温度测量电路[2.13]，微处理器3[2.12]分别连接红外信号发射电路[2.7]，时延提取电路[2.11]、温度测量电路[2.13]、超声信号放大电路[2.8]，其中时延提取电路[2.11]分别连接电压比较电路[2.9]、[2.10]。
超声信号放大电路[2.8]主要包括低噪声前置放大电路[2.14][2.15]、第二级放大电路[2.16][2.17]、自动增益放大电路[2.18][2.19]、第四级放大电路[2.20][2.21]。前置放大电路[2.14]、[2.15]分别连接对超声信号进一步放大的第二级放大电路[2.16]、[2.17]，该第二级放大电路[2.16]、[2.17]分别连接自动增益放大电路[2.18][2.19]，该自动增益放大电路[2.18]、[2.19]分别连接第四级放大电路[2.20]、[2.21]。
前置放大电路[2.14][2.15]保证超声信号被放大的同时抑制噪声的影响；第二级放大电路[2.16][2.17]对超声信号进一步放大；自动增益放大电路[2.18][2.19]保证超声信号的幅度不会因为AGV运行的位置改变而有较大的波动。由于自动增益放大电路[2.18][2.19]的输出信号还未饱和，加入第四级放大电路[2.20][2.21]保证超声信号的第一个沿被饱和放大，以提高定位的精度。电压比较电路[2.9][2.10]与时延提取电路[2.11]处理放大后的超声信号，将处理过的信号传给微处理器3[2.12]。超声接收器是选用的是频率为40kHz的波束角为55度的超声接收器，型号为40LR16，是由Polaroid公司生产的。自动增益放大电路[2.18][2.19]使用Dallas公司生产的DS1868双路数字电位芯片，通过改变电位计阻值改变放大电路放大倍数。
由于超声波在介质中传播速度c的值会随着温度的变化而变化，在空气中c与温度的关系为c=331.41+t/273m/s]]>(t为摄氏温度)，本发明加入温度测量电路[2.13]，实时测量环境温度，对超声传播速度进行补偿，保证从发射传感器到接收传感器距离的精确测量。
微处理器3[2.12]通过自身AD转换器接收温度测量电路[2.13]送来的与温度有关的模拟信号，将其转化为数字量，每50ms发射驱动红外信号的脉冲，并提取车头、车尾发射过来的超声信号的时延信息。微处理器3[2.12]控制自动增益放大电路[2.18][2.19]，调整超声信号的幅度，最后，将时延、温度等信息通过通用异步串口传给PC机。上述工作过程循环重复。微处理器3使用的是Mirochip公司生产的Pic16f873单片机，通过它的CCP�？槔刺崛〕藕糯ナ奔�。
蓝牙无线通信�？�1[3.1]及蓝牙无线通信模块2[3.2]。
蓝牙无线通信�？�1[3.1]属于车载系统[5]用于无线接收，蓝牙无线通信模块2[3.2]属于导航控制系统[6]用于无线发射。蓝牙无线通信�？榈淖饔檬墙玃C机计算得的车辆控制参数传给车载控制执行�？閇1]。
蓝牙无线通信�？�1[3.1]与蓝牙无线通信�？�2[3.2]中的主要元件是蓝牙无线通信�？閇3.3]。蓝牙无线通信�？槭且恢值テ堆朗�、语音收发、组网蓝牙无线通信�？椋芍苯佑氲テ拇诹樱灰部赏ü缙阶坏缏酚爰扑慊诮型ㄑ�。
PC机[4]的作用是在Visual C++编程环境下实现通信与算法，包括通信[4.1]、位置显示[4.2]、路径规划[4.3]及路径跟踪[4.3]部分，其中通信[4.1]基于通用异步串口通信方式接收定位�？榈奈露刃畔⒑褪毖有畔⒉⒋透尴咄ㄐ欧⑸淠？榭刂菩畔ⅲ晃恢孟允綶4.2]部分根据当前温度计算超声传播速度，根据四个时延信息算得车头车尾到达两个信标的距离，由定位原理得到车头车尾坐标，并在屏幕上用数字、图形显示出来。定位原理如下位于固定位置的两路超声接收器接收来自于车头S1、车尾S2的超声发射信号，通过时延提取电路得到车头、车尾发射的超声到达R1、R2的传播时间，分别为τS1R1、τS1R2、τS2R1、τS2R2。根据串口传送来的温度信息，由公式1可以计算得到超声当前的传播速度c。由公式(1-4)可以求出S1R1、S1R2、S2R1、S2R2四条线段的长度S1R1=τS1R1×c---(1)]]>S1R2=τS1R2×c---(2)]]>S2R1=τS2R1×c---(3)]]>S2R2=τS2R2×c---(4)]]>又根据平面几何原理有
S1R1=(x1-xa)2+(y1-ya)2S1R2=(x1-xb)2+(y1-ya)2----(5)]]>S2R1=(x2-xa)2+(y2-ya)2S2R2=(x2-xb)2+(y2-ya)2----(6)]]>由(1)、(2)、(5)可以得到S1点坐标(x1，y1)x1=(xb2-xa2)-(cτS1R2)2+(cτS1R1)22(xb-xa)y1=ya+(cτS1R1)2-(x1-xa)2----(7)]]>由(3)、(4)、(6)可以得到S2点坐标(x2，y2)x2=(xb2-xa2)-(cτS2R2)2+(cτS2R1)22(xb-xa)y2=ya+(cτS2R1)2-(x2-xa)2----(8)]]>从而得到车头车尾、坐标。
路径规划[4.3]是要根据AGV在所要运行地图中的当前位置，及所要到达的终点位置确定AGV的运行路径。本发明设计了三种路径输入方式鼠标输入路径方式[4.5]、CAD输入路径方式[4.6]、手写输入系统路径输入方式[4.7]。手写输入系统路径输入方式[4.7]是操作者用手写笔在一块与车实际运行区域相互映射的输入板上根据实际需要输入行车路径(当然其中也有一些规则)，主控系统会自动对路径识别并存储在系统中作为自定义路径。
路径跟踪[4.4]是要保证AGV沿着事先定义好的路径前进，当AGV与预定路径有了偏差能够通过根据路径算法得到的控制信息令车重新返回路径。本发明有两种路径跟踪方式手动控制方式及自动控制方式。手动控制方式用户可以根据车在工作环境中的位置及环境信息手动控制车的速度转向；路径跟踪方式本发明采用几何原理与控制理论联合算法。算法涉及的参数比较多，主要有车的当前姿态(车头、车尾位置)，运行速度、加速度、舵方向、车的给定运动轨迹的曲率及方向，地面质量(摩擦力不同)。
本项发明基于超声传感器的主动信标定位理论对AGV进行高精度定位，通过多种自定义路径的路径规划方式，鲜明地体现了自动引导车辆“柔性化”的特点，本项发明对今后自动引导车辆将产生很大的积极影响，有关本发明的突出优点可概括如下1本发明实现了AGV无线定位、导航与控制，在成本上，使用了最为经济、高效、稳定的方案及设备。而如上所述的其他类型的自动引导车辆的装置，不是定位方法昂贵(如图像定位专利9所述)，就是控制装置昂贵(如专利6所述)。
2本发明的定位精度高，利用红外信号传播的高速特性精确确定超声发射时间；使用了低噪声前置放大电路，保证超声信号被放大的同时抑制噪声的影响；使用自动增益放大电路，保证超声信号的幅度不会因为AGV运行的位置改变而有较大的波动。
3在路径规划的实现上，以上所述的代表性专利路径很难实现自由化，要么是所控制区域给出几个点，车在各个点之间运动，并不能保证每次在两点之间行走都能沿同一路径，要么存储固定的几条曲线，这样的智能引导车辆无法实现对车的自由引导。而本发明AGV的路径规划采用多种方式，可以根据环境不同任意修改，更加自由、灵活。
4对车的路径跟踪控制采用了几何跟踪方式与控制理论相结合的方式，不用对车载控制执行系统作太大的改动就能保证路径跟踪有很高的精度，提高了对车控制的灵敏度。
5通信方式采用基于蓝牙技术的无线通信方式，使用的工作频段为2.4GHz。使用的频段为工业、科学和医学频段，在全球范围适用，使用跳频方式来扩展频谱具有很好的抗干扰能力、同时还具有低功耗、接口标准，能组网、成本低等优点。
总之，本发明能以最少的投入获得最大的效益，也就是性价比越高越好，本项发明恰恰以最为经济的投入，获得对AGV最好的控制，最精确的定位，以及最为灵活的路径输入方式。实现了自定义路径下基于超声传感器定位的AGV系统，经济、准确、灵活是本发明的优势所在。
图1为AGV的无线定位、导航与控制系统框2为车载系统框3为导航控制系统框4车载控制执行模块电路原理5为微处理器1的流程框6为微处理器2的流程框7为红外接收电路原理8为超声发射电路原理9为红外信号发射电路原理10为超声信号的一、二级放大电路原理11为超声信号的三、四级放大电路原理12为微处理器3的电路原理13为微处理器3的流程框14为蓝牙无线通信�？槎ナ�15蓝牙无线通信�？�1电路原理16蓝牙无线通信�？�2电路原理图。
图17为PC机上程序流程框18为定位原理示意图具体实施方式
如图1，结构由两部分组成车载系统[5]、导航控制系统[6]，该车载系统[5]包括车载控制执行模块[1]、无线定位�？�1[2.1]、蓝牙无线通信模块1[3.1]，导航控制系统[6]包括无线定位�？�2[2.2]、蓝牙无线通信�？�2[3.2]、PC机[4]，电源管理�？閇1.1]作为一个相对独立的部分，分别为蓝牙无线通信模块1[3.1]、无线定位�？�1[2.1]，车载控制执行�？閇1]提供合适的电源电压，其中如图2，车载控制执行�？閇1]属于车载系统[5]，用于根据控制数据来完成对车的协调控制，使车完成前进、后退、加速、减速、停车、左转、右转等动作，它主要包括控制车前进、后退的主电机电路[1.2]、控制车转向的舵机电路[1.3]及微处理器1[1.4]，其中主电机电路[1.2]包括电子调速器[1.5]、主电机[1.6]；舵机电路[1.3]包括伺服控制电路[1.7]、H桥电机驱动电路[1.8]、舵机[1.9]，微处理器1[1.4]连接伺服控制电路[1.7]，该伺服控制电路[1.7]连接H桥电机驱动电路[1.8]，该H桥电机驱动电路[1.8]连接舵机[1.9]，微处理器1[1.4]还连接电子调速器[1.5]，该电子调速器[1.5]连接主电机[1.6]；该车载控制执行�？閇1]电路原理如图4，车载控制执行�？閇1]电路中微处理器使用的是Mirochip公司生产的Pic16f873单片机，该芯片能够输出PWM脉冲。
控制车速的驱动信号及控制转向的驱动信号由微处理器1[1.3]输出，微处理器1[1.3]根据蓝牙无线通信�？�2[1.2]接收到的PC机的控制命令，改变输出脉冲的周期及占空比，输出控制运动速度的PWM脉冲给主电机电路[1.1]及控制转向的PWM脉冲给舵机电路[1.2]。微处理器1流程图如图5。
无线定位模块1[2.1]、2[2.2]采用基于超声传感器的主动信标定位方法，以红外发射时间作为基准，用于提取车上发射超声信号到接收端接收到超声信号的时延信息，对车进行精度达毫米级的精确定位。
无线定位模块1[2.1]属于车载系统[5]，无线定位�？�2[2.2]属于导航控制系统[6]。
无线定位�？�1[2.1]的电路主要有红外接收电路[2.3]、微处理器2[2.4]、车头超声发射电路[2.5]、车尾超声发射电路[2.6]，红外接收电路[2.3]接收红外信号作为微处理器2[2.4]发射超声驱动信号的时间基准，红外接收电路[2.3]与微处理器2[2.4]连接，该微处理器2[2.4]分别连接车头超声发射电路[2.5]和车尾超声发射电路[2.6]。
红外接收电路[2.3]接收到的红外信号作为微处理器2[2.4]发射超声驱动信号的时间基准。微处理器2[2.4]在接收到红外信号后立即输出车头超声发射电路[2.5]所需要的驱动信号，25ms后发射车尾超声发射电路[2.6]所需要的驱动信号。微处理器2[2.4]的流程框图如图6所示。车头超声发射电路[2.5]、车尾超声发射电路[2.6]接收到微处理器2[2.4]的驱动信号后发射超声信号。红外接收电路原理图如图7，使用的是红外接收�？椋苯雍玫缭春偷厥毙藕哦丝梢约觳夂焱庑藕诺挠形薏⒉嘤Φ穆龀濉Ｍü溲蛊魃骨⑸浯衅鳎⑸淦餮∮玫氖瞧德饰�40kHz的全向超声发射器，型号为US40KT-01，是由美国精量公司生产的，超声发射电路中的微处理器使用的是Mirochip公司生产的Pic12c508,单片机，该芯片具有体积�。δ芮康挠诺恪�
无线定位�？�2[2.2]上的电路主要有红外信号发射电路[2.7]、超声信号放大电路[2.8]、电压比较电路[2.9][2.10]、时延提取电路[2.11]、微处理器3[2.12]、温度测量电路[2.13]，微处理器3[2.12]分别连接红外信号发射电路[2.7]，时延提取电路[2.11]、温度测量电路[2.13]、超声信号放大电路[2.8]，其中时延提取电路[2.11]分别连接电压比较电路[2.9]、[2.10]。
超声信号放大电路[2.8]主要包括低噪声前置放大电路[2.14][2.15]、第二级放大电路[2.16][2.17]、自动增益放大电路[2.18][2.19]、第四级放大电路[2.20][2.21]。前置放大电路[2.14]、[2.15]分别连接对超声信号进一步放大的第二级放大电路[2.16]、[2.17]，该第二级放大电路[2.16]、[2.17]分别连接自动增益放大电路[2.18][2.19]，该自动增益放大电路[2.18]、[2.19]分别连接第四级放大电路[2.20]、[2.21]。
前置放大电路[2.14][2.15]保证超声信号被放大的同时抑制噪声的影响；第二级放大电路[2.16][2.17]对超声信号进一步放大；自动增益放大电路[2.18][2.19]保证超声信号的幅度不会因为AGV运行的位置改变而有较大的波动。由于自动增益放大电路[2.18][2.19]的输出信号还未饱和，加入第四级放大电路[2.20][2.21]保证超声信号的第一个沿被饱和放大，以提高定位的精度。电压比较电路[2.9][2.10]与时延提取电路[2.11]处理放大后的超声信号，将处理过的信号传给微处理器3[2.12]。红外信号发射电路原理图如图9；超声信号的低噪声前置放大电路[2.14][2.15]、第二级放大电路[2.16][2.17]原理图如图10。超声接收器是选用的是频率为40kHz的波束角为55度的超声接收器，型号为40LR16，是由Polaroid公司生产的。超声信号的自动增益放大电路[2.18][2.19]、第四级放大电路[2.20][2.21]原理图如图11。其中自动增益放大电路[2.18][2.19]使用Dallas公司生产的DS1868双路数字电位芯片，通过改变电位计阻值改变放大电路放大倍数。
由于超声波在介质中传播速度c的值会随着温度的变化而变化，在空气中c与温度的关系为c=331.41+t/273m/s]]>(t为摄氏温度)，本发明加入温度测量电路[2.13]，实时测量环境温度，对超声传播速度进行补偿，保证从发射传感器到接收传感器距离的精确测量。
微处理器3[2.12]通过自身AD转换器接收温度测量电路[2.13]送来的与温度有关的模拟信号，将其转化为数字量，每50ms发射驱动红外信号的脉冲，并提取车头、车尾发射过来的超声信号的时延信息。微处理器3[2.12]控制自动增益放大电路[2.18][2.19]，调整超声信号的幅度，最后，将时延、温度等信息通过通用异步串口传给PC机。上述工作过程循环重复。微处理器3[2.12]的电路原理图如图12，微处理器3使用的是Mirochip公司生产的Pic16f873单片机，通过它的CCP模块来提取超声信号传播时间。微处理器3[2.12]流程图如图13。
蓝牙无线通信�？�1[3.1]及蓝牙无线通信�？�2[3.2]蓝牙无线通信�？�1[3.1]属于车载系统[5]用于无线接收，蓝牙无线通信�？�2[3.2]属于导航控制系统[6]用于无线发射。蓝牙无线通信�？榈淖饔檬墙玃C机计算得的车辆控制参数传给车载控制执行模块[1]。
蓝牙无线通信�？�1[3.1]与蓝牙无线通信�？�2[3.2]中的主要元件是蓝牙无线通信模块[3.3]。蓝牙无线通信�？槎ナ油既缤�14。蓝牙无线通信�？槭且恢值テ堆朗�、语音收发、组网蓝牙无线通信�？椋芍苯佑氲テ拇诹樱灰部赏ü缙阶坏缏酚爰扑慊诮型ㄑ�。蓝牙无线通信�？�1[3.1]电路原理图如图15，蓝牙无线通信�？�2[3.2]电路原理图如图16。
PC机[4]PC机[4]的作用是在Visual C++编程环境下实现通信与算法。包括通信[4.1]、位置显示[4.2]、路径规划[4.3]及路径跟踪[4.3]几个部分，图17为PC机上程序流程框图，其中通信[4.1]部分基于通用异步串口通信方式接收定位�？榈奈露刃畔⒑褪毖有畔⒉⒋透尴咄ㄐ欧⑸淠？榭刂菩畔�。
位置显示[4.2]部分根据当前温度计算超声传播速度，根据四个时延信息算得车头车尾到达两个信标的距离，由定位原理得到车头车尾坐标，并在屏幕上用数字、图形显示出来。定位原理示意图如图18，定位原理如下位于固定位置的两路超声接收器接收来自于车头S1、车尾S2的超声发射信号，通过时延提取电路得到车头、车尾发射的超声到达R1、R2的传播时间，分别为τS1R1、τS1R2、τS2R1、τS2R2。根据串口传送来的温度信息，由公式1可以计算得到超声当前的传播速度c。由公式(1-4)可以求出S1R1、S1R2、S2R1、S2R2四条线段的长度S1R1=τS1R1×c---(1)]]>S1R2=τS1R2×c---(2)]]>S2R1=τS2R1×c---(3)]]>S2R2=τS2R2×c---(4)]]>
又根据平面几何原理有S1R1=(x1-xa)2+(y1-ya)2S1R2=(x1-xb)2+(y1-ya)2----(5)]]>S2R1=(x2-xa)2+(y2-ya)2S2R2=(x2-xb)2+(y2-ya)2----(6)]]>由(1)、(2)、(5)可以得到S1点坐标(x1，y1)x1=(xb2-xa2)-(cτS1R2)2+(cτS1R1)22(xb-xa)y1=ya+(cτS1R1)2-(x1-xa)2----(7)]]>由(3)、(4)、(6)可以得到S2点坐标(x2，y2)x2=(xb2-xa2)-(cτS2R2)2+(cτS2R1)22(xb-xa)y2=ya+(cτS2R1)2-(x2-xa)2----(8)]]>从而得到车头车尾、坐标。
路径规划[4.3]是要根据AGV在所要运行地图中的当前位置，及所要到达的终点位置确定AGV的运行路径。本发明设计了三种路径输入方式鼠标输入路径方式[4.5]、CAD输入路径方式[4.6]、手写输入系统路径输入方式[4.7]。手写输入系统路径输入方式[4.7]是操作者用手写笔在一块与车实际运行区域相互映射的输入板上根据实际需要输入行车路径(当然其中也有一些规则)，主控系统会自动对路径识别并存储在系统中作为自定义路径。
路径跟踪[4.4]是要保证AGV沿着事先定义好的路径前进，当AGV与预定路径有了偏差能够通过根据路径算法得到的控制信息令车重新返回路径。本发明有两种路径跟踪方式手动控制方式及自动控制方式。手动控制方式用户可以根据车在工作环境中的位置及环境信息手动控制车的速度转向；路径跟踪方式本发明采用几何原理与控制理论联合算法。算法涉及的参数比较多，主要有车的当前姿态(车头、车尾位置)，运行速度、加速度、舵方向、车的给定运动轨迹的曲率及方向，地面质量(摩擦力不同)。
权利要求
1.一种自动引导车辆无线定位、导航与控制系统，其特征在于结构由两部分组成车载系统[5]、导航控制系统[6]，该车载系统[5]包括车载控制执行�？閇1]、无线定位�？�1[2.1]、蓝牙无线通信�？�1[3.1]，导航控制系统[6]包括无线定位�？�2[2.2]、蓝牙无线通信�？�2[3.2]、PC机[4]，PC机[4]分别与蓝牙无线通信�？�2[3.2]、无线定位�？�2[2.2]连接，蓝牙无线通信�？�1[3.1]与蓝牙无线通信�？�2[3.2]无线连接，蓝牙无线通信�？�1[3.1]与车载控制执行�？閇1]连接，车载控制执行�？閇1]与无线定位�？�1[2.1]彼此独立无连接，无线定位�？�1[2.1]通过超声与无线定位�？�2[2.2]无线连接，电源管理�？閇1.1]作为一个相对独立的部分，分别为蓝牙无线通信模块1[3.1]、无线定位�？�1[2.1]，车载控制执行�？閇1]提供合适的电源电压。
2.根据权利要求1所述的自动引导车辆无线定位、导航与控制系统，其特征在于控制AGV的前进速度与方向的车载控制执行�？閇1]属于车载系统[5]，用于根据控制数据来完成对车的协调控制，使车完成前进、后退、加速、减速、停车、左转、右转等动作，它主要包括控制车前进、后退的主电机电路[1.2]、控制车转向的舵机电路[1.3]及微处理器1[1.4]，其中主电机电路[1.2]包括电子调速器[1.5]、主电机[1.6]；舵机电路[1.3]包括伺服控制电路[1.7]、H桥电机驱动电路[1.8]、舵机[1.9]，微处理器1[1.4]连接伺服控制电路[1.7]，该伺服控制电路[1.7]连接H桥电机驱动电路[1.8]，该H桥电机驱动电路[1.8]连接舵机[1.9]，微处理器1[1.4]还连接电子调速器[1.5]，该电子调速器[1.5]连接主电机[1.6]。
3.根据权利要求2所述的自动引导车辆无线定位、导航与控制系统，其特征在于车载控制执行�？閇1]电路中微处理器使用的是Mirochip公司生产的Pic16f873单片机，该芯片能够输出PWM脉冲。
4.根据权利要求1所述的自动引导车辆无线定位、导航与控制系统，其特征在于无线定位�？�1[2.1]采用基于超声传感器的主动信标定位方法，以红外发射时间作为基准，用于提取车上发射超声信号到接收端接收到超声信号的时延信息，对车进行精度达毫米级的精确定位，其电路主要有红外接收电路[2.3]、微处理器2[2.4]、车头超声发射电路[2.5]、车尾超声发射电路[2.6]，红外接收电路[2.3]接收红外信号作为微处理器2[2.4]发射超声驱动信号的时间基准，红外接收电路[2.3]与微处理器2[2.4]连接，该微处理器2[2.4]分别连接车头超声发射电路[2.5]和车尾超声发射电路[2.6]。
5.根据权利要求4所述的自动引导车辆无线定位、导航与控制系统，其特征在于车头超声发射电路[2.5]和车尾超声发射电路[2.6]中的微处理器使用的是Mirochip公司生产的Pic12c508，单片机，通过变压器升压驱动超声发射传感器1和2，超声发射器选用的是频率为40kHz的全向超声发射器，型号为US40KT-01，是由美国精量公司生产的。
6.根据权利要求1所述的自动引导车辆无线定位、导航与控制系统，其特征在于无线定位�？�2[2.2]上的电路主要有红外信号发射电路[2.7]、超声信号放大电路[2.8]、电压比较电路[2.9]、[2.10]、时延提取电路[2.11]、微处理器3[2.12]、温度测量电路[2.13]，微处理器3[2.12]分别连接红外信号发射电路[2.7]，时延提取电路[2.11]、温度测量电路[2.13]、超声信号放大电路[2.8]，其中时延提取电路[2.11]分别连接电压比较电路[2.9]、[2.10]。
7.根据权利要求6所述的自动引导车辆无线定位、导航与控制系统，其特征在于超声信号放大电路[2.8]主要包括低噪声前置放大电路[2.14][2.15]、第二级放大电路[2.16][2.17]、自动增益放大电路[2.18][2.19]、第四级放大电路[2.20][2.21]。前置放大电路[2.14]、[2.15]分别连接对超声信号进一步放大的第二级放大电路[2.16]、[2.17]，该第二级放大电路[2.16]、[2.17]分别连接自动增益放大电路[2.18][2.19]，该自动增益放大电路[2.18]、[2.19]分别连接第四级放大电路[2.20]、[2.21]。
8.根据权利要求1所述的自动引导车辆无线定位、导航与控制系统，其特征在于蓝牙无线通信模块1[3.1]属于车载系统[5]，用于无线接收，蓝牙无线通信�？�2[3.2]属于导航控制系统[6]，用于无线发射，将PC机[4]计算得的车辆控制参数传给车载控制执行�？閇1]。
9.根据权利要求1所述的自动引导车辆无线定位、导航与控制系统，其特征在于PC机[4]的作用是在Visual C++编程环境下实现通信与算法，包括通信[4.1]、位置显示[4.2]、路径规划[4.3]及路径跟踪[4.3]部分，其中通信[4.1]基于通用异步串口通信方式接收定位�？榈奈露刃畔⒑褪毖有畔⒉⒋透尴咄ㄐ欧⑸淠？榭刂菩畔ⅲ晃恢孟允綶4.2]是根据当前温度计算超声传播速度，根据四个时延信息算得车头车尾到达两个信标的距离，由定位原理得到车头车尾坐标，并在屏幕上用数字、图形显示出来；路径规划[4.3]根据AGV在所要运行地图中的当前位置，及所要到达的终点位置确定AGV的运行路径；路径跟踪[4.4]是保证AGV沿着事先定义好的路径前进，当AGV与预定路径有了偏差能够通过根据路径算法得到的控制信息令车重新返回路径。
全文摘要
本发明涉及一种自动引导车辆无线定位、导航与控制系统，结构由两部分组成车载系统、导航控制系统，该车载系统包括车载控制执行�？椤⑽尴叨ㄎ荒？�1、蓝牙无线通信�？�1，导航控制系统包括无线定位�？�2、蓝牙无线通信�？�2、PC机，电源管理�？樽魑桓鱿喽远懒⒌牟糠郑直鹞堆牢尴咄ㄐ拍？�1、无线定位�？�1，车载控制执行模块提供合适的电源电压。本发明能以最少的投入获得最大的效益，也就是性价比越高越好，本发明优点是以最经济的投入，获得对AGV最好的控制，最精确的定位，以及最为灵活的路径输入方式。实现了自定义路径下基于超声传感器定位的AGV系统。
文档编号G01C21/26GK1651863SQ200410011248
公开日2005年8月10日 申请日期2004年11月24日 优先权日2004年11月24日
发明者王树勋, 燕学智, 孙晓颖, 郭纲, 胡封晔, 魏晓莉, 马中胜, 李辛, 黄志强, 尹万宇 申请人:吉林大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
