基于GPS和UWB混合定位的自动搜救机器人设计

游雪辉 张松

摘要：为解决人工搜救效率低且需耗费大量时间和资源的问题，提出基于GPS和UWB混合定位方式的自动搜救机器人方案。通过混合定位的方式，搜救机器人实现了给定目标位置后，进行自动路径规划和运动决策控制，自动从起点行驶至目标点，为搜救机器人提供高精度的定位信息。在行进过程中，搜救机器人使用无线数据传输方式将采集到的环境信息、图像信息等发送到搜救站。样机测试结果表明系统实现了预期的设计目标，提高了搜救效率，缩短了搜救时间。

关键词：GPS;UWB;搜救机器人;自动巡航

中图分类号：TP242文献标志码：A

文章编号：2095-5383（2019）02-0029-05

Abstract：At present，the search and rescue mission mainly relies on manual completion. However，manual search and rescue is often inefficient and takes a lot of time and resources.   An automatic search and rescue robot based on GPS and UWB hybrid positioning was proposed in this paper. Through hybrid positioning，the search and rescue robot performs automatic path planning and motion decision control，and automatically travels from the starting point to the target point. During the travel process，the search and rescue robot sends the collected environmental information，image information，etc. to the search and rescue station using wireless data transmission.  The prototype test results show that the system achieves the expected design goals，improves search and rescue efficiency，and shortens search and rescue time.

Keywords： Global Positioning System; Ultra Wide-Band; search and rescue robot; automatic cruise

我國是一个自然灾害频发的国家自然灾害发生后，搜救工作一般依靠人工进行，由于灾后现场环境的复杂性和危险性不利于灾后救援工作的开展，大面积受灾时人工搜救效率低，所以结合现阶段的搜救要求，设计一个能应用于搜救工作的自动搜救机器人显得至关重要。本文研究了搜救机器人的选型和系统设计方案，给出了自动搜救机器人的测试结果。

1 搜救机器人系统设计

1.1 搜救机器人设计与选型

设计之初，为保证搜救机器人能在复杂的路面自如前行，拟定了3个设计方案：

普通四轮搜救机器人：搜救工作需要经过的路面往往不同于城市道路，四轮搜救机器人在通过泥泞和较大坑洼路面时会发生打滑、高强度抖动，甚至翻车的情况，因此普通四轮搜救机器人不适于此设计。

六足机器人：六足机器人基于仿生学原理，模拟六足昆虫的机械结构，通过控制多个舵机实现机器人的姿态控制。六足机器人能够有效的克服路面不平坦而带来的阻碍搜救设备前进的问题。但六足机器人采用了多个独立控制的舵机，加大了控制难度，其行进速度较为缓慢，在实际救援过程中应尽量缩短救援时间[1-2]。

履带搜救机器人：履带搜救机器人通过电机带动齿轮，齿轮带动履带的方式使搜救机器人前行。若搜救机器人左侧履带的速度大于右侧履带的速度，搜救机器人实现右转弯;当右侧履带的速度大于左侧履带的速度，搜救机器人将实现左转弯的功能。履带车在山地能够有效越过普通障碍物，即使在泥泞和坑洼路段也能自如前行，不会发生破胎等事故，并且履带搜救机器人具有较快速度和较为容易的控制算法。因此本系统选用履带搜救机器人设计，以提高搜救效率，降低人工搜救强度[3-4]。

1.2 搜救机器人系统设计

搜救人员将搜救机器人放置在道路上，并在搜救机器人上设置目标点的位置信息。启动搜救机器人后，搜救机器人开始和全球定位系统（Global Positioning System，GPS）卫星进行通信，获取自己的位置信息。同时搜救机器人会初始化电子罗盘，拥有自身位置和目标点位置信息，根据两个位置进行计算，求得偏航角。对比电子罗盘获得的数据，搜救机器人旋转修正航向角度。当搜救机器人进入到超带宽（Ultra Wideband，UWB）[5]定位区域后，将开启UWB定位功能，通过GPS+UWB的混合定位方式继续获取自身位置信息。搜救机器人在行进过程中，位置会不断改变，在遇到障碍物时需要越过或者绕过障碍物，航向将改变，所以搜救机器人必须不断更新航行角度，以到达正确的位置。搜救机器人系统设计如图1所示。

2 搜救机器人系统各模块设计

搜救机器人设计框图如图2所示。

2.1 主控芯片的选型

主控芯片是搜救机器人的核心部件，屏幕LCD选用并行24位可变静态存储控制器（Flexible Static Memory Controller，FSMC）总线，触摸屏使用了集成电路总线（Inter-Integrated Circuit，IIC）接口，UWB模块和GPS模块使用了通用异步收发传输器（Universal Asynchronous Receiver/Transmitter）接口，通过微控制器产生脉冲宽度调制（Pulse Width Modulation，PWM）信号驱动电机，编码器用以计算搜救机器人当前速度，MPU9250使用了IIC接口，4G无线通信模块使用了通用串行总线（Universal Serial Bus，USB）接口，针对以上外设资源，主控芯片必须具有FSMC总线接口、IIC接口、SPI接口、UART接口以及比较和捕获模块（用于产生PWM信号），根

据硬件资源配置和性格比的考虑，选择了STM32F767做主控芯片。

2.2 定位模块设计

本系统使用了GPS+UWB混合定位的模式。选用GPS+北斗双定位模块ATGM332D，此模块经过数据解算后直接将位置信息通过UART发送至搜救机器人主控模块，搜救机器人再对接收到的位置信息进行处理。UWB定位采用目前最流行的技术TDOA技术方案[6]。TDOA通过检测信号到达不同监测站的时间差，来计算信号源的位置进行定位，因为利用了时间差而非信号到达的绝对时间，TDOA降低了时间同步的要求。DOA的原理如图3所示：不同监测站将同一时间测得的同一信号获得的数据发送给监测站A，监测站A经过解算得到信号到达不同的监测站的时间差，将时间差转换为距离将得到3条双曲线，双曲线的交点为信号源的位置。

如果要建立一个三维空间坐标，至少需要3个参考点来建立另外一个点的方程，其建立的方程式为：

其中：（x，y，z）为被定位点位置;（xi，yi，zi）为空间中参考点的位置坐标;C为光速，3×108 m/s;ti为参考点位置的到达时间;t0为定位点发出信号的时间。

若已知3个参考点的坐标信息（

xi，yi，zi）以及t0，可通过解四元二次方程组得到被定位点的位置信息（x，y，z）。为提高四元二次方程组有解的概率，将参考点增加至4个，并选用方程组的最优解，由此提高被定位点位置的坐标精度。

主控模块使用Fang算法对UWB终端天线接收到的数据进行解算，并将解算的数据添加帧头、帧类型、帧编号、帧校验后通过UART发送到主控模块。

2.3 姿态捕获

主控制器需要不断地获取搜救机器人姿态数据，通过计算目标位置和当前位置的斜率以及当前方向获得转向角度。本系统获取搜救机器人姿态使用MPU9250，MPU9250内部集成陀螺仪、加速度传感器和电子罗盘、2个内部时钟源和一个锁相环（Phase Locked Loop，PLL）。把空间划分为X轴，Y轴和Z轴，MPU9250可以對3个轴的角速度、加速度以及磁感应强度进行测量。加速度传感器和电子罗盘具有较高频率的噪声，为了抑制噪声，在搜机器人系统中，使用MPU9250的电子罗盘来获取搜救机器人的方向，获取的方向数据通过IIC总线传输到微控制器。

2.4  电机驱动模块设计

单片机I/O口输出的最大电压为3.3 V，最大输出电流为15 mA，最大输出功率约为50 mW，I/O口直接带负载能力极弱，故需要增加驱动电路以提高其带负载能力。

PWM信号的频率越高，对输出的响应速度越快，频率越低对输出响应速度越慢。为了使输出响应速度快，电机工作稳定，主控模块产生一个10 kHz的PWM信号作为搜救机器人的控制信号。在一个周期内高电平持续时间决定了信号的有效值，即占空比的值越大PWM信号的有效值就越大，后级输出的功率越大。本系统中驱动电路选用TB6612，TB6612具有两路PWM输入和两路PWM输出信号，可以同时驱动两个电机，通过AIN1，AIN2，BIN1，BIN2引脚的置位或者复位控制电机的正转、反转及停止功能。将TB6612的输出引脚AO1、AO2和BO1，BO2分别连接到电机上，通过程序设置AIN1、AIN2和BIN1、BIN2及2个输入PWM信号的占空比，便能实现搜救机器人的运动状态。TB6612控制真值表如表1所示。

驱动电路需要单片机输出2组PWM（PWMA、PWMB），同时还需要输出4个控制信号AIN1、AIN2、AIN3、AIN4。驱动电路工作电压为12 V，内部的数字器件工作电压为5 V，驱动电路如图4所示。

2.5 4G无线通信模块

搜救机器人在行进过程中，需要将获取到的信息实时的发送回搜救站，搜救站也要通过无线网络下发指令给搜救机器人。因此需要一个能够实现远距离无线通信的电路，此设计中使用了华为ME909S-821 4G全网通无线通信模块。通过PCIe转USB后连接至搜救机器人。将搜救机器人各个部分获得的数据经过滤波、压缩、编码和打包后通过4G无线通信模块发送给搜救站，搜救站接收到搜救机器人发送的数据后对数据进行解压缩后，得到搜救机器人的数据。搜救站通过无线方式下发控制指令给搜救机器人，通信模块将接收到的控制指令传送给MCU，MCU通过分析和处理后，控制搜救机器人各个部件执行相应的操作，从而控制搜救机器人进行搜救工作。

2.6 搜救机器人原理图设计

在以上各模块分析的基础上，完成搜救机器人系统原理图如图5所示。

3 样机及性能测试

搜救机器人原理图设计完成后，按图6所示将对搜救机器人进行焊接和调试。各个模块连接完成并确认无误后，进行上电调试。使用上位机配置UWB的工作模式，将固定在搜救机器人上的UWB模块配置成定位模式，另外4个UWB模块配置成基站模式。配置成功后，将UWB基站模块进行顺序摆放，将UWB模块的串口通过USB转串口模块连接至电脑，使用串口调试助手能够查看到UWB模块通过串口发送的数据，这些数据中的有效信息为UWB定位模块的位置信息。将搜救机器人的UWB模块通过USB数据线连接至电脑，上位机将看到搜救机器人相对于基站的位置信息。移动搜救机器人的位置，在上位机中将看到定位点的位置也随之发生移动。通过系统调试后，能实现自动巡航与偏航计算，远距离无线数据传输与控制，实现搜救机器人样机的各项功能。搜救机器人样机及工作状态如图6所示。

4 结语

基于GPS和UWB混合定位的自动搜救机器人完成了GPS定位，UWB定位，搜救机器人姿态解算，航向角度计算与规划，远距离无线数据传输与控制，LCD屏显示数据的基本功能，搜救机器人履带转动实现了搜救机器人的转向与移动。通过样机验证，基本实现了GPS+UWB混合定位搜救机器人的各项功能。

参考文献：

[1]韩建海，赵书尚，李济顺.六足机器人行走步态的协调控制[J].机电工程，2004，21（4）：8-10.

[2] 柳天虹，姜树海.仿生六足机器人稳定性分析与仿真[J].计算机仿真，2013，30（12）：360-364.

[3] 罗建国，卜泽昊.具有类柔性结构的履带救援机器人越障分析[J].华北科技学院学报，2018（1）：55-61.

[4] 李秋生，闫小军.履带式煤矿救援机器人行走机构研究[J].煤炭技术，2017，36（10）：199-201.

[5] FONTANA R J.Recent system applications of short-pulse ultra-wideband （UWB） technology[J].IEEE Transactions on Microwave Theory and Techniques，2004，52（9）：2087-2104.

[6] 张新跃，沈树群.UWB超宽带无线通信技术及其发展前景[J].数据通信，2004（2）：9-12.