微型航姿系统抗加速度干扰姿态估计研究

桂林电子科技大学 电子工程与自动化学院，广西桂林 541004

一、引言

小型无人机采用的航姿参考系统（Attitude and Heading Reference System，AHRS）利用3轴加速度计、3轴陀螺仪和3轴磁强计分别测量重力场、地磁场和载体角速度矢量，并由此估计载体姿态（包括航向角、俯仰角与横滚角）[1-4]。然而，3轴加速度计的读数实际上是重力加速度与载体运动加速度的矢量和。因此，当无人机处于加速状态时，重力观测值会受到运动加速度的干扰，进而影响AHRS航姿精度。

以AHRS中最常用的EKF（扩展卡尔曼滤波）姿态估计算法[1-4]为基础，本文对抑制运动加速度干扰的方法进行了理论分析，并讨论了航姿误差的收敛性及其改进措施。通过仿真和实验对各种方法进行了对比，结果表明对运动加速度进行外部估计所需计算量最小，且补偿效果最佳，适合实际应用。

二、算法分析

1、EKF姿态算法

采用EKF进行姿态估计，首先需选择合适的姿态表示方式来构造状态向量。除了常用的欧拉角（包括航向角、俯仰角和横滚角）、姿态四元素及方向余弦矩阵（DCM）外，采用乘性姿态误差可最大限度地降低EKF状态向量的维数[5]。以乘性姿态误差及陀螺仪零漂作为状态变量，EKF的状态方程可写为：

其中，α —乘性姿态误差；

δb—陀螺仪零漂；

wα—乘性姿态误差的状态噪声；

wb—陀螺仪零漂的状态噪声；

Ω×—角速度 ω 的叉乘矩阵（即 Ω×α=ω × α）。

EKF的量测方程如下：

vg—重力的量测噪声；

vh—磁场的量测噪声。

2、运动加速度估计及补偿方法

可将运动加速度作为EKF状态向量的一部分，由EKF进行估计[6-8]；也可将运动加速度的估计置于EKF外部[9,10]。由此可将运动加速度的估计与补偿方法分为内部估计法和外部估计法。

记运动加速度为a，将a作为状态变量，则内部估计法的EKF状态方程为：

其中，wacc—a的状态噪声。

状态向量中加入运动加速度后，EKF的量测方程变为：

另一方面，外部估计法不改变EKF结构设计，即仍采用式（1）与式（2）实现EKF算法。

在k时刻，利用上一时刻的运动加速度估计值对加速度计读数（比力）进行修正，得到当前时刻的重力观测值

综上所述，外部估计法通过不断地交替利用式（5）与式（6），即可实现运动加速度的估计与补偿。

3、讨论

内部估计法由于将运动加速度纳入EKF状态向量中，使得滤波器维数增加，因而所需存储量和计算量都会随之显著增大。相比之下，外部估计法只需计算式（5）与式（6），对存储量与计算量的影响很小。因此，外部估计法比内部估计法更简便易行。

内部估计法和外部估计法实际上都将运动加速度序列｛ak｝作为白噪声驱动的随机游走过程处理，即任意相邻时刻的运动加速度改变量为正态随机变量：

在动态条件下，载体的运动加速度在不断变化，因此估计算法本身必须能很好地跟踪运动加速度的变化。这是影响运动加速度估计与补偿效果的关键因素之一。

对于内部估计法，式（3）中的wacc即反映了运动加速度变化的剧烈程度。因此，wacc的协方差取值必须足够大，才能保证运动加速度的估计值能够较好地跟踪载体运动状态的实际变化。

对于外部估计法，采用式（6）修正加速度计读数时，实际上使得运动加速度的估计误差叠加在重力的量测噪声之上。因此，外部估计法的效果取决于EKF算法本身在量测噪声作用下的稳定性与收敛性。

一个值得重视的问题是，不论内部估计法或外部估计法，当运动加速度估计值存在误差时，该误差将持续影响重力观测值，并进而导致姿态误差的累积和发散。

应当注意，ρ的取值应小于1而接近于1。若ρ取值过小，虽可使的误差快速衰减，但也会使得运动加速度的补偿效果变差，甚至完全失去补偿效果。

三、仿真验证

1、仿真条件

仿真中，设载体沿x轴做变速直线运动，加速度变化规律如图1所示。

AHRS各传感器采样频率均为50Hz，其中.加速度计噪声标准差设为0.01m/s2，磁强计噪声标准差为0.1μT，陀螺仪噪声标准差为0.05°/s，陀螺仪零漂为一阶随机游走过程（驱动噪声标准差为0.05°/s2）。

重力矢量取为：g=(0 0 9.8)T(m/s2)

地磁矢量取为：h=(40 0 30)T(μT)

按式（1）构造EKF算法，对运动加速度不作补偿，仿真得到的航姿误差变化曲线如图2所示。其中，航向角、俯仰角、横滚角的均方误差（RMSE）依次为0.05°、1.23°和0.03°，可见沿x轴的运动加速度主要影响俯仰角精度。

2、内部估计法仿真

表1给出了不同参数设定下，内部估计法的仿真结果，包括航向角、俯仰角与横滚角的均方误差。

表1 内部估计法航姿误差（RMSE）

由表1可见，当运动加速度协方差σacc取0.01m/s2且未采用遗忘因子（即ρ=1）时，航姿误差很大，表明内部估计法此时起不到补偿运动加速度干扰的作用。对应的航姿误差曲线如图3所示。

如前所述，造成这一现象的原因有二：过小的σacc使得EKF无法跟随载体运动状态的变化，导致运动加速度估计值出现较大误差；运动加速度的估计误差会随时间而累积和发散。

适当增大σacc并引入遗忘因子ρ有利于改善补偿效果。但由表1可见，过大的σacc（1m/s2）或过小的ρ（0.9）均会使得航姿误差不减反增。ρ=0.95且σacc=0.1 m/s2时的补偿效果最好，此时的航姿误差变化曲线如图4所示。

图4所示航姿误差与无补偿的情形（即图2）相比，虽然俯仰角的误差得到了较好的抑制，然而航向角和横滚角的均方误差却超过1°，说明内部估计法的补偿效果并不十分理想。

3、外部估计法仿真

表2为外部估计法仿真结果。与表1对比可知，外部估计法的航姿误差总体上明显优于内部估计法。

表2 外部估计法航姿误差（RMSE）

图5和图6分别为ρ=1和ρ=0.95时的航姿误差曲线，可见引入遗忘因子有助于避免误差的累积。

四、转台实验

采用基于MPU9250传感器的AHRS进行实验验证。MPU9250本身由3轴加速度计、3轴磁强计和3轴陀螺仪构成，故又称为9轴传感器。

将AHRS固定于速率转台上，使其y轴指向转台轴心，x轴沿切线方向，且x、y轴均在水平面内。转台绕竖直轴转动，AHRS与转轴的距离约为0.5m。

实验中，转台首先由静止加速至120°/s，角加速度设定为10°/s2（即加速过程历时12s），而后保持120°/s匀速旋转12s，再以-10°/s2的角加速度减速至静止。在此过程中，x轴与y轴的加速度计读数分别对应圆周运动的切向与法向加速度，如图7所示。

传感器采样数据分别采用无补偿EKF算法、内部估计法以及外部估计法进行航姿解算后，航姿误差（均方根）如表3所示。由于实验中法向加速度较大、切向加速度相对较小，因而横滚角受影响最大，其次是航向角（因求解航向角需先计算地磁场水平分量），未补偿运动加速度时这两者的均方误差都达到4°以上。分别采用内部估计法和外部估计法对运动加速度进行估计和补偿后，航姿误差得到明显改善。

表3 AHRS实测数据滤波结果

尽管内部估计法与外部估计法补偿效果十分相近，但后者的计算量远小于前者，因而更为便捷实用。

五、结束语

本文对EKF姿态估计算法中的两种运动加速度补偿方法——内部估计法和外部估计法进行了分析和验证，通过参数调节改善了补偿效果并保证了误差的收敛性。仿真和实验结果表明，外部估计法性能更为稳定，能很好地消除运动加速度对航姿精度的不利影响，且其所需存储量和计算量远小于内部估计法，具有很好的实用性。