1、惯性导航系统原理惯性导航系统原理3 捷联式惯导系统捷联式惯导系统 程向红程向红 2010.03.192010-03-1923 捷联式惯导系统捷联式惯导系统3.1 捷联式惯导算法概述捷联式惯导算法概述3.2 姿态矩阵的计算姿态矩阵的计算3.3 姿态矩阵计算机执行算法姿态矩阵计算机执行算法2010-03-1933.1 捷联式惯导算法概述捷联式惯导算法概述捷 联 式 惯 导 算 法 bibf bibP,R,H,L,VE,VN捷联式惯导航系统是一个信息处理系统，就是将载体上安装的惯性捷联式惯导航系统是一个信息处理系统，就是将载体上安装的惯性 仪表所测量的载体运动信息，经过计算处理成所需要的导航信息。仪
2、表所测量的载体运动信息，经过计算处理成所需要的导航信息。b姿态矩 阵 计算 加速度计组 导 航 计算机 VE初始条件 bSFVNnCbnSFbintHP R陀 螺仪组 ib捷联式惯性导航系统捷联式惯性导航系统=信息处理系统信息处理系统根据捷联式惯导的应用和功能要求不同，计算的内容和要根据捷联式惯导的应用和功能要求不同，计算的内容和要 求，有很大的差别。常有求，有很大的差别。常有SINSStrapdown Inertial Navigation Systems SVRUStrapdown Vertical Reference Uint SAHRSStrapdown Attitude and He
3、ading Reference Systems IMUInertial measurement Unit捷 联 式 惯 导 算 法bibfibbP,R,HEN,L,V ,V2010-03-19接联式惯导的算法的基本内容接联式惯导的算法的基本内容（1）系统的启动和自检系统的启动和自检测测（2）系统初始化系统初始化（3）惯性仪表的误差补惯性仪表的误差补偿偿（4）姿态矩阵的计算姿态矩阵的计算（5）导航计算导航计算（6）制导和控制信息的制导和控制信息的提取提取2010-03-19（1）系统的启动和自检测）系统的启动和自检测系统启动后，各个部分的工作是否正常，要系统启动后，各个部分的工作是否正常，要 通
4、过自检测程序加以检测，其中包括电源、惯通过自检测程序加以检测，其中包括电源、惯 性性仪表、计算机以及计算机软件。仪表、计算机以及计算机软件。通过自检测，发现有不正常，则发出告警信息通过自检测，发现有不正常，则发出告警信息(或或 故障码故障码)。系统的自检测是保证系统进入导航。系统的自检测是保证系统进入导航状态状态 后能正常工作、提高系统可靠性的措施。后能正常工作、提高系统可靠性的措施。2010-03-19（2）系统初始化）系统初始化为何要初始化？为何要初始化？给定载体（舰船、飞行器、车辆等）的给定载体（舰船、飞行器、车辆等）的初始位置初始位置（经度和纬度）和（经度和纬度）和初始速度初始速度等初
5、始信息。等初始信息。导航平台的初始对准导航平台的初始对准惯性仪表的校准惯性仪表的校准Calibration平台式平台式姿态矩阵的初始值姿态矩阵的初始值用物理的方法来实现用物理的方法来实现标度系数标度系数加速度计加速度计捷联式捷联式陀螺仪陀螺仪进行测定进行测定漂移漂移 偏置偏置2010-03-19（3）惯性仪表的误差补偿）惯性仪表的误差补偿对捷联式惯导系统来说，由于惯性仪表直接安装对捷联式惯导系统来说，由于惯性仪表直接安装 在载体上，因此，载体的线运动和角运动都引起在载体上，因此，载体的线运动和角运动都引起 较大的误差。较大的误差。为了保证系统的精度，必须对惯性仪表的误差进为了保证系统的精度，必
6、须对惯性仪表的误差进行行 补偿，最好的补偿方法是计算机补偿。补偿，最好的补偿方法是计算机补偿。在计算机中通过专用的软件来实现误差补偿。在计算机中通过专用的软件来实现误差补偿。2010-03-19（4）姿态矩阵的计算）姿态矩阵的计算姿态矩阵的计算是捷联式惯导算法中最重要的一姿态矩阵的计算是捷联式惯导算法中最重要的一 部分，也是捷联式系统所部分，也是捷联式系统所特有的特有的。不管捷联式惯导应用和功能要求如何，姿态矩阵不管捷联式惯导应用和功能要求如何，姿态矩阵 的计算却是不可少的。姿态矩阵算法是本章重点的计算却是不可少的。姿态矩阵算法是本章重点 讨论的内容。讨论的内容。2010-03-19（5）导航
7、计算）导航计算导航计算就是把加速度计的输出信息变换到导航坐导航计算就是把加速度计的输出信息变换到导航坐 标系，然后，计算载体速度、位置等导航信息。标系，然后，计算载体速度、位置等导航信息。2010-03-19（6）制导和控制信息的提取）制导和控制信息的提取制导和控制信息的提取，载体的制导和控制信息的提取，载体的姿态姿态既可用来既可用来 显示也是控制系统最基本的控制信息。显示也是控制系统最基本的控制信息。此外，载体的此外，载体的角速度角速度和和线速度线速度信息也都是控制信息也都是控制 载体所需要的信息。载体所需要的信息。这些信息可以从这些信息可以从姿态矩阵的元素和陀螺加速度姿态矩阵的元素和陀螺加
8、速度计的输出中计的输出中提取出来。提取出来。2010-03-19捷联式惯导系统算法流程图捷联式惯导系统算法流程图启动启动 自自 检检 测测 初初 始始 化化 姿态姿态阵计阵计算算 迭迭 代代 次次 数数 控控 制制 信信 息息 提提 取取 返回返回92010-03-19YES导导 航航 计计算算 NO2010-03-19133.2 姿态矩阵的计算姿态矩阵的计算捷联式惯导中，载体捷联式惯导中，载体地理位置地理位置就是地理坐标系相对就是地理坐标系相对 地球坐标系的方位。而载体的地球坐标系的方位。而载体的姿态和航向姿态和航向则是则是载体载体 坐标系坐标系相对于相对于地理坐标系地理坐标系的方位关系。确
9、定两个坐的方位关系。确定两个坐 标系的方位关系问题，是力学中的刚体定点转到理标系的方位关系问题，是力学中的刚体定点转到理 论。在刚体定点转动理论中，描述动坐标系相对参论。在刚体定点转动理论中，描述动坐标系相对参 考坐标系方位关系的方法有多种。考坐标系方位关系的方法有多种。四参数法四参数法 1843年发明的，首先在数学中引入四元数，以年发明的，首先在数学中引入四元数，以 后用在刚体定位问题。凯里后用在刚体定位问题。凯里.克莱茵（克莱茵（Cayley-Klein）参数法，是在）参数法，是在1897年提出的。年提出的。九参数法九参数法基于方向余弦的概念，也称基于方向余弦的概念，也称 方向余弦法方向余
10、弦法。三参数法三参数法欧拉角法欧拉角法，是欧拉在，是欧拉在1776年提出的。年提出的。四元数法四元数法。威廉。威廉.哈密顿哈密顿(William Hamilton)在在等效转动矢量法等效转动矢量法3.2 姿态矩阵的计算姿态矩阵的计算3.2.1 欧拉角法欧拉角法3.2.2 方向余弦法方向余弦法3.2.3 四元数法四元数法3.2.4 等效转动矢量法等效转动矢量法2010-03-193.2.1 欧拉角法欧拉角法XbENU作为作为参考坐标系参考坐标系，则航向，则航向 角角H，纵摇角（俯仰角），纵摇角（俯仰角）P和横和横 摇角（横滚角、倾斜角）摇角（横滚角、倾斜角）R。就。就 是一组欧拉角。是一组欧拉角
11、。欧拉角没有严格的定义，根欧拉角没有严格的定义，根 据据需要，可以选用不同的欧拉需要，可以选用不同的欧拉 角角组。第一次转动，可以绕三组。第一次转动，可以绕三 个个轴中的任一个转动，故有轴中的任一个转动，故有3种种 可可能，第二次有能，第二次有2种可能，第三种可能，第三 次次也有也有2种可能。总共有种可能。总共有12种可种可 能。能。EXbOUNH.ZbYbXb YYbZbbZbP.R.HPR一个一个动坐标系动坐标系相对相对参考坐标系参考坐标系的方位，完全可以由动坐的方位，完全可以由动坐 标系依次绕标系依次绕3个不同的轴转动的个不同的轴转动的3个转角来确定。个转角来确定。如把如把OXbYbZb
12、作为作为动坐标系动坐标系，2010-03-192010-03-1916用欧拉角表示的姿态矩阵用欧拉角表示的姿态矩阵001U 0N 0 E Y sin H cos Hb Xb sin Hcos HZ v-_CHb0 sin Pcos P Z 00cos Psin P Yb b X v-_ 0 1 Z b X YbbCPbcos R Z Yb b 0 sin R X 01 v-_sin R0 Yb cos Rb Z X0bCRbcos P cos R cos P sin Rsin R cos H sin P cos R sin H cos R cos H sin P sin R sin Hcos
13、R sin H sin P sin R cos Hcos P cos Hsin R sin H sin P cos R cos Hcos P sin Hsin Pb nCEX bOU ZbbNH.Y XbX bY YbbZ bbZP.R.HPRHPR欧拉角微分方程欧拉角微分方程表示表示载体坐标载体坐标系系相对相对地理坐标系地理坐标系的角的角 速度矢量在载体坐标系速度矢量在载体坐标系 轴向的分量构成的列矩轴向的分量构成的列矩 阵。阵。EXbOU ZbbNH.Y XbX bY YbbZ bbZP.R.HPRbnb 0 0 R.0 0 P.R 0 CH.0 RP C Cnby nbz b bbnbx
14、HPR2010-03-19欧拉角微分方程欧拉角微分方程cos P cos R H.R.0 sin R cos P P.010sin Rnby nbz cos Rnbx b bsin Pbnby nbz bcos P cos R sin R R.cos R0 sin R cos P01sin P0H.P.1bb nbx cos R sin Pnby nbz bsin R cos P b R.cos P sin P sin R 1 cos P cos R0 cos P0sin RH.P.bnbxcos RbCn求解微分方程求解微分方程3个欧拉角个欧拉角航向角航向角(H)姿态角姿态角(P,R)201
15、0-03-192010-03-1919欧拉角法应用中的问题欧拉角法应用中的问题求解方程可以直接得到航向和姿态信息，欧 拉角法得到的姿态阵永远是正交阵，用这个矩 阵将比力fbfn信息的坐标变换时，变换后的信 息中不存在非正交误差。因此，用欧拉角法得 到的姿态矩阵无需进行正交化处理。欧拉角微分方程中包含三角函数的运算，给实时计算带来困难，当P=90。时，方程式 出现“奇点”，使计算溢出。cos P cos R0 cos P0sin R cos P R 1 sin P sin Rcos R sin Pcos P sin Rcos Rb nbxbnby b nbz P.H.返回返回3.2垂垂直直发发射
16、射困困难难！3.2.2 方向余弦法方向余弦法方向余弦表示的姿态矩阵方向余弦表示的姿态矩阵方向余弦法方向余弦法用矢量的方向余弦来表示姿态矩阵的方法。用矢量的方向余弦来表示姿态矩阵的方法。用用in,jn,kn表示沿表示沿地理坐标系地理坐标系轴向的单位矢量。轴向的单位矢量。ib,jb,kb沿沿载体坐标系载体坐标系轴向的单位矢量。轴向的单位矢量。ib在地理坐在地理坐 标系内的方位完全可以由标系内的方位完全可以由ib的三个方向余弦来确定，其的三个方向余弦来确定，其 表达式为表达式为ib (ib in)in (ib jn)jn (ib kn)kncos(ib in)jb (jb in)in (jb jn)
17、jn (jb kn)knkb (kb in)in (kb jn)jn (kb kn)kn2010-03-19方向余弦法方向余弦法kb kn kn n j jj k jib kn in n kb in j j i ib inkb jnib jnkb b ib bbnbnkb b b j ib kn n jn in b Cbnnkb kn bn j jj kib kn kb in j i ib inkb jnib jnbnbnCbn写成矩阵形式为：写成矩阵形式为：2010-03-19矢量的坐标变换矢量的坐标变换旋转矢量的坐标变换旋转矢量的坐标变换固定矢量的坐标变换固定矢量的坐标变换固定矢量的坐标变
18、换是固定矢量的坐标变换是一个在空间大小和方向都不一个在空间大小和方向都不 变的矢量变的矢量在在两个不同方位的坐标系轴向分量之间的变两个不同方位的坐标系轴向分量之间的变 换关系换关系，也即同一个矢量在两个不同的坐标系轴向投，也即同一个矢量在两个不同的坐标系轴向投 影之间的变换关系。影之间的变换关系。是指是指一个矢量大小不变一个矢量大小不变，但在，但在方向上转动方向上转动了一个位了一个位 置，这个矢量转动置，这个矢量转动前前和转动和转动后后在同一个坐标系轴向在同一个坐标系轴向 分量之间的变换关系。分量之间的变换关系。2010-03-19固定矢量的坐标变换固定矢量的坐标变换 Z k r bT br
19、X bib Yb jbbbb:载体坐标系载体坐标系 n:地理坐标系地理坐标系一个矢量一个矢量r，写成，写成载体坐标系轴向载体坐标系轴向分量形式：分量形式：Z k r nT nr X nin Yn jnnn同一个矢量同一个矢量r，如果写成，如果写成地理坐标系轴向地理坐标系轴向分量形式：分量形式：r bT b r nT n Zb b b j Xb r b Ykb b ib Zn n X n Yr nn jn kn in b Cb nnbTbTbnTnr b r C n r nrbT Cb r nTn由于由于r是同一个矢量，故是同一个矢量，故由于正交阵，故由于正交阵，故n bbTb1n(Cn )(C
20、)C两边求转置两边求转置nTTb TbTT(Cn)(r)(r)Cn r bbr n Cb r nnr b2010-03-19旋转矢量的坐标变换由于动坐标系随同矢量转动，故由于动坐标系随同矢量转动，故rbT=rnT互逆互逆r转动前的矢量转动前的矢量r 转动后的矢量假定有一个动坐标系和矢量固连，在矢量转动 前，取动坐标系b和参考坐标系n重合，则：r=rnTnb Cb nr r nT Cb nnnr=rbTb如果用如果用r n表示转动表示转动后后的矢量在的矢量在参考坐标系参考坐标系轴向的轴向的 分量构成的矩阵，则分量构成的矩阵，则r rnT nrnT r nT Cbn C n r nbrn Cb r
21、 nnr b由于坐标系不动而是矢量转动，它由于坐标系不动而是矢量转动，它相应于矢量固定时坐标系方向转动相应于矢量固定时坐标系方向转动nTbn rC n2010-03-192010-03-1925方向余弦矩阵微分方程方向余弦矩阵微分方程由矢量相对导数和绝对导数的关系式由矢量相对导数和绝对导数的关系式 rdr drdtdtnbnb假定地理坐标系为假定地理坐标系为参考坐标系参考坐标系，作为参考，作为参考 坐标系认为它在空间是不动的，即坐标系认为它在空间是不动的，即 0ndtdr nb rdr dtb b r bk rnbb nbbbr.nbx nb nbz0 nbznby0 0nbynbxbkbnb
22、载体坐标系载体坐标系相对相对地理坐标系地理坐标系的转动角速度在的转动角速度在b系轴系轴 向分量的向分量的反对称矩阵反对称矩阵（Skew symmetric matrix）2010-03-1926方向余弦矩阵微分方程方向余弦矩阵微分方程另外，从固定矢量的坐标变换关系式有另外，从固定矢量的坐标变换关系式有 C.br n Cbr.n nnr.b Cb r nnr b两边求导两边求导 0r.br.br.n C.br n C.bCnrbnnb考虑考虑bn Cn Cbbknb.b两边同右乘两边同右乘Cnbkb nb Cnb nC.nb rb nb rbbbknbk Cb nbnC.bbk nb(bk)Tn
23、bbk1(nb)返回返回3.2方向余弦矩阵微分方程的方向余弦矩阵微分方程的几种表示形式几种表示形式bkb nb Cnb nn bC.nbkbnb CC.式中的角速度都是用式中的角速度都是用载体坐标系内载体坐标系内的分量的分量 表示的，如果角速度在表示的，如果角速度在地理坐标系轴向地理坐标系轴向的的 分量表示时，则可用角速度反对称矩阵的分量表示时，则可用角速度反对称矩阵的 相似变换来得到。相似变换来得到。bnknnnbbbk nb C Cnbkbbnbnnk nb CC左式可以用展开的方式推导左式可以用展开的方式推导bnknnbb n C C.nknnbnbb CC.在捷联惯导系统中，由于陀螺是
24、固联于载体上的，在捷联惯导系统中，由于陀螺是固联于载体上的，所以直接测量的角速度是所以直接测量的角速度是载体坐标系轴向载体坐标系轴向的分量。的分量。那么计算时哪个公式最方便？那么计算时哪个公式最方便？常用的姿态矩阵微分方程的常用的姿态矩阵微分方程的4种形式。种形式。2010-03-19方向余弦矩阵微分方程方向余弦矩阵微分方程陀螺仪测量的是载体相对于惯性空间的角速度陀螺仪测量的是载体相对于惯性空间的角速度 bnnbkbbnb CC.ib而式中需要的则是而式中需要的则是bk nb两者的关系为：两者的关系为：bk inbk ibbk nb bnknnibbbk ibC C bk)n(bk ibinb
25、n b CC.nkninbnbkbibC C 包括载体的包括载体的 姿态和航向姿态和航向 的变换角速的变换角速 度，数值较度，数值较 大（如飞机大（如飞机 可达可达400。/s）则是地球角速则是地球角速 度和载体的位度和载体的位 移运动相对地移运动相对地 心形成的角速心形成的角速 度，这个角速度，这个角速 度比较小，一度比较小，一 般为每小时几般为每小时几 十度。十度。在实时计算上式时，第一在实时计算上式时，第一项需要用较高的速度计算项需要用较高的速度计算，用迭代算法时，迭代频，用迭代算法时，迭代频 率要高，而第二项则可用率要高，而第二项则可用 较低迭代频率计算。可以较低迭代频率计算。可以 看
26、作是对第一项的修正。看作是对第一项的修正。2010-03-192010-03-19293.2.2.4 矩阵微分方程的解矩阵微分方程的解下面是解方程的推导过程。下面是解方程的推导过程。C (t)C(0)C(t)dtnbkbnbb0C (t)C(0)C(0)C (t)dtdttnbkbknbnbtbnb00C (t)dtdt C(0)C(0)dt tttbknbbknbnbbknb 000把等式右边的表达式逐次代入积分号内把等式右边的表达式逐次代入积分号内 dtdt C (t)C(0)C(0)dt C(0)tttbknbbknbbknbnbttbknbbknbtbknbnbC (t)dtdtdt0
27、 0 00 00dtdtC(t)C(0)I dt tttbknbbknbbknbnb 000 C(t)dtdtdt .tttbknbbknbbknbnb000第第2次代入得次代入得这样不断的进行代入，便得到这样不断的进行代入，便得到bk 20120000 0(dt)dtdt dtddt tnbtbknbttbknbttbknbbknbbk 30160 0 0(dt)C (t)dtdtdt tnbtttbknbbknbbknbnbC.n Cn bk bbnb变系数的齐次微分方变系数的齐次微分方程程tn可可用毕卡用毕卡(Peano-Baker)逼近法求解，逼近法求解，积分上式积分上式则有则有第第1
28、次代入得次代入得C (t)C(0)I dt dt dt3 .061bk2 0021tbknbtnbtbk bnb故故nCb (t)C(0)etbknbdtn02010-03-1930矩阵微分方程的解矩阵微分方程的解Cb (t)C(0)etbknbdtn0C(t t)C(t)etn1tnbknb dtnbbknb dt nbtn1tnbkbkC n(t t)C(t)enbb b b 0 b b00bnbynbxb nbxnbznbynbzbknbbk212nb 3e K I K K()nbbkbknbI单位阵单位阵;K1,K2,K3系数。系数。t=tn+1-tn下面来求三个系数。由矩阵的特征方程
29、下面来求三个系数。由矩阵的特征方程如果知道了如果知道了K1,K2,K3三个系数，则三个系数，则矩阵指数函数矩阵指数函数就可以表示成就可以表示成 一个矩阵二次方程。一个矩阵二次方程。bknb来求它的特征值。来求它的特征值。b bdet(I bk)b b b bnbynbxnbxnbnbznbznby 2 0223bbnbzbnbynbx 20222 bnbzbnbybnbx3 2 001 =0 2,3=士士j 0令令将矩阵的特征值代入方程将矩阵的特征值代入方程=0，K1=1 K I K bk K(bk)212nb3nbenbbk用用201四四0-03参参-19数法。数法。31矩阵微分方程的解矩阵
30、微分方程的解=j 0=-j 0bk2nb3nb201e K I K K()bk bknb j0e e0K2 bk)2 200 sin 0 bk 1 cos 0 (nbnbe I bknb=0，K1=12e K1 K 2 j0 K3(j0)ej2 K1 K2 j0 K3(j0)j 02 2K1 2K3(0)2K2 0 j 0j 0e e()20 1 cos 03Kj0sin 0Cn(t t)C(t)I sin 0 bk 1 cos 0 (bk)2 200nbbnb矩阵微分方程的精确解矩阵微分方程的精确解这个精确解的前提条件是这个精确解的前提条件是nbkbbnb.nC C+bknb dt nbtn
31、1tnbk这个式子只有在这个式子只有在 t=tn+1-tn内角速度矢量内角速度矢量 nb方向不变的条件下才有意义，由于方向不变的条件下才有意义，由于转动转动 的不可交换性的不可交换性，当，当 nb方向随时间变化时，角速度的积分是无意义的。方向随时间变化时，角速度的积分是无意义的。用方向余弦用方向余弦法求解姿态矩阵避免了欧拉角法方程退化的现象，可以全姿态工作，但法求解姿态矩阵避免了欧拉角法方程退化的现象，可以全姿态工作，但 是，由于是，由于方向余弦矩阵具有九个元素，所有，解算矩阵微分方程时，实际上是结算方向余弦矩阵具有九个元素，所有，解算矩阵微分方程时，实际上是结算 九个联九个联立微分方程，一般
32、说来，计算工作量比较大，为了减小计算工作量，可以采立微分方程，一般说来，计算工作量比较大，为了减小计算工作量，可以采3.2.3 四元数法四元数法四元数理论是四元数理论是数学中数学中的一个古老的分支，的一个古老的分支，1943年由威廉年由威廉.哈密顿哈密顿(William Hamilton)首先提出，目点是研究空间首先提出，目点是研究空间 几何，一种类似平面问题中使用复数那样的方法。但是几何，一种类似平面问题中使用复数那样的方法。但是，这个理论建立以后，长期没有得到实际应用，直到空，这个理论建立以后，长期没有得到实际应用，直到空 间技术出现以后，特别是捷联式制导技术出现以后，这间技术出现以后，特
33、别是捷联式制导技术出现以后，这 一古老的数学分支，又重新受到人们的重视，得到了实一古老的数学分支，又重新受到人们的重视，得到了实 际的应用。际的应用。四元数的基本概念四元数的基本概念四元数是由四元数是由1个实数单位个实数单位1和和3个虚数单位个虚数单位i,j,k组成的含组成的含有有4个元的数，其形式为个元的数，其形式为Q (q0,q1,q2,q3)q0 q1i q2 j q3k q0 q标量标量 矢量矢量2010-03-193.2.3 四元数法四元数法3.2.3.1 四元数的基本概念四元数的基本概念3.2.3.2 四元数理论四元数理论3.2.3.3 矢量坐标变换的四元数描矢量坐标变换的四元数描
34、述述3.2.3.4 四元数和方向余弦矩阵的四元数和方向余弦矩阵的关系关系3.2.3.5 四元数微分方程四元数微分方程2010-03-19ZRe实轴Im 虚轴O z cos j z sin Z z1 jz2 z e jj 1u uxi u y j uz ku 1Z sin ki u Z sin juux Z sin Z Z cos q3zq2yq1q0 v-_ v-_ v-_ _ v-2010-03-193.2.3.1 四元数的基本概念四元数的基本概念在平面问题中，一个复数在平面问题中，一个复数Z=z1+jz2可以表示二维空间中的一个矢量可以表示二维空间中的一个矢量.四元数的基本概念四元数的基本
35、概念 Z cos ux sini uy sinj uz sink(Quaternions)Q ZZ cos q0Z ux sin q1Z uy sin q2Z uz sin q3 Q cos u sin Q q0 q1i q2 j q3 k Q eu由于它具有和复数类似的形式，可看作是复数的由于它具有和复数类似的形式，可看作是复数的推广，因此，也有推广，因此，也有“超复数超复数”之称。之称。四元数的四元数的3种表示形式种表示形式2010-03-19坐标系的等效转动坐标系的等效转动EXbOU ZbNH.YbXbXb YYbZbbZbP.R.HPRXbX ruYbYrbZZr2010-03-19四
36、元数的基本概念四元数的基本概念如果用如果用u表示表示欧拉轴向欧拉轴向的单位矢量，则动坐标系的方的单位矢量，则动坐标系的方 位，完全可由位，完全可由u和和 两个参数两个参数来确定。来确定。用用u和和 两个参两个参 数，可以构造一个四元数，数，可以构造一个四元数，1如果把如果把u写成分量的形式则：写成分量的形式则：Q cos u sin i u sin j u sin k2x2y2z 2 q0 q1i q2 j q3k2q0 cos2q1 ux sin2q2 u y sin12q3 uz sinQ cos u sin 22四元数是张量为四元数是张量为1的四元数，即的四元数，即Q (q 2 q 2
37、q 2 q 2)2 10123这样的四元数这样的四元数称作称作“规范化规范化”的四元数，而用来的四元数，而用来描描述刚体定点转动的四元数就称作变换四元数。述刚体定点转动的四元数就称作变换四元数。u e22010-03-193.2.3.2 四元数理论四元数理论四元数相等 如果两个四元数对应的元素相等，则两个四元数相等。四元数相加 0 1i 2 j 3 k m0 m1i m2 j m3k 0 m0 (1 m1)i (2 m2)j (3 m3)k对应元素相加对应元素相加则则四元数相加，服从一般四元数相加，服从一般加法的交换律和结合律加法的交换律和结合律，即，即 ()()（交换律）（交换律）（结合律）
38、（结合律）2010-03-19四元数理论四元数理论四元数四元数与标量相乘与标量相乘a a0 a1i a2 j a3k式中式中a标量标量各个元素分别乘以标量各个元素分别乘以标量(ab)(ba)a a(a b)a ba()a a（分配律）（分配律）（交换律）（交换律）（结合律）（结合律）2010-03-19四元数理论四元数理论四元数四元数与四元数相乘与四元数相乘 0 1i 2 j 3k m0 m1i m2 j m3 k。(0 1i 2 j 3 k)。(m0 m1i m2 j m3 k)0 m0 1m1 2 m2 3m3 0(m1i m2 j m3k)乘积的矢量形式乘积的矢量形式2010-03-19
39、 m0(1i 2 j 3k)ijk123m1m2m3。M 0m0 0m m0 m m2010-03-19 3 41四元数理论。0m0 1m1 2m2 3m3 i(0m1 1m0 2m3 3m2)j(0m2 2m0 3m1 1m3)k(0m3 3m0 1m2 2m1)乘积的四元数形式乘积的四元数形式n1 0m1 1m0 2m3 3m2n2 0m2 2m0 3m1 1m3n3 0m3 3m0 1m2 2m1n0 0m0 1m1 2m2 3m3nnn TQ(n)n3210 TQ()3210mmm TQ(m)m123000 1 2 3 01 101 223013223 3210 3 n m n m n
40、 m n m Q(n)M()Q(m)n0 m0m1 m2 m3 0 n1 m1m0m3m2 1 m3m0m2m1n2 m2m1 2 m0n3 m3 -矩阵四元数矩阵四元数Q(n)M (m)Q()矩阵的矩阵的“核核”2010-03-1942四元数理论M()和和M*(m)除元素不同外，其核互为转置。除元素不同外，其核互为转置。这种四元数乘积的矩阵形式，也可推广到三个这种四元数乘积的矩阵形式，也可推广到三个 以上的四元数乘积。如：以上的四元数乘积。如：Q(。)M()Q(。)M()M (P)Q(m)M (P)Q(。)M (P)M()Q(m)M()M (P)M (P)M()说明说明M*和和M具有可交换性
41、。而一般的矩阵相乘，则是不可交换的具有可交换性。而一般的矩阵相乘，则是不可交换的.Q(。)M()Q(。)M()M(m)Q(P)M(。)Q(P)M(。)M()M(m)Q(。)M (P)Q(。)M (P)M (m)Q()M (。)Q()M(。)M(P)M(m)顺序相乘顺序相乘逆序相乘逆序相乘类似正交阵的乘积的转置或方阵乘积的求逆，也是逆序类似正交阵的乘积的转置或方阵乘积的求逆，也是逆序四元数理论四元数理论。(。)。(。)。()。四元数的四元数的共轭共轭 如果一个如果一个四元数为四元数为八八=+则定义其则定义其共轭四元数为共轭四元数为*=结合律结合律分配律分配律推理推理1(八八+M)*=八八*+M*
42、四元数之和的共轭等于共轭之四元数之和的共轭等于共轭之和和推理推理2(八八 M)*=M*八八*两个四元数之积的共轭等于共轭两个四元数之积的共轭等于共轭四元数等于两个四元数共轭之积取相反的顺序。四元数等于两个四元数共轭之积取相反的顺序。四元数的四元数的范数范数四元数的范数定义为四元数的范数定义为22223210 2010-03-19四元数理论四元数理论=八八 八八*=八八*八八 八八=1的四元数称为规范化的四元数。的四元数称为规范化的四元数。八八 M =八八 M=M 八八 四元数的逆四元数的逆则则八八-1 八八=八八 八八-1122012322 2010-03-193.2.3.3 矢量坐标变换的四
43、元数描述矢量坐标变换的四元数描述一个矢量一个矢量r在参考坐标系（这里用地理坐标系作参在参考坐标系（这里用地理坐标系作参 考系）轴向的分量形式为考系）轴向的分量形式为r=xnin+yn jn+znkn式中式中xn,yn,zn为为r在地理坐标系轴向的分量。在地理坐标系轴向的分量。in,jn,kn为地理坐标系轴向的单位矢量。为地理坐标系轴向的单位矢量。用用xn,yn,zn把把r写成四元数形式即：写成四元数形式即：Rn=0+xni+yn j+znk=0+r Rn就叫做矢量就叫做矢量r在地理坐标在地理坐标系上的四元数影像。系上的四元数影像。i,j,k是四元数的虚数单位，而是四元数的虚数单位，而r则是四元
44、数的矢量则是四元数的矢量部分。部分。显然，如果认为显然，如果认为i,j,k和和in,jn,kn重合，则四元重合，则四元数的矢量部分就是三维空间的矢量数的矢量部分就是三维空间的矢量r本身。本身。2010-03-19旋转矢量的坐标变换旋转矢量的坐标变换定义定义假设矢量假设矢量r绕通过定点绕通过定点“O”的某一轴转动了一个的某一轴转动了一个 角度角度，则和，则和矢量固联的动坐标系矢量固联的动坐标系和和参考坐标系参考坐标系之间的之间的 变换四元数变换四元数为：为：22Q cos usin式中式中u为转轴方向为转轴方向的的单位矢量单位矢量。这个四元数的范数为。这个四元数的范数为Q q2 q2 q2 q2
45、 10123转动前的矢量用转动前的矢量用r表示，转动后的矢量用表示，转动后的矢量用r表示表示,则则r和和r的关系可由四元数来描述，即的关系可由四元数来描述，即称作称作“规范化规范化”的四元的四元数数.r Q。r。Q*Q*cos usin四元数的共轭四元数四元数的共轭四元数2 2黄黄式两边同时左乘式两边同时左乘Q*右乘右乘Q得得因为因为Q*。r。Q Q*。Q。r。Q*。QQ*。Q Q。Q*12 2(cos usin)。(cos usin)cos sin 1222222r Q*。r。Q2010-03-192010-03-1947证明证明AO当矢量当矢量r绕绕OO旋转时，矢端旋转时，矢端A在空间的轨
46、迹是在空间的轨迹是一个圆，这个一个圆，这个 圆平面和转轴垂直，圆心为圆平面和转轴垂直，圆心为O在在旋转轴上。在圆上取一点旋转轴上。在圆上取一点B，使使 AOB=90。，则按矢量关系有下列关系式：则按矢量关系有下列关系式：OrrABuAOOBOOuOur A OO(r u)uOO OA r因为因为a b=|a|b|cos OA=r OO=r(r u)u OB=u OA=u(r(r u)u)=u r (r u)u u=u ru u=0OB=u r2010-03-1948证明ABOAcosAOOBsinOOrrABuA如果如果Q=q0+q，R=r0+r则利用则利用式式可以写成矢量形式为：可以写成矢量
47、形式为：Q R=q0 r0+q0r+qr0 q r+q r利用上式将利用上式将Q r Q*展开展开v-_ v-_q0qQ cos usin22r=R=r0+r r0=0q0OA=r(r u)uOB=u rq sin (ur)cos r(ur)sin v-2 _ 2 v-_2八八=0 m0+0 m+m0 m+mOA=OA cos+OBsin =cos (r(r u)u)+u r sin r=OO+OA=(r u)u+cos r cos (r u)u+sin (u r)=(1 cos)(r u)u+cos r+sin (u r)Q。r cos r usin r(ur)sin2222010-03-1
48、949推导推导q 0 q0q 0 q*qq0(cos r (u r)sin )(usin )(cos r (u r)sin )(usin )2 v-2 _2 2 v-2 _2qq*qq*sin cos(u r)sin2 (u r)u cos(cos r (u r)sin )2 v-2 _ 2v-_ 2 2 v-_2Q。r。Q*(sin(u r)cosr (u r)sin)。(cos usin)22222q=sincos(u r)sin2(u r)u cos2r sincos(u r)sincos(u r)2222Q。R q0r0 q0r qr0 q r q rsin2 (u r)u sin c
49、os(r u)sin2 (u r)u222 2第一项与第五项相消，第六项为零第四项与第七项合并第一项与第五项相消，第六项为零第四项与第七项合并 sin2 (u r)u cos2 r sin(u r)sin2 (u r)u2222222q0q0，q证明Q r Q*sin2 (u r)u cos2 r sin(u r)sin2 (u r)u222利用利用Q r Q*sin2 (u r)u cos2 r sin(ur)sin2 (uu)r(u r)u 22 2 cos2 r sin2 r sin(ur)2sin2 (u r)u222r Q。r。Q*cos r sin(ur)(1 cos)(u r)u
50、r=OO+OA=(r u)u+cos r cos (r u)u+sin (u r)=(1 cos)(r u)u+cos r+sin (u r)(u r)u=(u u)r(u r)u2010-03-19固定矢量的坐标变换固定矢量的坐标变换如果矢量固定不动，而如果矢量固定不动，而动动坐标系相对坐标系相对参考参考坐标系转动了一个角度，坐标系转动了一个角度，则以四元数描述的矢量在两个坐标系上的分量的变换关系为则以四元数描述的矢量在两个坐标系上的分量的变换关系为r Q。r。Q*如果将四元数如果将四元数Q,R,M的四个元写成列矢量，即表示成的四个元写成列矢量，即表示成3.2.3.4 四元数和方向余弦矩阵的
