互补滤波算法姿态解算

定义 的大小和方向是使参考系绕 转动一个角度 ，就能与载体坐
标系重合。
利用四元数进行矢量变换
首先定义一个四元数rb’ rb=ix+jy+kz rb’=0+ix+jy+kz
有： rn’=q*rb’*q’ rn’=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0-iq1-jq2-kq3) =0+{(q0^2+q1^2-q2^2-q3^2)x+2(q1q2-q0q3)y+2(q1q3+q0q2)z}i +{2(q1q2+q0q3)x+(q0^2-q1^2+q2^2-q3^2)y+2(q2q3-q0q1)z}j +{2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q0^2-q1^2-q2^2+q3^2)z}k
为
q的虚部。
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义
q q ,q q q q q q 设 q
0
i
1
j
2
k
3
,
,
01
, R
23
其中，i，j，k满足
i^2=j^2=k^2=-1
ij=-ji=k, jk=-kj=i, ki=-ik=j
则称数q为四元数，
而q0称为四元数q的实部，
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。q q q q Fra bibliotekarctan(
2
12 2
) 0 3
2
2
0
1
2
3
四、互补滤波的姿态解算算法
姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求 解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用 于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小， 无奇点且可以满足飞行器运动过程中姿态的实时解算。
qq) 01
q q q q 2
2
2
2
0
1
2
3
3.4 方向余弦、欧拉角和四元数的关系
q q q q
2
0
2
1
2
2
2 3
2(q q 12
qq) 03
2(q q 13
q q ) 02
2(q q q q )
12
03
q2 q2 q2 q2
0
1
2
3
2(q q q q )
23
01
2(q q q q )
c c c n C c c c b
11 21
12 22
13 23
c c c 31
32
33
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
c c c n C c c c b
11 21
12 22
4.2 四元数姿态解算步骤
（1）初始化四元数 姿态计算初始，将已知载体初始姿态角带入下式，求出初始时刻的四
元数。 q0 q1 q2 q3
（2）获取角速度、加速度、磁力计值
读取到的加速度测量值ax,ay,az;陀螺仪测量值wx,wy,wz；磁力计测量值
mx,my,mz (3) 将加速度计测量值、磁力计测量值化为单位向量
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
rn’也可以表示成矩阵形式
r c r n'
' b'
c
'
0 0
0 c
q q q q
2
0
2
1
2
2
2 3
C
2(q q 12
qq) 03
2(q q 13
qq) 02
2(q q q q )
12
03
q2 q2 q2 q2
0
1
2
3
2(q q q q )
23
01
2(q q q q )
13
02
2(q q 23
2(q2q3q0q1)
T
2(q1q3
q
0q
2)
0
2(q2q3q0q1) 0
q02q12q
2 2
q32
1
乘以
2(q q q q )
13
0 2
2(q q q q )
23
01
q q q q
2
0
2
1
2
2
2 3
加速度计的实际测量值为 (ax,ay,az)T， (vx,vy,vz)T和(ax,ay,az)T 均表示在b系中的竖直向下的向量
四、互补滤波的姿态解算算法
姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度 ，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单
次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量
方向和转动大小的函数。
G G C 互补滤波器
估计的姿态 矩阵
(s) C(s) , (s) s 1 s C(s) 2 s C(s)
C
L
用C（s）表示飞行器运动过程中真实的姿态矩阵，
低频误差累积
加速度计和磁力计 观测到的姿态矩阵
C 0C H
观测到 高频噪 声
G1具有一阶低通滤波特性，G2具有一阶 高通滤波特性
因此，通过互补滤波器能够消除高频噪声和 低频误差的累积，能够很好的融合各传感器 的数据
13
02
2(q q q q )
23
01
q q q q 2
2
2
2
0
1
2
3
这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移，四元数参数可以用表示为
q 0
1 2
1
(1c11c22c33) 2
q 1
1
4q
c c (
)
32
23
0
绕参考坐标系的z轴转动 角
绕新坐标系的y轴转动 角
绕新坐标系的x轴转动 角
称为欧拉转动角
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标
系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。
四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示，是一个3*3阶的矩阵，矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
(ex,ey,ez)T = (vx,vy,vz)T 叉乘(ax,ay,az)T
地磁场向量 可不可以采用和加速度计一样的修正方法来修正？ 加速度计在静止时测量的是重力加速度(0,0,1)T ，是有大小和方向的； 同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下 而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作(bx,by,bz)T ，这里我 们让x轴对准北边，所以by=0，即 (bx,0,bz)T 倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。
四、互补滤波的姿态解算算法
姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。