当 90
时，方程退化，故不能全姿态工作。
实用文档
捷联惯导系统
2.2 方向余弦法（九参数法） C bn Cbnωnbbk
矢量的方向余弦表示姿态矩阵的方法； 可全姿态工作，但需要解含有九个未知量的线性方程组，计算量大， 工程上不实用。
实用文档
捷联惯导系统
2.3 四元数法（四参数法）
2.3.1 四元数基本概念 四元数是由一个实数单位1和一个虚数单位i、j、k组成的含有四个
直线拟合：ω b nb (tk)a2b
Φ (h)θ1θ22 3θ1θ2
抛物线拟合： ω n b b(tk)a 2 b 3 c2
3 3
5 7
Φ ( h ) θ 1 θ 2 θ 3 8 0 θ 1 θ 3 8 0 θ 2 (θ 3 θ 1 )
三次抛物线ω ：n b b (tk)Байду номын сангаасa 2 b 3 c2 4 d3
qbn
1 2
qbn
ωnbb
q q q q n
n (m )
b (m ) n (m 1 )
n b (m 1 )
b (m ) b (m 1 )
毕卡求解法（角增量） 1）定时采样增量法：采样时间间隔相同； 2）定量采样增量法：角增量达到一固定值时才更新；
Q(tk1)(I2Θ)Q(tk)
实用文档
捷联惯导系统
q
0
1 2
1 T 11 T 22 T 33
- Q c o s u s i n c o s ( ) u s i n ( ) c o s 2 u s i n 2
22 2 2 2 2
表征旋转的四元数应该是规范四元数； Q 1 计算误差，失去规范性，需归一化处理；
qi
qˆi qˆ02 qˆ12 qˆ22 qˆ32
实用文档
捷联惯导系统
2.3.4 从姿态矩阵中提取姿态角 θ∈﹙-90，90﹚度 γ∈﹙-180，180﹚度 Ψ∈﹙-180，180﹚度 或 Ψ∈﹙0，360﹚度
c o sc o s s in s in s in s in c o s s in c o s c o ss in s in C b n c o ss in s in s ic n o s c o s s in c o s s in c o s s in s in c o s c c o o s s c o s s in
Φu
qcos Φ Φsin Φ 2Φ 2
表Φ 征 旋转n b b ( 的t)另 1 2 一Φ 种 形ω 式b n b ( ：t) 1 1 2 Φ (Φ ω b n b (t))
实用文档
捷联惯导系统
泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）
0h
常数拟合：ωnbb(tk )a
Φ(h) θ
2.3.3 四元数初值的确定与归一化
q1
q 2
q
3
1
2 1
2 1
2
1 T 11 T 22 T 33 1 T 11 T 22 T 33 1 T 11 T 22 T 33
4 4
q1 q 0 q2q0
T32 T13
T23 T31
4q3 q0 T21 T12
sig(qn1)sig(qn0)[sig(Tn32T23)] sig(qn2)sig(qn0)[sig(Tn13T31)] sig(qn3)sig(qn0)[sig(Tn21T12)]
捷联惯导系统
框图
捷联惯导系统原理
实用文档
捷联惯导系统
• 姿态更新算法 • 速度更新算法 • 位置更新算法 • 系统误差方程
实用文档
捷联惯导系统
2. 姿态更新算法（核心）
基本思想：刚体的定点转动 nb（ b ibb-ibn）
C
n b
2.1 欧拉角法（三参数法）
一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕
元的数。（超复数） Q q 0 ,q 1 ,q 2 ,q 3 q 0 q 1 i q 2 j q 3 k
四元数的大小——范数 Qq0 2q1 2q2 2q3 2
四元数表达方式
三角式
Qcosusin
2
2
基本运算
实用文档
捷联惯导系统
动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转 轴转动一个角度（θ，u）
2(q1q2q0q3) q0 2q1 2q2 2q3 2
2 2((q q1 2q q3 3 q q0 0q q2 1))
2(q1q3q0q2) 2(q2q3q0q1) q0 2q1 2q2 2q3 2
捷联惯导中的姿态更新实质上是如何计算四元数。
实用文档
捷联惯导系统
2.3.2 四元数微分方程
差，称为转动不可交换性误差。
为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是 采用
等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的
概念 将θ 四 元t 数 t微d 分t方 程转Φ 化 为等t 效 t( 旋转 矢) 量d t微分方程(即Bortz方程)：
t
t
Φ (h)θ1θ2θ3θ47 93 46 5(θ1θ2θ3θ4) 3 93 44 5(θ1θ3θ2θ4)5 92 46 5θ1θ49 64 55 4θ2θ3
三个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为 参考系则 、 和 即为一组欧拉角。
sincos sin coscos
cos
0
sin
101 nnbbbbyx
sin cos cos
0 nbbz sin tan
0 0
cos cos sin
nnbbbbxy
1 cos tannbbz
四元数描述转动： Qcosusin
2
2
四元数是刚体转动的一种描述形式。
结论：
• 四元数可以描述刚体的定点转动，Q包含了等
效旋转的全部信息；
• 四元数与姿态矩阵的关系；
• 描述刚体转动的四元数是规范化四元数；
C b R q0 2 2( q1 q q 1 2 2 q q 2 2 0q 3q )3 2
T11 T21 T31
C
n b
T12
T22
T3
2
T13 T23 T33
真值表判断
sin 1(T 32 )
主
ta n 1(
T 31 ) T 33
主
ta n 1( T12 ) T 22
实用文档
捷联惯导系统
2.4 等效旋转矢量法
四元数法求解中用到了角速度矢量的积分。
当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生 误