摘 要： 建立了INS（Inertial Navigation System）/GPS紧耦合组合导航系统，针对测量粗差对系统定位结果的影响，将抗差估计理论应用于非线性滤波算法，提出了基于等价权原理的抗差UKF定位算法。加入模拟粗差进行实验，结果表明，观测量异常时，抗差UKF算法能成功探测和剔除观测量粗差，实现定位结果的有效估计。
0 引言
为克服单一导航系统的缺点，实现性能互补，以卫星导航和自主式导航结合形成的组合导航技术便成为导航领域的主要研究和发展方向[1-2]。惯性导航系统（Inertial Navigation System，INS）是一种以牛顿力学原理为基础的自主式导航系统。而由INS和GPS组合构成的INS/GPS组合导航系统可以充分发挥子系统的优势，为进一步克服松耦合系统高度依赖卫星信号质量的缺点，设计基于伪距和伪距率测量值的紧耦合组合导航系统成为研究的重点[3-4]。
INS/GPS紧耦合系统定位算法在实现有效定位的同时，如何克服测量粗差对状态估计结果的影响，建立组合导航系统的抗差定位算法成为导航领域研究的重要内容[5]。为排除有害信息的干扰，防止滤波发散，通过综合考虑非线性滤波技术及抗差估计理论，将传统无迹Kalman滤波（Unscented Kalman Filtering，UKF）算法与抗差估计理论相结合[6-7]，提出基于等价权原理的抗差UKF定位算法，以期保障定位结果有效性的同时，提高系统的健壮性。
1 紧耦合导航系统的系统方程
紧耦合系统是从INS模块解算出位置、速度信息，并由该位置、速度信息计算出INS伪距和伪距率信息，然后将惯导器件预测的伪距和伪距率信息与通过GPS得到的伪距和伪距率信息相减后再作星间差作为观测量[8-9]。
1.1 紧耦合导航系统状态方程
INS/GPS紧耦合系统需要考虑载体运动的位置、速度和加速度等信息，为全面反映载体运动状态的变化，基于惯性坐标系得到INS/GPS紧耦合系统的状态向量为[2]：
其中，δR为位置误差，δV为速度误差，δ?渍为平台失准角，δε为陀螺仪零偏，δa为加速度计零偏，假设δε、δa为常值偏差，则基于上述状态变量的状态误差方程为：
方程中Fe是比力向量的反对称矩阵，Ne是引力系数矩阵，是地球系相对于惯性系的角速度在地球系的投影，C代表载体系到惯性系的方向余弦矩阵，M和N为一阶马尔可夫过程的常量表示[2]。
将式（2）整理并离散化可写成：
式中，w为系统噪声矩阵。
1.2 紧耦合导航系统量测方程
伪距与多普勒频移测量残余为GPS接收模块得到的伪距、多普勒频移实际测量值与依靠惯性器件计算的伪距、多普勒频移预测值相减后得到的结果[1]：
为消除本地时钟与卫星时钟的时差，将相对于不同卫星得到的伪距与伪距率测量残余相减，可以得到伪距与伪距率星间差[10]：
式中A为量测矩阵，v为测量噪声。
2 基于等价权原理的抗差算法
2.1 等价权原理
设观测向量为L，则系统的误差方程为[5，11]：
式（9）中，V为n维残差向量，A为量测矩阵，则状态量的最小二乘估计解为：
其中，B为先验权矩阵，由式（9）可知，最小二乘估计是由观测序列{Li}对参数{xi}进行估计，所以观测值会直接影响状态估计结果。为了抵制异常值对{xi}的影响，应用抗差M估计，可建立如下准则函数：
对式（11）求极值解，则得到参数向量的抗差M估值为：
X=（ATBA）-1ATBL（12）
可见，式（12）只是将式（10）最小二乘估值中的权矩阵换成了等价权矩阵B，其迭代形式为：
Xk+1=（ATBkA）-1ATBkL（13）
2.2 IGG方案权函数
IGG方案对应的权函数采用三段法，即保值域直接采用量测值作为解算变量，降值域降低观测值的权从而部分采用量测数据，拒绝域通过给等价权Bi赋零从而完全抑制粗差的影响，通过对数据质量进行针对性处理实现最优估计[12]，IGG方案等价权函数为：
式中，vi为基于观测残差的标准化残差统计量。k0，k1为常量，一般取k0=1.0，k1=2.5:8.0，|vi|≤k0为保值域，k0< |vi|<k1为降值域，|vi|≥k1 为拒绝域，函数图形如图1所示。
3 抗差UKF滤波算法设计
根据UKF滤波采用UT变换对称采样可知，这种方法采样得到的Sigma点完全取决于前一时刻系统的状态估计值，如果组合导航系统在前一时刻的状态估计误差较大，则会导致结果严重失真[6]。根据等价权替换，构造基于IGG方案的抗差UKF算法，用等价权替换迭代方程权值，则更新序列的协方差矩阵为：
其中：Rk为Rk的等价协方差矩阵，可以由等价权矩阵求逆获得，即：Rk=（Bk）-1，于是与等价协方差阵有关的变量修正如下：
卡尔曼滤波增益：
根据分析，抗差UKF算法的滤波方程只需要在标准UKF算法的基础上，对噪声协方差阵Rk进行其等价协方差替换，通过等价权替换调节卡尔曼滤波增益Kk，从而使滤波方程剔除观测粗差实现估计的有效性。
4 紧耦合系统的抗差UKF算法实验
本文建立紧耦合系统状态方程及量测方程，利用四元数法求解惯导系统微分方程，分别采用UKF和抗差UKF进行数据融合。本次实验直接采用地理坐标系作为参考坐标系，实验的初始条件及常量设定值：地球自转角速度：e=7.292 115 146 7×10-5 rad/s；基准椭球体的长半径：a=6.378 137×10-6 m；椭球偏心率为：0.081 8；基准重力加速度常量：g0=9.8 m/s2。
本次实验设定载体处于恒加速运动状态，载体加速度an=[3 4 0]T，采样间隔取Ts=0.01 s，仿真时间为tall=400 s，每100个采样点进行一次数据融合输出，载体初始位置在WGS-84地心地固直角坐标系下设定为：x0= -1 710 547 m，y0=4 933 560 m，z0=3 569 065 m。
初始方向余弦矩阵C=1 0 00 1 00 0 1
状态变量的初值设为：
x=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]T
状态变量的协方差阵初值为：
p0=diag[1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]
系统噪声阵为：
Q=diag[10 10 10 0.1 0.1 0.1 0.01 0.01 0.01 1 1 1 1 1 1]
量测噪声阵为：
R=diag[10 10 10 0.1 0.1 0.1]
GPS数据由星历参数模型计算而来，对伪距和伪距率分别加上均方根为1 m和0.1 m/s的高斯白噪声来模拟GPS误差。惯导数据由芯片ADIS16365采集得到，运动偏移稳定性为0.009°/s，陀螺仪零偏稳定度为90°/h，加速度计精度为3.333 mg，陀螺仪精度为0.05°/s。实验取等价权函数区间常量k0，k1分别为1.5、3。为了检测抗差算法的估计效果，本次实验在历元m=10|20|30|40|50|51|70|71|90|91|110|111|112|200|201|202|300|301|302时将观测量调整为原值的2.5倍，用来模拟现实粗差的干扰，然后分别利用标准UKF和抗差UKF进行滤波输出，结果分别如图2、3所示。
4.1 姿态误差估计结果对比图
图2姿态估计结果显示，标准UKF在粗差历元处姿态估计出现明显跳变，随着载体的运行，航向角yaw仍会收敛到36.7°，但估计异常却十分明显。而抗差UKF算法在实现准确姿态估计的同时能有效地抑制粗差干扰。为量化分析姿态结果的改善情况，对仿真结果从50 s以后进行误差统计如表1所示。
4.2 状态误差估计结果对比图
考察图3位置估计结果，2.5倍的观测量异常使得UKF位置结果抖动明显。同样以东向位置Pe为例计算误差均方根，标准UKF算法为99.65 m，而抗差UKF算法为53.97 m，可见抗差UKF算法抑制粗差效果明显。
4.3 大地坐标系下的输出结果
图4为大地坐标系定位结果显示，抗差UKF算法成功地实现了结果输出，定位结果为北纬34.243 8°、东经108.908 9°、高度396 m，系统输出结果与理论计算值相同。
5 结论
以等价权抗差估计算法结合非线性滤波理论，提出了基于伪距、多普勒频移观测量的抗差UKF定位算法。实验表明，在观测量异常时，标准UKF算法不能抑制粗差，而抗差UKF算法能成功实现对观测量粗差的探测和剔除，定位输出的经度、纬度、高度结果表明，抗差UKF算法是一种性能稳定的数据整合定位算法。
参考文献
[1] 谢钢.GPS原理与接收机设计[M].北京：电子工业出版社，2009.
[2] 董绪荣，张守信，华仲春.GPS/INS组合导航定位及其应用[M].长沙：国防科学技术大学出版社，1998.
[3] 刘建业，曾庆化，赵伟.导航系统理论与应用[M].西安：西北工业大学出版社，2009.
[4] JWO D J， CHUNG F C. Fuzzy adaptive unscented Kalman filter for ultra-tight GPS/INS integration[C]. International Symposium on Computational Intelligence and Design， Taiwan， 2010：229-235.
[5] 杨元喜.卫星导航的不确定性不确定度与精度若干注记[J].测绘学报，2012，41（5）：646-650.
[6] JULIER S J， UHLMANN J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE Aerospace and Electronic Systems， 2004，92（3）：401-422.
[7] 杨元喜.自适应动态导航定位[M].北京：测绘出版社，2006.
[8] 邓正隆.惯性技术[M].哈尔滨：哈尔滨工业大学出版社，2006.
[9] 唐康华.GPS/MIMU嵌入式组合导航关键技术研究[D].长沙：国防科学技术大学，2008.
[10] JULIER S J. Estimating and exploiting the degree of independent information in distributed data fusion[C]. 12th International Conference on Information Fusion Seattle， WA， 2009：772-779.
[11] BAE J， KIM Y. Nonlinear estimation for spacecraft attitude using decentralized unscented information filter[C]. International Conference on Control Automation and Systems， Gyeonggi-do， 2010：1562-1566.
[12] 杨元喜.自适应抗差滤波理论及应用的主要进展[C].中国测绘学会第九次全国会员代表大会暨学会成立50周年纪念大会论文集，北京，2009：37-47.