1、 开发设计开发设计 2023 年 第 44 卷 第 1 期 自动化与信息工程 33 本文引用格式：唐震宇,张云.多传感器数据融合的四轴飞行器姿态角解算J.自动化与信息工程,2023,44(1):33-38.TANG Zhenyu,ZHANG Yun.Multi-sensor data fusion for attitude angle calculation of four axis aircraftJ.Automation&Information Engineering,2023,44(1):33-38.多传感器数据融合的四轴飞行器姿态角解算 唐震宇 张云 （山东交通学院轨道交通学院，山东
2、济南 250000）摘要：针对四轴飞行器的姿态角解算方法存在周期长、精度低的问题，提出扩展卡尔曼滤波方法。首先，分析陀螺仪、加速度计、Mahony 互补滤波 3 种姿态角的解算方法；然后，对扩展卡尔曼滤波方法进行公式推导，指出通过迭代可改变卡尔曼增益大小，提高解算精度；最后，通过实验得出，与 Mahony 互补滤波相比，扩展卡尔曼滤波更可靠。利用扩展卡尔曼滤波方法进行姿态角解算，可提高四轴飞行器的控制精度和稳定性，满足高精度工作场所的需求，具有一定的实践意义。关键词：扩展卡尔曼滤波；Mahony 互补滤波；四轴飞行器；姿态角解算；数据融合 中图分类号：TP249 文献标志码：A 文章编号：16
3、74-2605(2023)01-0006-06 DOI：10.3969/j.issn.1674-2605.2023.01.006 Multi-sensor Data Fusion for Attitude Angle Calculation of Four Axis Aircraft TANG Zhenyu ZHANG Yun (Shandong Jiaotong University Rail Transit College,Jinan 250000,China)Abstract:Aiming at the problem of long period and low accuracy of
4、 attitude angle calculation method for four axis aircraft,an extended Kalman filter method is proposed.First of all,the three attitude angle calculation methods of gyroscope,accelerometer and Mahony complementary filter are analyzed;Then,the formula of extended Kalman filtering method is deduced,and
5、 it is pointed out that the Kalman gain can be changed by iteration to improve the solution accuracy;Finally,the experiment shows that the extended Kalman filter is more reliable than Mahony complementary filter.The extended Kalman filter method is used to solve the attitude angle,which can improve
6、the control accuracy and stability of the four axis aircraft,and meet the requirements of high-precision workplaces.It has certain practical significance.Keywords:extended Kalman filter;Mahony complementary filtering;four axis aircraft;attitude angle calculation;data fusion 0 引言 四轴飞行器具有操控灵活方便，执行效率高等
7、特点，广泛应用于生产生活的多个领域，如电网巡检、矿山航摄、农业灌溉、森林消防、航拍摄像等1。四轴飞行器主要的设计难点在于平衡控制，其核心为读取姿态角。传统的姿态角解算方法主要有 2 种：1）利用MPU6050 芯片的DMP 库读取姿态角，该方法虽然由硬件完成，可减轻处理器负担，但姿态角的输出频率 仅有 200 Hz2，无法满足复杂环境的工作需求；2）采用 Mahony 互补滤波进行姿态解算3，将通过陀螺仪得到的角速度数据和加速度计得到的重力加速度数据进行融合，得到较为可靠的姿态数据。但互补滤波主要通过 PI 运算进行误差补偿，需要在不同的条件下分别进行参数调整，较为繁琐。在Mahony互补滤波
8、的基础上提出扩展卡尔曼滤波的数据融合方法，即将陀螺仪和加速度计得到的数据进行融合，并随着运算迭代自动调节陀螺仪和加速 34 度计的原始数据在姿态角解算中的占比4-5，从而提高解算后姿态角的精度，以满足飞行器在不同环境中工作的需要。本文利用扩展卡尔曼滤波方法进行姿态角解算。首先，将黄金五条公式于后验估计点展开，并通过雅可比矩阵进行线性化6；然后，将陀螺仪和加速度计得到的原始数据进行融合并不断迭代，得出较为准确的四元数；最后，将四元数转化为姿态角。1 姿态解算算法推导 四轴飞行器通过双环PID 运算进行控制。PID 控制器的输出对应四旋翼4 个电机的占空比，通过不断迭代运算，实时更新占空比，实现四
9、轴飞行器的平稳控制7。PID 控制器的输入为期望姿态角。在姿态角读取过程中，为修正非线性误差以及弥补传感器的设计缺陷，需将加速度计与陀螺仪读取到的数据进行融合，得到准确的当前姿态角。1.1 姿态角的描述 在姿态角解算过程中，需用到欧拉角表示的旋转矩阵、四元数表示的旋转矩阵以及四元数和欧拉角之间的转化公式。1.1.1 欧拉角 欧拉角是用来确定定点转动刚体位置的一组独立参考参量，包括横滚角、俯仰角、偏航角，物理示意图如图1 所示。图 1 欧拉角物理示意图 欧拉角运算过程较复杂，可能出现万向锁锁死的问题，为此引入四元数概念。1.1.2 四元数 四元数在代数上是复数的扩展，类似于复数是实数的扩展8，基
10、本方程为 2221jikijk=-(1)四元数形如a+bi+cj+dk，其中a、b、c、d 均为实数，且a2+b2+c2+d2=1。通过四元数进行欧拉角求解，可以减少芯片运算负担，提高运算速度。1.1.3 欧拉角表示的旋转矩阵 欧拉角的物理含义为机体坐标系从一个空间状态转移到另一个空间状态所需的数据运算9，其矩阵表达形式为coscoscossinsinsincoscossincossinsinsincossinsinsincoscossinsincoscossinsincossincoscosgb-+=+-R (2)式中：gbR为从机体坐标系 b 到全局坐标系 g 的变化，为横滚角，为俯仰角，
11、为偏航角。1.1.4 四元数的获取 四元数的计算公式为 00112233001020 xyzxzyyzxzyxqqqqqqqq-=-?(3)式中：x、y、z分别为陀螺仪得到的 x、y、z轴角速度，qx为四元数。1.1.5 四元数表示的旋转矩阵 四元数表示的旋转矩阵（公式(4)）与公式(2)具有相同功能，可将复杂的三角运算简化。222312031 30222120313230 1221 302230 11212()2()2()2()12()2()2()2()12()gbqqqqq qq qq qqqq qqqq qq qqqq qq qq qqq-+=+-+-+R (4)1.1.6 四元数转化为
12、欧拉角 令公式(2)和公式(4)位于相同行列的矩阵元素对x y z 唐震宇 张云：多传感器数据融合的四轴飞行器姿态角解算 2023 年 第 44 卷 第 1 期 自动化与信息工程 35 应相等，联立方程求解，可得到当前状态下的四元数，将其代入公式(5)，可得到当前状态下的姿态角。0123021301222223120301222322arcsin2()arctan()arctan()q qq qq qq qqqqqq qq qqqqq=-+=-+=-+-(5)1.2 姿态角解算 1.2.1 基于陀螺仪的姿态角解算方法 陀螺仪得到的原始数据为x、y、z轴的角速度x、y、z，姿态角变化率和角速度的
13、关系为 1tansintancos0cossin0sin coscos/cosxyz =-(6)当机体处于水平静止状态时，、为 0，公式(6)可简化为 100010001xyz =(7)由公式(7)可以看出：当机体处于水平静止状态时，姿态角变化率与角速度之间为简单的积分关系；当机体处于非水平静止状态时，姿态角变化率与角速度之间为非线性关系。1.2.2 基于加速度计的姿态角解算方法 基于加速度计的姿态角解算方法的思路为：将地球坐标系的重力加速度分解到机体坐标系，即通过对公式(2)转置，得到从地球坐标系转化到机体坐标系的转化矩阵bgR，并通过重力方向矩阵运算，得到x、y、z 轴上的重力加速度，如公
14、式(8)所示。0sin0cos sincoscosxbgyzggggaaRa-=(8)式中：ax、ay、az为加速度计得到的x、y、z 轴加速度，bgR为从全局坐标系g到机体坐标系b的变化。以求解某一时刻的姿态角 为例，将加速度计得到的原始数据代入公式(8)，进一步转化为公式(9)，通过求解反三角函数计算俯仰角。arcsinxag=-(9)需注意，飞行器机头的指向无论如何改变，都不能改变重力在 x、y、z 轴上的分量，因此无法通过此方法求解偏航角。1.2.3 基于Mahony 互补滤波的姿态角解算方法 若仅使用陀螺仪得到的角速度求解姿态角，需通过积分的方式21ttdt=。由于陀螺仪自身存在着误
15、差，且该误差在积分过程中会累计，并随着时间的延长被放大。因此，将加速度计和陀螺仪得到的数据进行融合，可减小非线性误差。通过加速度计修正累计误差。误差判断图如图 2所示，其中 g 为加速度计得到的实际重力加速度，v为四元数推导的重力加速度。令向量g 和v 进行叉乘运算，得到向量 的模值，可反映当前推导的重力加速度与实际重力加速度的误差大小。图 2 误差判断图 向量 v 可通过转化矩阵bgR推导，如公式(10)g v 36 所示。1302230122122()2(0)12()01bgq qq qq qq qqq -=-vR (10)向量 g 可通过公式(8)求得，则误差 ei的计算公式为 ie=v
16、g (11)误差ei可反映陀螺仪数据的可靠程度。若误差ei为 0，说明陀螺仪得到的数据与真实值相同，可靠性较高；若误差ei较大，说明陀螺仪得到的数据可靠性较低。因此，为权衡陀螺仪与加速度计在姿态角解算中的权重，采用误差积分的方式，如公式(12)所示。()(1)()iie ke ke kt=-+(12)式中：ei(k)为当前累计的误差值，e(k)为当前误差值。通过公式(12)计算误差ei，并通过PI控制器补偿，可得到较为精确的角速度：PIxKEEK=+(13)式中：x为当前解算出的角速度，PK为比例项参数，IK为积分项参数，E为当前的角速度与期望角速度间的误差。1.2.4 基于扩展卡尔曼滤波的姿态角解算方法 在姿态角解算的过程中，根据陀螺仪得到的估计值、加速度计得到的测量值与真实值的误差并非一成不变的，会因外界因素及器件自身的影响在一定范围内波动10。利用 Mahony 互补滤波进行数据融合时，这2 个数据的占比无法改变，而采用扩展卡尔曼滤波方法，这2 个数据的占比可随着系统的迭代更新随时改变，从而提高姿态角的解算精度。1.2.4.1 解算模型分析 数据融合过程建立的系统是非线性系统，而
