多传感器数据融合的四轴飞行器姿态角解算_唐震宇.pdf

开发设计开发设计 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.多传感器数据融合的四轴飞行器姿态角解算 唐震宇 张云 （山东交通学院轨道交通学院，山东 济南 250000）摘要：针对四轴飞行器的姿态角解算方法存在周期长、精度低的问题，提出扩展卡尔曼滤波方法。首先，分析陀螺仪、加速度计、Mahony 互补滤波 3 种姿态角的解算方法；然后，对扩展卡尔曼滤波方法进行公式推导，指出通过迭代可改变卡尔曼增益大小，提高解算精度；最后，通过实验得出，与 Mahony 互补滤波相比，扩展卡尔曼滤波更可靠。利用扩展卡尔曼滤波方法进行姿态角解算，可提高四轴飞行器的控制精度和稳定性，满足高精度工作场所的需求，具有一定的实践意义。关键词：扩展卡尔曼滤波；Mahony 互补滤波；四轴飞行器；姿态角解算；数据融合 中图分类号：TP249 文献标志码：A 文章编号：1674-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 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 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 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 引言 四轴飞行器具有操控灵活方便，执行效率高等特点，广泛应用于生产生活的多个领域，如电网巡检、矿山航摄、农业灌溉、森林消防、航拍摄像等1。四轴飞行器主要的设计难点在于平衡控制，其核心为读取姿态角。传统的姿态角解算方法主要有 2 种：1）利用MPU6050 芯片的DMP 库读取姿态角，该方法虽然由硬件完成，可减轻处理器负担，但姿态角的输出频率 仅有 200 Hz2，无法满足复杂环境的工作需求；2）采用 Mahony 互补滤波进行姿态解算3，将通过陀螺仪得到的角速度数据和加速度计得到的重力加速度数据进行融合，得到较为可靠的姿态数据。但互补滤波主要通过 PI 运算进行误差补偿，需要在不同的条件下分别进行参数调整，较为繁琐。在Mahony互补滤波的基础上提出扩展卡尔曼滤波的数据融合方法，即将陀螺仪和加速度计得到的数据进行融合，并随着运算迭代自动调节陀螺仪和加速 34 度计的原始数据在姿态角解算中的占比4-5，从而提高解算后姿态角的精度，以满足飞行器在不同环境中工作的需要。本文利用扩展卡尔曼滤波方法进行姿态角解算。首先，将黄金五条公式于后验估计点展开，并通过雅可比矩阵进行线性化6；然后，将陀螺仪和加速度计得到的原始数据进行融合并不断迭代，得出较为准确的四元数；最后，将四元数转化为姿态角。1 姿态解算算法推导 四轴飞行器通过双环PID 运算进行控制。PID 控制器的输出对应四旋翼4 个电机的占空比，通过不断迭代运算，实时更新占空比，实现四轴飞行器的平稳控制7。PID 控制器的输入为期望姿态角。在姿态角读取过程中，为修正非线性误差以及弥补传感器的设计缺陷，需将加速度计与陀螺仪读取到的数据进行融合，得到准确的当前姿态角。1.1 姿态角的描述 在姿态角解算过程中，需用到欧拉角表示的旋转矩阵、四元数表示的旋转矩阵以及四元数和欧拉角之间的转化公式。1.1.1 欧拉角 欧拉角是用来确定定点转动刚体位置的一组独立参考参量，包括横滚角、俯仰角、偏航角，物理示意图如图1 所示。图 1 欧拉角物理示意图 欧拉角运算过程较复杂，可能出现万向锁锁死的问题，为此引入四元数概念。1.1.2 四元数 四元数在代数上是复数的扩展，类似于复数是实数的扩展8，基本方程为 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 的变化，为横滚角，为俯仰角，为偏航角。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 四元数转化为欧拉角 令公式(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，姿态角变化率和角速度的关系为 1tansintancos0cossin0sin coscos/cosxyz =-(6)当机体处于水平静止状态时，、为 0，公式(6)可简化为 100010001xyz =(7)由公式(7)可以看出：当机体处于水平静止状态时，姿态角变化率与角速度之间为简单的积分关系；当机体处于非水平静止状态时，姿态角变化率与角速度之间为非线性关系。1.2.2 基于加速度计的姿态角解算方法 基于加速度计的姿态角解算方法的思路为：将地球坐标系的重力加速度分解到机体坐标系，即通过对公式(2)转置，得到从地球坐标系转化到机体坐标系的转化矩阵bgR，并通过重力方向矩阵运算，得到x、y、z 轴上的重力加速度，如公式(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=。由于陀螺仪自身存在着误差，且该误差在积分过程中会累计，并随着时间的延长被放大。因此，将加速度计和陀螺仪得到的数据进行融合，可减小非线性误差。通过加速度计修正累计误差。误差判断图如图 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=vg (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 解算模型分析 数据融合过程建立的系统是非线性系统，而卡尔曼滤波主要应用于线性系统，因此需要通过雅可比矩阵、泰勒公式将系统线性化，从而得到扩展卡尔曼滤波，其对应的黄金五条公式也随之变化为 11(,0)kkkxf xu-=(14)T1kkk-=+PA PAQ (15)TT1()kkkkkkKR-=+PHH P H (16)()kkkkkxxKzh x-=+-(17)()kkkkIK-=-PHP (18)式中：kx-为先验估计，kx为后验估计，1kx-为上次运算周期中的后验估计，1ku-为外部输入，k-P为先验估计协方差矩阵，1k-P为上次运算周期中的后验估计协方差矩阵，Ak为状态转移矩阵，zk 为实际测量值，Kk为卡尔曼增益，()kh x-为当前估计值，kH为雅可比矩阵，Q 为系统的噪声协方差矩阵。666610000010000010000010-=Q (19)1.2.4.2 扩展卡尔曼滤波的应用 在数学模型的推导过程中，由于采用了近似估计方法，因而其模型中含有过程噪声。在数据测量的过程中，受外界不可控因素的影响，导致测量结果与真实值之间存在偏差，因而测量模型含有测量噪声11。上述两种噪声在实际工程中无法测得。因此，扩展卡尔曼滤波的主要思路为：在一个不准确的数学模型得到的估计值（陀螺仪计算的四元数），以及一个不准确的测量结果（加速度计测得的重力加速度）的基础上，通过扩展卡尔曼滤波方法对姿态角的最优值进行估计。以俯仰角为例，扩展卡尔曼滤波姿态估计主要分为下面5 个步骤。1）计算先验估计值kx-读取陀螺仪的数据，并代入公式(3)，可求当前的先验四元数值。唐震宇 张云：多传感器数据融合的四轴飞行器姿态角解算 2023 年 第 44 卷 第 1 期 自动化与信息工程 37(1)kkkxx-=A (20)式中：1kx-为上一次计算得到的四元数矩阵的后验估计值，Ak为状态转移矩阵，计算公式为 12kIT=+A (21)2）计算先验估计的协方差矩阵k-P 将状态转移矩阵 Ak以及系统噪声协方差矩阵 Q代入公式(15)，可得到先验估计的协方差矩阵k-P。3）计算卡尔曼增益Kk 在机体坐标系下加速度计的表示方式如公式(22)所示。1302101232222012322()20021bkgq qq qh qq qq qqqqq-=-+R (22)因为加速度计是在地球坐标系下读取重力加速度的，所以需要利用转移矩阵bgR将重力加速度从地球坐标系转移到机体坐标系。bgR为公式(4)的转置。由公式(22)可以看出，加速度计由 3 个函数和 4 个未知数组成，其对应的雅可比矩阵为 111111110123121212120123131313130123khhhhqqqqhhhhqqqqhhhhqqqq=H 230110320123222222222222qqqqqqqqqqqq-(23)将以上求得的结果代入公式(16)，便可求得当前的卡尔曼增益Kk。4）计算后验估计kx（最终输出值）将四元数的先验估计kx-、卡尔曼增益 Kk、陀螺仪得到的角速度旋转矩阵、在机体坐标系下加速度计的表达式()kh x-、当前加速度计的测量值zk代入公式(17)，可得到当前四元数的后验估计值。5）更新协方差矩阵kP 将卡尔曼增益Kk、雅可比矩阵kH代入公式(18)，得到协方差矩阵的后验估计kP，将其重新进入第一步进行迭代。通过以上5 步多次迭代，可得到更加精确的四元数。将该四元数代入公式(5)，即可解算当前姿态角。2 实验验证 本实验利用 STM32 单片机读取 MPU6050 的原始数据，并通过串口将当前陀螺仪的角速度数据与加速度计的加速度数据实时发送至上位机。首先，利用MATLAB 将基于陀螺仪、加速度计、Mahony 互补滤波、扩展卡尔曼滤波的 4 种姿态角解算方法以程序的形式实现；然后，将原始数据输入到上述4 种姿态角的解算方法；最后，通过曲线图观察各解算方法的优劣。2.1 传统的解算方法 在对 MPU6050 施加外部抖动的情况下，通过Mahony 互补滤波、陀螺仪、加速度计 3 种解算方法得到的俯仰角数据图谱如图3 所示。图 3 Mahony 互补滤波、陀螺仪、加速度计仿真结果对比 时间/s 俯仰角/deg 加速度计估计 时间/s 时间/s 俯仰角/deg 俯仰角/deg 互补滤波估计 陀螺仪积分 38 由图3 可知：通过Mahony 互补滤波方法得到的数据较为准确，能够实时显示姿态角的变化；仅通过陀螺仪解算得到的姿态角，在初始时刻较为准确，但随着时间的推移，其误差不断积分，显现明显的漂移现象；仅通过加速度计解算得到的姿态角因外部磁场的影响，噪声明显、数据可靠性差。2.2 扩展卡尔曼滤波解算方法 在对 MPU6050 施加外部抖动的前提下，通过加速度计、陀螺仪、Mahony 互补滤波、扩展卡尔曼滤波 4 种解算方法得到的俯仰角数据图谱如图 4 所示。图 4 加速度计、陀螺仪、互补滤波、扩展卡尔曼滤波 仿真结果对比 由图4 可以看出，通过扩展卡尔曼滤波方法得到的数据相比Mahony 互补滤波，噪声更小，拟合的曲线更平滑，解算效果更好。3 结束语 本文通过公式推导，详细介绍陀螺仪、加速度计、Mahony 互补滤波 3 种基本姿态角的解算方法。在此基础上，对扩展卡尔曼滤波的解算方法进行公式推导。通过实验将加速度计、陀螺仪、Mahony 互补滤波、扩展卡尔曼滤波4种姿态角解算方法的解算结果以图谱的形式进行比对分析，发现各解算方法的变化规律符合理论推导。实验结果表明，通过引入卡尔曼增益，可提高角度解算精度，相比其他解算方法更可靠，可满足高精度工作环境的需求。参考文献 1 张成,赵玫,杨洪勇,等.野外航拍用四轴飞行器的设计J.鲁东大学学报(自然科学版),2018,34(4):378-384.2 李臣龙,强俊.基于 STM32 和 MPU6050 姿态解算的研究与实现J.佳木斯大学学报(自然科学版),2017,35(2):295-298;316.3 陈国定,周鹏豪,胡朕豪,等.基于 MPU6050 的四轴硬件姿态解算研究J.机电工程,2018,35(1):95-100.4 伏家杰,周翟和,尹辉,等.基于四元数二阶互补滤波的四旋翼姿态解算J.机械制造与自动化,2019,48(2):169-172;176.5 李庆党,张义龙.基于卡尔曼滤波和互补滤波的改进型姿态解算方法J.青岛科技大学学报(自然科学版),2019,40(6):85-89;98.6 龙子洋,项鹏,隋国荣.基于扩展卡尔曼滤波的球形机器人姿态解算J.软件工程,2022,25(7):47-50.7 孙春虎,方愿捷,王静,等.一种新型四轴飞行器姿态角控制方法与仿真J.巢湖学院学报,2019,21(6):127-132.8 张捍卫,喻铮铮,雷伟伟.四元数的基本概念与向量旋转的欧拉公式J.大地测量与地球动力学,2020,40(5):502-506.9 程烺,俞家勇,马龙称,等.单位四元数、罗德里格转换模型与欧拉角的映射关系J.北京测绘,2020,34(1):44-50.10 段敏,赵凌,周莹.基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法J.现代信息科技,2022,6(4):7-11.11 温彬彬,温鹏智,胡雪花.EKF 在机器人目标跟踪中的应用J.电脑知识与技术,2021,17(36):116-118.作者简介：唐震宇，男，2000 年生，本科，主要研究方向：自动化。E-mail: 张云（通信作者），1977 年生，男，博士，副教授，主要研究方向：嵌入式系统。E-mail: 时间/s 时间/s 时间/s 俯仰角/deg 俯仰角/deg 俯仰角/deg 时间/s 俯仰角/deg 互补滤波估计 卡尔曼滤波估计 加速度计估计 陀螺仪积分