基于 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