四元数在惯性导航系统中的应用课件.ppt
Lecture 11-Quaternion,1,Review,Computing a vehicles attitude by solving,where,using Peano-Baker solution,or using numerical integration.,Lecture 11-Quaternion,2,Quaternions in SINS,四元数在惯性导航系统中的应用,Lecture 11-Quaternion,3,Outline,四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解,Lecture 11-Quaternion,4,1.0 Hamilton 和四元数,四元数:描述刚体的转动(by Hamilton),理论上的突破-1843.10.16,2005 朝拜之旅,“Here as he walked by on the 16th of October 1843,Sir William Rowan Hamilton in a flash of genius discovered the fundamental formula for quaternion multiplication carved on a stone on the bridge”,在捷联惯性导航及图像处理中应用的优势,Lecture 11-Quaternion,5,n,1.1*四元数(quaternions)定义,一个有固定点的刚体通过绕该点的某个轴转过特定角度可达到任何姿态,转轴的方向可以表示成一个单位矢量:,则描述该转动的四元数可以表示成:,四元数既反映了转动的方向又反映了转动的幅值.,Lecture 11-Quaternion,6,1.2 四元数的组成,四元数的表示：,-标量部分,-矢量部分,包括一个实数单位 1 和三个虚数单位 i,j,k,Lecture 11-Quaternion,7,Outline,四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解,Lecture 11-Quaternion,8,2.1*加法和减法,加法和减法：,或简写成：,Lecture 11-Quaternion,9,2.2 虚数单位的乘法规则,i,j,k 在乘法运算中的规则:,对比 Hamilton 的公式,Lecture 11-Quaternion,10,2.3*四元数乘法,或简单地表示成：,Lecture 11-Quaternion,11,2.3 四元数乘法自定义函数,function q1=qmul(q,m)lm=q(1);p1=q(2);p2=q(3);p3=q(4);q1=lm-p1-p2-p3 p1 lm-p3 p2 p2 p3 lm-p1 p3-p2 p1 lm*m;,a=1 2 2 3;,b=2 4 2 3;,q=qmul(a,b),q=-0.7796 0.3282 0.4924 0.2052,Lecture 11-Quaternion,12,2.3*四元数乘法表示符号,四元数乘法的符号,关于交换率和结合律,Lecture 11-Quaternion,13,2.4*共扼和范数,q 和 q*彼此互为四元数.,可以证明：,四元数的范数,-定义成,是规范化的,Lecture 11-Quaternion,14,2.5*四元数的逆和除法,和,因为,除法:,function qi=qinv(q)%inverse of quaternionqn=norm(q);q(2:4)=-q(2:4);qi=q/qn2;,Lecture 11-Quaternion,15,Outline,四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解,Lecture 11-Quaternion,16,3.1*矢量的旋转,如果矢量 R 相对固定坐标系旋转,并且该旋转可以用四元数 q 描述，新矢量记为 R，,则 R 和 R 之间的变换可以表示成下述四元数运算:,含义:矢量 R 相对固定坐标系旋转，,旋转的角度和轴向由 q 决定,上述运算中,R 被当成一个标量部分为零的四元数，即：,Lecture 11-Quaternion,17,3.2*坐标系的旋转,一个矢量 V 相对于坐标系 OXYZ 固定:,从坐标系 OXYZ 转动了 q,得到一个新坐标系 OXYZ.,V 分解在新坐标系 OXYZ 中,矢量 V 在两个坐标系之间的坐标变换:,记：,Lecture 11-Quaternion,18,3.3 四元数和方向余弦,-表示坐标系旋转,其中,q 和 C 之间是什么关系?,应用四元数乘法,得到,Lecture 11-Quaternion,19,3.3 四元数和方向余弦,Lecture 11-Quaternion,20,3.4 四元数转动变换的两种形式,如果一个矢量 V 固定，坐标系旋转按照四元数 q 进行了旋转，得到了一个新坐标系，则该矢量分别在新旧坐标系中投影表达式间的关系借助映像方式可以表示为：,如果一个坐标系固定,一个矢量 VE 按照四元数 q 相对该坐标系进行了转动，得到一个新的矢量 VE，则新旧矢量之间的关系为：,Lecture 11-Quaternion,21,Outline,四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解,Lecture 11-Quaternion,22,4.0*转动四元数的合成,连续的多次转动可以等效成一次转动.,假设四元数 q1 和 q2 分别代表第一次和第二次坐标系旋转.,q1,q2,其中 q1 和 q2 的轴必须表示成映像形式.,若 q1 和 q2 的轴都表示在原来坐标系中,则,Lecture 11-Quaternion,23,4.1四元数合成例子:非映像方式,坐标系顺序旋转情况下四元数的合成,坐标系 OXYZ 相对坐标系 OXYZ 多次旋转,首先,绕 Z 轴转过角度,瞬时转轴 n 和 k 轴重合,则,Lecture 11-Quaternion,24,第二次旋转:绕 X 转过角度,旋转轴 n 表示为:,4.1 四元数合成:非映像方式,对应的四元数:,Lecture 11-Quaternion,25,4.1 四元数合成:映像的方式,这里 q1 和 q2 的转动轴表示为非映像的形式,因此合成的四元数为:,转动次数越多，合成后四元数的表达式会愈发复杂,Lecture 11-Quaternion,26,4.2*四元数合成:映像的方式,每次转动的瞬时转轴都以映像方式给出.,对第一次转动,瞬时转轴 n1 的映像方式和非映像方式相同：,因此,Lecture 11-Quaternion,27,4.2 四元数合成:映像的方式,第二次转动绕着 OX 转过了,转轴 n2 沿着 OX,n2 在坐标系 XYZ 中的映像为：,因此 q2 的映像形式为,Lecture 11-Quaternion,28,4.2 四元数合成:映像的方式,第三次转动绕着 OY 轴转过.,转轴 n3 沿着 OY,轴 OY 是由原来坐标系的 OY 轴转动得到的，因此 n3 的映像形式为:,这样，四元数 q3 的映像形式为,Lecture 11-Quaternion,29,4.2 四元数合成:映像的方式,因为 q1,q2 和 q3 都表示成了映像的形式,所以合成的四元数 q 的计算公式为:,由 q 可以进一步得到合成转动对应的方向余弦矩阵,Lecture 11-Quaternion,30,Outline,四元数的定义四元数的运算利用四元数进行旋转变换利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解,Lecture 11-Quaternion,31,5.0*姿态解算的四元数微分方程,对SINS，如用方向余弦矩阵,微分方程为,如果用四元数,其微分方程为,其中 q 为描述载体转动的四元数,为载体相对导航参考坐标系的角速度,也表示为四元数的形式:,则:,或,-只包含四个一阶微分方程,Lecture 11-Quaternion,32,5.1一阶 Runge-Kutta 算法,假设捷联惯导的“数学平台”跟踪地理坐标系,则,四元数微分方程及简化,利用 Runge-Kutta 数值积分算法,一阶 Runge-Kutta 算法的解,从时刻 t 到 t+T:,Lecture 11-Quaternion,33,5.1 二阶 Runge-Kutta 算法,其二阶 Runge-Kutta 算法的解为:,对于四元数微分方程:,Lecture 11-Quaternion,34,5.1*四阶 Runge-Kutta 算法,对于四元数微分方程,Lecture 11-Quaternion,35,对于,5.2*角增量算法,其 Peano-Baker 解为:,其中,或写成迭代的形式:,Lecture 11-Quaternion,36,5.2 角增量算法,一阶增量算法:,Lecture 11-Quaternion,37,5.2 二阶到四阶增量算法,2nd:,3rd:,4th:,Lecture 11-Quaternion,38,Thanks,
