再假设
?D11D12D13??1??0??0??,?0?,?1?,?0? （2.2-20） D??DDDi?j?k?212223iii?????????????D31D32D33???0???0???1??将式（2.2-19）和（2.2-20）代入过渡矩阵式（2.2-2），得
?Dii?iiDji?iiDki?ii??D11D12D13????D??D （2.2-21） P??Di?jDj?jDk?jDDiiiiii212223??????Dii?kiDji?kiDki?ki????D31D32D33??这表明，矩阵D正好是从参考坐标系i系到动坐标系b系的过渡矩阵，它也是从b系到i系的坐标变换矩阵，可重新记式（2.2-18）为：
iCb?I?sin?(u?)?(1?cos?)(u?)2 （2.2-22）
进一步，若记???u和???，则有u????，将其代入式（2.2-22），可得
iCb?I?sin??(??)?1?cos??2(??)2 （2.2-23）
这里?称为等效旋转矢量（Rotation Vector），根据图2.2-1，等效旋转矢量的矢量方向表示转轴方向，而模值大小表示旋转角度大小。从转动的物理含义上看，(??2kπ)u(k?0,1,2,)表示的是相同的转动，这可通过将其代入式（2.2-23）进行验证，即Cbi与k的取值无关。如果限定转角的取值范围0???2π，则等效旋转矢量和方向余弦阵之间存在一一对应关系。从坐标系的定轴转动中可以看出，等效旋转矢量
ib（或单位转轴）是一种比较特殊的矢量，它在i系和b系下的坐标值完全相等，即有?i?Cb???b（或
b。有时为了更加明确地显示b系相对于i系的转动关系，可利用下角标进行标注，比如?ibb（或uib）。 ui?ub）
将式（2.2-23）与向量反对称阵的矩阵函数（2.1-20）对比，可看出两者形式上完全一致，这说明式（2.1-20）中三维向量具有等效旋转矢量的物理含义。根据式（2.1-30）和（2.1-31）还可以看出，方向余弦阵的一个特征值恒为1（?1??1），与其对应的单位特征向量（u1）表示转轴方向；方向余弦阵的另
?和?3?）即为等效旋转矢量模值的幂指函数e外两个共轭特征值（?2量的转角大小。
i若将方向余弦阵Cb看作是等效旋转矢量?的函数，可简记为
?j?，特征值的幅角表示等效旋转矢
iCb?MRV(?) （2.2-24）
并且有
iTCib?(Cb)?MRV(??) （2.2-25）
特别地，若分别取?1???100?、?2???010?和?3???001?，则有
2TTT0?000??000??1??(1?cos?)?00?1???0cos?iCb(?1)?MRV(?1)?I?sin??00?1????????010???010????0sin??001??001??cos???(1?cos?)?000???0iCb(?2)?MRV(?2)?I?sin??000?????????100????100?????sin??0?10??0?10??cos???(1?cos?)?100???sin?iCb(?3)?MRV(?3)?I?sin??100?????????000??000????0意转动都可以由三次基本转动合成，参见附录B。
16
2? （2.2-26a） ?sin???cos???0sin?? （2.2-26b） 10??0cos???02?sin?cos?00? （2.2-26c） 0??1??上述三式分别称为以坐标轴为旋转轴的基本转动矩阵，或称Givens矩阵或初等旋转矩阵，空间的任
由等效旋转矢量与方向余弦阵之间的一一对应关系可知，方向余弦阵虽然含有9个元素，但它只有3个独立参数，包含了6个约束条件，即行向量之间两两正交（3个）及每个行向量模值均为1（3个）。3个独立参数即为3个自由度，这与三维空间中的转动自由度是一致的。
2.3方向余弦阵微分方程及其求解
2.3.1 方向余弦阵微分方程
假设动坐标系（b系）和参考坐标系（i系）具有共同的原点，b系相对于i系转动的角速度为ωib，
i从i系到b系的坐标系变换矩阵记为Cb，它是时变矩阵，再假设在i系中有一固定矢量r，则固定矢量r在两坐标系下投影的转换关系（即坐标变换），为
ibri?Cbr （2.3-1）
将上式两边同时微分，得
ibibri?Cbr?Cbr （2.3-2）
注意到，r是i系中的固定矢量，则有ri?0；由于b系相对于i系的角速度为ωib，则在b系上观察rb的角速度应为?ωib（或写为ωbi??ωib），并且有rb??ωib?rb，因而式（2.3-2）可化为
ibibibib0?Cb(?ωib?rb)?Cbr 即 Cbr?Cb(ωib?)rb （2.3-3）
由于上式对于任意i系固定矢量r都成立，任选三个不共面的非零矢量r1、r2和r3，则有
ibbbibbbb?Cb?r1r2r3???Cb(ωib?)??r1r2r3??
bbb显然矩阵??rrr123??可逆，所以必定有
iibCb?Cb(ωib?) （2.3-4）
这便是方向余弦阵微分方程，或称为姿态阵微分方程，它建立了动坐标系相对于参考坐标系之间方向余弦阵与动坐标系运动角速度之间的关系。
此外，通过如下矢量运算关系
iibibibωib?ri?Cb(ωib?rb)?Cb(ωib?)rb?Cb(ωib?)Cibri
比较上式两边，可得反对称阵的相似变换公式
iib(ωib?)?Cb(ωib?)Cib （2.3-5）
ii?1iT根据式（2.3-4）和式（2.3-5），并考虑到Cb是单位正交阵，即有(Cb)?(Cb)?Cib，容易证明以
下四种方向余弦阵微分方程是相互等价的：
iibCb?Cb(ωib?) （2.3-6a） iii （2.3-6b） Cb?(ωib?)CbbCib?(ωbi?)Cib （2.3-6c） iCib?Cib(ωbi?) （2.3-6d）
2.3.2 方向余弦阵微分方程的求解
iib以下讨论微分方程Cb?Cb(ωib?)的求解，为了书写简便，略去各量的上下角标，但明确写出时变量
b的时间参数，并记反对称阵Ω(t)??ωib?(t)???，将姿态阵微分方程表示为
C(t)?C(t)Ω(t) （2.3-7）
显然，这是一个典型的时变系数齐次微分方程，需采用毕卡（Peano-Baker/Picard?）法求解。
首先，对式（2.3-7）在时间段[0,t]上积分，得
17
C(t)?C(0)??C(?)Ω(?)d? （2.3-8）
0t由于上式右边第二项被积函数依然含有待求的C(t)，重复使用上式右边整体代入积分号内，第一次代入，得
t?C(t)?C(0)???C(0)??C(?1)Ω(?1)d?1?Ω(?)d??0?0??tt??C(0)?C(0)Ω(?)d??C(?)Ω(?)d?Ω(?)d? （2.3-9）
?0??00111tt??C(0)?I??Ω(?)d?????C(?1)Ω(?1)d?1Ω(?)d???000??第二次代入，可得
tt?C(t)?C(0)?I??Ω(?)d????Ω(?1)d?1Ω(?)d????000?? （2.3-10）
??t00????10C(?2)Ω(?2)d?2Ω(?1)d?1Ω(?)d?依此不断代入，便可得到以无限重积分表示的所谓的毕卡级数
tt?C(t)?C(0)?I??Ω(?)d????Ω(?1)d?1Ω(?)d??000? （2.3-11）
??才容易得到闭合解。
t00????10Ω(?2)d?2Ω(?1)d?1Ω(?)d?????上述级数是收敛的，但一般情况下得不到闭合形式的解（初等解），只有在所谓的定轴转动特殊情形下
考虑时间段[0,T]，对于任意时间参数t,??[0,T]，假设转动角速度满足如下可交换性条件
Ω(t)Ω(?)?Ω(?)Ω(t) （2.3-12）
则有
?t0Ω(t)Ω(?)d???Ω(?)Ω(t)d? 即 Ω(t)?Ω(?)d???Ω(?)d?Ω(t) （2.3-13）
000ttt现计算以下微分
2d?td?t????tΩ(?)d??Ω(?)d??Ω(?)d??????0???0????0?dt?dt?ttt?? （2.3-14）
?Ω(t)?Ω(?)d???Ω(?)d?Ω(t)?2?Ω(?)d?Ω(t)000注意，上式的最后一个等号是在式（2.3-12）条件下才能成立的。根据式（2.3-14），有如下积分成立
2t?1?t?Ω(?)d? （2.3-15） ?0?0Ω(?1)d?1Ω(?)d??2????0?同理，有
23t?2?1t1?1?t???Ω(?1)d?1Ω(?)d???Ω(?)d? （2.3-16） ?0?0?0Ω(?2)d?2Ω(?1)d?1Ω(?)d???02?????0??0?6?等等。
至此，在可交换条件（2.3-12）下，毕卡级数式（2.3-11）可简化成闭合解形式
23t?1?t1?t???C(t)?C(0)?I??Ω(?)d??Ω(?)d??Ω(?)d?????0?3!???0?0??2!??? （2.3-17）
Ω(?)d??C(0)e?0t下面说明可交换条件式（2.3-12）的几何含义。 设角速度的分量形式为ω(t)????1x?1y?1z??和ω(?)????2x?2y?2z??，则有
???1y?2y??1z?2z??1y?2x?1z?2x?? （2.3-18）
Ω(t)Ω(?)??ω(t)???ω(?)?????1x?2y??1x?2x??1z?2z?1z?2y???1x?2z?1y?2z??1x?2x??1y?2y???18
TT
???1y?2y??1z?2z??2y?1x?2z?1x?? （2.3-19）
Ω(?)Ω(t)??ω(?)???ω(t)?????2x?1y??1x?2x??1z?2z?2z?1y???2x?1z?2y?1z??1x?2x??1y?2y???令上述两式相等，可解得
??1x?2y??2x?1y???1x?2z??2x?1z （2.3-20） ??????2y1z?1y2z如果式（2.3-20）中所有的角速率分量都不为0，则有
?1x?1y?1z （2.3-21） ???2x?2y?2z这表示在时间段[0,T]内，b系相对于i系的转动角速度方向始终不变，即为定轴转动；如果式（2.3-20）中某些角速率分量为0，也容易得出该转动是定轴转动的结论；如果所有角速度分量均为0，即为静止，它亦可视为定轴转动的特殊情形。综合上述三种情况，说明闭合解式（2.3-17）只有在定轴转动情形下才能严格成立。
针对时间段[0,T]，记角增量θ(T)??ω(?)d?且模值?(T)?θ(T)，考虑到矩阵指数函数式（2.1-19），
0T则有
Ω(?)d?sin?(T)1?cos?(T)2e?0?e[θ(T)?]?I??θ(T)???2?θ(T)?? （2.3-22）
?(T)?(T)因此，式（2.3-17）在T时刻的解可简写为
0 （2.3-23） C(T)?C(0)CTT其中
sin?(T)1?cos?(T)2?θ(T)???2?θ(T)?? （2.3-24） ?(T)?(T)i若将时间区间从[0,T]更改为[tm?1,tm]，且假设已知tm?1时刻的方向余弦阵为Cb，[tm?1,tm]时间段(m?1)0CT?I?的角增量为Δθm??tmtm?1ib的公式为 ωib(t)dt且记模值??m?Δθm，则求解tm时刻的姿态阵Cb(m)iib(m?1) （2.3-25） Cb(m)?Cb(m?1)Cb(m)sin??m1?cos??m?I?(Δθm?)?(Δθm?)2 （2.3-26） 2??m??mb(m?1)Cb(m)上述两式便是姿态阵离散化更新的递推计算公式。值得注意的是，式（2.3-26）严格成立的前提条件是b系在时间[tm?1,tm]内必须是定轴转动，该式与式（2.2-23）相比，两者在形式上完全一致，因而可以认为定轴转动时角增量Δθm是以b(tm?1)系为参考，b(tm)系相对于b(tm?1)系转动的等效旋转矢量；否则，如果可交换性条件式（2.3-12）不成立，依然简单地利用式（2.3-26）进行计算将会引起姿态求解的不可交换误差，不可交换性是高维时变系统（时变矩阵微分方程）的普遍特性。
b同理，类似于式（2.3-25）和（2.3-26），可求得另一种姿态阵微分方程表示形式Cib?(ωbi?)Cib的更
新公式，为
b(m)b(m?1) （2.3-27） Cib(m)?Cb(m?1)Ci??sin??m1?cos??m??)???)2?I?(Δθm(Δθm2??)??m(??m （2.3-28）
sin??m1?cos??m?I?(Δθm?)?(Δθm?)22??m(??m)Tb(m)Cb(m?1)其中，Δθm???
mb(m)bb??Δθm?。显然，有Cb?b(m?1)?成立。 ωbi(t)dt???ωib(t)dt??Δθm、??m(m?1)??Cb(m)?tm?1tm?1tmt19
对于非定轴转动下的姿态更新方法，主要是通过等效旋转矢量算法进行不可交换误差补偿，这些内容将在本章后续小节详细介绍。
2.4姿态更新的四元数表示
四元数（quaternion）的概念最早在1843年由数学家哈密顿（W R Hamilton）提出，它可用于描述刚体转动或姿态变换，与方向余弦阵相比，四元数表示方法虽然比较抽象，但却十分的简洁。
2.4.1 四元数的基本概念
顾名思义，四元数就是包含四个元的一种数，可表示为
Q?q0?qv?q0?q1i?q2j?q3k （2.4-1）
其中，q0、q1、q2和q3都是实数，q0称为实部、qv?q1i?q2j?q3k称为虚部。四元数可以看作是复数概念的扩充，有时也称其为超复数，当q2?q3?0时四元数即退化为复数。四元数的虚数单位i,j,k之间满足如下乘法运算规则
?ii?jj?kk??1 （2.4-2a） ??ij?k,jk?i,ki?j,ji??k,kj??i,ik??j即
i2?j2?k2?ijk??1 （哈密顿公式） （2.4-2b） 其中，运算符“”表示四元数乘法运算，在不引起歧义的情况下可写成“?”号或直接省略。式（2.4-2a）中第一行运算规则与复数中虚数的运算规则完全相同；第二行运算规则与三维向量空间中坐标轴单位矢量的叉乘运算规则相同。四元数可以看作是四维空间中的一种数，但因其虚部单位矢量的叉乘运算特点，也可将四元数的虚数部分qv?q1i?q2j?q3k看成是在三维空间中的映象（image），反之，一个三维矢量可以看作是一个零标量四元数。
假设有如下三个四元数
P?p0?pv?p0?p1i?p2j?p3k （2.4-3a） Q?q0?qv?q0?q1i?q2j?q3k （2.4-3b） S?s0?sv?s0?s1i?s2j?s3k （2.4-3c）
两个四元数相等当且仅当它们的四个元分别对应相等，即
?p0?q0?p?q?11P?Q???p2?q2??p3?q3 （2.4-4）
两个四元数之间的加（或减法）定义为
P?Q?(p0?p1i?p2j?p3k)?(q0?q1i?q2j?q3k)?(p0?q0)?(p1?q1)i?(p2?q2)j?(p3?q3)k或者记为
（2.4-5a）
P?Q?(p0?pv)?(q0?qv)?(p0?q0)?(pv?qv) （2.4-5b）
容易验证，四元数的加法满足交换律和结合律，即有P?Q?Q?P和(P?Q)?S?P?(Q?S)。
考虑到运算规则（2.4-2a），两个四元数的乘法结果为
20