基于自适应平方根UKF的微机械传感器融合航姿估计①

马帮立

（郧阳师范高等专科学校物理与电子工程系，湖北 十堰 442700）

0 引言

航姿估计广泛应用于导航制导、智能交通、光电稳定平台以及惯性稳定平台等系统中[1].但是，目前广泛使用的车载航姿测量系统大多是基于高精度惯性器件和GPS组合的姿态航向参考系统(attitude and heading reference system，AHRS)，造价昂贵，严重制约了车载航姿测量系统的应用和推广.快速发展的微机电技术(micro-electromechanical systems，MEMS)为开发低成本航姿估计系统提供了很好的方向.目前，采用MIMU(MEMSinertialmeasurementunit，MIMU)和GPS设计组合导航系统已成为导航领域研究的热点[2].

目前广泛报道的航姿估计方法[3-5]依靠陀螺积分得到姿态角，增加辅助传感器校正陀螺漂移，利用微机械惯性器件构成低成本姿态估计，采用MEMS陀螺、MEMS加速度计以及磁传感器估计姿态.但采用加速度计测量重力分量估计水平姿态，容易受到载体机动加速度的影响.文献[3]中采用测量手臂姿态，没有考虑机动加速度干扰问题.文献[4]中采用开关判断即g判断，加速度测量矢量的范数大于g时不融合，文献[5]中研究了机器人的姿态估计问题，也采用了开关判断，采用卡尔曼滤波器对陀螺零偏值进行估计，克服载体的机动加速度对加速度计姿态测量的影响.但3个姿态角的估计开关同时控制，不能区分线加速度和向心加速度，估计精度受到影响，特别是对于两种机动加速度交替连续出现的情况，系统无法工作；同时该开关算法对于车体振动等外界干扰过于敏感，判断误差较大；采用的互补滤波器无法给出陀螺零偏估计，短时间无测量值时难以利用陀螺积分保持精度.

本文中采用姿态估计结构即由陀螺积分得到姿态角，辅以磁强计和加速度计校正陀螺漂移[3-5]，并提出一种新的基于自适应平方根UKF算法，估计和补偿机动加速度，有效克服了开关判断即g判断的缺陷.跑车实验结果表明，该算法有效地去除机动加速度的干扰，提高车辆的航姿估计精度.

1 系统原理与姿态运动学描述

陀螺的动态响应较好，在短时间内精度较高，但是存在慢变的零偏误差，积分得到的角度存在随时间积累的误差，长期精度较低.通过加速度计测量重力分量可以得到载体的俯仰角和横滚角，不存在随时间的累积误差，长期精度较好，但是加速度计对于车辆振动等噪声比较敏感，短时精度不高.因此，利用陀螺与加速度计的互补特性可以得到较好的水平姿态估计.但是，当载体存在机动加速度时，如线加速度(起步、刹车等)和向心加速度(转弯)，会带来较大的姿态估计误差，因此利用陀螺和加速度计融合得到姿态角需要去除机动加速度的影响.

四元数在描述刚体旋转方面有独特的优势，不仅是参数最少的无奇异姿态表示方式，而且用四元数表示的时间传递方程是线性的，在计算过程中避免了欧拉角奇异问题以及复杂的三角函数非线性关系[6].姿态四元数是基于刚体旋转绕矢量轴一次转成的原理，由4个分量表示为

四元数与方向余弦矩阵Cbn(qˉ)有关系

其时间传递方程为

其中

其中ωt为真实角速度，b为陀螺零偏，nω与nb分别为零均值与白噪声，（3）式中，由于微机械（MEMS）陀螺精度较低，地球自转角速度以及载体运动牵连角速度可以忽略不计，因此认为微机械陀螺测量数据中只包含有载体姿态变化角速度和陀螺零偏噪声.

参考惯性导航中的比力方程[6]可得，加速度计量测值为为求角速度矢量的反对称矩阵.

陀螺误差的一般建模为慢变零偏以及测量白噪声，慢变零偏认为是白噪声驱动的随机游走[7]

式中的θ为俯仰角，φ为横滚角为载体坐标系的加速度计测量的重力的3个分量，单位为g，nθ与nφ分别为加速度计的测量误差.

2 UKF融合估计姿态

姿态估计是典型的非线性问题，可以采用的方法有扩展卡尔曼滤波器算法(extended Kalman filter，EKF)、无味卡尔曼滤波器算法(unscented Kalman filter，UKF)以及粒子滤波器算法(particle filter，PF)等.其中EKF算法复杂度最低，但是存在较大的线性化误差，对初值的要求较高，较大的初值误差可能导致其发散，同时求解Jacobian较为复杂；PF算法估计精度高，但是由于采用随机采样传递状态的概率密度函数，其计算量很大，对于多维系统，难以做到实时估计；而UKF算法在克服EKF算法缺点的同时计算量适中，利于实时估计[8].为了增强滤波器的稳定性，采用平方根形式的UKF算法，保证协方差阵的正定性，其算法实现过程如下：

1)初始化，

式中，S0为初始方差阵的Cholesky分解.

2)计算采样点，采用对称采样的采样点为2n+1个，其中γi,i=0,2,…,2n为Sigma点序列[9]，

4)更新采样点，利用一步预测值更新采样点，

其中，

其中chol{·}为Cholesky分解，qr{·}为QR分解，cholupdate{·}为平方根矩阵的更新[9].

由于陀螺零偏误差较大，因此状态变量含有需要估计的姿态角及陀螺零偏误差，即状态变量为由(2)～(3)式得到状态方程为

由(5)式，结合欧拉角与四元数的关系[6]，得到不考虑机动加速度时的测量方程为

式中的ψm由磁强计得到，通过水平姿态角的一步预测值进行磁强计测量的水平姿态补偿，得到水平面内的磁强计测量分量，然后通过反正切计算得出航向角[10]，式中的nψ为测量误差.将状态方程(12)～(13)式带入平方根UKF算法中即可实现航姿融合估计，同时可以实现微机械陀螺（包含航向陀螺，俯仰陀螺以及横滚陀螺）的零偏估计和校正.

3 自适应UKF算法

(13)式中的俯仰角和横滚角由加速度计测量得到，未考虑机动加速度对姿态角测量的干扰.由(4)式可知，在有机动加速度干扰的情况下，实际的测量值为

式中的ax、ay分别为车辆机动引起的向心加速度和前向加速度，这些机动加速度值均会体现在滤波过程中的测量更新阶段即残差中.定义测量矢量为机动加速度矢量为ak=[0,ay,k,ax,k]T，则残差表示为rk=yk-y^k|k-1，由于其中含有机动加速度干扰，故残差的协方差为

式中的Syy由(11)式得到，Rk为测量噪声，Ca,k为机动加速度矢量，是ak的协方差矩阵，即

如果能够估计协方差矩阵Ca,k，则可以分别计算车辆的向心加速度与前向加速度干扰.

新息序列的协方差可以通过如下公式近似得到

式中的N为估计滑动窗，通过（16）式可得到Ca,k的估计值

结合(15)～(17)式可得到机动加速度的估计值.但是，该方法必须保证(17)式给出的协方差矩阵的估计值是正定的，因此，必须对(17)式作必要的改动.由于Wk为对称矩阵，对其做特征值分解，可得

式中的λi,k为Wk的特征值，相应的正交矢量为ui,k.令ξi,k满足

由于λi,k≥0，并且ξi,k≥0，则可以得到

则最终的最适应算法为

上式中的判断窗口M以及阈值κ均是为了确保能够计及由于加速度计自身的测量噪声对机动加速度的估计产生的影响，将估计的加速度值在加速度计的测量中进行补偿即可实现自适应UKF融合估计姿态.

4 实验结果与分析

实验设备主要包括星网宇达公司的微机械惯性测量单元XW-IMU5220，参考系统为XW-ADU7612.其中IMU5220中包含有3个MEMS陀螺（零偏≤0.08°/s，零偏重复性≤0.05°/s，测量噪声RMS≤0.05°/s）和3个MEMS加速度计，并且做了相应的温度补偿，能够稳定地输出100 Hz的测量值.参考系统XW-ADU 7612能以100 Hz的数据更新速率，给出被测对象精确的姿态角(航向角、俯仰角和横滚角)，其航向角精度为0.1°，俯仰角和横滚角的精度静态时均达0.05°，动态时均可达0.1°.根据所用器件的静态测量数据给定如下滤波器参数，预测时间0.01 s，更新时间0.1 s，过程噪声和测量噪声均为零均值白噪声，过程噪声方差分别为测量噪声方差分别为

在校园进行了绕圈实验，采集时间为267 s，由于实际更新频率为100Hz，数据量较大，作图时每1 s显示一个数据.

图1表示各传感器姿态角单独估计结果与对比.可以看出，以陀螺积分得到的姿态角明显存在累积误差；由于3轴耦合，航向陀螺在转弯时数值较大，因此陀螺积分得到的俯仰角和横滚角在转弯阶段，其累积误差淹没在航向耦合带来的误差中；采用加速度计计算的姿态角不存在累积误差，但是噪声较大，并且在有机动加速度时存在较大的偏差.按照坐标系定义，线加速度对俯仰角产生影响，向心加速度对横滚角产生影响，在车起步和停车过程中以及转弯过程中，采用加速度计单独估计姿态角均有较大偏差.

图1 姿态角单独估计结果与对比

图2和图3分别是俯仰角和横滚角的UKF算法估计结果与对比.可以看出，采用UKF算法可以实现陀螺数据与加速度数据的融合，去除了采用加速度计单独估计时的误差，说明陀螺的短时精度与加速度计的长时精度相结合能够得到较为精确的姿态估计；采用自适应UKF估计机动加速度并且进行补偿之后，误差明显变小，基本上与参考姿态曲线重合，说明本文中提出的自适应UKF算法能够有效地去除加速度计中的机动加速度干扰.图4为自适应UKF算法估计得出的机动加速度，自适应参数选择为N=10，M=10，阈值选择为κ=0.1.

图2 俯仰角UKF算法估计结果与对比

图3 横滚角UKF算法估计结果与对比

图4中的线加速度与向心加速度估计结果分别与图3中未采用UKF自适应算法时的俯仰角和横滚角估计误差相对应.图5为姿态角自适应UKF算法的估计误差.可以看出，航向角误差绝对值在0.4°以内，俯仰角和横滚角误差绝对值均小于0.6°，满足一般的车辆姿态估计精度要求.由于机动加速度对航向角的估计无影响，航向角估计依靠磁强计，图5中给出了其估计误差.

图4 机动加速度自适应UKF算法估计结果

图5 姿态角自适应UKF算法估计误差

5 结论

本文中提出一种适用于车载环境的低成本航姿测量系统，采用3轴陀螺积分得到姿态角，同时利用加速度计校正水平陀螺零偏，利用磁强计保证航向角精度.针对加速度计测量姿态角容易受到非重力加速度的影响，本文中提出了一种自适应UKF算法，可实时估计并补偿机动加速度对姿态角估计的影响.实际跑车实验结果表明，提出的自适应UKF算法能够得到较高精度的姿态角估计.此外，该低成本姿态测量系统还可用于车载稳定平台、指向跟踪系统以及小型无人机的姿态控制系统中.

[1]Beljour H，Coryell L，Tat F.Army SATCOM on the move technology initiatives[C]//IEEE Military Communications Conference.Boston，MA，2009:1-7.

[2]祝燕华，蔡体菁，杨卓鹏.MEMS-IMU/GPS组合导航系统的实现[J].中国惯性技术学报，2009，17(5)：552-556.

[3]沈晓卫，姚敏立，赵鹏.基于互补滤波的动中通天线姿态估计[J].中国惯性技术学报，2011，19(2)：194-197.

[4]Yun Xiaoping，Bachmann Eric R.Design，implementation，and experimental results ofa quaternion-based Kalman filter for human bodymotion tracking[J].IEEE Transactionson Robotics，2006，22(6):1216-1227.

[5]Henrik Rehbinder，Hu Xiaoming.Drift-free attitude estimation for accelerated rigid bodies[J].Automatica，2004，40:653-659.

[6]秦永元.惯性导航[M].北京：科学出版社，2006.

[7]Crassidis John L，Landis Markley F，Yang Cheng.Survey of nonlinear attitude estimation0methods[J].Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

[8]Julier SJ，Uhlmann JK，Durrant-whyte H F.A newmethod for nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactionson Automatic Control，2000，45(3):477-482.

[9]伍宗伟，姚敏立，马红光，等.移动卫星通信低成本多传感器融合姿态估计方法[J].西安交通大学学报，2012，46(12):55-61.

[10]Crassidis John L，Landis Markley F，Yang Cheng.Applicaiton of magnetic sensors for low cost compass system[C].IEEE Plans，Position Location and Navigation Symposium，2000:12-28.