多旋翼无人机自适应DUKF姿态估计算法

蔡安江，刘凯峰，刘立博，翟彦昭

(西安建筑科技大学机电工程学院，陕西 西安 710055)

1 引言

多旋翼无人机的姿态估计是多旋翼无人机导航及控制的基础，姿态估计的精度极大地影响着无人机导航和控制等方面的精度[1-2]。现有研究人员对于如何使用存在噪声干扰的陀螺仪、加速度计和磁力计数据获得精确的姿态角的问题进行了大量的研究[3-6]。在静止或稳定飞行的状态下，使用加速度计可以很方便地估计出无人机的横滚角和俯仰角，但无人机处于加速或减速状态时，由于外部加速度的存在，会极大地影响横滚角和俯仰角估计的精度，甚至会使测量的横滚角和俯仰角完全无法使用。同理，当环境中存在较大的电磁干扰时，磁力计测得的数据会严重失真，在估计航向角时，也会出现较大误差。针对这个问题，文献[7]提出一种自适应滤波方法，该方法基于扩展卡尔曼滤波算法进行无人机姿态姿态解算，在卡尔曼滤波的校正阶段，以测量的加速度的模与重力加速度绝对值的差值为基数乘以单位矩阵作为加速度计的噪声协方差，以测量的磁场的模与当地地磁矢量绝对值的差值为基数乘以单位矩阵作为磁力计的噪声协方差，当存在较大的外部加速度或干扰磁场时，加速度计或磁力计的噪声协方差会增加，在卡尔曼滤波的解算中会减小对加速度计或磁力计数据的权重，外部干扰越大，加速度计或磁力计数据的权重会越小，在一定程度上可以减小外部干扰。但这种方法使加速度计或磁力计的每个分量都具有相同的噪声方差，而实际情况中一般不会出现每个分量都具有相同的干扰，这样构造的噪声协方差会与实际情况不相符，严重时可能导致滤波发散。

针对这些问题，提出一种以残差矢量为基础构造相应的传感器噪声协方差阵的自适应策略，这样构造的传感器的噪声方差完全与外部干扰分量相对应，不会造成构造的协方差与实际情况不相符的情况。同时，结合一种精度高且实时性好的DUKF算法[8-9]，设计了一种使用DUKF的多旋翼无人机自适应姿态估计算法。最后，通过提取静止状态PIXHAWK飞控中的惯性传感器和磁力计数据，进行相关的仿真实验，验证了所提算法的有效性。

2 姿态估计系统模型

2.1 姿态估计系统的状态方程

则可以建立连续时间的状态方程为[10]：

为了方便迭代计算，将连续时间的状态方程转换为离散时间状态方程：

式中：T—解算周期；I—四阶单位矩阵。为了描述方便，后续内容中符号将用q代替。

2.2 姿态估计系统的量测方程

使用三轴加速度计可测得重力在机体坐标系下的三个分量，使用三轴磁力计可测得地磁场在机体坐标系下的三个分量。重力场在地球坐标系的三个分量为［0，0，|g|］，其中g表示重力加速度，现假定当地磁场在地球坐标系的三个分量为［bx，by，bz］，则有基于三轴加速度计和三轴磁力计数据的量测方程为：

式中：O—三维零矩阵；—用四元数表示的地球坐标系到机体坐标系的姿态转移矩阵。

3 自适应DUKF姿态估计算法

3.1 自适应构造传感器噪声协方差阵

卡尔曼滤波过程中，残差表示理论估计值与实际测量值之差，正常情况下这个差值是比较小的，但是当存在较大的外部干扰时，这个差值就会变得比较大，并且这个值与外部干扰的大小是成正比的。所以提出根据卡尔曼滤波过程中的残差来构造加速度计和磁力计的测量噪声协方差的自适应策略。

在加速度计校正过程中，假设加速度残差为：

其中εk1(1)、εk1(2)和εk1(3)就分别表示x轴、y轴和z轴外部加速度干扰的程度，此时构造加速度计噪声协方差为：

式中：ka—常数，表示设置的权重因子。

则当某轴存在外部加速度时，相应轴的残差分量可以表示其外部加速度大小，外部加速度较大时，该轴的量测噪声方差值也较大，卡尔曼滤波器在估计过程中对该轴加速度计测量值的信任度较低，使用该轴加速度数据对姿态角估计的校正程度也就越低，在很大程度上就丢弃了该存在干扰的数据；反之，当残差值较小时，表示存在的外部加速度干扰较小，此时构造的测量噪声方差值也较小，卡尔曼滤波器在估计过程中对该轴加速度计的测量值的信任度较高，该轴加速度数据对姿态角估计的校正程度也就越高，使正确的加速度值能更好的校正姿态估计值。

同理，在磁力计校正过程中，假设地磁场测量残差为：

式中：εk2(1)、εk2(2)和εk2(3)—x轴、y轴和z轴外部电磁场干扰的程度，此时构造磁力计噪声协方差为：

式中：km—常数，表示设置的权重因子。

则总的噪声协方差阵为：

3.2 自适应DUKF姿态估计算法

根据前面的系统状态方程和量测方程，以姿态四元数为状态向量的非线性系统为：

式中：qk—k时刻系统的四元数状态向量；zk—k时刻三轴加速度计数据和三轴磁力计数据组成的量测向量；Ak-1—式(5)中的状态转移矩阵；wk，vk—互不相关的零均值高斯白噪声过程，其方差分别为：

则自适应DUKF姿态估计算法流程为：

(1)系统初始化

设置初始状态四元数为q0=[ 10 0 0]T，初始状态误差协方差阵为：

(2)计算系统先验估计

①读取三轴陀螺仪数据，获得wx,wy和wz，将其带入式(4)中计算得到Ω。

②计算离散时间状态转移矩阵

③时间更新

由于系统状态方程具有线性特征，系统先验状态估计值及其误差协方差阵的计算与线性卡尔曼滤波相同，则k-1时刻系统先验状态估计值为：

k-1时刻系统先验状态估计误差协方差为：

④Sigma点选取

根据先验状态估计值及其误差协方差选择一组加权Sigma点，选取的Sigma点为：

式中：a∈R—调节参数，控制Sigma点在周围的分布；i—矩阵均方根的第i列。

(3)量测更新

①计算变换后的Sigma点

经过非线性量测方程h(·)变换后的Sigma点为：

②读取加速度计和磁力计数据

③计算残差εk，并使用自适应的方法构造总的噪声协方差阵Rk

加速计噪声协方差为：

式中：ka—加速度计噪声的自适应系数；

εk(i)—矢量εk的第i个元素。

磁力计噪声协方差为：

总噪声协方差为：

④计算量测预测值及其协方差

计算先验状态估计值与量测预测值间的互协方差

⑤确定卡尔曼增益矩阵

(4)进入下一个迭代周期，重复步骤2-3的过程，算法流程，如图1所示。

图1 算法结构框图Fig1 Algorithm Block Diagram

4仿真实验及结果分析

通过提取水平静止状态的PIXHAWK飞控120s的陀螺仪、加速度计和磁力计的数据进行相关仿真实验。其中，为了验证所提自适应算法在存在外部加速度的抗干扰能力，在加速度数据中按照一定时间间隔加入固定的常值偏移以模拟存在的外部加速度干扰，使用未加自适应环节的DUKF算法和所提算法(A-DUKF)进行仿真实验对比，仿真结果，如图2所示。并对各算法的姿态估计误差均方根值(RMS)进行计算，计算结果，如表1所示。为了验证所提自适应算法在存在外部磁场干扰时的抗干扰能力，在磁力计数据中按照一定时间间隔加入固定的常值偏移以模拟外部环境中存在的磁异常，使用未加自适应环节的DUKF算法和所提算法(ADUKF)进行仿真实验对比，仿真结果如图3所示，并对各算法的姿态估计误差均方根值(RMS)进行计算，计算结果，如表2所示。

图2 存在外部加速度干扰的姿态角对比Fig.2 Attitude Angle Comparison with External Acceleration Interference

表1 存在外部加速度干扰的姿态角估计误差(°)Tab.1 Attitude Angle Estimation Error with External Acceleration Interference(°)

图3 存在外部磁场干扰的姿态角对比Fig.3 Attitude Angle Comparison of External Magnetic Field Interference

表2 存在外部磁场干扰的姿态角估计误差(°)Tab.2 Attitude Angle Estimation Error with External Magnetic Field Interference(°)

由图2(a)、图2(b)、图2(c)和表1可以看出，由于外部加速度的干扰，DUKF算法的横滚角、俯仰角和航向角估计均存在较大偏差，其横滚角、俯仰角和航向角估计的误差均方根值分别为0.8147°、1.0256°和0.3823°；而所提的自适应姿态估计算法(ADUKF)则具有较高的姿态估计精度，其横滚角、俯仰角和航向角的姿态估计误差均方根值分别为0.0570°、0.1533°和0.0435°。由图3(a)、图3(b)、图3(c)和表2可以看出，由于外部电磁场的干扰，DUKF算法估计的横滚角、俯仰角和航向角均存在很大的误差，其横滚角、俯仰角和航向角的姿态估计误差均方根值分别为0.9475°、0.9804°和3.9198°；而所提自适应姿态估计算法(A-DUKF)则能较精确的估计这三个姿态角，其横滚角、俯仰角和航向角的姿态估计误差均方根值分别为0.0168°、0.0195°和0.2051°。

5 结论

为了提高存在外部加速度或外部电磁场干扰时多旋翼无人机的姿态估计精度，使用根据残差构造加速度计和磁力计噪声协方差的自适应策略，并结合一种计算精度高且计算量较小的衍生无迹卡尔曼滤波(DUKF)算法，由此提出一种多旋翼无人机自适应DUKF姿态估计算法。利用从水平静止状态的PIXHAWK飞控中提取的传感器数据，将所提算法(A-DUKF)与未使用自适应策略的DUKF算法进行仿真实验对比，结果表明，与DUKF算法相比，所提算法(A-DUKF)在存在外部加速度干扰的情况下，横滚角估计误差均方根值由0.8147°减小为0.0570°，俯仰角估计误差均方根值由1.0256°减小为0.1533°，航向角姿态估计误差均方根值由0.3823°减小为0.0435°；在存在外部磁场干扰的情况下，横滚角估计误差均方根值由0.9475°减小为0.0168°，俯仰角估计误差均方根值由0.9804°减小为0.0195°，航向角姿态估计误差均方根值由3.9198°减小为0.2051°。证明了所提算法在抗干扰能力方面的有效性，对工程实践具有一定的参考意义。