互补滤波算法姿态解算
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
1 2 3
四元数的共轭为 1.2 四元数的示方式
qq
0
qiq jq k
1 2 3
1.3 四元数运算
二、东北天坐标系
东北天坐标系（表示为n系）是一种当地地理坐标系，原点位于导航系统 所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为 北东地坐标系
三、姿态表示方法
三、姿态表示方法
转动一个角度
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度
2(q q q
2 0 1 0 2 2 2 3 2 3 0 1 2 2 2 2 2 3 0 1 0 1 2 3
3.4 方向余弦、欧拉角和四元数的关系
2 2 2 2 q 0 q1 q 2 q 3 2(q q q q ) 1 2 0 3 2(q q q q ) 1 3 0 2
为
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k , 其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。 四元数的共轭为
1 2 3
四元数的共轭为 1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q qiq jq k , 0 1 2 3
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 q i q j q k 为q的虚部。
转动一个角度
，就能与载体坐
利用四元数进行矢量变换
首先定义一个四元数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 rn’也可以表示成矩阵形式 有：
一、四元数
1.1 四元数定义 设 q q qiq jq k , 0 1 2 3
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 q i q j q k 为q的虚部。
4.1 互补滤波器解算原理
陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加 速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互 补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的 动态性能。
互补滤波器的传递函数
C（s） =kp+ki/s
C (s) s 互补滤波器 ( s) , 2( s ) 1 估计的姿态 s C (s) s C (s) 矩阵 用C（s）表示飞行器运动过程中真实的姿态矩阵，
四、互补滤波的姿态解算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。
基于互补滤波AHRS 姿态解算算法介绍
Mini INS/GPS姿态仪
介绍内容： 1、四元数 2、姿态表示的方法 3、姿态解算原理
一、四元数
1.1 四元数定义
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。
转动一个角度
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。
三、姿态表示方法
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。 c11 c12 c13 n C b c 21 c 22 c 23 c 31 c 32 c 33 第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。 在载体坐标系中定义的矢量 r
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。 c11 c12 c13 n C b c 21 c 22 c 23 c 31 c 32 c 33
（1）初始化四元数 姿态计算初始，将已知载体初始姿态角带入下式，求出初始时刻的四 元数。 q0 q1 q2 q3
（2）获取角速度、加速度、磁力计值 读取到的加速度测量值ax,ay,az;陀螺仪测量值wx,wy,wz；磁力计测量值 mx,my,mz n系中，加速度计 (3) 将加速度计测量值、磁力计测量值化为单位向量 输出（重力向量） （4）从四元数里获得重力向量和磁场向量
为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k ,
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 为q的虚部。 四元数的共轭为 1.2 四元数的表示方式
q ) 2(q q q q ) 2( ) q q q q qq qq 2(q q q q ) q q q q
2(q q q
1 2 2 2 0 1 0 3 1 3 0 2 2 2 2 3 2 3 0 1 2 2 2 2 2 3 0 1 0 1 2 3
q , q ,q , q R
0 1 2 3
为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k ,
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。 四元数的共轭为
陀螺仪测量得到的数据 计算得到的姿态矩阵
G
G
C C
L
低频误差累积
加速度计和磁力计 观测到的姿态矩阵
C 0 C
观测到 高频噪 声
G1具有一阶低通滤波特性，G2具有一阶 高通滤波特性 因此，通过互补滤波器能够消除高频噪声和 低频误差的累积，能够很好的融合各传感器 的数据
H
4.2 四元数姿态解算步骤
2(q
四、互补滤波的姿态解算算法
姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求 解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用 于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小， 无奇点且可以满足飞行器运动过程中姿态的实时解算。
四、互补滤波的姿态解算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移，四元数参数可以用表示为
1 1 q 0 2 (1 c11c 22c 33) 2 1 q 1 4 (c 32c 23) q 0
q q
Fra Baidu bibliotek
2
1 4q 1 4q
0 0
(c13c 31) (c 21c 12)
3
用欧拉角表示四元数
q0
q1
q2 q3
用方向余弦表示欧拉角
90o
四元数表示欧拉角
q q q ) arctan( ) q q q q arcsin(2(q q q q )) 2(q q q q ) arctan( ) q q q q
2 3 2 2 0 2 1 2 0 1 2 3 2 3 0 1 1 2 2 2 0 2 2 3 2 0 1 3
b
，可以通过该矢量左乘方向余弦矩阵
C
n b
，即
r
n
C br
n
b
3.2 欧拉角 一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次 连续转动来实现。从参考系到一个新的坐标系的变换可以表示： 绕参考坐标系的z轴转动 角 绕新坐标系的y轴转动 角 绕新坐标系的x轴转动 角 称为欧拉转动角
r cr
2 2 2 2 q 0 q1 q 2 q 3 C 2(q q q q ) 1 2 0 3 2(q q q q ) 1 3 0 2
n'
' b'
c
1 2 2
'
0 0 0 c
3 1 3 0 2
q ) 2(q q q q ) q q q q 2(q q q q ) 2(q q q q ) q q q q
四元数的共轭为 1.2 四元数的示方式
0
qiq jq k
1 2 3
1.3 四元数运算
二、东北天坐标系
东北天坐标系（表示为n系）是一种当地地理坐标系，原点位于导航系统 所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为 北东地坐标系
三、姿态表示方法
三、姿态表示方法
转动一个角度
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
转动一个角度
2(q q q
2 0 1 0 2 2 2 3 2 3 0 1 2 2 2 2 2 3 0 1 0 1 2 3
3.4 方向余弦、欧拉角和四元数的关系
2 2 2 2 q 0 q1 q 2 q 3 2(q q q q ) 1 2 0 3 2(q q q q ) 1 3 0 2
为
四元数的共轭为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k , 其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。 四元数的共轭为
1 2 3
四元数的共轭为 1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q qiq jq k , 0 1 2 3
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 q i q j q k 为q的虚部。
转动一个角度
，就能与载体坐
利用四元数进行矢量变换
首先定义一个四元数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 rn’也可以表示成矩阵形式 有：
一、四元数
1.1 四元数定义 设 q q qiq jq k , 0 1 2 3
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 q i q j q k 为q的虚部。
4.1 互补滤波器解算原理
陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加 速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互 补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的 动态性能。
互补滤波器的传递函数
C（s） =kp+ki/s
C (s) s 互补滤波器 ( s) , 2( s ) 1 估计的姿态 s C (s) s C (s) 矩阵 用C（s）表示飞行器运动过程中真实的姿态矩阵，
四、互补滤波的姿态解算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。
基于互补滤波AHRS 姿态解算算法介绍
Mini INS/GPS姿态仪
介绍内容： 1、四元数 2、姿态表示的方法 3、姿态解算原理
一、四元数
1.1 四元数定义
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。
转动一个角度
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们 所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在 误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐 标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。 这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正 了。
三、姿态表示方法
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。 c11 c12 c13 n C b c 21 c 22 c 23 c 31 c 32 c 33 第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。 在载体坐标系中定义的矢量 r
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
3.1 方向余弦矩阵 n 方向余弦矩阵用符号 C b 表示，是一个3*3阶的矩阵，矩阵的列表示 载体坐标系中的单位矢量在参考坐标系中的投影。 c11 c12 c13 n C b c 21 c 22 c 23 c 31 c 32 c 33
（1）初始化四元数 姿态计算初始，将已知载体初始姿态角带入下式，求出初始时刻的四 元数。 q0 q1 q2 q3
（2）获取角速度、加速度、磁力计值 读取到的加速度测量值ax,ay,az;陀螺仪测量值wx,wy,wz；磁力计测量值 mx,my,mz n系中，加速度计 (3) 将加速度计测量值、磁力计测量值化为单位向量 输出（重力向量） （4）从四元数里获得重力向量和磁场向量
为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k ,
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数， 而q0称为四元数q的实部， 称 为q的虚部。 四元数的共轭为 1.2 四元数的表示方式
q ) 2(q q q q ) 2( ) q q q q qq qq 2(q q q q ) q q q q
2(q q q
1 2 2 2 0 1 0 3 1 3 0 2 2 2 2 3 2 3 0 1 2 2 2 2 2 3 0 1 0 1 2 3
q , q ,q , q R
0 1 2 3
为
1.2 四元数的表示方式
一、四元数
1.1 四元数定义 设 q q 0 q1i q 2 j q 3k ,
q , q ,q , q R
0 1 2 3
其中，i，j，k满足 i^2=j^2=k^2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数q为四元数，而q0称为四元数q的实部，称 q的虚部。 四元数的共轭为
陀螺仪测量得到的数据 计算得到的姿态矩阵
G
G
C C
L
低频误差累积
加速度计和磁力计 观测到的姿态矩阵
C 0 C
观测到 高频噪 声
G1具有一阶低通滤波特性，G2具有一阶 高通滤波特性 因此，通过互补滤波器能够消除高频噪声和 低频误差的累积，能够很好的融合各传感器 的数据
H
4.2 四元数姿态解算步骤
2(q
四、互补滤波的姿态解算算法
姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求 解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用 于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小， 无奇点且可以满足飞行器运动过程中姿态的实时解算。
四、互补滤波的姿态解算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在 求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可 用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量 小，无奇点且可以满足飞行器运动过程中姿态的实时解算。
，就能与载体坐
3.3 四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标 系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单 次转动来实现。 四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量 方向和转动大小的函数。
定义 的大小和方向是使参考系绕 标系重合。
这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移，四元数参数可以用表示为
1 1 q 0 2 (1 c11c 22c 33) 2 1 q 1 4 (c 32c 23) q 0
q q
Fra Baidu bibliotek
2
1 4q 1 4q
0 0
(c13c 31) (c 21c 12)
3
用欧拉角表示四元数
q0
q1
q2 q3
用方向余弦表示欧拉角
90o
四元数表示欧拉角
q q q ) arctan( ) q q q q arcsin(2(q q q q )) 2(q q q q ) arctan( ) q q q q
2 3 2 2 0 2 1 2 0 1 2 3 2 3 0 1 1 2 2 2 0 2 2 3 2 0 1 3
b
，可以通过该矢量左乘方向余弦矩阵
C
n b
，即
r
n
C br
n
b
3.2 欧拉角 一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次 连续转动来实现。从参考系到一个新的坐标系的变换可以表示： 绕参考坐标系的z轴转动 角 绕新坐标系的y轴转动 角 绕新坐标系的x轴转动 角 称为欧拉转动角
r cr
2 2 2 2 q 0 q1 q 2 q 3 C 2(q q q q ) 1 2 0 3 2(q q q q ) 1 3 0 2
n'
' b'
c
1 2 2
'
0 0 0 c
3 1 3 0 2
q ) 2(q q q q ) q q q q 2(q q q q ) 2(q q q q ) q q q q