四旋翼姿态解算算法研究

院老虎，连冬杉，卢联杰，刘 义

(1.沈阳航空航天大学 航空宇航学院，沈阳 110136;2.湖北三江航天万峰科技发展有限公司 测控技术研究室,湖北 孝感 432000;3.北京机械工业自动化研究所有限公司 工业云平台事业部,北京 100120)

近年来，随着无人机的技术更新和需求增长，预示着无人机将得到快速发展。四旋翼飞行器是四浆非共轴多旋翼飞行器的简称。作为非共轴多旋翼飞行器的代表，与固定翼飞行器相比，它具有结构简单、体积小、隐蔽性强、载荷高、可垂直起降、定点悬停和低速飞行等优势[1,2]，在民用和军事领域均体现出极高的研究和应用价值。四旋翼飞行器是无人机飞行器领域中的一个热点研究课题[3,4]。

四旋翼飞行器虽然结构简单，但它是一个多变量、强耦合的非线性欠驱动系统[5,6]，机体对外界和自身的干扰表现的十分敏感。因此，获得精确地姿态角数据显得极为重要。姿态角解算精确程度直接影响着四旋翼飞行器在飞行过程中的灵活性、稳定性、抗干扰性，所以对姿态解算算法的研究具有重大意义。

目前，欧拉角、方向余弦矩阵和四元素算法在姿态解算中得到了较多的关注与应用[7-9]。欧拉角矩阵表示最为直观，但存在奇点，不能实现对全部姿态的表示。方向余弦计算不存在奇点，然而矩阵有9个元素，解算时实际需要计算9个联合微分方程，运算量很大，不适合实时运算。四元素法是一种全局的姿态描述方法，没有奇点，且运算为一般代数运算，速度快，易于操作。

本文针对四旋翼飞行器惯导系统中的姿态解算问题，分别利用四元素、方向余弦矩阵和欧拉角算法对姿态角进行姿态解算。把加速度计与陀螺仪当成观测量，利用叉积来修正陀螺仪角速度后，实时更新姿态，并对3种姿态解算的俯仰角在静态与动态进行研究对比和最优化设计。

1 姿态计算两点前提假设

假设一：由于惯性系与机体系均为直角坐标系，各轴相互正交，把坐标系理解成刚体，考虑姿态角的变化，可以只研究坐标系角度的变换关系。把其中一个坐标系作平移，使其原点与另一个坐标系原点重合。因此，可以将两坐标系间的空间角位置关系理解成刚体的定点转动。不管是用欧拉角、方向余弦矩阵，还是用四元素表示姿态都是基于这一假设出发。

假设二：四旋翼多处于悬停、匀速飞行状态，且在四轴起飞、降落等加减速阶段，其加速度相对重力加速度可以忽略不计。因此可以假设三轴加速度计仅仅测量重力加速度，其输出为重力加速度在3个机体轴上的分量。在地球系上，重力加速度计的方向与大小是固定的。可以将重力加速度(地球坐标系)投影到机体坐标系，得到加速度计与水平面的角度关系。当绕着重力轴旋转三轴加速度计，其ADC值将不会改变，即加速度无法感知水平旋转，只能观测俯仰角与滚转角，无法观测偏航角。

2 四元素姿态解算

2.1 基于IMU的四元素姿态解算

基于IMU的四元素姿态解算如图1所示。它利用机体坐标系的加速度与上一时刻估算出的加速度估计值进行叉积，得到方向误差估计，通过低通滤波器去除加速度计的高频信号，与机体角速度进行互补后有效抑制了陀螺漂移。

图1 基于IMU的四元素姿态解算流程图

步骤一：四元素初始化

将[φ,θ,ψ]T=[0,0,0]T带入[q0,q1,q2,q3]T=[1,0,0,0]T作为四元素的初始值。

步骤二：加速度计修正角速度

记反馈方向误差量为ea，根据叉积的性质：A×B=-B×A可得

(1)

通过PI控制器将ea与角速度融合来修正角速度

(2)

步骤三：四元素更新

根据一阶龙格库塔方程更新四元素的值(Δt为采样周期)

(3)

步骤四：四元素规范化

(4)

步骤五：输出姿态角

被输出的三个姿态角如下

(5)

至此，整个IMU解算完成。

图2展示了解算后的俯仰角与加速度计、陀螺仪的对应关系。图3为融合的滚转角φ0与单独用加速度计值解算的滚转角φ1比较，静态下φ0、φ1在小角度下是紧密贴合的，一旦角度增大，加速度计的精度就会变差，在2000采样点附近甚至会出现类似脉冲等毛刺现象。而φ1在整个采样过程中则表现地非常平滑。图4与图5是使用单独的加计与四元素法解算出的俯仰角与滚转角，动态的加速度计受机体振动的影响较为明显。

图2 解算俯仰角Accx与bωy(静态)

图3 解算滚转角与加计的滚转角(静态)

图4 解算俯仰角与加计的俯仰角(动态)

图5 解算滚转角与加计的滚转角(动态)

2.2 基于MARG的四元素姿态解算

基于MARG的四元素姿态解算是在基于IMU的四元素姿态解算的基础上，将加速度计、陀螺仪、磁力计数据相融合。流程图如图6所示。

图6 基于MARG的四元素姿态解算

步骤一：确定磁力计参考矢量

将机体系的磁力计值转换到惯性系下

(6)

获得磁力计的参考矢量

(7)

步骤二：获得磁力计方向误差

将磁力计参考矢量通过加速度计校正的旋转矩阵转到机体系

(8)

得到磁力计方向误差em

(9)

方向误差ea、em通过PI控制器修正陀螺仪角速度，Kmp，Kmi用来控制修正陀螺仪的速度

emp=Kmpem

(10)

emi=emi+Kmiem

(11)

b′ω=b′ω+eap+eai+emp+emi

(12)

步骤三：更新四元素并输出姿态角

与基于IMU的四元素姿态解算方法相同。

至此，整个MARG解算完成。

如图7所示，将四轴静置，加入磁力计观测后，有效地抑制了偏航角的漂移现象。同时，静置的磁力计精确度极低，在叉积法融合磁力计后，明显降低了偏航角的波动幅度。图8是四轴悬停时观测的偏航角，明显发现融合后的偏航角波动范围较小。

图8 磁力计观测下的偏航角(动态)

图7 磁力计观测下的偏航角(静态)

3 基于MEMS的DCM姿态解算

方向余弦矩阵(DCM)不存在奇点,没有近似值,且包含从惯性系到机体系以及机体系到惯性系的所有信息，具体的流程图如图9所示。

图9 基于MEMS的DCM姿态解算流程图

步骤一：DCM初始化

(13)

作为DCM的初始值。

步骤二：获得加速度计与磁力计方向误差获取磁力计参考矢量

(14)

带入方向余弦矩阵得

(15)

根据惯性系下的磁力计值和地磁场的参考矢量得到磁力计的参考矢量

(16)

(17)

(18)

获取加速度计方向误差ea。

(19)

再根据转换公式获取磁力计方向误差em。方向误差ea，em带入式(11)，通过PI控制器修正陀螺角速度，Kmp，Kmi用来控制修正陀螺仪的速度。

步骤三：更新并正交化DCM

将校正后的陀螺仪带入旋转矩阵变换率与角速度的转换公式中更新DCM，根据Paul提出的一种强制正交法[9]对其正交化。

步骤四：欧拉角转换

利用旋转矩阵元素反解出欧拉角

(20)

θ=arctan(-rxz)

(21)

(22)

至此，整个MEMS解算完成。

图2与图10比较，在静态时，两者都表现出非常优秀的滤波效果，解算后的俯仰角比直接用加计读取的俯仰有更好的光滑程度。由图11可知，在静态、无线性振动情况下，单独使用加速度计可以反映出滚转角，在使用DCM的融合解算后，加计的毛刺得到有效抑制。

图10 解算俯仰角与和(静态)

图11 解算滚转角与加计的滚转角(静态)

4 基于IMU的欧拉角姿态解算

由于高速旋转的陀螺对线性振动不敏感，因此，可以通过陀螺仪去修正加速度计，其输出值为

(23)

将其进行单位化处理

(24)

(25)

(26)

算法的流程图如图12所示。

图12 基于IMU的欧拉角解算流程图

(27)

步骤二：更新Axz(n)，Ayz(n)

(28)

(29)

陀螺仪测量的是Axz角度变化率，利用龙格库塔二阶方程更新

bωy(n-1))dt

(30)

同理可得

(31)

步骤三：更新Rg

对Rg作一些恒等变换，首先Rg的模为1：

(32)

则Rg在b′x轴的分量Rgx

(33)

由于以下等式

(34)

tan(Ayz)=Rgy/Rgz

(35)

(36)

则有

(37)

同理可得

(38)

结合式(32)又可以得到Rgz

(39)

Rgz的方向可由cos(Axz)的正负确定。

(40)

步骤五：规范化

(41)

步骤六：求出俯仰角θ′与滚转角φ′

(42)

根据经验可知，w在5～20区间内可以得到满意的效果。

至此，整个IMU解算完成。

图13展示了俯仰角与加速度计、俯仰角与陀螺仪的对应关系。比较加速度计俯仰角的峰值，发现俯仰角的峰值较加速度计滞后，这是以上两个算法都没出现过的现象。在小范围内，解算出的角速度还能比较精确，一旦角度增大，此算法就会出现掉度现象。因此，这种算法只能适用于小范围的角度变化。图14可以看出此算法起到了滤波作用，但解算效果远远没有先前两种算法好。在大滚转角下，会出现掉度现象。

图13 解算俯仰角与Accx和bωy(静态)

图14 解算滚转角与Accx和bωy(静态)

5 姿态算法对比

四轴在静止状态下，除重力加速度外无其他加速度，符合算法的假设。因此，可以比较由加速度计解算的俯仰角、滚转角与用四元素和DCM解算出的俯仰角与滚转角。图15是静态时，同一姿态解分别用加计、方向余弦矩阵、四元素、欧拉角法解算出的俯仰角；图16是观测其在大负的俯仰角下的解算情况；图17是观测其在大正的俯仰角下的解算情况。比较后可以发现4种方法解算出的俯仰角在小角度时近似相等的。单独用加计解算出的角度不够光滑但能反应角度的大小变化。当使用3种算法融合后，角度就会变得柔和光滑。图16可以观察到，在俯仰角的负极值点处，4种方法解算出的角度存在明显的不同。使用DCM能与加计很好的贴合，使用四元素解较加计解大，而使用欧拉解就出现较大的角度偏差。类似，由图17可以观察到在正极值点附近欧拉角法出现明显的偏离，并且极值点出现滞后现象。DCM解算的效果最好，与加计解最为接近。

图15 加速度计、四元素、方向余弦矩阵、欧拉角解算出的俯仰角(静态)

图16 加速度计、四元素、方向余弦矩阵、欧拉角法解算出大负俯仰角(静态)

在四轴增稳飞行时，其飞行加速度小，可以忽略不计，满足算法的假设二。由图18可以观测到在动态过程中，线性振动已经使得加速度计失真，无法像静态过程单独使用加计去观测俯仰角。DCM与四元素解算后如图19所示，在小角度下，两者解算出的角度趋于一致，在大的角度以及风速干扰下出现一些偏差，但两者能反应姿态的变化。在姿态控制中可以通过PID去消除这种偏差。

图19 DCM、四元素解算俯仰角(动态)

6 结论

本文针对四旋翼飞行器惯导系统中的姿态解算问题，首先提出了两点基本假设，并分别利用四元素、方向余弦矩阵、欧拉角等姿态表示法设计了姿态解算算法。把加速度计与陀螺仪当成观测量，叉积去修正陀螺仪角速度，实时更新姿态。最后，对方向余弦矩阵、四元素法、欧拉法做了动态与静态的分析，舍弃了欧拉法。选择在静态与动态表现较好的方向余弦矩阵、四元素法对四轴进行姿态解算。