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