基于四元数的四旋翼飞行器姿态解算研究

　　摘 要： 四旋翼飞行器的飞行控制效果取决于姿态信息获取的准确性，所以姿态解算是飞行器研究的关键技术。针对目前飞行器高动态特性和复杂环境下的姿态解算误差较大的问题，提出一种基于四元数的二阶互补滤波算法，通过增加一个PI反馈控制环节，能更好地利用多传感器进行姿态融合，使经典互补滤波算法中的低通阻带衰减速度过慢、陀螺仪漂移等问题得以改善。同时文中自主设计了IMU系统，完成数据采集和姿态解算算法的实现，分别进行三轴转台实验，改进后姿态解算的结果表明，无论是在静态还是动态环境下，俯仰、横滚角的误差小于0.5°，验证了该算法的可行性。

　　关键词： 姿态解算; 四旋翼飞行器; 四元数; 互补滤波; 姿态融合; IMU系统设计

　　0 引 言

　　四旋翼飞行器集合了不同飞行器的优点，凭借其结构简单、使用方便、成本低、便于操作等特点，已经被广泛应用在各行各业。如今针对四旋翼飞行器的研究不止限于熟知的军事、工业领域，更多的被应用到航拍和物流行业中，产生了巨大的商业价值，因此众多的科研院校和商业国内公司都争相投入到四旋翼的研发生产当中。

　　而在研究当中关键项目之一是四旋翼飞行器的姿态解算研究[1]。因为四旋翼飞行器在飞行的过程中，不仅会受到自身欠驱动系统的影响，也易受到噪声、空气阻力等复杂环境的干扰，导致姿态信息的获取增加了难度，而姿态信息获取的精准性是相应飞行器的独立飞行最基本的前提，所以姿态解算研究的重要性不言而喻。

　　在姿态解算中，卡尔曼滤波器的应用较为广泛，文献[2]通过卡尔曼滤波(Kalman Filtering) 融合陀螺仪和加速度计数据，递推计算出最优估计值来修正输出误差。但是飞行器系统要满足非线性系统和高斯白噪声的前提。因此扩展卡尔曼(EKF)提出，应用在非线性系统中[3]。但是EKF算法计算量大，在线性化假设无法成立的情况下，不满足四旋翼飞行器高机动、实时性的要求。而互补滤波算法简单有效，易实现传感器测量精度。四元数的旋转特性解决了飛行器姿态解算中的奇异性问题。

　　本文自主设计了以嵌入式MEMS控制器和惯性测量单元(Inertial Measurement Unit，IMU)组合的系统平台，采用四元数方法描述系统模型，提出了一种基于四元数的改进型互补滤波算法，致力于降低计算的复杂性和难度，提高滤波的效果。通过三轴转台实验和实物飞行测试，验证本文提出的算法可以适用于四旋翼飞行器系统，能提高姿态解算的效率和精度。

　　1 四元数法的姿态解算

　　姿态被用来描述载体坐标系和参考(地理)坐标系之间的角位置关系，四旋翼飞行器姿态解算则是飞行器的载体坐标系相对地理坐标系的姿态矩阵问题求解。

　　1.1 坐标系定义

　　根据参考坐标系，才能确定物体的速度、姿态、位置等信息。在惯性导航中，常用的有5种坐标系，分别为地球、惯性、载体、地理、导航坐标系[4]。参考坐标系如图1所示。

　　地球坐标系([O?xeyeze])：将地球中心作为坐标系的原点，坐标轴始终与地球维持一致。其中，[xe]和[ye]沿地球的赤道平面和格林尼治子午面的交线[5]，[ze]的方向平行于地球的极轴方向，并以地球自转角速度相对于地心惯性坐标系旋转。

　　导航坐标系([O?xnynzn])：即当地的地理坐标系，坐标的原点位于P点，坐标轴指向北、东和当地垂线方向(向下)。通常把导航坐标系相对于地球固连坐标系的旋转角速率称为转移速率，速率的大小取决于P点相对于地球运动的快慢。

　　载体坐标系([O?xbybzb])：不同于导航坐标系的“东北天”，机体坐标系取载体的“前左上”，描述其姿态的角度分别为横滚(Roll)角γ、俯仰(Pitch)轴θ和航向(Heading)角ψ。

　　1.2 四元数姿态表示方法

　　四元数由实数和复数组合构成：即一个实数单位和三个虚数单位组成的四维向量，可以用来表示刚体和坐标系在三维立体空间里的姿态方向。表达式如下：

　　式中：[q0]是标量部分的四元数系数，其大小等于坐标系旋轉角度一半的余弦值，是四元数的标量部分的系数;[q1]～[q3]表示矢量的部分;[θ] 是矢量在动坐标系上转动的角度值[6]。

　　通过归一化的四元数可以计算出坐标轴旋转矩阵：

　　得旋转矩阵为：

　　欧拉角由四元数方法表达为：

　　式中，四元数方法描述转动、估计姿态都优于其他方法，因为始终可以避免奇异性，且仅需要4个参数进行计算，计算效率得以提升。载体系相对于地理系的角度求解，可以被看作是由地理系只经过一次旋转形成载体系，四元数方法可以支持这种等效计算。

　　2 姿态算法实现

　　2.1 互补滤波

　　姿态解算需要采用加速度计、陀螺仪采集的数据进行计算，陀螺仪的测量值是载体坐标系的角加速度，加速度计的测量值则是其线加速度。各自都有缺点，特别是在飞行器载体因干扰产生抖动的时候，来自陀螺仪、加速度计会更容易产生高斯白噪声;单个陀螺仪漂移误差与工作时间产生正比的递增，表现形式为误差随时间增长而累积，但是在较短的时间内可获取较为精确的数据;加速度计与其相反，静态特性好，但是短时间内的精度表现较差，无法满足飞行器易抖动的特性。

　　总结为两者在频率上呈互补关系，互补滤波则是将加速度计和陀螺仪分别在不同频率上的优势融合。即利用加速度计测得短时间的瞬时值来补偿陀螺仪长时间积累出现的漂移误差[7]。其原理图如图2所示。

　　设[θ]为姿态角，融合后的姿态角估计值为：

　　式中：[ωg] 是陀螺仪测量的角速度值;[θα]则是角度值，来自加速度计测量;高通滤波器为;低通滤波器为。高通滤波器的作用是减小低频下陀螺仪的漂移误差，低通滤波器则是限制高频下的加速度计噪声[8]。式(5)可经过反拉普拉斯变换得：

　　观察式(6)，互补滤波是通过调整[kP]值改变滤波器的截止频率，达到对陀螺仪、加速度计所占比重的处理。

　　2.2 互补滤波改进设计

　　经过分析，第2.1节介绍的一阶互补滤波可以在某个阶段抑制陀螺仪积分产生漂移误差、降低高频噪声。但是文献[9?11]等人对经典互补滤波进行了研究，得到以下结论：由于其低通阻带衰减缓慢，四旋翼飞行器具有高动态性，固定的调节参数设置无法在所有的情况下得到最优的估计值。所以一阶互补滤波器不能随时有效消除系统误差。

　　因此在互补滤波原理的基础上，针对传统互补滤波应用在姿态解算上的不足，结合误差来源，提出在互补滤波基础上增加一个PI(比例积分)反馈控制环节，完成互补滤波的改进。理论上可以加快低通阻带的衰减速度，可以在不同的情况下，动态地调整补偿的系数：如果处于匀速状态，增大加速度计的所占权重;高动态时，减小加速度及占的比重，增大陀螺仪比重。

　　整个流程即是先将加速度计解算出的角度与最终角度产生的误差实现PI控制，促进误差的修正。修正后和陀螺仪解算出的姿态角相加后再进行积分得到一个新姿态角。改进后的互补滤波算法如图3所示。

　　算法的实现过程如下：

　　1) 首先对加速度计测量的三维向量进行规范化：

　　对载体坐标进行转换到地理坐标系后，可得到载体坐标系下重力加速度的三维向量：

　　2) 加速度计的测量值向量[A]和陀螺仪积分后的姿态估计向量[A]之间的误差向量利用向量间的叉积表示：

　　改进增加的比例积分环节可以求解得出陀螺仪的漂移补偿向量：

　　如上所述可修正误差。

　　3) 通过一阶龙格库塔法求解四元数微分方程，求解出的四元数的更新方程为：

　　4) 将漂移误差向量代入到式(9)中，得到角速度微分四元数。

　　5) 对更新后的四元数积分规范化处理，由式(4)求解得出姿态角估计值。

　　推荐阅读：科技核心投稿论文要求有基金吗

期刊VIP网，您身边的高端学术顾问

文章名称： 基于四元数的四旋翼飞行器姿态解算研究

文章地址： http://www.qikanvip.com/shengwukexue/52687.html