基于反幂法和扩展卡尔曼滤波的姿态估计算法

张 雄，黄卫华，陈劲峰

(武汉科技大学 信息科学与工程学院，湖北 武汉 430081)

0 引 言

低成本的微机电系统(micro-electro-mechanical systems，MEMS)惯性传感器广泛用于无人机的姿态测量，准确的姿态估计是无人机实现各种复杂任务的重要前提[1]。然而，单一的MEMS传感器难以获取准确的姿态信息[2]，多传感器的优势互补，已成为获得无人机高精度姿态信息的主要方式。

扩展卡尔曼滤波(extended Kalman filtering，EKF)是实现多传感器数据融合的有效方法之一。但常规EKF的测量值维数过高，且测量值线性化的过程中会引入截断误差，从而导致滤波器算法稳定性下降和收敛速度慢[3]。为了降低测量值的维数，避免高阶项截断误差，文献[4]采用四元数估计算法(quaternion estimation，QUEST)、文献[5]采用Davenport的四元数法将计算的四元数作为EKF的测量值；为了提高了算法的稳定性和收敛速度，文[6]提出一种基于最优四元数法构造Davenport矩阵K原理的改进算法，对EKF测量矩阵进行改造。但是，由于测量值都需要先计算Davenport矩阵K的准确特征值，然后根据特征值函数计算特征向量(四元数)，当特征值的准确性下降时，会导致测量值的精度降低[7]，最终引起EKF的精度降低。

综上所述，本文设计了一种基于反幂法和EKF的姿态估计算法。基于反幂法(inverse power method，IPM)，在已知近似特征值时，直接计算出准确的特征向量[8]，有效降低EKF精度对特征值的准确性的依赖程度，并减小测量值的维数。首先通过观测矢量和参考矢量构造Davenport矩阵K，然后根据Davenport矩阵K的近似特征值，采用反幂法计算矩阵K特征向量，并将其作为EKF的测量值，最后将本文所设计的算法用于自行搭建的无人机平台上，实现了无人机的稳定飞行，满足了系统的高精度及实时性要求。

1 基于反幂法和EKF的姿态估计算法的算法结构

随着MEMS技术的发展，在姿态测量场合用惯性测量单元对姿态进行精确实时测量变得非常普遍。本文以加速度计、陀螺仪、磁力计组成姿态测量系统，对无人机进行实时姿态测量。基于反幂法和EKF的滤波系统结构如图1所示。

图1 基于反幂法和EKF的组合滤波系统结构

首先通过加速度计、磁力计的参考矢量和观测矢量构造Davenport矩阵K。其次根据Davenport矩阵K的近似特征值，使用带原点位移的反幂法计算Davenport矩阵K的特征向量(四元数)。反幂法的初始向量为上一次EKF的估计值，反幂法的终止条件为相邻两次迭代特征向量的距离到达规定的阈值或者到达最大迭代次数。最后由四元数的微分方程得到状态量qω，将反幂法求解的姿态四元数qIPM作为EKF的测量值，根据EKF滤波算法更新估计四元数qest，并计算姿态角。

2 基于反幂法和EKF的姿态解算算法

2.1 基于反幂法的确定性算法

2.1.1 近似特征值求解

(1)

(2)

式中：x,y,z分别为坐标系3个轴上的分量。

当加速度计和磁力计旋转后的参考矢量和载体矢量分别相等时，可得载体的最优姿态四元数。考虑到不同传感器精度不一样，为了降低精度较差传感器的影响，本文定义损失函数f(q) 为

(3)

其中，ai为传感器的权重，i=1,2，且

(4)

其中，σi为第i个传感器的标准差，由传感器3个轴标准差的平均值代替。由于标准差σi越大精度越差，ai使得精度越差的传感器所占的权重越小。

(5)

其中，四元数q=[q0,q1,q2,q3]T，D1、D2、D3是关于四元数q的矩阵。

将式(5)代入式(3)中，令

(6)

则式(3)变为

(7)

D1THxT+D2THyT+D3THzT-q=0

(8)

其中

(9)

将式(8)中D1、D2、D3的四元数q提取出来，则Davenport矩阵K为

(K-I)q=0

(10)

其中，Davenport矩阵K是式(9)中Hx,Hy,Hz各分量的线性组合，其具体形式为

(11)

考虑到加速度计和磁力计输出存在噪声、温漂等不确定性因素，导致系数矩阵K-I可能存在秩亏问题，从而使得式(10)无法求解。为了增强算法的鲁棒性，在四元数q后面增加一个误差因子ε1q，其中误差因子系数ε1数量级为10-5，略大于单精度浮点数的精度。式(10)可以表示为

Kq=(1+ε1)q

(12)

其中，1+ε1是Davenport矩阵K的近似特征值，四元数q是特征值1+ε1对应的特征向量。

2.1.2 反幂法求解

求解特征向量的经典方法是首先使用牛顿法或解析法求出Davenport矩阵K特征多项式的准确特征值，然后再根据特征值函数求特征向量。上述方法没有考虑特征值准确性对四元数精度的影响。为了降低四元数对特征值精度的敏感度，本文根据Davenport矩阵K的近似特征值，使用带原点位移的反幂法，通过迭代序列直接求解Davenport矩阵K的特征向量q。 反幂法公式为

(13)

其中，α为近似特征值。

为了减少计算量，对系数矩阵K-αI进行列主元LU分解，将其分解为下三角矩阵L和上三角矩阵U

K-αI=LU

(14)

LU分解使得式(13)在迭代过程中每一步只需要求解两个三角方程组即可求得四元数q(k+1)

(15)

其中，q(k)的极大值规格化是为了防止四元数溢出。

当采样间隔较小时，当前反幂法计算的四元数q(k+1)与上一次EKF的估计值qest,t-1的夹角较小。由于幂法初始向量与所求特征向量的夹角较小时，算法收敛速度较快，即使在终止条件阈值较大时仍可以得到误差较小的结果。故第一次运行时反幂法的初始向量设为q(0)=[1,0,0,0]T, 之后的初始向量设为上一次EKF的估计值qest,t-1。 上述设置既考虑了历史信息，加快了收敛速度，也避免了初始向量选取的盲目性。

反幂法的终止条件为到达最大迭代次数N。为了避免固定的迭代次数使Davenport矩阵K随机变化和初始向量的不同选择导致计算精度波动较大，以及为了能够度量特征向量的精度。反幂法的另一个终止条件设为前后两次迭代所求特征向量之间的距离d到达阈值ε2[10]

(16)

其中，ε2<10-4。

为了满足四元数的范数条件，迭代终止后四元数需要归一化。

2.1.3 算法步骤

经过上述推导，本文获得了加速度计和磁力计融合后的计算四元数q(k+1)，其计算简单且对特征值精度不敏感。基于反幂法的确定性算法的步骤为：

步骤2 按式(9)计算Davenport矩阵K的基本元素Hx,Hy,Hz，按式(11)确定Davenport矩阵K；

步骤3 代入四元数误差因子系数ε1，获得反幂法近似特征值α；代入初始向量q(0)、终止条件中的阈值ε2、最大迭代次数N；

步骤4 对方程系数矩阵K-αI进行列主元LU分解，获得下三角矩阵L和上三角矩阵U，置迭代次数k=0；

步骤5 解式(15)中的三角形方程组，获得四元数q(k+1)；

步骤6 若相邻两次迭代四元数之间的距离d<ε2，则输出归一化的四元数q(k+1)，停算；否则，转步骤7；

步骤7 若k小于最大迭代次数N，置k=k+1，转步骤5；否则停算。

2.2 扩展卡尔曼滤波器设计

2.2.1 滤波观测方程

选取反幂法根据近似特征值迭代出的四元数q(k+1)作为测量值qmeasure,t。 考虑到反幂法每次求解的四元数q(k+1)，其正负都是Davenport矩阵K的解。正负四元数都表示相同的姿态信息，即四元数具有双值性。其作为测量值与EKF的预测值进行融合时，存在四元数的正负符号选取问题。只有测量值与预测值方向基本相同时，才可以得到准确的估计值。EKF的预测值通过陀螺仪的积分得到，积分具有连续性，一旦积分初值(上一次EKF估计值)给定，那么预测值将连续变化。预测值与上一次EKF估计值qest,t-1的方向基本相同，不存在正负号选取问题。故只需要保证测量值qmeasure,t与上一次EKF估计值qest,t-1方向基本相同，就可以得到准确的估计值。余弦相似性常被用来度量两单位向量的接近程度。如果求解的四元数q(k+1)与上一次EKF的估计值qest,t-1内积大于0，那么两者夹角小于90°，方向基本相同。反之，方向基本相反。综上测量值qmeasure,t的正负符号选取算法为

(17)

测量值qmeasure,t的观测噪声vt主要来源于加速度计和磁力计的输出，假定其服从高斯分布。在t时刻的观测方程可以表示为

zt=qmeasure,t+vt

(18)

由于式(13)也可以看作是关于四元数qmeasure,t的更新公式

qmeasure,t=(K-αI)qmeasure,t-1

(19)

所以观测噪声的协方差矩阵Σνt可近似为

Σvt=JΣacc,magJT

(20)

其中，雅可比矩阵J为

(21)

其中，ax,ay,az分别为加速度计的三轴数据；mx,my,mz分别为磁力计的三轴数据； Σacc,mag为加速度计和磁力计噪声构成的协方差矩阵

Σacc,mag=diag[Σacc,Σmag]

(22)

其中，Σacc,Σmag分别是加速度计和磁力计经过归一化后的协方差矩阵。

2.2.2 滤波状态方程

选取四元数作为状态变量，根据四元数微分方程可知

(23)

其中，Ωb为

(24)

式中：ωx,ωy,ωz分别是陀螺仪的三轴输出。设采样时间为Ts。 为了计算方便，采用二阶龙格库塔法求解式(23)。假定陀螺仪噪声t服从高斯分布。在t时刻的状态方程为

(25)

其中，Δθ2为

Δθ2=(ω(x,t-1)Ts)2+(ω(y,t-1)Ts)2+(ω(z,t-1)Ts)2

(26)

(27)

其中，陀螺仪的方差Σgyro为

(28)

其中，σωx,σωy,σωz分别为各轴角速度的标准差； Ξt为式(25)中状态量qt关于陀螺仪三轴输出ωx,ωy,ωz的雅克比矩阵

(29)

根据以上建立的状态方程(25)和观测方程(18)，首先，利用EKF算法的递推方程对状态向量和协方差矩阵进行预测，并计算滤波增益。然后，根据测量值qmeasure,t更新得到估计四元数qest,t，并更新协方差矩阵，为下一次循环做准备。最后，对估计四元数qest,t进行归一化处理，并计算姿态角。

3 实验与结果分析

3.1 实验测试平台

本文搭建了四旋翼无人机平台，用于验证算法的实际运行效果。微处理器选用STM32F405；惯性传感器为MPU6050，包含三轴加速度计和三轴陀螺仪；磁力计为IST8310，无人机系统实物如图2所示。

图2 无人机系统

采集无人机静止时加速度计、磁力计、陀螺仪传感器的数据。以矩阵的形式存放在MATLAB中,使用协方差函数Cov分别计算3个传感器的协方差矩阵。协方差矩阵对角线上的元素即为传感器3个轴的方差。本文传感器的标准差见表1。

表1 传感器标准差

由于加速度计和磁力计的单位不同，且参与运算的载体矢量均为归一化的矢量。故加速度和磁力计的标准差还要分别除以当地重力加速度和磁场总强度进行归一化。已知武科大青山校区的重力为9.7936 m/s2,磁场强度为49863.1 nT，则协方差矩阵Σacc,Σmag分别为

(30)

由式(30)和式(4)可知，加速度计和磁力计的权重ai分别为0.7507和0.2493。

3.2 实验结果分析

3.2.1 实时性实验

影响EKF算法实时性的主要因素是测量值的解算速度。本节比较QUEST算法、快速线性姿态估计算法[11](fast linear attitude estimator，FLAE)，以及本文提出的基于反幂法的确定性算法的解算速度。采集了一组四旋翼动态运动时的传感器原始数据，使用MATLAB仿真，比较3种算法的时间消耗。为了保证3种算法精度相当，仿真参数设置见表2。IPM算法的近似特征值为1+10-6，其中特征向量距离误差限3.7×10-5等价于另外两种算法的特征值误差限9.8186×10-6。

表2 仿真参数设置

实验结果如图3和表3所示。由图3可知IPM算法与FLAE算法(牛顿解)滤波时间相近，两者明显比QUEST算法快。这是因为QUEST算法求解Davenport矩阵K的过程中需要计算伴随矩阵和行列式，而IPM算法和FLAE算法不需要，所以显著减少了时间消耗。

图3 时间消耗比较

算法平均时间(10-5s)最大时间/s最小时间(10-5s)QUEST5.39960.00404.3761FLAE2.70650.00712.1151IPM2.91660.00441.8963

由表3可知，IPM算法的平均时间比经典的QUEST算法的平均时间快45.98%，仅比FLAE算法的平均时间多7.76%。但IPM算法的最小时间比FLAE算法的最小时间还少10.34%，最大时间比FLAE最大时间少38.02%。综上，IPM算法是一种快速的算法，能够为无人机提供实时的姿态测量值。

3.2.2 静态实验

进行静态实验时，将无人机水平放置，静止不动。去除传感器零偏后，使用梯度下降扩展卡尔曼滤波算法[12](gradient descent-extended kalman filtering，GD-EKF)与本文的EKF算法同时进行姿态解算。考虑到无人机的实时性，两种算法的最大迭代次数均设为3次。本文EKF算法的地磁参考矢量根据第12代国际地磁参考场模型得到，GD-EKF算法的偏航角由倾角补偿得到。实验结果如图4所示。

图4 静态测试

由图4可知，本文EKF算法与GD-EKF算法的俯仰角波动范围均为±0.1°，本文EKF算法的横滚角在±0.2°，GD-EKF算法的横滚角变化范围超过0.2°。GD-EKF算法的偏航角有明显的毛刺和突变分量，可能会导致无人机的震荡。GD-EKF算法3个角的波动幅度均大于本文EKF算法。综上，本文EKF算法在静态环境下具有较高精度，且稳定性比GD-EKF算法较好。

3.2.3 绕轴实验

定义绕X轴旋转对应俯仰角，绕Y轴旋转对应横滚角，绕Z轴旋转对应偏航角。首先将四旋翼实验平台绕X轴顺时针旋转40°，接着逆时针旋转80°，再顺时针旋转40°回到0°。然后将实验平台绕Y轴重复以上步骤。最后将平台绕Z轴顺时针旋转2圈，回到0°后，再逆时针旋转2圈。实验结果如图5所示。

图5 绕轴测试

通过对比可知，本文EKF算法与GD-EKF算法计算的横滚角和俯仰角基本一致，在绕轴过程中GD-EKF算法对横滚角和俯仰角的跟随略滞后本文的EKF算法。当四旋翼不水平时，GD-EKF算法计算的偏航角误差较大，而本文EKF算法通过调整传感器权重降低了偏航角误差。当四旋翼水平时，两种算法计算的偏航角一致。综上，本文的EKF算法姿态跟踪性能优于梯度下降扩展卡尔曼滤波算法，能满足无人机的姿态测量需要。

3.2.4 自由飞行实验

移植商业飞控的姿态解算算法到本文搭建的平台上，使用商业算法计算的姿态角作为参考角，将传感器原始数据和参考角数据发送到上位机。实验结果如图6、图7所示。

图6 本文算法估计值与参考值

图7 本文算法估计值与参考值误差

由图6可知，本文的EKF算法可以很好跟踪参考值。由图7可知，俯仰角和横滚角的最大误差均小于2°，仅偏航角在±180°切换时出现了较大误差，之后很快回到正常值。偏航角出现较大误差的原因是传感器原始数据与参考角数据是在不同控制周期发送的，数据传输存在延迟。本文算法计算的俯仰角的均方根误差为0.396°，横滚角均方根误差为0.3558°。综上，本文算法具有较高的精度，能够满足四旋翼的实际飞行需要。

4 结束语

本文设计了一种基于反幂法和EKF的姿态解算算法。反幂法基于近似特征值，利用迭代序列直接求解Davenport矩阵K的特征向量(四元数)，降低了测量值对特征值精度的敏感度；此外，将反幂法的计算四元数作为EKF的测量值，降低了测量值维数并有效避免了测量值线性化过程中产生的误差。仿真实验和无人机自主飞行实验结果表明，本文所设计算法能够满足无人机的姿态解算任务，具有较高的精度，算法简单且易于实现。本文下一步的工作是解决运动加速度和磁场干扰对算法精度的影响，以及大范围运动时磁参考矢量的修正问题。