捷联式姿态解算过程

（1）加速度记测得载体相对惯性空间比力b ib a ，经过误差补偿之后的b

ib a 经过姿态矩阵n b C 的变换得到n ib a ：n n b ib b ib =C a a

将n ib a 进行误差补偿之后通过积分运算得到速度分量n en V 。n en V 一方面作

为系统的输出，一方面作为输入用来求解位置角速率n en W 。 （2）陀螺仪测得载体相对惯性空间角速率b i b W ， 首先通过速度分量n en V 求得位移角速率n en W ，因为n eny W =0，

n

enx n eny W W ????????=22yt xt 22y yt xt yt t x x t t sin cos ()R R sin cos R 11sin cos R R 11sin cos R R R αααααααα??

-????????

-???-++?

-（）（）n x n y V V ??

??????

这样就根据陀螺仪测得的角速率b i b W ，上面所求得的位移角速率n

en W ，

加上已知的地球角速率e i e W 来求的姿态角速率b n b W 。 b b n b n e i e n i e n e i e W =C W =C C W 且 b b n e n n e n W =C W b b b b n b i b i e e n W =W W W --=b i b W b n e n n e i e e n C C W +W -（）

。 然后根据姿态角速率利用四元数微分方程求出四元数中的元素a,b,c,d.

四元数描述了一个坐标系或一个矢量相对某一个坐标系的旋转。a 是标量部分表示了转角的一半余弦值，b ，c ，d 是矢量部分表示瞬时转轴的方向，瞬时转动轴与参考坐标系轴间方向的余弦值。 表达式A=a+bi+cj+dk 。

四元数微分方程：.

b

n b A =1/2AW

..x

..001/200b

b b

nbx nby

nbz

b b b nb nbz

nby

b b b nby

nbz

nbx

b b b nbz nby

nbz

a w w w a w w w

b b w w w

c c w w w

d d ??????---????????-??????=??????-??????-??????

??????

这样求出四元数中a ，b ，c ，d 四个元数。

这里的四元数是用来描述载体坐标系相对游动方位坐标系的转动，即：

2222

b n 2222

n b 2222n b x x a b c d 2bc ad 2bd+ac y =2bc+ad a b c d 2cd-ab y 2bd ab 2cd+ab a -b -c d z z ??+---??

????????-+-?????????

???-+??????（）

（）（

）（）（

）（）

这样就可方向余弦矩阵与四元数姿态矩阵是完全等效的，即

2222

n 2222

b 2222a b

c

d 2bc ad 2bd+ac C =2bc+ad a b c d 2cd-ab 2bd ab 2cd+ab a -b -c d ??

+---??

-+-?

???-+??

（）（）（）（）（

）

（）

而n b cos sin sin cos sin cos cos sin sin cos sin s sin sin sin C =sin sin in cos cos sin sin cos sin cos co sin s sin cos cos γψγθψ

θψγψγθψγψγθψθψγψγθψγθθ

γθ-??

??????-?

+?

-+-

这样利用姿

态矩阵和四元数中各对应项相等的原则得出姿态矩阵中的各个元素，然后就能求出姿态参数，即三个姿态角航向角ψ、俯仰角θ、滚转角γ。

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

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

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

姿态解算 一、主线 姿态表示方式：矩阵表示,轴角表示,欧拉角表示，四元数表示。 惯性测量单元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

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

姿态解算 一、主线 姿态表示方式：矩阵表示，轴角表示，欧拉角表示，四元数表示。 惯性测量单元IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺仪和加速度计，分别测量三轴加速度和三轴角速度。注意，传感器所测数据是原始数据，包含了噪声，无法直接用于飞行器的姿态解算，因此需要对数据进行滤波。 滤波算法：非线性互补滤波算法，卡尔曼滤波算法，Mahony互补滤波算法。 二、知识点补充 加速度计和陀螺仪 加速度计：加速度计,可以测量加速度,包括外力加速度和重力加速度,因此，当被测物体在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系（绝对坐标系）是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角。把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不能表明物体的姿态和运动状态。 陀螺仪：陀螺仪测量角速度。陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。 图1[引自网络] 上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。 加速度计工作原理介绍（摘自网络） 大多数加速度计可归为两类：数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定围的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。不管使用什么类型的ADC模块，都会得到一个在一定围的数值。例如一个10位ADC模块的输出值围在0-1023间。假设我们从10位ADC模块得到了以下的三个轴的数据： === 586,630,561 AdcRx AdcRy AdcRz