基于模糊自适应PI的姿态解算算法研究

刘佳 谢富强

摘  要：小型无人机姿态信息获取的实时性与准确性主要依靠于惯性测量单元的实测数据，本文针对惯性测量单元中陀螺仪长时间工作会出现数据漂移的问题，提出了一种使用加速度计数据基于模糊自适应PI控制的互补滤波算法。最后，在由STM32F427VIT6所搭建的飞控系统平台上将该姿态解算算法所得的姿态角与利用四元数和基于Mahony互补滤波算法解算出的姿态角进行比较。研究结果表明：此算法能够准确获取小型无人机的各项姿态信息，且响应速度更优于Mahony互补滤波算法，具有更强的实时性，能够满足小型无人机的控制要求。

关键词：模糊自适应;方向余弦矩阵（DCM）;姿态解算;互补滤波

中图分类号：TP241      文献标识码：A 文章编号：2096-4706（2019）09-0139-03

0  引  言

小型无人机的飞行控制器上均会搭载一块或多块惯性测量单元（IMU），来实时获取更新无人机的姿态数据。将传感器数据转换成无人机相对于地球的方位的过程就被称为姿态解算，而此类方位可以表示成一个旋转。表示旋转的方法目前主要分为两大类：方向余弦法和四元数法。虽然方向余弦法相较于四元数法使用的参数较多，但在实际应用方面，方向余弦矩阵具有使用方便，计算量小，编程容易等特点，且二者的运算精度与误差趋势基本相当。

针对常用的惯性测量单元的姿态解算算法。互补滤波算法也有了比较成熟的研究与应用。互补滤波算法主要利用加速度计数据校准陀螺仪的误差，但是由于加速度计的数据易受外界干扰，当互补滤波算法中的参数固定时，容易使互补滤波出现较大的误差，造成姿态解算结果不理想，实时性差的问题。对于这一问题，国内外进行了大量的研究，出现了利用模糊自适应算法优化互补滤波算法参数的方法，该方法利用模糊自适应优化比例参数，提高了互补滤波解算的精度，但对积分参数的操作仅限于积分分离与积分限幅，解算的响应时间并没有提高。

针对上述问题，本文借助MPU6000惯性测量单元，设计了基于模糊自适应PI的姿态解算算法，根据加速度计与重力加速度参考向量的误差同时优化P、I参数，最终利用方向余弦矩阵求取出小型无人机的姿态角。

1  平台搭建

1.1  整体结构框图

本研究以MPU6000为姿态传感器，将姿态传感器原始数据使用SPI通信发送至主控芯片STM32F427VIT6进行姿态解算得出姿态角。由传感器原始数据转换为姿态角数据的整体控制框图如图1所示。

1.2  惯性测量单元MPU6000简介

惯性测量单元（IMU）是测量物体三轴角速率以及加速度的装置。一般地，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，分别测量物体在三维空间中的角速度和加速度并以此解算出物体的姿态。

MPU6000是9轴运动处理传感器。它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP（Digital Motion Processor）。

MPU6000对陀螺仪和加速度计分别用了三个16位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，其所有设备寄存器之间的通信均采用400kHz的I2C接口或1MHz的SPI接口。

3  姿態解算结果分析

本研究使用两块基于STM32F427VIT6的以MPU6000为姿态传感器的飞控板，将两块飞控板固定于同一平面上，且两块飞控的初始横滚、俯仰角均为0°。当该平面处于静止状态时，使用同一姿态解算算法的二者姿态角误差在0.5°左右。

在此前提下，一块飞控板使用本文所设计的基于模糊自适应PI的姿态解算算法，另一块飞控板使用基于传统Mahony互补滤波的姿态解算算法，并通过MATLAB分别绘制出二者的横滚角与俯仰角数据，比较结果如图3、图4所示。

测试结果表明，随着时间的推移，与基于传统互补滤波的姿态解算算法相比，本文所设计的姿态解算算法响应速度提高了4%～5%，具有更强的实时性。

4  结  论

本文介绍了一种基于模糊自适应PID的姿态解算算法，利用姿态传感器MPU6000的原始数据，根据加速度计与陀螺仪的实测误差，使用模糊自适应PI算法校正陀螺仪数据漂移，最终利用方向余弦矩阵计算出姿态角，并与基于Mahony互补滤波的姿态解算算法计算出的姿态角进行对比。

结果表明，本文所设计的姿态解算算法具有更快的响应速度，实时性更强。经过实际的飞行测试，该算法能够满足一般小型无人机的飞行控制要求。

参考文献：

[1] 尹剑，陈红，杨萌，等.捷联姿态计算中方向余弦与四元数法分析比较 [J].四川兵工学报，2015，36（9）：106-110.

[2] 陈国定，周鹏豪，胡朕豪，等.基于MPU6050的四轴硬件姿态解算研究 [J].机电工程，2018，35（1）：95-100.

[3] 周湛杰，王新生，王岩.基于模糊自适应算法的航天器姿态控制 [J].电机与控制学报，2019，23（2）：123-128.

[4] 王焱春，张新英，梁芬.多传感器信息融合的四旋翼姿态解算研究 [J].单片机与嵌入式系统应用，2018，18（8）：39-42.

作者简介：刘佳（1994-），男，汉族，湖南岳阳人，本科，硕士，研究方向：智能控制与信息处理;通讯作者：谢富强（1971-），男，汉族，湖南衡山人，副教授，博士，研究方向：智能控制理论及应用。