基于多传感器融合的全地形机器人系统设计

朱 颖，吴钰鑫，侯夏嘉，张亚婉，陈庆盛

(广州华立学院，广州 511325)

0 引言

小城镇规模快速扩大，但随着防洪排涝设施老旧、排涝能力不足等，提高了发生洪灾的可能性。灾后的环境经过重塑，使得搜救难度大大增加。以往的搜救机器人一般注重于救援行为上，可以辅助搜救人员进行搜救，却无法有效的提高实际的搜救效率[1]。

传统的洪涝搜救机器人多为气垫船的运动结构且运用单一传感器，反馈的信息有限。文献[2]提出了一种以ARM的水面机器人控制系统，该系统使用摄像头反馈现场信息，以GPS作为导航定位器，设计复杂、功耗高。文献[3]使用SolidWorks设计了一种轮船式搜救机器人，并按照设定好的航行轨迹进行巡逻，这种机器人能够在稳定的环境内进行工作，但不适应于复杂多样的灾后地形。文献[4]通过MSP430F147单片机设计低功耗的救援机器人，通过上位机可获取机器人的实时定位，但是缺少了环境信息的获取。在此背景下，针对灾后环境信息收集，利用惯性测量单元、轮式里程计及GPS融合数据作为定位框架，LORA扩频无线模块回传生命、空气及烟雾等环境数据给救援中心。结构上设计了履带气垫一体化运动机构。在积水环境下，机器人使用气垫船运动模式，可在积水中自由行进；待积水退去，机器人将气垫收起，切换履带运动模式。

1 系统总体设计

全地形搜救机器人通过多传感器模块获取环境信息，FPGA控制机器人自主行走和对信息进行处理。上位机下达机器人运动目标指令，机器人开始自主行走，轮式里程计和姿态传感器MPU6050获取机器人的位姿信息，主控器对位姿信息进行扩展卡尔曼滤波算法处理后，对机器人位姿进行预测，控制机器人自主稳定行走；同时生命探测仪探测遇险人员，当发现遇险人员，FPGA通过GPS北斗双模定位模块获取机器人的位置信息并通过LORA无线扩频模块发送给上位机并发出警报，使救援人员迅速开展救援行动；视觉传感器负责采集环境图像信息并回传给上位机，并通过空气质量检测仪实时监测灾区环境状况。

多传感器融合的全地形搜救机器人系统主要包括两部分：硬件系统和软件系统。系统硬件电路主要包含：EP4CE10-FPGA现场可编程开发平台、MPU6050惯性传感器、霍尔编码器、LORA无线扩频模块、GPS北斗双模定位模块、COMS摄像头、生命特征仪、MQ-2检测模块以及机器人的底盘驱动电路与电机。硬件系统框图如图1所示。

图1 系统总框图

2 硬件系统设计

2.1 主控器

全地形搜救机器人主控器需要对传感器采集到的信息进行处理并回传至上位机，同时还需要对机器人自主移动进行实时控制，要求主控器的处理速度快、接口丰富、通讯快速稳定。机器人以英特尔Cyclone系列的EP4CE10芯片作为整个系统的处理和控制中心，相比于ARM、DSP等处理器，FPGA的并行性及可重构性，更能满足数据复杂的机器人系统设计。板卡主要由FPGA、SDRAM和FLASH构成，EP4CE10采用FBGA进行封装，工作温度最高达85℃，器件运行速度最高达400 Mhz，拥有1万个逻辑单元，同时，还板载了先进的视频解码晶片、百兆网卡、环境光传感器和其他各类接口芯片，满足机器人在复杂灾后环境下应用的需求。

2.2 环境信息采集模块

该机器人环境采集模块包括：视觉传感器、空气质量检测传感器、生命探测传感器和环境定位模块。

全地形机器人通过视觉传感器获取灾后的环境图像传给上位机，为救援人员提供直观的灾后环境，以便救援人员对环境进行分析做出正确的救援计划。为获取清晰稳定的环境图像，机器人采用OV5640视觉传感器实现图像采集。OV5640作为单晶片图像感应器，支持自动曝光，其像素达到500 W，能实现最快90 fps的图像采集，且OV5640摄像头具有极强的扩展性及重构性，能为机器人的图像感知处理留有发展空间。

灾后环境可能充斥着氨气、硫化物及苯系蒸气等有害气体，机器人对空气质量进行监测可方便救援人员根据环境准备救援工具与防护。机器人采用MQ-2传感器对空气进行检测，该传感器可将硬件电路电导率的改变量换算为与危害气体含量相对应的输出信息。相比于MQ-135等传感器，MQ-2的精度、稳定性更高，能到达对环境气体检测的目的[5]。

洪灾常伴有房屋倒塌泥石流等灾难造成人员被困，机器人搭载生命探测仪对被困人员进行探测。生命探测仪采用HLK-LD1115H-24 G传感器，其主要由24 G人体存在传感器与热释传感器组成。人体存在传感器使用24 GHz频率的毫米波雷达对机器人附近人体进行检测，该模块除了多普勒人体检测雷达模块对大幅动作和微小肢体动作检测人体存在还可通过对微小呼吸进行累计探测人体。热释传感器通过红外信号检测，弥补人体存在传感器的不足，两者互补相比于单一的热释传感器，本系统的生命检测组合的检测精度及范围更高，能够高效对生命进行检测。

当机器人发现被困人员时，机器人迅速向上位机发出救援信号，并发送经度、纬度和高度位置信息，便于救援人员迅速准确定位开展救援行动，缩短救援时间。机器人采用ATK1218模块作为双定位传感器，该传感器板载S1216芯片，使用NMEA-0183作为通信协议，最大精确度达到2.5 mCEP，最大捕获追踪灵敏度为-165 dBm，是一种GPS/北斗双模定位模块[6]，满足机器人位置定位需求。环境信息架构图如图2所示。

图2 环境信息架构图

2.3 惯性-定位导航器

机器人自主定位导航系统主要由轮式里程计和惯性测量单元组成。

轮式里程计提供的位姿信息连续且较为稳定，其结构简单，主要使用安装在直流电机对尾部的霍尔编码器进行工作，由霍尔码盘和霍尔元件组成，编码器通过角位移或者非线性的角位移转换来确定转化信号为一个电信号。轮子转动时能够将角位移或者角速度转换成电数字脉冲，可以在短时间内较精确的测量到移动机器人的位姿[9]。

惯性测量单元IMU加速度计和陀螺仪，分别提供加速度和角速度[7]。IMU采集角速度与加速度信息通过积分运算获得载体的位置姿态信息，具有短时间内精度高、响应快速、可捕获载体快速运动信息、能够估计出载体的绝对尺度信息和不易受外界环境干扰等优点[8]。全地形机器人惯性测量单元MPU6050整合了三轴陀螺仪、三轴加速度及一个运动处理器[10]，外界信号对其干扰几乎为零，因此得到的定位结果更为稳定，适合灾后复杂的地形环境。

2.4 全地形行走机构

全地形搜救机器人为满足陆地与水面行走的需要设计了履带气垫一体化运动机构。

在陆地采用“粗齿锯”模型无人履带机构，该机构能自动将适当大小的动力依次传输给每一块履带板，并且相比于普通履带具有更好的平衡感，在任何地况都能活动自如。履带由TB6612对370大扭矩电机进行驱动控制，每个TB6612由FPGA根据航迹信息输出两个PWM控制，TB6612对FPGA的PWM进行放大，使电机改变转动方向和转速，可以实现全方位移动。同时考虑灾后地面的不平整性，在机器人底盘设置液压避震筒来提升整车攀爬能力和避免小车在跨越障碍时发生侧翻。

机器人驶入水域时，通过主风机转动向气垫进行充气，同时连接在气垫中部的链条机构向下旋转配合风机撑开气垫，是气垫能迅速展开充气，通过气垫的气压是机器人底盘抬升。当气垫充满后，启动尾部两侧的风机从而推进机器人向目标方向行驶。通过对尾部风机的风速调控来调整机体速度，并根据机器人尾部摆块来改变风向，达到转向的效果。返回陆地时，主风机向外排气，链条机构向上旋转收纳气垫。

3 多信息融合算法

软件系统主要通过混合式多传感器信息融合技术进行构建框架，部分传感器采用聚集式融合方法，其余的传感器采取散布式融合方式[11]。主要包括融合构架、多传感器信息融合与卡尔曼滤波。

3.1 混合式信息融合框架设计

全地形机器人的应用主要在灾后地区，具有固定区域的特点，信息采集工作需要结合GPS定位，为机器人的作业规定平面范围。在灾区范围内，全地形机器人根据上位机指令，初级信息融合中心1获取里程计及惯性单元信息反馈给中级信息中心1进行位移，中级信息融合中心2，在指定的时间段完成环境信息采集任务[12]。混合式信息融合框架如图3所示。

图3 混合式信息框架

在终级信息融合中心中，系统搭建了云端及自适应低功耗模块。在云端模块中，FPGA将LORA模块的AUX及MDO端口进行拉低处理，进入模块的配置中心，将波特率设为115 200 bps，空中速率为19.2 k，发射功率20 dBm，模块通过UART协议将环境信息数据传回用户中心。在低功耗模块中，对各个传感器进行自适应低功耗设置，传感器在5 s内未进行读写操作，即进入休眠状态，待下次指令到达再进行唤醒操作，从而降低了系统的整体功耗，提高了系统的续航能力[13]。

3.2 建立机器人动态模型

系统运用扩展卡尔曼滤波算法对各个传感器的数据进行分析调和之前，需要构建机器人履带动态模型，把全地形机器人坐标系和传感器坐标系相融合，用直角坐标系表示机器人的运动范围，在机器人中心建立局部坐标，便于确定机器人的位置相对于环境中的观察点[14]。

图4 机器人动态模型

根据机器人履带差速运动模型，建立无噪声情况下全地形运动方程式：

(1)

在式(1)中，θ为机器人实时姿态角，表示机器人的运动方向与X轴方向之间的角度；点O1为机器人的转动中心；w为机器人转动的角度；L为两履带之间距离；v为履带底盘中心的线速度；[x,y,θ]T为上一时刻机器人的位置和朝向矩阵；[x1,y1,θ1]T为经过时间间隔△t后机器人的位置和朝向矩阵[15]。

根据线速度、角速度、转弯半径关系式：v=wr，得到机器人中心线速度、角速度与两履带中心的线速度之间的关系式：

(2)

(3)

3.3 传感器数据分析

3.3.1 里程计测速数据

在由光电编码器组成的测量体系中，最常见的测速方式大致有三类:即M法、T法、M/T法。定位系统主要使用M/T法，因为M/T法结合了M法和T法的优点[16]，既适合高速测量也适合低速测量，测量公式为：

(4)

将测量公式结合动态模型，得到履带机器人的线速度式：

(5)

将线速度代入关系式(3)可求得角速度。再将角速度、线速度代入运动方程式即可得到位姿矩阵。

3.3.2 惯性单元航迹推算

系统利用MPU6050陀螺仪获取机体的加速度及角速度。加速度在导航坐标系中，分解为x,y,z三轴的加速度，对3个速度进行一次积分得到机器人运动的速度，进行二次位移得到机器人运动的位移。在使用陀螺仪时首先给定一个基准方向，机器人运动中的位姿将由自身角度变化量的不断累加得到[17]。

全地形机器人使用履带结构，故机器人只能进行前进(Vt)与旋转(Wt)，不能随意规定方位移动。推算系统航迹时，需要优先考虑相邻时刻(MS)，把两临近的运动轨迹视为直线，即向X轴运动Vt*△t，将其代入运动坐标系，得机器人在t+1期间相对于t期间在运动坐标中移动的位移：

(6)

求得到相邻时刻的位移后，如欲知一段时间内的运动轨迹，则只需将这一时期的位移增量加以累积求和：

(7)

3.3.3 GPS定位信息采集

GPS对机器人进行区域划定之前，主要需要完成两项任务：接收数据及处理数据。GPS与FPGA连接后，FPGA开始采集GPS数据，并对ATK1218模块数据协议进行约定，GPS模式具有多种模式，全地形搜救机器人采用GNRMC推荐定位信息模式，其他数据位包含UTC时间、经纬度、卫星数量、海拔高度等信息，模块支持最快1秒输出20次定位信息，系统需要在指定时间内，将模块数据缓存到FIFO中，并进行解析。解析后，中级信息融合中心获的机器人此时坐标A，终级信息融合中心以A为起点坐标构建矩形坐标系，既可获得机器人作业区域。

3.3.4 数据传输及缓存

全地形机器人信息庞大，各个传感器时钟域不同，数据更新频率不同，此时需对数据缓存做处理[18]。系统设计了FIFO+SDRAM一体的读写缓存器。在数据进行采集之前，用户需要对外部存储芯片W9825G6DH进行初始化、行激活，接着用户将传感器数据写入FIFO-A，得到SDRAM控制器响应后，FIFO-A中的数据写入W9825G6DH，当FIFO-B中的数据量小于突发长度是将W9825G6DH中的数据读出，送入下一级处理中心。

3.4 扩展卡尔曼滤波信息融合

传统的卡尔曼滤波算法可以在线性高斯模型条件下，对目标状态作出最优估算，不过，机器人在实际运动中，并不会固定直线行驶，在废墟或者积水区域作业中，也会使机器人产生颠簸或者侧倾，机器人运动中会具有非线性的特征[20]，此时就需要通过扩展卡尔曼滤波把非线性问题转化为一种类似线性滤波问题。扩展卡尔曼滤波算法(EKF)适用于非线性系统，原理是通过一阶泰勒展开将非线性方程转换为线性方程，再采用卡尔曼滤波算法对状态量进行估计。扩展卡尔曼滤波算法本质上由以下两部分构成：预测模型和观测模型两个部分[21]。

3.4.1 状态预测

扩展卡尔曼滤波状态预测方程为：

u1=Gtut-1+Wt

(8)

∑t=Gt∑t+1GtT+At

(9)

系统使用轮式里程计为状态预测提供控制量ut+1=(Vodo，Vodo，Wodo)。根据运动模型预测t+1时刻的位姿估计值为：

(10)

式中，Xt+1为机器人t+1时刻的位姿估值[20]。∑t为预测状态向量估计值的协方差矩阵，∑t+1即为t+1时刻的状态协方差矩阵[22]。将式(5)所求的履带中心线速度代入运动方程式，再结果代入状态方程式，便能获取机器人目前时刻的行驶状态估计值。

3.4.2 观测预测

卡尔曼滤波观测方程式为：

Zt=Htut+Vt

(11)

式中，Zt为系统观测量，Ht为系统量测矩阵，Vt为t时刻的测量噪声。对卡尔曼滤波算法进行降维扩展得到新的观测方程式：

Zt=h(ut)+Vt

(12)

式中，h(ut)是非线性函数，Vt符合统计学特征的高斯白噪声[23]。计算卡尔曼滤波增益矩阵：

(13)

通过卡尔曼增益矩阵可改变机器人的状态矩阵，从而获得新的观测值，然后通过改变估计系统状态的协方差矩阵，最终将系统的协方差矩阵均方误差最小化，并以此实现数据融合的目的。

4 实验验证

4.1 实验场景

实验场景选择雨后天气，在城市工地内，将GPS定位模块波特率设置为38 400，初始化接收卫星数据为4颗，等待3 S，接收到7颗卫星及以上时，开始利用NMEA-0183协议对接收的数据进行解析，划定废墟以及积水区域(大致1 000 m2)模拟灾后多种地形环境。在实验区域内，以A点为原点为全地形机器人设置4个目标点A(0 m，0 m),B(0 m，50 m)，C(20 m,55 m)，D(22 m,0 m)，并建立实验区域坐标系。

图5 GPS区域云端图

图6 位置点坐标示意

4.2 实验效果

全地形机器人在GPS规定的实验场地内，按照A→B→C→D路线进行行驶及信息采集。在场景内，机器人以0.2 m/s的初速度进行行驶，FPGA通过惯性测量单元获取机器人y与x方向的速度值，存入读写缓存器，并进行状态预测。如图7所示：X方向最大速度误差为0.1 m/s，Y方向最大误差为0.15 m/s，机器人在废墟等崎岖的位置传感器测量值震荡较为强烈。

图7 y与x方向速度误差

为进一步分析机器人航迹的实际情况，通过轮式里程计计算机器人的位移距离，并进行状态更新。如图8所示。X方向最大位移误差为反向0.45 m，Y方向最大位移误差为反向0.58 m，从实验数据可看出，速度误差与位移误差成正比，机器人0.5 m的位移差，中等程度偏离航向，要实现机器人的航迹按指定程序完成作业，需对传感器的数据进行扩展卡尔曼滤波融合。

图8 y与x方向的位移误差

经过扩展卡尔曼滤波后，得到的测量位置误差低于0.1 m，虽然与机器人的实际位置有点差别，但也已经无限贴近实际位置曲线，能够按要求完成机器人准确巡航任务。

图9 全地形机器人修复结果对比图

在机器人运动过程中，通过预存指令对ABCD四个点的空气、经纬度、高度、生命特征、光照系数进行检测，并将采集的数据与实验员现场勘测数据对比。从表格，可以看出机器人采集的环境空气指数最高为B点废墟环境37 PPM，最低为D点森林环境14，传感器测量数据平均误差0.75，A点出现较大误差，经检查，为传感器初始化预热慢，导致出现较大误差；环境高度最高为B点废墟42米，最低为A点水域39，平均误差0.425；光照系数平均误差5 lx；生命特征正确检测4次，在A点检测到生命时，向用户中心返回了A点经纬度：23.248 69,113.864 036，提示生命所在位置。根据以上指标，机器人能完成一般信息采集任务。采集信息如表1所示。

表1 机器人环境信息采集表

5 结束语

在灾区环境中，多传感器信息融合是机器人感知周围环境信息技术中一项行之有效、必不可缺的关键技术。

全地形信息采集机器人，在混合式融合架构下充分利用了多传感器技术,以实现对多感应器的探测信息的合理支配与利用,将多感应器的空隙及时间上的多余或相互利用信息加以综合。

实验结果表明，机器人凭借多种传感器数据融合，能够提高机器人定位精度，通过扩展卡尔曼滤波算法，能够校正崎岖环境下累积的误差，从而使得机器人得到最优信息，助力灾后救援。