1：GPS/DR组合定位方法
2：GPS/MM组合定位方法
原作者：黄智，钟志华
一、前言
1994年全球定位系统（global position system，GPS）正式投入使用以来，采用GPS的车辆导航技术获得了广泛应用。GPS信号采用直线传播，能量低，遇到障碍物会影响信号的正常接收，在城市交通环境中，由于高楼、高架桥、隧道等障碍物的阻隔，经常发生短时的通信中断和定位精度降低的现象。航位推测法（dead reckoning，DR）利用车载传感器测量速度、里程和航向角信息，采用航位推算计算车辆方位，具有高度自治性和较高的定位精度。DR定位误差随积分过程增加，若长时间独立工作，误差将发散。GPS/DR组合导航实现两种定位技术的优缺点互补，提高定位系统精度和可靠性，是多传感器组合导航系统主要采用的技术之一。
融合GPS和DR信息是组合导航定位技术的关键，卡尔曼滤波是对随机系统的统计最优估计，在组合导航定位系统中的应用研究有较长历史。采用集中卡尔曼滤波对多传感器组合系统进行信息融合可以实现统计最优估计，但由于是集中处理，当系统状态维数增加时，算法存在2个问题：①计算量与状态维数的3次方成正比，实时计算困难；②容错性差，一个传感器故障，整个系统信息都会被污染。
20世纪70年代末提出的分散卡尔曼滤波是对传统卡尔曼的改进，采用分级的滤波器结构，提高滤波器的容错性，但算法复杂，滤波器之间的通信量大，工程实现困难。
20世纪80年代末Carlson提出了联合滤波器，算法是对分散滤波器的改进，根据信息守恒原理，用信息分配系数把全局状态估计信息和系统噪声信息分配给各局部滤波器。算法具有简单、容错性好和便于工程实施的优点。
作者采用联合卡尔曼滤波器（federal Kalman fifter，FKF）融合GPS和DR信息，根据定位传感器精度低和车载导航计算机计算能力较低的特点，在现有的研究基础上，改进联合滤波算法，稍微降低融合精度，提高原算法计算效率。
二、GPS/DR组合定位
作者采用的GPS/DR组合定位系统见图1。
GPS接收卫星信号，产生定位信息直接输出到信息融合算法。DR传感单元包括角速率陀螺、轮速传感器和MCU信号采样。陀螺测量车辆的横摆角速度，通过积分得到航向角增量。轮速传感器是车辆的标准部件，轮速信号可以取自里程传感器，也可以由ABS的轮速脉冲传感器得到。由MCU和模数转换电路完成传感器信号采样，并将采样数据输入到融合算法。融合算法运行在上位导航计算机中，融合GPS和DR单元的信息，产生定位输出给导航软件。
为降低系统成本，导航计算机通常采用外设较全的处理器，如ARM、XScale、MIPS等，与DSP和X86架构的CPU相比，计算能力较弱，并且要运行占用大量系统资源的导航软件，分配给融合算法的计算资源有限。工程样机采用以Samsung公司的S3C2440A（400MhzARM920内核，无浮点协处理器）为主控CPU的导航计算机，分配给融合算法的计算资源约为30MIPS。因此，在维持合理精度条件下降低融合算法的计算复杂性是融合算法工程实现的关键。
图1 GPS/DR组合定位系统
三、GPS/DR联合滤波
（一）滤波器公共状态方程
FKF由GPS、DR的局部滤波器和主滤波器构成。GPS滤波器状态Xg和DR滤波器状态Xi相同，取公共状态为 
，其中e和n分别为东向和北向的位置； 
分别为东向和北向的速度； 
分别为东向和北向的加速度。加速度采用当前加速度统计模型描述为
式中 
分别为当前加速度均值，τae和τan分别为加速度模型时间常数的倒数，ωae和ωan分别为均值为零、方差分别为 
的高斯白噪声。
系统状态方程描述为
式中
用加速度的一步预测代替当前机动加速度均值，可以得到离散系统状态方程的简化形式为
Xc（k+1）=Φ（k）Xc（k）+Wc（k）（4）
式中T为离散系统时间步长；Wc（k）为离散白噪声序列，噪声协方差用Q（k）[8]表示为
式中
（二）DR局部滤波器观测方程
DR局部滤波器的观测值取误差补偿后的DR估计的航向角θ和位移s，分别为
式中εθ和εs分别为DR航向角和位移的观测噪声，均值为零，方差分别为 
。得到一阶线性化离散方程为
式中
Wi（k）为观测噪声矩阵，H（k）为h（k-1）对Xi（k-1）的一阶微分
DR观测值受传感器误差累积影响，因此当GPS信号良好时，用GPS观测值来校正DR观测值。取λθ（k）和λs（k）分别为GPS的航向和里程观测值与DR对应观测值的误差，为抑制GPS的噪声，对其进行限幅处理，分别得到λ′θ（k）和λ′s（k），DR观测值的自适应误差反馈校正过程如下。
式中θ0为初始航向角；ω为陀螺测量的横摆角速度；s0为里程传感器原始输出；Kθ和Ks分别为误差反馈系数，取值与传感器参数校正状态有关，传感器参数校正精度越高，反馈系数越小；βv为GPS局部滤波器的速度信息分配系数，其物理意义为：当GPS子系统精度降低，在融合结果中的权重减小时（即βv减小），亦减小式（8）中的反馈，以抑制GPS误差污染DR的观测值。因此，提出的联合滤波算法具有一定的容错性。
（三）GPS局部滤波器观测方程
观测量取GPS输出的东向和北向的定位eg和ng，观测方程为
式中ωe、ωn分别为观测噪声，取开阔接收环境的噪声均方根分别为σe和σn。
观测噪声矩阵为
GPS的观测噪声是一个时变过程，文献[9]提出用递推估计系统噪声和量测噪声方差矩阵，但实际应用中存在计算量大的问题。HDOP值是GPS在水平面上定位误差的放大因子[10]。作者通过在各种接收环境下重复进行GPS的静态定位试验，结果表明很难建立一个准确的接收卫星数SVs、水平精度因子HDOP与GPS观测噪声的数学模型。静态试验只得到一般性的结论：当SVs≥6时，GPS观测误差较小，当SVs<6时，定位噪声增大，HDOP值粗略地描述GPS的定位精度。
根据静态试验结果，作者提出GPS观测噪声矩阵R自适应调整方法为
式中Kσ按如下规则确定：当接收卫星数量SVs≥6时，取Kσ=HDOP；否则Kσ=max（2，HDOP），且Kσ<5。
（四）全局滤波过程
主滤波器只对局部滤波器的输出状态和协方差进行融合，计算主滤波器的协方差Pm和状态Xm，主滤波器无时间更新和测量更新过程。典型的融合过程为
式中Pg和Pi分别为GPS和DR局部滤波器的协方差。
因矩阵求逆的计算量大，作者改进协方差融合过程，取协方差阵的对角元素构成对角矩阵代替原协方差阵求逆，融合精度稍有降低，但减少了计算量。
用信息分配系数β重置局部滤波器的协方差为
并对局部滤波器状态重置为 
信息分配系数是各局部滤波器在融合输出中的权重，因此该值需要反映各局部滤波器的状态估计精度。局部滤波器对各状态估计的精度不同，GPS局部滤波器对位置的估计精度高，而DR局部滤波器对速度、加速度的估计精度高，因此，作者提出采用2个信息分配系数βv和βp分别用于速度、加速度和位置分量的信息融合。由如下公式计算得
四、试验结果与分析
为验证作者提出的改进FKF算法，采用该算法对路试数据进行融合处理。滤波器使用的参数为试验路线见图2（由于采用低精度扫描地图，实际轨迹偏离了地图道路），位置为深圳南山高科技园北区的某视野开阔的四边形路段，SVs=6～10，在该路段上的GPS轨迹重复性误差小于110m，在缺乏绝对定位基准的情况下，以该GPS定位为真实值（TRUE）。
图2 试验路线
在原始GPS定位信息中加入一阶马尔可夫随机噪声，仿真GPS在一定接收条件下的定位噪声。
噪声模型为
式中Nn为高斯白噪声，均方根为50，τn为过程时间常数倒数，τn=0.01s-1.计算得到东向、北向定位噪声方差分别为26.74和49.84，根据所做的静态试验统计结果，近似等效为SVs=5和HDOP=2.2的接收条件。
DR传感器的信息由路试过程中同步采集，同步信号为GPS输出的秒脉冲，采样频率为40Hz。DR模型中，陀螺标度因子误差为-1.6％，零漂误差为0.048°/s，里程标度因子误差为-0.4％。采用提出的改进算法融合混有噪声的GPS定位信息和DR传感器的信息，以验证算法的有效性。
图3为改进FKF结果与GPS定位、真实位置的比较图。从图中可知混有噪声的GPS定位明显偏离了真实轨迹，采用提出的改进算法进行滤波后，融合定位大多落在真实轨迹附近。
图3 联合卡尔曼滤波位置估计图
表1为改进联合滤波算法、典型联合滤波算法以及GPS定位的误差的统计值分析。改进算法提高组合系统的定位精度，由于简化主滤波器的协方差融合过程，算法的精度稍低于典型的融合算法。
表1 定位误差的统计分析
m改进FKF信息融合的另一个重要作用是提高航向估计精度。图4为改进FKF与GPS估计航向角的比较图。GPS估计的航向有很大的噪声，在低速时噪声尤为明显。经过FKF融合DR和GPS信号后，航向估计准确跟随了真实航向。
图4 FKF航向估计图
改进算法移植到导航计算机中，经测试，在8次/s融合计算的条件下（正常工作每秒融合一次），导航软件仍可以正常操作，因而改进算法的效率基本满足要求。
五、结论
针对低成本车辆GPS/DR组合定位系统传感器精度低和计算能力弱的特点，提出6维状态变量的FKF算法，并简化主滤波器协方差计算过程，在稍微降低融合精度的条件下，大大减少融合计算量。提出的自适应误差反馈补偿和GPS观测噪声自适应调整分别提高DR和GPS子系统观测精度，并且具有较好的容错性。道路试验结果验证所提出的改进信息融合算法是有效的。