多传感器信息融合的四旋翼姿态解算研究*

，，

(1.郑州自来水投资控股有限公司，郑州 450000；2.中原工学院)

引 言

四旋翼无人机相对于固定翼和直升机来说，体积小、重量轻、使用方便，集合了各种飞行器的优点，已经广泛应用于各行各业中。而在四旋翼的飞行过程中，核心问题就是姿态的解算[1]。一般常用的单个传感器检测元件(如陀螺仪、加速度计和电子罗盘)，虽然性能稳定，价格适中，但是又有各自的缺点。如传感器信息融合对来自加速度计、陀螺仪以及单个加速度计在四旋翼无人机机体抖动的时候容易带来高斯白噪声，单个陀螺仪存在温度漂移问题，单个电子罗盘受外部磁场干扰也会造成测量误差。本文基于互补滤波算法，采用多个电子罗盘的数据信号进行综合，不仅降低了姿态解算的运算量，而且提高了效率和精度[2]。同时，加入模糊PID 控制方法对四旋翼进行控制，使无人机的飞行更加稳定可靠。

1 姿态角的表述方式推导过程

四旋翼无人机的姿态解算是指将机体上的惯性单元的输出信号实时转换为机体姿态信号,即把机体坐标系(b系，b表示Body)转换为地面惯性坐标系(e系，e表示Earth)的角位置。四旋翼无人机的姿态角包括横滚角(roll)、俯仰角(pitch)、偏航角(yaw)。地面惯性坐标系用于确定机体的空间位置坐标，将地球表面假设成平面，首先在地面上任选一点，作为四旋翼起飞位置。然后对三轴进行定义：其中xe轴在水平面内指向某一方向，ze轴垂直于地面向下，按右手定则确定ye轴。对于机体坐标系，其原点o取在四旋翼的重心上，其中xb轴在四旋翼对称平面内指向四旋翼机体头部方向，zb轴垂直xb轴向下，同样按右手定则确定yb轴。方向确定以后，机体坐标系和地面惯性坐标系的关系可以在一个图示上说明。

图1 两个坐标系的关系图

(1)

已知旋转矩阵，求欧拉角的表达式为：

(2)

得出欧拉角函数：

(3)

而四元数与欧拉角的关系转换为：

⊗qy(θ)qx(φ)

(5)

(6)

则姿态角表示为：

基础埋深确定后，再确定零层板标高。根据上部泵房部分初选钢柱柱脚尺寸，柱脚高度大约为450mm，按照建筑专业要求，柱脚不宜露出地坪，因此，初步确定零层板标高为-0.500m。

(7)

2 四旋翼无人机姿态角测量和解算

2.1 多传感器信息融合的姿态测量

采用基于加速度计、电子罗盘、陀螺仪的捷联式惯性测量系统，属于多传感器信息融合的智能测量系统。传感器固定在四旋翼无人机上，传感器的坐标轴和机体坐标轴保持一致。对三轴电子罗盘Xb、Yb的输出值之比进行反正切函数处理可得到偏航角，三轴陀螺仪的三轴Xb、Yb、Zb的输出值分别为ωx、ωy、ωz，即翻滚角速度、俯仰角速度和偏航角速度，如表1所列。

表1 多传感器姿态测量描述

加速度计的缺点很明显，动态响应慢，受机体振动的影响大,而电子罗盘受外部磁场干扰的影响大，所以这两种传感器测得的翻滚角φ、俯仰角θ、偏航角ψ都存在一定误差，对姿态角度要求精确的四旋翼无人机来说，不能满足有效的实时控制要求[8]。对于陀螺仪，虽然通过对测量出来的角速度进行积分可以来获取三轴具有良好动态特性的姿态角，然而陀螺仪的温度漂移会使误差随着时间的增加而累加变大，积分后会得到更大的姿态角误差，数据甚至无法使用[6-7]。

结合三种单个传感器的优缺点，科学合理地进行姿态解算，采用加速度计、电子罗盘用于低频段的姿态测量，陀螺仪用于高频段的姿态测量，测量的整体误差将大大减小。融合各种传感器的数据信息将有助于综合各种特征值，得出更准确、有效的数据。

2.2 基于互补滤波的四旋翼无人机姿态解算

根据上述各传感器的优缺点，对电子罗盘、陀螺仪、加速度计三种传感器所测得的数据进行综合、校正。线加速度的测量由电子罗盘和加速度计完成，角加速度由陀螺仪完成[8]。陀螺仪经过积分得到姿态角，测量误差会随时间的积累而增加。而加速度计测量，虽然动态响应速度比较慢，但在一段时间内，测量误差几乎不变。鉴于三种传感器在频率上的互补关系，采用互补滤波算法作为基本的数据处理方法，把测得的姿态数据进行融合，提高了姿态数据的动态响应和精度。

互补滤波结构框图如图2所示。

图2 互补滤波结构框图

图中低通滤波器为dt/(τ+dt)，高通滤波器为τ/(τ+dt)，其中参数τ为时间常数，1/τ为滤波器的截止频率，dt为滤波器采样时间。对于低通滤波器，频率高于1/τ将被滤除，低于1/τ的能顺利通过，高通滤波器则相反。加速度计通过低通滤波器能限制运动加速度对于姿态测量的高频抖动；高通滤波器的加入，可以有效抑制陀螺仪长时间内积累的漂移误差，提高陀螺仪的测量精度。由图2可得：

(8)

式中: φa为电子罗盘和加速度计的姿态角，φg为陀螺仪测量姿态角，φ为互补滤波处理后的姿态角。

又因为:

(9)

所以有：

(10)

由式(10)分析可得在姿态解算中，以截止频率为界限值，在高于此界限值的高频段，陀螺仪起主要作用，低于界限值的低频段，电子罗盘和加速度计占决定作用。对电子罗盘、加速度计和陀螺仪权重的调整，可通过调整时间常数τ改变截止频率。

2.3 PID控制的改进型互补滤波

互补滤波相对于其他滤波方法，滤波效果比较明显，能高效地实现姿态数据的融合，同时消除了高频和低频的干扰问题。但是由于在低通阻带，互补滤波的衰减速度较慢且干扰噪声较大时，误差相对也较大，滤波效果并不理想，只适用于噪声比较小的情况。为了改变这种现象，在互补滤波基础上增加PID 控制，构成改进型互补滤波器，图3 为横滚角互补滤波结构框图[9-12]。

图3 改进型互补滤波结构框图

又因

x1=(φa-φ)KpKi

(11)

(12)

x2=y1+2×(φa-φ)Kp+φg

(13)

(14)

式(11)～(14)中，Kp、Ki为比例系数，x1、x2、y1为中间变量，x2等于修正后的姿态值加上陀螺仪积分的姿态值，整个系统框图属于负反馈结构。

3 实验结果分析

通过C语言进行编程，模仿出PWM波，电机提供动力来使飞行器起飞。重复多次测试后，准确找出飞行器起飞所对应的PWM值。 MPU60506轴传感器通过串口向控制器STM32发送数据，控制器接收并检测接收的数据是否正确，通过软件编程进一步对得到的数据进行修正，使四旋翼无人机能够实现稳定的起飞、悬停、前进、后退以及降落等功能。

图4 程序流程图

主要实现思路为：STM32单片机上电，实现初始化。利用单片机定时器设定电机PWM周期，实现串口初始化。在初始化结束后，设定目标姿态，实时检测AHRS是否接收到有效的姿态数据，从而利用姿态控制函数进行控制。结合模糊PID融合控制算法，利用当前MPU6050检测到的实时姿态数据解算出不同姿态，从而调整输出量，如此循环往复，不断地接收姿态数据、解算数据、调整量输出，才能控制四旋翼飞行器各个角度的飞行姿态，使四轴飞行器稳定、正确地飞行，程序运行结束[13-15]。图4为本设计的软件流程图。

根据本系统中所述的基于多传感器信息融合的改进型互补滤波的四旋翼自主飞行器，采用模糊PID控制规则，选取仿真控制对象：四旋翼飞行器的偏航角[14-15]，进行了大量的软件仿真实验。

图5所示为实际角度和互补滤波角度对比图，采用改进型互补滤波算法得到的解算角度和实际角度相比，偏航角的最大误差约为±1.2°，动态误差小于±4°。

图5 实际角度和互补滤波角度对比图

假设输入条件为阶跃信号，阶跃响应的仿真结果如图6所示。

图6 互补滤波的模糊PID控制阶跃响应结果

由仿真结果可知，基于多传感器信息融合的互补滤波算法，系统响应速度快、超调量小，控制精度高、抗干扰能力强，仿真控制效果较好，因此，其构成的模糊PID控制器能更好地实现对四旋翼飞行器的控制。

结 语

本文基于多传感器信息融合的改进型互补滤波的四旋翼自主飞行器，利用四元数法进行姿态解算，并通过互补滤波算法进行数据融合，利用电子罗盘、陀螺仪、加速度计在姿态测量中的优点，为四旋翼完成各种飞行任务提供保证。