柴艳菊，欧吉坤，袁运斌，阳仁贵，钟世明，王海涛

中国科学院测量与地球物理研究所动力大地测量学重点实验室，湖北武汉430077

如果差分Doppler测速方差阵为ΣVg，则方位角中误差估计公式为

1 引 言

GPS／INS组合导航技术能为用户提供连续的高精度位置、速度和姿态等导航信息，在军、民用领域具有广阔的应用前景［1－3］。常用组合方式（松组合或紧组合）的不足是建立的滤波观测模型与姿态误差、INS元件误差等参数无关，这类参数必须依赖状态预报模型中与位置误差、速度误差等参数之间的关系来估计，又称为间接可测参数。为提高这类参数估计的精确性，以往多从改进滤波状态预报模型方面进行研究，如对状态参数噪声利用波估计理论建模、利用状态参数误差变化区间构造状态预报精度的扩展区间Kalman滤波、基于神经网络的自适应滤波、将抗差估计与Kalman滤波结合的一类抗差自适应滤波等［4－8］。这些滤波技术的关键是将速度误差分解为姿态误差引起的部分、元件误差引起的部分和初始速度误差部分［9］，即准确获取这些参数的先验精度。由于状态参数先验精度的确定由INS的噪声水平决定，而INS噪声与制造、使用、环境因素等有关［10］，因此这类参数的准确确定非常困难。

随着GPS多天线测姿技术的发展，许多学者提出附加GPS测姿信息的组合方式，如位置速度姿态全组合方式、附加基线约束的组合等［3，11－13］。这些组合方式需要多个GPS天线，不仅增加系统的成本，而且GPS多天线测姿精度与多条基线解算的精度及基线的长度成正比。由于载体本身范围的限制，多天线测姿的基线一般比较短，虽然基线解算精度比较高，但是测姿精度不高［12－15］。

近年来，一些学者提出GPS单天线测姿技术［16］，即载体运动情况下，由GPS观测得到的速度和加速度结果计算载体运动的姿态角。由于俯仰角和翻滚角受风速、加速度误差等影响，精度比较低。方位角的计算只与水平方向速度有关，相对比较准确，其精度与水平方向速度的大小及精度有关，一般可以达到0.1°～0.4°［16－17］。文献［18］提出利用这一信息直接校正INS的方位角；文献［19］提出利用GPS得到的方位角和INS方位角之差来估计INS陀螺漂移误差。这些做法是将GPS的方位角作为参考值，而GPS得到的方位角精度不高，因此这些做法并不合适。

基于以上分析，对于单天线GPS／INS组合系统，提出如下处理方案：在静止时，采用经典组合Kalman滤波。在运动条件下，一方面，将GPS测得的方位信息作为虚拟观测值输入滤波器，形成附加方位约束的组合方式；另一方面，将选权自适应卡尔曼滤波方法拓展到组合系统中，以提高导航参数误差估计的精确性。

2 GPS单天线测方位原理及精度估计［16］

假设地理系中由GPS计算的速度矢量为Vg＝［VgxVgyVgz］T，载体运动的速度矢量为Va＝［VaxVayVaz］T，则有关系Vg＝Va＋Vf，其中Vf＝［00V3］T风速矢量。

载体的方位角为

式中，Va的水平分量可以用Vg分量代替，将上式微分得

如果差分Doppler测速方差阵为ΣVg，则方位角中误差估计公式为

3 常用GPS／INS组合模型参数估值分析

地固系常用组合Kalman滤波模型可以表示为

由于松组合和紧组合观测方程的系数阵结构类似，可以采用统一的形式Hk＋1＝ ［H1O6×9］。松组合时H1＝I6为6阶单位阵，紧组合时H1＝为相位观测方程位置参数对应的系数阵，O为零矩阵。由Hk＋1可知，在观测方程中仅有位置、速度误差参数为直接可测参数，其他状态参数为间接可测参数。下面对这些间接可测参数的估值特点进行分析。

组合的目的是利用GPS的观测信息来不断估计并校正INS的导航误差。当GPS观测可用时，相信GPS的观测信息，即HT1Pk＋1H1≫P1，同时要求HT1Pk＋1H1≫P2P－13PT2，即忽略间接可测参数预报值对直接可测参数估计的影响。增益矩阵可以近似写成

联合式（8）、（10），可得

4 附加方位约束的松组合

4.1 附加方位约束的松组合模型

在运动条件下，将GPS单天线测得的方位角信息纳入滤波观测方程，形成新的附加方位约束的松组合模式，模型仍用式（5）表示。其状态模型不变，观测方程表达式变为

式中，虚线以上为经典松组合观测方程，下面为附加的方位角约束方程；rI、VI、yawI及rG、VG、yawG分别为INS和GPS计算的位置、速度和方位角；（3）为矩阵的第3行元素。

从式（12）可知，附加GPS测方位角约束后，相当于增加与姿态相关的外部观测信息，部分平台角误差在观测方程中成为直接可测参数。

4.2 确定附加方位约束观测的权

附加的约束观测对参数估计的作用由附加观测值的精度决定，对附加约束观测值进行合理定权（或方差）是该方法的关键。如果对约束观测值定权偏大，属于强制附合，不仅达不到约束的目的，甚至歪曲估值结果。

附加方位约束的松组合观测值方差阵的确定取决于GPS定位、测速和测方位角的精度。k＋1时刻观测值方差阵Rk＋1的表达式可写成

diag（·）表示对角阵。对于短基线情况，模糊度准确固定后，位置观测量中误差估值可取为厘米级；差分doppler测速中误差可取为cm／s级；方位约束观测值中误差可由式（4）实时估计。

5 GPS／INS组合模型中选权自适应卡尔曼滤波方法的拓展

从式（12）可知，附加GPS方位约束后，观测信息仍不足以估计全部状态参数。为减弱状态预报值不准对滤波估值的影响，将近年来课题组提出的一种克服状态模型异常的选权自适应卡尔曼滤波法拓展到组合滤波中。其模型与抗差自适应滤波相同，区别在于状态参数等价权因子阵的确定充分考虑到状态个体的差异，而不是采用一个等价权因子对所有的状态参数进行自适应降权处理，有关抗差自适应滤波的详细介绍参见文献［7—8］。该方法在GPS快速精密点定位、网络RTK系统误差的分离等方面取得了比较好的效果，主要是充分利用了模糊度为时不变参数及网络RTK中参考站坐标比较精确的先验信息［21］。针对GPS／INS组合系统的特殊情况，下面详细介绍构造等价权因子阵w的新思路。

选权自适应卡尔曼滤波用方差阵的表示形式为［22］

式中，w－1是对角阵。

载体静止时，采用经典滤波，因为INS误差状态方程是在平稳状态下建立的，滤波估值在状态预报值和观测值之间达到平衡是合理的，此时等价权因子阵取单位阵。

载体运动后，平稳状态下建立的INS误差模型难以准确表示复杂的运动状态，需要根据具体条件不断调整状态预报值的方差阵。由式（12）可知，观测方程中只有位置参数、速度参数和部分平台角误差参数直接可测，其等价权因子利用状态参数不符值来构造；其他参数为间接可测参数，这类参数的等价权因子的构造依赖状态参数的先验信息。松组合观测模型中，由于位置、速度误差参数不符值等于相应的观测值。这类参数的等价权因子阵的形式可写为

平台角误差参数预报值权因子的确定相对比较复杂，首先利用坐标转换及协方差传播律得到姿态误差角参数及其预报误差方差阵Σϑ，其关系为

式中，Σφ为状态预报误差方差阵Σk，k－1中平台误差角对应的部分。一般车辆在平坦的地面上运动时，俯仰角和翻滚角变化比较小，δpitch和δroll相对比较小，对应的等价权因子取1。由于方位角变化比较快，其误差受运动动态性（转角大小和加速度变化等）影响比较复杂，尤其在急速转弯时，因此这里只对方位角误差参数进行自适应估计。其等价权因子构造为

姿态误差角等价权因子阵wϑ确定后，将其转换成平台误差角的权因子阵wφ，转换公式

根据元件误差的特点，对于1s的更新率，元件误差变化比较小，其权因子全部取1。全部状态参数的等价权因子阵形式可写为

w阵确定后，利用式（14）可以确定全部状态参数等价预报方差阵，取代滤波公式中原状态预报方差阵（或转换成权阵），得到新的滤波解。

6 算例分析与讨论

试验数据采集于2002－08－30某经济开发区，由于此处道路平坦，高楼较少，INS动态环境和GPS观测条件比较好。试验设备：一辆面包车、两台Trimble 4700Total Station GPS接收机（一台基准站，一台流动站），基线平均长度为1km，一套iMar INAV－FMS INS。GPS和INS的技术指标为：GPS观测值包括C／A码、双频载波相位和Doppler观测，采样率1Hz；INS陀螺漂移1°／h，陀螺随机游走加速度计偏置1× 10－5m／s2，采样率100Hz。同步观测共767个历元。载体先静止约4.5min，然后以约60km／h的速度运动，运行轨迹包括直线、转弯、复杂的“8”字等，轨迹如图1。

图1 流动站运动轨迹图Fig.1 The trajectory of the rover station

为了说明新方案的效果，采用如下三种处理方案：经典松组合方案；附加方位约束的抗差自适应滤波（因为观测量少于参数个数，自适应因子采用预报残差构造［7］）；附加方位约束选权自适应卡尔曼滤波。

下面比较三种方案得到的位置差和速度差（INS校正后的位置、速度与GPS定位、测速之差）如图2、图3。当模糊度准确固定后，一般差分GPS相位定位精度可达厘米级，差分Doppler测速精度为cm／s级，因此位置差和速度差反映INS经过误差校正后导航参数的精确性。位置差和速度差统计结果见表1。

图2 三种方案计算的位置差Fig.2 The position differences of the above three programs

图3 三种方案计算的速度差（采样间隔1s）Fig.3 The velocity differences of the above three programs（interval of 1s）

表1 三种方案位置差和速度差统计结果比较Tab.1 The statistic results of the position and velocity differences of the three programs

比较图2、图3及表1中三种方案的统计结果可知，方案3得到的位置差和速度差变化及其最大值、最小值、均值和标准差比方案1和方案2结果明显减小，这说明新方案输出的滤波结果与GPS结果更接近，精确性得到提高。方案2结果比方案1有改善，各方向的位置精度可以提高50%以上，速度精度提高30%左右，而方案3各方向的位置和速度精度比方案1提高均在70%以上，主要是抗差自适应滤波确定的状态预报值的等价权因子忽略了参数之间的差异，当只有个别参数出现量级不大的异常时，平均后可能判别不出异常，而GPS测得的方位角精度比较低，限制了GPS信息的作用。

下面对三种方案结果进行详细分析：首先分析经典组合滤波结果存在的问题，然后通过比较三种方案的增益阵及载体系（三轴指向右前上）X方向的速度，进一步阐述新方案结果的合理性。

以往许多文献指出INS姿态误差受载体的机动性（尤其是转角变化）影响比较大，首先比较方案1输出的方位角差（系统输出的方位角与由GPS测速计算的方位角之差）和1s内转角变化来分析该方案姿态误差估计的不合理性，其结果见图4。

图4 松组合方位角差与1s内转角关系Fig.4 The relation between the yaw difference and the rotation angle within 1second

图4表明方位角差变化曲线与1s内θ／10的变化曲线相似，当｜θ｜大时（急转弯），方位角差也比较大，即与实际方位角偏离比较远，这主要是因为大转弯情况下姿态误差估计不准造成的。由于INS姿态误差参数的估计由滤波中的增益矩阵决定的，下面以开始转弯时刻391历元（1s内转过0.19°）和急速转弯时刻398历元（1s内转过14.6°）为例，分析计算姿态误差所对应的增益阵Kφ（式（7）计算的增益阵Kk＋1中第6—9行对应部分）的变化情况。

上述结果表明，虽然391历元和398历元1s内转角差异比较大，但是增益矩阵的差别并不大，因此姿态误差估计Kφ×L相差不大，这也正是运动后姿态误差估计表面上趋于稳定的原因，显然经典滤波估计出的姿态误差估值在大转角情况下偏小，使姿态误差校正滞后。

采用新方案的改进主要体现在滤波增益阵的变化，下面随机取运动后第489历元，比较3种方案增益阵的变化，进一步阐述新方案的合理性。根据INS各类误差的特点，只对前9个参数构造等价权因子阵，其他参数等价权因子为1，因此下面只列出增益阵的前9行。

方案1前9×6阶增益矩阵为

方案2前9×7阶增益矩阵

方案3前9×7阶增益矩阵

方案3的状态预报方差的等价权因子阵为：位置、速度参数对应的为w＝diag（0.069 0，0.018 90，0.108 5，0.563 5，0.123 4，0.146 4）；姿态角对应的为wϑ＝diag（1，1，0.148 4）；其他参数对应的全为1。方案2确定的等价权因子为1。

比较三种方案的增益阵可知，方案3与方案1和2的增益阵主要区别为：方案3的位置和速度误差参数对应的增益矩阵对角元接近1，其他元素比较小，因此INS位置和速度误差滤波估值接近观测值的最小二乘估值，经过误差校正后INS位置和速度接近GPS定位和测速结果，为下一个历元提供比较准确的积分初值，这符合组合思想。由于GPS测方位的精度不高，其方差取0.012rad2，而该历元方位角预报误差方差为0.0162rad2，平台角误差参数滤波估值接近预报值与观测值的平均值，这是合理的。

方案2的增益阵对角元介于方案1与方案3之间，因为方案2确定等价权因子依据的是状态参数不符值范数的平均值［20］，没有顾及参数之间的差异，虽然个别参数有异常，但是平均后表现为正常。因此要提高组合导航结果的合理性，不仅要在观测模型中增加观测信息，而且还要对观测信息、状态参数的先验预报信息配赋合理的精度。

由于b系中X方向的速度理论值为0，INS经过校正后的速度接近GPS测速精度，即cm／s级。如果姿态矩阵准确，Vbx的大小应该在cm／s量级。如果该量比较大，说明姿态矩阵不合理。3种方案计算的Vbx结果见图5。

图5 三种方案计算的b系X方向的速度估值比较（采样间隔为1s）Fig.5 The estimators of the velocity in Xdirection in body frame using the three methods（interval of 1s）

从图5可知，当载体运动后，方案2比方案1结果变化的幅度有所减小，而比方案3结果明显要大，这也说明方案3输出的姿态矩阵更趋合理。

综合以上对INS位置差、速度差及姿态结果的分析可知，方案3结果是合理的。

6 结束语

根据滤波的关键矩阵增益阵的特点，得出GPS／INS组合模型及Kalman滤波算法存在的主要问题是观测信息严重不足及状态参数在复杂运动情况下难以准确预报。提出将运动条件下GPS单天线测方位信息纳入滤波观测模型，以增加姿态误差的外部观测量；并将选权自适应卡尔曼滤波新方法拓展应用到组合数据处理中。新方案的关键是对新的观测值定权依据GPS定位、测速和测方位角的实时估计精度；对状态参数等价权因子的构造，直接可测参数依据状态参数不符值，而间接可测参数依据这类参数的误差变化特性。其优势是在机动情况下，状态参数估计更合理。最后采用车载GPS／INS实测数据，验证了新思路的有效性。

［1］ SKALOUD J.Optimizing Georeferencing of Airborne Survey Systems by INS／DGPS［D］.Calgary：University of Calgary，1999.

［2］ UM J.Relative Navigation and Attitude Determination Using a GPS／INS Integrated System near the International Space Station［D］.Austin：University of Texas，2001.

［3］ SUN Hongxing.DGPS／INS Integrated Position and Attitude Determination and Its Applications in MMS［D］.Wuhan：Wuhan University，2004.（孙红星.差分GPS／INS组合定位定姿及其在MMS中的应用［D］.武汉：武汉大学，2004.）

［4］ ZHANG X H.Integration of GPS with a Medium Accuracy IMU for Metre－level Positioning［D］.Calgary：University of Calgary，2003.

［5］ HE Xiufeng，YANG Guang.Extend Interval Kalman Filter and Its Applications in Nonlinear GPS／INS Integrated System［J］.Acta Geodaetica et Cartographica Sinica，2004，33（1）：47－52.（何秀凤，杨光.扩展区间Kalman滤波器及其在GPS／INS组合导航中的应用［J］.测绘学报，2004，33（1）：47－52.）

［6］ CHIANG K W.INS／GPS Integration Using Neural Networks for Land Vehicular Navigation Application［D］.Calgary：University of Calgary，2004.

［7］ YANG Y X，GAO W G.An Optimal Adaptive Kalman Filtering［J］.Journal of Geodesy，2006，80（4）：177－183.

［8］ WU Fumei.Error Compensation and Extension of Adaptive Filtering Theory in GNSS／INS Integrated Navigation［D］.Zhengzhou：Information Engineering University，2010.（吴富梅.GNSS／INS组合导航误差补偿与自适应滤波理论的拓展［D］.郑州：信息工程大学，2010.）

［9］ ZHANG Y F，GAO Y.Performance Comparison between Point and Differential GPS／INS Systems［C］∥ION GNSS 18th International Technical Meeting.Manassas：ION Inc，2005：2504－2510.

［10］ MOHAMED A H.Optimizing the Estimation Procedure in INS／GPS Integration for Kinematic Applications［D］.Calgary：University of Calgary，1999.

［11］ WANG Huinan，HE Feng.Real－time Kinematic Attitude Determination of Aircraft Using GPS Carrier Phase［J］.Chinese Journal of Space Science，1998，18（1）：69－74.（王惠南，何峰.利用GPS载波相位实时测定动态飞行器姿态［J］.空间科学学报，1998，18（1）：69－74.）

［12］ ZENG Yuanping.Research on the Attitude of GPS［D］.Nanjing：Nanjing University of Aeronautics and Astronautics，2003.（曾元萍.GPS测姿系统研究［D］.南京：南京航空航天大学，2003.）

［13］ LU G.Development of a GPS Multiple－antenna System for Attitude Determination［D］.Calgary：University of Calgary，1995.

［14］ NAYAK R A.Reliable and Continuous Urban Navigation Using Multiple GPS Antennas and a Low Cost IMU［D］.Calgary：University of Calgary，2000.

［15］ HARVEY R S.Development of a Precision Pointing System Using an Integrated Multi－sensor Approach［D］.Calgary：University of Calgary，1998.

［16］ WANG Hui.Research on Using Single－antenna GPS to Determine the Attitude of Aircraft［D］.Nanjing：Nanjing University of Aeronautics and Astronautics，2005.（王辉.GPS单天线测姿系统研究［D］.南京：南京航空航天大学，2005.）

［17］ FORD T，NEUMANN J，FENTON P，et al.Inertial：a Tightly Integrated Decentralised Inertial／GPS Navigation System［C］∥Proceedings of ION－GPS.Manassas：ION Inc，2001：3153－3163.

［18］ LIEVENS K P A，MULDERYAND J，CHUZ A P.Single GPS Antenna Attitude Determination of a Fixed Wing Aircraft Aided with Aircraft Aerodynamics［C］∥AIAA Guidance，Navigation and Control Conference.Virginia：AIAA Inc，2005：1－14.

［19］ SALYTCHEVA A O.Medium Accuracy INS／GPS Integration in Various GPS Environments［D］.Calgary：University of Calgary，2004.

［20］ CHAI Yanju.Theory and Method for Improving the Navigation Accuracy of GPS／INS Integration by Digging the Hidden Information［D］.Wuhan：Institute of Geodesy and Geophysics，2008.（柴艳菊.挖掘信息提高GPS／INS导航精度的理论与方法研究［D］.武汉：中国科学院测量与地球物理研究所，2008.）

［21］ LUO Xiaowen.The Study and Implementation of Network RTK Rapid Positioning Algorithms［D］.Wuhan：Institute of Geodesy and Geophysics，2007.（罗孝文.网络RTK快速定位算法研究与实现［D］.武汉：中国科学院测量与地球物理研究所，2007.）

［22］ ZHU Yaozhong，SUN Heping.Progress in Geodesy and Geodynamics［M］.Wuhan：Hubei Science and Technology Press，2004：816－823.（朱耀仲，孙和平.大地测量与地球动力学进展论文集［M］.武汉：湖北科学技术出版社，2004：816－823.）