微惯性/卫星组合导航系统算法研究

石 拓，陈家斌，田晓春，宋春雷,杨黎明，尹静源

(1.中电54所，石家庄 050081；2.北京理工大学自动化学院，北京 100081;.华北光学仪器有限公司，北京 100053；4.北京机电工程总体设计部，北京 100039)

微惯性/卫星组合导航系统算法研究

石 拓1,2，陈家斌2，田晓春2，宋春雷2,杨黎明3，尹静源4

(1.中电54所，石家庄 050081；2.北京理工大学自动化学院，北京 100081;.华北光学仪器有限公司，北京 100053；4.北京机电工程总体设计部，北京 100039)

微惯导系统(MINS)与卫星导航系统(GNSS)的结合不仅可以充分利用微惯导系统良好的短期性能和GNSS的长期稳定性，弥补MINS系统误差随时间积累和GNSS采样频率低、动态性能差的缺陷，而且使得组合导航系统的精度和动态能力同步提高，保证了系统在卫星信号受遮挡或失效的环境下仍能够以较高的精度自主运行一段时间，正常工作。采用星网宇达的产品采集真实数据进行了离线仿真，比较了多种滤波算法，实验证明，采用UKF取得的效果最好，EKF次之，KF最差。

微惯导；组合导航；Kalman滤波

0 引言

随着微机电系统(Micro-Electro-Mechanical System，MEMS)技术的日渐成熟，以及微型飞行器、微型机器人等微小型设备的广泛应用，低成本、小型化、低功耗的微惯性器件发展迅速[1-2]。微机电惯性导航系统(MEMS Inertial Navigation System,MINS)是指基于MEMS技术的惯性测量单元，是一种集成化的微型捷联惯导系统，与之前常用的惯性器件不同，MINS在保证测量精度不明显下降的前提下，体积和质量更小、成本更低。正是由于MINS系统的这些优点，使得MINS/全球导航卫星系统(Global Navigation Satellite System，GNSS)组合导航成为近年来研究的热点。

1 组合导航系统数学模型

对于MINS/GNSS组合导航系统，建立准确的滤波数学模型是采用滤波技术进行组合的基础。根据姿态误差(失准角)估计值的大小不同，可以将组合导航系统滤波数学模型分为线性模型和非线性模型[3]。在车载环境下，可将姿态误差角视为两种情况：一种是将三个姿态误差角都看成小角，建立线性模型；另一种是将俯仰角和横滚角看成小角，而将航向角看成大失准角，建立非线性模型。

设组合导航系统的状态变量为

XI= [φEφNφUδvEδvNδvUδLδλ

取MINS和GNSS输出的位置和速度之差作为组合导航系统的观测量。

1.1 线性系统的数学模型

对于线性系统，设其状态方程为

系统噪声向量由陀螺仪和加速度计的随机误差组成，表达式为

其中，FN为对应的9维基本导航参数矩阵，其非零项可由下述误差方程确定。

由于姿态误差角为小角度，即有[4]

则姿态误差方程为:

式中，下角标E、N、U分别为地理坐标系东、北、天3个方向；RM为当地子午面内主曲率半径；RN为与子午面垂直平面上的主曲率半径；L为地理纬度；λ为地理经度；h为地理高度；ωie为地球自转角速度。

速度误差方程为:

式中，fE、fN、fU为加速度计输出在导航坐标系下的投影。

位置误差方程为:

FS和FM的取值如下

1.2 非线性系统的数学模型

对于非线性系统设其状态方程为

车载环境下，通常俯仰角和横滚角为小角，航向角为大失准角，即有

则姿态误差方程为:

深化市县机构改革，夯实基层基础。市县机构改革必须面向人民群众、符合基层事务特点，在职能配置上更加突出民生，强化社会管理和公共服务职能，在机构设置上不简单照搬上级机关设置模式，既允许“一对多”，也允许“多对一”，确保上下贯通，执行有力。全面落实乡镇、街道相关改革，统筹推进第二批经济发达镇行政管理体制改革，实行扁平化和网格化管理，使各类机构、组织在服务保障群众需求上有更大作为。

sinφUεE-cosφUεN

位置误差与惯性器件误差与线性系统相同，这里不再赘述。

取SINS和GNSS输出的位置和速度之差作为量测量，其中，速度量测矢量为

式中

位置观测矢量为

其中

上述各式中，vGE、vGN、vGU为GNSS沿东北天方向上的速度误差；NGE、NGN、NGU为GNSS沿东北天方向上的位置误差。

联合速度和位置量测方程，组合导航的量测方程为

2 组合导航系统离线仿真

本车载实验是在北京理工大学中关村校区附近进行的，跑车路线是围绕北京理工大学中关村校区顺时针1周，起点和终点都是北京理工大学北门。实际路线如图1所示。

图1 实验跑车路线Fig.1 Experiment path

本实验仪器选用北京星网宇达科技股份有限公司所研发的惯性/卫星组合导航系统，根据该公司提供的软件，可以分别设置惯导和卫星的数据输出频率。本实验设定全球定位系统(GlobalPositioningSystem，GPS)的采样频率为0.5Hz，MINS的采样频率为50Hz。

实验将GPS给出的速度和位置与惯导解算出的速度和位置作差作为观测量，用Kalman滤波(KF)估计出系统的误差，再对惯导系统进行补偿。

图2 KF组合姿态Fig.2 KF combined attitude

图3 KF组合速度Fig.3 KF combined speed

图4 KF组合位置Fig.4 KF combined position

图2～图9所示为运用标准KF进行组合导航得到的结果。由图2可以看出，组合后的俯仰角和横滚角变化在3°左右，明显小于纯惯导解算的结果。图4所示为位置解算结果，红色代表GPS跑车路线，蓝色代表组合后的跑车路线，可以看出，组合后的位置比单独GPS给出的位置平滑很多，如在图中下部等红灯阶段，GPS数据产生了微小的跳变，而组合之后位置曲线很平滑，并且在GPS丢星的时间里，组合系统运用纯惯导数据进行解算，虽然存在一定的误差，但大致方向正确。图5所示为滤波估计出的姿态误差，可以看出俯仰角和横滚角误差最大在2°左右，但是航向角误差较大，最大值在5°左右。图6所示为滤波估计出的速度误差，水平方向的最大值在5m/s左右，天向能达到20m/s。滤波后的位置误差如图7所示，在GPS信号良好的情况下，组合导航的位置误差在5m以内；在无法获得卫星信号的大约20s时间段内，系统由微惯导导航，纬度误差达到了15m，经度误差约为25m，高度误差超过200m。另外组合导航系统还经过KF估计出了陀螺的常值漂移为100(°)/h，加速度计零偏为10mg。

图5 KF姿态角误差Fig.5 KF attitude error

图6 KF速度误差Fig.6 KF speed error

图8 陀螺常值漂移Fig.8 Gyro drift

图9 加计零偏Fig.9 Accelerometer bias

论文基于大方位失准角条件下的非线性误差方程，分别设计了扩展Kalman滤波(EKF)和无迹Kalman滤波(UKF)两种非线性滤波方法，仿真结果如图10～图17所示。

图10 EKF组合位置Fig.10 EKF combined position

图11 EKF姿态误差Fig.11 EKF attitude error

图12 EKF速度误差Fig.12 EKF speed error

图13 EKF位置误差Fig.13 EKF position error

图14 UKF组合位置Fig.14 UKF combined position

图15 UKF姿态误差Fig.15 UKF attitude error

图16 UKF速度误差Fig.16 UKF speed error

图17 UKF位置误差Fig.17 UKF position error

图10～图13所示为采用EKF方法进行组合导航的结果。图11所示的姿态角误差可以看出俯仰角和横滚角误差最大在1°左右，小于图5所示的结果，航向角也相对较小；水平方向的速度误差最大值在3m/s左右，天向也减小到15m/s。

图14～图17所示为采用UKF方法进行组合导航的结果。虽然姿态误差没有明显的提升，但速度误差和位置误差都有较好的结果。从图17所示的位置误差可以看出，UKF滤波后的纬度误差在20m左右，经度误差在10m以内，高度误差缩小到了100m以内，使得丢星后的定位误差有较大的提升。

3 结论

由以上分析可以看出，丢星之前采用KF、EKF和UKF方法进行组合导航都可以较好地完成导航任务，丢星后UKF定位精度最高，EKF次之，KF效果最差。

[1] 闫鑫. 基于抗差估计的GPS/MIMU组合导航滤波算法研究[D].哈尔滨: 哈尔滨工程大学,2010.

[2] 黄丽斌.MINS/GPS组合导航系统算法研究[D].南京:东南大学, 2004.

[3] 全伟, 刘百奇, 宫晓琳, 等. 惯性/天文/卫星组合导航技术[M].北京: 国防工业出版社, 2011.

[4] 秦永元. 惯性导航(第二版)[M].北京：科学出版社, 2014.

[5] 付梦印,邓志红, 闫丽萍.Kalman滤波理论及其在导航系统中的应用(第二版)[M].北京:科学出版社,2010.

[6] 严恭敏.捷联惯导算法及车载组合导航系统研究[D].西安: 西北工业大学, 2004.

[7] 谢钢.GPS原理与接收机设计[M].北京: 电子工业出版社, 2009.

[8] 王君帅, 王新龙.SINS/GPS紧组合与松组合导航系统性能仿真分析[J], 航空兵器, 2013(2): 14-19.

[9]GodhaS,CannonME.GPS/MEMSINSintegratedsystemfornavigationinurbanareas[J].GPSSolutions, 2007, 11(3):193-203.

[10]SolovievA.TightcouplingofGPS,INS,andlaserforurbannavigation[J].IEEETransactionsonAerospaceandElectronicSystems, 2010, 46(4): 1731-1746.

[11] 应士君, 李金金, 刘卫, 等.BD-2/GPS组合导航系统选星算法及定位分析[J].上海海事大学学报, 2013, 34(2): 1-4.

[12]BrennerM.IntegratedGPS/inertialfaultdetectionavailability[C]//ProceedingsofIONGPS,PalmSprings,CA, 1995.

[13] 刘华, 刘彤, 张继伟. 陆地车辆GNSS/MEMS惯性组合导航机体系约束算法研究[J].北京理工大学学报, 2013, 33(5): 500-515.

[14] 张光理. 北斗/SINS组合导航系统研究[D].哈尔滨工程大学, 2012.

Algorithm Research on MINS/GNSS Integrated Navigation System

SHI Tuo1,2, CHEN Jia-bin2, TIAN Xiao-chun2,SONG Chun-lei2, YANG Li-ming3, YIN Jing-yuan4

(1.CETC 54, Shijiazhuang 050081, China;2.School of Automation, Beijing Institute of Technology, Beijing 100081, China;3.North China Optical Instrument Co., Ltd.,Beijing 100053, China;4. Beijing Electro-mechanical Engineering System Design Department, Beijing 100039, China)

Micro inertial navigation system(MINS) with the combination of the Global navigation satellite system (GNSS) can make full use of the short-term performance of MINS and the long-term stability of the GNSS.It can make up for the defects of error accumulated over time of MINS, and for the low sampling frequency and poor dynamic performance of GNSS. Thus, it can improve the precision and dynamic capabilities of integrated navigation system, and it can also ensure the system to operate a period of time autonomously at high precision when the satellite signal is shielded or failure. We use StarNeto products to collect real data and compare a variety of filtering algorithms. Experimental results show that the effect achieved by UKF is best, EKF followed, KF worst.

MINS; Intergrated navigation; Kalman filter

10.19306/j.cnki.2095-8110.2016.06.006

2015-09-19；

2015-10-29。

石拓(1989 - )，男，硕士，主要从事卫星与惯性组合导航技术方面的研究。E-mail: shituo5858@126.com

U666.11

A

2095-8110(2016)06-0026-07