基于扩展卡尔曼滤波的动中通低成本姿态估计

闫林波，贾维敏，姚敏立，沈晓卫

(第二炮兵工程大学，西安 710025)

0 引言

动中通(satcom on-the-move，SOTM)作为一种宽带移动卫星通信系统，具有运动中、宽带及超视距通信的优点，因此，在军、民领域都有着重要的应用意义。动中通是指利用FSS频段的卫星资源，能够在运动状态下实时建立并保持卫星和天线之间通信链路畅通［1］。为了实现运动中载体与卫星之间的通信链路畅通，天线波束需做到实时对准卫星，也就是说，需要精确的姿态控制算法对载体的姿态进行实时估计。

对于动中通来说，为保证控制精度，目前大多数动中通厂商都采用了稳定可靠的高成本姿态航向参考系统(Attitude and Heading Reference System，AHRS)［2］作为测控方案。为了降低成本、重量及小型化，基于低成本微机械传感器(Micro Electro Mechanical System，MEMS)姿态估计融合算法得到了广泛的研究。但是，MEMS存在噪声、零偏及安装误差，所以姿态估计精度较差。为了改善姿态估计的精度和可靠性，两种典型的传感器融合算法得到了广泛的应用，一类是互补滤波算法(Complementary Filter，CF)，它是利用陀螺的高频特性和加速度计的低频特性进行优势互补，从而补偿陀螺的零偏和低动态下加速度计的误差。文献［3］在基于多轴互补滤波算法的基础上，融合微机械陀螺、加速度计和磁强针信息对载体的姿态角进行估计，同时采用自适应算法自动调整交接频率来校正载体的机动加速度。另一类是卡尔曼滤波算法(KF)，其实质是利用反馈控制的方法估计过程状态，即滤波器估计过程某一时刻的状态，然后以(含噪声)测量变量的方式获得反馈。文献［4］利用扩展卡尔曼滤波算法(EKF)融合陀螺和加速度计信息进行姿态估计，同时利用自适应算法去除机动加速度对姿态估计带来的影响。但是，上述方法由于陀螺零偏和加速度计测量噪声的存在，同时磁阻传感器容易受车上元件及外部物体的电磁干扰，使得姿态角的估计精度较差。文献［5-6］在基于四元数更新算法的基础上，分别利用CF和EKF算法融合陀螺和单天线GPS信息对陀螺零偏和姿态进行估计。但单天线GPS应用到动中通中存在两个问题:一是在载体的运动速度较低时，单天线GPS给出的速度信息误差比较大［5］，很难达到SOTM指向精度要求;二是当载体转弯时，估计的伪姿态角误差较大。

鉴于此，本文通过EKF算法融合单基线GPS、微机械陀螺和加速度计信息以获得实时的姿态估计。同时，为了解决载体机动加速度带来的影响，首先通过加速度补偿法对加速度计进行初步补偿;当载体发生转弯时，利用侧滑角补偿法进一步补偿。最后，采用实测数据进行了仿真验证。

1 载体姿态确定

1.1 陀螺姿态确定

在捷联系统中，惯性测量单元(Inertial Measurement Unit，IMU)放在载体上时，通过陀螺可以测量出载体坐标系相对于地理坐标系下的三轴角速率。定义地理坐标系(t系)，x轴指向当地的正东方向，y轴指向当地的正北方向，z轴垂直于x轴、y轴且构成右手直角坐标系;载体坐标系(b系)，x轴指向载体的右方，y轴指向载体的前方，z轴指向载体的上方。按照ψ→θ→φ的旋转顺序，可以得到载体系相对于地理系3个欧拉角，即航向角ψ，俯仰角θ和横滚角φ。用欧拉角方法得到其微分方程形式为［7］

式中，ωx，ωy，ωz是陀螺输出的三轴角速率。由于陀螺零偏随着时间的推移不断积累，所以仅仅依靠陀螺只适用于短时的姿态角估计。

1.2 加速度计姿态确定

在捷联系统中，加速度计测量的是载体坐标系加速度和重力加速度的迭加。假定载体不受重力以外的加速度作用，则三轴加速度计的输出即为重力矢量g在载体坐标系的分量，重力矢量g在地理坐标系的坐标为(0，0，-g)T。则俯仰角和横滚角可由如下等式得到［8］

式中，fx，fy，fz为加速度计输出的三轴加速度。当载体受到机动加速度影响时，姿态角估计精度较差，这时就需要对机动加速度进行去除，以改善估计效果。

1.3 单基线GPS姿态确定

GPS可以给出精确的位置和速度信息，所以它在姿态估计中有着非常重要的作用。单基线GPS不仅能够提供准确的航向角，还可以得到速度信息。利用GPS的速度信息可以导出速率航向ψV［6］为

式中，ve，vn分别是地理坐标系下的东向速度和北向速度。

当载体直线运动时，速率航向与载体航向一致;当载体发生转弯时，载体航向和速率航向之间就会产生一个夹角，即侧滑角。文献［9］给出了一种计算侧滑角的方法，利用单基线GPS提供载体航向角ψ和速度信息导出速率航向角ψV，进而得到侧滑角β为

2 机动加速度补偿

从上面的分析可以看出，载体的机动加速度对加速度计的估计精度影响很大，同时侧滑角的存在影响载体的机动加速度，所以，侧滑角的估计和机动加速度的去除对于载体的姿态估计显得十分必要。

2.1 加速度补偿

加速度补偿是指对因载体机动而产生的非重力加速度进行去除。利用GPS测得的速度信息可以很容易对其进行补偿。由载体的三向速度导出载体的速度V为

式中:vt是地理坐标系下的天向速度;则线性加速度为ay=d V/d t;径向加速度为ax=ωz×V。

线性加速度和径向加速度是载体机动时影响加速度计估计精度的主要干扰。结合式(2)得到加速度补偿后姿态角为［10］

2.2 侧滑角补偿

侧滑角是指载体进行转弯运动时，因载体航向角和速率航向角不同而产生的夹角［9］。载体转弯产生侧滑角如图1所示。

图1 载体转弯产生侧滑角示意图Fig.1 Sideslip angle resulted from vehicle turning

图1中:弯曲的曲线为载体的运行轨迹;O为转弯的中心点;aI为载体转弯时产生的向心加速度，垂直于速率航向。

载体进行直线运动时，速率航向和载体航向基本一致;在载体发生转弯时，从图中可以看出，速度方向与载体方向出现夹角，此时速率航向与载体航向之间形成侧滑角β，进而影响加速度补偿效果，所以，在加速度计姿态确定过程中，需要进一步校正。由图可以得出载体坐标系x轴和y轴的机动加速度ax、ay为

式中，V·β是侧滑角引起的向心加速度。利用式(7)进一步对机动加速度进行补偿，可以更好地提高加速度计的姿态估计精度。

3 EKF融合算法

载体的姿态估计是一个典型的非线性问题，为了解决这一问题，必须对卡尔曼滤波进行改进。常用的非线性滤波算法有扩展卡尔曼滤波器和无迹卡尔曼滤波器(UKF)两种。但在实际工程应用中，由于低成本MEMS传感器各种不确定因素的影响和运算量的原因，使UKF很难满足动中通中对姿态角估计的实时性要求。所以本文选取EKF算法融合低成本MEMS传感器和单基线GPS信息进行姿态估计。

3.1 四元数姿态解算

利用陀螺信息可以得到三轴角速率信息，进而利用式(1)可以得到载体的姿态角。虽然利用欧拉角解算方法理解分析简单，但其运算复杂且抽象，还会出现奇点。而四元数运算线性化且不会出现奇点，所以本文建立在四元数更新算法的基础上进行姿态估计。

四元数定义为

四元数随时间更新等式为

为了便于理解，通常将解算出的四元数转化为欧拉角，四元数与欧拉角的关系为

3.2 融合算法

由于陀螺零偏随着时间的推移不断积累，所以必须进行实时校正。状态变量定义为

式中，bx、by、bz是三轴陀螺零偏。结合式(9)可得非线性状态方程为

式中:b=［bxbybz］T，是三轴陀螺零偏;η是过程噪声向量，得到状态噪声协方差Q。

观测变量定义为y=［ψ θ φ］T，则非线性观测方程为

式中:ψ、θ、φ为加速度计和单基线GPS输出的姿态角构成的观测向量;n为观测噪声向量，得到观测噪声协方差R;nψ、nθ、nφ为加速度计和单基线GPS导出的欧拉角所得观测噪声。

EKF算法流程如图2所示。

图2 姿态估计算法流程图Fig.2 Flow chart of the proposed sensor fusion algorithm

姿态估计算法的整体流程:在单基线GPS可以提供航向信息时，以GPS测得的航向角作为航向角观测变量，同时利用速度信息对加速度计产生的非重力加速度进行初步补偿;在载体进行转弯运动时，速率航向角与载体航向角偏离较大，由式(7)可知，需要利用侧滑角补偿法进行进一步校正。根据式(6)，利用校正后的加速度信息导出俯仰和横滚角作为俯仰和横滚角观测变量，进而通过EKF算法融合陀螺的角速率信息对载体的姿态角和陀螺零偏进行实时估计，最后将陀螺零偏经平滑后用于实时校正陀螺。

4 实验结果和讨论

测量组件采用ADU5220微机械IMU模块，其中IMU采样频率为50 Hz;同时采集光纤航姿系统(输出结果曲线表示为Reference，陀螺漂移0.5(°)/h，姿态精度±0.2°，航向精度±1°)的姿态信息作为参考值。通过与参考航姿结果进行比较，验证本文算法的有效性。

为了验证EKF姿态估计算法，进行了综合行车实验。跑车的机动状态包括:起步加速，4次转弯，直线加、减速和停止减速。

实验中，采用北京星网宇达科技公司研制的AHRS(XW-ADU7612)作为高精度惯性测量基准，它以100 Hz的数据更新速率给出载体的姿态角(方位、俯仰和横滚角)，其方位角精度为0.1°，俯仰角和横滚角的精度静态时达0.05°，动态时可达0.1°。在车体的前端和后端分别安装GPS(XW-ADU3601)天线，基线长度为1.615 m，两天线的连线与载体坐标系的y轴平行，以10 Hz的速率更新数据。IMU采用北京星网宇达科技公司研制的惯性测量单元XW-IMU5220，它含有3个低成本微机械陀螺和3个加速度计，分别与载体坐标系的三轴平行，可实现对姿态运动、线运动的实时测量，以100 Hz的数据更新速率为载体提供稳定的加速度和角速率值。图3分别给出了AHRS、陀螺、加速度计和GPS姿态角估计图。

图3 陀螺、加速度计和GPS与AHRS输出角Fig.3 The attitude estimated by different methods

从图中可以看出，陀螺姿态角估计具有短时精度高的特点，由于陀螺零偏随着时间的推移不断积累，所以估计的姿态角误差不断增大;在常规路面上行驶时，俯仰角和横滚角变化很小，根据式(1)可知欧拉角之间的耦合对航向角影响较小，但陀螺估计的俯仰角和横滚角受载体转弯影响较大且航向角误差不断增大。加速度计姿态估计具有长时稳定性，但由于振动噪声和载体机动加速度的影响，估计的俯仰角在载体加、减速时误差较大，横滚角在载体转弯时误差较大。所以，对陀螺的短时精确性和加速度计的长时稳定性进行融合就显得非常必要。

图4给出了AHRS、EKF和经加速度补偿(Acceleration Compensation，AC)后的EKF算法姿态估计对比图。从图中可以看出，EKF滤波算法可以很好地滤除不同传感器的测量噪声，同时经过加速度补偿后可以得到较好的载体姿态角估计。由于航向角不受加速度的影响，所以估计精度保持较好。从图中可以看出，在初始和最后十几秒中，俯仰角受载体加、减速引起的线加速度的影响变化较大，经加速度补偿后，估计的姿态角与参考值趋于一致。但是，通过进一步观察可以发现，当载体转弯时，经过补偿后的姿态角估计误差超过±0.5°，不能满足动中通的应用要求。这时就需要利用侧滑角补偿法进行进一步补偿。

图4 EKF、EKF-AC输出角与AHRS输出角Fig.4 The attitude estimated by EKF with and without acceleration compensation

图5给出了通过单基线GPS导出的偏流角估计图。从图中可以看出当载体转弯时，偏流角较大。由于经加速度补偿和侧滑角补偿(Sideslip Compensation，SC)的EKF算法姿态角估计值比较接近，所以采用两者的误差图进行如比较，如图6所示。从图6可以看出，经过侧滑角补偿法校正后的EKF融合算法估计的姿态角精度达到了±0.3°以内。所以，利用侧滑角补偿法对提高载体姿态角的估计精度有着非常重要的意义。

EKF、EKF-AC与EKF-SC算法所得姿态角误差均值和方差如表1所示。从表1中可以看出，利用侧滑角补偿法进行EKF算法融合可以得到最好的姿态角估计。

图5 补偿角Fig.5 Compensation angle

图6 EKF-AC误差与EKF-SC误差Fig.6 The sideslip angle effect on attitude estimation with acceleration compensation

表1 EKF、EKF-AC与EKF-SC误差角结果Table 1 Attitude error statistics from the test

5 结论

本文采用EKF算法融合微机械陀螺、加速度计和单基线GPS信息构成动中通低成本姿态估计系统。该算法成功融合了陀螺的短时精确性、加速度计的长时稳定性和GPS精确的测速和定位功能，同时通过GPS速度信息对加速度计的非重力加速度分量进行补偿，有效地抑制了机动加速度对姿态估计的影响。实验结果表明，在单基线GPS能够提供航向角信息时，姿态角估计误差控制在±0.5°以内，满足了动中通的应用要求。

［1］李耐和.“动中通”卫星通信技术［J］.现代军事，2007(7):60-63.

［2］JAMESD.Control system for mobile satcom antennas［J］.IEEE Control Systems Magazine，2008，28(1):86-101.

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

［4］KANG C W，PARK C G.Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter［C］//Proceedings of the European Control Conference，Budapest，Hungary，August 23-26，2009:3713-3718.

［5］LAI Y C，JAN SS.Attitude estimation based on fusion of gyroscopes and single antenna GPSfor small UAVs under the influence of vibration［J］.GPS Solut，2011(15):67-77.

［6］TENN H K，JAN SS，HSIAO F B.Pitch and roll attitude estimation of a small-scaled helicopter using single antenna GPSwith gyroscopes［J］.GPSSolutions，2009，13(3):209-220.

［7］朱丰超，姚敏立，贾维敏.基于低成本陀螺和倾角仪的姿态估计［J］.宇航学报，2011，32(8):1728-1733.

［8］GEBRE-EGZIABHER D，HAYWARD R C，POWELL JD.Design of multi-sensor attitude determination systems［J］.IEEE Trans Aerosp Electron Syst，2004，40(2):627-649.

［9］RYU J，GERDESJC.Integrating inertial sensors with GPS for vehicle dynamics control［J］.Journal of Dynamics，Systems，Measurement，and Control，2004，126(2):243-254.

［10］刘启超.动中通低成本航向姿态估计算法研究［D］.西安:第二炮兵工程大学，2011.