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

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

2、Cb,n,SF,b,in,t,H P R,3,10/21/2020,捷联式惯性导航系统=信息处理系统,根据捷联式惯导的应用和功能要求不同，计算的内容和要 求，有很大的差别。常有 SINSStrapdown Inertial Navigation Systems SVRUStrapdown Vertical Reference Uint SAHRSStrapdown Attitude and Heading Reference Systems IMUInertial measurement Unit,捷 联 式 惯 导 算 法,b,ib fib,b,P, R, H,EN, L,V ,V,2010

3、-03-19,4,10/21/2020,接联式惯导的算法的基本内容 （1）系统的启动和自检测 （2）系统初始化 （3）惯性仪表的误差补偿 （4）姿态矩阵的计算 （5）导航计算 （6）制导和控制信息的提取,2010-03-19,5,10/21/2020,（1）系统的启动和自检测,系统启动后，各个部分的工作是否正常，要 通过自检测程序加以检测，其中包括电源、惯 性仪表、计算机以及计算机软件。 通过自检测，发现有不正常，则发出告警信息(或 故障码)。系统的自检测是保证系统进入导航状态 后能正常工作、提高系统可靠性的措施。,2010-03-19,6,10/21/2020,（2）系统初始化,为何要初始化

4、？ 给定载体（舰船、飞行器、车辆等）的初始位置 （经度和纬度）和初始速度等初始信息。,导航平台的初始对准,惯性仪表的校准 Calibration,平台式,姿态矩阵的初始值,用物理的方法来实现,标度系数 加速度计,捷联式,陀螺仪,进行测定,漂移 偏置,2010-03-19,7,10/21/2020,（3）惯性仪表的误差补偿,对捷联式惯导系统来说，由于惯性仪表直接安装 在载体上，因此，载体的线运动和角运动都引起 较大的误差。 为了保证系统的精度，必须对惯性仪表的误差进行 补偿，最好的补偿方法是计算机补偿。 在计算机中通过专用的软件来实现误差补偿。,2010-03-19,8,10/21/2020,（

5、4）姿态矩阵的计算,姿态矩阵的计算是捷联式惯导算法中最重要的一 部分，也是捷联式系统所特有的。 不管捷联式惯导应用和功能要求如何，姿态矩阵 的计算却是不可少的。姿态矩阵算法是本章重点 讨论的内容。,2010-03-19,9,10/21/2020,（5）导航计算,导航计算就是把加速度计的输出信息变换到导航坐 标系，然后，计算载体速度、位置等导航信息。,2010-03-19,10,10/21/2020,（6）制导和控制信息的提取,制导和控制信息的提取，载体的姿态既可用来 显示也是控制系统最基本的控制信息。 此外，载体的角速度和线速度信息也都是控制 载体所需要的信息。 这些信息可以从姿态矩阵的元素和

6、陀螺加速度 计的输出中提取出来。,2010-03-19,11,10/21/2020,捷联式惯导系统算法流程图,启动 自 检 测 初 始 化 姿态阵计算,迭 代 次 数,控 制 信 息 提 取 返回9,2010-03-19,12,YES 导 航 计算,NO,10/21/2020,2010-03-19,13,3.2 姿态矩阵的计算,捷联式惯导中，载体地理位置就是地理坐标系相对 地球坐标系的方位。而载体的姿态和航向则是载体 坐标系相对于地理坐标系的方位关系。确定两个坐 标系的方位关系问题，是力学中的刚体定点转到理 论。在刚体定点转动理论中，描述动坐标系相对参 考坐标系方位关系的方法有多种。,四参数法

7、 1843年发明的，首先在数学中引入四元数，以 后用在刚体定位问题。凯里.克莱茵（Cayley- Klein）参数法，是在1897年提出的。 九参数法基于方向余弦的概念，也称 方向余弦法。,三参数法,欧拉角法 ，是欧拉在1776年提出的。 四元数法。威廉.哈密顿(William Hamilton)在,等效转动矢量法,13,10/21/2020,3.2 姿态矩阵的计算,3.2.1 欧拉角法 3.2.2 方向余弦法 3.2.3 四元数法 3.2.4 等效转动矢量法,2010-03-19,14,10/21/2020,3.2.1 欧拉角法,Xb,ENU作为参考坐标系，则航向 角H，纵摇角（俯仰角）P和

8、横 摇角（横滚角、倾斜角）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,10/21/2020,2,010-03-1916,用欧拉角表示的姿态矩阵,001U ,0N ,0 E , Y si

9、n H, cos H, ,b , Xb ,sin H cos H,Z, v- _ CH, b,0 sin Pcos P Z ,00 cos 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 si

10、n P sin R cos H cos P cos H sin R sin H sin P cos R cos H,cos P sin H,sin P,b n,C,E,X ,b,O,U Zb,b N,H.,Y ,Xb,X ,b,Y Yb,b,Z ,b,b,Z,P.,R.,H,P,R,HPR,16,10/21/2020,欧拉角微分方程,表示载体坐标,系相对地理坐标系的角 速度矢量在载体坐标系 轴向的分量构成的列矩 阵。,E,Xb,O,U Zb,b N,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 ,

11、 0 C,H. , 0 ,RP , CC,nby ,nbz , b, b,b,nbx,HPR,2010-03-19,17,10/21/2020,欧拉角微分方程,cos P cos R H. , R. ,0 sin R cos P P. 01 0,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 P 01sin P 0,H. , P. ,1,b,b nbx,cos R sin Pnby ,nbz , b,sin R cos P b, R. cos P

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

13、算溢出。,cos P cos R0 cos P 0,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,垂,直,发,射,困,难,！,19,10/21/2020,3.2.2 方向余弦法,方向余弦表示的姿态矩阵 方向余弦法用矢量的方向余弦来表示姿态矩阵的方法。 用in, jn, kn表示沿地理坐标系轴向的单位矢量。 ib, jb, kb沿载体坐标系轴向的单位矢量。ib在地理坐 标系内的方位完全可以由ib的三个方向余弦来确定，其 表达式为 ib (ib i

14、n )in (ib jn ) jn (ib kn )kn cos(ib in ) jb ( jb in )in ( jb jn ) jn ( jb kn )kn kb (kb in )in (kb jn ) jn (kb kn )kn,2010-03-19,20,10/21/2020,方向余弦法,kb kn kn ,n ,j jj k j,ib kn in ,n ,kb in, j j i, ib in,kb jn,ib jn,kb ,b , ib ,b,bn,bn,kb ,b ,b j, ib ,kn ,n j n, in ,b Cbn,n,kb kn ,bn ,j jj k,ib kn ,

15、kb in, j i, ib in,kb jn,ib jn,bn,bn,Cb,n,写成矩阵形式为：,2010-03-19,21,10/21/2020,矢量的坐标变换,旋转矢量的坐标变换,固定矢量的坐标变换,固定矢量的坐标变换是一个在空间大小和方向都不 变的矢量在两个不同方位的坐标系轴向分量之间的变 换关系，也即同一个矢量在两个不同的坐标系轴向投 影之间的变换关系。,是指一个矢量大小不变，但在方向上转动了一个位 置，这个矢量转动前和转动后在同一个坐标系轴向 分量之间的变换关系。,2010-03-19,22,10/21/2020,固定矢量的坐标变换,Zk r bT b,r X bib Yb jb,

16、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), C,两边求转置,nTT,bTbTT,(Cn )(r), (r), Cn r

17、 b,b,r n, Cb r n,n,r b,2010-03-19,23,10/21/2020,旋转矢量的坐标变换,由于动坐标系随同矢量转动，故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,由于坐标系不动而是矢量转动，它 相应于矢量固定时坐标系方向转动,nTb,n,

18、 rCn,2010-03-19,24,10/21/2020,2010-03-19,25,方向余弦矩阵微分方程,由矢量相对导数和绝对导数的关系式, r,dr dr dtdt,nb,nb,假定地理坐标系为参考坐标系，作为参考 坐标系认为它在空间是不动的，即, 0,n,dt,dr, nb r,dr dt,b, b r bk r nbbnbb,b,r., nbx , ,nb nbz,0, nbznby 0, ,0,nbynbx,bk,b,nb,载体坐标系相对地理坐标系的转动角速度在b系轴 向分量的反对称矩阵（Skew symmetric matrix）,25,10/21/2020,2010-03-19

19、,26,方向余弦矩阵微分方程,另外，从固定矢量的坐标变换关系式有, C. br n Cbr. n nn,r. b, Cb r n,n,r b,两边求导, 0 r.b r.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 )T nb,bk1,(nb ),返回3.2,26,10/21/2020,方向余弦矩阵微分方程的几种表示形式,bkb, nb Cn,b n n b,C,.,nbk,bnb, C,C

20、,.,式中的角速度都是用载体坐标系内的分量 表示的，如果角速度在地理坐标系轴向的 分量表示时，则可用角速度反对称矩阵的 相似变换来得到。,bnkn,nnbb,bk nb, C C,nbkb,bnbn,nk nb, CC,左式可以用展开的方式推导,bnk,nnb,b n, C ,C,.,nkn,n bnbb, C,C,.,在捷联惯导系统中，由于陀螺是固联于载体上的， 所以直接测量的角速度是载体坐标系轴向的分量。 那么计算时哪个公式最方便？,常用的姿态矩阵微分方程的4种形式。,2010-03-19,27,10/21/2020,方向余弦矩阵微分方程,陀螺仪测量的是载体相对于惯性空间的角速度 b,nn

21、bk bbnb, C,C,.,ib,而式中需要的则是,bk nb,两者的关系为：,bk in,bk ib,bk nb, , ,bnkn,nibb,bk ib,C C, ,bk ),n (bk ib,in,b,n b, C,C, ,.,nkn inb,nbk bib,C, C, ,包括载体的 姿态和航向 的变换角速 度，数值较 大（如飞机 可达400。/s）,则是地球角速 度和载体的位 移运动相对地 心形成的角速 度，这个角速 度比较小，一 般为每小时几 十度。,在实时计算上式时，第一 项需要用较高的速度计算 ，用迭代算法时，迭代频 率要高，而第二项则可用 较低迭代频率计算。可以 看作是对第一项

22、的修正。,2010-03-19,28,10/21/2020,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)dt C(0),ttt,bk,nb,bk,nb,bk,nb,n,b,tt,bk,nb,bk,

23、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次代入得,这样不断的进行代入，便得到,bk 2,0,1 2,0,00,0 0,(dt),dtdt dtddt , ,t,nb,t,bk,nb,tt,bk,nb,tt,bk,nb,bk,nb,bk 3,0,1 6,0 0 0,(dt),C (t)dtdtdt , ,t,nb,ttt,bk,nb,bk,nb,bk,nb,n,

24、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,29,10/21/2020,2010-03-19,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,t n,bk,bk,C n (t t) C(

25、t)enb,b, , b b, , 0, b b,0,0 b,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,det(I bk ) b b, b b,nbynbx,nbx,nbnbz,nbznby, , ,2 0,2,2,3b,b,nbz,b,nby,nbx, , ,2 0,2,2,2, ,

26、b,nbz,b,nby,b,nbx,3 2 0,01, =0 ,2,3,=士j,0,令,将矩阵的特征值代入方程,=0， K1=1, K I K bk K( bk )2 12nb3nb,enb,bk,30,10/21/2020,用201四0-03参-19数法。,31,矩阵微分方程的解,=j0 =-j0,bk2,nb3nb,2 0,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,=0， K1=1 2,e K1 K 2 j0 K3 ( j0 ) e,j,2, K1 K2 j0

27、 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,矩阵微分方程的精确解,这个精确解的前提条件是,nbk bbnb,. n,C C,+,bk,nb dt nb,tn1,t n,bk,这个式子只有在t=tn+1- tn内角速度矢量nb方向不变的条件下才有意义，由于转动 的不可交换性，当nb方向随时间变化时，角速度的积分是无意义的。 用方向余弦法求解姿态矩阵避免了欧拉角法方程退化的现

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

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

30、,Z sin k,iuZ sin ju,ux Z sin ,Z Z cos ,q3,z,q2,y,q1,q0, v- _, v- _, v- _,_, v-,2010-03-19,34,10/21/2020,四元数的基本概念, 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-

31、03-19,35,10/21/2020,坐标系的等效转动,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,Yr,b,Z,Zr,2010-03-19,36,10/21/2020,四元数的基本概念,如果用u表示欧拉轴向的单位矢量，则动坐标系的方 位，完全可由u和 两个参数来确定。用u和 两个参 数，可以构造一个四元数， 1,如果把u写成分量的形式则： Q cos usin i usin j usin k 2x2y2z 2 q0 q1i q2 j q3k, 2,q0 cos, 2,q1 ux sin, 2,q2 u y

32、sin, 12,q3 uz sin,Q cos u sin ,22,四元数是张量为1的四元数，即,Q (q 2 q 2 q 2 q 2 ) 2 1 0123,这样的四元数称作“规范化”的四元数，而用来描 述刚体定点转动的四元数就称作变换四元数。,u e2,2010-03-19,37,10/21/2020,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 对应元素相加,则,四元数相加，服从一般加法的交换律和结合律，即,

33、( ) ( ),（交换律） （结合律）,2010-03-19,38,10/21/2020,四元数理论,四元数与标量相乘 a a0 a1i a2 j a3k,式中a标量,各个元素分别乘以标量,(ab) (ba) a a (a b) a b a( ) a a,（分配律）,（交换律）,（结合律）,2010-03-19,39,10/21/2020,四元数理论,四元数与四元数相乘 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),乘积的矢量形式,2

34、010-03-19,40,10/21/2020,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 3m2 n2 0m2 2m0 3m1 1m3 n3 0m3 3m0 1m2 2m1,n0 0m0 1m1 2m2 3m3 ,nnn T,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,

35、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 m3m0 m2m1,n2 m2,m1 2 m0,n3 m3, , ,-矩阵四元数,Q(n) M (m)Q(),矩阵的“核”,41,10/21/2020,2010-03-19,42,四元数理论,M()和M*(m)除元素不同外，其核互为转置。 这种四元数乘积的矩阵形式，也可推广到三个 以上的四元数乘积。如：,Q( 。 。 ) M()Q( 。 ) M ()M (P)Q(m) M (P)Q( 。 ) M (P)M()Q(m)

36、,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),顺序相乘,逆序相乘,类似正交阵的乘积的转置或方阵乘积的求逆，也是逆序,42,10/21/2020,四元数理论 。 。 ,( 。 ) 。 。 ( 。 ) 。 ( ) 。 。 四元数的共轭 如果一个四元数为八=+ 则定义其共轭四元数为*=,结合律 分配律

37、,推理1 (八+M)*= 八*+M* 四元数之和的共轭等于共轭之和 推理2 (八M)*= M*八*两个四元数之积的共轭等于共轭 四元数等于两个四元数共轭之积取相反的顺序。 四元数的范数,四元数的范数定义为,2222,3,2,1,0, ,2010-03-19,43,10/21/2020,四元数理论,=八八* =八*八 八=1的四元数称为规范化的四元数。 八M =八 M=M 八,四元数的逆,则八-1八=八八-1,1,22,0123,2,2, ,2010-03-19,44,10/21/2020,3.2.3.3 矢量坐标变换的四元数描述,一个矢量r在参考坐标系（这里用地理坐标系作参 考系）轴向的分量形

38、式为 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则是四元数的矢量 部分。 显然，如果认为i, j, k和in, jn, kn重合，则四元 数的矢量部分就是三维空间的矢量r本身。,2010-03-19,45,10/21/2020,旋转矢量的坐标变换,定义假设矢量r绕通过定点“O”的某一轴转动了一个 角度，则和矢量固联的动坐

39、标系和参考坐标系之间的 变换四元数为：,22,Q cos usin,式中u为转轴方向的单位矢量。这个四元数的范数为,Q q2 q2 q2 q2 1 0123,转动前的矢量用r表示，转动后的矢量用r表示,则r和r的关系可由四元数来描述，即,称作“规范化”的四元数.,r Q。 r 。Q*,Q* cos usin四元数的共轭四元数 2 2,黄式两边同时左乘Q*右乘Q得 因为,Q*。r。Q Q*。Q 。 r 。 Q*。Q Q*。Q Q 。 Q* 1,2 2 (cos usin) 。 (cos usin) cos sin 1,22,22,2,2,r Q* 。 r。Q,2010-03-19,46,10/2

40、1/2020,2010-03-19,47,证明,A O 当矢量r绕OO旋转时，矢端A在空间的轨迹是一个圆，这个 圆平面和转轴垂直，圆心为O在旋转轴上。在圆上取一点B， 使AOB=90。，则按矢量关系有下列关系式：,O,r,r,A,B,u,A,O,O,B,OO,u,O u,r,A,OO (r u)u,OO OA r,因为ab=|a|b|cos,OA =r OO= r(ru)u OB= uOA= u( r(ru)u),= ur (ru)uu= ur,uu=0,OB= ur,47,10/21/2020,2010-03-19,48,证明, A,B,OAcos A,O,OBsin,O,O,r,r,A,B

41、,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,q0 OA = r(ru)u OB= ur,q, sin (ur) cos r (ur)sin v-2 _ 2 v- _2,八= 0 m0+0 m+ m0 m+ m OA = OA cos+ OBsin = cos ( r(ru)u)+ ur sin r=OO+OA =(ru)u+cosr cos (ru)u+sin (ur),=(1 cos)(ru)u +cos r+sin

42、(ur),Q。r cosr usinr (ur)sin,222,48,10/21/2020,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 _2 qq*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),2,222,2q, = sincos

43、(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) u 222 2 第一项与第五项相消，第六项为零第四项与第七项合并 sin2 (u r)u cos2 r sin (u r) sin2 (u r) u,222,22,22,q0,q0，q,49,10/21/2020,证明 QrQ* sin2 (u r)u cos2 r sin(u r) sin2 (u r) u,222,利用 QrQ*, sin2 (u r)u co

44、s2 r sin (ur) sin2 (uu)r (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)u r=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,50,10/21/2020,固定矢量的坐标变换,如果矢量固定不动，而动坐标系相对参考坐标系转动了一个角度， 则以四元数描述的矢量在两个坐标系上的分量的变换关系为,r Q

45、。r 。 Q*,如果将四元数Q,R,M的四个元写成列矢量，即表示成,3.2.3.4 四元数和方向余弦矩阵的关系,Q(q) 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,51,10/21/2020,3.2.3.4四元数和方向余弦矩阵的关系,Q(Rb ) M(Q*)M(Q)Q(Rn ),Q(R ) 0 xyz ,T,

46、bb,bb,Q(R ) 0,xyz ,T,n q0 q1 q2 q3 qq,n,n,n,2 ,qq q,M (Q*) q qq q2, q, q3, q0q1,q1q0,1,0,3,2,3,10,q2q3,q,2 ,q q ,q2,M *(Q) ,q3,0 q3q0q1 q2 q1q0 q0q1q2q3 qqq,3,1,yn ,n , qx,zn , q1 q2 q3 0 qq,2 ,q2 q3,2 ,qq,q0,03 q3q0q1 q2 q1q0, q2,b , q3,03 q3q0q1 q2 q1q0,yb ,x,zb , 0 ,1,1,2010-03-19,52,10/21/2020,四

47、元数与姿态矩阵的关系, 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,0 2(q1q2 q0q3 ), q2 q2 23,q0 q1,z,2(q q,0 0 0,0,0,2222,2301,02,13,2,2 23,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

48、 q2 q2 q2 0123 2(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 q2 23,q0 q1,2(q1q2 q0q3 ), q2 q3,012,2301,1302,22,2222,b,2010-03-19,53,10/21/2020,由四元数计算姿态矩阵,由 b Cbn 可知上式可写为,n,zb T31T33 zn ,23 n ,y,T13 xn ,y T,T11T12 22 T32,b ,xb ,TT,21,T23 2(q2q3 q0q1 ) T31 2(q1q3 q0