技术领域technical field
本发明属控制技术领域,涉及一种室内地面运输车超声波定位控制系统及其控制方法。The invention belongs to the technical field of control, and relates to an ultrasonic positioning control system and a control method for an indoor ground transport vehicle.
背景技术Background technique
现有室内超声波定位技术中超声波接收装置为固定装置，若干超声波接收探头固定于建筑物内部天花板，而现有的改进多为添加多个超声波接收探头或改变定位计算规则，而这些改进并不能解决超声波接收时造成的丢波现象，从而无法彻底解决由丢波造成的误差，即由于超声波发射探头到每个超声波接收探头的距离不相等，而造成超声波接收探头接收到的超声波不同步。在室内超声波定位技术中，超声波接收探头丢掉若干个波，这是引起超声波定位误差的根本原因。并且，超声波接收装置固定在天花板上会造成在超声波发射探头发生移动时，超声波接收探头存在一定的盲区，由超声波特性可知，超声波发射/接收探头在0角度的时候，发射/接收效果最好，随着角度的增大，信号强度迅速降低。一般取30°为发射/接收区域。因此，固定在天花板上的超声波接收探头都存在一定的盲区，同时多个超声波接收探头盲区又相互叠加，这造成现有超声定位系统定位盲区较大。In the existing indoor ultrasonic positioning technology, the ultrasonic receiving device is a fixed device, and several ultrasonic receiving probes are fixed on the ceiling inside the building. However, most of the existing improvements are adding multiple ultrasonic receiving probes or changing the positioning calculation rules, and these improvements cannot solve the problem. The wave loss phenomenon caused by ultrasonic reception cannot completely solve the error caused by wave loss, that is, the ultrasonic waves received by the ultrasonic receiving probes are not synchronized due to the unequal distance from the ultrasonic transmitting probe to each ultrasonic receiving probe. In the indoor ultrasonic positioning technology, the ultrasonic receiving probe loses several waves, which is the root cause of the ultrasonic positioning error. Moreover, the ultrasonic receiving device fixed on the ceiling will cause a certain blind area in the ultrasonic receiving probe when the ultrasonic transmitting probe moves. According to the characteristics of ultrasonic waves, when the ultrasonic transmitting/receiving probe is at 0 angle, the transmitting/receiving effect is the best. As the angle increases, the signal strength decreases rapidly. Generally, 30° is taken as the transmitting/receiving area. Therefore, the ultrasonic receiving probes fixed on the ceiling all have a certain blind area, and at the same time, the blind areas of multiple ultrasonic receiving probes overlap with each other, which results in a larger positioning blind area in the existing ultrasonic positioning system.
发明内容Contents of the invention
本发明为解决现有技术中的问题，提供一种室内地面运输车超声波定位控制系统及其控制方法。它能做到实时对准接收定位信号，避免丢波造成的定位误差，并且减少定位系统盲区。In order to solve the problems in the prior art, the present invention provides an ultrasonic positioning control system and a control method for an indoor ground transport vehicle. It can align and receive positioning signals in real time, avoid positioning errors caused by wave loss, and reduce blind spots in the positioning system.
本发明的思路是，考虑到超声波在空气中传播到达超声波接收装置存在时间延迟与超声波到达四个接收探头不同步造成丢波数不确定造成无法准确定位的问题，结合空气温度与气体流速，使用全方位超生波接收装置，可以完成对发射点的跟随及对准，进而保证了发射点到每个接收点的直线距离差在容错阈值内，因此四个接收探头收到的超声波信号的强度差在容错阈值内，避免了接收时间误差及丢波等问题造成的误差，保证了定位的准确性。并且，通过全方位超生波接收装置完成与发射点的完全跟随及对准，减少了由于超过超声波接收范围造成的盲区。The idea of the present invention is that, considering that there is a time delay when the ultrasonic wave propagates in the air and reaches the ultrasonic receiving device, and the ultrasonic wave arrives at the four receiving probes asynchronously, resulting in the uncertain number of lost waves and the inability to accurately locate the problem, combined with the air temperature and gas flow rate, the use of the full The azimuth ultrasonic wave receiving device can complete the follow-up and alignment of the transmitting point, thereby ensuring that the linear distance difference between the transmitting point and each receiving point is within the error tolerance threshold, so the intensity difference of the ultrasonic signals received by the four receiving probes is within Within the fault tolerance threshold, errors caused by problems such as receiving time errors and wave loss are avoided, ensuring the accuracy of positioning. Moreover, complete follow-up and alignment with the transmitting point is accomplished through the omnidirectional ultrasonic wave receiving device, which reduces blind spots caused by exceeding the ultrasonic receiving range.
本发明采用以下技术方案予以实现：The present invention adopts following technical scheme to realize:
一种室内地面运输车超声波定位控制系统，包括上位机系统及AGV系统；所述上位机系统通过AGV系统中的无线通信模块与AGV系统通信，上位机向AGV系统发送数据信息；An indoor ground transport vehicle ultrasonic positioning control system, including a host computer system and an AGV system; the host computer system communicates with the AGV system through a wireless communication module in the AGV system, and the host computer sends data information to the AGV system;
所述上位机系统包括参数设置模块、电子地图模块、串口配置模块、命令传输模块、处理模块，所述参数设置模块设定电子地图的大小、障碍物的排布和起止位置的数值，设置的数值通过处理模块的运算得到全局栅格电子地图与全局最优路线，全局栅格电子地图与最优路线在电子地图模块中显示；在串口配置模块中设置预先选择的与上位机通信的无线通信模块的串口信息，点击命令传输模块中的命令发送键确认发送命令，将全局最优路线发送至AGV系统中的主控单片机；所述处理模块包括嵌入了Matlab Script软件、并调用Matlab软件的Labview软件，处理模块通过参数设置模块所提供的数据得出全局栅格电子地图并完成蚁群算法的计算得出全局最优路线；The upper computer system includes a parameter setting module, an electronic map module, a serial port configuration module, a command transmission module, and a processing module. The parameter setting module sets the size of the electronic map, the arrangement of obstacles and the numerical value of the start and end positions. The value is obtained through the operation of the processing module to obtain the global grid electronic map and the global optimal route, and the global grid electronic map and the optimal route are displayed in the electronic map module; the pre-selected wireless communication with the host computer is set in the serial port configuration module The serial port information of the module, clicks the command transmission key in the command transmission module to confirm the sending command, and the global optimal route is sent to the main control microcontroller in the AGV system; the processing module includes Labview embedded with Matlab Script software and called Matlab software Software, the processing module obtains the global grid electronic map through the data provided by the parameter setting module and completes the calculation of the ant colony algorithm to obtain the global optimal route;
所述AGV系统包括主控单片机、超声波发送系统、自动跟随超声波接收系统、避障系统、AGV运动系统、显示屏；所述主控单片机和超声波发送系统及自动跟随超声波接收系统构成定位系统，通过自动跟随超声波接收系统根据超声波测距与舵机角度获得定位数据，主控单片机接收到定位数据经过可靠性判断获得AGV系统在电子地图中所在位置，主控单片机通过超声波发送系统中的无线通信模块接收到上位机发送的全局最优路线；所述主控单片机通过避障系统得到周围环境信息并且经过坐标变换转化为以AGV小车为中心的局部栅格电子地图，主控单片机基于得到的局部栅格电子地图运算处理得到局部避障路线，主控单片机1结合全局最优路线和局部避障路线做出处理，得到AGV小车运动路线的数据，然后将AGV小车运动路线的数据命令发送至AGV运动系统控制AGV小车运行，同时主控单片机将AGV小车的姿态、速度、位置、障碍物及规划路径的信息在显示屏上显示，并以每秒一次的频率更新。The AGV system includes a main control single-chip microcomputer, an ultrasonic transmission system, an automatic follow-up ultrasonic receiving system, an obstacle avoidance system, an AGV motion system, and a display screen; Automatically follow the ultrasonic receiving system to obtain the positioning data according to the ultrasonic distance measurement and the angle of the steering gear. The main control microcontroller receives the positioning data and obtains the location of the AGV system in the electronic map through reliability judgment. The main control single chip transmits the wireless communication module in the system through ultrasonic The global optimal route sent by the host computer is received; the main control single-chip microcomputer obtains the surrounding environment information through the obstacle avoidance system and converts it into a local grid electronic map centered on the AGV car through coordinate transformation, and the main control single-chip microcomputer is based on the obtained local grid. The grid electronic map calculation process obtains the local obstacle avoidance route, the main control microcontroller 1 combines the global optimal route and the local obstacle avoidance route to process, obtains the data of the AGV car movement route, and then sends the data command of the AGV car movement route to the AGV movement The system controls the operation of the AGV car, and at the same time, the main control single-chip computer displays the information of the AGV car's attitude, speed, position, obstacles and planned path on the display screen, and updates it at a frequency of once per second.
所述超声波发送系统包括一号舵机组、超声波发射探头、无线通信模块、位姿采集器；The ultrasonic transmission system includes No. 1 rudder unit, an ultrasonic transmission probe, a wireless communication module, and a pose collector;
所述无线通信模块为单芯片型射频收发机NRF24L01来实现主控单片机与上位机的通信；The wireless communication module is a single-chip radio frequency transceiver NRF24L01 to realize the communication between the main control single-chip microcomputer and the upper computer;
所述位姿采集器选用三轴加速度计三轴MPU6050六轴模块，由其陀螺仪功能来确定AGV小车的姿态；The pose collector selects the three-axis accelerometer three-axis MPU6050 six-axis module, and determines the attitude of the AGV car by its gyroscope function;
所述超声波发射探头为TCT40-16T/R超声波探头，所述一号舵机组由两台M90舵机组成一个二维云台，可以使超声波发射探头指向空中任一角度，实现在接收方方向上信号的最大强度传播。The ultrasonic transmitting probe is a TCT40-16T/R ultrasonic probe, and the No. 1 rudder unit is composed of two M90 steering gears to form a two-dimensional pan/tilt, which can make the ultrasonic transmitting probe point to any angle in the air, and achieve the goal in the direction of the receiving party. The maximum intensity propagation of the signal.
所述自动跟随超声波接收系统包括二号舵机组、超声波接收装置、无线模块、单片机、外接直流电源；单片机由外接直流电源供电，二号舵机组、超声波接收装置、无线模块均通过与单片机相连供电，所述自动跟随超声波接收系统中的单片机控制无线模块发送数据信号给超声波发送系统中的无线通信模块并同时启动单片机中定时器的计时功能，主控单片机接收到无线通信模块传输的信号的同时控制超声波发射探头发射超声波信号，自动跟随超声波接收系统中的单片机在超声波接收装置接收到超声波的同时令定时器停止计时，单片机根据内置的定时器所计的时间与二号舵机组的传输的转向角度以及超声波接收装置所处高度完成定位计算，得到超声波定位数据，单片机通过脉冲宽度调制控制自动跟随超声波接收系统中二号舵机组的转向，使超声波接收装置正对超声波发射探头所在方向，自动跟随超声波接收系统通过无线模块发送超声波定位数据，超声波发送系统中无线通信模块接收到超声波定位数据传送至AGV的主控单片机，主控单片机判断可靠性，若为0，则丢弃，并连续10次存储超声波定位数据，去掉其中最大值、最小值后取平均，得出最终超声波定位数据，主控单片机结合最终超声波定位数据以及位姿采集器所传输当前AGV小车的姿态数据处理得到超声波发送系统中一号舵机需要转动的对准角度；The automatic following ultrasonic receiving system includes No. 2 rudder unit, ultrasonic receiving device, wireless module, single-chip microcomputer, external DC power supply; the single-chip microcomputer is powered by an external DC power supply, and No. 2 rudder unit, ultrasonic receiving device, and wireless module are all powered by connecting with the single-chip microcomputer , the automatic follow-up of the single-chip microcomputer in the ultrasonic receiving system controls the wireless module to send data signals to the wireless communication module in the ultrasonic sending system and simultaneously starts the timing function of the timer in the single-chip microcomputer, and the main control single-chip microcomputer receives the signal transmitted by the wireless communication module Control the ultrasonic transmitting probe to transmit ultrasonic signals, and automatically follow the single-chip microcomputer in the ultrasonic receiving system to stop the timer when the ultrasonic receiving device receives the ultrasonic wave. The positioning calculation is completed by the angle and the height of the ultrasonic receiving device, and the ultrasonic positioning data is obtained. The single-chip microcomputer automatically follows the steering of No. 2 steering unit in the ultrasonic receiving system through pulse width modulation control, so that the ultrasonic receiving device is facing the direction of the ultrasonic transmitting probe and automatically follows The ultrasonic receiving system sends ultrasonic positioning data through the wireless module. The wireless communication module in the ultrasonic sending system receives the ultrasonic positioning data and transmits it to the main control single-chip microcomputer of the AGV. The main control single-chip microcomputer judges the reliability. The ultrasonic positioning data is averaged after removing the maximum and minimum values to obtain the final ultrasonic positioning data. The main control single-chip computer combines the final ultrasonic positioning data and the attitude data of the current AGV car transmitted by the position and attitude collector to process the ultrasonic transmission system. The alignment angle at which the No. steering gear needs to be rotated;
所述外接直流电源设定为5V。The external DC power supply is set to 5V.
所述一号舵机组及二号舵机组均为由两台舵机构成的二维云台，可完成挠性运动，即以垂直地面向下建立z轴，使超声波接收系统在与z轴构成45o角的曲面以下空间范围内转动；The No. 1 rudder unit and the No. 2 rudder unit are both two-dimensional pan-tilts composed of two rudder gears, which can complete flexible motion, that is, establish the z-axis vertically downwards on the ground, so that the ultrasonic receiving system is formed with the z-axis. Rotate within the space below the curved surface at an angle of 45o ;
所述超声波接收装置为分布于亚克力板上的超声波接收头组，全部超声波接收头所构成图形的几何中心到达每个超声波接收头的距离相等，并以正方形分布于亚克力板上四个角上，所述超声波接收头组包括超声波接收头、有源带通滤波器、计数器及数值比较器；每个超声波接收头分别与有源带通滤波器串连，有源带通滤波器与计数器串连，全部计数器与数值比较器相连，由于在超声波定位中，超声波发射探头发出的超声波的频率为38kHZ～40kHZ，超声波接收头接收超声波信号，超声波信号通过有源带通滤波器，通过有源带通滤波器将超声波频率控制在40kHZ，滤掉其他频率的干扰波，经过有源带通滤波器过滤的超声波传输至计数器，计数器分别记录每个超声波接收头在1s内接收到的超声波个数，每个计数器记录的超声波个数以DCB码的形式传送到数值比较器中进行比较，在数值比较器中判断1s时间内每个超声波接收头接收到的超声波个数是否相同，通过数值比较器的判断结果来判断每个超声波接收头是否存在丢波现象，若没有丢波则判定这组信号为可靠信号；The ultrasonic receiving device is a group of ultrasonic receiving heads distributed on the acrylic plate. The geometric center of the figure formed by all the ultrasonic receiving heads reaches the same distance from each ultrasonic receiving head, and is distributed on the four corners of the acrylic plate in a square shape. The ultrasonic receiving head group includes an ultrasonic receiving head, an active band-pass filter, a counter and a numerical comparator; each ultrasonic receiving head is connected in series with the active band-pass filter respectively, and the active band-pass filter is connected in series with the counter , all the counters are connected with the numerical comparator, because in the ultrasonic positioning, the ultrasonic frequency emitted by the ultrasonic transmitting probe is 38kHZ ~ 40kHZ, the ultrasonic receiving head receives the ultrasonic signal, the ultrasonic signal passes through the active band-pass filter, and passes through the active band-pass The filter controls the ultrasonic frequency at 40kHZ, filters out the interference waves of other frequencies, and transmits the ultrasonic waves filtered by the active band-pass filter to the counter, and the counter records the number of ultrasonic waves received by each ultrasonic receiving head within 1s, and every The number of ultrasonic waves recorded by each counter is transmitted to the numerical comparator in the form of DCB code for comparison. In the numerical comparator, it is judged whether the number of ultrasonic waves received by each ultrasonic receiving head is the same within 1s. Through the judgment of the numerical comparator The results are used to judge whether there is a wave loss phenomenon in each ultrasonic receiving head. If there is no wave loss, it is determined that this group of signals is a reliable signal;
自动跟随超声波接收系统中由两台舵机构成的二号舵机组为由闭环PID系统控制的两直流无刷电机，由两直流无刷电机控制超声波接收头所在面板的转向，并由闭环PID系统控制实际角度，若偏转角度与目标角度不同则通过闭环PID系统反馈系统进行调整，同时将陀螺仪与单片机以导线相连，通过陀螺仪采集超声波接收头与垂直方向的角度数据，陀螺仪将角度数据传输至自动跟随超声波接收系统中的单片机进行定位计算。The No. 2 rudder unit composed of two steering gears in the automatic follow ultrasonic receiving system is two DC brushless motors controlled by a closed-loop PID system. The two DC brushless motors control the steering of the panel where the ultrasonic receiving head is located, and the closed-loop PID system Control the actual angle. If the deflection angle is different from the target angle, it will be adjusted through the closed-loop PID system feedback system. At the same time, the gyroscope will be connected to the single-chip microcomputer with a wire, and the angle data between the ultrasonic receiving head and the vertical direction will be collected by the gyroscope, and the angle data will be collected by the gyroscope. It is transmitted to the single-chip microcomputer in the automatic follow-up ultrasonic receiving system for positioning calculation.
所述超声波接收装置中的接收探头选用基于CX20106a的超声波接收模块，无线模块选用NRF24L01射频模块，所述有源带通滤波器由低通RC环节和高通RC环节组成，有源带通滤波器的输出频率为40.2kHz，超声波发射器发出的超声波的频率为38kHz～40kHz，通过有源带通滤波器滤波的输出频率符合超声波频率范围的要求，所述计数器为CD4518双BCD加法计数器，所述数值比较器为74LS85BCD数值比较器，超声波接收头采用四个，四个超声波接收头与对应的四个有源带通滤波器相连，其中，每个超声波接收头的INT端口分别与对应的有源带通滤波器的Ui端口连接；四个有源带通滤波器与对应的四个计数器连接，每个有源带通滤波器的Uo端口与对应的计数器的CP1端口连接。每个计数器的EN1端口与外接直流电源相连，由外接直流电源提供5V直流电压。全部计数器中，和第一个超声波探头连接的计数器有三路输出信号A3、A2、A1，这三路输出信号相同，另外与其他3个超声波探头相连的计数器分别输出1路信号，分别由三个计数器的B3、B2、B1端口输出；全部计数器的输出信号输出至数值比较器；The receiving probe in the described ultrasonic receiving device selects the ultrasonic receiving module based on CX20106a, the wireless module selects the NRF24L01 radio frequency module, and the active band-pass filter is made up of a low-pass RC link and a high-pass RC link, and the active band-pass filter The output frequency is 40.2kHz, and the ultrasonic frequency emitted by the ultrasonic transmitter is 38kHz to 40kHz. The output frequency filtered by the active band-pass filter meets the requirements of the ultrasonic frequency range. The counter is a CD4518 dual BCD addition counter, and the value The comparator is a 74LS85BCD numerical comparator. Four ultrasonic receiving heads are used. The four ultrasonic receiving heads are connected to the corresponding four active band-pass filters. Among them, the INT ports of each ultrasonic receiving head are respectively connected to the corresponding active band-pass filters. The Ui port of the pass filter is connected; the four active band pass filters are connected to the corresponding four counters, and the Uo port of each active band pass filter is connected to the CP1 port of the corresponding counter. The EN1 port of each counter is connected with an external DC power supply, which provides 5V DC voltage. Among all the counters, the counter connected to the first ultrasonic probe has three output signals A3, A2, and A1, and the three output signals are the same. In addition, the counters connected to the other three ultrasonic probes output one signal respectively, which are respectively composed of three The B3, B2, and B1 ports of the counter are output; the output signals of all counters are output to the numerical comparator;
所述有源带通滤波器由低通RC环节和高通RC环节组成，其中电阻R1、电阻R2、电阻R3、电阻R4均为1.2kΩ，电容C1与电容C2均为3300pF，有源带通滤波器的输出频率为40.2kHz，超声波发射器发出的超声波的频率为38kHz～40kHz。The active band-pass filter is composed of a low-pass RC link and a high-pass RC link, wherein the resistors R1, R2, R3, and R4 are all 1.2kΩ, and the capacitors C1 and C2 are both 3300pF. The active band-pass filter The output frequency of the transmitter is 40.2kHz, and the frequency of the ultrasonic wave emitted by the ultrasonic transmitter is 38kHz-40kHz.
所述AGV运动系统包括电机控制装置、电源控制装置及车体，所述车体经由电机控制装置控制并由电源控制装置供电完成运动；所述车体包括车架、和位于车架下面的全向轮；所述电机控制装置包括与全向轮数目相同的电机、编码器以及与电机数目对应的驱动器，所述驱动器将主控单片机传输的运动命令解码成各全向轮的转速，转速通过CAN总线连接传输至各全向轮的电机，电机与全向轮相连带动全向轮运动决定AGV运动速度与方向，编码器与电机相连测电机实际转速，实际转速传输至驱动器，若编码器所测全向轮实际速度与命令有所偏差，则驱动器对各全向轮的转速作出调整；所述电源控制模块包括锂电池及电源总线接口电路，锂电池经由电源总线接口电路调压至主控单片机所需压值后，对主控单片机以及超声波发送系统、避障系统、AGV运动系统进行供电；The AGV motion system includes a motor control device, a power control device and a car body, the car body is controlled by the motor control device and powered by the power control device to complete the movement; to the wheel; the motor control device includes the same motor as the number of omni-directional wheels, an encoder and a driver corresponding to the number of motors, and the driver decodes the motion command transmitted by the main control single-chip microcomputer into the rotating speed of each omni-directional wheel, and the rotating speed passes through The CAN bus is connected to the motors of the omnidirectional wheels. The motors are connected to the omnidirectional wheels to drive the movement of the omnidirectional wheels to determine the movement speed and direction of the AGV. The encoder is connected to the motor to measure the actual speed of the motor, and the actual speed is transmitted to the driver. If there is a deviation between the actual speed of the measured omnidirectional wheels and the command, the driver will adjust the rotational speed of each omnidirectional wheel; the power control module includes a lithium battery and a power bus interface circuit, and the lithium battery is regulated to the main controller via the power bus interface circuit. After the required voltage value of the single-chip microcomputer, power is supplied to the main control single-chip microcomputer, ultrasonic transmission system, obstacle avoidance system, and AGV motion system;
所述AGV运动系统中电机为直流减速电机，驱动器为四轴驱动器，车轮为QMA10全向轮，通过四个轮子速度配合完成AGV小车位姿不变情况下任意角度转向。The motor in the AGV motion system is a DC geared motor, the driver is a four-axis drive, and the wheels are QMA10 omnidirectional wheels. The AGV trolley turns at any angle under the condition that the posture of the AGV trolley remains unchanged through the speed coordination of the four wheels.
所述避障系统包括扫描仪、转换模块，主控单片机将环境检测命令经转换模块转化成以太网格式输出给扫描仪，扫描仪完成一次扫描后，将周围环境数据经转换模块将其转化成串口格式并传输至主控单片机，主控单片机接收到扫描仪检测到的周围环境信息后，将其进行坐标变换处理，用于建立以AGV小车所在位置为中心的局部栅格电子地图，得到局部栅格电子地图后通过A*算法运算得到局部避障路线；The obstacle avoidance system includes a scanner and a conversion module. The main control single-chip microcomputer converts the environmental detection command through the conversion module into an Ethernet format and outputs it to the scanner. After the scanner completes a scan, the surrounding environment data is converted into The serial port format is transmitted to the main control single-chip microcomputer. After the main control single-chip microcomputer receives the surrounding environment information detected by the scanner, it performs coordinate transformation processing to establish a local grid electronic map centered on the position of the AGV car, and obtains a local After the grid electronic map, the local obstacle avoidance route is obtained through the A* algorithm operation;
所述避障系统中的扫描仪选用SICK_TIM_351型激光扫描仪，用以实现对AGV小车周围障碍物的检测，其检测数据用于对局部栅格电子地图的更新；The scanner in the obstacle avoidance system selects SICK_TIM_351 type laser scanner to realize the detection of obstacles around the AGV trolley, and its detection data is used for updating the local grid electronic map;
选取的动态避障范围为以AGV小车为中心半径4m的范围，A*算法采取栅格地图法，栅格地图的分度值为0.05m，搜索的地图总栅格数为25600个，主控单片机所采用型号提供了以太网接口，主控单片机与串口-WIFI模块相连，串口-WIFI模块另一端与激光扫描仪相连，通过串口-WIFI模块将信号转换实现主控单片机和避障系统的通讯。The selected dynamic obstacle avoidance range is the radius of 4m centered on the AGV car. The A* algorithm adopts the grid map method. The division value of the grid map is 0.05m. The total grid number of the searched map is 25600. The model used by the single-chip microcomputer provides an Ethernet interface, the main control single-chip microcomputer is connected to the serial port-WIFI module, and the other end of the serial port-WIFI module is connected to the laser scanner, and the signal is converted through the serial port-WIFI module to realize the communication between the main control single-chip microcomputer and the obstacle avoidance system .
所述AGV系统与上位机之间还安装有两个串口-WIFI模块，用以实现数据的实时传输；Two serial port-WIFI modules are also installed between the AGV system and the host computer to realize real-time data transmission;
所述AGV小车还安装有两个串口-WIFI模块与主控单片机连接，用以实现与上位机系统之间数据的实时传输，当定位系统完成对AGV小车的定位后，主控单片机通过两个串口-WIFI模块将接收到的定位信息发送至上位机，在上位机中电子地图模块实时显示AGV小车位置。The AGV car is also equipped with two serial port-WIFI modules connected with the main control single-chip microcomputer, in order to realize the real-time transmission of data between the upper computer system. The serial port-WIFI module sends the received positioning information to the host computer, and the electronic map module in the host computer displays the position of the AGV car in real time.
所述主控单片机为STM32F407VET6单片机，单片机的PA2、PA3引脚与串口-WIFI模块相连，完成主控单片机与上位机的通讯，单片机的PC10、PC11引脚与串口-WIFI模块相连，完成主控单片机与SICK_TIM_351扫描仪的数据传输，单片机的PA0、PA1引脚与M6050六轴模块相连，使主控单片机接收AGV小车的位姿数据，单片机的PB10、PB11引脚与一号舵机组中两个舵机相连，使主控单片机控制超生波发射的方向，单片机的PC6、PC7引脚与超声波发射探头相连，使主控单片机控制发射超声波信号，单片机的PB8、PB9引脚通过CAN总线向AGV运动系统的驱动器发送命令，单片机的PA5～7、PA12引脚与无线通信模块相连，接收超声波定位数据至主控单片机；The main control single-chip microcomputer is STM32F407VET6 single-chip microcomputer, and the PA2 and PA3 pins of the single-chip microcomputer are connected with the serial port-WIFI module to complete the communication between the main control single-chip microcomputer and the host computer, and the PC10 and PC11 pins of the single-chip microcomputer are connected with the serial port-WIFI module to complete the main control For the data transmission between the MCU and the SICK_TIM_351 scanner, the PA0 and PA1 pins of the MCU are connected to the M6050 six-axis module, so that the main control MCU can receive the pose data of the AGV trolley, and the PB10 and PB11 pins of the MCU are connected to the two The steering gear is connected so that the main control single-chip microcomputer controls the direction of ultrasonic emission, the PC6 and PC7 pins of the single-chip microcomputer are connected with the ultrasonic transmitting probe, so that the main control single-chip microcomputer controls the emission of ultrasonic signals, and the PB8 and PB9 pins of the single-chip microcomputer move to the AGV through the CAN bus The driver of the system sends commands, and the PA5~7 and PA12 pins of the single-chip microcomputer are connected to the wireless communication module to receive the ultrasonic positioning data to the main control single-chip microcomputer;
10、一种权利要求1至9所述的室内地面运输车超声波定位控制系统的控制方法，包括以下步骤：10. A control method for the ultrasonic positioning control system of an indoor ground transport vehicle according to claims 1 to 9, comprising the following steps:
a.自动跟随超声波接收系统中的无线模块向AGV系统上超声波发送系统发出一列数据信号，AGV系统中的主控单片机通过无线通信模块接收自动跟随超声波接收系统中的无线模块发送的数据信号；a. Automatically follow the wireless module in the ultrasonic receiving system to send a series of data signals to the ultrasonic sending system on the AGV system, and the main control microcontroller in the AGV system receives the data signal sent by the wireless module in the ultrasonic receiving system automatically through the wireless communication module;
b.主控单片机开启超声波发射定时器，令超声波发送系统发射超声波信号。同时自动跟随超声波接收系统中的无线模块自动应答，自动跟随超声波接收系统中的单片机开启自带定时器的计时功能进行超声波接收计时；b. The main control single-chip microcomputer starts the ultrasonic transmission timer, so that the ultrasonic transmission system transmits ultrasonic signals. At the same time, it automatically responds automatically with the wireless module in the ultrasonic receiving system, and automatically follows the single-chip microcomputer in the ultrasonic receiving system to start the timing function with its own timer for ultrasonic receiving timing;
c.超声波接收装置接收超声波发送系统发射的超声波信号，超声波信号传输至单片机，单片机中的定时器完成计时，超声波接收头接收到的超声波信号经超声波接收装置中的有源带通滤波器滤波，在超声波定位中，超声波发射探头发出的超声波的频率大约为38kHZ～40kHZ，通过有源带通滤波器去除不在超声波发送系统发射的40kHz超声波阈值内的杂波，并且通过每个超声波接收探头组中的CD4518双BCD加法计数器对每秒所接收到的超声波个数进行计数，每个超声波接收头接收到的超声波个数被传送到74LS85BCD数值比较器中进行比较，全部计数器中，和第一个超声波探头连接的计数器有三路输出信号A3、A2、A1，这三路输出信号相同，另外与其他3个超声波探头相连的计数器分别输出1路信号，分别由三个计数器的B3、B2、B1端口输出，信号输出至数值比较器。以与第一个超声波接收头相连的计数器的3路输出信号为准，分别与和其余三个超声波接收头相连的加法计数器输出的信号进行对比，若对比结果相符，则证明4个超声波接收头接收的波形数目相同，则这组信号判定为可靠信号，若与其余行相符，则代表4个超声波接收头接收的波形数目不相同，即存在丢波，则信号舍弃不进行操作；c. The ultrasonic receiving device receives the ultrasonic signal transmitted by the ultrasonic sending system, the ultrasonic signal is transmitted to the single-chip microcomputer, the timer in the single-chip microcomputer completes the timing, and the ultrasonic signal received by the ultrasonic receiving head is filtered by the active band-pass filter in the ultrasonic receiving device, In ultrasonic positioning, the frequency of the ultrasonic wave emitted by the ultrasonic transmitting probe is about 38kHZ ~ 40kHZ, and the clutter that is not within the 40kHz ultrasonic threshold emitted by the ultrasonic transmitting system is removed through an active band-pass filter, and passed through each ultrasonic receiving probe group The CD4518 dual BCD addition counter counts the number of ultrasonic waves received per second, and the number of ultrasonic waves received by each ultrasonic receiving head is transmitted to the 74LS85BCD numerical comparator for comparison. Among all counters, it is compared with the first ultrasonic wave The counter connected to the probe has three output signals A3, A2, and A1, and the three output signals are the same. In addition, the counters connected to the other three ultrasonic probes output one signal respectively, which are respectively output by the B3, B2, and B1 ports of the three counters. , the signal is output to the numerical comparator. Based on the 3-way output signal of the counter connected to the first ultrasonic receiving head, compare them with the signals output by the adding counter connected to the other three ultrasonic receiving heads. If the comparison results are consistent, it proves that the four ultrasonic receiving heads If the number of waveforms received is the same, this group of signals is judged as a reliable signal. If it matches the rest of the lines, it means that the number of waveforms received by the four ultrasonic receiving heads is different, that is, there is wave loss, and the signal is discarded and no operation is performed;
d.可靠信号被传输至单片机，单片机根据可靠信号对应单片机所计时的时间计算每个超声波接收头所测的距离；单片机根据每个超声波接收头所测的距离判断当前超声波信号的可靠性，即再次判断每个超声波接收头所接收到是否为同一列超声波信号，以及是否对同一列超声波信号每个超声波接收头都响应，根据每个超声波接收头所测的距离进行几何计算，若到达所有超声波接收头几何平面中点的误差不超过所设阈值，即为可靠，得到每个超声波接收头接收到的可靠距离数据；d. The reliable signal is transmitted to the single-chip microcomputer, and the single-chip microcomputer calculates the distance measured by each ultrasonic receiving head according to the time counted by the reliable signal corresponding to the single-chip microcomputer; the single-chip computer judges the reliability of the current ultrasonic signal according to the distance measured by each ultrasonic receiving head, that is Determine again whether each ultrasonic receiving head receives the same row of ultrasonic signals, and whether each ultrasonic receiving head responds to the same row of ultrasonic signals, and perform geometric calculations based on the distance measured by each ultrasonic receiving head. The error of the midpoint of the geometric plane of the receiving head does not exceed the set threshold, which is reliable, and the reliable distance data received by each ultrasonic receiving head is obtained;
e.自动跟随超声波接收系统中的单片机根据每个超声波接收头接收到的可靠距离数据结合当前二号舵机组角度以及接收系统所处高度，得出AGV超声波定位数据，超声波接收装置根据当前定位数据通过脉冲宽度调制PWM控制自动跟随超声波接收系统中二号舵机组转向对准AGV小车；e. Automatically follow the single-chip microcomputer in the ultrasonic receiving system to obtain the AGV ultrasonic positioning data according to the reliable distance data received by each ultrasonic receiving head combined with the current No. 2 rudder unit angle and the height of the receiving system. Through pulse width modulation PWM control, it automatically follows the No. 2 rudder unit steering in the ultrasonic receiving system and aligns it with the AGV trolley;
f.自动跟随超声波接收系统通过无线模块发送超声波定位数据，超声波发送系统中的无线通信模块接收到超声波定位数据传送至AGV系统的主控单片机，主控单片机判断可靠性，若数据为0，则丢弃，并连续10次存储超声波定位数据，去掉其中最大值、最小值后取平均，得出最终超声波定位数据；f. Automatically follow the ultrasonic receiving system to send ultrasonic positioning data through the wireless module. The wireless communication module in the ultrasonic sending system receives the ultrasonic positioning data and transmits it to the main control single-chip microcomputer of the AGV system. The main control single-chip microcomputer judges the reliability. If the data is 0, then Discard and store the ultrasonic positioning data for 10 consecutive times, remove the maximum and minimum values and take the average to obtain the final ultrasonic positioning data;
g.主控单片机结合最终超声波定位数据以及位姿采集器所传输当前AGV小车姿态数据处理得到超声波发送系统中的一号舵机组需要转动的对准角度；g. The main control single-chip microcomputer combines the final ultrasonic positioning data and the current AGV car attitude data transmitted by the pose collector to process the alignment angle that the No. 1 rudder unit in the ultrasonic sending system needs to rotate;
h.主控单片机将对准角度传输至一号舵机组，超声波发送系统中的一号舵机组完成转动，完成对发射超声波信号时AGV小车所在点的定位及超声波发送系统与自动跟随超声波接收系统对准，进入循环或循环上述步骤对AGV小车新的当前位置进行定位。h. The main control MCU transmits the alignment angle to the No. 1 rudder unit, and the No. 1 rudder unit in the ultrasonic sending system completes the rotation, and completes the positioning of the AGV car when the ultrasonic signal is transmitted, and the ultrasonic sending system and the automatic follow-up ultrasonic receiving system Align, enter the loop or loop the above steps to locate the new current position of the AGV.
本发明与现有技术相比具有的突出效果为：The outstanding effect that the present invention has compared with prior art is:
①超生波接收装置以及超声波发射探头可通过一号舵机组和二号舵机组根据计算出的定位数据完成对发射探头的跟随及对准，进而保证了发射点到每个接收点的直线距离差在根据实际环境尺寸实验后所设阈值范围内。又，发出的同一超声波信号的强度衰减和传输距离成正比，因此此超声波接收装置的四个接收探头收到的超声波信号的强度差在根据实际环境尺寸实验后所设阈值范围内，进而保证了超声波传至每个接收模块时，接收模块引起的丢波个数一致。①The ultrasonic receiving device and the ultrasonic transmitting probe can follow and align the transmitting probe through the No. 1 rudder unit and the No. 2 rudder unit according to the calculated positioning data, thereby ensuring the straight-line distance difference between the transmitting point and each receiving point Within the threshold range set after the actual environment size experiment. Also, the intensity attenuation of the same ultrasonic signal sent is proportional to the transmission distance, so the intensity difference of the ultrasonic signals received by the four receiving probes of the ultrasonic receiving device is within the threshold range set after the actual environment size experiment, thereby ensuring When the ultrasonic wave is transmitted to each receiving module, the number of lost waves caused by the receiving module is the same.
②该超声波系统的超生波接收装置以及超声波发射探头的跟随及对准可以解决由超声波丢波引起的定位误差，提高定位精度。同时，此装置可以实现全方位的超声波定位与跟踪，实现了超生波接收装置以及超声波发射探头0角度对准接收超声波信号，解决了超声波由于发射/接收存在角度造成的盲区问题，因此解决了传统超声波定位系统的盲区过大的问题。②The ultrasonic receiving device of the ultrasonic system and the following and alignment of the ultrasonic transmitting probe can solve the positioning error caused by the ultrasonic wave loss and improve the positioning accuracy. At the same time, this device can realize all-round ultrasonic positioning and tracking, realize the 0-angle alignment of the ultrasonic receiving device and the ultrasonic transmitting probe to receive ultrasonic signals, and solve the problem of blind spots caused by the angle of ultrasonic emission/reception, thus solving the problem of traditional The blind area of the ultrasonic positioning system is too large.
③超声波接收装置中添加滤波器和波形计数器，滤波器可避免杂波干扰，波形计数器可检测每个超声波接收头是否存在丢波，提高接收到的超声波信号的可靠性，减少了误差，提高了超声波定位系统的效率。③A filter and a waveform counter are added to the ultrasonic receiving device. The filter can avoid clutter interference, and the waveform counter can detect whether there is wave loss in each ultrasonic receiving head, which improves the reliability of the received ultrasonic signal, reduces errors, and improves Efficiency of ultrasonic positioning systems.
④发明的超声波定位控制器及其控制方法，可用于仓储货物的运输定位，也可用于其他平面小车运输的场所，如室内车间的工装、车间生产原件的运送、医院医疗看护、家庭医疗看护等类似场所。④ The invented ultrasonic positioning controller and its control method can be used for the transportation and positioning of warehoused goods, and can also be used for other places where plane trolleys are transported, such as the tooling of indoor workshops, the delivery of original production parts in workshops, hospital medical care, family medical care, etc. Similar places.
⑤本发明的应用，可节约人力物力投资成本，在国内市场还未见技术相同的设备的应用，具有广阔的市场前景。5. The application of the present invention can save the investment cost of manpower and material resources, and the application of equipment with the same technology has not been seen in the domestic market, and has broad market prospects.
附图说明Description of drawings
图1为本发明提供的超声波定位控制器的原理示意图。Fig. 1 is a schematic diagram of the principle of the ultrasonic positioning controller provided by the present invention.
图2为本发明超声波定位控制器控制方法的程序框图。Fig. 2 is a program block diagram of the control method of the ultrasonic positioning controller of the present invention.
图3为本发明超声波定位控制器中超声波接收系统单片机部分接线图。Fig. 3 is a partial wiring diagram of the single chip microcomputer of the ultrasonic receiving system in the ultrasonic positioning controller of the present invention.
图4为超声波接收模块电路图。(基于CX20106的超声波接收模块电路图)Figure 4 is a circuit diagram of the ultrasonic receiving module. (Circuit diagram of ultrasonic receiving module based on CX20106)
图5为射频模块电路图。(NRF24L01无线射频模块电路图)Figure 5 is a circuit diagram of the radio frequency module. (NRF24L01 wireless radio frequency module circuit diagram)
图6为滤波电路图。Figure 6 is a filter circuit diagram.
图7为超声波波形计数器及数值比较器电路图。Fig. 7 is a circuit diagram of an ultrasonic waveform counter and a numerical comparator.
具体实施方式Detailed ways
下面结合附图对本发明的具体实施方式进行详细说明。Specific embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings.
参见图1～图7。See Figures 1 to 7.
本发明一种室内地面运输车超声波定位控制器，包括上位机系统及AGV系统；所述上位机系统通过AGV系统中的无线通信模块与AGV系统通信，上位机向AGV系统发送数据信息；The present invention is an ultrasonic positioning controller for an indoor ground transport vehicle, comprising a host computer system and an AGV system; the host computer system communicates with the AGV system through a wireless communication module in the AGV system, and the host computer sends data information to the AGV system;
所述上位机系统包括参数设置模块、电子地图模块、串口配置模块、命令传输模块、处理模块，所述参数设置模块设定电子地图的大小、障碍物的排布和起止位置的数值，设置的数值通过处理模块的运算得到全局栅格电子地图与全局最优路线，全局栅格电子地图与最优路线在电子地图模块中显示；在串口配置模块中设置预先选择的与上位机通信的无线通信模块的串口信息，点击命令传输模块中的命令发送键确认发送命令，将全局最优路线发送至AGV系统中的主控单片机；所述处理模块包括嵌入了Matlab Script软件、并调用Matlab软件的Labview软件，处理模块通过参数设置模块所提供的数据得出全局栅格电子地图并完成蚁群算法的计算得出全局最优路线；The upper computer system includes a parameter setting module, an electronic map module, a serial port configuration module, a command transmission module, and a processing module. The parameter setting module sets the size of the electronic map, the arrangement of obstacles and the numerical value of the start and end positions. The value is obtained through the operation of the processing module to obtain the global grid electronic map and the global optimal route, and the global grid electronic map and the optimal route are displayed in the electronic map module; the pre-selected wireless communication with the host computer is set in the serial port configuration module The serial port information of the module, clicks the command transmission key in the command transmission module to confirm the sending command, and the global optimal route is sent to the main control microcontroller in the AGV system; the processing module includes Labview embedded with Matlab Script software and called Matlab software Software, the processing module obtains the global grid electronic map through the data provided by the parameter setting module and completes the calculation of the ant colony algorithm to obtain the global optimal route;
所述AGV系统包括主控单片机、超声波发送系统、自动跟随超声波接收系统、避障系统、AGV运动系统、显示屏；所述主控单片机和超声波发送系统及自动跟随超声波接收系统构成定位系统，通过自动跟随超声波接收系统根据超声波测距与舵机角度获得定位数据，主控单片机接收到定位数据经过可靠性判断获得AGV系统在电子地图中所在位置，主控单片机通过超声波发送系统中的无线通信模块接收到上位机发送的全局最优路线；所述主控单片机通过避障系统得到周围环境信息并且经过坐标变换转化为以AGV小车(平面运输车)为中心的局部栅格电子地图，主控单片机基于得到的局部栅格电子地图运算处理得到局部避障路线，主控单片机结合全局最优路线和局部避障路线做出处理，得到AGV小车运动路线的数据，然后将AGV小车运动路线的数据命令发送至AGV运动系统控制AGV小车运行，同时主控单片机将AGV小车的姿态、速度、位置、障碍物及规划路径的信息在显示屏上显示，并以每秒一次的频率更新。The AGV system includes a main control single-chip microcomputer, an ultrasonic transmission system, an automatic follow-up ultrasonic receiving system, an obstacle avoidance system, an AGV motion system, and a display screen; Automatically follow the ultrasonic receiving system to obtain the positioning data according to the ultrasonic distance measurement and the angle of the steering gear. The main control microcontroller receives the positioning data and obtains the location of the AGV system in the electronic map through reliability judgment. The main control single chip transmits the wireless communication module in the system through ultrasonic Receive the global optimal route sent by the host computer; the main control single-chip microcomputer obtains the surrounding environment information through the obstacle avoidance system and converts it into a local grid electronic map centered on the AGV trolley (plane transport vehicle) through coordinate transformation, and the main control single-chip microcomputer The local obstacle avoidance route is obtained based on the obtained local grid electronic map operation and processing, and the main control microcontroller combines the global optimal route and the local obstacle avoidance route to process to obtain the data of the AGV car movement route, and then the data command of the AGV car movement route Send it to the AGV motion system to control the operation of the AGV car. At the same time, the main control single-chip microcomputer displays the information of the attitude, speed, position, obstacles and planned path of the AGV car on the display screen, and updates it at a frequency of once per second.
所述超声波发送系统包括一号舵机组、超声波发射探头、无线通信模块、位姿采集器；The ultrasonic transmission system includes No. 1 rudder unit, an ultrasonic transmission probe, a wireless communication module, and a pose collector;
所述无线通信模块为结构简单、运行稳定的单芯片型射频收发机NRF24L01来实现主控单片机与上位机的通信；The wireless communication module is a single-chip radio frequency transceiver NRF24L01 with simple structure and stable operation to realize the communication between the main control single-chip microcomputer and the upper computer;
所述位姿采集器为三轴加速度计三轴MPU6050六轴模块，由其陀螺仪功能来确定AGV小车的姿态。The pose collector is a three-axis accelerometer three-axis MPU6050 six-axis module, and its gyroscope function determines the attitude of the AGV car.
所述超声波发射探头为TCT40-16T/R超声波探头，The ultrasonic transmitting probe is a TCT40-16T/R ultrasonic probe,
所述一号舵机组由两台M90舵机组成一个二维云台，可使超声波发射探头指向空中任一角度，实现在接收方方向上信号的最大强度传播。The No. 1 rudder unit consists of two M90 rudders to form a two-dimensional pan-tilt, which can make the ultrasonic transmitting probe point to any angle in the air, and realize the maximum intensity propagation of the signal in the direction of the receiver.
所述自动跟随超声波接收系统包括二号舵机组、超声波接收装置、无线模块、单片机、外接直流电源；单片机由外接直流电源供电，二号舵机组、超声波接收装置、无线模块均通过与单片机相连供电，所述自动跟随超声波接收系统中的单片机控制无线模块发送数据信号给超声波发送系统中的无线通信模块并同时启动单片机中定时器的计时功能，主控单片机接收到无线通信模块传输的信号的同时控制超声波发射探头发射超声波信号，自动跟随超声波接收系统中的单片机在超声波接收装置接收到超声波的同时令定时器停止计时，单片机根据内置的定时器所计的时间与二号舵机组的传输的转向角度以及超声波接收装置所处高度完成定位计算，得到超声波定位数据，单片机通过脉冲宽度调制控制自动跟随超声波接收系统中二号舵机组的转向，使超声波接收装置正对超声波发射探头所在方向，自动跟随超声波接收系统通过无线模块发送超声波定位数据，超声波发送系统中无线通信模块接收到超声波定位数据传送至AGV的主控单片机，主控单片机判断可靠性，若为0，则丢弃，并连续10次存储超声波定位数据，去掉其中最大值、最小值后取平均，得出最终超声波定位数据，主控单片机结合最终超声波定位数据以及位姿采集器所传输当前AGV小车的姿态数据处理得到超声波发送系统中一号舵机需要转动的对准角度；The automatic following ultrasonic receiving system includes No. 2 rudder unit, ultrasonic receiving device, wireless module, single-chip microcomputer, external DC power supply; the single-chip microcomputer is powered by an external DC power supply, and No. 2 rudder unit, ultrasonic receiving device, and wireless module are all powered by connecting with the single-chip microcomputer , the automatic follow-up of the single-chip microcomputer in the ultrasonic receiving system controls the wireless module to send data signals to the wireless communication module in the ultrasonic sending system and simultaneously starts the timing function of the timer in the single-chip microcomputer, and the main control single-chip microcomputer receives the signal transmitted by the wireless communication module Control the ultrasonic transmitting probe to transmit ultrasonic signals, and automatically follow the single-chip microcomputer in the ultrasonic receiving system to stop the timer when the ultrasonic receiving device receives the ultrasonic wave. The positioning calculation is completed by the angle and the height of the ultrasonic receiving device, and the ultrasonic positioning data is obtained. The single-chip microcomputer automatically follows the steering of No. 2 steering unit in the ultrasonic receiving system through pulse width modulation control, so that the ultrasonic receiving device is facing the direction of the ultrasonic transmitting probe and automatically follows The ultrasonic receiving system sends ultrasonic positioning data through the wireless module. The wireless communication module in the ultrasonic sending system receives the ultrasonic positioning data and transmits it to the main control single-chip microcomputer of the AGV. The main control single-chip microcomputer judges the reliability. The ultrasonic positioning data is averaged after removing the maximum and minimum values to obtain the final ultrasonic positioning data. The main control single-chip computer combines the final ultrasonic positioning data and the attitude data of the current AGV car transmitted by the position and attitude collector to process the ultrasonic transmission system. The alignment angle at which the No. steering gear needs to be rotated;
所述外接直流电源设定为5V；The external DC power supply is set to 5V;
所述一号舵机组及二号舵机组均为由两台舵机构成的二维云台，可完成挠性运动，即以垂直地面向下建立z轴。The No. 1 rudder unit and the No. 2 rudder unit are both two-dimensional pan-tilts composed of two rudder gears, which can complete flexible motion, that is, establish the z-axis vertically downward.
所述超声波接收装置为以一定规则分布于亚克力板上的超声波接收头组，如图4所示，所需遵循规则为全部超声波接收头所构成图形的几何中心到达每个超声波接收头的距离相等，本装置现为以正方形分布于亚克力板上四个角上。所述超声波接收头组包括超声波接收头、有源带通滤波器、计数器及数值比较器；每个超声波接收头分别与有源带通滤波器串连，有源带通滤波器与计数器串连，有源滤波器电路如图6所示，全部计数器与数值比较器相连，超声波波形计数器电路图如图7所示。由于在超声波定位中，超声波发射探头发出的超声波的频率大约为38kHZ～40kHZ，超声波接收头接收超声波信号，超声波信号通过有源带通滤波器，通过有源带通滤波器将超声波频率控制在40kHZ，滤掉其他频率的干扰波，经过有源带通滤波器过滤的超声波传输至计数器，计数器分别记录每个超声波接收头在1s内接收到的超声波个数，每个计数器记录的超声波个数以DCB码的形式传送到数值比较器中进行比较，在数值比较器中判断1s时间内每个超声波接收头接收到的超声波个数是否相同，通过数值比较器的判断结果来判断每个超声波接收头是否存在丢波现象，若没有丢波则判定这组信号为可靠信号。The ultrasonic receiving device is a group of ultrasonic receiving heads distributed on the acrylic plate according to certain rules, as shown in Figure 4, the required rule is that the distance from the geometric center of the figure formed by all the ultrasonic receiving heads to each ultrasonic receiving head is equal , the device is now distributed on the four corners of the acrylic board in a square shape. The ultrasonic receiving head group includes an ultrasonic receiving head, an active band-pass filter, a counter and a numerical comparator; each ultrasonic receiving head is connected in series with the active band-pass filter respectively, and the active band-pass filter is connected in series with the counter , The active filter circuit is shown in Figure 6, all the counters are connected to the numerical comparator, and the circuit diagram of the ultrasonic waveform counter is shown in Figure 7. In the ultrasonic positioning, the frequency of the ultrasonic wave emitted by the ultrasonic transmitting probe is about 38kHZ ~ 40kHZ, the ultrasonic receiving head receives the ultrasonic signal, the ultrasonic signal passes through the active band-pass filter, and the ultrasonic frequency is controlled at 40kHZ through the active band-pass filter , to filter out interference waves of other frequencies, the ultrasonic wave filtered by the active band-pass filter is transmitted to the counter, and the counter records the number of ultrasonic waves received by each ultrasonic receiving head within 1s, and the number of ultrasonic waves recorded by each counter is The form of the DCB code is sent to the numerical comparator for comparison. In the numerical comparator, it is judged whether the number of ultrasonic waves received by each ultrasonic receiving head is the same within 1s, and the judgment result of the numerical comparator is used to judge whether each ultrasonic receiving head Whether there is a wave loss phenomenon, if there is no wave loss, it is determined that this group of signals is a reliable signal.
所述超声波接收装置中的接收探头选用基于CX20106a的超声波接收模块，无线模块选用NRF24L01射频模块，图4为本装置选用的超声波接收模块电路图，图5为本装置选用的射频模块电路图。所述有源带通滤波器由低通RC环节和高通RC环节组成，如图6所示。其中电阻R1、电阻R2、电阻R3、电阻R4均为1.2kΩ，电容C1与电容C2均为3300pF，有源带通滤波器的输出频率为40.2kHz，超声波发射器发出的超声波的频率为38kHz～40kHz，通过有源带通滤波器滤波的输出频率符合超声波频率范围的要求。所述计数器为CD4518双BCD加法计数器，所述数值比较器为74LS85BCD数值比较器。本装置采用四个超声波接收头，四个超声波接收头与对应的四个有源带通滤波器相连，其中，每个超声波接收头的INT端口分别与对应的有源带通滤波器的Ui端口连接；四个有源带通滤波器与对应的四个计数器连接，每个有源带通滤波器的Uo端口与对应的计数器的CP1端口连接。每个计数器的EN1端口与外接直流电源相连，由外接直流电源提供5V直流电压。全部计数器中，和第一个超声波探头连接的计数器有三路输出信号A3、A2、A1，这三路输出信号相同，另外与其他3个超声波探头相连的计数器分别输出1路信号，分别由三个计数器的B3、B2、B1端口输出；全部计数器的输出信号输出至数值比较器，如图7所示。The receiving probe in the ultrasonic receiving device selects the ultrasonic receiving module based on CX20106a, and the wireless module selects the NRF24L01 radio frequency module, and Fig. 4 is a circuit diagram of the ultrasonic receiving module selected by the device, and Fig. 5 is a circuit diagram of the radio frequency module selected by the device. The active bandpass filter is composed of a low-pass RC link and a high-pass RC link, as shown in FIG. 6 . The resistance R1, resistance R2, resistance R3 and resistance R4 are all 1.2kΩ , the capacitance C1 and capacitance C2 are both 3300pF , the output frequency of the active bandpass filter is 40.2kHz, and the frequency of the ultrasonic wave emitted by the ultrasonic transmitter is 38kHz ~40kHz, the output frequency filtered by the active band-pass filter meets the requirements of the ultrasonic frequency range. The counter is a CD4518 double BCD addition counter, and the numerical comparator is a 74LS85BCD numerical comparator. This device adopts four ultrasonic receiving heads, and the four ultrasonic receiving heads are connected with the corresponding four active band-pass filters, wherein, the INT port of each ultrasonic receiving head is respectively connected with the Ui port of the corresponding active band-pass filter Connection; four active band-pass filters are connected to the corresponding four counters, and the Uo port of each active band-pass filter is connected to the CP1 port of the corresponding counter. The EN1 port of each counter is connected with an external DC power supply, which provides 5V DC voltage. Among all the counters, the counter connected to the first ultrasonic probe has three output signals A3, A2, and A1, and the three output signals are the same. In addition, the counters connected to the other three ultrasonic probes output one signal respectively, which are respectively composed of three The B3, B2, and B1 ports of the counter are output; the output signals of all counters are output to the numerical comparator, as shown in Figure 7.
所述超声波接收装置中的接收探头选用基于CX20106a的超声波接收模块，无线模块选用NRF24L01射频模块，图4为本装置选用的超声波接收模块电路图，图5为本装置选用的射频模块电路图。图3中，A为单片机，B为2号舵机组，C为射频模块，D为超声波接收装置，E为超声波接收探头组，F为数值比较器，自动跟随超声波接收系统中的单片机的P1B2～4引脚分别和四个超声波接收探头组中数值比较器的三个OAGTB引脚相连；单片机的PW1、PW2引脚分别和2号舵机组的两个舵机信号线相连。单片机的PG6～7和PB3～5引脚依次和射频模块的NRF CE、NRF CS、NRF IRQ、NRF SCK、NRF MOSI、NRF SCK引脚相连。The receiving probe in the ultrasonic receiving device selects the ultrasonic receiving module based on CX20106a, and the wireless module selects the NRF24L01 radio frequency module, and Fig. 4 is a circuit diagram of the ultrasonic receiving module selected by the device, and Fig. 5 is a circuit diagram of the radio frequency module selected by the device. In Figure 3, A is the single-chip microcomputer, B is the No. 2 steering unit, C is the radio frequency module, D is the ultrasonic receiving device, E is the ultrasonic receiving probe group, and F is the numerical comparator, which automatically follows the P1B2～ of the single-chip microcomputer in the ultrasonic receiving system. The 4 pins are respectively connected to the three OAGTB pins of the numerical comparator in the four ultrasonic receiving probe groups; the PW1 and PW2 pins of the single-chip microcomputer are respectively connected to the two steering gear signal lines of the No. 2 steering group. The PG6~7 and PB3~5 pins of the microcontroller are connected to the NRF CE, NRF CS, NRF IRQ, NRF SCK, NRF MOSI, and NRF SCK pins of the radio frequency module in turn.
所述AGV运动系统包括电机控制装置、电源控制装置及车体，所述车体经由电机控制装置控制并由电源控制装置供电完成运动；所述车体包括车架、和位于车架下面的全向轮；所述电机控制装置包括与全向轮数目相同的电机、编码器以及与电机数目对应的驱动器，所述驱动器将主控单片机传输的运动命令解码成各全向轮的转速，转速通过CAN总线连接传输至各全向轮的电机，电机与全向轮相连带动全向轮运动决定AGV运动速度与方向，编码器与电机相连测电机实际转速，实际转速传输至驱动器，若编码器所测全向轮实际速度与命令有所偏差，则驱动器对各全向轮的转速作出调整；所述电源控制模块包括锂电池及电源总线接口电路，锂电池经由电源总线接口电路调压至主控单片机所需压值后，对主控单片机以及超声波发送系统、避障系统、AGV运动系统进行供电；The AGV motion system includes a motor control device, a power control device and a car body, the car body is controlled by the motor control device and powered by the power control device to complete the movement; to the wheel; the motor control device includes the same motor as the number of omni-directional wheels, an encoder and a driver corresponding to the number of motors, and the driver decodes the motion command transmitted by the main control single-chip microcomputer into the rotating speed of each omni-directional wheel, and the rotating speed passes through The CAN bus is connected to the motors of the omnidirectional wheels. The motors are connected to the omnidirectional wheels to drive the movement of the omnidirectional wheels to determine the movement speed and direction of the AGV. The encoder is connected to the motor to measure the actual speed of the motor, and the actual speed is transmitted to the driver. If there is a deviation between the actual speed of the measured omnidirectional wheels and the command, the driver will adjust the rotational speed of each omnidirectional wheel; the power control module includes a lithium battery and a power bus interface circuit, and the lithium battery is regulated to the main controller via the power bus interface circuit. After the required voltage value of the single-chip microcomputer, power is supplied to the main control single-chip microcomputer, ultrasonic transmission system, obstacle avoidance system, and AGV motion system;
所述AGV运动系统中电机选用响应快、功率大、扭矩强的进口直流减速电机，驱动器为四轴驱动器，车轮选用更加灵活可实现保证位姿不变移动的QMA10全向轮，通过四个轮子速度配合完成AGV小车位姿不变情况下任意角度转向。The motor in the AGV motion system uses an imported DC gear motor with fast response, high power and strong torque. The driver is a four-axis drive. The speed is coordinated to complete the steering at any angle under the condition that the AGV trolley remains unchanged.
所述避障系统包括扫描仪、转换模块，主控单片机将环境检测命令经转换模块转化成以太网格式输出给扫描仪，扫描仪完成一次扫描后，将周围环境数据经转换模块将其转化成串口格式并传输至主控单片机，主控单片机接收到扫描仪检测到的周围环境信息后，将其进行坐标变换处理，用于建立以AGV小车所在位置为中心的局部栅格电子地图，得到局部栅格电子地图后通过A*算法运算得到局部避障路线。The obstacle avoidance system includes a scanner and a conversion module. The main control single-chip microcomputer converts the environmental detection command through the conversion module into an Ethernet format and outputs it to the scanner. After the scanner completes a scan, the surrounding environment data is converted into The serial port format is transmitted to the main control single-chip microcomputer. After the main control single-chip microcomputer receives the surrounding environment information detected by the scanner, it performs coordinate transformation processing to establish a local grid electronic map centered on the position of the AGV car, and obtains a local After the grid electronic map, the local obstacle avoidance route is obtained through the A* algorithm operation.
所述避障系统中的扫描仪选用SICK_TIM_351型激光扫描仪，用以实现对AGV小车周围障碍物的检测，其检测数据用于对局部栅格电子地图的更新。选取的动态避障范围为以AGV小车为中心半径4m的范围，A*算法采取栅格地图法，栅格地图的分度值为0.05m，搜索的地图总栅格数为25600个。主控单片机所采用型号提供了以太网接口，主控单片机与串口-WIFI模块相连，串口-WIFI模块另一端与激光扫描仪相连，通过串口-WIFI模块将信号转换实现主控单片机和避障系统的通讯，这款串口-WIFI模块配置简单、接口丰富、运行稳定。The scanner in the obstacle avoidance system uses a SICK_TIM_351 laser scanner to detect obstacles around the AGV trolley, and its detection data is used to update the local grid electronic map. The selected dynamic obstacle avoidance range is the radius of 4m centered on the AGV car. The A* algorithm adopts the grid map method. The division value of the grid map is 0.05m, and the total grid number of the searched map is 25600. The model of the main control MCU provides an Ethernet interface, the main control MCU is connected to the serial port-WIFI module, and the other end of the serial port-WIFI module is connected to the laser scanner, and the signal conversion is realized through the serial port-WIFI module to realize the main control MCU and the obstacle avoidance system communication, this serial port-WIFI module has simple configuration, rich interfaces and stable operation.
所述AGV系统与上位机之间还安装有两个串口-WIFI模块，用以实现数据的实时传输。又一个创新点，优点Two serial port-WIFI modules are also installed between the AGV system and the host computer to realize real-time data transmission. Another innovation point, advantages
所述AGV小车还需要安装两个串口-WIFI模块与主控单片机连接，用以实现与上位机系统之间数据的实时传输，当定位系统完成对AGV小车的定位后，主控单片机通过两个串口-WIFI模块将接收到的定位信息发送至上位机，在上位机中电子地图模块实时显示AGV小车位置。The AGV car also needs to install two serial port-WIFI modules to connect with the main control single-chip microcomputer to realize the real-time transmission of data between the upper computer system. When the positioning system completes the positioning of the AGV car, the main control single-chip computer passes two The serial port-WIFI module sends the received positioning information to the host computer, and the electronic map module in the host computer displays the position of the AGV car in real time.
所述主控单片机采用支持多功能的STM32F407VET6单片机，单片机的PA2、PA3引脚与串口-WIFI模块相连，完成主控单片机与上位机的通讯，单片机的PC10、PC11引脚与串口-WIFI模块相连，完成主控单片机与SICK_TIM_351扫描仪的数据传输，单片机的PA0、PA1引脚与M6050六轴模块相连，使主控单片机接收AGV小车的位姿数据，单片机的PB10、PB11引脚与一号舵机组中两个舵机相连，使主控单片机控制超生波发射的方向，单片机的PC6、PC7引脚与超声波发射探头相连，使主控单片机控制发射超声波信号，单片机的PB8、PB9引脚通过CAN总线向AGV运动系统的驱动器发送命令，单片机的PA5～7、PA12引脚与无线通信模块相连，接收超声波定位数据至主控单片机。The main control single-chip microcomputer adopts the STM32F407VET6 single-chip microcomputer that supports multiple functions, and the PA2 and PA3 pins of the single-chip microcomputer are connected with the serial port-WIFI module to complete the communication between the main control single-chip microcomputer and the host computer, and the PC10 and PC11 pins of the single-chip microcomputer are connected with the serial port-WIFI module , to complete the data transmission between the main control MCU and the SICK_TIM_351 scanner, the PA0 and PA1 pins of the MCU are connected to the M6050 six-axis module, so that the main control MCU receives the pose data of the AGV car, and the PB10 and PB11 pins of the MCU are connected to the No. 1 rudder The two steering gears in the unit are connected so that the main control single-chip microcomputer controls the direction of ultrasonic emission. The PC6 and PC7 pins of the single-chip microcomputer are connected with the ultrasonic transmitting probe, so that the main control single-chip microcomputer controls the emission of ultrasonic signals. The PB8 and PB9 pins of the single-chip microcomputer pass CAN The bus sends commands to the driver of the AGV motion system, and the PA5-7 and PA12 pins of the single-chip microcomputer are connected to the wireless communication module to receive ultrasonic positioning data to the main control single-chip microcomputer.
自动跟随超声波接收系统中由两台舵机构成的二号舵机组可更换为由闭环PID系统控制的两直流无刷电机，由两直流无刷电机控制超声波接收头所在面板的转向，并由闭环PID系统控制实际角度，若偏转角度与目标角度不同则通过闭环PID系统反馈系统进行调整，同时将陀螺仪与单片机以导线相连，通过陀螺仪采集超声波接收头与垂直方向的角度数据，陀螺仪将角度数据传输至自动跟随超声波接收系统中的单片机进行定位计算。舵机组及控制可由闭环PID控制的两直流无刷电机代替，可做到角度更精确更快速、反应更灵敏更灵活。The second rudder unit composed of two steering gears in the automatic follow ultrasonic receiving system can be replaced by two DC brushless motors controlled by a closed-loop PID system. The two DC brushless motors control the steering of the panel where the ultrasonic receiving head is located, and the closed-loop The PID system controls the actual angle. If the deflection angle is different from the target angle, it will be adjusted through the closed-loop PID system feedback system. At the same time, the gyroscope is connected to the single-chip microcomputer with a wire, and the angle data between the ultrasonic receiving head and the vertical direction is collected by the gyroscope. The gyroscope will The angle data is transmitted to the single-chip microcomputer in the automatic follow-up ultrasonic receiving system for positioning calculation. The steering unit and control can be replaced by two DC brushless motors controlled by closed-loop PID, which can achieve more accurate and faster angles, more sensitive and flexible responses.
所述自动跟随超声波接收系统中的二号舵机组选用两个s3003futaba舵机构成可完成挠性运动的二维云台，实现全方位超声波定位与跟踪，解决了传统超声波定位系统盲区过大的问题。The No. 2 rudder unit in the automatic following ultrasonic receiving system uses two s3003futaba rudders to form a two-dimensional pan-tilt that can complete flexible motion, realize omni-directional ultrasonic positioning and tracking, and solve the problem of excessive blind areas of traditional ultrasonic positioning systems .
优点：自动跟随超声波接收系统中的超声波接收装置由闭环PID系统控制两直流无刷电机可更加灵敏并且精度更高的控制超声波接收头的角度，精确快速校正超声波接收头实际角度与目标角度的偏差，使自动跟随超声波接收系统完成定位参数的误差更小。Advantages: Automatically follow the ultrasonic receiving device in the ultrasonic receiving system. The closed-loop PID system controls the two DC brushless motors, which can control the angle of the ultrasonic receiving head more sensitively and with higher precision, and accurately and quickly correct the deviation between the actual angle of the ultrasonic receiving head and the target angle. , so that the error of automatically following the ultrasonic receiving system to complete the positioning parameters is smaller.
所述有源带通滤波器由低通RC环节和高通RC环节组成，如图6所示。其中电阻R1、电阻R2、电阻R3、电阻R4均为1.2kΩ，电容C1与电容C2均为3300pF，有源带通滤波器的输出频率为40.2kHz，The active bandpass filter is composed of a low-pass RC link and a high-pass RC link, as shown in FIG. 6 . The resistors R1, R2, R3, and R4 are all 1.2kΩ, the capacitors C1 and C2 are both 3300pF, and the output frequency of the active bandpass filter is 40.2kHz.
本发明实施例中用于仓储物流的平面运动小车(AGV小车)，首先操作员进入上位机中参数设置模块，设置电子地图格局、尺寸，经由处理模块得到电子地图的最优路径，然后进入串口配置模块配置接口参数，并在命令传输模块发送已设参数数据内容。In the embodiment of the present invention, for the planar motion trolley (AGV trolley) used for warehousing and logistics, the operator first enters the parameter setting module in the host computer, sets the pattern and size of the electronic map, obtains the optimal path of the electronic map through the processing module, and then enters the serial port The configuration module configures the interface parameters, and sends the set parameter data content in the command transmission module.
上位机系统通过无线通信模块将数据传送给主控单片机，主控单片机将得到的最短路径参数数据，结合定位系统与避障系统数据，将处理结果发送至电机控制装置，电机控制装置通过控制电机转速控制AGV小车运动。The upper computer system transmits the data to the main control single-chip microcomputer through the wireless communication module. The shortest path parameter data obtained by the main control single-chip microcomputer, combined with the data of the positioning system and the obstacle avoidance system, sends the processing results to the motor control device, and the motor control device controls the motor. The speed controls the movement of the AGV trolley.
超声波定位过程为：The ultrasonic positioning process is:
a.AGV系统的主控单片机接收自动跟随超声波接收系统中无线通信模块发送的数据信号；a. The main control microcontroller of the AGV system receives and automatically follows the data signal sent by the wireless communication module in the ultrasonic receiving system;
a.自动跟随超声波接收系统中无线模块向AGV系统上超声波发送系统发出一列数据信号，AGV系统的主控单片机通过无线通信模块接收自动跟随超声波接收系统中无线模块发送的数据信号；a. Automatically follow the wireless module in the ultrasonic receiving system to send a series of data signals to the ultrasonic sending system on the AGV system, and the main control microcontroller of the AGV system receives the data signal sent by the wireless module in the ultrasonic receiving system automatically through the wireless communication module;
b.主控单片机开启超声波发射定时器，令超声波发送系统发射超声波信号。同时自动跟随超声波接收系统中无线模块自动应答，自动跟随超声波接收系统中单片机中的定时器开启计时功能进行超声波接收计时；b. The main control single-chip microcomputer starts the ultrasonic transmission timer, so that the ultrasonic transmission system transmits ultrasonic signals. At the same time, it automatically responds automatically with the wireless module in the ultrasonic receiving system, and automatically follows the timer in the single-chip microcomputer in the ultrasonic receiving system to start the timing function for ultrasonic receiving timing;
c.超声波接收装置接收超声波发送系统发射的超声波信号，通过超声波接收头接收到的超声波信号经超声波接收装置中的有源带通滤波器滤波，在超声波定位中，超声波发射探头发出的超声波的频率大约为38kHZ～40kHZ，通过有源带通滤波器去除不在超声波发送系统发射的40kHz超声波阈值内的杂波，并且通过每个超声波接收探头组中的CD4518双BCD加法计数器对每秒所接收到的超声波个数进行计数，每个超声波接收头接收到的超声波个数被传送到74LS85BCD数值比较器中进行比较，全部计数器中，和第一个超声波探头连接的计数器有三路输出信号A3、A2、A1，这三路输出信号相同，另外与其他3个超声波探头相连的计数器分别输出1路信号，分别由三个计数器的B3、B2、B1端口输出，信号输出至数值比较器。以与第一个超声波探头相连的计数器的3路输出信号为准，分别与和其余三个超声波探头相连的计数器输出的信号进行对比，若对比结果与表1所示的真值表最后一行相符，则证明4个探头接收的波形数目相同，则这组信号判定为可靠信号，若与其余行相符，则代表4个探头接收的波形数目不相同，即存在丢波，则信号舍弃不进行操作；可靠信号被传输至单片机，单片机中的定时器完成计时，单片机根据超声波接收装置中每个超声波接收头的响应时间以100ms一次的频率计算每个超声波接收头所测的距离；单片机根据每个超声波接收头所测的距离判断当前超声波信号的可靠性，即再次判断每个超声波接收头所接收到是否为同一列超声波信号，并且同一列超声波信号每个超声波接收头都响应，若根据每个超声波接收头所测的距离进行几何计算，到达所有超声波接收头几何平面中点的误差不超过所设阈值，即为可靠，得到每个超声波接收头接收到的可靠距离数据；c. The ultrasonic receiving device receives the ultrasonic signal transmitted by the ultrasonic sending system, and the ultrasonic signal received by the ultrasonic receiving head is filtered by the active band-pass filter in the ultrasonic receiving device. In ultrasonic positioning, the frequency of the ultrasonic wave emitted by the ultrasonic transmitting probe It is about 38kHZ ~ 40kHZ. The active band-pass filter is used to remove the clutter that is not within the threshold of the 40kHz ultrasonic wave emitted by the ultrasonic sending system, and the CD4518 dual BCD addition counter in each ultrasonic receiving probe group is used to count the clutter received per second. The number of ultrasonic waves is counted, and the number of ultrasonic waves received by each ultrasonic receiving head is transmitted to the 74LS85BCD numerical comparator for comparison. Among all counters, the counter connected to the first ultrasonic probe has three output signals A3, A2, and A1 , the three output signals are the same, and the counters connected to the other three ultrasonic probes output one signal respectively, which are respectively output by the B3, B2, and B1 ports of the three counters, and the signals are output to the numerical comparator. Based on the three output signals of the counter connected to the first ultrasonic probe, compare them with the output signals of the counters connected to the other three ultrasonic probes respectively. If the comparison result is consistent with the last line of the truth table shown in Table 1 , it proves that the number of waveforms received by the four probes is the same, and this group of signals is judged as a reliable signal. If it matches the rest of the lines, it means that the number of waveforms received by the four probes is different, that is, there is wave loss, and the signal is discarded and no operation is performed. The reliable signal is transmitted to the single-chip microcomputer, and the timer in the single-chip microcomputer completes timing, and the single-chip microcomputer calculates the distance measured by each ultrasonic receiving head with a frequency of 100ms according to the response time of each ultrasonic receiving head in the ultrasonic receiving device; The distance measured by the ultrasonic receiving head judges the reliability of the current ultrasonic signal, that is, judges again whether each ultrasonic receiving head receives the same row of ultrasonic signals, and each ultrasonic receiving head responds to the same row of ultrasonic signals. The distance measured by the ultrasonic receiving head is geometrically calculated, and the error of reaching the midpoint of the geometric plane of all ultrasonic receiving heads does not exceed the set threshold, which is reliable, and the reliable distance data received by each ultrasonic receiving head is obtained;
d.自动跟随超声波接收系统中单片机根据每个超声波接收头接收到的可靠距离数据结合当前二号舵机组角度以及接收系统所处高度，得出AGV小车的超声波定位数据，超声波接收装置根据当前定位数据通过脉冲宽度调制PWM控制自动跟随超声波接收系统中二号舵机组转向对准AGV小车；d. The single-chip microcomputer in the automatic follow-up ultrasonic receiving system obtains the ultrasonic positioning data of the AGV car according to the reliable distance data received by each ultrasonic receiving head combined with the angle of the current No. 2 rudder unit and the height of the receiving system. The ultrasonic receiving device is based on the current positioning The data automatically follows the No. 2 steering unit in the ultrasonic receiving system and aligns it with the AGV car through pulse width modulation PWM control;
e.自动跟随超声波接收系统通过无线模块发送超声波定位数据，超声波发送系统中无线通信模块接收到超声波定位数据传送至AGV系统的主控单片机，主控单片机判断可靠性，若为0，则丢弃，并连续10次存储超声波定位数据，去掉其中最大值、最小值后取平均，得出最终超声波定位数据；e. Automatically follow the ultrasonic receiving system to send ultrasonic positioning data through the wireless module. The wireless communication module in the ultrasonic sending system receives the ultrasonic positioning data and sends it to the main control single-chip microcomputer of the AGV system. The main control single-chip microcomputer judges the reliability. If it is 0, it is discarded. And store the ultrasonic positioning data for 10 consecutive times, remove the maximum and minimum values and take the average to obtain the final ultrasonic positioning data;
f.主控单片机结合最终超声波定位数据以及位姿采集器所传输当前AGV姿态数据处理得到超声波发送系统中一号舵机组需要转动的对准角度；f. The main control single-chip microcomputer combines the final ultrasonic positioning data and the current AGV attitude data transmitted by the attitude collector to process and obtain the alignment angle that the No. 1 rudder unit needs to rotate in the ultrasonic transmission system;
g.主控单片机将对准角度传输至一号舵机组，超声波发送系统中的一号舵机组完成转动，完成对发射超声波信号时AGV小车所在点的定位及超声波发送系统与自动跟随超声波接收系统对准，进入循环对AGV小车新的当前位置进行定位。g. The main control MCU transmits the alignment angle to the No. 1 rudder unit, and the No. 1 rudder unit in the ultrasonic sending system completes the rotation, and completes the positioning of the AGV trolley when the ultrasonic signal is transmitted, the ultrasonic sending system and the automatic follow-up ultrasonic receiving system Align and enter the cycle to locate the new current position of the AGV car.
避障过程为：主控单片机设定避障系统的避障范围为4m，A*算法采用栅格地图法，栅格电子地图的分度值为0.05m，栅格电子地图总栅格数为25600个。The obstacle avoidance process is as follows: the main control microcontroller sets the obstacle avoidance range of the obstacle avoidance system to 4m, the A* algorithm adopts the grid map method, the division value of the grid electronic map is 0.05m, and the total grid number of the grid electronic map is 25600.
a.主控单片机将环境检测命令经以太网模块转化成以太网格式输出给扫描仪。a. The master microcontroller converts the environmental detection command into Ethernet format and outputs it to the scanner through the Ethernet module.
b.扫描仪完成一次扫描后，将环境数据经转换模块将其转化成串口格式并传输至主控单片机。b. After the scanner completes a scan, the environmental data is converted into a serial port format by the conversion module and transmitted to the main control microcontroller.
c.主控单片机在接收到扫描仪检测到的环境信息后，将其进行坐标变换处理，用于建立以AGV小车为中心的局部栅格电子地图。c. After the main control microcontroller receives the environmental information detected by the scanner, it performs coordinate transformation processing to establish a local grid electronic map centered on the AGV car.
d.在构建出的局部栅格电子地图基础上，运行算法进行快速局部路径规划。根据规划路径，确定AGV小车的局部避障路线，通过每次扫描更新局部栅格电子地图，循环上述步骤完成运行过程中对环境中障碍物的躲避。d. On the basis of the constructed local grid electronic map, run the algorithm for fast local path planning. According to the planned path, determine the local obstacle avoidance route of the AGV car, update the local grid electronic map through each scan, and cycle the above steps to complete the avoidance of obstacles in the environment during operation.
表1为数值比较器真值表。(74LS85真值表)Table 1 is the truth table of the numerical comparator. (74LS85 truth table)
此外，以上所述的实施例仅为本发明的个别实施例，并不用以限制本发明的保护范围，凡采用等同替换或等效变换形成的技术方案，以及符合本发明保护主题思路的技术方案，均在本发明要求的保护范围内。In addition, the above-mentioned embodiments are only individual embodiments of the present invention, and are not intended to limit the protection scope of the present invention. All technical solutions formed by equivalent replacement or equivalent transformation, and technical solutions that conform to the protection subject of the present invention , are all within the scope of protection required by the present invention.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711083579.2ACN107907861A (en) | 2017-11-07 | 2017-11-07 | A kind of flooring transport vehicle ultrasonic wave positioning control system and its control method |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711083579.2ACN107907861A (en) | 2017-11-07 | 2017-11-07 | A kind of flooring transport vehicle ultrasonic wave positioning control system and its control method |
| Publication Number | Publication Date |
|---|---|
| CN107907861Atrue CN107907861A (en) | 2018-04-13 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711083579.2APendingCN107907861A (en) | 2017-11-07 | 2017-11-07 | A kind of flooring transport vehicle ultrasonic wave positioning control system and its control method |
| Country | Link |
|---|---|
| CN (1) | CN107907861A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108549386A (en)* | 2018-05-22 | 2018-09-18 | 汇专科技集团股份有限公司 | AGV trolleies managing and control system and method |
| CN109050693A (en)* | 2018-07-11 | 2018-12-21 | 济南大学 | It is a kind of with the step climbing-up device and its implementation that follow function |
| CN109540152A (en)* | 2019-01-18 | 2019-03-29 | 河北科技大学 | Ultrasonic probe, ultrasonic receiver, ultrasonic transmission device and indoor ultrasonic locating system |
| CN109665464A (en)* | 2018-12-24 | 2019-04-23 | 宁波如意股份有限公司 | A kind of method and system that movable type fork truck automatically tracks |
| CN109946650A (en)* | 2019-04-12 | 2019-06-28 | 扬州市职业大学(扬州市广播电视大学) | A positioning system and method for wireless synchronous transceiver separation |
| CN110262501A (en)* | 2019-06-27 | 2019-09-20 | 广东志邦速运供应链科技有限公司 | A kind of intelligence cargo transport system and method |
| CN111243315A (en)* | 2020-01-16 | 2020-06-05 | 河北科技大学 | Vehicle positioning control system and control method |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5280457A (en)* | 1992-07-31 | 1994-01-18 | The Administrators Of The Tulane Educational Fund | Position detecting system and method |
| CN102961213A (en)* | 2012-11-05 | 2013-03-13 | 柴永斌 | Blind person environment perception method and blind person environment perception system |
| CN105115498A (en)* | 2015-09-30 | 2015-12-02 | 长沙开山斧智能科技有限公司 | Robot location navigation system and navigation method |
| CN105911554A (en)* | 2016-06-06 | 2016-08-31 | 清华大学 | Target identification method based on ultrasonic sensor array |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5280457A (en)* | 1992-07-31 | 1994-01-18 | The Administrators Of The Tulane Educational Fund | Position detecting system and method |
| CN102961213A (en)* | 2012-11-05 | 2013-03-13 | 柴永斌 | Blind person environment perception method and blind person environment perception system |
| CN105115498A (en)* | 2015-09-30 | 2015-12-02 | 长沙开山斧智能科技有限公司 | Robot location navigation system and navigation method |
| CN105911554A (en)* | 2016-06-06 | 2016-08-31 | 清华大学 | Target identification method based on ultrasonic sensor array |
| Title |
|---|
| 于培培: "全方位移动式AGV系统及导航避障关键技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》* |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108549386A (en)* | 2018-05-22 | 2018-09-18 | 汇专科技集团股份有限公司 | AGV trolleies managing and control system and method |
| CN109050693A (en)* | 2018-07-11 | 2018-12-21 | 济南大学 | It is a kind of with the step climbing-up device and its implementation that follow function |
| CN109050693B (en)* | 2018-07-11 | 2024-02-02 | 济南大学 | Step-up device with following function and implementation method thereof |
| CN109665464A (en)* | 2018-12-24 | 2019-04-23 | 宁波如意股份有限公司 | A kind of method and system that movable type fork truck automatically tracks |
| CN109540152A (en)* | 2019-01-18 | 2019-03-29 | 河北科技大学 | Ultrasonic probe, ultrasonic receiver, ultrasonic transmission device and indoor ultrasonic locating system |
| CN109946650A (en)* | 2019-04-12 | 2019-06-28 | 扬州市职业大学(扬州市广播电视大学) | A positioning system and method for wireless synchronous transceiver separation |
| CN109946650B (en)* | 2019-04-12 | 2023-09-15 | 扬州市职业大学(扬州市广播电视大学) | Positioning system and method for wireless synchronous receiving and transmitting separation |
| CN110262501A (en)* | 2019-06-27 | 2019-09-20 | 广东志邦速运供应链科技有限公司 | A kind of intelligence cargo transport system and method |
| CN111243315A (en)* | 2020-01-16 | 2020-06-05 | 河北科技大学 | Vehicle positioning control system and control method |
| Publication | Publication Date | Title |
|---|---|---|
| CN107907861A (en) | A kind of flooring transport vehicle ultrasonic wave positioning control system and its control method | |
| CN108037754A (en) | A kind of flooring transport vehicle ultrasonic wave positioning control system | |
| CN107861511A (en) | A kind of control device and its control method for ground handling trolley | |
| EP3603372A1 (en) | Moving robot, method for controlling the same, and terminal | |
| CN102331783B (en) | Autopilot for indoor airship | |
| WO2019109635A1 (en) | Method and chip for monitoring pet on the basis of robot employing grid map | |
| CN112066989A (en) | Indoor AGV automatic navigation system and method based on laser SLAM | |
| CN107728624A (en) | A kind of control device for ground handling trolley | |
| CN105682047A (en) | UWB-based indoor mobile robot navigation and positioning system | |
| CN105425801A (en) | Intelligent cleaning robot based on advanced path programming technology and cleaning method thereof | |
| WO2019037668A1 (en) | Self-moving robot and travel method thereof, and obstacle distribution display method | |
| CN105093237A (en) | Detection device of barriers for unmanned plane and detection method thereof | |
| CN106441306A (en) | Intelligent life detecting robot with capabilities of independent positioning and map building | |
| CN207189673U (en) | Self-movement robot | |
| WO2018228203A1 (en) | Human and machine intelligence cooperation-based robot system having sweeping function | |
| CN104089649A (en) | System and method for collecting indoor environment data | |
| CN101763120A (en) | Robot locating device based on sound guidance and locating method thereof | |
| CN103076804B (en) | Based on the automatic guide vehicle of ultrasonic distance measuring apparatus, system and air navigation aid | |
| CN114355910A (en) | An autonomous mapping and navigation system and method for indoor robot based on Jetson Nano | |
| CN211786766U (en) | Livestock and poultry house inspection robot walking control device based on laser radar | |
| CN113311826A (en) | Automatic following system based on annular infrared array and working method thereof | |
| CN107702692B (en) | Unmanned aerial vehicle set surveying and mapping method based on AGV matrix | |
| CN207020537U (en) | A kind of unmanned plane | |
| CN108957464A (en) | Four dot matrix fish finding sonars | |
| CN106598066A (en) | Power line inspection four-rotor unmanned aerial vehicle autonomous obstacle avoidance system |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication | Application publication date:20180413 |