三阶算法：(40,)2C(n+ 1)=C(n)[ + (1(8+(2.60)62泗阶算法：(46.)3((8 ×)1C(n+ 1) =C(n)EI +(1-46 X-(2.61)624*1（四）方向余弦矩阵坐标系的转动校正由(2.51)式Cg = Cfw X = C(e X - a, ×)由相似变换gXCyf X Cg得C-CI×-× Cf(2.62)(2.62)式中第一项为载体坐标系的转动对方向余弦阵的校正。此时把导航坐标系（这里为地理坐标系）看作是不动的，相当于惯性坐标系。由Cf = Cfate ×得Ch+, = CtBACa+n(2.63)式中ACha*i eex(2.62)式中第二项为地理坐标系的转动对方向余弦矩阵的校正。此时把载体坐标系看作是不动的，相当于惯性坐标系。由Cf -- at. × Ct得Cr+B=AC+1Ch+n(2. 64)式中ACra+n = e-x（2.64）式还可写成ChatACratCACt(2. 65)载体坐标系转动角速度通常比较大，因此载体坐标系转动对方向余弦阵的校正频率要高，且需要采用高阶（三阶或四阶）算法。耐导航坐标系转动角速度f较小，因此导航坐标系转动对方向余弦阵的校正频率可以低，且可以采用低阶（二阶）算法。（五）方向余弦矩阵的正交化姿态矩阵应是正交阵，但由于陀螺的误差和计算误差，使计算得到的姿态阵失去正交性，从而给比力的变换带来误差。为了消除这种误差，需要对计算的姿态阵周期地进行正交化。矩阵的最优正交化，就是找一个最接近计算矩阵的正交阵。最接近的概念是正交阵和计算阵的差值范数最小，即欧几里德空间的距离最短。我们用℃表示计算的矩阵，C。表示正交化后的矩阵。如果有IC。-C要小或C。Cl最尔则称C。是C的最优正交化阵。设N=C。-C"△Tr[(C。--C)r(C。-C)]由aN/ac0，得C,- (CCT)t(CT)-1(2.66)-36
C。= C(CrC)-(2. 67)或机上执行算法采用级数或送代算法。表示E4CC-1则(2.66)式可以写成C,= C(1 + E)-++3XE+.=C[ 22X42×4X6取前两项，则(C - C1 -写成选代形式，用X~表示初值，X+1为C，则上式可以写成3XN1(2, 68)XN+iXNXIXN-2E-CTC-1表示则(2.66)式可以写成C。=(I +E)(CT)-I1.1 X3LEE+E + ..J(CT)-I-[1 +2X42X4X62取前两项，则C = [1 + E(CT)-1 = [1 + (CCT - ](C'r)-1写成选代形式[(XT)- +XN)(2.69)XN+12用（2.68）、（2.69)式进行矩阵正交化，送代2～3次就可以了。用方向余弦法计算姿态阵，没有方程退化问题，可以全姿态工作，但需联立求解九个微分方程，计算量较大。三、四元数法四元数理论是数学中的一个古老的分支，在空间技术和捷联式惯导系统中得到了实际应用。（一）四元数的基本概念四元数是一个由四个元构成的数，其形式为Q=(g091-924.)=40+gi+92j+9k=40+9式中g。为标量：g为矢量：而i，j遵守下列相乘规则：i.i=j.j=kk=-lioj=-ji-k(2.70)j.k-oj=ikoim-i.kmi式中符号“，”表示四元数相乘。此符号也可不用，但需记牢上述运算规则。-37-
在刚体定点转动理论中，根据欧拉定理，动坐标系相对参考坐标系的方位，等效于动坐标系绕某一个等效转轴转动一个角度6。如果用u表示等效转轴方向的单位向量如图2.19所示，则动坐标系的方位完全由u和两个参数来确定。用u和6可构造一个四元数：e0Q = cos+usin(2.71)2这个四元数的范数为Q=+++(2.72)=1称作“规范化”的四元数，也叫变换四元数。这样，图2.19坐标系等效转动我们就把三维空间和一个四维空间联系起来，用四维空间中四元数的性质和运算规则来研究三维空间中的刚体定点转动问题。（二）矢量坐标变换的四元数描述三维空间的一个矢量，可以看作是标量为零的四元数。1.旋转失量的坐标变换假定失量绕通过定点0的某一转轴转动了一个角度?，则转动四元数为68Q= coS+usinz如转动后的失量用r表示，则以四元数描述的一和的坐标变换关系为1r=Qr.Q(2.73)60式中-usinQ:=cos22称为Q的共轭四元数。因QQ=Q.Q=1故r=Q:.r.Q(2.74)2.固定矢量的坐标变换如果失量固定不动，雨动坐标系相对参考坐标系转动了一个角度，则以四元数描述的失量在两个坐标系上的分量的变换关系为[3]R,=Q.R..Q(2.75)R.-Q.R,.Q(2.76)（三）四元数和方向余弦阵的关系1.四元数乘积的表达形式设有两个四元数=+++=+M=n-mi+nj+mk=m+m则按（2.70）式的相乘规则可得乘积38-
N-A.M(2.77)=入mu+Aom+moa-入.m+入×m(2.78)或N=n+ni+nj+nk式中ng=mg-a,nm-Ann=A+A+am-Amz(2.79)ng=Amg+Agmo+Aymi-Amsn=+am+a=am（2.77)式为四元数乘积的矢量形式，（2.79)式为四元数乘积的四元数形式。如果把四元数N，4，M的四个元写成列矢量，即表示成Q(n) =[no n nz ns"Q() =[ Q(m)=[mommgm,J则按（2.79)式可以把四元数乘积写成矩阵形式：一[no]2o—2一MImo广二A2ntm(2.80)入2o一Amn2A入aA11-ng表示为Q(n)=M(1)Q(m)或者写成如下矩阵形式；no[no]nm2ms2msnymimom2(2.81)mo入min2m3mm?m1mgm.n.Q(n) = M*(m)Q(A)表示为M(>)和M（m）除元素不同外，矩阵的“核"互为转置。上述四元数相乘的矩阵表达式可以推广到三个或四个四元数相乘。2.四元数和方向余弦的关系将固定失量的坐标变换即（2.76)式写成矩阵形式，并以地理坐标系为参考坐标系，则有Q(R) = M(Q")M(Q)Q(R,)展开得000+++gTO00+--22(q192+q093)2(g193—9092)0--2(9293+Q09)2(q192—4093)V402(q193 + 9092)2(q293—Q091)-f+zr39
把第一行第一列去掉，则得2(g1+9093)--2(993—40)x-+-(2.82)2(914: --9043)2(9+J01)yeYx好一旺一好+赔2(414s+q092)2(y.939091)2按（2.82)式即可由四元数求得姿态矩阵。（四）四元数微分方程及其解由(2.71)式08Q=C0%2+usin2mg0+dQ1%）+(sin）6、du有(sinu(cosdt2du式中×ubuxu0dA69-Q则u[cosusin222bu=wg而w是的四元数形式。故四元数微分方程可以写作Q-.Q(2.83)如果把角速度表示在机体坠标系，则由坐标变换得,=Q··Q:代入（2.75)式得Q=-Q...Q1.Q由于Q:.Q=1:故得Q(2.84)把(2.83和(2.84)式写成矩阵形式，侧有Q()=M(ag)Q(g)(2.85)1m(a)Q(g)0(g)=(2.86)2四元数微分方程的求解，类似矩阵微分方程，可用毕卡（Peano-Baker)逼近法求解。按(2.86)式，其解为Q(g) = e (g(g), = 0g(t) = ettaig(0)简写成(2.87)40