基于 Matlab 的捷联惯导算法设计及仿真1doc

基于 Matlab 的捷联惯导算法设计及仿真1

严恭敏 西北工业大学航海学院，西安 (710072) E-mail：yangongmin@163.com

摘要：根据圆锥误差补偿算法和划船误差补偿算法的研究成果，考虑到实际捷联惯导算法 仿真程序编写的方便性，总结了一些与捷联惯导更新算法有关的函数的计算公式。对圆锥误

差补偿算法和捷联惯导算法进行了仿真，仿真结果和理论分析结论吻合。在附录中给出了

Matlab 的 m 文件源程序代码，具有一定的参考价值。 关键词：捷联惯导；四元数；等效旋转矢量；Matlab；算法；仿真 中图分类号：V249.3

1. 引言

在捷联惯导系统中采用数学平台，姿态更新解算是捷联惯导系统算法的核心部分，由于 四元数法算的优良特性，它在工程实际中经常被采用。为了减小姿态计算的不可交换性误差， 前人研究并建立了等效旋转矢量方程，高精度姿态更新解算的研究主要集中在等效旋转矢量 方程的求解上，在圆锥运动环境下，许多研究者提出并完善了圆锥误差补偿算法。基于圆锥 误差补偿算法和划船误差补偿算法的等效原理，可将圆锥误差补偿算法移植到划船误差补偿 算法中去，从而减少了划船误差推导的繁琐过程。上述研究都已经比较成熟[1-6]，本文根据 这些研究结果，并考虑到实际仿真程序编写的方便性，总结了一些与捷联惯导算法有关的函 数的计算公式或步骤，其中更详细的推导过程可见参考文献[7,8]。最后，对圆锥误差补偿算 法和捷联惯导算法进行了仿真。附录中给出了Matlab的m文件源程序代码具有一定的参考价 值。

2. 捷联惯导算法

文中选取东-北-天(E-N-U)地理坐标系为导航坐标系，记为 n 系；捷联惯组坐标系记 为 b 系。

2.1 相关函数

(1)四元数的共轭与乘积。四元数 q 可表示为 q = q0 + qv = q0 + q1i + q2 j + q3 k 。用 q

表示 q 的共轭四元数； q = q1q2 表示四元数 q 是四元数 q1 与 q2 的乘积，四元数相乘是不可

交换的。

(2)四元数与向量相乘。一般情况下利用矩阵进行坐标变换，如关系式 v n = C n v b ，

同样利用四元数也可以表示坐标变换。设变换四元数 q n 与变换矩阵 C n 相对应，则定义变换

bb

四元数 q n 和向量 v b 的乘法，记为 v n = q n v b ，它由以下规则实现：首先，将向量扩展成四

bb

元数，令 qb = 0 + v b ；其次，做四元数乘法，令 q n = q n qb q n ? ；最后，从四元数 q n 中提取

bb

向量，有 v n = q n 。

(3)等效旋转矢量与四元数之间的转换。等效旋转矢量 v 转化为四元数 q 的公式为

1 本课题得到水下信息处理与控制国家级重点实验室基金(9140C230206070C2306)资助。

q = Fv→q

(v) = cos( v / 2) + v sin( v / 2)

v

(1)

其中 Fv→q (?) 表示从等效旋转矢量转换到四元数的函数，容易看出有 q

= Fv→q (?v) 成立。

假设 v 是小量，则从 q 中可以求出 v ，首先令 vn 2 = v / 2 = arccos(q0 ) ，再求得

v = Fq→v

(q) =

2 ? vn 2 q

sin(vn 2 )

(2)

其中 Fq→v (?) 表示从四元数转换到等效旋转矢量的函数。

(4)姿态向量 A 与姿态四元数 q n 之间的转换。记由俯仰角θ 、横滚角γ 和航向角ψ 组

成的向量 A = [θ

γψ ]T 为姿态向量，通过姿态矩阵 C n 作为过渡，容易实现 A 与 q n 之间

bb

的转换，此处不作详细分析。

2.2 地球模型

通常给出的地球椭球模型参数为长半轴 Re 和扁率 f ，由椭球几何学容易得到其它参数，

在惯性导航算法中经常用到，它们是偏心率 e =

2 f ? f 2 、短半轴 R

= (1 ? f )Re 、第二

偏心率 ep =

2 + R 2 / R 、子午圈半径 R

= Re (1 ? e

) /(1 ? e2

sin 2

L)3 / 2

、卯酉圈半径

RN = Re

/(1 ? e2 sin 2 L)1/ 2 。另外，重力加速度 g 和地球自转角速率ω 也是惯性导航解算

的必备参数，在 GRS80 椭球模型中，正常重力公式为

g = g

0 (1 + 5.27094 × 10

2

sin 2 L

+ 2.32718 × 10?5

sin

4 L) ? 3.086 × 10

-5

?6