t t
t t
θ t dt Φ t ( )dt
表征旋转的另一种形式： Φ u
q cos Φ Φ sin Φ 2Φ 2
Φ&
b nb
(t
)
1 2
Φ
ωbnb
(t
)
1 12
Φ
(Φ
ωbnb
(t
))
捷联惯导系统
泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）
0 h
常数拟合：ωnbb (tk ) a
考系则 、 和 即为一组欧拉角。
& sin cos
&
sin
& cos cos
cos
0
sin
0 1
1
nnbbbbyx
sin cos cos
0 0
cos cos sin
nnbbbbxy
0 nbbz
sin tan
1
cos
tan
nbbz
当 90o时，方程退化，故不能全姿态工作。
q q q q n b(m)
n(m) n(m1)
n b(m1)
b(m) b(m1)
毕卡求解法（角增量） 1）定时采样增量法：采样时间间隔相同； 2）定量采样增量法：角增量达到一固定值时才更新；
Θ
Q(tk1) (I 2 )Q(tk )
捷联惯导系统 2.3.3 四元数初值的确定与归一化
q1
q2
T13 T23 T33
真值表判断
sin1(T32 )
主
tan 1 (
T31 T33
)
主
tan 1 ( T12 T22
)
捷联惯导系统
2.4 等效旋转矢量法
四元数法求解中用到了角速度矢量的积分。 当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生误 差，称为转动不可交换性误差。 为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是采用 等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的 概念将四元数微分方程转化为等效旋转矢量微分方程(即Bortz方程)：
元的数。（超复数） Qq0, q1, q2, q3 q0 q1i q2 j q3k
四元数的大小——范数 Q q02 q12 q22 q32
四元数表达方式
三角式
Q cos usin
2
2
基本运算
捷联惯导系统
动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转 轴转动一个角度（θ，u）
2(q1q2 q0q3 ) q02 q12 q22 q32
2(q2q3 q0q1)
2(q1q3 q0q2 ) 2(q2q3 q0q1)
q02 q12 q22 q32
捷联惯导中的姿态更新实质上是如何计算四元数。
捷联惯导系统
2.3.2 四元数微分方程
q&bn
1 2
qbn
ωnbb
四元数描述转动： Q cos usin
2
2
四元数是刚体转动的一种描述形式。
结论：
v 四元数可以描述刚体的定点转动，Q包含了等
效旋转的全部信息；
v 四元数与姿态矩阵的关系；
v 描述刚体转动的四元数是规范化四元数；
CbR
q022 (q1qq122
q22 q0
q32 q3 )
2(q1q3 q0q2 )
-Q cos usin cos( ) usin( ) cos 2 usin 2
2
2
2
2
2
2
表征旋转的四元数应该是规范四元数； Q 1 计算误差，失去规范性，需归一化处理；
qi
qˆi
qˆ02
qˆ12
qˆ
2 2
qˆ32
捷联惯导系统
2.3.4 从姿态矩阵中提取姿态角 θ∈﹙-90，90﹚度 γ∈﹙-180，180﹚度 Ψ∈﹙-180，180﹚度 或 Ψ∈﹙0，360﹚度
(θ1
θ2
θ3
θ4
)
334 945
(θ1
θ3
θ2
θ4
)
526 945
θ1
θ4
654 945
θ2
效旋转矢量法的区别：
v 原理相同：计算姿态四元数完成姿态更新；
v 四元数算法
等效旋转矢量的单子样算法；
Φ(h) θ1 θ2
Φ(h)
θ1
θ2
2 3
θ1
θ2
v 算法思路不同； 等效旋转矢量法思路：
捷联惯导系统
• 姿态更新算法 • 速度更新算法 • 位置更新算法 • 系统误差方程
捷联惯导系统
2. 姿态更新算法（核心）
基本思想：刚体的定点转动 nb（b ibb -ibn）
Cbn
2.1 欧拉角法（三参数法）
一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三
个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为参
q3
1 2
1 2
1 2
q
0
1 2
1 T11 T22 T33 1 T11 T22 T33 1 T11 T22 T33 1 T11 T22 T33
44qq12qq00
T32 T13
T23 T31
4q3q0 T21 T12
sign(q1 ) sign(q0 )[sign(T32 T23 )] sign(q2 ) sign(q0 )[sign(T13 T31 )] sign(q3 ) sign(q0 )[sign(T21 T12 )]
q q q q n b(m)
n(m) n(m1)
n b(m1)
b(m) b(m1)
n in
b ib
捷联惯导系统
2.4 几种姿态算法的比较
欧拉角法：概念直观；只适应水平姿态角变化不大的情况，不能全姿态 解算。
捷联惯导系统
2.2 方向余弦法（九参数法）
Cbn
C
n b
ωnbbk
矢量的方向余弦表示姿态矩阵的方法； 可全姿态工作，但需要解含有九个未知量的线性方程组，计算量大， 工程上不实用。
捷联惯导系统
2.3 四元数法（四参数法）
2.3.1 四元数基本概念 四元数是由一个实数单位1和一个虚数单位i、j、k组成的含有四个
Φ(h) θ
直线拟合：ω
b nb
(t
k
)
a
2b
Φ(h)
θ1
θ2
2 3
θ1
θ2
抛物线拟合：ωnbb (tk ) a 2b 3c 2
Φ(h)
θ1
θ2
θ3
33 80
θ1
θ3
57 80
θ2
(θ3
θ1)
三次抛物线：ωnbb (tk ) a 2b 3c 2 4d 3
Φ(h)
θ1
θ2
θ3
θ4
736 945
cos cos sin sin sin
Cbn cos sin sin cos sin
sin cos
sin cos cos cos
sin
sin cos cos sin sin
sin
sin
cos
cos
sin
cos cos
T11 T21 T31
Cbn T12
T22
T32