基于扩展卡尔曼滤波器的无人机状态估计

王文丽 何博

摘  要：在无人机飞行过程中，由于低成本无人机对状态反馈不准确和外部干扰对传感器的影响，导致飞行时间一长会出现漂移现象，使得无人机产生浮动等不良问题。为了解决这些问题，提出一种适用于小型无人机状态估计的四元数扩展卡尔曼滤波数据融合算法，该算法通过建立四元数系统观测模型和传感器量测模型，解决了低成本传感器对无人机位置和姿态直接测量的低精度和易被外界干扰的问题，有效提高了状态估计的精度。仿真实验验证了算法的有效性。

关键词：无人机;四元数;扩展卡尔曼滤波器;状态估计;数据融合

中图分类号：TP183     文献标识码：A文章编号：2096-4706（2022）04-0118-05

UAV State Estimation Based on Extended Kalman Filter

WANG Wenli， HE Bo

（Department of Aerospace Science and Technology， Aerospace Engineering University of the PLA Strategic Support Force， Beijing  101416， China）

Abstract： During the flight of the UAV， due to the inaccurate state feedback of the low-cost UAV and the influence of external interference on the sensor， the drift phenomenon will occur over a long flight time， causing the UAV to float and other undesirable problems. In order to solve these problems， a quaternion extended Kalman filter data fusion algorithm suitable for small UAV state estimation is proposed. The algorithm solves the problems of low accuracy and easy interference from the outside world in the direct measurement of the position and attitude of the UAV for the low-cost UAV， by establishing the quaternionic system observation model and the sensor measurement model. It effectively improves the accuracy of the state estimation. The simulation experiments demonstrate the effectiveness of the algorithm.

Keywords： UAV; quaternion; extended Kalman filter; state estimation; data fusion

0  引  言

随着无人机技术的发展，固定翼无人机由于其成本低、飞行速度快、高度高、可按规划航迹自主飞行等特点，广泛应用于军事和民用领域，进而对无人机飞行时完整的状态反馈提出了更高的要求。但同时，大多数无人机配置着由低成本微机电系统、磁力计、GPS及气压传感器组成的低成本飞控系统，这些传感器在测量过程中会产生持续的噪声和漂移，从而造成严重后果。且利用这样的飞控系统研究无人机自主飞行控制问题，虽可以明显降低其研究成本，但随着时间的增加，传感器的测量误差被叠加，严重影响无人机的控制精度。因此本文从低成本传感器入手，研究固定翼无人机自主飞行控制问题。

由于无人机系统的内部动态规律无法直接测量，状态估计作为传感器测量与控制器解算的中间层，以可获取的测量数据作为输入估算出无人机当前状态，反馈至控制器形成闭环，其准确性直接影响无人机的定位精度和控制效果。本文关心状态估计应用于低成本传感器对误差的影响。

本文针对无人机状态反馈问题，结合无人机模型预测控制算法探索无人机飞行过程中良好的运动估计方法。扩展卡尔曼滤波是通过这采样时刻点附近线性化获得近似线性模型，然后采用经典卡尔曼滤波方法进行状态估计，以解决非线性动态过程的状态估计问题，获得可以不直接测量的无人机状态，为解决低成本传感器带来的漂移现象和控制精度等问题提供思路[1]。

1  无人机建模及问题描述

1.1  坐标系之间的转换

固定翼无人机通过调节升降舵、副翼和方向舵的角度来改变无人机的受力大小，进而调整无人机的姿态，无人机的推力用于控制飞行速度。无人机运动估计涉及惯性坐标系FI、机体坐标系FB，其二者之间的转换关系如图1所示。

其中惯性坐标系（NED轴系统）原点在地球上某一点处，N轴指向北，并且和地球表面相切;E轴沿着一条恒定纬度指向东方，并且和地球表面相切;D轴指向地球的中心。在本文中，慣性坐标系随着经度和纬度的变化而变化。

机体坐标系FB原点位于无人机质心，XB平行于机身轴线，指向机头方向为正;YB垂直于对称面，指向右翼为正;ZB在飞机对称面内垂直于XB，指向下方为正。

无人机的位置、速度和姿态系统为非线性系统，其离散时间的状态空间模型为：

（1）

图1  无人机坐标示意图

其中，x表示系统状态变量，分别为速度分量V，α，β，角速度分量p，q，r，四元数表示的姿态q1，q2，q3，q4，实时位置N，E，h，推力T;y为系统观测向量;f（x，u）、h（x）均为非线性向量函数或矩阵;ω，υ分别为系统方程和量测方程中不可避免的过程噪声和量测噪声。

1.2  四元数描述姿态角

由于传感器所测量的值是相对于机体坐标系的，因此常在机体坐标系中描述无人机数学模型。参考方程（1）首先在慣性坐标系中无人机绕ZB轴转动偏航角ψ度，在此基础上，绕YB轴旋转俯仰角θ度，然后，绕XB轴旋转滚转角φ 度，此时坐标系与无人机机体坐标系重合。

（2）

三个欧拉角（ψ，θ，φ）和旋转顺序Z-Y-X完全定义了姿态。以欧拉角表示的旋转矩阵为：

（3）

由方程（3）可解算出无人机的三个姿态角为：

（4）

由于欧拉角包含大量的三角运算，计算时间较长，且可能出现奇异现象;而四元数法仅含四个未知量且易编程实现，故中本文所研究的项目中使用四元数法对无人机的姿态进行描述。在惯性参考系中，四元数的常规表示形式为，其中实部与旋转角μ有关，虚部与旋转轴i，j，k有关，ex，ey，ez是XI，YI，ZI轴的单位分向量，如下式[2]（5）;

（5）

在方程（5）中，矢量q可描述无人机的姿态，前三个参数包含旋转矢量信息，第四个参数仅包含旋转角度信息。ex，ey，ez可为旋转瞬轴和旋转方向。

机体坐标系经过三次顺序旋转得到的旋转矩阵T（q）可用四元数表示为：

（6）

由方程（6）可得方程（4）中的无人机的偏航角ψ、俯仰角θ和滚转角φ也可以表示为：

（7）

即用四元数表示姿态角为：

（8）

2  扩展卡尔曼滤波状态估计

扩展卡尔曼滤波器利用含噪声的传感器使随机系统的均方估计误差最小化，在采样点附近将非线性系统一阶线性化后忽略高阶项以满足近似线性，是高斯白噪声情况下的最小方差次优滤波器。估计周期可分为预测和校正两大部分，流程图[3]如图2所示。

2.1  引入地心地固坐标系

为了更好地描述无人机的飞行位置和姿态并对其进行测量和估计，引入地心地固坐标系（ECEF）[4]，该坐标系如图3所示，假设一个恒定半径为R的圆形地球模型，坐标原点与地球质心重合，z轴与地轴平行指向北极点;x轴指向赤道与本初子午线的交点;y轴在赤道平面上与x轴和z轴构成右手笛卡尔坐标系。引用λ表示纬度，指向北半球为正，在±90°之间取值;φ表示经度，本初子午线东边为正，在±180°之间取值;h表示高度，是地球表面和无人机的距离，指向地心为正。

2.2  建立预测方程模型

分析图3表示的经度、纬度、高度的时间导数，并以NED速度分量的形式表示为：

（9）

各个分量的时间导数为：

（10）

其中，方程（10）中的aN，aE，aD是在机体坐标系中获得的加速度测量值aX，aY，aZ经四元数逆变换到惯性参考系中获得的相应的加速度测量值，是补偿未测量的重力加速度。

（11）

结合方程（6）和方程（11）可得到速度分量的时间导数为：

（12）

GPS中收到信号解算过程延迟时间较长，如果不被建模，估计出的状态向量可能会出现较大误差，因此用一阶帕德近似对连续时间延迟建模[5]：

（13）

下标d表示延迟状态。将方程（13）写成微分方程的形式，并且应用于方程（9）和方程（12）以获得延迟位置和速度的模型为：

（14）

（15）

至此，定义出预测模型中的状态向量为：

（16）

2.3  建立量测模型

本文中使用GPS系统提供延迟位置和速度的直接测量值。定义如下：

（17）

磁力计提供和姿态有关的测量值，假设磁场是恒定的，通过四元数转换矩阵定义磁力计测量值为：

（18）

非线性测量向量由GPS和磁力计测量值组成，定义如下：

h（x）=[h1，h2，h3，h4，h5，h6，h7，h8，h9]T （19）

2.4  飞行状态估计

在k采样时刻，扩展卡尔曼滤波具体计算方法[6]如下：

Step1：设立系统模型，

（20）

方程（20）中，过程噪声和量测噪声的协方差矩阵可表达为：

E[ωωT]=QC，E[υυT]=RC（21）

计算状态矩阵和量测矩阵的雅可比矩阵对扩展卡尔曼滤波器进行线性化;

（22）

利用离散采样时间Ts将方程（22）转换到离散时域为：

（23）

并且，由于连续过程噪声直接映射到状态向量上，因此将过程噪声协方差矩阵QC离散化为：

（24）

Step2：设定采样时刻真实状态向量和估计状态向量之间的差值和误差协方差矩阵为：

（25）

Step3：设定初始状态和误差协方差P（0）;

Step4：k+1时刻的状态变量的先验估计;

（26）

Step5：k+1时刻的误差协方差的先验估计;

P-（k+1）=Φ（k）P（k）Φ（k）T+Q（k）（27）

Step6：k时刻卡尔曼滤波增益计算;

Kk=P（k+1）H（k+1）T[H（k+1）

P（k+1）H（k+1）T+R（k+1）]-1  （28）

Step7：k+1时刻状态量测变量的更新;

（29）

Step8：k+1时刻误差协方差的更新;

P（k+1）=[I-K（k+1）H（k+1）]P-（k+1）[I-K（k+1）

H（k+1）]T+K（k+1）R（k+1）K（k+1）T   （30）

在k+1时刻返回step1，循环往复，直至飞行任务结束。

3  仿真实验与分析

本文采用Matlab中的Simulink建立了固定翼无人机的控制系统模型，对某小型固定翼无人机进行整机控制律设计，在此基础上，对无人机位置、姿态进行相应的估计，验证本文所述的扩展卡尔曼滤波算法。其中，无人机参数为：m=5 kg，

g=9.81 ms-2，l=0.38 m，Jx=0.2 kgm2，Jy=0.36 kgm2，Jz=0.525 kgm2，量测数据通过捷联传感器获得，传感器模型如2.3节所示，控制器采用模型预测控制算法。

由于传感器的测量值易被白噪声破坏，因此将过程噪声协方差建模并参考文献[7]赋值为：

量测噪声协方差建模并参考文献[7]赋值为：

如图4所示的飞行航迹，姿态图显示了无人机在刚开始就向下俯冲，在大约5 s时反向上爬升直至收敛在一个稳定的高度。

仿真结果证明，滤波器在大约10 s后收敛并且几乎稳定在稳定解。随后，当滤波器在稳定状态下工作时，状态误差协方差的增量被各自元素的量测值连续抵消。

如图5、图6和图7所示，分别是EKF对偏航角、俯仰角和滚转角的估计效果。由图可知，加入扩展卡尔曼滤波器后，无人机在控制器的作用下，大概在10 s后收敛并趋于稳定，只是偏航角的变化较大，如图8所示的状态误差图可看出只是在飞行初始阶段会有较大的变化，待飞行平稳后便会逐渐收敛并于某一小值稳定。

4  结  论

本文研究基于扩展卡尔曼滤波算法应用在固定翼无人机飞行控制中，解決因低成本传感器的漂移等不良问题，具体是将测量数据和估计数据进行数据融合，从而修正陀螺仪产生的随机漂移误差，解决状态误差累计造成更严重后果的问题。通过实验，发现采用扩展卡尔曼滤波不仅可以有效解决上述问题，还可以帮助提高测量精度。

参考文献：

[1] GREWAL M S，ANDREWS A P.卡尔曼滤波理论与实践（MATLAB版）：第4版 [M].北京：电子工业出版社，2017.

[2] BEARD R W，MCLAIN T W.Small Unmanned Aircraft：Theory and Practice [M].Princeton：Princeton University Press，2012.

[3] 杨兆，沈作军.基于扩展卡尔曼滤波的小型固定翼无人机姿态估计方法分析 [J].航空科学技术，2017，28（11）：15-21.

[4] 金舒灿，胡越黎，张贺.基于EKF的四旋翼姿态解算仿真与设计 [J].电子技术应用，2017，43（9）：127-131+136.

[5] 丁磊，吴蔚劼，方挺.基于扩展卡尔曼滤波的多旋翼飞行器融合姿态解算算法 [J].安徽工业大学学报（自然科学版），2018，35（3）：240-248.

[6] 石宇芃，马宏军，陈豹.基于四元数的四旋翼无人机扩展卡尔曼滤波算法 [J].控制工程，2021，28（11）：2131-2135.

[7] HOUGH W J.Autonomous aerobatic flight of a fixed wing unmanned aerial vehicle [J].Stellenbosch University of Stellenbosch，2007.

作者简介：王文丽（1996.11—），女，汉族，甘肃酒泉人，硕士研究生在读，研究方向：无人机自主飞行控制。