基于卡尔曼滤波的四旋翼飞行器控制算法

胡开明，李跃忠，刘 薇

(1.东华理工大学放射性地质与勘探技术国防重点学科实验室，江西 南昌 330013；2.江西省新能源工艺及装备工程技术研究中心，江西 南昌 330013 )

0 引言

微型旋翼飞行器因具有飞行姿态灵活、体积小、悬停和低速性能优越等特点而受到广泛的关注，目前已成为微型无人飞行器研究领域的热点[1]。可以方便地实现垂直起降、空中悬停的功能，也可以携带摄像机、测绘仪器及各种特定装置，在自动操控或者人工操作下，按照预期的航线和姿态飞行，以完成既定的任务。但是由于飞行器姿态检测中的加速度数据掺杂着高斯白噪声干扰，致使飞行器稳定性较差[2]。目前研究大多数集中在飞行器的设计与飞行算法的研究上，没有对飞行器惯性单元产生的干扰信号进行分析处理。本文针对此问题，提出了以四元数法与卡尔曼滤波相结合对飞行器的姿态数据做互补的融合处理方法。

1 四旋翼飞行器工作原理与结构组成

1.1 四旋翼飞行器工作原理

四旋翼飞行器由四台电机组成，一号和三号电机一对按顺时针旋转，二号和四号电机一对按逆时针旋转。当四轴飞行器工作运行时，空气动力扭矩效应和陀螺效应可以相互抵消，如图1所示。四轴飞行器具有各旋翼的旋转方向与旋翼对机身所施加的反扭矩方向相反的特点，因此当一号电机和三号电机顺时针旋转时，二号电机和四号电机做逆时针旋转，用来平衡各旋翼对机身的扭矩作用。四轴飞行器具有6个自由度，通过调节各旋翼的转速来控制这6个自由度，以实现俯仰运动、滚转运动、垂直运动、偏航运动、前后运动和侧向运动[3]。

图1 四旋翼飞行器工作原理Fig.1 Working principle of of quadroter aircraft

1.2 四旋翼飞行器的结构组成

四旋翼飞行器主要由惯性测量模块、姿态算解模块、姿态控制模块、电机驱动模块、通信模块、电源管理模块组成。总体设计框图如图2所示。

图2 四旋翼飞行器控制框图Fig.2 Control diagram of quadroter aircraft

设计中姿态算解、姿态控制模块由ST公司的STM32F103单片机完成计算。STM32F103单片机是ST公司推出的高性能ARM-Cortex M3架构微处理器。除此之外，STM32F103还要完成读取通信模块信息，读取惯性测量模块测量值，产生电机驱动所需的PWM信号等任务。通信模块采用台产NRF24L01 2.4 G无线通信模块，与STM32F103通过SPI接口相连。惯性测量单元采用MPU6050，内部集成3轴加速度计与三轴陀螺仪，与STM32F103通过I2C接口相连。

2 四旋翼飞行器控制算法设计

2.1 飞行器姿态算解

2.1.1机身坐标系

微型四旋翼飞行器具有飞行高度低、运行速度小的特点，因此在定义参考坐标系和机身坐标系时可以忽略地球自转对飞行器姿态描述的影响。设大地是一个平面，参考坐标系OoXoYo与地面固定，坐标原点Oo定义为四旋翼飞行器的起飞点，三个坐标轴OoXo指向为北，OoYo指向东，OoZo指向朝上并保持与地面垂直。将飞行器机身坐标系ObXbYb固定在机体上，机身坐标系的原点Ob保持与机体的几何中心重合，则坐标轴ObXb为飞行器前向飞行的方向，坐标轴ObYb指向飞行器的右方，坐标轴ObZb指向地面且与垂直于机体平面ObXbYb。

根据上面所定义的参考坐标系与机身坐标系可以引入参考坐标系与机身坐标系之间的欧拉角，即偏航角φ、俯仰角θ和翻滚角ψ。偏航角φ表示由坐标轴Xo投影到平面OoXoYo的角度，其定义区间为[-180°，180°]，绕动方向如果与坐标轴Zb指向相同取值为正，相反为负。俯仰角θ表示坐标轴Xb与平面OoXoYo间的角度，俯仰角的定义区间为[-90°，90°]，坐标轴Xb指向上为正，向下为负。翻滚角ψ为坐标轴Yb与OoXoYo平面间的角度，翻滚角范围区间 [-180°，180°]，坐标轴Yb向下指向为正，向上为负。

如果机身坐标系与参考坐标系的偏航角、俯仰角和翻滚角已知，就可以把相对于机身坐标系的测量参数转换成相对于参考坐标系的测量参数。

2.1.2姿态表达式

机身姿态采用四元数法来表示以解决欧拉角的万向节锁死的问题[4]。方向余弦矩阵的列表示机身坐标系中的单位向量在参考坐标系中的投影。在机身坐标系中定义的向量rb通过左乘方向余弦矩阵C来旋转到参考坐标系。

r0=Crb

(1)

将偏航角φ、俯仰角θ和翻滚角ψ写成三个独立的方向余弦矩阵[4]。得到欧拉角形式的方向余弦矩阵为：

(2)

式(2)中，C′为C的转置。

四元数表达式是一个具有四个参数的姿态表达式。机身坐标系到大地坐标系的变化可以看作是通过绕定义在参考坐标系中的矢量μ的单次旋转来实现的。 四元数是一个四维矢量，用符号q来表示：

(3)

式(3)中，μx，μy，μz为μ在各个坐标轴上的分量：

将四元数用三角函数代换掉后可得

(4)

2.2 卡尔曼滤波器

2.2.1卡尔曼滤波器设计

机身姿态算解的核心就是解上节所述的四元数微分运动方程。电机运转时加速度计输出的加速度数据参杂着高斯白噪声，适合采用抗干扰能力较强的卡尔曼滤波器来设计。卡尔曼滤波器是一套可以由计算机实现的实时递推算法[5]。它以随机信号作为处理对象，依据系统噪声和观测噪声的统计特性，将观测量作为滤波器的输入量，状态变量的估计值作为输出量，根据系统的状态方程和观测方程对系统状态信号进行最优估计的算法[6]。

(5)

(6)

式(6)中，R是观测测量噪声的协方差。

(7)

由上述卡尔曼滤波器原理分析可知，只要给定系统初始状态和误差协方差矩阵，就能根据观测值递推到第k时刻的估计值。

将姿态四元数作为状态变量，系统状态矩阵A为：

(8)

式(8)中，ωx，ωy，ωz为陀螺仪中读出的三轴角速度,Δt为两倍采样时间。

h=CV

(9)

对h求偏导数得到在状态变量X附近的线性化系数H：

(10)

即Zk可以近似的表示为：

Zk=HXk

(11)

根据式(8)得到的系统状态矩阵和式(11)得到的系统输出方程，代入卡尔曼滤波器递推公式(5)—(7)即可推出卡尔曼滤波器的估值。

2.2.2卡尔曼滤波器程序设计

卡尔曼滤波器程序设计将卡尔曼函数表示成矩阵运算的数据结构体的形式，包含卡尔曼滤波器初始化和卡尔曼滤波器更新。卡尔曼滤波器初始化函数会将卡尔曼滤波器结构体中的矩阵初始化好，并对它们赋予初值。卡尔曼滤波器更新函数会将根据MPU6050测量结果及时更新卡尔曼滤波器结构体中的矩阵。根据过去时刻的姿态和当前时刻的测量值由卡尔曼滤波器计算出当前时刻新的姿态进行姿态更新[7]。卡尔曼滤波器初始化函数和更新函数流程图如图3所示。

图3 卡尔曼滤波器软件流程图Fig.3 Software flow chart of kalman filter

2.3 PID控制

2.3.1PID控制回路

系统采用PID控制器，根据飞行运动状态将控制回路简化为四个运动控制回路，分别是垂直回路、偏航回路、滚转回路和俯仰回路。控制回路的给定信号来源于无线通信模块。无线通信模块接收来自遥控系统的信号。机身姿态反馈信号来自卡尔曼滤波器，卡尔曼滤波器计算出的机身姿态四元数转换为姿态角送给各个控制回路。高度反馈信号来自气压计，反馈信号和给定信号之差即误差信号经过PID控制器校正后送到电机驱动从而驱动电机运行[8]。

2.3.2电机控制

电机调速方法采用脉冲宽度调制(PWM)法[7]。当频率较高的PWM信号经过电机时，电机可以看成一个一阶惯性环节，所以PWM信号可以等效为一直流信号，其幅值为PWM信号的占空比D乘以PWM信号的峰值Vpp。

姿态控制回路中输出升力u1，u2，u3进行电机控制。根据飞行器的基本原理可以由u1，u2，u3和位置控制回路中的u4导出每个电机的升力大小：

(12)

已知电机转速和电机升力成平方关系，所以电机转速可以表示为：

(13)

而电机转速乘以系数C即可变成占空比D。

3 试验与结果

3.1 卡尔曼滤波器的调试

设计中采用串口将机身姿态数据发送至上位机，通过上位机实现监控功能，从而比较各种参数下的系统性能。图4为其飞控状态监控界面图，可以完成飞行器的姿态角的3D显示和各电机的PWM值的显示，也能对姿态角等数据通过波形的方式显示出来。

图4 运行状况监控图Fig.4 Operation monitoring chart

卡尔曼滤波器在滤波过程中有三个滤波参数会随着时间和输入量而改变，因为三个方向的数据都是从一个加速度计中得来的，所以其滤波参数都是一样的，其中随时间变化的三个参数分别为：卡尔曼增益Kg，新的状态误差方差Now_P和上一次观测误差方差Last_P ，而它们随着滤波的进行会快速的收敛，最后为一定值。为了验证这三个参数的变化，将这三个数值用三个数组分别提取出来，每个数组取了80个数，然后将这三组各80个数的数据用Excel进行数据处理得到三个参数的散点图。图5为Now_P的散点图，图6为Last_P的散点图，图7为Kg的散点图。

图5 Now_P的散点图Fig.5 Scatter diagram of Now_P

图6 Last_P的散点图Fig.6 Scatter diagram of Last_P

图7 为Kg的散点图Fig.7 Scatter diagram of Kg

从上面三组数据曲线可以看出，这三个变量会随着时间变化而趋于一稳定值。

将三个方向的角加速度卡尔曼滤波前和滤波后的值用UART口发给上位机，通过上位机得到三个方向的各两组加速度曲线图，以验证卡尔曼的滤波效果。其中图8(a)和图8(b)分别为X轴方向的加速度滤波前和滤波后的值，图9(a)和图9(b)分别为Y轴方向的加速度滤波前和滤波后的值，图10(a)和图10(b)分别为Z轴方向的加速度滤波前和滤波后的值。

从三组对比图可以看出卡尔曼滤波器的滤波效果较好，能很好地滤除高斯噪声。

3.2 运行测试

设计制作四旋翼飞行器，采用PID控制以PWM方式驱动电机，分别控制偏航角、俯仰角和翻滚角。整定翻滚角PID控制参数Kp为1.6，Ki为0.4，Kd为 0.3，飞行器处于悬停状况下进行动态测试的数据如表1所示。

图9 Y轴方向加速度曲线Fig.9 Acceleration curve in Y-axis

图10 Z轴方向加速度曲线Fig.10 Acceleration curve in Z-axis

从表1中的各组数据的变化可以看出期望值与反馈值的差值越大控制量的输出值越大，符合控制要求。偏航角和俯仰角的测试方法类似。

表1 翻滚角PID控制数据Tab.1 PID control data of roll angle

PID的控制算法最后得到的控制量为控制电机的PWM的点空比，通过改变飞行器在旋飞时的姿态角或者通过遥控器改变飞行器的姿态角期望值，查看各个电机的PWM输入量的变化来验证电机控制的效果。图11为向前飞行时四个电机的PWM值，同样可以得到向后飞行、向左飞行和向右飞行的PWM曲线。

图11 向前飞行时四个电机的PWM值Fig.11 PWM values of four motors in forward flight

由图11可知当摇杆控制飞行器向前飞行时，电机1至电机4的PWM值分别为68%，4%，6.5%，71.5%，飞行器的俯仰角有递减的趋势，能实现向前飞行的功能。经过测试，飞行器能够较快地恢复到平衡并最终稳定下来，表明设计合理，满足要求。

4 结论

本文提出了四元数法与卡尔曼滤波算法相结合的方法，对姿态数据进行融合处理，并研究了微型四旋翼飞行器的PID控制方法，完成了飞行控制效果的试验测试。测试结果与理论分析表明，该方法实现了良好的滤波效果，达到了飞行器平稳飞行的目的。