吕冰1,2 ,邓超凡1,2
1.中国船舶集团有限公司第七一〇研究所，2.国防科技工业弱磁一级计量站，湖北 宜昌 443003
摘要：针对传统的航姿参考系统AHRS(Attitude and Heading Reference System)中姿态角精度不高的问题，设计了一种基于无迹卡尔曼滤波(Unscented Kalman Filter，UKF)的姿态测量系统，通过融合微机电系统(micro-electro-mechanical system，MEMS)三轴陀螺仪、三轴加速度计以及三轴磁力计的数据，克服了单独三轴陀螺仪进行姿态解算存在累积误差，单独加速度计和三轴磁力计易受到外界干扰加速度及磁干扰的影响的缺点，充分发挥各传感器的优势。最终试验结果表明，在静态测试中，系统无累积误差，且不受磁干扰的影响，在动态测试中，UKF算法消除了只使用加速度计和磁强计在动态测量中的毛刺，有效改善了系统的动态性能。
关键词：无迹卡尔曼滤波(UKF)；航姿参考系统(AHRS)；姿态解算；微机电系统(MEMS)
1.引言
姿态测量系统在航天航空、机器人、人体运动姿态捕捉等方面都有着广泛的应用。随着MEMS技术的发展，与传统的传感器相比，MEMS传感器体积更小、质量轻、功耗更低，这使得体积微小的姿态测量系统有了更加广泛的应用。
常见的MEMS姿态测量系统通常由MEMS三轴陀螺仪、三轴加速度计、三轴磁力计组成，然而，由于MEMS器件自身精度问题，单独利用三轴陀螺仪数据积分得到的姿态角随时间漂移过大，而利用三轴加速度计及三轴磁力计数据计算得到的姿态角不随时间漂移但是会受到加速度以及磁干扰的影响，因此，需要应用合适的滤波算法去除噪声得到最优融合信息[1]。
2.航姿参考系统姿态估计
选取北东地坐标系作为导航坐标系，在准静态条件下，重力加速度在导航坐标系中的水平方向上没有投影分量，加速度计只在Z轴方向上有测量值输出，测量值为g。重力加速度在载体坐标系下的三轴分量为[2~5]：
.png)
航姿参考系统的四元数q的实时值可通过求解四元数姿态微分方程而得到，四元数微分方程如式(9)所示：
.png)
其中：为更新的步长。
3.无迹卡尔曼滤波器设计
卡尔曼滤波是一种基于最小线性方差的估计方法，通过逐步递推的方式估计状态量来减少噪声的影响。但是传统的卡尔曼滤波只针对线性系统。
非线性滤波中所要解决的最基本、最重要的问题是计算随机变量经过变换以后的统计特性。另外，无论状态具有怎样的分布，它在己知观测值时的条件均值总是最小方差估计，而方差是对估计精度的一种度量。UKF方法的建立是基于如下的事实：逼近任意的分布比逼近它的任何非线性函数更容易。UKF的基础是Unscented变换(Unscented Transform，UT)，其基本思想是用一组确定的离散采样点(sigma点)来近似状态变量的分布。UKF假定状态满足高斯分布，因此只需逼近其均值和方差[6]。
UKF基于UT变换方法直接使用系统的非线性模型，不像扩展卡尔曼滤波(Extended Kalman Filter，EKF)那样需要对非线性系统线性化，也不需要如一些二次滤波方法那样计算Jacobian或者Hessians矩阵，且具有和EKF方法相同的算法结构。对于线性系统，UKF和EKF具有同样的估计性能，但对于非线性系统，UKF方法则可以得到更好的估计[7~9]。
构造状态向量，其中为载体姿态四元数估计值，为陀螺仪零偏估计，可以用随机游走建模。可以得到系统状态方程：
.png)
.png)
4.试验验证
为了验证本文提出的基于无迹卡尔曼滤波的航姿参考系统算法，设计了静态试验和动态车载试验对本文提出的滤波算法进行了实际数据验证。实验板集成了MEMS三轴陀螺仪，三轴加速度计以及三轴磁力计，数据输出频率为100Hz。
4.1 静态试验验证
为了验证系统使用加速度计和磁强计的测量方式解算得到的姿态角没有积累误差，对系统做了静态测试，测试结果如图2~图4所示。在图中，横轴为采样时间，纵轴分别是横滚角、俯仰角和航向角，单位是度。测量结果分别是使用加速度计、磁强计和方向余弦矩阵的测量结果（记作六轴输出）以及使用加速度计、磁强计和陀螺仪，经过UKF算法对传感器数据进行融合后的结果（记作九轴输出）。
.png)
在图中，经过165s的采样时间，系统输出不存在累积误差，使用UKF融合三轴陀螺仪、三轴加速度计以及三轴磁力计得到的姿态角输出结果更加精确。在测试过程中，用铁磁性物体靠近姿态测量模块附近来回晃动，可以看到，此时的六轴输出受到磁干扰影响输出误差很大，而UKF融合滤波算法 的输出则不受此影响。
4.2 动态试验验证
为了验证UKF算法对系统姿态角输出性能的改善效果，对系统做了动态试验验证，验证结果如图5~图7所示。
.png)
在动态测量中，当快速旋转传感器模块时，加速度计和磁强计的测量结果产生了很大的毛刺，这是由于加速度计除了测量重力加速度外，也能测量到外力产生的加速度，在利用式(3)~(4)计算横滚角和俯仰角时，加速度计三个轴的测量的结果中包含了外力加速度，从而导致横滚角和俯仰角的计算不准确，而航向角是利用加速度计的测量结果来计算，所以航向角的结果也是不准确的；在使用陀螺仪和UKF算法后。由于陀螺仪很好的体现了系统的动态性能，消除了只使用加速度计和磁强计在动态测量中的毛刺，改善了系统的动态性能。由此可见，在使用陀螺仪和UKF算法后克服了只使用加速度计和磁强计测量姿态的缺点。
5.结论
本文提出一种基于UKF的MEMS三轴陀螺仪、三轴磁传感器以及三轴磁力计的数据融合方法，通过静态测试以及动态测试试验验证了算法的有效性，从试验结果来看，本文提出的方法能够充分发挥各传感器的优势，实现性能互补，使得最终结算的姿态角既不存在累积误差，且不受到外界干扰加速度及磁干扰的影响，极大的提高了测试精度，具有较强的实用价值[12]。
参考文献
[1] 马建萍．GPS辅助捷联惯导系统慕座初始对准新方法[J]．传感技术学报，2010，23(11)：1656-1661．
[2] Yun Xiaoping，Eric R．Bachmann．Design，Implementation，and Experimental
Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking[J]．IEEE Transcation on Robotics，2006，22(6)：317-332．
[3] Yang A Y，Jafari R，Sastry S S．Distributed recognition of human actions using wearable motion sensor networks[J]．Journal of Ambient Intelligence and Smart Environment，2009，1(2)：103-115
[4] Mandic D P，Kanna S，Constantinides A G．On the intrinsic relationship between the least mean square and Kalman filters[J]．Signal Processing Magazine IEEE，2015，32(6)：117一122．
[5] 张帆，曹喜滨，邹经湘．一种新的全角度四元数与欧拉角的转换算法[J]．南京理工大学学报，2002，26(4)：376-380
[6] 陈玉坤，司锡才，李志刚．联合卡尔曼滤波在多传感器信息融合技术中的应用[J]．弹箭与制导学报，2005，25(4)：946-948
[7] 党宏社，韩崇昭，段战胜．基于多卡尔曼滤波器的自适应传感器融合[J]．系统工程与电子技术，2004，26(3)：311-313
[8] 朱荣，周兆英．基于MEMS的姿态测量系统[J]．测控技术，2002．21(10)：6-8．
[9] 李荣冰，刘建业，曾庆化等. 基于MEMS 技术的微型惯性导航系统的发展现状[J]. 中国惯性技术学报，2004（6）：90-96.
[10] 蔡春龙，刘翼，刘一薇.MEMS仪表惯性组合导航系统发展现状与趋势[J].中国惯性技术学报，2009（5）：562-567.
[11] 杨业飞，申文涛. 惯性稳定平台中陀螺技术的发展现状和应用研究[J].飞航导弹，2011（2）：72-79.
[12] 杨淑洁，曾庆双，伊国兴. 低成本无人机姿态测量系统研究[J].传感器与微系统，2012，25（2）：15-18.