消防机器人GPS导航系统的精度提高方案

机器人运行时的震动和外界的强磁干扰会影响电子罗盘的精度。震动对其影响约 ，可以在CPU端使用软件滤波器来解决这个问题[2];当外界的强磁场远远超过地球磁场时，甚至可能会使电子罗盘完全失效。本系统利用软件里程计来计算航向，并可用于判定电子罗盘是否工作正常。

里程计与航位推算

　　消防机器人左右轮有各自的驱动电机和传动系统，控制两个电机运行不同的时间以实现不同角度转弯。所以也可以利用左右里程计的数值差来计算机器人车体的相对转角。可以使用光码盘对驱动轮的转动次数计数，然后再用先验公式计算里程，精确性较好。还可以在CPU内部开辟寄存器空间，记录所有电机运行命令中的时间参数，以计算左右轮的里程，作为预期参考值，但是驱动电机的误差会影响精确性。

　　假设机器人两个驱动轮间隔L，驱动轮半径r，驱动电机每秒钟可驱动轮子转n圈;则若要向左旋转Δθ度，只要控制右轮比左轮多运行 秒。由于机器人硬件参数相对固定，因而转过一个固定角度的时间参数也可以先验获取，经过多次试验可以获得比较精确的数值。为简化计算复杂度，机器人的前进和转向是作为两种运动方式来处理的，即机器人只会直行，转向时产生的位移由先验参数进行补偿。

　　由于消防机器人主要在地面工作，因此定位时也暂不考虑水平高度的变化。系统二维定位坐标中，以正北方向为Y轴正方向，航向角度为机器人前进方向顺时针偏离正北方向的角度。如图二所示，机器人先直行ΔS，然后向右转这里认为机器人直行时轨迹为理想直线，转向时的轨迹为理想圆弧，实际使用时需添加修正因子。

　　直行的相对方位变化可由以下公式得到：

（式中ΔS提取命令中时间参数计算得来，相对方位角由航向记录修正电子罗盘数据提供;γ和λ为修正因子）

　　根据机器人实际航行情况，测得转弯半径为R，则转向位移为：

（式中顺时针转向时n=1，逆时针转向时n=0;φ和ψ为修正因子）

　　由于驱动轮地面摩擦情况可能发生变化，电机驱动误差等因素，实际运行中的轨迹并非理想状态，可利用先验误差因子作修正;并且在GPS和电子罗盘工作正常的情况下，周期性校验方位及航向记录，以避免误差的叠加。而在外界干扰严重（建筑物遮挡，无法收到GPS信号;环境磁场扰乱地磁场）的情况下，就要使用可靠的预测算法来提取有用信息。

改进自适应卡尔曼滤波与信息综合

　　在误差干扰下提取机器人的正确位置信息，需要使用到卡尔曼滤波器[3]。这是一种递推线性最小方差估计，广泛应用于信息提取，信息融合，追踪、导航等方面。它基于以下两个前提：首先，系统状态可以由以下线性方程定义：

　　状态方程：;（wk为过程误差）

输出（测量）方程：（zk为测量误差）

　　其次，过程误差与测量误差分布满足零均高斯，且不相关;需要先验的误差分布参数。基本滤波公式如下：

K是卡尔曼增益，P是预计方差矩阵， 是过程误差矩阵期望值, 是测量误差矩阵期望值。

　　卡尔曼滤波器的性能与误差分布的先验参数密切相关，因此在实际应用中存在诸多问题。首先系统的状态方程可能是非线性的，线性拟合方程随时间推移会引入较大的误差;其次外界干扰产生的误差特性是未知的，错误的先验信息会导致滤波结果与实际被离。自适应卡尔曼滤波器[4]使用滑动窗法及最大似然准则，利用实际测量值动态改变滤波器参数（测量方差矩阵和系统方差矩阵），其根本思想在于找出与预期误差最小的结果，并给予最大的权重。