捷联惯导姿态算法若干问题的研究

α cos 2 0 α Q (t ) = sin cos ω t 2 sin α sin ω t 2
Q (t + h) = Q (t )È q( h)
图1
锥运动
设由 ｔ 至 ｔ＋ｈ，ｂ 系的更新四元数为 q( h) ，则在 t 和 t+h 时刻 Ｒ 系至 ｂ 系的旋转四元数有如下关系：
第 10 卷第 2 期
中国惯性技术学报
2002 年 4 月
・系统研究与分析・
文章编号： 1005－6734(2002)02－0001－06
捷联惯导姿态算法若干问题的研究
张士邈，刘 放，秦永元
（西北工业大学 ９０５ 教研室，西安 ７１００７２） 摘 要： 研究了捷联系统姿态更新的等效旋动矢量算法，并以锥运动为条件，对等效旋动矢量算 法进行了优化。此外还对四元数、旋动矢量及其优化算法进行了仿真比较，仿真结果表明：等 效转动矢量算法的精度明显优于四元数算法，且等效转动矢量的采样频率越高，姿态更新算法 的精度就越高。 关 键 词： 捷联惯导系统；锥运动；四元数；等效旋动矢量 中图分类号： U666.12 文献标识码： A
_____________________ 收稿日期： 2002－01－02 作者简介： 张士邈（1974—） ，女，西北工业大学在读硕士，从事惯性导航系统技术研究。
2
中国惯性技术学报
2002 年 4 月
２ 锥运动
v 设 XYZ 为参考坐标系，记为 Ｒ；xyz 为机体坐标系，记为 ｂ； OL 为 YOZ 平面内的一条射线，xyz v v （ｂ 系）是由 XYZ（Ｒ 系）绕 OL 旋转 α 后形成的（见图 １） 。当 OL 以角速度 ω 绕原点 O 在 YOZ 平面内 v 旋转时，则 Ox 轴的轨迹形成一个锥面，锥顶位于 O 点，对称轴为 OX 轴。应注意，OL 并不与 ｂ 系固联，
~ Φe ≈ 2∂q
新算法的精度准则。 ４ 姿态更新算法 ４．１ 三子样旋转矢量的求解 旋转矢量的微分方程（２）为：
（４）
~ 为 ∂q 的矢量部分，则Φ 表征了计算得到的变换四元数 q 其中 ∂q ˆ 的计算误差，我们把它作为检验姿态更 e
（６）
根据泰勒级数展开得
2 & (t k −1 ) h + h Ö &&(t k −1 ) + L Ö(t k −1 + h) = Ö(t k −1 ) + Ö 2!
（７）
式中， Ö(t k −1 + 0) = 0 。 记
è = ù(t k −1 + ô )dô ∫
（９）
代入（７）式得 Ö(t k −1 + h) = ah + b h 2 + c h 3 + 1 a × b h 3 + 1 a × c h 4 + 1 b × c h5 6 4 10 式中系数 a、 b、 c 可根据（８）式确定：即将时间段 [t k −1 , t k ] 作三等分，记 （１０）
而仅与 Ox 轴具有固定的相对角关系。
v v v 根据欧拉定理，ｂ 系可视为由 Ｒ 系的一次旋转形成。设该旋转矢量为Ö ， Ö 沿 OL 方向，大小为 v v v v v Ö = α 。设 OL 的单位向量为 u L ，并取 OL 与 OY 轴重合时的点为起点（ｔ＝０），则 u L 在 Ｒ 系内的分量为 0 0 vR v vR vR α cos ω t Ö Ö ( t ) = ⋅ u ( t ) = uL t (t) = cos ω L ，因此： 。 t sin ω α sin ω t v 由 Ö 所形成的四元数，即表征 Ｒ 系到 ｂ 系的四元数为：
èi =
i h 3
i −1 h 3
∫ ω(t
k −1
+ τ )dτ
（１１）
当 ｉ 分别取 １、２、３ 时，可得关于 a、b、c 的三个独立方程，因此可唯一确定出这些系数。将所得系数 代入（１０）式，得：
Ö(t k −1 + h) = è + X (è 1 × è 3 ) + Yè 2 × (è 1 − è 3 )
（１２）
4
中国惯性技术学报
2002 年 4 月
其中， è = è 1 + è 2 + è 3 ，X＝０．４１２ ５，Y ＝０．７１２ ５。 上述系数是根据角速度为二次抛物线的假设得出的，而实际角速度并非真正如此，所以所得系数并 不能保证算法漂移最小。 ４．２ Ｍｉｌｌｅｒ 三子样优化算法 Ｍｉｌｌｅｒ 优化算法的实质是把锥运动作为检验算法优劣的基础， 以使锥误差达到最小为准则， 重新确 定（１２）式三子样旋转矢量的系数 Ｘ 和 Ｙ。我们在第 ２ 节中已推导出锥运动的角速度ù(t ) ，代入（１１） 式得到 è 、 è 1 、 è 2 、 è 3 ，然后再代入（１２）式，则计算得到的旋转矢量为
2 2
（１）
ωh h cos ω ( t + ) sin α sin 2 2
& (t ) ， & (t ) = 1 Q (t )È Ω bq (t ) ，有 Ω bq (t ) = 2Q −1 (t )È Q 由四元数微分方程 Q 2
即 ω
α ωh ωh ωh − 2(ωh) sin2 + 8sin2 α sin2 ( ) sin( )[ X cos + Y ] 2 6 3 3 Φ x = − 2sinα sin(ωh ) − 8 (ωh) sin2 α sinα sin(ωh ) sin(ωh ) sinω(t + h ) 。 （１３） Ö= Φ y 2 3 2 6 3 2 ωh 8 ωh ωh h Φz 2α 2sinα sin( ) + (ωh) sin sinα sin( ) sin( ) cosω(t + ) 2 3 2 6 3 2
ˆ* ⋅ R n' ⋅ q ˆ ，代入（３）式得 现考虑 q 有误差，设计算得到的变换四元数为 q ˆ ，则有 R b = q
ˆ* ⋅ R n' ⋅ q ˆq* ， R n = qq
第2期
张士邈等：捷联惯导姿态算法若干问题的研究
3
令 ∂q = qq ˆ* ，则 R n = ∂q ⋅ R n' ⋅ ∂q * ，即 n 和 n' 之间的误差变换四元数为 ∂q ，即 n' 相对 n 有一个误差等效转 动，我们用Ö表示，Φ = Ö 为其转角，则 ∂q = cos Φ + Ö sin Φ 。考虑到Φ 为小量，取 cos Φ ≈ 1 ， sin Φ ≈ Φ ， 2 2 Φ 2 2 2 则上式可写为 ∂q ≈ 1 + Ö 。令 2
& = ù + 1 Ö× ù + 1 Ö× (Ö× ù) Ö 2 12
（５）
设 Ö(tk −1 + h) 为 [t k −1 , t k ] 时间段内的等效旋转矢量，机体角速度用二次曲线拟合，即
ù(t k −1 + τ ) = a + 2bτ + 3c τ 2 sin x = 2 ω y − ω sin α sin ω t ω z ω sin α cos ω t 0
（２）
其中矢量 ù(t ) 即为假定的锥运动情况下 ｂ 系相对 R 系的角速度在 b 系内的分量。 ３ 精度准则 设 R b 和 R n 分别表示同一个固定矢量在 ｂ 坐标系和 ｎ 坐标系的轴向分量构成的四元数。q 是坐标系 ｂ 和坐标系 ｎ 之间的变换四元数 [1] ，则 R n = q ⋅ R b ⋅ q * 式中 q * 为 q 的共轭四元数。 （３）
Study on Some Questions of Attitude Algorithm for Strapdown Inertial Navigation System
ZHANG Shi-miao，LIU Fang，QIN Yong-yuan (Northwestern Polytechnical University, xi’an 710072, China) Abstract： The attitude updating algorithm is an determinant factor in strapdown inertial navigation system(SINS). This paper studies the rotation vector algorithm, and their improved algorithms are discussed under the coning base motion. The simulation results show that when the sample frequency is higher, the accuracy of the rotation vector algorithm is better. Key words ： SINS; coning motion; quaternion; rotation vector １ 引 言 在捷联惯导中，姿态更新算法是其算法的核心，也是影响捷联惯导系统精度的主要因素之一，因此 设计和采用合理的姿态更新算法就成为需要研究的课题。在工程实践中常用四元数法，但 由于刚体有限 转动的不可交换性，在四元数算法中不可避免地引入了不可交换误差，特别是作高动态飞行时，误差很 大，必须采取有效措施加以克服。１９７１ 年，Ｂｏｒｔｚ 提出的等效转动矢量概念，有效地解决了不可交换误 差的问题。以后的研究就都集中在等效旋转矢量的求解问题上，其中 Ｍｉｌｌｅｒ 的三子样优化算法最具代 表性。他提出了机体在作锥运动时，即把锥运动作为检验算法的基准，对三子样算法进行了优化，并给 出其算法漂移。 本文主要是对 Ｍｉｌｌｅｒ 优化算法的一些问题进行了研究：首先推导了锥运动的更新四元数和角速率； 然后针对 Ｍｉｌｌｅｒ 优化算法精度准则难以理解的问题，重新进行了阐述，并根据 Ｍｉｌｌｅｒ 的三子样优化算 法推导出二子样优化算法、四子样优化算法及其算法漂移，还对各种算法进行了仿真比较；最后给出一 种推导 Ｍｉｌｌｅｒ 优化算法的简单方法。本文的理论研究对提高捷联惯导算法精度具有很高的参考价值。