低成本GPS/DR容错组合导航系统设计

张 强，孙 尧，万 磊，夏全喜

（哈尔滨工程大学 自动化学院 水下机器人技术国防科技重点实验室，哈尔滨 150001）

目前大多数室外陆地导航任务均采用GPS作为导航设备。但是在高楼林立或林荫覆盖的道路上GPS信号很容易被遮挡而影响定位精度。通常的办法是利用辅助导航设备与 GPS一起构成组合导航系统，不但能够提高导航系统的精度而且当GPS信号严重衰减的时候，仍可以继续提供精确的定位信息。传统的GPS/DR组合导航系统通常采用里程计作为距离传感器。但是利用里程计构成的GPS/DR组合导航系统存在诸如安装不便、测量误差大，成本较高等缺陷[1]。文献[2-4]利用由多个MEMS陀螺仪和MEMS加速度计配置成的惯性测量单元与GPS构成INS/GPS组合导航系统，能够提供较为精确的定位和姿态信息，但城市路面导航一般只需要精确的位置信息而不太需要车辆的姿态信息，因此这种组合导航系统成本相对较高且都采用诸如神经网络[2-3]、在线估计系统噪声矩阵[4-5]等较为复杂的算法来克服元件误差，因此系统模型复杂，运算量较大，容错能力不强，系统鲁棒性不好。文献[6-7]提出一种试探消减 MEMS陀螺仪随机漂移的方法——零偏试探消减法（Heuristic Drift Reduction，HDR）。该方法无需建立误差模型就可以提高MEMS惯性元件的精度，具有鲁棒性好算法简单的优点。针对由一个MEMS陀螺仪和一个MEMS加速度计与GPS构成的低成本GPS/DR组合导航系统设计的容错UKF/KF联邦Kalman滤波器，由于无需考虑MEMS惯性元件的误差模型，系统模型较为简单，鲁棒性好。仿真结果表明，这种低成本GPS/DR组合导航系统的定位精度能够达到0.5 m，并且具有较强的容错能力，特别适用于普通车辆的城市导航定位。

1 GPS/DR组合导航系统的结构及模型

1.1 GPS/DR组合导航系统的组成

如图1所示，DR导航系统传感器由一个MEMS陀螺仪和一个MEMS加速度计构成。它们的输出信号经试探性零偏消减后，同GPS的位置信号和速度信号一起被送入容错UKF/KF联邦Kalman滤波器。同一般的联邦Kalman滤波器不同的是，容错UKF/KF联邦Kalman滤波器只采用两个局部Kalman滤波器KF和UKF，分别处理GPS和DR传感器的测量数据。GPS信号检测模块用于检测车辆行驶过程中GPS信号由于树木或楼房的遮蔽而产生的突然变化使系统具有容错能力。同时主滤波器无信息分配，为抑制DR导航系统发散过快，将全部信息分配给DR导航系统，主滤波器仅完成对两个子滤波器信息的最优融合估计，这样系统计算量小，滤波速度快。

图1 GPS/DR组合导航系统结构图Fig.1 Structure of GPS/DR integrated navigation system

1.2 容错UKF/KF联邦Kalman滤波模型

1.2.1 状态方程的建立

对于在二维平面内运动的GPS/DR组合导航系统的Kalman滤波的状态变量可取为：

其中，xe、xn分别为车辆东向和北向位置分量，ve、vn分别为车辆东向和北向速度分量，ae、an分别为车辆东向和北向加速度分量。

采用“当前”统计模型[8]描述系统状态变化过程，并通过典型的离散化处理方法得到离散状态方程为：程噪声协方差阵Q(k)的表达式及计算参见文献[9]。

1.2.2 观测方程的建立

将GPS接收机输出的车辆东向和北向位置信息eobs、nobs（单位均化为m），以及车辆东向和北向速度信息ve、vn（单位均化为m/s）作为观测矢量，得到GPS子系统的观测方程如下：

2 容错UKF/KF联邦Kalman滤波设计

2.1 GPS信号检测方法

采用残差2χ检验法判断GPS信号的有效性。由Kalman滤波方程可知GPS局部滤波器的新息为：

可以证明，当GPS信号没有被屏蔽时，新息r1(k)为零均值高斯白噪声；当GPS信号被屏蔽时，新息r1(k)均值为非零值。因此通过对r1(k)均值的检验即可判断GPS信号的有效性。对r1(k)做二元假设：

GPS信号被屏蔽：

T是设定的门限值。设误报警率为5‰，又GPS观测矢量Z1(k)的维数为 4，查表可得T=14.86。用上述方法即可对GPS信号的有效性进行检测。

2.2 容错UKF/KF联邦Kalman滤波方程

从式（3）知，DR导航子系统的观测方程是非线性的，一般采用扩展Kalman滤波进行线性化，但是考虑到MEMS惯性元件的精度较低，且观测方程的非线性程度较高，因此使用UKF作为DR子系统的滤波器，并采用Sigma点对称采样策略确保任意分布的近似精度达到泰勒展开式二阶截断，而且计算量与EKF同阶。根据UKF和KF滤波递推方程以及所建立的GPS/DR组合定位系统的状态方程式（1）和观测方程式（2）（3），可以建立GPS子系统和DR子系统的Kalman滤波方程，具体状态的最优综合和信息分配过程如下：

1） 时间传播方程

3 零偏试探消减法

如图2所示，MEMS惯性元件的输出信号，经低通滤波、零偏试探消减（Heuristic Drift Reduction，HDR）和滞后消除后输入DR导航系统。

3.1 低通滤波和滞后消除

DR导航推算前一般不需要进行低通滤波，因为积分运算本身就是一个低通滤波器[6-7]，但是由于对 MEMS惯性元件的零偏试探消减是在 DR导航推算之前进行的，因此采用如式（16）所示的数字低通滤波器对MEMS陀螺仪和加速度计的输出信号进行滤波，可以消除高频噪声对零偏试探消减算法的影响。

图2 零偏试探消减法程序流程图Fig.2 Program flowchart of heuristic drift reduction

其中，T为采样时间，ok′-1为低通滤波信号，τ为低通滤波器时间常数。

虽然数字低通滤波器平滑了MEMS惯性元件的输出信号，但是数字低通滤波器同样会引起MEMS惯性元件输出信号的延迟。式（17）所示的滞后消除算法可解决这个问题。

其中od,k为经零偏试探消减校正，并消除延迟后的MEMS惯性元件的最终输出信号。

3.2 零偏试探消减算法

试探消减MEMS惯性元件的零偏是基于如下假设：即在城市路况下，路面车辆基本保持匀速巡航状态，车辆变速行驶或转弯情况较少。由于车辆在巡航状态下惯性器件的输入值为零，惯性器件的输出是由白噪声、常值零偏和随机零漂构成的误差信号。因此可以通过零偏试探消减法（HDR）消减MEMS惯性元件误差信号中的随机零漂，提高MEMS惯性元件的精度。零偏试探消减法（HDR）由自适应积分因子计算和自适应零偏积分运算两部分组成。自适应积分因子根据车辆机动情况自动调节自身大小：当车辆做变速运动或转弯运动时，MEMS惯性元件的输出值不再为误差信号，此时自适应积分因子会根据输出信号幅值的大小以一定速率趋向于零，可以抑制零偏试探消减法对实际信号的影响；当车辆处于巡航状态时，自适应积分因子会接近于1。

自适应积分因子Aa、Aω可由下式计算：

式中，Ka、Kω为衰减因子，决定自适应积分因子的衰减速度；为加速度幅值。自适应零偏积分运算会根据自适应积分因子的大小实时计算MEMS惯性元件的随机零漂，具体计算过程如式（20）和式（21）所示：

这里iac、icω为与加速度计和陀螺仪随机零漂相对应的固定增量值，可根据实际情况进行调节；sign()为符号函数；Iak、分别为MEMS加速度计和MEMS陀螺仪随机零漂的估计值。则MEMS惯性传感器的输出可以由下式补偿：

4 算法性能检验

仿真采用的惯性器件的性能如下：MEMS陀螺仪的随机零漂为1 (°)/s，角度随机游走系数为0.05 (°)/(s·Hz1/2)。MEMS加速度计的随机零漂为0.02 m/s2，白噪声方差为0.002 m/s2，GPS接收机输出的东向位置和北向位置的观测噪声标准差分别为16 m和15 m，东向和北向速度分量的观测噪声标准差为0.2 m/s和0.3 m/s，采样周期为1 s。

仿真实验路径如图3所示。以A点作为起点，D点作为终点，其中AB段做匀加速运动，BC段做匀速运动，CD段做匀减速运动。首先假设GPS信号被屏蔽的条件下，由于CD段情况与AB段相同，因此分别在AB段和在BC段对比DR导航系统传感器经零偏试探消减和未经零偏消减时的东向和北向的定位误差，其中点划线为前者的定位误差，实线为后者的定位误差。如图4所示，首先，在AB段，车辆作匀加速直线运动，由于加速度的存在，零偏试探消减法无法消除车辆横滚轴向上的 MEMS加速度计的随机零漂，但由于此时车辆航向不发生变化，因此 MEMS陀螺仪的随机零漂得到了零偏试探消减法（HRD）的有效补偿，提高了航向计算精度，进而提高了DR导航系统的精度。

图3 仿真实验路径Fig.3 Simulation experiment route

图4 HDR DR与DR AB段和BC段的位置误差对比Fig.4 Position error comparison between HDR DR and DR of AB and BC segments

其次，在BC段，车辆作匀速运动。BC段由两种路段组成，即三个匀速弯道和两个匀速直线路段。在三个匀速弯道处由于角速度的存在，零偏试探消减法（HDR）无法消除 MEMS陀螺仪的随机零漂，但可有效消除车辆横滚轴向上的 MEMS加速度计的随机零漂，且由于通过时间较短，因此HDR-DR可以保持一定的定位精度。在两个匀速直线路段，零偏试探消减法（HDR）有效消除了 MEMS陀螺仪和加速度计的随机零漂，可见零偏试探消减法（HDR）在车辆巡航的条件下同样可以起到提高MEMS惯性器件精度的效果。

图5 GPS/DR与GPS子系统位置误差对比Fig.5 Position error comparison between GPS/DR and GPS subsystem

在能够接收GPS信号的条件下，对比GPS/DR组合的容错UKF/KF联邦Kalman滤波与GPS接收机单独构成的导航子系统1在前25 s的定位精度，因为导航系统在10 s后便趋于稳定，二者稳态时定位误差的标准差没有差异。结果如图5所示，前者定位误差为点划线，后者定位误差为实线。由图5可见，两者稳态时定位误差均为0.5 m(1σ)，这是由于MEMS惯性元件的精度较低的缘故，其对定位精度并没有显著的改善。但与GPS接收机单独构成的导航子系统1相比，GPS/DR组合的容错UKF/KF联邦Kalman滤波的定位轨迹更为平滑，定位时间也更短。当GPS数据突然失效时，比较23 s至45 s之间两者的定位误差（如图6所示）可见，虽然二者都具有一定的鲁棒性，但是GPS/DR组合的容错UKF/KF联邦Kalman滤波表现出了更好的容错能力，在GPS信号被屏蔽的条件下，短时间内定位精度相对较高。

图6 GPS/DR与GPS子系统容错能力对比Fig.6 Fault tolerance ability comparison between GPS/DR and GPS subsystem

5 结 论

GPS接收机作为车辆普遍采用的导航设备，虽然能够满足城市车辆定位的大部分需求，但是其定位精度往往因信号被高楼、树木遮挡而受到影响，并且不具备容错能力。文中设计的低成本GPS/DR容错组合导航系统，在基于容错UKF/KF联邦Kalman滤波算法将GPS输出的位置、速度信息与MEMS陀螺仪、加速度计输出的经零偏试探消减后的航向角速率和车辆加速度信息融合后，得到更为光滑、收敛性更好的定位结果；并且当 GPS信号因遮挡而突然失效时，该导航系统依然能够提供较为准确的定位信息，具有一定的容错能力。该低成本GPS/DR容错组合导航系统具有以下特点：其一，系统在原有GPS接收机基础上仅增加了一个MEMS陀螺仪和一个MEMS加速度计，硬件设计简单，成本较低；其二，应用零偏试探消减算法提高MEMS惯性元件输出信号精度，简化了导航系统滤波模型，降低了Kalman滤波器的维数，算法实现简单；其三，在仿真条件下，系统达到了0.5 m(1σ)的定位精度，且容许GPS数据的突然失效，表明该系统适用于车辆导航的城市应用。

（References）：

[1] Kaplan E D, Hegarty C. Understanding GPS: Principles and applications[M]. 2nd edition. Boston and London: Artech House, 2005: 491-522.

[2] El-Sheimy N, Abdel-Hamid W. An adaptive neuro-fuzzy model to bridge GPS outages in MEMS-INS/GPS land vehicle navigation[C]//ION GNSS 2004. Long Beach, California, September 21-24, 2004.

[3] Kai-Wei Chiang, Nassar S, El-Sheimy N. A constructive and autonomous integration scheme of low-cost GPS/MEMS IMU for land vehicular navigation application[C]// IEEE/ION PLANS 2006. San Diego, California, April 25-27, 2006: 235-243.

[4] Tang K H, Wu M P, Hu X P. Multiple model Kalman filtering for MEMS-IMU/GPS integrated navigation[C]//2007 Second IEEE Conference on Industrial Electronics and Applications. Harbin, China: May 23-25, 2007: 2062-2066.

[5] 周艳丽，张海，高婷婷，等. 一种改进的GPS/DR组合位置自适应滤波算法[J]. 中国惯性技术学报，2009，17(6)：728-733.ZHOU Yan-li, ZHANG Hai, GAO Ting-ting, et al. An improved GPS /DR integrated position adaptive filtering algorithm[J]. Journal of Chinese Inertial Technology, 2009, 17(6): 728-733.

[6] Borenstein J, Ojeda L, Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems[J]. The Journal of Navigation, 2009, 62: 41-58.

[7] Borenstein J, Ojeda L. Heuristic reduction of gyro drift in vehicle tracking application[J]. International Journal of Vehicle Autonomous Systems, 2009(2): 78-98.

[8] 巴宏欣，赵宗贵，杨飞，等. 机动目标的模糊自适应跟踪算法[J]. 系统仿真学报，2004，16(6)：1181-1186.BA Hong-xin, ZHAO Zong-gui, YANG Fei, et al. Fuzzy adaptive tracking algorithm for maneuvering target[J]. Journal of System Simulation, 2004, 16(6): 1181-1186.

[9] 寇艳红，张其善，李先亮. 车载GPS/DR组合导航系统的数据融合算法[J]. 北京航空航天大学学报，2003，29(3)：264-268.KOU Yan-hong, ZHANG Qi-shan, LI Xian-liang. Data fusion algorithm for GPS/DR integrated vehicle navigation system[J]. Journal of Beijing University of Aeronautics and Astronautics, 2003, 29(3): 264-268.