基于互补滤波的飞行器姿态解算 ———————————————————————————————— 作者： ———————————————————————————————— 日期：

ﻩ姿态解算 一、主线 姿态表示方式：矩阵表示,轴角表示,欧拉角表示，四元数表示。 惯性测量单元IMU(Inertｉal Ｍeasureｍｅnt Unｉｔ）：MＰU60５0芯片，包含陀螺仪和加速度计,分别测量三轴加速度和三轴角速度。注意,传感器所测数据是原始数据，包含了噪声,无法直接用于飞行器的姿态解算,因此需要对数据进行滤波。 滤波算法:非线性互补滤波算法，卡尔曼滤波算法,Maｈonｙ互补滤波算法。 二、知识点补充 加速度计和陀螺仪 加速度计：加速度计，可以测量加速度，包括外力加速度和重力加速度,因此,当被测物体在静止或匀速运动（匀速直线运动)的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系(绝对坐标系）是固连的，通过这种关系,可以得到加速度计所在平面与地面 的角度关系 也就是横滚角和俯仰角。把加速度传感器水平静止放在桌子上，它的Ｚ轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时,它的输出就不正确了,或者说它的输出不能表明物体的姿态和运动状态。 陀螺仪:陀螺仪测量角速度。陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。

图1[引自网络] 上图中，Rxz是Ｒ在XZ面上的投影,与Z轴的夹角为Ａxz。Ryｚ是Ｒ在ZＹ面上的投影，与Z轴的夹角为Ayz。陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。 加速度计工作原理介绍（摘自网络) 大多数加速度计可归为两类:数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息,而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ＡDC（模拟量转数字量）模块将其转换为数字值。不管使用什么类型的ADC模块,都会得到一个在一定范围内的数值。例如一个1０位ADＣ模块的输出值范围在0－1023间。假设我们从10位ADC模块得到了以下的三个轴的数据：

586,630,561AdcRxAdcRyAdcRz 每个AＤC模块都有一个参考电压，假设在我们的例子中，它是3.3Ｖ。要将一个1０位的ADＣ值转成电压值，我们使用下列公式：

1023VrefVoltsRxAdcRx

将3个轴的值代入上式，得到： 3.3586 1.8910233.3630 2.0310233.3561 1.811023VoltsRxVVoltsRyVVoltsRzV

 每个加速度计都有一个零加速度的电压值，这个电压值对应于加速度为0g。通过计算相对0g电压的偏移量我们可以得到一个有符号的电压值。比方说,0g电压值1.65VzeroGV,通过下面的方式可以得到相对０ｇ电压的偏移量：

1.891.650.242.031.650.381.811.650.16DeltaVoltsRxVVVDeltaVoltsRyVVVDeltaVoltsRzVVV

现在我们得到了加速度计的电压值,但它的单位还不是2(9.8/)gms，最后的转换，我们还需要引入加速度计的灵敏度，单位通常是/mVg。比如，加速度计的灵敏度478.5/0.4785/SensitivitymVgVg。灵敏度值可以在加速度计说明书中找到。要获

得最后的单位为g的加速度,我们使用下列公式计算： DeltaVoltsRxRxSensitivity

0.240.47850.50.380.47850.790.160.47850.33RxgRygRzg

综上,可以把以上步骤用以下公式表达

1023Sensitivity

1023Sensitivity

1023Sensitivity

VrefAdcRxVzeroGRxVrefAdcRyVzeroGRyVrefAdcRzVzeroGRz





 现在我们得到了惯性力矢量的三个分量,如果设备除了重力外不受任何外力影响,那我们就可以认为这个方向就是重力矢量的方向。（自此明白了文献［1]中所说只使用加速度计获得的角度是基于飞行器在匀速飞行或静止的条件下得到的）

图2[引自网络] 我们感兴趣的角度是向量R和Ｘ,Ｙ，Z轴之间的夹角,那就令这些角度为Axｒ，Ayｒ，Azｒ。观察由R和Rx组成的直角三角形

cos coscosRxRyRzAxrAyrAzrRRR

图2中,2222RRXRYRZ,那么，角度即为 arccosarccosarccosRxRyRzAxrAyrAzrRRR 三、互补滤波算法 加速度计是极易受外部干扰的传感器（如机械振动),但是测量值的误差不随时间的变化。陀螺仪输出的角速度可以积分得到角度，动态性能好，受外部干扰小，但积分会造成误差累积。可以看出，它们优缺点互补,结合起来才能有好的效果。 经典互补滤波算法(Ｃｌaｓsｉcal Complemeｎtarｙ Ｆｉlter) 经典互补滤波算法基本原理是充分利用加速度计提供的低频角度信号和陀螺仪提供的高频角速度信号，对加速度计进行低通滤波,对陀螺仪进行高通滤波,分别滤出相应的干扰信号，为两者的有效融合提供了很好的解决方案[2］。

图3 经典互补滤波算法—频域形式原理图［２] 融合后姿态角估计值为 ˆgp

app

Ks

sKssK

其中,g为陀螺仪测量的角速度,a为加速度计测量的角度值，pK为比例系

数,1()psFssK为高通滤波器，21()1()ppKFsFssK为低通滤波器。

图4 经典互补滤波算法—时域形式原理图[2] 对ˆgp

app

Ks

sKssK进行反拉氏变换,可得时域微分形式为

ˆˆ()pagK

改进后的互补滤波算法(Ｅxｐliｃit Compｌｅmentａry Fｉlter) 经典互补滤波算法实现简单,但是估算精度角度较低，文献[3］提出了一种改进算法（ECＦ)，在经典互补滤波算法的补偿环节加入积分器,以消除陀螺仪漂移常值误差,原理框图如下图。

图５ 改进后的互补滤波算法［2] 时域微分形式为

ˆˆˆ()()IpaagKKs

四、四旋翼飞行器姿态解算流程 四旋翼姿态解算整体流程框图如下

图5 四元数表示的姿态解算整体流程框图[４] 其中，Gyroscopｅ为陀螺仪,b是陀螺仪输出的测量数据，Ａｃceleｒoｍeteｒ是加速度计,在Ａirｓpeｅd模块以及bf作用下，加速度输出测量数据ˆbg。 第一，对加速度计测出来的数据进行归一化处理:ˆˆbgg； 第二,将重力加速度旋转到机体坐标，得到重力加速度在三个轴上的加速度分量,作为标准加速度。



222201231203021322221203012323000011cccssscsscss0csccssssccss0ssccc12()2()2()2(TbnnbTCCqqqqqqqqqqqqqqqqqqqqqqq012222130201230123

1302012322220123

0)02()2()12()2()Tqqqqqqqqqqqqqqqqqqqqqqqqq













其中bnC为世界坐标系到机体坐标系的变换矩阵。 (文献［4]图中此处原文中有错误,已经在截图中进行了修改） 第三，归一化后的加速度计值与标准加速度做叉积运算,求得误差向量e; 第四,对误差进行滤波，改进后的互补滤波算法（ECＦ）可以表示为以下四元数形式：

1ˆˆ()

2qqp

pIkeke ,

ˆevv

第四,求解四元数微分方程1ˆˆ()2qqp,求得最新的0123,,,qqqq; 第五,代入以下公式求得姿态角。 012322220123

2()arctanqqqqqqqq



1302arcsin2()qqqq

120322220123

2()arctanqqqqqqqq

