基于动态判断的组合导航性能改进方法

丛 丽，王音心，路 辉

(北京航空航天大学电子信息工程学院，北京 100191)

基于动态判断的组合导航性能改进方法

丛 丽，王音心，路 辉

(北京航空航天大学电子信息工程学院，北京 100191)

针对微机电系统(Micro Electro Mechanical System，MEMS)-惯性导航系统(Inertial Navigation System，INS)/全球定位系统(Global Positioning System，GPS)组合导航在城市车载导航应用中的定姿性能和GPS中断时的定位性能精度较差的问题，提出了一种基于动态判断的组合导航性能改进方法。该方法将车辆的动态分为三种，并设置了车辆在各个动态下的姿态约束和运动模型，然后利用惯性元件的输出判断出当前动态，并选择对应动态的姿态约束实时修正MEMS-INS定姿结果。当GPS中断时，则选择相应的运动模型进行定位推算，以便控制MEMS-INS定位误差。通过实验证明，该方法有效改进了MEMS-INS/GPS车载组合导航的性能。

惯性导航系统；动态判断；姿态约束；运动模型

0 引言

车载导航应用要求导航系统能够提供较高精度的位置、速度和姿态信息。在城市车载导航中，考虑到成本问题，一般选取微机电系统(Micro Electro Mechanical System，MEMS)-惯性导航系统(Inertial Navigation System，INS)与全球定位系统(Global Positioning System，GPS)进行组合定位。

受制作工艺的影响， MEMS-INS精度较低，其误差随时间积累严重。普通GPS接收机在卫星可见性较好的情况下能提供较为准确的位置、速度信息，来修正MEMS-INS的定位误差，但却无法准确修正MEMS-INS姿态误差。因此，需要对MEMS-INS的姿态误差进行修正，来提高车载动态姿态测量的精度及可靠性。在与GPS组合的条件下一般采用多天线GPS测姿[1]方法和单天线GPS测姿方法[2]，但前者存在系统结构和算法比较复杂、成本较高、安装难度大等理论和技术问题，后者在速度较低时定姿效果会受到影响，都具有一定的局限性。

同时，在城市环境中由于建筑物的遮挡，GPS信号易发生中断，此时MEMS-INS的快速漂移误差会使MEMS-INS/GPS组合导航系统的定位性能迅速恶化[3]。因此，GPS信号中断时MEMS-INS定位误差的控制，也是车载GPS/MEMS-INS组合导航需要解决的重要问题。在GPS 中断时的定位方法，其基本的类型主要有两类。一是通过对MEMS/INS系统中的惯性传感器误差建模来防止中断时导航性能恶化，例如设计了基于自适应模糊模型的INS/GPS组合，能通过融合INS和GPS位置和速度数据来估计INS误差并提升INS定位精度[4]。二是通过车辆运动约束来减少定位误差，比如带非完整约束的陆地车辆运动模型[5]。由于低成本MEMS-INS的误差较大，并且容易受环境影响，因此采用神经网络等复杂非线性模型进行INS误差建模的方法，会存在计算量较大、实时性较差、模型环境适应能力差等问题，因此采用车辆运动约束来控制MEMS-INS误差的方法更具优势。

为了将以上两个定姿和定位问题的解决相结合，又考虑到城市环境道路比较平坦，车辆运动模式也相对简单，如果能够对车辆运动状态进行准确判断[6]，对车辆运动状态的约束将会更加准确，从而进一步改进MEMS-INS定姿性能和GPS中断时的MEMS-INS定位误差控制效果。本文针对上述问题，设计了一种基于动态判断的MEMS-INS /GPS车载导航性能改进方法。

1 MEMS-INS/GPS组合原理与惯性元件输出

本文采用GPS/INS紧组合模式，用Kalman滤波算法以及反馈校正方式实现信息融合[7-8]。首先是MEMS惯性测量单元(Inertial Measurement Unit,IMU)实时输出载体的陀螺仪和加速度计信息，然后INS解算模块根据力学编排方程获得实时的INS定位和定姿结果。其中位置的初始值由GPS定位结果提供，姿态的初始值由调平法加磁罗盘辅助获得[9]。同时将GPS的伪距和伪距率作为观测量对INS导航误差进行滤波，最后获得的误差估计来修正INS导航结果以获得组合导航输出，然后将修正后的导航结果反馈回INS解算模块，再进行下一时刻的解算和滤波。

其中，MEMS-IMU输出的参数是加速度计和陀螺仪输出的三轴加速度与角速度，分别表示为fb=[fbx,fby,fbz]和ωb=[ωbx,ωby,ωbz]，b为载体坐标系，该坐标系X轴指向载体右侧，Y轴沿载体纵轴方向向前，Z轴垂直于载体竖直向上。本文采用带非完整运动约束的陆地车辆运动模型，该模型假设车辆在行驶过程中，运动方向的法向和天向没有速度，忽略地球自转并将重力加速度设为恒定。利用该模型可以推出加速度计和陀螺仪输出参数与车辆动态的关系[10]：

(1)

(2)

式中，vf为车辆行驶速度，(θ,γ,ψ)为车辆俯仰角、横滚角、航向角，g为重力常数。

2 基于动态判断的车载组合性能改进方法

2.1 总体设计

在第一章介绍的MEMS-INS/GPS组合流程基础上，本文设计了GPS/INS组合模式和GPS中断两种模式，如图1所示。

当存在GPS信号时，在紧组合模式中加入了动态判断模块，利用IMU输出数据判断车辆当前运动状态，分别是匀速直线(静止)、变速直线以及转弯。然后结合INS姿态解算结果和IMU输出数据，选择相应动态的姿态约束公式获得最终姿态角，并用其校正INS姿态误差。

当GPS中断时，则采用另一套GPS中断定位模式。该模式中定姿方法与GPS/INS组合模式相同，而取代紧组合的定位方法则是在动态判断后利用不同动态状况选择相应运动模型进行定位推算，最后将最终获得的位置、速度、姿态约束结果反馈给INS解算模块。

图1 总体结构示意图Fig.1 The overall structure scheme

2.2 动态判断

本文将城市车辆基本动态类型划分为以下三种：1)匀速直线运动(包括静止)，该状态下姿态角和速度基本不发生变化；2)变速直线运动，该状态下姿态角基本不发生变化，会有速度变化；3)转弯运动，该状态下航向角有较大变化。通过式(1)、式(2)，推出以上3种状态下的惯性元件近似输出如下：

通过以上不同动态间的输出差别，定义两种动态数据如下：

其中，tk为当前离散时间，n为平滑统计的离散时间点数目。

这里的判断条件有两步，首先判断匀速和变速；如果是变速，再判断是直线或是转弯运动。采用门限判断的方法。其判断条件如下：

上式表示了三种运动状态的判断方式，这里的门限λ1、λ2从原理上应该是由加速度计和陀螺仪生成公式推出的，但是由于低成本惯导的噪声较大而且难以预测，因此这里是通过同一惯导的多次实验数据获得。

2.3 不同动态下的姿态约束和运动模型

当车辆运动状态确定后，需要选择对应状态下的姿态约束来对INS定姿结果进行修正。同时GPS中断时，也需要根据动态判断结果选择对应动态的运动模型进行定位。以下将介绍三种运动状态下的姿态约束和运动模型。

1)匀速直线或静止状态

航向角难以直接约束，但是该状态下航向角基本不会发生变化：

ψ(ts)=ψ(ts-1)

其中，ts表示当前路段采样时间。

该动态的特点是速度不变，因此运动模型采用地理坐标系下的速度模型：

其中，RM和RN分别为子午圈半径与卯酉圈半径，λ,φ,h为车辆经度、纬度、高度，(ve,vn,vu)为东向、北向、天向速度。

2)变速直线状态

该状态下的姿态角可以通过测得的组合速度进行计算[11]：

γ=arccos(l·p)/(|l·p|)

其中，提升加速度矢量l为载体法向加速度an减去重力法向加速度gn，即l=an-gn，水平参考矢量p为重力矢量g乘以载体速度矢量vg，即p=g×vg。载体加速度信息可以通过速度的Kalman滤波模型估计得出。

当车辆进行变速直线运动，其速度随着路况会不断变化，因此运动模型采用地理坐标系下的加速运动模型：

由于实际车辆运动中加速度并非恒定，需要通过实时加速度修正模型。从式(2)推出非完整约束模型所获得的加速度：

用该加速度实时修正模型中的加速度。

3)转弯运动状态

该状态下俯仰角和横滚角与变速直线运动相同。对于航向角，可通过式(1)进行近似计算。航向角变化率：

最终计算航向角为：

其中，ti为姿态约束转弯段起始时刻，tj为当前时刻。

这里的运动模型采用式(8)的速度模型，但是由于转弯时速度会发生变化，因此需要时刻修正模型中的速度，该速度由式(2)推出:

3 实验测试与验证

现场测试是在城市环境中验证GPS/MEMS-INS组合导航的性能。图2给出了MEMS-IMU/GPS/磁罗盘组合导航实验系统组成框图。其中GPS包括天线、射频前端、高速数据采集器，一个MEMS-IMU提供惯性传感器输出参数，磁罗盘提供初始定位航向角。这三套装置通过串口与计算机连接，利用配套的MEMS-INS/GPS组合导航软件进行操作并输出定位结果。

这里使用的GPS接收机是实验室自主研发的软件接收机SatRecv。它的输出频率是25Hz，定位精度是5m，定速精度是0.2m/s。MEMS-IMU是BeijingStarnetoTechnologyCo.Ltd.(Beijing,China)生产的XW-IMU5220。它的陀螺零偏稳定性是0.02 (°)/s，加速度计零偏稳定性是8mg,输出频率是100 Hz。磁罗盘的定向精度是1.5°，输出频率是10 Hz。参考系统为NovAtel的SPAN，其输出频率是100 Hz,单点定位精度约1.5 m，定速精度是0.02 m/s，测姿精度俯仰角为0.05°，横滚角0.05°，航向角0.01°。

图2 GPS/INS组合导航实验系统Fig.2 The test GPS/INS integrated navigation system

本次现场测试路段为北京市奥林匹克森林公园附近，如图3所示，实验时间约500 s，其中含有2个转弯，行车过程中因为路况原因会出现一些静止情况。

在上述测试环境下，我们首先需要判断整个过程的车辆运动状态。图4所示为本次测试中动态判断结果。我们每0.5 s判断一次运动状态，这里的运动状态分别是匀速直线(或静止)状态、变速直线状态以及转弯状态。基于多次实验数据，获得的匀速与变速判断门限值λ1为0.03g，直线与转弯判断门限值λ2为0.07 (°)/s。用实线代表动态判断结果，并利用参考系统的输出速率和转弯角速度作为动态判断依据。

从图4可知，在真实行车过程中，基本不存在完全的匀速行驶状态，因此这里判断的匀速直线运动都是静止状态。通过速度、转弯角速度与载体运动类型可以看出，基本上能正确判断出静止、直线和转弯状态。这里共有2个弯和2个长静止段，在350 s左右有个较为短暂的静止部分。

为了证明姿态约束方法对定姿性能的提升，将INS定姿结果和采用姿态约束后的结果进行对比，对比结果如图5-图7所示。

由图5-图7可看出，INS单独定姿会产生较大的姿态积累误差，导致定姿效果变差。在加入姿态约束后，姿态精度有了明显提升，具体的精度统计结果见表1。

图3 实验路线 (来自谷歌地球)Fig.3 Test trajectory (from Google Earth)

图4 动态判断结果Fig.4 Results of vehicle dynamics identification

图5 俯仰角测量结果对比Fig.5 Estimation result comparison of the pitch

图6 横滚角测量结果对比Fig.6 Estimation result comparison of the roll

图7 航向角测量结果对比Fig.7 Estimation result comparison of the yaw

定姿方法姿态角误差均方根/(°)俯仰角横滚角航向角INS定姿6.354.567.61姿态约束1.510.771.37

表1展示了整个测试过程中采用姿态约束前后的定姿精度。可知，加入姿态约束能提升姿态角精度。最后获得的姿态角相对于INS单独定姿，俯仰角提升了76%，横滚角提升了83%，航向角提升了82%。

为了验证GPS中断时的定位性能，这里仿真4个GPS中断的情况，如图8所示。第一段包括变速直线和转弯部分，中断40 s，第二段包括变速直线部分，中断45 s，第三段包括变速直线和转弯部分，中断50 s，第四段包括变速直线和静止部分，中断100 s。

图9给出了存在GPS中断时的定位路线结果。并对使用模型推算和INS单独定位的导航结果进行对比。

图 8 仿真GPS中断路线图Fig.8 Trajectory with simulated GPS outages

图9 仿真GPS中断部分定位结果Fig.9 Estimation results of position during simulated GPS outages

由图9可知，当GPS中断后，由INS单独定位，定位结果漂移将会非常严重，而采用模型推算后能保证定位结果与参考路线基本一致。具体的定位精度对比由表2所示。

表 2 GPS中断时定位精度对比

Tab.2 Comparison of position estimation accuracy comparison during GPS outages

方向测试段位置误差均方根/mINS单独定位模型推算东向157.979.572153.976.843182.246.104672.579.87北向172.854.20247.219.353631.488.334302.529.57天向118.537.21238.906.333223.875.90487.199.21

由表2和图9可以看出，相对于INS单独定位，模型推算能大幅度减小定位误差，并保证在40 s内定位误差不超过10 m。尤其是存在静止段时，能在更长的时间段内保持较好的定位精度。

4 结论

本文提出了基于动态判断的陆地车辆导航性能提升方法。该方法首先利用加速度计和陀螺仪输出的数据进行陆地车辆动态判断，然后选择对应的姿态约束来修正MEMS-INS姿态角。在GPS中断时，根据实时的动态判断结果选择相应的运动模型进行导航状态推算，从而有效控制MEMS-INS定位误差。最后通过跑车实验证明，该方法有效改进了MEMS-INS/GPS车载组合导航的性能。下一步可加入爬坡、下坡、爬坡转弯的运动状态的判断和运动模型的设计，进一步满足车辆运动环境的需求。

[1]Hwang D H, Sang H O, Sang J L, et al. Design of a low-cost attitude determination GPS/INS integrated navigation system[J]. GPS Solut, 2005, 9(4)：294-311.

[2]王辉.GPS单天线测姿系统研究[D].南京：南京航空航天大学,2005.

[3]Atia M M,Noureldin A,Korenberg M.Gaussian process regression approach for bridging GPS outages in integrated navigation systems[J]. Electronics Letters, 2011, 47(1):52-53.

[4]Noureldin A,El-Shafie A, El-Sheimy N. Adaptive neuro-fuzzy module for inertial navigation system/global positioning system integration utilising position and velocity updates with real-time cross-validation[J]. Iet Radar Sonar Navigation, 2007, 1(5):388-396.

[5]Brandt A, Gardner J F. Constrained navigation algorithms for strapdown inertial navigation systems with reduced set of sensors[C]// Proceedings of the American Control Conference, 1998(3):1848-1852.

[6]Wang J H, Gao Y. Land vehicle dynamics-aided inertial navigation[J]. Aerospace & Electronic Systems IEEE Transactions on, 2010, 46(4): 1638-1653.

[7]Li Y, Wang J, Rizos C,et al.Low-Cost Tightly Coupled GPS/INS Integration Based on a Nonlinear Kalman Filtering Design[J]. Institute of Navigation National Technical Meeting 2006：958-966.

[8]Petovello M. Real-time Integration of a Tactical-Grade IMU and GPS for High-Accuracy Positioning and Navigation[D]. Calgary :University of Calgary , 2003.

[9]翟海廷. 低成本惯导与单天线GPS高精度紧组合定位定姿技术[D].北京：北京航空航天大学,2012

[10]Dissanayake G, Sukkarieh S, Nebot E, et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J]. IEEE Transactions on Robotics & Automation, 2001, 17(5):731-747.

[11]翟海廷, 丛丽, 秦红磊,等. 基于单天线的MEMS-INS/GPS组合定姿方法[J]. 计算机工程与设计, 2012, 33(10):3999-4003..

Modified Method of Integrated Navigation Performance Based on Dynamics Identification

CONG Li,WANG Yinxin,LU Hui

(School of Electronic and Information Engineering,Beihang University,Beijing 100191, China)

For the problems of low-precision attitude determination performance and positioning performance during global positioning system (GPS) outages of micro electro mechanical system-inertial navigation system (MEMS-INS)/GPS integration in urban vehicle application,a method based on land vehicle dynamics was proposed. This method divided the vehicle dynamics into three types,and set up vehicle attitude constraints and motion model in each dynamics. Then, the vehicle dynamics by the INS sensors output was identified，and corresponding attitude constraints to modify the result of INS attitude determination was selected. During GPS outages, corresponding model was selected to control INS positioning error.Through the land vehicle experiments,it was indicated that the proposed methods could effectively modify MEMS-INS/GPS navigation performance.

inertial navigation system;dynamic identification; attitude constraint

2016-12-05

国家自然科学基金项目资助(61101077)

丛丽(1981— )，女，北京人，博士，讲师，研究方向：卫星导航、组合导航和战术导航。E-mail:congli_hlj@163.com。

TN967.2

A

1008-1194(2017)02-0001-06