互补滤波算法姿态解算PPT课件

-,1,基于互补滤波AHRS姿态解算算法介绍,-,2,MiniINS/GPS姿态仪,-,3,-,4,-,5,-,6,介绍内容：1、四元数2、姿态表示的方法3、姿态解算原理,-,7,一、四元数,1.1四元数定义其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的表示方式,-,8,一、四元数,1.1四元数定义设,其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的表示方式,-,9,一、四元数,1.1四元数定义设,其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的表示方式,-,10,一、四元数,1.1四元数定义设,其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的表示方式,-,11,一、四元数,1.1四元数定义设,其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的表示方式,-,12,一、四元数,1.1四元数定义设,其中，i，j，k满足i2=j2=k2=-1ij=-ji=k,jk=-kj=i,ki=-ik=j则称数q为四元数，而q0称为四元数q的实部，称为q的虚部。四元数的共轭为,1.2四元数的示方式,-,13,-,14,1.3四元数运算,-,15,二、东北天坐标系,东北天坐标系（表示为n系）是一种当地地理坐标系，原点位于导航系统所处的位置P点，坐标轴指向北、东和当地垂线方向（向下），也有称为北东地坐标系,-,16,三、姿态表示方法,-,17,三、姿态表示方法3.1方向余弦矩阵方向余弦矩阵用符号表示，是一个3*3阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。,-,18,三、姿态表示方法3.1方向余弦矩阵方向余弦矩阵用符号表示，是一个3*3阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。,-,19,三、姿态表示方法3.1方向余弦矩阵方向余弦矩阵用符号表示，是一个3*3阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。在载体坐标系中定义的矢量，可以通过该矢量左乘方向余弦矩阵，即,-,20,3.2欧拉角一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次连续转动来实现。从参考系到一个新的坐标系的变换可以表示：绕参考坐标系的z轴转动角绕新坐标系的y轴转动角绕新坐标系的x轴转动角称为欧拉转动角,-,21,3.3四元数四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量的单次转动来实现。四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义的大小和方向是使参考系绕转动一个角度，就能与载体坐标系重合。,-,22,3.3四元数四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量的单次转动来实现。四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义的大小和方向是使参考系绕转动一个角度，就能与载体坐标系重合。,-,23,3.3四元数四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量的单次转动来实现。四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义的大小和方向是使参考系绕转动一个角度，就能与载体坐标系重合。,-,24,3.3四元数四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系中的矢量的单次转动来实现。四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。定义的大小和方向是使参考系绕转动一个角度，就能与载体坐标系重合。,-,25,利用四元数进行矢量变换,首先定义一个四元数rbrb=ix+jy+kzrb=0+ix+jy+kz,有：rn=q*rb*qrn=(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)zk,rn也可以表示成矩阵形式,-,26,3.4方向余弦、欧拉角和四元数的关系,这样，四元数可以用方向余弦、欧拉角表示，同样，欧拉角也可以用方向余弦和四元数表示。,-,27,用方向余弦表示四元数,用欧拉角表示四元数q0q1q2q3,对于小角度位移，四元数参数可以用表示为,-,28,用方向余弦表示欧拉角,四元数表示欧拉角,-,29,四、互补滤波的姿态解算算法,姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。,-,30,四、互补滤波的姿态解算算法姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。,-,31,四、互补滤波的姿态解算算法姿态解算常用的算法有欧拉角法、方向余弦法和四元数法。欧拉角法在求解姿态时存在奇点（万向节死锁），不能用于全姿态的解算；方向余弦可用于全姿态的解算但计算量大，不能满足实时性要求。四元数法，其计算量小，无奇点且可以满足飞行器运动过程中姿态的实时解算。姿态解算的原理：对于一个确定的向量，用不同的坐标系表示时，他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的旋转矩阵存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，我们修正的就是四元数，这样姿态就被修正了。4.1互补滤波器解算原理陀螺仪动态响应特性良好，但计算姿态时会产生累积误差。磁力计和加速度计测量姿态没有累积误差，但动态响应较差。因此他们在频域上特性互补，可以采用互补滤波器融合这三种传感器的数据，提高测量精度和系统的动态性能。,-,32,互补滤波器的传递函数,因此，通过互补滤波器能够消除高频噪声和低频误差的累积，能够很好的融合各传感器的数据,用C（s）表示飞行器运动过程中真实的姿态矩阵，,互补滤波器估计的姿态矩阵,加速度计和磁力计观测到的姿态矩阵,观测到高频噪声,陀螺仪测量得到的数据计算得到的姿态矩阵,低频误差累积,G1具有一阶低通滤波特性，G2具有一阶高通滤波特性,C（s）=kp+ki/s,-,33,（1）初始化四元数姿态计算初始，将已知载体初始姿态角带入下式，求出初始时刻的四元数。q0q1q2q3,4.2四元数姿态解算步骤,-,34,（2）获取角速度、加速度、磁力计值读取到的加速度测量值ax,ay,az;陀螺仪测量值wx,wy,wz；磁力计测量值mx,my,mz(3)将加速度计测量值、磁力计测量值化为单位向量（4）从四元数里获得重力向量和磁场向量重力向量,n系中，加速度计输出（重力向量）,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)T,-,35,地磁场向量,可不可以采用和加速度计一样的修正方法来修正？,加速度计在静止时测量的是重力加速度(0,0,1)T，是有大小和方向的；,而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作(bx,by,bz)T，这里我们让x轴对准北边，所以by=0，即(bx,0,bz)T,同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下,倘若我们知道bx和bz的精确值，那么我们就可以采用和加速度计一样的修正方法来修正。,只不过在加速度计中，我们在n系中的参考向量是（0,0,1)T，变成了地磁计的(bx,0,bz)T,但实际上，当地地磁场相对于东北天坐标的夹角无法测量,我们的做法：反过来从b系推往n系,-,36,地磁计在b系中的输出,乘以,加速度计修正后的b系到n系的旋转矩阵XOY平面重合,地磁计在XOY平面上（n系）的向量的大小必定相同,对于bz的处理，不做变动,经过这样处理之后的(bx,0,bz)T,经过n系到b系的旋转矩阵再转到b系得(wx,wy,wz)T,向量做叉乘：(ex,ey,ez)T=(mx,my,mz)T