基于扩展卡尔曼滤波的多旋翼飞行器融合姿态解算算法

丁 磊，吴蔚劼，方 挺

(1.安徽工业大学电气与信息工程学院，安徽马鞍山243000；2.西安电子科技大学通信工程学院，陕西西安710126)

多旋翼飞行器是一种能够自由起降，且能稳定悬停于空中的六自由度飞行器。该飞行器起飞不受场地限制，可在任意场合下工作，甚至可用于执行各种高危任务，故其在侦察敌情、灾区救援、道路监控、农业植保、电力巡线等领域有较好的应用前景[1]。姿态控制是飞行器总体设计的重要组成部分，而姿态解算是飞行控制系统的核心。飞行器具有欠驱动、多变量等比较复杂的特性[2]，其稳定飞行与灵活控制对实时姿态解算的要求更高[3]，故飞行器姿态角的实时解算算法是多旋翼飞行器研究的热点之一。

利用惯性导航测量器件如加速度计、陀螺仪和电子罗盘可快速、稳定地测量飞行器的当前姿态角，但多旋翼飞行器飞行时电机高速转动，机身随之产生高频振动，加速度计对机身振动异常敏感，致使其测量数据中掺杂大量高频噪声，导致后续姿态解算的结果产生较大误差。常用飞行器飞行姿态的解算算法有欧拉角、方向余弦、四元数互补滤波算法等。其中：欧拉角法解析过程比较简单，但包含大量的三角函数运算，计算时间较长，且当俯仰角转动90°时会出现万向锁现象，不能进行全姿态解算[4]；方向余弦法可避免万向锁现象的发生，能够进行全姿态的解算，但姿态矩阵微分方程是线性微分方程并含9个未知量，计算量大，实时性较差[5]；四元数法仅含4个未知量，计算量小且算法易编程实现，可进行全姿态的解算[6]；互补滤波算法是一种数据融合算法，其根据传感器的不同特性，通过融合低通滤波和高通滤波接近真实信号[7]。径向基(radial basis function，RBF)神经网络是一种局部逼近网络，具全局唯一最佳逼近的特性，且其结构简单、训练速度快[8-9]。鉴于此，以四元数互补滤波算法解算的多旋翼飞行器姿态角作为参考向量，采用RBF神经网络设计加速度计非线性高频滤波器，用扩展卡尔曼滤波算法实时解算飞行器的姿态角，以期抑制机身振动对加速度噪声的干扰，提高飞行器姿态角的解算精度。

1 姿态解算

1.1 惯性导航

假设飞行器坐标系为Hb，大地坐标系为Hc，将飞行器坐标系分解成按Z-Y-X轴顺序转动，3次转动均绕飞行器坐标系的某一轴。首先飞行器绕大地坐标系Z轴，以右手系转动角度ψ得到飞行器新的坐标系，ψ为飞行器的偏航角Yaw。绕Z轴定轴转动的基本旋转矩阵为

再绕新的机体坐标系Y轴，以右手系转动角度θ，得到飞行器新的坐标系。θ为飞行器的俯仰角Pitch，绕Y轴定轴转动的基本旋转矩阵为

最后绕新的机体坐标系X轴，以右手系转动角度φ，得到飞行器的最终坐标系。φ为飞行器的横滚角Roll，绕X轴定轴转动的基本旋转矩阵为

3次分解的动作可视为飞行器从初始位置到最终位置的一次性等效旋转。按欧拉角Z-Y-X依次进行转动，令旋转矩阵为，则

由式(4)旋转矩阵可直接解算出飞行器的偏航角ψ、俯仰角θ和横滚角φ，如

1.2 四元数互补滤波算法

1.2.1 姿态角的四元数法表示

从式(7)可获得飞行器的偏航角ψ、俯仰角θ和横滚角φ。

1.2.2 互补滤波算法

对于6轴运动处理组件MPU6050，加速度计对多旋翼飞行器的加速度较为敏感，用加速度计的实时值计算飞行器当前倾角误差较大；陀螺仪经过积分得到的角度不受加速度计的影响，但随时间的推移，其积分漂移和温漂会引起较大误差[11-13]。

图1 互补滤波原理Fig.1 Principle of complementary filter

根据互补滤波的工作原理(如图1)，假设加速度计的测量数据为t1=r+e+u1，陀螺仪的测量数据为t2=r+e+u2，其中r为真实数据；e为传感器本身的误差；u1，u2分别为高频噪声和低频噪声。设传递函数之和为1，为低通滤波器，用来滤除加速度计的高频噪声；为高通滤波器，用来滤除陀螺仪的低频噪声。通过互补滤波得到估计传递函数：

其中：R(s)和E(s)为全通量，包含真实数据和传感器本身的误差，而加速度计中所含的高频噪声信号U1(s)被H1(s)低通滤波器滤除，只容许低频信号通过；陀螺仪所含的低频噪声信号U2(s)被H2(s)高通滤波器滤除，只容许高频信号通过。互补滤波算法根据陀螺仪、加速度计的不同特性，短时间内采用陀螺仪的数据作为最优参考，同时对加速度计的数据进行滑动窗口滤波处理，用其得到的值不断地对陀螺仪的数据进行校正[14-15]，通过融合低通滤波和高通滤波计算出接近真实信号的数据，供后续飞行器姿态解算使用。

2 RBF神经网络非线性滤波

RBF神经网络输入层与隐藏层间的映射为非线性关系，隐藏层与输出层间的映射为线性关系[16]，其网络结构如图2。RBF神经网络隐藏层神经元的输出为

其中：X为含有噪声的m维加速度计数据，作为输入向量；Ci为第i个神经元基函数的中心；σi为第i个神经元的基宽；Gi为高斯径向基函数。

图2 RBF神经网络结构Fig.2 Structure of RBF neural network

RBF神经网络输出层的输出yj为

其中：Wij为隐藏层第i个神经元与输出层第j个神经元间的权重；bj为输出层第j个神经元的阈值。

RBF神经网络辨识器的性能评价函数J(k)为

其中：k为任一时刻；yj(k)为实际输出；为网络输出，经过网络训练，使得J越小越好，满足J≤ε的条件，ε为一个极小的正数。

RBF神经网络中心的选取采用自组织学习法，通过监督学习训练隐藏层与输出层间的权值和阈值[17]。网络的权值经由梯度下降算法计算不断改变，能够快速收敛到全局最优点。权值的更新算法为

其中a为学习速率，文中取0.1。学习速率太大则无法快速逼近最优值，会在其附近来回振荡；太小则收敛速度太慢。

非线性滤波器的权值系数含实时性的问题，需能够实时、快速求解。系统的非线性差分方程可表示为

式中：p为当前时刻，p+1为下一时刻，前者为系统的状态方程；后者为系统的观测方程；f，h为非线性函数；v和w为白噪声序列且均值为0。非线性滤波的最优值是通过y(p)来估计出系统的(p)，并使其是x(p)的最优估计。RBF神经网络实际上是系统对f和h两个非线性函数的最佳逼近。

图3 加速度计RBF神经网络非线性滤波基本原理Fig.3 Basic principle of nonlinear filtering of accelerometer RBF neural network

利用RBF神经网络对加速度计姿态解算的角度值进行非线性滤波的基本原理如图3。图3中加速度计解算的角度作为输入信号，该信号包含真值X与噪声ε，参考信号d为互补滤波姿态解算算法解算出的角度，经RBF神经网络非线性滤波算法得到输出信号Y。

3 扩展卡尔曼滤波融合姿态解算算法

多旋翼飞行器系统为非线性系统，传统的卡尔曼滤波算法只适用于线性系统，故文中采用扩展卡尔曼滤波算法(EKF)解决多旋翼飞行器系统的非线性问题[18-20]。扩展卡尔曼滤波算法将系统的非线性部分进行泰勒级数展开，忽略其高阶无穷小部分，同时保留系统的线性部分，最终得到近似模型，将其非线性问题转化为线性问题[21-23]。其通过上一时刻的角度X(k)预测更新下一时刻的角度X(k-1)，是一种典型的递推算法，包括系统状态方程和观测方程的建立。

建立系统的状态方程，如

其中W(k-1)为系统的噪声向量，选取互补滤波姿态解算算法解算出的姿态角为系统的状态量，误差较小。

建立系统的观测方程，如

式中：H为单位矩阵；Vk为观测的测量噪声。将经RBF神经网络非线性滤波后的加速度计解算的角度作为观测向量。

之后，通过以下步骤完成系统状态的预测与更新[24-25]。

1)状态的预测

式中：A为系统参数矩阵；X(k|k-1)为利用上一时刻状态预测的结果；X(k-1|k-1)为上一时刻的最优预测值；B为控制参数矩阵；U(k)为当前状态的控制量。通过上一时刻的最优预测值预测下一时刻的状态值。

2)协方差矩阵的预测

其中P(k|k-1)为X(k|k-1)对应的协方差矩阵，同时也是P(k-1|k-1)协方差矩阵的估计，Q为系统噪声。

3)卡尔曼增益的更新

式中：Kg(k)为卡尔曼增益；R为观测噪声的协方差矩阵。

4)状态的更新

其中Z(k)为观测矩阵。

5)协方差矩阵的更新

其中E为单位矩阵。

扩展卡尔曼滤波融合姿态解算算法的原理图如图4。

图4 扩展卡尔曼滤波融合姿态解算算法原理Fig.4 Principle of extended Kalman filter fusion attitude solving algorithm

4 实验仿真分析

基于STM32单片机、MPU6050六轴运动传感器及HMC5883电子罗盘，组成飞行器姿态解算硬件系统，利用提出的四元数互补滤波姿态解算算法对飞行器的俯仰角、横滚角、偏航角分别进行静态和动态的实时解算。偏航角的解算需用到电子罗盘，利用加速度计只能单独进行俯仰角及横滚角静态和动态两种情况的解算，之后采用RBF神经网络对加速度计解算的姿态角进行非线性高频滤波，其中径向基函数的扩展系数取1.0。最后通过扩展卡尔曼滤波融合姿态解算算法解算出飞行器静态和动态的俯仰角和横滚角。系统每2 ms进行1次姿态角解算。互补滤波姿态算法解算的结果如图5，6。加速度计姿态角滤波前的结果如图7。加速度计解算的姿态角经RBF滤波后的结果如图8。扩展卡尔滤波解算的姿态角静态动态结果如图9，10。

从图5可看出：互补滤波姿态解算算法解算出的静态姿态角效果良好，俯仰角的解算范围在(-0.23°，+0.23°)，横滚角的解算范围在(-0.2°，+0.27°)，经过水平校准后，可基本保持在0°附近，且在长时间测量过程中角度没有发散；偏航角的解算幅度在0.5°以内，长时间范围内稳定、不发散。从图6可看出，互补滤波姿态解算算法解算出的动态姿态角数据曲线有少许毛刺，但从总体效果来看，姿态角的解算效果实时性强、不发散，姿态跟踪速度快，能够满足基本日常飞行需求。从图7可看出，加速度计仅通过滑动窗口滤波，因高频噪声的干扰导致姿态角波动范围较大，静态时的角度波动范围达到2°，动态时有尖峰产生，波形毛刺较多，测量误差较大。从图8可看出：加速度计解算的姿态角经RBF神经网络非线性高频滤波，静态测量的姿态角较为稳定，趋近于0°且高频噪声得到有效滤除；姿态角振幅在0.2°左右，动态测量的姿态角与滤波前相比，滤波效果较为明显，尖峰部分得到一定程度减弱，高频噪声得到有效抑制，数据波形更加稳定、光滑。

图6 互补滤波姿态解算动态结果Fig.6 Dynamic results of complementary filtering attitude solution

图7 加速度计姿态解算滤波前的结果Fig.7 Results of accelerometer attitude solution before filtering

图9 扩展卡尔曼滤波解算的姿态角静态结果Fig.9 Static results of the attitude angle calculated by extended Kalman filter

图10 扩展卡尔曼滤波融合姿态解算的姿态角动态结果Fig.10 Dynamic results of the attitude angle of extended Kalman filter fusion attitude solution

从图9，10可看出：扩展卡尔曼滤波融合姿态解算的效果比互补滤波解算的效果更好、精度更高，静态时，俯仰角的解算范围在(-0.07°，+0.13°)，横滚角的解算范围在(-0.04°，+0.15°)，姿态角的解算幅度在0.2°以内，解算精度有很大提高；与互补滤波解算效果相比，动态时，解算的姿态角曲线更为平滑，毛刺部分得到有效消除，稳定性更高，响应速度和收敛速度更快，能够实时追踪飞行器的当前姿态角。

5 结 论

针对飞行器飞行时机身抖动对加速度计传感器产生较大的高频噪声干扰，采用基于RBF神经网络的非线性高频滤波算法，并结合互补滤波算法对飞行器的姿态角进行扩展卡尔曼滤波的融合姿态解算。实验表明：扩展卡尔曼滤波的融合姿态解算算法对高频噪声有很好的抑制作用，提高了加速度计对姿态的测量精度以及飞行控制系统中姿态解算算法的解算精度，使得解算出的姿态角精度得到有效提高且在长时间范围内稳定性更强。