简化超球体SRUKF的GPS/MIMU组合姿态估计

沈晓卫，常瑞花，袁 丁

（1. 火箭军工程大学 核工程系，西安 710025；2. 武警工程大学 研究所，西安 710086）

简化超球体SRUKF的GPS/MIMU组合姿态估计

沈晓卫1，常瑞花2，袁 丁1

（1. 火箭军工程大学 核工程系，西安 710025；2. 武警工程大学 研究所，西安 710086）

针对加速度计、磁强计易受机动加速度和周围环境影响的问题，提出了一种单基线GPS/MIMU组合姿态估计方法。该方法利用单基线GPS天线代替磁强计提供航向信息，并利用GPS的速度信息对加速度计输出的机动加速度分量进行补偿，构成单基线GPS/MIMU组合姿态确定单元。同时考虑实时性要求，在SRUKF的基础上，引入加性非扩展形式和超球体单形采样，提出了简化超球体平方根UKF算法，通过减少状态维数和采样点的数量，降低算法计算量。建立加性噪声下的单基线GPS/MIMU姿态模型，并采用简化超球体平方根UKF算法进行姿态估计。实验结果表明，单基线GPS的速度信息可以有效提高加速度计对水平倾角的确定精度，姿态估计算法收敛后的最大误差小于0.8°，估计精度与UKF相当，且执行时间仅是UKF的42%。

姿态估计；平方根无迹卡尔曼滤波；机动加速度；超球体分布采样

姿态确定在无人机、稳定平台和天线稳定系统等领域得到了广泛应用。为保证测控的精度，往往采用高精度航姿系统（attitude and heading reference system，AHRS）或者高精度惯性导航系统（inertial navigation system，INS）对扰动进行稳定隔离[1]。这种方法虽然测控精度高 ，运行稳定可靠，但成本较高，仅AHRS或INS的价格就达数十万乃至上百万，严重制约了推广应用。

为了降低成本，一个有效的途径是采用微机械惯性传感器的姿态确定方法。由于微机械陀螺存在漂移及姿态初值问题，需要以陀螺为基准，通过姿态估计算法融合加速度计和磁强计等传感器的辅助信息，提高姿态确定精度[2]。但加速度计易受机动加速度的影响，磁强计易受周围环境如车体平台、车上元件，以及附近的建筑物、高压电线、大的金属物等电磁场的干扰，大多应用在低精度、低动态的无人机和机器人等领域。为了提高机动状态下的姿态估计精度，文献[3]提出了一种自适应扩展卡尔曼滤波算法，通过对载体机动行为的判断，自适应调节测量噪声方差，但这种方法采用向量模的平方作为判据，存在判断灵敏度低的问题。文献[4]利用模糊逻辑对机动状态进行判别，结合Kalman滤波器的观测噪声方差进行自适应调整，构成自适应姿态算法。上述方法均采用自适应滤波器对机动加速度进行抑制，在有机动加速度时，通过调整观测噪声方差，更多地依赖陀螺信息估计姿态。由于陀螺误差，载体在短时间机动时可取得较好的效果，但长期机动时却不能保持稳定的姿态精度。另外，姿态确定系统大多是非线性系统，多采用非线性滤波算法进行融合。传统的扩展卡尔曼滤波（extended Kalman filter，EKF）是姿态确定领域应用最广的非线性滤波方法，但EKF简单地将非线性模型线性化，存在估计精度较低和发散问题。相对于EKF的一阶泰勒近似，无迹卡尔曼滤波（unscented Kalman filter，UKF）对任何非线性非高斯数据可达到二阶精确，但存在计算量大的问题[5]。文献[6]将超球体采样和平方根滤波相结合，提出了超球体平方根无迹卡尔曼算法（square root unscented Kalman filter，SRUKF），采用超球体采样减少采样点数量，同时在UKF滤波的时间更新和量测更新阶段采用平方根滤波将状态估计误差协方差阵的Cholesky因子形式直接传递，解决了UKF数值不稳定性。但是该算法采用非扩展形式，将过程噪声和测量噪声引入状态向量中，导致了维数和sigma点的增加。

针对上述问题，本文采用单基线GPS天线代替磁强计提供航向信息，并利用GPS的速度信息对加速度计输出的机动加速度分量进行补偿，构成单基线GPS/MIMU组合姿态确定单元。同时考虑实时性要求，在SRUKF的基础上，引入加性非扩展形式和超球体单形采样，提出了简化超球体平方根 UKF算法（simple spherical simplex square root unscented Kalman filter，SSSRUKF）。实验结果表明该算法能改善UKF的计算效率。

1 基于单基线GPS/MIMU的组合模型

MIMU（包括三轴微机械陀螺和三轴加速度计）具有短时间高精度特性，能及时敏感载体的姿态变化，但其单独工作下姿态误差较大；单基线GPS天线在载体上前后放置，在无阴影情况下可以准确输出速度和航向角信息，但也存在信号遮蔽、更新频率低等缺陷。因此，MIMU和单基线GPS具有很好的互补特性，两者构成的组合姿态确定模块可以有效提高精度。

如图1所示，本文利用单基线GPS提供的航向角和加速度计提供的俯仰角、横滚角作为辅助观测信息，并建立基于单基线GPS/MIMU的状态方程和测量方程，通过姿态估计算法对姿态进行估计。由于加速度计是利用重力分量计算水平倾角，为了避免机动加速度对其影响，利用GPS接收机的速度信息结合载体角速率对加速度计输出进行补偿，然后再以此计算水平倾角。

图1 单基线GPS天线/MIMU组合框架Fig.1 Combination architecture of single baseline GPS/MIMU

1.1 状态方程

定义导航坐标系和载体坐标系分别为参考坐标系和移动坐标系。导航坐标系（n系），是一种当地地理坐标系，坐标轴指向北、东和地。载体坐标系（b系），轴向分别对应载体的横滚轴、俯仰轴和偏航轴。

定义状态变量为x=q，根据式(1)可以得到状态方程为

1.2 测量方程

加速度计置于载体上敏感的是比力，输出为[7]

当载体处于静止和匀速时，可忽略机动加速度和向心加速度的影响，此时加速度计敏感的是重力场分量。根据重力场分量得到载体的水平倾角：

其中，θacc和φacc分别为加速度计输出估计的俯仰角和横滚角。

对机动加速度补偿后，式(4)可表示为：

结合单基线GPS的航向角和加速度计的水平倾角，根据欧拉角与四元数的关系，其测量方程可以表示为：

测量噪声v为随机独立的零均值高斯白噪声序列。

2 简化超球体平方根UKF姿态估计算法

根据公式(2)陀螺角速率提供的状态方程和公式(7)辅助测量信息提供的测量方程，建立姿态确定系统的非线性系统模型，离散化后表示为：

针对公式(8)中过程噪声和测量噪声都是加性的，研究简化超球体平方根 UKF算法融合各个传感器的信息，提高姿态精度。简化超球体平方根UKF算法在平方根UKF的基础上，引入加性噪声非扩展滤波减少状态维数，同时采用超球体单形采样减少采样点的数量，降低了算法的计算量。

2.1 加性超球体采样策略

UKF通过设计采样Sigma点的unscented变换（UT）实现预测过程，其计算量与采样点数量密切相关。在对实时性要求比较高的系统中，可采用超球体单形采样减少Sigma点的数目。超球体单形采样如下：

2）确定Sigma权值

3）比例修正

4）初始化向量序列（对应状态为1维的情况）

式中，j为向量的维数，i为采样点的顺序。

6）定义Sigma权值

2.2 简化超球体平方根UKF算法

采用加性非扩展形式和超球体单形采样策略进行平方根 Unscented卡尔曼滤波运算，并对预测方程和更新方程作相应的修改，即可构成简化超球体平方根UKF算法。具体步骤为：

1）初始化状态变量和协方差平方根

2）根据公式(12)计算Sigma点

3）时间更新

4）测量更新

5）k=k+1，转至第2）步。

3 实验分析

为了验证本文算法的有效性，通过跑车实验对上面的单基线GPS/MIMU组合姿态确定模型和姿态估计算法进行验证。如图2所示，MIMU采用星网宇达公司的XW-IMU5220，陀螺零偏小于0.08 (°)/s，零偏稳定性小于0.05 (°)/s，加速度计零偏小于0.005g，零偏稳定性小于0.001g；单基线GPS接收模块采用XW-ADU3601，其航向角误差小于0.1°（2 m基线），速度误差小于0.02 m/s；光纤航姿系统XW-ADU7612的姿态信息作为参考值，姿态角误差小于0.1°。实验中测试车沿典型水泥路面机动，车速限制在15 m/s，车辆行驶状态包括直线运动、转弯运动等。车辆周围较为空旷，单基线GPS接收模块可以准确输出载体的航向角和速度信息，其中，MIMU采样频率为100 Hz，GPS接收模块的输出频率为10 Hz，基线长度为1.57 m。

图2 设备安装图Fig.2 Diagram of equipment installation

图3 加速度计校正前后的倾角对比Fig.3 Comparison on inclinations of accelerometer before and after correction

图3给出了加速度计校正前后的倾角对比。加速度计校正之前，加速度计输出值受机动加速度的影响，其计算的倾角存在较大偏差（最大误差接近 7°）。其中，载体变速运动产生的前向加速度对俯仰角影响较大，而载体转弯运动产生的向心加速度对横滚角影响较大。利用 GPS的速度信息对上述机动加速度补偿后，俯仰角和横滚角估计精度均有了较大提高，其中俯仰角最大误差小于0.5°，横滚角最大误差小于2°。

图4给出了SSSRUKF估计的姿态曲线，结果表明单基线GPS/MIMU经过SSSRUKF融合后，姿态精度得到了较大改善，机动加速度对姿态估计影响不大，其估计值与参考值基本吻合。

图5给出了SSSRUKF和UKF在相同条件下的姿态估计误差，SSSRUKF需要一段时间才能收敛。表1从估计精度和计算耗时两方面对 ASSRUKF与 UKF进行比较，其中滤波耗时为独立运行10次的平均值。

从表 1可以看出：UKF的姿态估计精度略好于SSSRUKF；UKF和SSSRUKF同在四维状态变量情况下，SSSRUKF平均仿真耗时6.84 s（仿真完所有的采样点），而UKF平均仿真耗时16.30 s，SSSRUKF执行时间仅是UKF的算法的42%，且SSSRUKF收敛后的最大误差小于0.8°。表明从加性噪声非扩展滤波和超球体单形采样两方面，可以有效降低算法的计算量。

图4 姿态估计曲线图Fig.4 Curve of attitude estimation

图5 SSSRUKF和UKF的估计误差Fig.5 Estimation errors of SSSRUKF and UKF

表1 SSSRUKF和UKF算法性能对比Tab.1 Comparison on SSSRUKF and UKF

4 结 语

本文构建了一种基于单基线GPS和MIMU的组合姿态确定模型，俯仰角和横滚角上利用GPS速度信息对加速度计的非重力加速度分量进行补偿，抑制机动加速度对姿态估计的影响；航向上选择单基线GPS的信息作为观测量，校正陀螺漂移误差。采用简化超球体平方根 UKF姿态估计算法来融合各个传感器信息，在平方根UKF的基础上引入加性非扩展形式和超球体单形采样，减少了状态维数和采样点的数量。该方法在精度上与UKF相当，执行时间仅是UKF的算法的42%。此外，本文研究主要针对是单基线GPS接收模块收星正常的情况，实际上载体的运动环境复杂，后续工作可针对不同的收星情况展开研究。

（References）：

[1] 张华强, 许敬, 赵剡. 一种用于天线跟踪稳定平台的SINS转动基座初始对准方法[J]. 中国惯性技术学报, 2014, 22(1): 32-37. Zhang Hua-qiang, Xu Jing, Zhao Yan. Initial alignment of SINS on revolving base for antenna tracking stabilized platform[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 32-37.

[2] Sheng Han-lin, Zhang Tian-hong. MEMS-based low-cost strap-down AHRS research[J]. Measurement, 2015, 59: 63-72.

[3] Li Wei, Wang Jin-ling. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems[J]. Journal of Navigation, 2015, 66(1): 99-113.

[4] Wang Ling-ling, Fu Li, Hu Xiao-guang, et, al. Attitude estimation for UAV with low-cost IMU/ADS based on adaptive-gain complementary filter[C]//13th International Symposium on Neural Networks. St. Petersburg, Russia, 2016: 346-355.

[5] Zhao Jian-wei, Jia Wei-min, Wang Rong, et, al. Attitude estimation based on the spherical simplex transformation modified unscented Kalman filter[J]. Mathematical Problems in Engineering, 2014: 1-10.

[6] Tang Xiao-jun, Yan Jie, Zhong Du-du. Square-root sigmapoint Kalman filtering for spacecraft relative navigation [J]. Acta Astronautica, 2010, 66: 704-713.

[7] Shen Xiao-wei, Yao Min-li, Jia Wei-min, et al. Adaptive complementary filter using fuzzy logic and simultaneous perturbation stochastic approximation algorithm[J]. Measurement, 2012, 45(5): 1257-1265.

[8] Julier S J. The scaled unscented transformation[C]//Proceedings of the American Control Conference. Jefferson City, USA, 2002: 4555-4559.

[9] Giannitrapani A, Ceccarelli N, Scortecci F. Comparison of EKF and UKF for spacecraft localization via angle measurements[J]. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47(1): 75-84.

[10] Henrique M T M, João Y I, Geovany A B, et, al. A systematization of the unscented Kalman filter theory[J]. IEEE Transactions on Automatic Control, 2015, 60(10): 2583-2598.

GPS/MIMU integrated attitude estimation based on simple spherical simplex SRUKF

SHEN Xiao-wei1, CHANG Rui-hua2, YUAN Ding1
(1. Department of Nuclear Engineering, Rocket Force University of Engineering, Xi’an 710025, China;
2. Research Department, Engineering University of CAPF, Xi’an 710086, China)

In view that accelerometers and magnetometer are susceptible to maneuvering acceleration and surrounding environment, an attitude estimation method combining single baseline GPS with MIMU is proposed. The single baseline GPS antenna instead of the magnetometer is used to provide the heading information. The velocity information coming from GPS measurements can be used to compensate the outputs of accelerometers and eliminate the influence of maneuver acceleration on attitude estimation. Based on the square-root unscented Kalman filter, an additive non-augmented unscented transforms and a spherical simplex sampling are introduced to reduce the state dimension and the number of sigma points. Then the proposed algorithm is used to estimate the attitude under the additive noise condition. The results show that the single baseline GPS can effectively improve the horizontal-inclination determination accuracy of the accelerometer. The maximum error by the proposed algorithm is less than 0.8 ° after the convergence, which is equivalent to that of UKF, while the execution time is only 42% of UKF’s.

attitude estimation; square-root unscented Kalman filter; maneuver acceleration; spherical simplex sampling

V249.3

A

1005-6734(2017)02-0197-06

10.13695/j.cnki.12-1222/o3.2017.02.011

2017-01-10；

2017-03-25

国家自然科学基金（61179005，61179004）

沈晓卫（1982—），男，博士，讲师，研究方向为惯性导航、非线性滤波、阵列信号处理。E-mail: shenxw602@163.com