（2.5-31）便可求得以角增量表示的等效旋转矢量二子样算法
2?(T)?(Δθ1?Δθ2)?Δθ1?Δθ2 （2.5-33）
3类似于前述二子样算法的推导思路，若设陀螺角速度输出为如下抛物线形式
ω(?)?a?2b??3c?2 0???T
并且在时间段[0,T]内进行三次角增量采样，分别记为
Δθ1??T/30ω(?)d?,Δθ2??2T/3T/3ω(?)d?,Δθ3??T2T/3ω(?)d?
则可求得等效旋转矢量三子样算法（过程不复杂但稍显繁琐，从略）
3357?(T)?(Δθ1?Δθ2?Δθ3)?Δθ1?Δθ3?(Δθ1?Δθ2?Δθ2?Δθ3) （2.5-34）
8080值得说明的是，基于泰勒级数展开的等效旋转矢量多子样算法是在式（2.5-19）的基础上推导的，式（2.5-19）又是式（2.5-9）在一定的近似条件下获得的；此外，高阶泰勒级数展开原则上要求函数足够光滑，而实际陀螺输出总会或多或少包含电气噪声，噪声并不反映载体的真实角运动，同时对角速度函数的光滑性也造成不良影响。因此，多子样算法的精度有限，并非子样数越多算法的实用精度就越高。
显然，若假设陀螺角速度输出为常值形式，此即简单的单轴旋转情形，则有等效旋转矢量单子样算法
?(T)?Δθ1??ω(?)d? （2.5-35）
0T特别地，还有一种称为“单子样+前一周期”的等效旋转矢量算法，它假设角速度输出为如下线性形式 ω(?)?a?2b? ?T???T 在时间段[0,T]内仅进行一次角增量采样，记Δθ1??ω(?)d?，但该算法在计算等效旋转矢量?(T)是还
0T会充分利用前一次的角增量信息，记Δθ0??ω(?)d?，通过如下方程组
?T0?T32??(T)?Ta?Tb?6a?b?T?2TΔθ?ω(?)d??a??b??Ta?T2b （2.5-36） ?1?00?00?Δθ0?ω(?)d??a??b?2?Ta?T2b??T?T??消去中间参量a和b，可求解得
?(T)?Δθ1?1Δθ0?Δθ1 （2.5-37） 12与单子样算法（2.5-35）相比，“单子样+前一周期”算法在陀螺采样频率相同的情况下提高了不可交换误差补偿精度；在相同采用频率下，“单子样+前一周期”算法与二子样算法（2.5-33）的精度量级相当，但前者提高了姿态输出频率。
2.6圆锥运动条件下的等效旋转矢量算法
2.6.1 圆锥运动的描述
19世纪50年代是机械陀螺仪飞速发展的一个重要时期，也正是在那时发现了著名的圆锥运动现象，即当陀螺仪在其旋转轴和输出轴出现同频不同相的角振动时，尽管其输入轴净指向不变（从整体上看没有随时间改变的趋势项），但陀螺仪在输入轴上还是会敏感到并输出常值角速率信号。在这种情况下，陀螺仪支架的运动角速度可描述如下
ω(t)??aΩsinΩtbΩcosΩtc? （2.6-1）
31
T其中，a、b和c均为常数，在x和y轴表现为同频但相位差90°的正弦角振动，振动频率为Ω，而在
z轴上表现为常值角速率。虽然输入轴z轴有角速率输入，但从长时间来看陀螺仪整体上并不绕着输入
轴产生明显偏转，这就是圆锥运动的神奇之处，曾颇令研究者们费解。
下面采用四元数描述来研究圆锥运动。假设动坐标系（b系）相对于参考坐标系（i系）的变换四元数为
?cos(?/2)??sin(?/2)cosΩt?? （2.6-2） Q(t)???sin(?/2)sinΩt???0??i式中，角度值?和频率Ω均为常值，为书写简便省略角标，Q(t)应理解为Qb(t)。对上式两边同时微分，
得
0???0???Ωsin(?/2)sinΩt???sinΩt????Ωsin??? （2.6-3） Q(t)???Ωsin(?/2)cosΩt?2?cosΩt?????00????根据四元数微分方程（2.4-35），可得角速度的四元数表示如下
?(t)Q*(t)ωq(t)?2Q*(t)Q(t)?2MQsinΩt?0?0???sinΩt?2Ωsin?02?cosΩt?cosΩt?0所以有角速度
?cosΩt00sinΩt0??cos(?/2)0?????sin(?/2)cosΩt???Ωsin?sinΩt?（2.6-4） ?cosΩt????????sinΩt???sin(?/2)sinΩt??Ωsin?cosΩt??????20??0???2Ωsin(?/2)???Ωsin?sinΩt???sinΩt???Ωsin??cosΩt? （2.6-5） ω(t)??Ωsin?cosΩt????2????2Ωsin(?/2)????tan(?/2)??这恰好与式（2.6-1）的角运动表现形式一致，可取a??sin?、b?sin?和c??2sin2(?/2)。
??(t)?，并对比式（2.6-2）
根据四元数与旋转矢量之间的关系Q(t)?cos?，可得 sin2?2?cosΩt?? （2.6-6） ?(t)???sinΩt????0??这说明在任意t时刻，动坐标系绕oxbyb平面上的转轴u(t)??cosΩtsinΩt0?相对于参考坐标系转动了?角度，转轴时刻在变化而转角恒定不变，动坐标系的zb轴在空间画出一个圆锥面（半锥角为?，z轴称为锥轴），参见图2.6-1，这正是该角运动称为圆锥运动的原因。
T
图2.6-1 圆锥运动
式（2.6-2）、（2.6-5）和（2.6-6）分别是圆锥运动的四元数、角速度和等效旋转矢量描述，它们形
32
式上都比较简单，这是除定轴转动之外的比较简单的角运动解析描述。与之相反，比如线性角速度运动式（2.5-23），其角速度表示虽然简单，但是很难得到相应的简单的等效旋转矢量或四元数描述。
不难验证圆锥运动的角速度ω(t)和等效旋转矢量?(t)满足Bortz方程，过程如下。 由式（2.6-5）和（2.6-6），可得
?cosΩt????sinΩt????sinΩttan(?/2)????Ωsin??cosΩt????Ωsin??cosΩttan(?/2)? （2.6-7） ??ω???sinΩt??????????????1?0?????tan(?/2)??????cosΩt????sinΩttan(?/2)???sinΩt?????Ωsin??cosΩttan(?/2)????2Ωsin???cosΩt????2ω （2.6-8） ??(??ω)???sinΩt??????????????1?0???????tan(?/2)??再将式（2.6-7）和（2.6-8）代入Bortz方程式（2.5-9）的右端，得
11????1????ω???ω?2?1?cot?(??2ω)???ω?cotω2??22?222??sinΩttan(?/2)???sinΩt?1???cot??Ωsin??cosΩt? （2.6-9） ??Ωsin??cosΩttan(?/2)??2??22???1????tan(?/2)????sinΩt??tan(?/2)?cot(?/2)?sin????sinΩt?1?????Ω?cosΩt??tan(?/2)?cot(?/2)?sin????Ω?cosΩt??2???0?0????上式正好等于式（2.6-6）直接微分的结果，验证完毕。
2.6.2 圆锥误差补偿算法
若记圆锥运动的四元数更新方程为
Q(tm)?Q(tm?1)Q(T) （2.6-10）
其中，T?tm?tm?1表示更新周期，Q(T)为该周期内的变化四元数（四元数增量）。上式两边同时左乘
Q*(tm?1)，可得
Q(T)?Q*(tm?1)Q(tm)?MQ*(tm?1)Q(tm)??????cossincosΩtsinsinΩt0?m?1m?1???cos222????2? ????sin?cosΩt???cos0?sinsinΩtm?1m?1???sincosΩtm?222?????2???????sinsinΩtm?10cossincosΩtm?1???222??sinsinΩtm?2????????00sinsinΩtm?1?sincosΩtm?1cos??????222? 33
?2??2?2?cos?sincosΩtcosΩt?sinsinΩtsinΩtm?1mm?1m??222?????????sincoscosΩtm?1?sincoscosΩtm??2222??????????sincossinΩtm?1?sincossinΩtm2222????2?2?sinsinΩtm?1cosΩtm?sincosΩtm?1sinΩtm???22?2???ΩT??????22?1?2?sincos???cos2?sin2cosΩT??22????????1sin??(cosΩt?cosΩt)???sin?sinΩTsinΩ?t?T??mm?1?m??2??22??????????1sin??(sinΩtm?sinΩtm?1)??sin?sinΩTcosΩ?t?T???m??2??22?????????sin2sinΩT???2???sinsinΩT?2??2? （2.6-11）
假设与变化四元数Q(T)对应的在时间段[tm?1,tm]内变化的等效旋转矢量为?(T)，即
Q(T)?cos?(T)?(T)?(T) （2.6-12）
?sin2?(T)2其中，?(T)??(T)为模值。比较式（2.6-11）和（2.6-12）的四元数矢量部分，可得
?ΩTT????sin?sinsinΩt??m??22??????(T)?(T)?ΩTT???sin??sin?sincosΩ?tm??? （2.6-13） ?(T)222????????sin2sinΩT??2??对上式两边同时取模，得
2当半锥角?和ΩT均为小量时，近似有
sin?(T)?sin2?sin2ΩT??sin4sin2ΩT （2.6-14） 222这说明等效旋转矢量?(T)也是小量，进一步近似有
sin?(T)?sin2?sin2ΩT?ΩT （2.6-15） ?22?(T)??ΩT （2.6-16）
因此，根据式（2.6-13）可知，在时间段[tm?1,tm]内的等效旋转矢量（旋转矢量增量）可近似为
?ΩTT???ΩTT?????sin?sinsinΩt??2sin?sinsinΩt??m??m??22???22?????????(T)?ΩTT???ΩTT?? （2.6-17） ???(T)?sin?sincosΩt??2sin?sincosΩt??m????m???(T)?2222????????sin2???????sin2sinΩT?2sin2sinΩT????22????再根据角速度式（2.6-5）积分，可得在等效旋转矢量计算时间段[tm?1,tm]内的角增量
34
??Ωsin?sinΩt??sin??(cosΩtm?cosΩtm?1)??dt??sin??(sinΩt?sinΩt)?Δθm??ω(t)dt???Ωsin?cosΩtmm?1???tm?1tm?1?22????2sin(?/2)?ΩT??2Ωsin(?/2)????tmtm?ΩTT????2sin?sinsinΩt??m??22??????ΩTT?????2sin?sincosΩ?tm???22??????2??2sin?ΩT??2?? （2.6-18）
比较式（2.6-17）和（2.6-18），它们在x轴和y轴上完全相同，而z轴上存在差异，这一差异使得
使用角增量代替旋转矢量进行姿态更新时会产生误差，并且误差随时间会不断累积。考虑到半锥角?为小量，定义如下误差
??(T)??(T)?Δθm???????? （2.6-19） 00??????00????????2?2?2???2sinsinΩT?(?2sin?ΩT)??2sin(ΩT?sinΩT)??22??2?为了补偿该误差，通常采用多子样补偿算法，在时间段[tm?1,tm]内进行N次采样，记采样间隔为h?T/N，参照式（2.6-18），可计算得每个采样间隔的内角增量（子样，sub-sample），为
??h????2sin?sinsinΩt?ih??m?1??22?????tm?1?ih??h???, （2.6-20） Δθm(i)??ω(t)dt??2sin?sincosΩ?tm?1?ih??? i?1,2,Ntm?1?(i?1)h22??????2??2sin????2??其中简记??Ωh，显然有时间段[tm?1,tm]内的总角增量
Δθm??tmtm?1ω(t)dt??i?1Δθm(i) （2.6-21）
N将式（2.6-20）中不同子样的角增量之间进行叉乘，可得
??h????h?????2sin?sinsinΩt?ih??2sin?sinsinΩt?jh?m?1m?1?????22???22????????????h??h????Δθm(i)?Δθm(j)??2sin?sincosΩ?tm?1?ih?????2sin?sincosΩ?tm?1?jh???
22???22?????????2?2??2?sin?2?sin????22???????h?h?????2??4?sinsin?sin?cosΩt?ih??cosΩt?jh???m?1??m?1????2222??????????h?h??? ?? ?2????4?sinsin?sin??sinΩ?tm?1?ih???sinΩ?tm?1?jh????22?2?2????????2???????2sin?sin?sin(i?j)???2???? 35