技术文章
消防机器人GPS导航系统的精度提高方案
日期：2013-07-02
来源：九游会J9
摘 要：
本文介绍了工作在特殊环境下的机器人导航系统，实现了利用罗盘和里程计等内部传感器对GPS定位误差的修正。文章给出了消防机器人导航的主要算法流程，以及利用改进自适应卡尔曼滤波器实现多传感器信息融合。
关键词：移动机器人，导航系统，GPS，多传感器，自适应卡尔曼滤波器
引言
消防侦察机器人是一类实用型机器人，它能替代消防救援人员遥控进入易燃、易爆、有毒、有害、易坍塌建筑物、大型仓库堆垛、缺氧、浓烟等室内外危险灾害现场， 进行现场探测、侦察。由于火灾现场的特殊环境（建筑物遮挡，浓烟，火焰等），控制人员必须能够在视野之外准确地操控消防机器人，因而必须采用可靠精确的导 航系统。一般消防机器人都载有摄像头，可以通过电缆或无线传输模块将视频信号返回给控制人员，由此可以实现机器人的视觉导航。然而视频信息的带宽需求比较 大，而且易受外部环境干扰，在许多场合无法使用。本文提出一种融合GPS，陀螺仪和里程计等传感器信息的综合导航系统方案，系统结构如图一所示。
GPS定位 GPS全球卫星定位系统是随着现代航天及无线电通讯科学技术的发展建立起来的一个高精度、全天候和全球性的无线电导航电位、定时的多功能系统。GPS可 提供实时的三维坐标、三维速度和高精度的时间信息，目前已成为当今世界上应用最广泛、最实用的全球精密授时、测距、导航、指挥、调度系统。 GPS模块可提供定位时间，经纬度，定位功能指示等12项数据，输出格式采用标准NMEA协议，可以直接使用获得的经纬度数据作为方位信息（东经 XXX.XXXX，北纬XXX.XXXX）。也可以根据功能指数和精度因子计算相对于出发点的位置信息（北：±XXXX.XXm; 东：±XXXX.XXm）。模块单机定位时精度为10～30米，如果使用了基准站差分定位技术则精度可以提高至5～10米[1]。这样的精度仍然无法满足 消防机器人准确操控的需求，另一方面，消防侦察机器人需要在工厂区，室内等无法接收到GPS卫星信号的环境下工作，因此必须用其他技术作为GPS定位的补 充。
电子罗盘系统
电 子罗盘COMPASS的操作原理是根据磁阻感应地球磁场变化而即时输出物体的运行方向和运动姿态数据。运动方向数据包括方位角（顺时针偏离正北方向的角 度）等数据;运动姿态数据包括俯仰角、旋转角等数据。COMPASS的使用目的是针对GPS接收数据的补偿，使得本系统在GPS卫星信号接收不理想的情况 下，依旧能产生恒定频率的机器人运行方向和姿态信息，并进一步根据相应算法得出其位置信息。
机器人运行时的震动和外界的强磁干扰会影响电子罗盘的精度。震动对其影响约 ，可以在CPU端使用软件滤波器来解决这个问题[2];当外界的强磁场远远超过地球磁场时，甚至可能会使电子罗盘完全失效。本系统利用软件里程计来计算航 向，并可用于判定电子罗盘是否工作正常。
里程计与航位推算
消 防机器人左右轮有各自的驱动电机和传动系统，控制两个电机运行不同的时间以实现不同角度转弯。所以也可以利用左右里程计的数值差来计算机器人车体的相对转 角。可以使用光码盘对驱动轮的转动次数计数，然后再用先验公式计算里程，精确性较好。还可以在CPU内部开辟寄存器空间，记录所有电机运行命令中的时间参 数，以计算左右轮的里程，作为预期参考值，但是驱动电机的误差会影响精确性。 假设机器人两个驱动轮间隔L，驱动轮半径r，驱动电机每秒钟可驱动轮子转n圈;则若要向左旋转Δθ度，只要控制右轮比左轮多运行 秒。由于机器人硬件参数相对固定，因而转过一个固定角度的时间参数也可以先验获取，经过多次试验可以获得比较精确的数值。为简化计算复杂度，机器人的前进 和转向是作为两种运动方式来处理的，即机器人只会直行，转向时产生的位移由先验参数进行补偿。 由于消防机器人主要在地面工作，因此定位时也暂不考虑水平高度的变化。系统二维定位坐标中，以正北方向为Y轴正方向，航向角度为机器人前进方向顺时针 偏离正北方向的角度。如图二所示，机器人先直行ΔS，然后向右转
，这里认为机器人直行时轨迹为理想直线，转向时的轨迹为理想圆弧，实际使用时需添加修正因子。 直行的相对方位变化可由以下公式得到： 
（式中ΔS提取命令中时间参数计算得来，相对方位角由航向记录修正电子罗盘数据提供;γ和λ为修正因子） 根据机器人实际航行情况，测得转弯半径为R，则转向位移为： 
（式中顺时针转向时n=1，逆时针转向时n=0;φ和ψ为修正因子） 由于驱动轮地面摩擦情况可能发生变化，电机驱动误差等因素，实际运行中的轨迹并非理想状态，可利用先验误差因子作修正;并且在GPS和电子罗盘工作正 常的情况下，周期性校验方位及航向记录，以避免误差的叠加。而在外界干扰严重（建筑物遮挡，无法收到GPS信号;环境磁场扰乱地磁场）的情况下，就要使用 可靠的预测算法来提取有用信息。
改进自适应卡尔曼滤波与信息综合
在误差干扰下提取机器人的正确位置信息，需要使用到卡尔曼滤波器[3]。这是一种递推线性最小方差估计，广泛应用于信息提取，信息融合，追踪、导航等方面。它基于以下两个前提：首先，系统状态可以由以下线性方程定义： 状态方程：
;（w[sub]k[/sub]为过程误差） 输出（测量）方程：
（z[sub]k[/sub]为测量误差） 其次，过程误差与测量误差分布满足零均高斯，且不相关;需要先验的误差分布参数。基本滤波公式如下： 
K是卡尔曼增益，P是预计方差矩阵， 是过程误差矩阵期望值, 是测量误差矩阵期望值。 卡尔曼滤波器的性能与误差分布的先验参数密切相关，因此在实际应用中存在诸多问题。首先系统的状态方程可能是非线性的，线性拟合方程随时间推移会引入 较大的误差;其次外界干扰产生的误差特性是未知的，错误的先验信息会导致滤波结果与实际被离。自适应卡尔曼滤波器[4]使用滑动窗法及最大似然准则，利用 实际测量值动态改变滤波器参数（测量方差矩阵和系统方差矩阵），其根本思想在于找出与预期误差最小的结果，并给予最大的权重。
实际应用中，自适应卡尔曼滤波器需要一定的训练序列与收敛时间，如果突然出现超出预计的误差值（如GPS接收机进入信号阴影区域），不可避免会引入误 差，甚至可能导致滤波器无法收敛，得不到正确的解。如图四所示，为使用[4]提供的自适应卡尔曼滤波算法，得到的机器人运行轨迹（Matlab仿真）。前 20个测量点是自适应滤波器的学习过程，这时使用先验的误差参数;结束学习过程后，预测误差有所减小。可以看到，在GPS接收良好的情况下，预计方位与真 实方位拟合得相当好，但是进入阴影区域后，就引入偏差;经过自适应收敛过程后，降低误差很大的GPS信息权重，轨迹曲线形态仍然相似，但是偏差已经无法挽 回了。 消防机器人是远程操控机器人，它的所有运动都是接收远程指令，然后送执行机构执行，因此它的预期运动方向及方位是可精确获知的。针对系统设计的这个特 点，在传感器信息进入自适应滤波之前，先进行判断与加权，与系统预期值距离远的数据获得较轻的权重。如此，当GPS信号突然恶化，误差加剧的时候，这部分 误差对滤波结果的影响很小，在滤波器收敛过程中不会引入很大的偏移，改进后的算法得到的预测轨迹如图四中所示。由于GPS信息在估算中几乎没有影响，无法 修正航位估算中的背离，因而经过一段时间的误差累加，估计值会逐渐偏离真实位置，如图中所示。
结论与展望
在 GPS定位盲区，本系统利用电子罗盘和路程记录进行精确方位的估算，能为远程盲操控消防机器人提供较为准确的导航信息。实际应用中，利用路程记录的小范围 短距离航位推算较为精确。但如果处于极端情况下，即在长时间接收不到正确的GPS定位信息，缺少定位基准点的情况下，方位推算将逐渐出现偏移。 电子罗盘主要用于指示航向，如改用惯性陀螺仪就可以避免外界磁干扰，增加系统强壮度，但要周期性修正陀螺仪偏移;如果使用光电相对位移传感器来测量机 器人与地面的相对移动，就可以大大提高航位估算的精度，并且避免由于驱动轮打滑等引起的计算误差，但由于目前光电传感器对使用环境要求较为苛刻，有待进一 步的改进。
参考文献：
【1】王广运,郭秉义,李洪涛编. 差分GPS定位技术与应用 北京:电子工业出版社,1996
【2】Donald G . Polvani. Magnetic Guidance for Autonomous Vehicle. IEEE 1986 CH2363 1407～1412
【3】 Toshihiro Aono. Positioning of vehicle on undulating ground using GPS and dead reckoning. Proceeding of IEEE 1998 International Conference on Robotic & Automatic Leuven, Belgium, May 1998, P3443～3448
【4】A. H. Mohamed, K. P. Schwarz. Adaptive Kalman Filtering for INS/GPS. Journal of Geodesy （1999） 73: 193～203
本文介绍了工作在特殊环境下的机器人导航系统，实现了利用罗盘和里程计等内部传感器对GPS定位误差的修正。文章给出了消防机器人导航的主要算法流程，以及利用改进自适应卡尔曼滤波器实现多传感器信息融合。
关键词：移动机器人，导航系统，GPS，多传感器，自适应卡尔曼滤波器
引言
消防侦察机器人是一类实用型机器人，它能替代消防救援人员遥控进入易燃、易爆、有毒、有害、易坍塌建筑物、大型仓库堆垛、缺氧、浓烟等室内外危险灾害现场， 进行现场探测、侦察。由于火灾现场的特殊环境（建筑物遮挡，浓烟，火焰等），控制人员必须能够在视野之外准确地操控消防机器人，因而必须采用可靠精确的导 航系统。一般消防机器人都载有摄像头，可以通过电缆或无线传输模块将视频信号返回给控制人员，由此可以实现机器人的视觉导航。然而视频信息的带宽需求比较 大，而且易受外部环境干扰，在许多场合无法使用。本文提出一种融合GPS，陀螺仪和里程计等传感器信息的综合导航系统方案，系统结构如图一所示。
电子罗盘系统
电 子罗盘COMPASS的操作原理是根据磁阻感应地球磁场变化而即时输出物体的运行方向和运动姿态数据。运动方向数据包括方位角（顺时针偏离正北方向的角 度）等数据;运动姿态数据包括俯仰角、旋转角等数据。COMPASS的使用目的是针对GPS接收数据的补偿，使得本系统在GPS卫星信号接收不理想的情况 下，依旧能产生恒定频率的机器人运行方向和姿态信息，并进一步根据相应算法得出其位置信息。
里程计与航位推算
消 防机器人左右轮有各自的驱动电机和传动系统，控制两个电机运行不同的时间以实现不同角度转弯。所以也可以利用左右里程计的数值差来计算机器人车体的相对转 角。可以使用光码盘对驱动轮的转动次数计数，然后再用先验公式计算里程，精确性较好。还可以在CPU内部开辟寄存器空间，记录所有电机运行命令中的时间参 数，以计算左右轮的里程，作为预期参考值，但是驱动电机的误差会影响精确性。 假设机器人两个驱动轮间隔L，驱动轮半径r，驱动电机每秒钟可驱动轮子转n圈;则若要向左旋转Δθ度，只要控制右轮比左轮多运行 秒。由于机器人硬件参数相对固定，因而转过一个固定角度的时间参数也可以先验获取，经过多次试验可以获得比较精确的数值。为简化计算复杂度，机器人的前进 和转向是作为两种运动方式来处理的，即机器人只会直行，转向时产生的位移由先验参数进行补偿。 由于消防机器人主要在地面工作，因此定位时也暂不考虑水平高度的变化。系统二维定位坐标中，以正北方向为Y轴正方向，航向角度为机器人前进方向顺时针 偏离正北方向的角度。如图二所示，机器人先直行ΔS，然后向右转
，这里认为机器人直行时轨迹为理想直线，转向时的轨迹为理想圆弧，实际使用时需添加修正因子。 直行的相对方位变化可由以下公式得到： （式中ΔS提取命令中时间参数计算得来，相对方位角由航向记录修正电子罗盘数据提供;γ和λ为修正因子） 根据机器人实际航行情况，测得转弯半径为R，则转向位移为： （式中顺时针转向时n=1，逆时针转向时n=0;φ和ψ为修正因子） 由于驱动轮地面摩擦情况可能发生变化，电机驱动误差等因素，实际运行中的轨迹并非理想状态，可利用先验误差因子作修正;并且在GPS和电子罗盘工作正 常的情况下，周期性校验方位及航向记录，以避免误差的叠加。而在外界干扰严重（建筑物遮挡，无法收到GPS信号;环境磁场扰乱地磁场）的情况下，就要使用 可靠的预测算法来提取有用信息。 改进自适应卡尔曼滤波与信息综合
在误差干扰下提取机器人的正确位置信息，需要使用到卡尔曼滤波器[3]。这是一种递推线性最小方差估计，广泛应用于信息提取，信息融合，追踪、导航等方面。它基于以下两个前提：首先，系统状态可以由以下线性方程定义： 状态方程：
;（w[sub]k[/sub]为过程误差） 输出（测量）方程：（z[sub]k[/sub]为测量误差） 其次，过程误差与测量误差分布满足零均高斯，且不相关;需要先验的误差分布参数。基本滤波公式如下： K是卡尔曼增益，P是预计方差矩阵， 是过程误差矩阵期望值, 是测量误差矩阵期望值。 卡尔曼滤波器的性能与误差分布的先验参数密切相关，因此在实际应用中存在诸多问题。首先系统的状态方程可能是非线性的，线性拟合方程随时间推移会引入 较大的误差;其次外界干扰产生的误差特性是未知的，错误的先验信息会导致滤波结果与实际被离。自适应卡尔曼滤波器[4]使用滑动窗法及最大似然准则，利用 实际测量值动态改变滤波器参数（测量方差矩阵和系统方差矩阵），其根本思想在于找出与预期误差最小的结果，并给予最大的权重。
图四 Matlab仿真结果
结论与展望
在 GPS定位盲区，本系统利用电子罗盘和路程记录进行精确方位的估算，能为远程盲操控消防机器人提供较为准确的导航信息。实际应用中，利用路程记录的小范围 短距离航位推算较为精确。但如果处于极端情况下，即在长时间接收不到正确的GPS定位信息，缺少定位基准点的情况下，方位推算将逐渐出现偏移。 电子罗盘主要用于指示航向，如改用惯性陀螺仪就可以避免外界磁干扰，增加系统强壮度，但要周期性修正陀螺仪偏移;如果使用光电相对位移传感器来测量机 器人与地面的相对移动，就可以大大提高航位估算的精度，并且避免由于驱动轮打滑等引起的计算误差，但由于目前光电传感器对使用环境要求较为苛刻，有待进一 步的改进。
参考文献：
【1】王广运,郭秉义,李洪涛编. 差分GPS定位技术与应用 北京:电子工业出版社,1996
【2】Donald G . Polvani. Magnetic Guidance for Autonomous Vehicle. IEEE 1986 CH2363 1407～1412
【3】 Toshihiro Aono. Positioning of vehicle on undulating ground using GPS and dead reckoning. Proceeding of IEEE 1998 International Conference on Robotic & Automatic Leuven, Belgium, May 1998, P3443～3448
【4】A. H. Mohamed, K. P. Schwarz. Adaptive Kalman Filtering for INS/GPS. Journal of Geodesy （1999） 73: 193～203
