卡尔曼滤波在履带式消防机器人GPS定位中的应用

曾文威，刘 毅

（天津城建大学 计算机与信息工程学院，天津 300384）

全球定位系统（GPS）是由美国国防部在1973年开始部署的一款中距离轨道卫星导航系统，由于它的高效率、全球性、高精度、抗干扰能力强、可移动定位等特点，GPS被广泛应用到道路工程、水电、汽车导航、交通管理、资源勘探和航海等领域中[1].目前，全球定位系统对民众开放的精度标准已不能满足人们日常对定位精度的需求.因此，人们希望在GPS应用上能够获得更高的精确度.

近年来，随着导航技术的发展，各种滤波技术在导航系统中得到了广泛的应用[2].卡尔曼滤波理论作为导航数据处理中实时递推算法的一种最优估计是被应用的最多的一种算法[3].随着研究的深入，基于卡尔曼滤波算法，学者们提出了许多改进的滤波算法，对卡尔曼滤波的局限性做了很好的补充.文献[4]提出欠观测条件下增量Kalman滤波的概念和定义，并建立增量Kalman滤波模型及其分析方法，可以在不精确的测量方程下应用，提高卡尔曼滤波的精度.

基于上述滤波算法，本文选用卡尔曼滤波算法对消防机器人GPS定位信号进行处理，用以消除粗大误差.

1 GPS定位误差分析

GPS误差主要分为三类：卫星误差、传播误差、接收误差[5].

1.1 卫星端相关误差

卫星端误差可分为星历误差、星钟误差以及相对论效应产生的误差.星历误差主要源于GPS卫星轨道摄动的复杂性和不稳定性[6]，星历误差越大导致GPS定位误差越大.另外通常相对论效应的表现形式就是星钟方面的误差，而导致这两个误差缘由就是由于GPS运行轨道是个椭圆形，导致常数不固定而出现的细微误差.

（1）相位缠绕效应误差改正.相位缠绕效应Δφwind-up的改正公式如下

（2）相对论效应误差改正.GNSS原子钟会受到相对论效应的影响，其比例关系如下式

式中：fs表示卫星原子钟频率；f0表示标准频率；RE和RS分别为地球半径和卫星轨道半径；vE和vS为地面时钟速度和卫星速度.

1.2 传播路径相关误差

传播导致GPS定位误差主要是由电离层时延改正误差、双流层时延改正误差、多路径误差、地球自转效应误差组成[7].电离层是距离地面可达50～1 000 km之间的大气层，而电离层密度与其他大气密度不同，当GPS信号经过电离层时会导致信号路径出现弯曲从而使其传播速度出现不同，进而出现定位误差.双流层也是同样原因，多路径误差就是间接波对直接波的破坏性干涉而引起的星站距离误差，这样的误差是不可改变且随时都影响GPS定位信号的.

1.3 接收机端相关误差

接收导致GPS定位误差主要有观测噪声误差、内时延误差、天线相位中心误差[8].GPS信号在接收机内部从一个电路转移到另一个电路时有一个时间延误，即为内时延，由于波道时延的不稳定性，中频相位抖动和接收天线的相位漂移，导致接收机内时延精确度不够而出现误差，观测噪声主要来源于天线噪声和环路噪声，引起原因就是高频射电、雷电、大气涨落引起的天电干扰噪声，这种误差是避免不了的，随时随地都影响着GPS定位.

（1）海洋负荷误差改正.其改正公式如下

式中：j代表11个分潮波；fj和uj为月球交点经度；ωj和xj为t=0时的潮汐度和天文幅度；Acj和φcj表示潮波分量的测站幅度和测站相位系数.

（2）极潮误差改正.当精密卫星轨道和钟差生成的时候，IGS分析中心和IGS官方大都考虑到了极潮改正.其改正公式如下

式中：xp和 yp表示极移的角度；和表示平均的极移角度；角度均为弧度制.

2 卡尔曼滤波算法应用

当前建立的模型比较适合应用在消防机器人上.

2.1 状态方程的建立

要定位某个物体的具体位置就要通过参照物来制定坐标，想要来确定地表GPS接收机的位置，就要先确定一个参照物.例如：地球，这时就要以地球作为参照系来制定关联于地球上随同地球转动的坐标系.

表达地球直角坐标系的含义为：首先设立原点字母O，使其与地球质心完全重合；同时设立字母线轴X轴并使其箭头指向地球赤道面与格林威治子午圈的交点（即0经度方向）；然后设立字母线轴Y轴使其在赤道平面里；最后设立字母线轴Z轴并使其箭头指向地球北极与XOY构成右手坐标系（意为指向东经90°的方向）.

定义消防机器人上的GPS接收机在地球直角坐标系中的系统状态向量为

式中：ωx，ωy，ωz分别为接收机在三个坐标轴上的高斯白噪声，τx，τy，τz为马尔科夫过程的相关时间常数.则系统的状态方程可表示为

式中：A为状态转移系数矩阵，经推导可得[9]

2.2 观测方程的建立

若消防机器人上的接收机在k时刻的定位结果为 x0，y0，z0，则

式中：x，y，z为理想状态变量；εx，εy，εz为一阶马尔科夫过程误差；ωbx，ωby，ωbz为测量误差.

若取

则矩阵形式为

其中的观测矩阵可表示为

由建立的方程得知，这是一个可观测的线性卡尔曼模型.

2.3 自适应卡尔曼滤波方程的建立

通过上述方程，可建立卡尔曼滤波方程如下

式（16）中：

其中：

式（19）中：Q（k）为系统的噪声协方阵差，Q 的离散化矩阵，且

3 实验结果

将消防机器人的定位数据导入到Google Earth得到如图1所示的运动轨迹图[10]，蓝线为加滤波算法前的轨迹图，红线为加滤波算法后的轨迹图.本次测试分为两组进行，路线均以电信楼正门为起点，绕计算机中心和控制学院一周，再回到电信楼侧门.从图中可以看出，加入滤波算法后有效提高了GPS定位精度.

图1 机器人运动轨迹

4 结语

从实验结果中可以看出，本文建立的方程是正确有效的，即用一个总的误差来替换掉因各种因素引起的小误差.但是使用这种方法会使系统产生短暂延时，得到的定位信息不能保证是机器人此时刻的位置信息.本文接下来的研究方向是优化一种算法或者是研究一种新的GPS接收机，使系统的定位精度和实时性能得到显著提升.