互补滤波算法姿态解算

1、 基于互补滤波基于互补滤波AHRS姿态解算算法介绍姿态解算算法介绍Mini INS/GPS姿态仪姿态仪介绍内容：介绍内容： 1、四元数、四元数 2、姿态表示的方法、姿态表示的方法 3、姿态解算原理、姿态解算原理一、四元数一、四元数1.1 四元数定义四元数定义 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j则称数则称数q为四元数，而为四元数，而q0称为四元数称为四元数q的实部，称的实部，称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 1.2 四元数的表示方式四元数的表示方式一、四元数一、四元数1.1 四元数定义四元数定义

2、 设设 , 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j则称数则称数q为四元数，而为四元数，而q0称为四元数称为四元数q的实部，称的实部，称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 0123qijkqqqq0123,Rq q q q 1.2 四元数的表示方式四元数的表示方式一、四元数一、四元数1.1 四元数定义四元数定义 设设 , 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j则称数则称数q为四元数，而为四元数，而q0称为四元数称为四元数q的实部，

3、称的实部，称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 0123qijkqqqq0123,Rq q q q 1.2 四元数的表示方式四元数的表示方式一、四元数一、四元数1.1 四元数定义四元数定义 设设 , 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数则称数q为四元数，为四元数， 而而q0称为四元数称为四元数q的实部，的实部， 称称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 0123qijkqqqq0123,Rq q q q 1.2 四元数的表示方式四元数的表示方式一、四元数一、四元数1.1 四

4、元数定义四元数定义 设设 , 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数则称数q为四元数，为四元数， 而而q0称为四元数称为四元数q的实部，的实部， 称称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 0123qijkqqqq0123,Rq q q q123ijkq qq 1.2 四元数的表示方式四元数的表示方式一、四元数一、四元数1.1 四元数定义四元数定义 设设 , 其中，其中，i，j，k满足满足 i2=j2=k2=-1 ij=-ji=k, jk=-kj=i, ki=-ik=j 则称数则称数q为四元数，为

5、四元数， 而而q0称为四元数称为四元数q的实部，的实部， 称称 为为q的虚部。的虚部。 四元数的共轭为四元数的共轭为 0123qijkqqqq0123,Rq q q q123ijkq qq0123ijkqqqqq 1.2 四元数的示方式四元数的示方式1.3 四元数运算四元数运算二、东北天坐标系二、东北天坐标系 东北天坐标系（东北天坐标系（表示为表示为n系系）是一种当地地理坐标系，原点位于导航系统）是一种当地地理坐标系，原点位于导航系统所处的位置所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为点，坐标轴指向北、东和当地垂线方向（向下），也有称为北东地坐标系北东地坐标系三、姿态表示

6、方法三、姿态表示方法 三、姿态表示方法三、姿态表示方法 3.1 方向余弦矩阵方向余弦矩阵 方向余弦矩阵用符号方向余弦矩阵用符号 表示，是一个表示，是一个3*3阶的矩阵，矩阵的列表示阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。载体坐标系中的单位矢量在参考坐标系中的投影。 nbC三、姿态表示方法三、姿态表示方法 3.1 方向余弦矩阵方向余弦矩阵 方向余弦矩阵用符号方向余弦矩阵用符号 表示，是一个表示，是一个3*3阶的矩阵，矩阵的列表示阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。载体坐标系中的单位矢量在参考坐标系中的投影。 nbC1112132122233

7、13233nbcccCcccccc三、姿态表示方法三、姿态表示方法 3.1 方向余弦矩阵方向余弦矩阵 方向余弦矩阵用符号方向余弦矩阵用符号 表示，是一个表示，是一个3*3阶的矩阵，矩阵的列表示阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。载体坐标系中的单位矢量在参考坐标系中的投影。 第第i行、行、j列的元素表示参考坐标系列的元素表示参考坐标系i轴和载体坐标系轴和载体坐标系j轴夹角的余弦。轴夹角的余弦。 在载体坐标系中定义的矢量在载体坐标系中定义的矢量 ，可以通过该矢量左乘方向余弦矩阵，可以通过该矢量左乘方向余弦矩阵 ，即，即 nbC111213212223313233nbc

8、ccCccccccbrnbCnnbbCrr3.2 欧拉角欧拉角 一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次次连续转动来实现。从参考系到一个新的坐标系的变换可以表示：连续转动来实现。从参考系到一个新的坐标系的变换可以表示： 绕参考坐标系的绕参考坐标系的z轴转动轴转动 角角 绕新坐标系的绕新坐标系的y轴转动轴转动 角角 绕新坐标系的绕新坐标系的x轴转动轴转动 角角 称为欧拉转动角称为欧拉转动角3.3 四元数四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标四元数姿态表达式是一个四参数的表达式。它的基本思路是：

9、一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单的单次转动来实现。次转动来实现。 四元数用符号四元数用符号q表示，它是一个具有表示，它是一个具有4个元素的矢量，这些元素是该矢量个元素的矢量，这些元素是该矢量方向和转动大小的函数。方向和转动大小的函数。定义定义 的大小和方向是使参考系绕的大小和方向是使参考系绕 转动一个角度转动一个角度 ，就能与载体坐，就能与载体坐标系重合。标系重合。3.3 四元数四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标四元数姿态表达式是一个四参数的表达式。它的基本思路是：

10、一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单的单次转动来实现。次转动来实现。 四元数用符号四元数用符号q表示，它是一个具有表示，它是一个具有4个元素的矢量，这些元素是该矢量个元素的矢量，这些元素是该矢量方向和转动大小的函数。方向和转动大小的函数。定义定义 的大小和方向是使参考系绕的大小和方向是使参考系绕 转动一个角度转动一个角度 ，就能与载体坐，就能与载体坐标系重合。标系重合。3.3 四元数四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标四元数姿态表达式是一个四参数的表达式。它的基本思路是：

11、一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单的单次转动来实现。次转动来实现。 四元数用符号四元数用符号q表示，它是一个具有表示，它是一个具有4个元素的矢量，这些元素是该矢量个元素的矢量，这些元素是该矢量方向和转动大小的函数。方向和转动大小的函数。定义定义 的大小和方向是使参考系绕的大小和方向是使参考系绕 转动一个角度转动一个角度 ，就能与载体坐，就能与载体坐标系重合。标系重合。3.3 四元数四元数 四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标四元数姿态表达式是一个四参数的表达式。它的基本思路是：

12、一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量 的单的单次转动来实现。次转动来实现。 四元数用符号四元数用符号q表示，它是一个具有表示，它是一个具有4个元素的矢量，这些元素是该矢量个元素的矢量，这些元素是该矢量方向和转动大小的函数。方向和转动大小的函数。定义定义 的大小和方向是使参考系绕的大小和方向是使参考系绕 转动一个角度转动一个角度 ，就能与载体坐，就能与载体坐标系重合。标系重合。 利用四元数进行矢量变换利用四元数进行矢量变换 首先定义一个四元数首先定义一个四元数rb rb=ix+jy+kz rb=0+ix+jy+k

13、z有：有： rn=q*rb*q rn=(q0+iq1+jq2+kq3)(0+ix+jy+kz)(q0-iq1-jq2-kq3) =0+(q02+q12-q22-q32)x+2(q1q2-q0q3)y+2(q1q3+q0q2)zi +2(q1q2+q0q3)x+(q02-q12+q22-q32)y+2(q2q3-q0q1)zj +2(q1q3-q0q2)x+2(q2q3+q0q1)y+(q02-q12-q22+q32)zkrn也可以表示成矩阵形式也可以表示成矩阵形式nbrc r0 00 cc2222012312031302222212030123230122221302230101232()2(

14、)2()2()2()2()Cq q q qqq q qqq q qqq q qq q q qq q q qqq q qq q q qq q q q3.4 方向余弦、欧拉角和四元数的关系方向余弦、欧拉角和四元数的关系 这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用方向余弦和四元数表示。方向余弦和四元数表示。2222012312031302222212030123230122221302230101232()2()2()2()2()2()q q q qqq q qqq q qqq q qq q q qq q q qqq q

15、qq q q qq q q q用方向余弦表示四元数用方向余弦表示四元数 1203 22 3101 33 1202 11 23011 12 23 321()41()41()4(1)qcccqccqqccqqccq用欧拉角表示四元数用欧拉角表示四元数 q0 q1 q2 q3对于小角度位移，四元数参数可以用表示为对于小角度位移，四元数参数可以用表示为用方向余弦表示欧拉角用方向余弦表示欧拉角四元数表示欧拉角四元数表示欧拉角 23012222012323011203222201232()arctan()arcsin(2()2()arctan()q qq qqqqqq qq qq qq qqqqq90o四

16、、互补滤波的姿态解算算法四、互补滤波的姿态解算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法欧拉角法在求在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦方向余弦可用于可用于全姿态的解算但计算量大，不能满足实时性要求。全姿态的解算但计算量大，不能满足实时性要求。四元数法四元数法，其计算量小，无，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。奇点且可以满足飞行器运动过程中姿态的实时解算。四、互补滤波的姿态解算算法四、互补滤波的姿态解算算法 姿

17、态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。小，无奇点且可以满足飞行器运动过程中姿态的实时解算。 姿态解算的原理姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们：对于一个确定的向量，用不同的坐

18、标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。了。 四、互补滤波的姿态解算算法四、互补滤波的姿态解

19、算算法 姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。小，无奇点且可以满足飞行器运动过程中姿态的实时解算。 姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们姿态解算的原理：对于一个确定的向量，

20、用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。了。4.1 互补滤波器解算原理互补滤波器解

21、算原理 陀螺仪陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加磁力计和加速度计速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。动态性能。 互补滤波器的传递函数互补滤波器的传递函数 12( )( ),( )( )( )C sssssC ssC sGG因此，通过互补滤波器能够消除高频噪声和低频误差的累积，能够很好的融合各传

22、感器的数据用用C（s）表示飞行器运动过程中真实的姿态矩阵，表示飞行器运动过程中真实的姿态矩阵，互补滤波器互补滤波器估计的姿态估计的姿态矩阵矩阵加速度计和磁力计加速度计和磁力计观测到的姿态矩阵观测到的姿态矩阵0HCC观测到观测到高频噪高频噪声声陀螺仪测量得到的数据陀螺仪测量得到的数据计算得到的姿态矩阵计算得到的姿态矩阵LCC低频误差累积低频误差累积 G1具有一阶低通滤波特性，具有一阶低通滤波特性，G2具有一阶具有一阶高通滤波特性高通滤波特性C（s）=kp+ki/s（1）初始化四元数）初始化四元数 姿态计算初始，将已知载体初始姿态角带入下式，求出初始时刻的四姿态计算初始，将已知载体初始姿态角带入下

23、式，求出初始时刻的四元数。元数。 q0 q1 q2 q34.2 四元数姿态解算步骤四元数姿态解算步骤（2）获取角速度、加速度、磁力计值）获取角速度、加速度、磁力计值 读取到的读取到的加速度加速度测量值测量值ax,ay,az;陀螺仪陀螺仪测量值测量值wx,wy,wz；磁力计磁力计测量值测量值 mx,my,mz (3) 将加速度计测量值、磁力计测量值化为单位向量将加速度计测量值、磁力计测量值化为单位向量（4）从四元数里获得重力向量和磁场向量）从四元数里获得重力向量和磁场向量 重力向量重力向量 13022301222012322220123120313020222201203012323011222

24、21302230101232()2()2()2()2()2()2()2()Txyzqqqqq qq qq qq qvq qq qqqqqq qq qvvq qq qq qq qqqqqq qq qq qq qqqq 2qn系中，加速度计系中，加速度计输出（重力向量）输出（重力向量）bCn（表示从（表示从n系到系到b系转换矩阵）系转换矩阵）乘以加速度计的实际测量值为加速度计的实际测量值为 (ax,ay,az)T，(vx,vy,vz)T和和(ax,ay,az)T 均表示在均表示在b系中系中的竖直向下的向量的竖直向下的向量(ex,ey,ez)T = (vx,vy,vz)T 叉乘叉乘(ax,ay,az

25、)T 地磁场向量地磁场向量可不可以采用和加速度计一样的修正方法来修正？可不可以采用和加速度计一样的修正方法来修正？加速度计在静止时测量的是重力加速度加速度计在静止时测量的是重力加速度(0,0,1) ，是有大小和方向的；，是有大小和方向的；而是与而是与x轴（或者轴（或者y轴）呈一个角度，与轴）呈一个角度，与z轴呈一个角度。记作轴呈一个角度。记作(bx,by,bz)T ，这里我，这里我们让们让x轴对准北边，所以轴对准北边，所以by=0，即，即 (bx,0,bz)T同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下倘若我们知道倘若我们知道bx和和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中，我们在只不过在加速度计中，我们在n系中的参考向量是（系中的参考向量是（0,0,1) ，变成了地磁计的，变成了地磁计的 (bx,0,bz)T但实际上，当地地磁场相对于东北天坐标的夹角无法测量但实际上，当地地磁场相对于东北天坐标的夹角无法测量我们的做法：反过来从我们的做法：反过来从b系推往系推往n系系22231 20 31 30