四旋翼无人机姿态角算法设计

王宽田，李鹏，唐海超

(桂林电子科技大学海洋工程学院，广西北海，536000)

0 引言

四旋翼无人机飞行器具有结构简单，能垂直起飞和降落，控制简单，性能优越，体积小，制造成本低廉等优点。并且适合应用在军事和民用等领域，如抢险救灾、军事打击、环境监测和航空航天等；近年来许多国内外企业与高校开展四旋翼无人机飞行器的研究和开发。四旋翼无人机飞行器也是集电机学、空气动力学和自动控制学等多个学科于一体的复杂的欠驱动控制系统;针对四旋翼无人机飞行器的控制特性[1]。

1 四旋翼姿态信息解算

姿态信息的获取是四旋翼无人机飞行器控制的前提，只有精确获取当前的姿态角（横滚角、仰俯角、偏航角）,才能正确判断无人机飞行器位置[2]，根据当前位置与控制位置的比较结果来控制无人机飞行器的刚性十字架末端的四个电机的速度改变四个旋翼产生的升力的大小，从而迅速精确达到控制位置要求。获取精确的姿态角是无人机飞行器控制的关键，飞行器姿态信息是通过安装在飞行器中间的控制模块的三轴陀螺仪和三轴加速度计等传感器获取。

三轴陀螺仪主要用于测量飞行器的各种姿态运动的角速度，角速度进行时间的积分就可以得到飞行器的姿态角；陀螺仪的动态性能很好，很准确获取运动中物体的角速度，但陀螺仪在测量过程中存在温漂和噪声干扰等误差，这些误差将会给飞行器位置信息的判断带来干扰。并且这些误差在进行时间上的积分时，误差将会越来越大，飞行稳定性将会越来越差。飞行器飞行时间越长，陀螺仪获取的姿态角信息的误差就会越大，因此陀螺仪不适合测量长时间运行的物体姿态信息。

三轴加速度计主要用了测量重力加速度在载体坐标系上三个方向的分量。在静止时，可以准确获取三个方向的加速度，经过计算可以得到角度，但物体在运动时，由于自身的抖动给三轴加速度计带来了噪声干扰；因此使用单一的传感器获取飞行器的姿态信息都会不可避免的产生噪声干扰，但通过以上分析发现，三轴陀螺仪的动态性能很好，而三轴加速度计的静态性能很好，所以只要将三轴陀螺仪和三轴加速度计搭配在一起测量，当四旋翼在动态飞行时主要使用三轴陀螺仪测量姿态信息，当四旋翼在静态时则主要用三轴加速度计测量，最后将两组数据经过计算融合出来的姿态信息的误差就很小了。

2 卡尔曼滤波姿态解算

卡尔曼滤波器是一种还原真实数据的最优自回归数据处理算法，主要应用在噪声干扰的排除，此算法在上世纪六十年代由匈牙利数学家埃米尔·卡尔曼提出[3]。此滤波算法的原理是将上一个预测值和最近一次传感器测得的信息结合起来，再计算得到当前信息，为了对下次数据的预测做准备，同时对当前的协方差和卡尔曼增益更新。只需获得前一时刻估计值和当前的观测数据就可以用递推方法估算出每一时刻的最优值，卡尔曼算法不需要保存过去全部数据，只需保存两个数据，用计算机编写程序实现非常方便、执行非常高效。

卡尔曼滤波最先应用在阿波罗计划的轨道预测上，现在卡尔曼滤波算法广发应用在GPS定位、机器人控制和图像处理等领域中，四旋翼无人机飞行器控制系统姿态角卡尔曼滤波结构框图如图1所示。

图1 卡尔曼滤波器原理框图

根据以上结构图定义以下两组四旋翼无人机飞行器卡尔曼滤波基本的方程，即：状态运动方程和观测方程：

在上面的两个数学模型中，控制系统第K时刻和第K-1时刻的状态值表示为X(k)和X(K-1)，U(k)是k时刻陀螺仪对四旋翼无人机飞行器控制系统计算出来的角度偏移值。A、B称为控制系统参数。从以上控制系统运动方程公式中可以看出相邻两个时刻目标状态之间的关系。控制系统状态方程Z(k)是存在噪声干扰时加速度计在k时刻测量值，H是测量系统的已知常数。W(k)表示控制系统状态方程噪声，V(k)分别表示控制系统观测方程的噪声。为了便于对系统分析，我们假设W和V的协方差是个不随系统变化和环境变化而改变的常数。

控制系统中的每个时刻的状态都是根据控制系统上一时刻预测得到的，如果处于K时刻，预测值为：

式(3)中，X(k|k-1)是控制系统根据上K-1时刻最优预测值得到的K时刻的预估值，X(k-1|k-1)是K-1时刻的最优预测值；U(k)为K时刻陀螺仪状态值。因为是利用状态方程对状态进行更新，反映了状态随时间演化的规律，因此，称之为时间更新。

经过以上几步的计算，控制系统的数据已经刷新为最新值了，接下计算预估值相对于真实值的协方差：

式(4)中，P(k|k-1)是根据K-1时刻的预测得到的预估协方差，P(k-1|k-1)是X(k-1|k-1)在K-1时刻的协方差，A’为A的转置矩阵，Q是系统过程的协方差。

经过以上计算得到控制系统当前状态的最新预估值，然后再根据传感器测量的状态值；将预估值和测量结合起来，就可计算控制系统当前状态最优估算值X(k|k)：

其中Kg为控制系统在K时刻的卡尔曼增益(Kalman Gain)：

图2 卡尔曼滤波器静态响应和静态误差

图3 卡尔曼滤波器动态响应

到现在为止，X(k|k)就是我们需要的第K时刻的最优估算值。为预测下一时刻的最优估算值，使得卡尔曼滤波器连续运行，需要更新K时刻的协方差：

P(k|k)为第K时刻的系统协方差，当系统进入k+1时刻时，就可以用这个协方差计算系统最优估算值了，P(k|k)就是式子(7)的P(k-1|k-1)。这样，每个时刻都执行以上的5个公式就可以得到此刻的最优化估算值和最新的协方差，在程序中采用递归算法可以实现。根据上述卡尔曼滤波器算法，在仿真软件Matlab[4]软件里编写程序进行仿真验证其滤波效果，分别对系统的静态值和动态值进行仿真，其中设定静态的理论值为0°，动态的理论值为5sin（t/100）°，系统采样频率为1KHz，假设测量误差的方差为0.5°，仿真分析结果如图2和图3所示。

3 结论

分析卡尔曼滤波器静态响应和静态响应误差可知，卡尔曼滤波器器能将偏离静态实际值较大的误差滤除掉，滤波效果较好。经过滤波器滤波后，静态误差理论值的误差能控制在0.5°以内，对四旋翼无人机飞行器控制系统的稳定性影响不大，卡尔曼动态响应和动态误差能控制在0.5°以下。经上述卡尔曼滤波器的滤波效果仿真分析，卡尔曼滤波适用于四旋翼无人机飞行器的姿态解算。