1、Lecture 11 - Quaternion1ReviewComputing a vehicles attitude by solving CCwhere000 xyxzyz220000)(cos1sin)()(ItCttCusing Peano-Baker solutionor using numerical integration. xyzENLecture 11 - Quaternion2Quaternions in SINS四元数在惯性导航系统中的应用四元数在惯性导航系统中的应用Lecture 11 - Quaternion3Outline四元数的定义四元数的定义四元数的运算四元数的
2、运算利用四元数进行旋转变换利用四元数进行旋转变换利用四元数进行旋转合成利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解捷联惯导系统基于四元数的姿态微分方程求解Lecture 11 - Quaternion41.0 Hamilton 和四元数和四元数四元数四元数: 描述刚体的转动描述刚体的转动 (by Hamilton)理论上的突破理论上的突破 - 1843.10.161222ijkkji2005 朝拜之旅朝拜之旅“Here as he walked by on the 16th of October 1843, Sir William Rowan Hamilton in a fl
3、ash of genius discovered the fundamental formula for quaternion multiplication carved on a stone on the bridge ”1222ijkkji在捷联惯性导航及图像处理中应用的优势在捷联惯性导航及图像处理中应用的优势Lecture 11 - Quaternion5n1.1*四元数四元数(quaternions)定义定义一个有固定点的刚体通过绕该点的某个轴转一个有固定点的刚体通过绕该点的某个轴转过特定角度可达到任何姿态过特定角度可达到任何姿态转轴的方向可以表示成一个单位矢量转轴的方向可以表示成一个
4、单位矢量: 则描述该转动的四元数可以表示成则描述该转动的四元数可以表示成:kjincoscoscosnq2sin2coskjicos2sincos2sincos2sin2cos四元数既反映了转动的方向又反映了转动的幅值四元数既反映了转动的方向又反映了转动的幅值.Lecture 11 - Quaternion61.2 四元数的组成四元数的组成 四元数的表示：四元数的表示：kjiqcos2sincos2sincos2sin2cos1P2P3PkPjPiPq321 - 标量部分标量部分kPjPiP321- 矢量部分矢量部分 包括一个实数单位包括一个实数单位 1 和三个虚数单位和三个虚数单位 i, j
5、, k 另一种表示法另一种表示法: Pq, P 代表矢量部分代表矢量部分Lecture 11 - Quaternion7Outline四元数的定义四元数的定义四元数的运算四元数的运算利用四元数进行旋转变换利用四元数进行旋转变换利用四元数进行旋转合成利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解捷联惯导系统基于四元数的姿态微分方程求解Lecture 11 - Quaternion82.1*加法和减法加法和减法kPjPiPq321kjivM321加法和减法：加法和减法： MqkPjPiPv)()()()(332211或简写成：或简写成： PvMq,Lecture 11 - Quat
6、ernion92.2 虚数单位的乘法规则虚数单位的乘法规则kPjPiPq321ijki, j, k 在乘法运算中的规则在乘法运算中的规则: 1kkjjiiijkjijkikjkijik1222ijkkji对比对比 Hamilton 的公式的公式Lecture 11 - Quaternion102.3*四元数乘法四元数乘法或简单地表示成：或简单地表示成： PvPPvMq)()(kjivkPjPiPMq321321)(332211PPPviPPvP)(233211jPPvP)(311322kPPvP)(122133321123132231321vPPPPPPPPPPPP32112313223132
7、1PPPvvvvLecture 11 - Quaternion112.3 四元数乘法自定义函数四元数乘法自定义函数function q1=qmul(q, m)function q1=qmul(q, m)lm=q(1); p1=q(2); p2=q(3); p3=q(4);lm=q(1); p1=q(2); p2=q(3); p3=q(4);q1=lm -p1 -p2 -p3q1=lm -p1 -p2 -p3 p1 lm -p3 p2 p1 lm -p3 p2 p2 p3 lm -p1 p2 p3 lm -p1 p3 -p2 p1 lm p3 -p2 p1 lm* *m;m; a=1 2 2 3
8、; a=1 2 2 3; b=2 4 2 3; b=2 4 2 3; q=qmul(a,b) q=qmul(a,b)q =q = -0.7796 -0.7796 0.3282 0.3282 0.4924 0.4924 0.2052 0.2052 Lecture 11 - Quaternion122.3*四元数乘法表示符号四元数乘法表示符号)()(kjivkPjPiPMq321321四元数乘法的符号四元数乘法的符号MqMqMqqM关于交换率和结合律关于交换率和结合律qMMq)()(321321qqqqqqLecture 11 - Quaternion132.4*共扼和范数共扼和范数共扼四元数的定
9、义共扼四元数的定义 - 两个四元数的标量部分相同，向两个四元数的标量部分相同，向量部分相反量部分相反kPjPiPq321kPjPiPq321*q 和和 q* 彼此互为四元数彼此互为四元数.可以证明：可以证明： *)*(qhhq四元数的范数四元数的范数 q- 定义成定义成 23222122PPP*qqq1q, 则则 q 成为规范化的四元数成为规范化的四元数 若若kjiqcossincossincossincos2222是规范化的是规范化的Lecture 11 - Quaternion142.5*四元数的逆和除法四元数的逆和除法121qq 若若则则 q1 和和 q2 彼此互为逆彼此互为逆, 写为写
10、为121 qq112 qq和和因为因为2q*qq12q*qq21q*qq1q*1qq除法除法:Mhq1hMqMqh1hMq 没有具体意义没有具体意义或或qMh function qi =qinv(q)function qi =qinv(q)% inverse of quaternion% inverse of quaternionqn=norm(q);qn=norm(q);q(2:4)=-q(2:4);q(2:4)=-q(2:4);qi=q/qn2;qi=q/qn2;Lecture 11 - Quaternion15Outline四元数的定义四元数的定义四元数的运算四元数的运算利用四元数进行旋
11、转变换利用四元数进行旋转变换利用四元数进行旋转合成利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解捷联惯导系统基于四元数的姿态微分方程求解Lecture 11 - Quaternion163.1*矢量的旋转矢量的旋转XYZnRR如果矢量如果矢量 R 相对固定坐标系旋转相对固定坐标系旋转, 并且该旋并且该旋转可以用四元数转可以用四元数 q 描述，新矢量记为描述，新矢量记为 R， 则则 R 和和 R 之间的变换可以表示成下述四元之间的变换可以表示成下述四元数运算数运算:1qRqR含义含义: 矢量矢量 R 相对固定坐标系旋转，相对固定坐标系旋转，旋转的角度和轴向由旋转的角度和轴向由 q
12、 决定决定上述运算中上述运算中, R 被当成一个标被当成一个标量部分为零的四元数，即：量部分为零的四元数，即：kRjRiRRzyx 0Lecture 11 - Quaternion173.2*坐标系的旋转坐标系的旋转XYZ一个矢量一个矢量 V 相对于坐标系相对于坐标系 OXYZ 固定固定 :VzkyjxiV从坐标系从坐标系 OXYZ 转动了转动了 q, 得到一个得到一个新坐标系新坐标系 OXYZ .nXYZV 分解在新坐标系分解在新坐标系 OXYZ 中中 kzjyixV矢量矢量 V 在两个坐标系之间的坐标变换在两个坐标系之间的坐标变换:记：记： kzjyixVezkyjxiVeqVqVee1则
13、则分别称为分别称为 V 在两个坐在两个坐标系中的映像标系中的映像.eV和和eVLecture 11 - Quaternion183.3 四元数和方向余弦四元数和方向余弦qVqVee1- 表示坐标系旋转表示坐标系旋转, 其中其中kzjyixVezkyjxiVezyxCzyxq 和和 C 之间是什么关系之间是什么关系 ?kPjPiPq321kPjPiPq3211假设假设则则kzjyixVe)(kPjPiP321)(zkyjxi0)(kPjPiP321应用四元数乘法应用四元数乘法, 得到得到Lecture 11 - Quaternion193.3 四元数和方向余弦四元数和方向余弦)(kPjPiP32
14、1)0(zkyjxi)(kPjPiP321kzjyixVeC方向余弦矩阵方向余弦矩阵zPPPyPPPxPPPx)(2)(2)(2313212322212zPPPyPPPxPPPy)(2)()(21322321222321zPPPyPPPxPPPz)()(2)(22221232132231zyxPPPPPPPPPPPPPPPPPPPPPPPPPPPzyx222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2Lecture 11 - Quaternion203.4 四元数转动变换的两种形式四元数转动变换的两种形式 如果一个矢量如果一个矢量
15、 V 固定，坐标系旋转按照四元数固定，坐标系旋转按照四元数 q 进行了旋转，进行了旋转，得到了一个新坐标系，则该矢量分别在新旧坐标系中投影表达得到了一个新坐标系，则该矢量分别在新旧坐标系中投影表达式间的关系借助映像方式可以表示为：式间的关系借助映像方式可以表示为：qVqVee1如果一个坐标系固定如果一个坐标系固定, 一个矢量一个矢量 VE 按照四元数按照四元数 q 相对该坐标系相对该坐标系进行了转动，得到一个新的矢量进行了转动，得到一个新的矢量 VE，则新旧矢量之间的关系，则新旧矢量之间的关系为：为：1qVqVEELecture 11 - Quaternion21Outline四元数的定义四元
16、数的定义四元数的运算四元数的运算利用四元数进行旋转变换利用四元数进行旋转变换利用四元数进行旋转合成利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解捷联惯导系统基于四元数的姿态微分方程求解Lecture 11 - Quaternion224.0*转动四元数的合成转动四元数的合成连续的多次转动可以等效成一次转动连续的多次转动可以等效成一次转动.XYZZXYZXY假设四元数假设四元数 q1 和和 q2 分别代表第一次和第分别代表第一次和第二次坐标系旋转二次坐标系旋转.q1q2则合成后的转动四元数则合成后的转动四元数 q 可以表示成可以表示成: q21qqq其中其中 q1 和和 q2 的
17、轴必须表示成映像形式的轴必须表示成映像形式.若若 q1 和和 q2 的轴都表示在原来坐标系中的轴都表示在原来坐标系中, 则则12qqqLecture 11 - Quaternion234.1四元数合成例子四元数合成例子: 非映像方式非映像方式坐标系顺序旋转情况下四元数的合成坐标系顺序旋转情况下四元数的合成坐标系坐标系 OXYZ 相对坐标系相对坐标系 OXYZ 多次旋转多次旋转首先首先, 绕绕 Z 轴转过角度轴转过角度 , 瞬时转轴瞬时转轴 n 和和 k 轴重合轴重合, 则则 kn 1kq221sincosLecture 11 - Quaternion24第二次旋转第二次旋转: 绕绕 X 转过角
18、度转过角度, 旋转轴旋转轴 n 表示为表示为: 4.1 四元数合成四元数合成: 非映像方式非映像方式 in 2jisincos对应的四元数对应的四元数: 2222nqsincos)sin(cos2sin2cosjiLecture 11 - Quaternion25kq221sincos4.1 四元数合成四元数合成: 映像的方式映像的方式)sin(cossincosjiq222这里这里 q1 和和 q2 的转动轴表示为非映像的的转动轴表示为非映像的形式形式, 因此合成的四元数为因此合成的四元数为:12qqqkji2222sincos)sin(cossincos转动次数越多，合成后四元数的表达式会
19、愈发复杂转动次数越多，合成后四元数的表达式会愈发复杂Lecture 11 - Quaternion264.2*四元数合成四元数合成: 映像的方式映像的方式每次转动的瞬时转轴都以映像方式给出每次转动的瞬时转轴都以映像方式给出.对第一次转动对第一次转动, 瞬时转轴瞬时转轴 n1 的映像的映像方式和非映像方式相同：方式和非映像方式相同：knne11kq2sin2cos1因此因此Lecture 11 - Quaternion274.2 四元数合成四元数合成: 映像的方式映像的方式第二次转动绕着第二次转动绕着 OX 转过了转过了转轴转轴 n2 沿着沿着 OX in 2n2 在坐标系在坐标系 XYZ 中的
20、映像为：中的映像为：ine2因此因此 q2 的映像形式为的映像形式为 2222enqsincosi 22sincosLecture 11 - Quaternion284.2 四元数合成四元数合成: 映像的方式映像的方式第三次转动绕着第三次转动绕着 OY 轴转过轴转过 .转轴转轴 n3 沿着沿着 OY jn 3轴轴 OY 是由原来坐标系的是由原来坐标系的 OY 轴转轴转动得到的，因此动得到的，因此 n3 的映像形式为的映像形式为:jne3这样，四元数这样，四元数 q3 的映像形式为的映像形式为3322enqsincosj22sincosLecture 11 - Quaternion294.2 四
21、元数合成四元数合成: 映像的方式映像的方式因为因为 q1, q2 和和 q3 都表示成了映像的都表示成了映像的形式形式, 所以合成的四元数所以合成的四元数 q 的计算公的计算公式为式为:321qqqqjik222222sincossincossincos由由 q 可以进一步得到合成转动对应的方向余弦矩阵可以进一步得到合成转动对应的方向余弦矩阵Lecture 11 - Quaternion30Outline四元数的定义四元数的定义四元数的运算四元数的运算利用四元数进行旋转变换利用四元数进行旋转变换利用四元数进行旋转合成利用四元数进行旋转合成捷联惯导系统基于四元数的姿态微分方程求解捷联惯导系统基于
22、四元数的姿态微分方程求解Lecture 11 - Quaternion315.0*姿态解算的四元数微分方程姿态解算的四元数微分方程 CC对对SINS，如用方向余弦矩阵，如用方向余弦矩阵, 微分方程为微分方程为 如果用四元数如果用四元数, 其微分方程为其微分方程为qq21其中其中 q 为描述载体转动的四元数为描述载体转动的四元数, 为载体相对导航参考坐标系为载体相对导航参考坐标系的角速度的角速度, 也表示为四元数的形式也表示为四元数的形式: kjizyx 0则则: 321321000021PPPPPPxyzxzyyzxzyxqqb21或或- 只包含四个一阶微分方程只包含四个一阶微分方程Lectu
23、re 11 - Quaternion325.1一阶一阶 Runge-Kutta 算法算法假设捷联惯导的假设捷联惯导的 “数学平台数学平台” 跟踪地理坐标系跟踪地理坐标系, 则则biEbibbEb四元数微分方程及简化四元数微分方程及简化)()(21)(tqtqbiEbib)(21)(tqtqb利用利用 Runge-Kutta 数值积分算法数值积分算法一阶一阶 Runge-Kutta 算法的解算法的解, 从时刻从时刻 t 到到 t + T:)()(21tqtKbTKtqTtq)()(Lecture 11 - Quaternion331K2Kqqb21tTt 5.1 二阶二阶 Runge-Kutta
24、 算法算法其二阶其二阶 Runge-Kutta 算法的解为算法的解为:对于四元数微分方程对于四元数微分方程:qqb21)()(211tqtKb11TKtqq)(12)(21qTtKb)(2)()(21KKTtqTtqLecture 11 - Quaternion341K2K3K4Kqqb21tTt 5.1*四阶四阶 Runge-Kutta 算法算法)()(211tqtKb对于四元数微分方程对于四元数微分方程qqb21112KTtqq)(12)2(21qTtKb222KTtqq)(23)2(21qTtKb33TKtqq)(34)(21qTtKbTKKKKtqTtq6224321)()(Lectu
25、re 11 - Quaternion35)(21)(tqtqb对于对于5.2*角增量算法角增量算法)(sincos)(022000qItq其其 Peano-Baker 解为解为:其中其中21ttbdt0000 xyzxzyyzxzyx22220ZYX或写成迭代的形式或写成迭代的形式:)(sincos)(nqInq000221Lecture 11 - Quaternion36SC)(sincos)(nqInq0002215.2 角增量算法角增量算法orderCS12341482120218120218120384814020482120一阶增量算法一阶增量算法:)()(nqInq211)(nqXYZXZYYZXZYX222221Lecture 11 - Quaternion375.2 二阶到四阶增量算法二阶到四阶增量算法)()(nqSICnq1orderCS123414821202181202181203848140204821202nd :)()()(nqInq21811203rd :)()()()(nqInq482181120204th :)()()()(nqInq4821384811204020Lecture 11 - Quaternion38Thanks
