第3期(总第136期) 2006年6月 机械工程与自动化 MECHAN1CAL ENGINEERING ＆ AUT0MAT1ON No．3 Jun． 文章编号：1672—6413(2006)03-0102—03 惯性导航系统航向姿态计算算法研究 赵耀霞 (中北大学自动控制系， 山西 太原030051) 摘要：通过初始对准，惯性航向姿态系统获得了初始坐标，紧接着就是在运动体的运动中不断地计算运动体的 航向和姿态。就惯性航向姿态系统的航向和姿态计算算法进行探讨和研究，以寻求最佳计算算法。 关键词： 四元数； 惯性导航；捷联矩阵 中图分类号：TB115 文献标识码：A 0 引言 捷联系统在航空航天、航海、战术导弹以及民用 领域有广泛的应用。捷联系统的动态方程之一是姿态 微分方程。以此计算姿态矩阵是导航系统最重要的一 部分工作，由此确定载体坐标系与导航坐标系之间的 方向余弦矩阵，是导航计算和准确控制的基础。根据 捷联矩阵的元素可以单值地确定运动体的姿态角，所 以运动体的航向和姿态计算归结于解算捷联矩阵各元 素的值，即捷联矩阵的即时修正。 1 捷联矩阵即时修正算法 捷联矩阵即时修正就是实时地给出捷联矩阵，而 这要通过一定的算法来完成 进行捷联矩阵即时修正 的算法很多，如方向余弦算法、欧拉角变换算法、欧 拉四参数算法、四元数算法、凯利一克莱恩参量算法 及矢量表示算法等，其中最典型的有三种：欧拉角法、 转动四元数法、方向余弦矩阵法。它们通过解不同的 微分方程得到姿态矩阵。 1．1 欧拉角法(三参数法)Eli 从导航坐标系 依次转过方位角 、俯仰角0、滚 动角y就可得到运动体坐标系b。这样，运动体坐标系 相对于导航坐标系的角速度向量 可以表示为： 一
+ 0+y。
将∞写成沿运动体坐标系的投影形式： ] Fi 0 0]FeosO 0-sinO-1 Fo] J J一』0 cos)"sin)'I』0 1 0 J J 0 J+ l I L0-sin?'COS LsinO 0 cosO_J l I - ● ● y 对式(1)进行矩阵求逆便可得到： ● - - y =
= L[l c Oo：Ss O垂csoi n ysions 一=c 0s．nyscino s 瑁JI I lfl
⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯
(2)
式(2)便是欧拉角微分方程，解方程式(2)就可求得 、
0、y三个参数，便可得到捷联矩阵C：。
对式(2)进行初步分析可以看出，求解欧拉角微分 方程需要三个微分方程，与其它算法相比需要求解的 方程数要少些。但用计算机进行数值积分时要进行超 越函数的运算，这样反而加大了计算的工作量， 占用 的计算机CPU时间较多。此外，当 =90。时，式(2) 将会出现奇点。因此，欧拉角法的应用有一定的局限 性。 1．2 四元数法(四参数法) 运动体坐标系相对于平台坐标系的转动可用转动 四元数Q来表示，即： 收稿日期：2005 I2 20；修回日期{2006—01—15 作者简介：赵耀霞(1979一)，女，山西祁县人，助教，硕士研究生，主要从事自动化研究。 ● 。
～
． 巾一 O ∞ 。 维普资讯
http://www.cqvip.com
2006年第3期 赵耀霞：惯性导航系统航向姿态计算算法研究 ·1O3· Q=q0+q1ib+q2 +q3kb 。 其中，四元数q。、q。、q 、q。的基i 、如、五 的取得与 运动体坐标系的基i。、^、 相一致。从而四元数微分 方程为： =
Q 。 ⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯(3)
厶 其中，cu—O+ i6+ + 五6 。 将式(3)写成矩阵形式，得到： 0 一 一 一 0 一 m， 一m 0 m ：
_ COx 0
q0 q1 q g3 式(4)为四元数微分方程，对它求解便可实时计算出 对式(4)进行初步分析可以看出，求解四元数微分 方程要解四个微分方程，虽然要解的微分方程数要比 欧拉角法多一个，但在数值积分求解时只需进行加减 法和乘法运算，求解的计算量要比欧拉角法少得多。此 外，四元数微分方程不存在奇点，在整个数域上均可 计算。因此，四元数法具有较多的优越性。 1．3 方向余弦法(九参数法) 方向余弦矩阵的微分方程为： C=CfJ 。⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯(5) 其中，n为 一∞的反对称矩阵。式(5)可写成： 。 11 。 12
]： ：； ]×
。 C‘ 33
-f 。。f32 C33-f
一 O 求解式(6)需要解九个微分方程，但只需要进行加 减法和乘法运算，虽然求解方向余弦矩阵微分方程要 比四元数微分方程多些，然而采用方向余弦法可以直 接求出捷联矩阵C：，这是方向余弦法的一个优点。 2 实用捷联矩阵修正与姿态计算算法 基于运动载体的不同要求，用于飞机、导弹、船 舶和车辆的捷联惯性系统有几种不同的类型，即所谓 全捷联系统和简易捷联系统。它们不仅物理结构不同， 而且数学模型和使用的算法也不相同。针对不同的运 动载体和不同的使用环境，使用不同的捷联系统，实 施不同的算法，以实现合理的费效比结果。 2．1 解四元数微分方程法 用四阶龙格一库塔法解四元数微分方程组，计算 公式如下： qo(n+1)=‰+丢(五 +2k2+2h。+ ql(n+1)一q + l (m l
+ 2m2+ 2m。+ )
q2(n+D—q +丢(s +2s +2s。+s ) q3(n+1)一q。 +百1( 。
+ 2 2+ 2 。+ )
⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯(7) 其中，五、m、s、 为参数。 2．2 实用四元数三阶泰勒展开计算法 三阶泰勒级数展开式为： ，(z)一，(z。)+ (z。)(z—z。)+去X Ef'(x。)X (x-xo)。]+者[，。 (z。)(X-Xo)。]。 则： Qj+一Q +Q △t+专Q： +吉Q 。一 (1+ 1 +吉∞ + ∞)。⋯⋯⋯⋯⋯⋯⋯8) 当角增量△ =O+△ +△ ．『+△ 五时，有： △ 。一一△ 一△ 一△ =一j△ j。。⋯⋯(9) 把式(9)代入式(8)，得： Qj+==： l十吉∞一吉l∞l 一 l∞l ∞)。 ⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯⋯
(1O)
那么，实用四元数三阶泰勒计算公式 为： +1=Qj(1+2Ag~j1_l一百1 f△ + f +1ArD
iX
j+l- I△ I。· 川)。⋯⋯⋯⋯一(11) 2．3 简易航姿计算算法 由式(2)可得： 一 咖y] × c。sinyc。s c。syc。s ]
匿] 。
：
鱼堕1 2 。⋯⋯⋯⋯⋯⋯⋯．．(12)
-
c1． 。 ⋯由式(5)得到：
。 ⋯⋯⋯⋯⋯⋯⋯⋯⋯
(13)
。 ⋯⋯⋯⋯⋯⋯⋯⋯⋯
(14)
]
o
—
维普资讯 http://www.cqvip.com
·104· 机械工程与自动化 2006年第3期 式(12)、式(13)、式(14)构成简易航姿计算的基本方 程。初始条件为； C31(0)一一sin00 。 C3z(0)一sin)'ocos0o 。 其中，0 y。由加速度计的 、口 测量，在实际使用 中，由于系统干扰的存在，0和y要小于70。，否则计 算精度要降低。 2．4 航向偏移量计算公式 如果存在一横滚角y，当俯仰角0变化时将出现 可视航向失调角Aa，在横滚角y与俯仰角0依次减小 为0时，可视航向失调角将一直保持；如果横滚角y保 持不变，而俯仰角变化量△ 等于零时可视航向失调 角将消失。这种情况将导致航向偏移计算误差，采用 以下公式即可消除可视航向误差： △ 一 AaJ—A0sin)' cos os0 。 式中：△ 实际航向角变化量； △ 一一可视航向角变化量。 3 几种实用航姿算法的比较 上面提出的4种算法在物理结构、应用范围、计 算精度、计算时间、占用的计算机内存容量等方面各 不相同。为了比较方便，令四元数微分方程四阶龙格 一
库塔法为算法1，实用四元数三阶泰勒计算公式为
算法2，简易航姿计算公式为算法3，航向偏移量计算 公式为算法4，4种捷联航姿计算算法比较见表1。 通过表1的比较，可以得出以下结论： (1)算法2最好，但系统造价高。 (2)算法3适用于陆地车辆，精度比较高且系统 造价比算法2低得多。 (3)算法4最简单，适用范围也最窄。 针对不同捷联惯性导航系统的不同要求，可以选 择任何一种算法进行航姿计算。但在一般情况下，可 选择算法2或算法3作为捷联惯性导航系统的航姿计 算算法。 裹I 4种捷联航姿计算算法的比较 算法 计算时间 适用范围 所需物理结构 S 1 18 任何运动体 两个双自由度陀螺 和三个加速度计 2 10 任何运动体 两个双自由度陀螺 和三个加速度计 3 2 陆地车辆 一个双自由度陀螺 <70。，y<70。 和两个加速度计 只需测量航向变 量且精度要求不 4 1 高的场合 航向陀螺 0=0．y= 0 < 一15。．y< 45。 参考文献： E13 张永红．惯性技术在角度测量中的应用研究[J]．火力与 指挥控制，1995，20(3)：32—37． [23 郭圣权．捷联惯性导航中一种算法的推证[J]．中国惯性 技术学报，2002．10(2)：24 27． Algorithm Study on the Attitude Caught by Inertial Navigation System ZHAO Yao—xia (Dept．of Automatic Control，North University of China．Taiyuan 030051，China) Abstract：The inertial navigation system gets the initial of the object through initial alignment，then computes the heading and attitude of the object during moving．This paper studies the algorithm of heading and attitude． Key words：quaternion；inertial navigation；strap down matrix (上接第101页) Research on the Parabola Interpolation Based on Pulse Interval Interp0lati0n CHEN Gai-yin (W uhan Institute of Shipbuilding Technology．Wuhan 430050，China) Abstract：The paper discusses the principle and approach about the parabola interpolation by the principle of pulse interval interpolation．It gives an example．The discussion is the extension of the pulse interval interpolation，which can be accepted by the beginner． Key words：pulse interval；interpolation}parabola 维普资讯
http://www.cqvip.com
选择“Disable on www.wenxuecity.com”
选择“don't run on pages on this domain”
