e1Θ(T)2?Θ(T)??Θ(T)??2?2??Θ(T)????????I?????2!3!?2?2223?Θ(T)??2?????n!44n??(T)???(T)?Θ(T)??(T)???(T)?Θ(T)II???2?2?22?2??Θ(T)??????????2??I??????2!3!4!5!?2??35???(T)?2??(T)?4????(T)???(T)?????????2??Θ(T)???(T)??2?2?2??????????I?1??????????2!4!3!5!???(T)??2????????(T)Θ(T)?(T)?Icos?sin2?(T)2将式（2.4-42）代入式（2.4-39），可得
??(T)Θ(T)?(T)?Q(T)??Icos?sin?Q(0)2?(T)2?? （2.4-42）
????????(T)? （2.4-43） ?cos????(T)??θ(T)?(T)???2???Icos???sin????Q(0)?Q(0)?θ(T)?(T)2?(T)2????2??????sin?2???(T)?若将研究时间区间从[0,T]改为[tm?1,tm]，则根据上式有
iib(m?1) （2.4-44） Qb(m)?Qb(m?1)Qb(m)??m??cos??2b(m?1)? （2.4-45） Qb(m)??Δθ???msinm??2????m?iib(m?1)其中，Qb、Qb分别表示tm?1和tm时刻的姿态变换四元数，Qb是从tm?1时刻到tm时刻的姿态四(m?1)(m)(m)b元数变化，且有Δθm??ωibdt和??m?Δθm。式（2.4-44）和式（2.4-45）便是姿态更新的四元数递
tm?1tm推计算公式，但应当注意到，这是在假设b系在时间段[tm?1,tm]内为定轴转动时才能严格成立的。
2.5等效旋转矢量微分方程及其泰勒级数解
方向余弦阵更新算法式（2.3-25）和四元数更新算法式（2.4-44）两种算法完全等价，只是后者计算量稍小一点而已。两种算法都是假设在更新周期内动坐标系作定轴转动才能严格成立，如果不是定轴转动，由角增量直接求解变化矩阵或四元数，容易引入转动不可交换误差。实际捷联惯导系统中的陀螺测量信号输出为角增量形式，比如激光陀螺，或者信号输出为角速率形式，但是为了降低噪声，常常采用了高频采样再滤波平滑处理后降频输出，这种方式也接近于增量输出方式，而非瞬时角速率输出。为了减小不可交换误差的影响，研究者们提出了先通过角增量求解等效旋转矢量、再利用等效旋转矢量更新方向余弦阵或四元数的方法。
2.5.1 等效旋转矢量微分方程
对于式（2.4-36），为书写简洁暂且省略角标，重写为
ω?u??usin??u?u(1?cos?) （2.5-1）
根据附录A式（A-6）～（A-8），有
26
u????(???)??? ??(???)，
， ??u?u?u???2?2?3将它们代入式（2.5-1），得
???(???)???(???)???ω?????sin??(1?cos?)?232????? （2.5-2）
????sin????(???)???(1?cos?)2??1?????2??上式移项，得
??ω?(1?cos?)????sin????(???) （2.5-3）
?1??2????2??这是旋转矢量微分方程的一种表示形式，但稍显不足的是等式右边依然含有微分项?，不利于实际使用。下面根据反对称阵的幂方特性，由式（2.5-2）求解?。
若记
a?则式（2.5-2）可简写为
(1?cos?)?2?sin??2 （2.5-4） ,b??1??/????ω???a????b(??)2? （2.5-5a）
使用?左叉乘上式的两边，可得
??ω?????a(??)2??b(??)3??????a(??)2??b?2??? （2.5-5b） ?(1?b?2)????a(??)2?再次使用?左叉乘上式的两边，可得
(??)2ω?(1?b?2)(??)2??a(??)3???a?????(1?b?)(??)?将上述三式合并在一起写成矩阵形式，有
222 （2.5-5c）
ab?????ω??1?01?b?2??????????ω? （2.5-6） a??????22?2?2?1?b???0?a???(??)????(??)ω??若将式（2.5-6）视为关于?、???和(??)2?的三元线性方程组，则不难求解得 ??ω?a????b(??)2??ω?a?(1?b?2)??ω?a(??)2ω?2222??(1?b?)?a?b?a?2??ω?(1?b?2)(??)2ω??2222??a??(1?b?) （2.5-7）
aa2?b(1?b?2)?ω???ω?(??)2ω22222222(1?b?)?a?(1?b?)?a?最后，重新将式（2.5-4）中的a和b表达式代入上式，得 1(1?cos?)2?(??sin?)sin?2??ω???ω?(??)ω22?2(1?cos?)12(1?cos?)??sin??ω???ω?(??)2ω222?(1?cos?)11?ω???ω?22?
（2.5-8）
??sin??21??2(1?cos?)?(??)ω??27
应用三角恒等式，上式还可等价于
??????2sincos11?22?(??)2ω??ω???ω?2?1??2??2? （2.5-9） ?2?2sin?2?11?????ω???ω?2?1?cot?(??)2ω2??22?这便是常见的等效旋转矢量微分方程，它是利用等效旋转矢量进行转动不可交换误差补偿的数学理论基础，该式最早于1971年由学者J E Bortz推导提出，后来通常称之为Bortz方程。
bii至此，可总结坐标系相对转动的四种数学描述，即角速度ωib、姿态阵Cb、四元数Qb和等效旋转矢
量?ibb，之间的关系，如图2.5-1所示。更多姿态描述之间的相互转换关系参见附录B。
b ωib ?ibbCbiQbi
图2.5-1 转动的四种数学描述之间的关系
Bortz方程（2.5-9）虽然在理论上是严格成立的，但实际应用时略显繁杂。当转动角度???为小量时，常常将方程右边三角函数cot(?/2)用泰勒级数展开，进行如下近似
??11???21???ω???ω?2?1???????(??)2ω2??2??32?? （2.5-10） 11?ω???ω?(??)2ω212如果再忽略上式右端三阶小量的影响，还可进一步近似为
1??ω???ω （2.5-11）
2对上式两边同时在时间段[tm?1,t]内积分，为了表述更加清晰，各符号标明时间变量参数，可得
tt11t?(t)??(tm?1)??ω(?)??(?)?ω(?)d???ω(?)d????(?)?ω(?)d?tm?1tm?122tm?1 （2.5-12）
1t?Δθ(t)???(?)?ω(?)d?2tm?1即
1t?(t)??(tm?1)?Δθ(t)???(?)?ω(?)d? （2.5-13）
2tm?1其中，Δθ(t)??ω(?)?d?表示从tm?1时刻开始由角速度累积的角增量且显然有Δθ(tm?1)?0。
tm?1t将式（2.5-13）右端整体再次代入其第三项的积分号内，可得
1t?1???(t)??(tm?1)?Δθ(t)????(tm?1)?Δθ(?)???(?1)?ω(?1)d?1??ω(?)d?2tm?1?2tm?1?11t （2.5-14） ??(tm?1)?Δθ(t)??(tm?1)?Δθ(t)??Δθ(?)?ω(?)d?22tm?11t?????(?1)?ω(?1)d?1?ω(?)d?4tm?1tm?1在时间段[tm?1,?]内，如果?(?1)是小量，式（2.5-14）右边的第5项远小于第4项，即有
28
??t?tm?1tm?1?(?1)?ω(?1)d?1?ω(?)d???12??t?tm?1tm?1ω(?1)d?1?ω(?)d???ttm?1Δθ(?)?ω(?)d? （2.5-15）
因而，式（2.5-14）可近似为
?(t)??(tm?1)?Δθ(t)??(tm?1)?Δθ(t)?1tΔθ(?)?ω(?)d? （2.5-16） ?tm?12特别地，若假设在tm?1时刻的等效旋转矢量?(tm?1)?0，则?(t)可表示从tm?1时刻开始的等效旋转矢量“增量”，式（2.5-16）可简化为
?(t)?Δθ(t)?其中，记
1tΔθ(?)?ω(?)d? （2.5-17） 2?tm?1?Δθ(t)?σ(t)σ(t)?1tΔθ(?)?ω(?)d? （2.5-18） ?tm?12表示等效旋转矢量增量?(t)与角增量Δθ(t)之间的差异，通常称为转动不可交换误差的修正量。
对式（2.5-17）两边同时求导，可得
?(t)?ω(t)?Δθ(t)?ω(t) （2.5-19）
上式可以看作是等效旋转矢量微分方程（2.5-11）的再近一步近似。需要特别指出的是，上式成立的前提条件是：?(tm?1)?Δθ(tm?1)??且需保证?(t)为小量，?(t)越小近似精度越高。
此外，在式（2.5-16）中，若令t?tm且设?(tm?1)?0，则有 1?(tm)??(tm?1)?Δθ(tm,tm?1)??(tm?1)?Δθ(tm,tm?1)?σ(tm,tm?1) （2.5-20）
2其中，?(tm?1)和?(tm)分别表示上一时刻（tm?1时刻）和当前时刻（tm时刻）的等效旋转矢量；将Δθ(tm)、
12σ(tm)分别更明确地记为Δθ(tm,tm?1)和σ(tm,tm?1)，表示从上一时刻至当前时刻的角增量和修正量。式（2.5-20）可视为等效旋转矢量递推的近似计算公式，运算简单且计算量小，但是在实际算法中并不常用，究其原因，主要是随着递推步数的增加和?(tm)变大，误差会不断积累。实际应用时，一般总是假设
?(tm?1)?0，再根据式（2.5-17）计算等效旋转矢量?(tm)，相当于只递推计算一步，这样有利于保证等
效旋转矢量始终为小量，降低公式推导过程中的近似误差。在获得?(tm)之后，改等效旋转矢量递推计算为方向余弦阵或四元数递推，即改用方向余弦阵或四元数完成姿态递推更新，以四元数为例（方向余弦阵类似），等效旋转矢量与四元数相配合的姿态更新算法如下
iib(m?1) （2.5-21） Qb(m)?Qb(m?1)Qb(m)b(m?1)Qb(m)?m??cos?2? （2.5-22）
??????msinm??2???m?其中，将?(tm)简记为?m，且有?m??m。注意，比较式（2.4-45）与式（2.5-22），两者虽然在形式上完全一样，但本质含义上存在重要区别：前者仅简单地使用角增量进行变化四元数计算，理论上只能适用于定轴转动情形；而后者在求解等效旋转矢量过程中考虑了转动不可交换误差的补偿。
2.5.2 等效旋转矢量微分方程的泰勒级数解
在实际应用中，从高精度捷联惯导陀螺中采样获得的往往是在一定采样间隔内的角增量信息，下文的主要目的就是借助式（2.5-19）由采样角增量求解等效旋转矢量。
针对算法式（2.5-21）和（2.5-22），不妨将时刻tm?1重新记为0时刻，陀螺在姿态四元数更新时间段[tm?1,tm]（即[0,T]，T?tm?tm?1）内可进行若干次等间隔角增量采样，暂且假设陀螺角速度输出为线
29
性形式
ω(?)?a?2b? 0???T （2.5-23）
则陀螺输出角增量为
Δθ(?)??ω(??)d???a??b?2 （2.5-24）
0?其中，a和b均为常数向量。
现计算角速度ω(0)和角增量Δθ(0)，以及它们的各阶导数，得
?ω(0)?a? （2.5-25） ?ω(0)?2b?ω(i)(0)?0i?2,3,4,??Δθ(0)?0??Δθ(0)?ω(0)?a （2.5-26） ??Δθ(0)?ω(0)?2b?Δθ(i)(0)?ω(i?1)(0)?0i?3,4,5,?再记
β(?)?Δθ(?)?ω(?) （2.5-27）
根据如下求导规则
0(n)1(n?1)(1)2(n?2)(2)(xy)(n)?Cnxy?Cnxy?Cnxy?n?Cnxy(n) （2.5-28）
求β(0)及其各阶导数，可得
?β(0)?0?01?β(0)?C1Δθ(0)?ω(tk)?C1?0?a?a?0 （2.5-29） ?012?β(0)?C2Δθ(0)?ω(tk)?C2Δθ(0)?ω(tk)?C2?0?2b?a?2?a?2b?2a?b?β(i)(0)?0i?3,4,5,?根据等效旋转矢量微分方程式（2.5-19），可计算得?(0)的各阶导数，如下
1??(0)?ω(0)?β(0)?ω(0)?a?2???(0)?ω(0)?1β(0)?ω(0)?2b? （2.5-30） 2??11??(0)?ω(0)?β(0)?β(0)?a?b22?(i)???(0)?0i?4,5,6,若将?(t)视为光滑函数且在t?0处展开成泰勒级数，并将式（2.5-30）代入，可得
T2T3?(T)??(0)?T?(0)??(0)??(0)?2!3!T32?0?Ta?Tb?a?b6T32?Ta?Tb?a?b6 （2.5-31）
式（2.5-31）中包含两个未知向量参数a和b，为了消去a和b并求解出?(T)，需在采样时间段[0,T]内进行两次角增量采样，记为
T/2?TT22T/2Δθ?ω(?)d??a??b??a?b?0?1?024 （2.5-32） ?2?Δθ?Tω(?)d??a??b?2T?Ta?3Tb2?T/2T/2??24由上式可求得以角增量表示的常数向量a?(3Δθ1?Δθ2)/T和b?2(Δθ2?Δθ1)/T2，再将其代入式
30