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

《捷联惯性导航系统的解算方法.ppt》由会员分享，可在线阅读，更多相关《捷联惯性导航系统的解算方法.ppt（59页珍藏版）》请在三一办公上搜索。

1、,惯性导航系统原理,3捷联式惯导系统 程向红,2010-03-19,2,3捷联式惯导系统,3.1 捷联式惯导算法概述3.2 姿态矩阵的计算3.3 姿态矩阵计算机执行算法,2010-03-19,3,3.1 捷联式惯导算法概述,捷 联 式 惯 导 算 法,b,ib,f b,ib,P,R,H,L,VE,VN,捷联式惯导航系统是一个信息处理系统，就是将载体上安装的惯性 仪表所测量的载体运动信息，经过计算处理成所需要的导航信息。,b,姿态矩 阵 计算,加速度计组,导 航 计算机,VE,初始条件,b,SF,VN,n,Cb,n,SF,b,in,t,HP R,捷联式惯性导航系统=信息处理系统,根据捷联式惯导的

2、应用和功能要求不同，计算的内容和要 求，有很大的差别。常有SINSStrapdown Inertial Navigation Systems SVRUStrapdown Vertical Reference Uint SAHRSStrapdown Attitude and Heading Reference Systems IMUInertial measurement Unit,捷 联 式 惯 导 算 法,b,ibfib,b,P,R,H,EN,L,V,V,2010-03-19,接联式惯导的算法的基本内容（1）系统的启动和自检测（2）系统初始化（3）惯性仪表的误差补偿（4）姿态矩阵的计算（5）

3、导航计算（6）制导和控制信息的提取,2010-03-19,（1）系统的启动和自检测,系统启动后，各个部分的工作是否正常，要 通过自检测程序加以检测，其中包括电源、惯 性仪表、计算机以及计算机软件。通过自检测，发现有不正常，则发出告警信息(或 故障码)。系统的自检测是保证系统进入导航状态 后能正常工作、提高系统可靠性的措施。,2010-03-19,（2）系统初始化,为何要初始化？给定载体（舰船、飞行器、车辆等）的初始位置（经度和纬度）和初始速度等初始信息。,导航平台的初始对准,惯性仪表的校准Calibration,平台式,姿态矩阵的初始值,用物理的方法来实现,标度系数加速度计,捷联式,陀螺仪,进

4、行测定,漂移 偏置,2010-03-19,（3）惯性仪表的误差补偿,对捷联式惯导系统来说，由于惯性仪表直接安装 在载体上，因此，载体的线运动和角运动都引起 较大的误差。为了保证系统的精度，必须对惯性仪表的误差进行 补偿，最好的补偿方法是计算机补偿。在计算机中通过专用的软件来实现误差补偿。,2010-03-19,（4）姿态矩阵的计算,姿态矩阵的计算是捷联式惯导算法中最重要的一 部分，也是捷联式系统所特有的。不管捷联式惯导应用和功能要求如何，姿态矩阵 的计算却是不可少的。姿态矩阵算法是本章重点 讨论的内容。,2010-03-19,（5）导航计算,导航计算就是把加速度计的输出信息变换到导航坐 标系，

5、然后，计算载体速度、位置等导航信息。,2010-03-19,（6）制导和控制信息的提取,制导和控制信息的提取，载体的姿态既可用来 显示也是控制系统最基本的控制信息。此外，载体的角速度和线速度信息也都是控制 载体所需要的信息。这些信息可以从姿态矩阵的元素和陀螺加速度计的输出中提取出来。,2010-03-19,捷联式惯导系统算法流程图,启动 自 检 测 初 始 化 姿态阵计算,迭 代 次 数,控 制 信 息 提 取 返回9,2010-03-19,YES导 航 计算,NO,2010-03-19,13,3.2 姿态矩阵的计算,捷联式惯导中，载体地理位置就是地理坐标系相对 地球坐标系的方位。而载体的姿态

6、和航向则是载体 坐标系相对于地理坐标系的方位关系。确定两个坐 标系的方位关系问题，是力学中的刚体定点转到理 论。在刚体定点转动理论中，描述动坐标系相对参 考坐标系方位关系的方法有多种。,四参数法 1843年发明的，首先在数学中引入四元数，以 后用在刚体定位问题。凯里.克莱茵（Cayley-Klein）参数法，是在1897年提出的。九参数法基于方向余弦的概念，也称 方向余弦法。,三参数法,欧拉角法，是欧拉在1776年提出的。四元数法。威廉.哈密顿(William Hamilton)在,等效转动矢量法,3.2 姿态矩阵的计算,3.2.1 欧拉角法3.2.2 方向余弦法3.2.3 四元数法3.2.4

7、 等效转动矢量法,2010-03-19,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,2

8、,010-03-1916,用欧拉角表示的姿态矩阵,001U,0N,0 E,Y sin H,cos H,b,Xb,sin Hcos H,Z,v-_CH,b,0 sin Pcos P Z,00cos Psin P Yb,b,X,v-_,0,1,Z,b,X,Yb,b,CP,b,cos R Z,Yb,b,0 sin R X 01,v-_,sin R0,Yb,cos R,b,Z,X,0,b,CR,b,cos P cos R,cos P sin R,sin R cos H sin P cos R sin H,cos R cos H sin P sin R sin H,cos R sin H sin P s

9、in R cos Hcos P cos Hsin R sin H sin P cos R cos H,cos P sin H,sin P,b n,C,E,X,b,O,U Zb,bN,H.,Y,Xb,X,b,Y Yb,b,Z,b,b,Z,P.,R.,H,P,R,HPR,欧拉角微分方程,表示载体坐标,系相对地理坐标系的角 速度矢量在载体坐标系 轴向的分量构成的列矩 阵。,E,Xb,O,U Zb,bN,H.,Y,Xb,X,b,Y Yb,b,Z,b,b,Z,P.,R.,H,P,R,b,nb,0,0 R.,0,0,P.,R,0 C,H.,0,RP,CC,nby,nbz,b,b,b,nbx,HPR,201

10、0-03-19,欧拉角微分方程,cos P cos R H.,R.,0 sin R cos P P.010,sin R,nby,nbz,cos R,nbx,b,b,sin P,b,nby,nbz,b,cos P cos R,sin R,R.,cos R,0 sin R cos P01sin P0,H.,P.,1,b,b nbx,cos R sin Pnby,nbz,b,sin R cos P b,R.cos P sin P sin R,1,cos P cos R0 cos P0,sin R,H.,P.,b,nbx,cos R,b,Cn,求解微分方程,3个欧拉角,航向角(H),姿态角(P,R),

11、2010-03-19,2010-03-19,19,欧拉角法应用中的问题,求解方程可以直接得到航向和姿态信息，欧 拉角法得到的姿态阵永远是正交阵，用这个矩 阵将比力fbfn信息的坐标变换时，变换后的信 息中不存在非正交误差。因此，用欧拉角法得 到的姿态矩阵无需进行正交化处理。欧拉角微分方程中包含三角函数的运算，给实时计算带来困难，当P=90。时，方程式 出现“奇点”，使计算溢出。,cos P cos R0 cos P0,sin R cos P,R 1 sin P sin R,cos R sin P,cos P,sin R,cos R,b nbx,b,nby,b,nbz,P.,H.,.,返回3.2

12、,垂,直,发,射,困,难,！,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)jn(jb kn)knkb(kb in)in(kb jn)jn(kb kn)kn,2010-03-19,方向余弦法,kb kn kn,n,j jj k j,ib kn in,n,kb in,j

13、j i,ib in,kb jn,ib jn,kb,b,ib,b,bn,bn,kb,b,b j,ib,kn,n jn,in,b Cbn,n,kb kn,bn,j jj k,ib kn,kb in,j i,ib in,kb jn,ib jn,bn,bn,Cb,n,写成矩阵形式为：,2010-03-19,矢量的坐标变换,旋转矢量的坐标变换,固定矢量的坐标变换,固定矢量的坐标变换是一个在空间大小和方向都不 变的矢量在两个不同方位的坐标系轴向分量之间的变 换关系，也即同一个矢量在两个不同的坐标系轴向投 影之间的变换关系。,是指一个矢量大小不变，但在方向上转动了一个位 置，这个矢量转动前和转动后在同一个坐

14、标系轴向 分量之间的变换关系。,2010-03-19,固定矢量的坐标变换,Zk r bT b,r X bib Yb jb,bb,b:载体坐标系 n:地理坐标系,一个矢量r，写成载体坐标系轴向分量形式：,Zk r nT n,r X nin Yn jn,nn,同一个矢量r，如果写成地理坐标系轴向分量形式：,r bT b r nT n,Zb,b,b j,Xb,r b Y,kb,b,ib,Zn,n,X n,Y,r n,n jn kn,in,b Cb n,n,bTbTbnT,n,rb rC n rn,rbT Cb r nT,n,由于r是同一个矢量，故,由于正交阵，故,n b,bTb1,n,(Cn)(C)

15、,C,两边求转置,nTT,bTbTT,(Cn)(r),(r),Cn r b,b,r n,Cb r n,n,r b,2010-03-19,旋转矢量的坐标变换,由于动坐标系随同矢量转动，故rbT=rnT互逆,r转动前的矢量,r 转动后的矢量假定有一个动坐标系和矢量固连，在矢量转动 前，取动坐标系b和参考坐标系n重合，则：r=rnTn,b Cb nr r nT Cb n,nn,r=rbTb,如果用r n表示转动后的矢量在参考坐标系轴向的 分量构成的矩阵，则,r rnT n,rnT r nT Cb,n,C n r n,b,rn,Cb r n,n,r b,由于坐标系不动而是矢量转动，它相应于矢量固定时坐

16、标系方向转动,nTb,n,rCn,2010-03-19,2010-03-19,25,方向余弦矩阵微分方程,由矢量相对导数和绝对导数的关系式,r,dr drdtdt,nb,nb,假定地理坐标系为参考坐标系，作为参考 坐标系认为它在空间是不动的，即,0,n,dt,dr,nb r,dr dt,b,b r bk rnbbnbb,b,r.,nbx,nb nbz,0,nbznby0,0,nbynbx,bk,b,nb,载体坐标系相对地理坐标系的转动角速度在b系轴 向分量的反对称矩阵（Skew symmetric matrix）,2010-03-19,26,方向余弦矩阵微分方程,另外，从固定矢量的坐标变换关系

17、式有,C.br n Cbr.n nn,r.b,Cb r n,n,r b,两边求导,0r.br.b,r.n,C.br n C.bCnrb,nnb,考虑,bn,Cn Cb,bk,nb,.,b,两边同右乘Cn,bkb,nb Cn,b n,C,.,nb rb nb rb,bbk,nbk,Cb nb,n,C.b,bk,nb,(bk)Tnb,bk1,(nb),返回3.2,方向余弦矩阵微分方程的几种表示形式,bkb,nb Cn,b nn b,C,.,nbk,bnb,C,C,.,式中的角速度都是用载体坐标系内的分量 表示的，如果角速度在地理坐标系轴向的 分量表示时，则可用角速度反对称矩阵的 相似变换来得到。,

18、bnkn,nnbb,bk nb,C C,nbkb,bnbn,nk nb,CC,左式可以用展开的方式推导,bnk,nnb,b n,C,C,.,nkn,nbnbb,C,C,.,在捷联惯导系统中，由于陀螺是固联于载体上的，所以直接测量的角速度是载体坐标系轴向的分量。那么计算时哪个公式最方便？,常用的姿态矩阵微分方程的4种形式。,2010-03-19,方向余弦矩阵微分方程,陀螺仪测量的是载体相对于惯性空间的角速度 b,nnbkbbnb,C,C,.,ib,而式中需要的则是,bk nb,两者的关系为：,bk in,bk ib,bk nb,bnkn,nibb,bk ib,C C,bk),n(bk ib,in

19、,b,n b,C,C,.,nkninb,nbkbib,C,C,包括载体的 姿态和航向 的变换角速 度，数值较 大（如飞机 可达400。/s）,则是地球角速 度和载体的位 移运动相对地 心形成的角速 度，这个角速 度比较小，一 般为每小时几 十度。,在实时计算上式时，第一项需要用较高的速度计算，用迭代算法时，迭代频 率要高，而第二项则可用 较低迭代频率计算。可以 看作是对第一项的修正。,2010-03-19,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,nb

20、kbk,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)dt C(0),ttt,bk,nb,bk,nb,bk,nb,n,b,tt,bk,nb,bk,nb,t,bk,nb,n,b,C(t)dtdtdt,0 0 0,0 0,0,dtdt,C(t)C(0)I dt,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次代入得,这样不断的进行代入，便得

21、到,bk 2,0,12,0,00,0 0,(dt),dtdt dtddt,t,nb,t,bk,nb,tt,bk,nb,tt,bk,nb,bk,nb,bk 3,0,16,0 0 0,(dt),C(t)dtdtdt,t,nb,ttt,bk,nb,bk,nb,bk,nb,n,b,C.n Cn bk bbnb变系数的齐次微分方程,t,n,可用毕卡(Peano-Baker)逼近法求解，积分上式则有,第1次代入得,C(t)C(0)I dt dtdt3.,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-1

22、9,30,矩阵微分方程的解,Cb(t)C(0)e,t,bk,nb,dt,n,0,C(t t)C(t)e,tn1,tn,bk,nb dt,n,b,bk,nb dt nb,tn1,tn,bk,bk,C n(t t)C(t)enb,b,b b,0,b b,0,0b,nbynbx,b nbx,nbz,nby,nbz,bk,nb,bk2,12nb 3,e K I K K(),nb,bk,bk,nb,I单位阵;K1,K2,K3系数。,t=tn+1-tn,下面来求三个系数。由矩阵的特征方程,如果知道了K1,K2,K3三个系数，则矩阵指数函数就可以表示成 一个矩阵二次方程。bk,nb,来求它的特征值。,b b

23、,det(I bk)b b,b b,nbynbx,nbx,nbnbz,nbznby,2 0,2,2,3b,b,nbz,b,nby,nbx,20,2,2,2,b,nbz,b,nby,b,nbx,3 2 0,01,=0,2,3,=士j,0,令,将矩阵的特征值代入方程,=0，K1=1,K I K bk K(bk)212nb3nb,enb,bk,用201四0-03参-19数法。,31,矩阵微分方程的解,=j0=-j0,bk2,nb3nb,20,1,e K I K K(),bk,bk,nb,j0,e e,0,K2,bk)2,2,00,sin 0 bk 1 cos 0(,nb,nb,e I,bk,nb,=

24、0，K1=12,e K1 K 2 j0 K3(j0)e,j,2,K1 K2 j0 K3(j0),j 0,2,2K1 2K3(0)2K2 0,j 0,j 0,e e,()2,0,1 cos 0,3,K,j0,sin 0,Cn(t t)C(t)I sin 0 bk 1 cos 0(bk)2,2,0,0,nb,bnb,矩阵微分方程的精确解,这个精确解的前提条件是,nbkbbnb,.n,C C,+,bk,nb dt nb,tn1,tn,bk,这个式子只有在t=tn+1-tn内角速度矢量nb方向不变的条件下才有意义，由于转动 的不可交换性，当nb方向随时间变化时，角速度的积分是无意义的。用方向余弦法求解

25、姿态矩阵避免了欧拉角法方程退化的现象，可以全姿态工作，但 是，由于方向余弦矩阵具有九个元素，所有，解算矩阵微分方程时，实际上是结算 九个联立微分方程，一般说来，计算工作量比较大，为了减小计算工作量，可以采,3.2.3 四元数法,四元数理论是数学中的一个古老的分支，1943年由威廉.哈密顿(William Hamilton)首先提出，目点是研究空间 几何，一种类似平面问题中使用复数那样的方法。但是，这个理论建立以后，长期没有得到实际应用，直到空 间技术出现以后，特别是捷联式制导技术出现以后，这 一古老的数学分支，又重新受到人们的重视，得到了实 际的应用。,四元数的基本概念,四元数是由1个实数单位

26、1和3个虚数单位i,j,k组成的含有4个元的数，其形式为Q(q0,q1,q2,q3)q0 q1i q2 j q3k q0 q标量矢量,2010-03-19,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,ZRe实轴,Im 虚轴,O,z cos j z sin,Z z1 jz2 z e j,j 1,u uxi u y j uz k,u 1,Z sin k,iuZ sin ju,ux Z sin,Z Z cos,q3,z,q2,y,q

27、1,q0,v-_,v-_,v-_,_,v-,2010-03-19,四元数的基本概念,Zcos ux sini uy sinj uz sink,(Quaternions)Q Z,Z cos q0,Z ux sin q1,Z uy sin q2,Z uz sin q3,Q cos u sin,Q q0 q1i q2 j q3 k Q eu,由于它具有和复数类似的形式，可看作是复数的推广，因此，也有“超复数”之称。,四元数的3种表示形式,2010-03-19,坐标系的等效转动,E,Xb,O,U Zb,N,H.,Yb,Xb,Xb,Y,Yb,Zb,b,Zb,P.,R.,H,P,R,Xb,X r,u,Yb

28、,Yr,b,Z,Zr,2010-03-19,四元数的基本概念,如果用u表示欧拉轴向的单位矢量，则动坐标系的方 位，完全可由u和 两个参数来确定。用u和 两个参 数，可以构造一个四元数，1,如果把u写成分量的形式则：Q cos usin i usin j usin k2x2y2z 2 q0 q1i q2 j q3k,2,q0 cos,2,q1 ux sin,2,q2 u y sin,12,q3 uz sin,Q cos u sin,22,四元数是张量为1的四元数，即,Q(q 2 q 2 q 2 q 2)2 10123,这样的四元数称作“规范化”的四元数，而用来描述刚体定点转动的四元数就称作变换四

29、元数。,u e2,2010-03-19,3.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对应元素相加,则,四元数相加，服从一般加法的交换律和结合律，即,()(),（交换律）（结合律）,2010-03-19,四元数理论,四元数与标量相乘a a0 a1i a2 j a3k,式中a标量,各个元素分别乘以标量,(ab)(ba)a a(a b)a ba()a a,（分配律）,（交换律）,（结合律）,2010-03-19,四元数理论,四元数与四元数

30、相乘 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,2010-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 2m1,n0 0m0 1m1 2m2 3m3,nnn T,

31、Q(n)n,3,2,1,0,T,Q(),3,2,1,0,mmm T,Q(m)m,123,0,00 1 2 3,0,1,1,0,1,2,2,3,0,1,3,2,2,3 3,21,0 3,n,m,n,m,n,m,n,m,Q(n)M()Q(m),n0,m0,m1 m2 m3 0,n1 m1,m0m3m2 1 m3m0m2m1,n2 m2,m1 2 m0,n3 m3,-矩阵四元数,Q(n)M(m)Q(),矩阵的“核”,2010-03-19,42,四元数理论,M()和M*(m)除元素不同外，其核互为转置。这种四元数乘积的矩阵形式，也可推广到三个 以上的四元数乘积。如：,Q(。)M()Q(。)M()M(P

32、)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*八*两个四元数之积的共轭

33、等于共轭四元数等于两个四元数共轭之积取相反的顺序。四元数的范数,四元数的范数定义为,2222,3,2,1,0,2010-03-19,四元数理论,=八八*=八*八八=1的四元数称为规范化的四元数。八M=八 M=M 八,四元数的逆,则八-1八=八八-1,1,22,0123,2,2,2010-03-19,3.2.3.3 矢量坐标变换的四元数描述,一个矢量r在参考坐标系（这里用地理坐标系作参 考系）轴向的分量形式为r=xnin+yn jn+znkn式中xn,yn,zn为r在地理坐标系轴向的分量。in,jn,kn为地理坐标系轴向的单位矢量。用xn,yn,zn把r写成四元数形式即：Rn=0+xni+yn

34、j+znk=0+r Rn就叫做矢量r在地理坐标系上的四元数影像。i,j,k是四元数的虚数单位，而r则是四元数的矢量部分。显然，如果认为i,j,k和in,jn,kn重合，则四元数的矢量部分就是三维空间的矢量r本身。,2010-03-19,旋转矢量的坐标变换,定义假设矢量r绕通过定点“O”的某一轴转动了一个 角度，则和矢量固联的动坐标系和参考坐标系之间的 变换四元数为：,22,Q cos usin,式中u为转轴方向的单位矢量。这个四元数的范数为,Q q2 q2 q2 q2 10123,转动前的矢量用r表示，转动后的矢量用r表示,则r和r的关系可由四元数来描述，即,称作“规范化”的四元数.,r Q。

35、r。Q*,Q*cos usin四元数的共轭四元数2 2,黄式两边同时左乘Q*右乘Q得因为,Q*。r。Q Q*。Q。r。Q*。QQ*。Q Q。Q*1,2 2(cos usin)。(cos usin)cos sin 1,22,22,2,2,r Q*。r。Q,2010-03-19,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(r u)u,OO OA r,因为ab=|a|b|cos,OA

36、=r OO=r(ru)u OB=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=q0 r0+q0r+qr0qr+qr利用上式将QrQ*展开,v-_v-_q0q,Q cos usin,22,r=R=r0+rr0=0,q0OA=r(ru)uOB=ur,q,sin(ur)cos r(ur)sin v-2 _ 2 v-_2,八=0 m0+0 m+m0 m+mOA=OA cos+OBsin=cos(r(ru

37、)u)+ur sin r=OO+OA=(ru)u+cosr cos(ru)u+sin(ur),=(1 cos)(ru)u+cos r+sin(ur),Q。r cosr usinr(ur)sin,222,2010-03-19,49,推导,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-_2,Q。r。Q*(sin(u r)cosr(u r)sin)。(cos usin

38、),2,222,2q,=sincos(u r)sin2(u r)u cos2r sincos(u r)sincos(u r),2222,Q。R q0r0 q0r qr0 q r q r,sin2(u r)u sin cos(r u)sin2(u r)u222 2第一项与第五项相消，第六项为零第四项与第七项合并 sin2(u r)u cos2 r sin(u r)sin2(u r)u,222,22,22,q0,q0，q,证明QrQ*sin2(u r)u cos2 r sin(u r)sin2(u r)u,222,利用QrQ*,sin2(u r)u cos2 r sin(ur)sin2(uu)r(

39、u r)u 22 2,cos2 r sin2 r sin(ur)2sin2(u r)u,222,r Q。r。Q*,cos r sin(ur)(1 cos)(u r)ur=OO+OA=(ru)u+cosr cos(ru)u+sin(ur)=(1 cos)(ru)u+cosr+sin(ur),(ur)u=(uu)r(ur)u,2010-03-19,固定矢量的坐标变换,如果矢量固定不动，而动坐标系相对参考坐标系转动了一个角度，则以四元数描述的矢量在两个坐标系上的分量的变换关系为,r Q。r。Q*,如果将四元数Q,R,M的四个元写成列矢量，即表示成,3.2.3.4 四元数和方向余弦矩阵的关系,Q(q)

40、q1 q2,q0,q3,Q(r)1 Q(m)r2,r0 m0 r m,r3 m3,m2,1,将固定矢量的坐标变换式，Rb=Q*RrQ写成矩阵形式，并以地理坐标系为参考坐标系，则有,Q(Rb)M(Q*)M(Q)Q(Rn),Rb=QRrQ,*,Rr=QRbQ*Rb=Q*RrQ,2010-03-19,3.2.3.4四元数和方向余弦矩阵的关系,Q(Rb)M(Q*)M(Q)Q(Rn),Q(R)0 xyz,T,bb,bb,Q(R)0,xyz,T,nq0 q1 q2 q3 qq,n,n,n,2,qq q,M(Q*)q qqq2,q,q3,q0q1,q1q0,1,0,3,2,3,10,q2q3,q,2,q q

41、,q2,M*(Q),q3,0q3q0q1q2 q1q0 q0q1q2q3 qqq,3,1,yn,n,qx,zn,q1 q2 q3 0 qq,2,q2q3,2,qq,q0,03q3q0q1q2 q1q0,q2,b,q3,03q3q0q1q2 q1q0,yb,x,zb,0,1,1,2010-03-19,四元数与姿态矩阵的关系,q2(q2q3 q0q1)yn,2(q1q3 q0q2)xn,n,0,0123,q2 q2 q2 q2,q q q q,2(q q q q),2(q q q q),q q,q q)q,02(q1q2 q0q3),q2 q223,q0 q1,z,2(q q,000,0,0,22

42、22,2301,02,13,2,223,22,1,0,03,12,22,0123,2(q2q3 q0q1)yn,n,2(q1q3 q0q2)xn,2(q1q3 q0q2),yb 2(q1q2 q0q3),q0 q1,q0 q1 q q,q2 q2 q2 q201232(q2q3 q0q1),2(q1q2 q0q3),q2 q3,zb,xb,z,2222,23,2,222,q2 q2 q2 q2,2(q2q3 q0q1),3,2(q1q3 q0q2),2(q q q q),Cn 2(q1q2 q0q3),q0 q1,2(q q q q),q2 q223,q0 q1,2(q1q2 q0q3),q2

43、 q3,012,2301,1302,22,2222,b,2010-03-19,由四元数计算姿态矩阵,由 b Cbn 可知上式可写为,n,zb T31T33 zn,23 n,y,T13 xn,y T,T11T1222T32,b,xb,TT,21,T23 2(q2q3 q0q1)T31 2(q1q3 q0q2)T32 2(q2q3 q0q1)T q2 q2 q2 q2,T q2 q2 q2 q2,220123,T q2 q2 q2 q2110123T12 2(q1q2 q0q3)T13 2(q1q3 q0q2)T21 2(q1q2 q0q3),两个变换矩阵相等，即对应元 素相等，如果知道了变换四元

44、 数的四个元，则可以求出姿态 矩阵的九个元素，并构成姿态 矩阵。反之，如果知道了姿态矩阵的 九个元素也可以求出相应的变 换四元数的四个元。,33,0,1,2,354,2010-03-19,(1 T11 T22 T33),2,q 1,0,T T 2q2 2q23301,22,4q1 4q0 2(T22 T33)1 T11 T22 T33 2(T22 T33)1 T11 T22 T33,22,(1 T T T),2,q 1,112233,1,T T 2q2 2q2113302,4q2 4q2 2(TT)1TT T 2(TT)1TTT2011331122331133112233,T T),(1 T,

45、2,q 1,112233,2,T T 2q2 2q2112203,4q2 4q2 2(T T)1 T T T 2(T T)1 T T T3011221122331122112233,q 1(1 T T T),3,11,22,33,2,2010-03-19,2010-03-19,56,由姿态矩阵元素计算四元数,(1 T T T),2,q 1,112233,0,T T),(1 T,2,q 1,33,22,11,1,T T),(1T,2,q 1,33,22,11,2,(1 T11 T22 T),2,q 1,33,3,符号可用如下方法确定，考虑相 对应的对角线对称的诸元素之 差，则有先决定q0的符号，

46、即可sign(q0)sign(q1)sign(T23 T32)sign(q2)sign(T31 T13),4q0q1 T23 T324q0q2 T31 T134q0q3 T12 T21,sign(q3)sign(T12 T21),2010-03-19,57,利用矢量r（坐标系相当于矢量）绕OO轴旋转角变 为r，用u表示转轴向的单位矢量，则r=QrQ*，,Q cos usin22定理 如果坐标系b经由Q确定的转动变为坐标系b，则Q在b和b上的“映像”称为变换的特征四元数，用Qc 表示。如果经过三次转动，则等效四元数由其三个特征四元 数的乘积决定，顺序与转动顺序相同。,2010-03-19,58,

47、四元数与欧拉角的关系,Q Qc。Qc。Qc123,Qc cos R jsin R,Q2 cos in sin,kn sin,Q cos,2,2,2,2,2P,2,3n,1,P,H,H,c,c,(coscoscossinsinsin)(cossincossincossin)in 222222222222(coscossin sinsincos)jn(sincoscos cossinsin)kn 222222222222,n,nn,HPRHPRHPRHPR,HPRHPRHPR,HPR,Q(cos H ksin H)。(cos P isin P)。(cos R jsin R),2222,22,q3 sincoscos cossinsin,q2 coscossin sinsincos,q cossincos sincossin,q0 cos 2 cos 2 cos 2 sin 2 sin 2 sin 2,HPR2HPR222HPR222,22,2,22,2,2,2,222,1,HPR,HPR,HPR,HPRHPR,谢谢！,