基于互补滤波的飞行器姿态解算

基于互补滤波的飞行器姿态解算

————————————————————————————————作者：————————————————————————————————日期：

ﻩ

姿态解算

一、主线

姿态表示方式：矩阵表示,轴角表示,欧拉角表示，四元数表示。

惯性测量单元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,561

AdcRx AdcRy AdcRz

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

1023

Vref

VoltsRx AdcRx =⨯

将3个轴的值代入上式，得到：

3.3

586 1.8910233.3

630 2.0310233.3

561 1.811023VoltsRx V VoltsRy V VoltsRz V

=⨯

==⨯==⨯=

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

1.89 1.650.24

2.03 1.650.381.81 1.650.16DeltaVoltsRx V V V DeltaVoltsRy V V V DeltaVoltsRz V V V

=-==-==-= 现在我们得到了加速度计的电压值,但它的单位还不是2

(9.8/)g m s ，最后的转换，我们还需要引入加速度计的灵敏度，单位通常是/mV g 。比如，加速度计的灵敏度

478.5/0.4785/Sensitivity mV g V g ==。灵敏度值可以在加速度计说明书中找到。要获

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

DeltaVoltsRx

Rx Sensitivity

=

0.240.47850.50.380.47850.790.160.47850.33Rx g Ry g Rz g

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

1023Sensitivity 1023Sensitivity 1023Sensitivity

Vref

AdcRx VzeroG Rx Vref

AdcRy VzeroG

Ry Vref

AdcRz VzeroG

Rz ⨯

-=

⨯-=

⨯-=

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

图2[引自网络]

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

()()()cos cos cos Rx Ry Rz Axr Ayr Azr R

R

R

=

=

=

图2中,2222R RX RY RZ =++,那么，角度即为

arccos

arccos

arccos

Rx Ry Rz Axr Ayr Azr R

R

R

=== 三、互补滤波算法

加速度计是极易受外部干扰的传感器（如机械振动),但是测量值的误差不随时间的变化。陀螺仪输出的角速度可以积分得到角度，动态性能好，受外部干扰小，但积分会造成误差累积。可以看出，它们优缺点互补,结合起来才能有好的效果。

经典互补滤波算法(Ｃｌa ｓs ｉcal Compleme ｎtar ｙ Ｆｉlter )

经典互补滤波算法基本原理是充分利用加速度计提供的低频角度信号和陀螺仪提供的高频角速度信号，对加速度计进行低通滤波,对陀螺仪进行高通滤波,分别滤出相应的干扰信

号，为两者的有效融合提供了很好的解决方案[2］

。

图3 经典互补滤波算法—频域形式原理图

［２]

融合后姿态角估计值为