四轴飞行器控制系统设计与实现

时间：2024-08-31

茹岩，张欣

(柳州铁道职业技术学院，广西柳州，545616)

0 引言

四旋翼飞行器最早出现在公众视野可能要追溯到2009年的著名印度电影《三傻》……2010年，法国Parrot公司发布了世界上首款流行的四旋翼飞行器AR.Drone。作为一个高科技玩具，它的性能非常优秀：轻便、灵活、安全、控制简单，还能通过传感器悬停，用WIFI传送相机图像到手机上。在航拍领域有着其独特的优势，目前在国内做的比较好的就是深圳的大疆公司，它研发的飞行器操作相对比较简单，飞行稳定性和可玩性都比较高。但是价格比较贵，目前还无法普及。

本文设计了一种低成本的捷连贯性导航系统，通过多种传感器对飞行姿态进行实时采集，并利用合适的滤波算法对传感器采集的数据进行处理，提高姿态角度的精度。借助无线数传模块，实现姿态数据的实时传输，完成了四轴飞行器的实时姿态控制。

1 姿态检测与控制系统组成

四轴飞行器主要无线通讯模块，姿态采集模块和电机驱动模块组成，系统的硬件平台如图1所示。

图1 硬件系统设计

本系统以STM32F103为主控芯片构建中央处理器模块，负责接收和处理传感器数据，根据姿态信息计算出相应驱动指令驱动四个电机达到想要的飞行效果。采用MPU9150模块作为姿态采集传感器，MPU9150集成了3轴MEMS陀螺仪、3轴MEMS加速度计和磁力计。模块通过IIC总线将采集到的x、y、z三轴上的数字信号传递给中央处理器，处理器对原始数据进行预处理得到初始角度和角速度值，再经过滤波融合算法得到准确稳定的姿态数据。最终中央处理器通过接收遥控模块发出的指令来驱动和控制四个直流无刷电机做出相应的飞行动作和保持动态的平衡。

本次设计的硬件系统模型如图2所示。

图2 硬件系统模型

2 传感器的数据融合

要使飞行器稳定飞行需要获取飞行器当前精确的飞行姿态，得到准确的姿态角就显得非常重要。这里我们使用陀螺仪、加速度计和磁力计冗余实时测量飞行器当前姿态。加速度计测量飞行器自身加速度，通过与重力加速度对比，计算当前飞行器倾角。加速度计对外界干扰噪声非常敏感，尤其是振动噪声。陀螺仪是测量机体旋转角速度的器件，通过积分运算，可以得到机体的旋转角度。由于陀螺仪的数据需要积分才能得到角度，存在积分漂移，随着时间的不断累积，误差会不断加大。而磁力计又容易受到外部磁场的干扰，解算出来的偏航角容易受到干扰。

由于传感器测量值存在误差和噪声干扰，所以如果直接使用原始数据计算姿态角度，累积误差会不断增大，可以采用滤波手段除去数据中的噪声干扰，并将陀螺仪、加速度计和陀螺仪三者进行数据融合，修正陀螺仪的累积误差，即可得到较为精确的飞行姿态。下面我们对卡尔曼滤波和互补滤波进行了对比。

2.1 卡尔曼滤波

卡尔曼滤波器是利用目标的动态信息，设法除去噪声干扰，从包含噪声的有限信号中预测出目标真实值的滤波算法，可以对目标位置的进行最优估计，可以对当前目标位置、将来目标位置或者过去目标位置进行估计。同时，卡尔曼滤波器采用递归式估计，只需当前状态的观测值和前一时刻的状态估计值就可以计算出当前状态的估计值，不必浪费大量内存记录观测值和估计估计值的历史数据，有效的节约系统资源，提高运算速度。

图3中Q为过程噪声，R为测量噪声，通过实验Q增大，动态响应变快，收敛稳定性变坏，R增大，动态响应变慢，收敛稳定性变好。

图3 卡尔曼滤波算法框图

2.2 互补滤波融合算法

飞行器有三个姿态角度，俯仰偏航和橫滚角。由于飞行器的偏航不会受到重力加速度的方向，而且横滚角和俯仰角也不会影响偏航角度，所以分成两路分别进行互补滤波。

利用加速度计的瞬时值来计算飞行器当前姿态角误差相对较大；而陀螺仪积分得到的角度不受飞行器加速度的影响，噪声干扰小，然而利用陀螺仪数据计算飞行器姿态角存在积分漂移和温度漂移，而且误差会随着时间的不断推移而不断增大。因此可以使这两个传感器取长补短，弥补彼此的缺点。如图4所示为俯仰角和横滚角的互补滤波器原理。

图4 陀螺仪与加速度互补

加速度计是测量机体加速度的器件，受外界环境干扰较大，噪声干扰严重。四轴飞行器由于环境及材料等原因，飞行过程振动比较强烈，尤其是遇到大风等情况，加速度计的测量很容易混入高频噪声。陀螺仪是测量角加速度的传感器，受外界环境的干扰较小，通过积分获得的角度在短时间内比较准确，但是存在积分误差和温度漂移，误差随着积分时间不断增长而不断增大。根据传感器的不同特性，可通过高通滤波器滤掉加速度计的高频信号，而通过低通滤波器滤掉陀螺仪低频信号，然后两个信号叠加即可得到整个频带的信号。互补滤波就是在短时间内以陀螺仪测量得到的角度为主，定时对加速度采样，利用三角函数计算得到的角度取平均值来修正陀螺仪的累积误差，从而得到正确的角度。简而言之，短时间内陀螺仪测量计算角度受噪声干扰小，相对准备，以其为主，长时间内加速度测量计算角度误差较小，此时增大加速度计的权重以纠正陀螺仪的误差。

假设Acc为加速度计的测量值，则有：

陀螺仪角速度为ω，经过积分得到的角度为θt：

互补滤波更新飞行器姿态角公式：

θ是最终融合更新的姿态角度，其中，K1、K2是由系统时间常数计算得到的常数，且K1+K2=1。在姿态解算中，主要条件这两个参数大小，在短时间内以陀螺仪为主，长时间则趋近与加速度计的平均值。

而对于偏航角，如图5 使用加速度和陀螺仪就无法准确测出飞行器的偏航角度，就需要磁力计和陀螺仪进行互补滤波，磁力计测得的航向角容易受到外部磁场的干扰，而陀螺仪不容易受到外部的干扰。互补的原理与加速度计和陀螺仪类似。

图5 陀螺仪与磁力计互补

2.3 滤波实验及分析

实验分析：将加速度计、陀螺仪和磁力计的姿态角及滤波前后的数据导入MATLAB进行仿真。

图6中加入了瞬间倾斜(强加速度)干扰，可以看出，此时的加速度计解算的角度值在干扰下将出现较大的毛刺，最大的误差达到了20度而互补滤波得到的角度值并不受干扰影响。

图6 加速度计与互补滤波

图7中而陀螺仪积分出来的角度值随者时间的积累将产生逐步增大的误差。最大误差将近10度。

图7 陀螺仪与互补滤波

图8中多次施加外部磁场干扰，可以看出只由磁力计解算出的偏航角在外部磁场靠近时，其偏航角度发生突变，其误差在150度。但是经过互补滤波后的基本不受影响。

图8 磁力计与互补滤波

图9中对互补滤波和卡尔曼滤波进行了对比，由图可以看出，互补滤波调节速度快，而卡尔曼滤波存在一定的滞后性，且灵敏度不高，这里我们选择互补滤波实现姿态数据的滤波融合。

图9 互补滤波与卡尔曼滤波

3 姿态解算算法

姿态解算涉及到坐标变换，向量运算、毕卡算法、四元数与欧拉角转换和互补滤波等算法，姿态解算的具体实现步骤为：

（1）将滤波后的三轴加速度传感器值归一化，该值为机体坐标系下重力向量测量值；

（2）把地理坐标系的重力向量转换到机体坐标系中。

（3）把地理坐标系转换到机体坐标系的重力向量与机体坐标系测量的重力向量外积（叉积），就获得了两坐标系的误差。

（4）因为陀螺仪会有误差，且四元数更新姿态是用陀螺仪来跟新的，所以陀螺仪的误差是导致机体坐标系产生误差的根本原因。这里用两坐标系误差的PI来补偿陀螺仪使跟新后的机体坐标系更加准确

（5）对四元数进行更新，使用四元数进行微分方程的一阶毕卡算法。

（6）由于误差的引入使得四元数的模不为1，导致四元数不是规范化四元数，所以在使用四元数计算欧拉角时，必须对四元数进行规范化处理

（7）四元数更新完成后即完成了飞行器的姿态解算过程，为方便直观的控制飞行器姿态，将四元数转换成欧拉角。

4 平衡控制及分析

4.1 串级PID控制器

四轴飞行器的平衡控制是通过负反馈来实现的，内环采用的是PD控制，控制的是角速度变化。外环采用的是PID控制，控制角度变化。

4.2 PID参数整定

在实际调试过程中，遵循先内环后外环，先比例后微分最后积分的原则。由于四轴飞行器机架的对称性，横滚角（roll）和俯仰角（pitch）参数一致，航向角的参数基本也与横滚角参数一致。所以串级PID调试主要调试5个参数，即横滚角的内环PD和外环PID参数，实际调试图如下图10所示。

图10 实际调试

在调试某个参数的时候，其它还未调的参数设定为零，根据施加干扰后实际效果来确定参数。内环P小了四轴会往一边倾斜，大了施加干扰四轴会晃动很厉害；内环D小了抑制扰动的效果不明显，大了四轴高频振动很厉害。外环P小了施加干扰四轴会慢慢往一边倾斜,大了四轴会在平衡位置晃动很厉害;外环D小了效果不明显,大了会在平衡位置高频振动;外环I小了四轴的平衡位置不在水平面上,外环I大了四轴会在平衡位置低频振动。在这些参数附近进行试验，直到寻找到比较理想的参数。