捷联惯性导航系统的解算方法.ppt

,惯性导航系统原理,3捷联式惯导系统程向红2010.03.19,2010-03-19,2,3捷联式惯导系统,3.1捷联式惯导算法概述3.2姿态矩阵的计算3.3姿态矩阵计算机执行算法,2010-03-19,3,3.1捷联式惯导算法概述,捷联式惯导算法,b,ib,fb,ib,P,R,H,L,VE,VN,捷联式惯导航系统是一个信息处理系统，就是将载体上安装的惯性仪表所测量的载体运动信息，经过计算处理成所需要的导航信息。,b,姿态矩阵计算,加速度计组,导航计算机,VE,初始条件,b,SF,VN,n,Cb,n,SF,b,in,t,HPR,捷联式惯性导航系统=信息处理系统,根据捷联式惯导的应用和功能要求不同，计算的内容和要求，有很大的差别。常有SINSStrapdownInertialNavigationSystemsSVRUStrapdownVerticalReferenceUintSAHRSStrapdownAttitudeandHeadingReferenceSystemsIMUInertialmeasurementUnit,捷联式惯导算法,b,ibfib,b,P,R,H,EN,L,V,V,2010-03-19,4,接联式惯导的算法的基本内容（1）系统的启动和自检测（2）系统初始化（3）惯性仪表的误差补偿（4）姿态矩阵的计算（5）导航计算（6）制导和控制信息的提取,2010-03-19,5,（1）系统的启动和自检测,系统启动后，各个部分的工作是否正常，要通过自检测程序加以检测，其中包括电源、惯性仪表、计算机以及计算机软件。通过自检测，发现有不正常，则发出告警信息(或故障码)。系统的自检测是保证系统进入导航状态后能正常工作、提高系统可靠性的措施。,2010-03-19,6,（2）系统初始化,为何要初始化？给定载体（舰船、飞行器、车辆等）的初始位置（经度和纬度）和初始速度等初始信息。,导航平台的初始对准,惯性仪表的校准Calibration,平台式,姿态矩阵的初始值,用物理的方法来实现,标度系数加速度计,捷联式,陀螺仪,进行测定,漂移偏置,2010-03-19,7,（3）惯性仪表的误差补偿,对捷联式惯导系统来说，由于惯性仪表直接安装在载体上，因此，载体的线运动和角运动都引起较大的误差。为了保证系统的精度，必须对惯性仪表的误差进行补偿，最好的补偿方法是计算机补偿。在计算机中通过专用的软件来实现误差补偿。,2010-03-19,8,（4）姿态矩阵的计算,姿态矩阵的计算是捷联式惯导算法中最重要的一部分，也是捷联式系统所特有的。不管捷联式惯导应用和功能要求如何，姿态矩阵的计算却是不可少的。姿态矩阵算法是本章重点讨论的内容。,2010-03-19,9,（5）导航计算,导航计算就是把加速度计的输出信息变换到导航坐标系，然后，计算载体速度、位置等导航信息。,2010-03-19,10,（6）制导和控制信息的提取,制导和控制信息的提取，载体的姿态既可用来显示也是控制系统最基本的控制信息。此外，载体的角速度和线速度信息也都是控制载体所需要的信息。这些信息可以从姿态矩阵的元素和陀螺加速度计的输出中提取出来。,2010-03-19,11,捷联式惯导系统算法流程图,启动自检测初始化姿态阵计算,迭代次数,控制信息提取返回9,2010-03-19,12,YES导航计算,NO,2010-03-19,13,3.2姿态矩阵的计算,捷联式惯导中，载体地理位置就是地理坐标系相对地球坐标系的方位。而载体的姿态和航向则是载体坐标系相对于地理坐标系的方位关系。确定两个坐标系的方位关系问题，是力学中的刚体定点转到理论。在刚体定点转动理论中，描述动坐标系相对参考坐标系方位关系的方法有多种。,四参数法1843年发明的，首先在数学中引入四元数，以后用在刚体定位问题。凯里.克莱茵（Cayley-Klein）参数法，是在1897年提出的。九参数法基于方向余弦的概念，也称方向余弦法。,三参数法,欧拉角法，是欧拉在1776年提出的。四元数法。威廉.哈密顿(WilliamHamilton)在,等效转动矢量法,3.2姿态矩阵的计算,3.2.1欧拉角法3.2.2方向余弦法3.2.3四元数法3.2.4等效转动矢量法,2010-03-19,14,3.2.1欧拉角法,Xb,ENU作为参考坐标系，则航向角H，纵摇角（俯仰角）P和横摇角（横滚角、倾斜角）R。就是一组欧拉角。欧拉角没有严格的定义，根据需要，可以选用不同的欧拉角组。第一次转动，可以绕三个轴中的任一个转动，故有3种可能，第二次有2种可能，第三次也有2种可能。总共有12种可能。,E,Xb,O,U,N,H.,Zb,Yb,Xb,Y,Yb,Zb,b,Zb,P.,R.,H,P,R,一个动坐标系相对参考坐标系的方位，完全可以由动坐标系依次绕3个不同的轴转动的3个转角来确定。如把OXbYbZb作为动坐标系，,2010-03-19,15,2,010-03-1916,用欧拉角表示的姿态矩阵,001U,0N,0E,YsinH,cosH,b,Xb,sinHcosH,Z,v-_CH,b,0sinPcosPZ,00cosPsinPYb,b,X,v-_,0,1,Z,b,X,Yb,b,CP,b,cosRZ,Yb,b,0sinRX01,v-_,sinR0,Yb,cosR,b,Z,X,0,b,CR,b,cosPcosR,cosPsinR,sinRcosHsinPcosRsinH,cosRcosHsinPsinRsinH,cosRsinHsinPsinRcosHcosPcosHsinRsinHsinPcosRcosH,cosPsinH,sinP,bn,C,E,X,b,O,UZb,bN,H.,Y,Xb,X,b,YYb,b,Z,b,b,Z,P.,R.,H,P,R,HPR,欧拉角微分方程,表示载体坐标,系相对地理坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵。,E,Xb,O,UZb,bN,H.,Y,Xb,X,b,YYb,b,Z,b,b,Z,P.,R.,H,P,R,b,nb,0,0R.,0,0,P.,R,0C,H.,0,RP,CC,nby,nbz,b,b,b,nbx,HPR,2010-03-19,17,欧拉角微分方程,cosPcosRH.,R.,0sinRcosPP.010,sinR,nby,nbz,cosR,nbx,b,b,sinP,b,nby,nbz,b,cosPcosR,sinR,R.,cosR,0sinRcosP01sinP0,H.,P.,1,b,bnbx,cosRsinPnby,nbz,b,sinRcosPb,R.cosPsinPsinR,1,cosPcosR0cosP0,sinR,H.,P.,b,nbx,cosR,b,Cn,求解微分方程,3个欧拉角,航向角(H),姿态角(P,R),2010-03-19,18,2010-03-19,19,欧拉角法应用中的问题,求解方程可以直接得到航向和姿态信息，欧拉角法得到的姿态阵永远是正交阵，用这个矩阵将比力fbfn信息的坐标变换时，变换后的信息中不存在非正交误差。因此，用欧拉角法得到的姿态矩阵无需进行正交化处理。欧拉角微分方程中包含三角函数的运算，给实时计算带来困难，当P=90。时，方程式出现“奇点”，使计算溢出。,cosPcosR0cosP0,sinRcosP,R1sinPsinR,cosRsinP,cosP,sinR,cosR,bnbx,b,nby,b,nbz,P.,H.,.,返回3.2,垂,直,发,射,困,难,！,3.2.2方向余弦法,方向余弦表示的姿态矩阵方向余弦法用矢量的方向余弦来表示姿态矩阵的方法。用in,jn,kn表示沿地理坐标系轴向的单位矢量。ib,jb,kb沿载体坐标系轴向的单位矢量。ib在地理坐标系内的方位完全可以由ib的三个方向余弦来确定，其表达式为ib(ibin)in(ibjn)jn(ibkn)kncos(ibin)jb(jbin)in(jbjn)jn(jbkn)knkb(kbin)in(kbjn)jn(kbkn)kn,2010-03-19,20,方向余弦法,kbknkn,n,jjjkj,ibknin,n,kbin,jji,ibin,kbjn,ibjn,kb,b,ib,b,bn,bn,kb,b,bj,ib,kn,njn,in,bCbn,n,kbkn,bn,jjjk,ibkn,kbin,ji,ibin,kbjn,ibjn,bn,bn,Cb,n,写成矩阵形式为：,2010-03-19,21,矢量的坐标变换,旋转矢量的坐标变换,固定矢量的坐标变换,固定矢量的坐标变换是一个在空间大小和方向都不变的矢量在两个不同方位的坐标系轴向分量之间的变换关系，也即同一个矢量在两个不同的坐标系轴向投影之间的变换关系。,是指一个矢量大小不变，但在方向上转动了一个位置，这个矢量转动前和转动后在同一个坐标系轴向分量之间的变换关系。,2010-03-19,22,固定矢量的坐标变换,ZkrbTb,rXbibYbjb,bb,b:载体坐标系n:地理坐标系,一个矢量r，写成载体坐标系轴向分量形式：,ZkrnTn,rXninYnjn,nn,同一个矢量r，如果写成地理坐标系轴向分量形式：,rbTbrnTn,Zb,b,bj,Xb,rbY,kb,b,ib,Zn,n,Xn,Y,rn,njnkn,in,bCbn,n,bTbTbnT,n,rbrCnrn,rbTCbrnT,n,由于r是同一个矢量，故,由于正交阵，故,nb,bTb1,n,(Cn)(C),C,两边求转置,nTT,bTbTT,(Cn)(r),(r),Cnrb,b,rn,Cbrn,n,rb,2010-03-19,23,旋转矢量的坐标变换,由于动坐标系随同矢量转动，故rbT=rnT互逆,r转动前的矢量,r转动后的矢量假定有一个动坐标系和矢量固连，在矢量转动前，取动坐标系b和参考坐标系n重合，则：r=rnTn,bCbnrrnTCbn,nn,r=rbTb,如果用rn表示转动后的矢量在参考坐标系轴向的分量构成的矩阵，则,rrnTn,rnTrnTCb,n,Cnrn,b,rn,Cbrn,n,rb,由于坐标系不动而是矢量转动，它相应于矢量固定时坐标系方向转动,nTb,n,rCn,2010-03-19,24,2010-03-19,25,方向余弦矩阵微分方程,由矢量相对导数和绝对导数的关系式,r,drdrdtdt,nb,nb,假定地理坐标系为参考坐标系，作为参考坐标系认为它在空间是不动的，即,0,n,dt,dr,nbr,drdt,b,brbkrnbbnbb,b,r.,nbx,nbnbz,0,nbznby0,0,nbynbx,bk,b,nb,载体坐标系相对地理坐标系的转动角速度在b系轴向分量的反对称矩阵（Skewsymmetricmatrix）,2010-03-19,26,方向余弦矩阵微分方程,另外，从固定矢量的坐标变换关系式有,C.brnCbr.nnn,r.b,Cbrn,n,rb,两边求导,0r.br.b,r.n,C.brnC.bCnrb,nnb,考虑,bn,CnCb,bk,nb,.,b,两边同右乘Cn,bkb,nbCn,bn,C,.,nbrbnbrb,bbk,nbk,Cbnb,n,C.b,bk,nb,(bk)Tnb,bk1,(nb),返回3.2,方向余弦矩阵微分方程的几种表示形式,bkb,nbCn,bnnb,C,.,nbk,bnb,C,C,.,式中的角速度都是用载体坐标系内的分量表示的，如果角速度在地理坐标系轴向的分量表示时，则可用角速度反对称矩阵的相似变换来得到。,bnkn,nnbb,bknb,CC,nbkb,bnbn,nknb,CC,左式可以用展开的方式推导,bnk,nnb,bn,C,C,.,nkn,nbnbb,C,C,.,在捷联惯导系统中，由于陀螺是固联于载体上的，所以直接测量的角速度是载体坐标系轴向的分量。那么计算时哪个公式最方便？,常用的姿态矩阵微分方程的4种形式。,2010-03-19,27,方向余弦矩阵微分方程,陀螺仪测量的是载体相对于惯性空间的角速度b,nnbkbbnb,C,C,.,ib,而式中需要的则是,bknb,两者的关系为：,bkin,bkib,bknb,bnkn,nibb,bkib,CC,bk),n(bkib,in,b,nb,C,C,.,nkninb,nbkbib,C,C,包括载体的姿态和航向的变换角速度，数值较大（如飞机可达400。/s）,则是地球角速度和载体的位移运动相对地心形成的角速度，这个角速度比较小，一般为每小时几十度。,在实时计算上式时，第一项需要用较高的速度计算，用迭代算法时，迭代频率要高，而第二项则可用较低迭代频率计算。可以看作是对第一项的修正。,2010-03-19,28,2010-03-19,29,3.2.2.4矩阵微分方程的解下面是解方程的推导过程。,C(t)C(0)C(t)dt,nbk,bnb,b,0,C(t)C(0)C(0)C(t)dtdt,t,nbkbk,nbnb,t,b,n,b,00,C(t)dtdt,C(0)C(0)dt,ttt,bk,nb,bk,nb,n,b,bk,nb,00,0,把等式右边的表达式逐次代入积分号内,dtdt,C(t)C(0)C(0)dtC(0),ttt,bk,nb,bk,nb,bk,nb,n,b,tt,bk,nb,bk,nb,t,bk,nb,n,b,C(t)dtdtdt,000,00,0,dtdt,C(t)C(0)Idt,ttt,bk,nb,bk,nb,bk,nb,n,b,00,0,C(t)dtdtdt.,ttt,bk,nb,bk,nb,bk,nb,n,b,000,第2次代入得,这样不断的进行代入，便得到,bk2,0,12,0,00,00,(dt),dtdtdtddt,t,nb,t,bk,nb,tt,bk,nb,tt,bk,nb,bk,nb,bk3,0,16,000,(dt),C(t)dtdtdt,t,nb,ttt,bk,nb,bk,nb,bk,nb,n,b,C.nCnbkbbnb变系数的齐次微分方程,t,n,可用毕卡(Peano-Baker)逼近法求解，积分上式则有,第1次代入得,C(t)C(0)Idtdtdt3.,0,6,1,bk2,00,2,1,t,bk,nb,t,nb,t,bk,bnb,故n,Cb(t)C(0)e,t,bk,nb,dt,n,0,2010-03-19,30,矩阵微分方程的解,Cb(t)C(0)e,t,bk,nb,dt,n,0,C(tt)C(t)e,tn1,tn,bk,nbdt,n,b,bk,nbdtnb,tn1,tn,bk,bk,Cn(tt)C(t)enb,b,bb,0,bb,0,0b,nbynbx,bnbx,nbz,nby,nbz,bk,nb,bk2,12nb3,eKIKK(),nb,bk,bk,nb,I单位阵;K1,K2,K3系数。,t=tn+1-tn,下面来求三个系数。由矩阵的特征方程,如果知道了K1,K2,K3三个系数，则矩阵指数函数就可以表示成一个矩阵二次方程。bk,nb,来求它的特征值。,bb,det(Ibk)bb,bb,nbynbx,nbx,nbnbz,nbznby,20,2,2,3b,b,nbz,b,nby,nbx,20,2,2,2,b,nbz,b,nby,b,nbx,320,01,=0,2,3,=士j,0,令,将矩阵的特征值代入方程,=0，K1=1,KIKbkK(bk)212nb3nb,enb,bk,用201四0-03参-19数法。,31,矩阵微分方程的解,=j0=-j0,bk2,nb3nb,20,1,eKIKK(),bk,bk,nb,j0,ee,0,K2,bk)2,2,00,sin0bk1cos0(,nb,nb,eI,bk,nb,=0，K1=12,eK1K2j0K3(j0)e,j,2,K1K2j0K3(j0),j0,2,2K12K3(0)2K20,j0,j0,ee,()2,0,1cos0,3,K,j0,sin0,Cn(tt)C(t)Isin0bk1cos0(bk)2,2,0,0,nb,bnb,矩阵微分方程的精确解,这个精确解的前提条件是,nbkbbnb,.n,CC,+,bk,nbdtnb,tn1,tn,bk,这个式子只有在t=tn+1-tn内角速度矢量nb方向不变的条件下才有意义，由于转动的不可交换性，当nb方向随时间变化时，角速度的积分是无意义的。用方向余弦法求解姿态矩阵避免了欧拉角法方程退化的现象，可以全姿态工作，但是，由于方向余弦矩阵具有九个元素，所有，解算矩阵微分方程时，实际上是结算九个联立微分方程，一般说来，计算工作量比较大，为了减小计算工作量，可以采,3.2.3四元数法,四元数理论是数学中的一个古老的分支，1943年由威廉.哈密顿(WilliamHamilton)首先提出，目点是研究空间几何，一种类似平面问题中使用复数那样的方法。但是，这个理论建立以后，长期没有得到实际应用，直到空间技术出现以后，特别是捷联式制导技术出现以后，这一古老的数学分支，又重新受到人们的重视，得到了实际的应用。,四元数的基本概念,四元数是由1个实数单位1和3个虚数单位i,j,k组成的含有4个元的数，其形式为Q(q0,q1,q2,q3)q0q1iq2jq3kq0q标量矢量,2010-03-19,32,3.2.3四元数法3.2.3.1四元数的基本概念3.2.3.2四元数理论3.2.3.3矢量坐标变换的四元数描述3.2.3.4四元数和方向余弦矩阵的关系3.2.3.5四元数微分方程,2010-03-19,33,ZRe实轴,Im虚轴,O,zcosjzsin,Zz1jz2zej,j1,uuxiuyjuzk,u1,Zsink,iuZsinju,uxZsin,ZZcos,q3,z,q2,y,q1,q0,v-_,v-_,v-_,_,v-,2010-03-19,34,四元数的基本概念,Zcosuxsiniuysinjuzsink,(Quaternions)QZ,Zcosq0,Zuxsinq1,Zuysinq2,Zuzsinq3,Qcosusin,Qq0q1iq2jq3kQeu,由于它具有和复数类似的形式，可看作是复数的推广，因此，也有“超复数”之称。,四元数的3种表示形式,2010-03-19,35,坐标系的等效转动,E,Xb,O,UZb,N,H.,Yb,Xb,Xb,Y,Yb,Zb,b,Zb,P.,R.,H,P,R,Xb,Xr,u,Yb,Yr,b,Z,Zr,2010-03-19,36,四元数的基本概念,如果用u表示欧拉轴向的单位矢量，则动坐标系的方位，完全可由u和两个参数来确定。用u和两个参数，可以构造一个四元数，1,如果把u写成分量的形式则：Qcosusiniusinjusink2x2y2z2q0q1iq2jq3k,2,q0cos,2,q1uxsin,2,q2uysin,12,q3uzsin,Qcosusin,22,四元数是张量为1的四元数，即,Q(q2q2q2q2)210123,这样的四元数称作“规范化”的四元数，而用来描述刚体定点转动的四元数就称作变换四元数。,ue2,2010-03-19,37,3.2.3.2四元数理论,四元数相等如果两个四元数对应的元素相等，则两个四元数相等。四元数相加01i2j3km0m1im2jm3k,0m0(1m1)i(2m2)j(3m3)k对应元素相加,则,四元数相加，服从一般加法的交换律和结合律，即,()(),（交换律）（结合律）,2010-03-19,38,四元数理论,四元数与标量相乘aa0a1ia2ja3k,式中a标量,各个元素分别乘以标量,(ab)(ba)aa(ab)aba()aa,（分配律）,（交换律）,（结合律）,2010-03-19,39,四元数理论,四元数与四元数相乘01i2j3km0m1im2jm3k。(01i2j3k)。(m0m1im2jm3k)0m01m12m23m30(m1im2jm3k),乘积的矢量形式,2010-03-19,40,2010-03-19,341,四元数理论,。0m01m12m23m3i(0m11m02m33m2)j(0m22m03m11m3)k(0m33m01m22m1)乘积的四元数形式,n10m11m02m33m2n20m22m03m11m3n30m33m01m22m1,n00m01m12m23m3,nnnT,Q(n)n,3,2,1,0,T,Q(),3,2,1,0,mmmT,Q(m)m,123,0,00123,0,1,1,0,1,2,2,3,0,1,3,2,2,33,21,03,n,m,n,m,n,m,n,m,Q(n)M()Q(m),n0,m0,m1m2m30,n1m1,m0m3m21m3m0m2m1,n2m2,m12m0,n3m3,-矩阵四元数,Q(n)M(m)Q(),矩阵的“核”,2010-03-19,42,四元数理论,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具有可交换性。而一般的矩阵相乘，则是不可交换的.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*四元数之和的共轭等于共轭之和推理2(八M)*=M*八*两个四元数之积的共轭等于共轭四元数等于两个四元数共轭之积取相反的顺序。四元数的范数,四元数的范数定义为,2222,3,2,1,0,2010-03-19,43,四元数理论,=八八*=八*八八=1的四元数称为规范化的四元数。八M=八M=M八,四元数的逆,则八-1八=八八-1,1,22,0123,2,2,2010-03-19,44,3.2.3.3矢量坐标变换的四元数描述,一个矢量r在参考坐标系（这里用地理坐标系作参考系）轴向的分量形式为r=xnin+ynjn+znkn式中xn,yn,zn为r在地理坐标系轴向的分量。in,jn,kn为地理坐标系轴向的单位矢量。用xn,yn,zn把r写成四元数形式即：Rn=0+xni+ynj+znk=0+rRn就叫做矢量r在地理坐标系上的四元数影像。i,j,k是四元数的虚数单位，而r则是四元数的矢量部分。显然，如果认为i,j,k和in,jn,kn重合，则四元数的矢量部分就是三维空间的矢量r本身。,2010-03-19,45,旋转矢量的坐标变换,定义假设矢量r绕通过定点“O”的某一轴转动了一个角度，则和矢量固联的动坐标系和参考坐标系之间的变换四元数为：,22,Qcosusin,式中u为转轴方向的单位矢量。这个四元数的范数为,Qq2q2q2q210123,转动前的矢量用r表示，转动后的矢量用r表示,则r和r的关系可由四元数来描述，即,称作“规范化”的四元数.,rQ。r。Q*,Q*cosusin四元数的共轭四元数22,黄式两边同时左乘Q*右乘Q得因为,Q*。r。QQ*。Q。r。Q*。QQ*。QQ。Q*1,22(cosusin)。(cosusin)cossin1,22,22,2,2,rQ*。r。Q,2010-03-19,46,2010-03-19,47,证明,AO当矢量r绕OO旋转时，矢端A在空间的轨迹是一个圆，这个圆平面和转轴垂直，圆心为O在旋转轴上。在圆上取一点B，使AOB=90。，则按矢量关系有下列关系式：,O,r,r,A,B,u,A,O,O,B,OO,u,Ou,r,A,OO(ru)u,OOOAr,因为ab=|a|b|cos,OA=rOO=r(ru)uOB=uOA=u(r(ru)u),=ur(ru)uu=ur,uu=0,OB=ur,2010-03-19,48,证明,A,B,OAcosA,O,OBsin,O,O,r,r,A,B,u,A,如果Q=q0+q，R=r0+r,则利用式可以写成矢量形式为：,QR=q0r0+q0r+qr0qr+qr利用上式将QrQ*展开,v-_v-_q0q,Qcosusin,22,r=R=r0+rr0=0,q0OA=r(ru)uOB=ur,q,sin(ur)cosr(ur)sinv-2_2v-_2,八=0m0+0m+m0m+mOA=OAcos+OBsin=cos(r(ru)u)+ursinr=OO+OA=(ru)u+cosrcos(ru)u+sin(ur),=(1cos)(ru)u+cosr+sin(ur),Q。rcosrusinr(ur)sin,222,2010-03-19,49,推导,q0q0q0q*qq0(cosr(ur)sin)(usin)(cosr(ur)sin)(usin)2v-2_22v-2_2qq*qq*,sincos(ur)sin2(ur)ucos(cosr(ur)sin)2v-2_2v-_22v-_2,Q。r。Q*(sin(ur)cosr(ur)sin)。(cosusin),2,222,2q,=sincos(ur)sin2(ur)ucos2rsincos(ur)sincos(ur),2222,Q。Rq0r0q0rqr0qrqr,sin2(ur)usincos(ru)sin2(ur)u2222第一项与第五项相消，第六项为零第四项与第七项合并sin2(ur)ucos2rsin(ur)sin2(ur)u,222,22,22,q0,q0，q,证明QrQ*sin2(ur)ucos2rsin(ur)sin2(ur)u,222,利用QrQ*,sin2(ur)ucos2rsin(ur)sin2(uu)r(ur)u222,cos2rsin2rsin(ur)2sin2(ur)u,222,rQ。r。Q*,cosrsin(ur)(1cos)(ur)ur=OO+OA=(ru)u+cosrcos(ru)u+sin(ur)=(1cos)(ru)u+cosr+sin(ur),(ur)u=(uu)r(ur)u,2010-03-19,50,固定矢量的坐标变换,如果矢量固定不动，而动坐标系相对参考坐标系转动了一个角度，则以四元数描述的矢量在两个坐标系上的分量的变换关系为,rQ。r。Q*,如果将四元数Q,R,M的四个元写成列矢量，即表示成,3.2.3.4四元数和方向余弦矩阵的关系,Q(q)q1q2,q0,q3,Q(r)1Q(m)r2,r0m0rm,r3m3,m2,1,将固定矢量的坐标变换式，Rb=Q*RrQ写成矩阵形式，并以地理坐标系为参考坐标系，则有,Q(Rb)M(Q*)M(Q)Q(Rn),Rb=QRrQ,*,Rr=QRbQ*Rb=Q*RrQ,2010-03-19,51,3.2.3.4四元数和方向余弦矩阵的关系,Q(Rb)M(Q*)M(Q)Q(Rn),Q(R)0 xyz,T,bb,bb,Q(R)0,xyz,T,nq0q1q2q3qq,n,n,n,2,qqq,M(Q*)qqqq2,q,q3,q0q1,q1q0,1,0,3,2,3,10,q2q3,q,2,qq,q2,M*(Q),q3,0q3q0q1q2q1q0q0q1q2q3qqq,3,1,yn,n,qx,zn,q1q2q30qq,2,q2q3,2,qq,q0,03q3q0q1q2q1q0,q2,b,q3,03q3q0q1q2q1q0,yb,x,zb,0,1,1,2010-03-19,52,四元数与姿态矩阵的关系,q2(q2q3q0q1)yn,2(q1q3q0q2)xn,n,0,0123,q2q2q2q2,qqqq,2(qqqq),2(qqqq),qq,qq)q,02(q1q2q0q3),q2q223,q0q1,z,2(qq,000,0,0,2222,2301,02,13,2,223,22,1,0,03,12,22,0123,2(q2q3q0q1)yn,n,2(q1q3q0q2)xn,2(q1q3q0q2),yb2(q1q2q0q3),q0q1,q0q1qq,q2q2q2q201232(q2q3q0q1),2(q1q2q0q3),q2q3,zb,xb,z,2222,23,2,222,q2q2q2q2,2(q2q3q0q1),3,2(q1q3q0q2),2(qqqq),Cn2(q1q2q0q3),q0q1,2(qqqq),q2q223,q0q1,2(q1q2q0q3),q2q3,012,2301,1302,22,2222,b,2010-03-19,53,由四元数计算姿态矩阵,由bCbn可知上式可写为,n,zbT31T33zn,23n,y,T13xn,yT,T11T1222T32,b,xb,TT,21,T232(q2q3q0q1)T312(q1q3q0q2)T322(q2q3q0q1)Tq2q2q2q2,Tq2q2q2q2,22