T11 T21 T31 Cbn = T12 T22 T32 T13 T23 T33
−1 θ = sin (T32 ) T γ 主 = tan −1 (− 31 ) T33 −1 T ψ 主 = tan ( 12 ) T22
真值表判断
δV N 位置误差方程： δL = − δh 位置误差方程： &
RM + h
δVE
RN + h
VN ( R M + h) 2
VE V sec L tan L sec L − δh E RN + h ( R N + h) 2
& δλ =
sec L + δL
& δh = δVU
MATLAB仿真
1、轨迹生成仿真 、 2、惯导器件输出信息的仿真 、 3、捷联惯导解算仿真 、 4、基本函数 、
泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）
0 ≤τ ≤ h
b 常数拟合：ωnb (tk + τ ) = a
Φ ( h) = ∆θ
2 Φ ( h) = ∆θ1 + ∆θ2 + ∆θ1 × ∆θ2 3
Φ (h) = ∆θ1 + ∆θ2 + ∆θ3 + 33 57 ∆θ1 × ∆θ3 + ∆θ2 × (∆θ3 − ∆θ1 ) 80 80
捷联惯导中的姿态更新实质上是如何计算四元数。 捷联惯导中的姿态更新实质上是如何计算四元数。
捷联惯导系统
2.3.2 四元数微分方程
&n qb =
1 n b qb ⊗ ωnb 2
n n m) n b m) qb ( m ) = qn ((m −1) ⊗ qb ( m −1) ⊗ qb ((m−1)
毕卡求解法（角增量） 1）定时采样增量法：采样时间间隔相同； 2）定量采样增量法：角增量达到一固定值时才更新；
∆θ = ∫
t +∆t
t
ω dt ⇒ Φ = ∫
t +∆t
t
(ω + σ )dt
q = cos Φ 2 + Φ Φ sin Φ 2
表征旋转的另一种形式：
Φ =θu
1 1 b & Φ = ωnb (t ) + Φ × ω b (t ) + Φ × (Φ × ωb (t )) nb nb 2 12
捷联惯导系统
Φ (h) = ∆θ1 + ∆θ2
2 Φ (h) = ∆θ1 + ∆θ2 + ∆θ1 × ∆θ2 3
• 算法思路不同； 等效旋转矢量法思路：
n n m) n b m) qb ( m ) = qn ((m −1) ⊗ qb ( m −1) ⊗ qb ((m−1)
ς
n ωin
Φ
b ωib
捷联惯导系统
2.4 几种姿态算法的比较
四元数的大小——范数
四元数表达方式 三角式
Q = cos
θ
2
+ u sin
θ
2
基本运算
捷联惯导系统
动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转 轴转动一个角度（θ，u） 四元数描述转动：
Q = cos
θ
2
+ u sin
θ
2
四元数是刚体转动的一种描述形式。 结论： • 四元数可以描述刚体的定点转动，Q包含了等 效旋转的全部信息； • 四元数与姿态矩阵的关系； • 描述刚体转动的四元数是规范化四元数；
−1
ω ω ω
b nbx b nby b nbz

sin γ cos θ = cos γ sin γ tan θ
b ωnbx b 0 ωnby b 1 − cos γ tan θ ωnbz 0 −
捷联惯导系统
2.3.4 从姿态矩阵中提取姿态角 θ∈﹙-90，90﹚度 γ∈﹙-180，180﹚度 Ψ∈﹙-180，180﹚度 或 Ψ∈﹙0，360﹚度
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 θ
0 [δ G ] = −δ Gz δ Gy
δ Gz
0 −δ Gx
−δ Gy δ Gx 0
0 δ K x [δ K ] = 0 δ K y 0 0
0 0 δ Kz
捷联惯导系统
捷联惯导系统误差方程
)b b n n %b % ωnb = ωib − Cn Cn′ωin′
L = arcsin P33
cos λ 0 − sin L sin λ cos L cos L sin λ sin L
λ主 = arctg
P32 P31
捷联惯导系统
4. 捷联惯导系统误差方差
捷联惯导系统误差源 • 惯性仪表的安装误差和刻度因子误差 • 陀螺漂移 ε b 和加速度计零位 ∇ b • 初始条件误差 • 计算误差
[ω (t ), a (t )]
t
[ ∆θ , ∆ v ]
% % [ ∆θ , ∆v ]
% % % [att , v , pos ]
[ att , v, pos ]
捷联惯导系统
捷联惯导系统原理框图
捷联惯导系统
• • • • 姿态更新算法 速度更新算法 位置更新算法 系统误差方程
捷联惯导系统
2. 姿态更新算法（核心） 姿态更新算法（核心）
基本思想： 基本思想：刚体的定点转动 2.1 欧拉角法（三参数法） 欧拉角法（三参数法）
ω （ω -ω ）
b nb b ib b in
欧拉角法：概念直观；只适应水平姿态角变化不大的情况，不能全姿态 解算。 方向余弦法：可全姿态工作；但计算量大，不实用。 四元数法：算法简单，计算量小；存在不可交换误差，适应于低动态运 载体。（等效旋转矢量的单子样） 等效旋转矢量法：可对不可交换性误差进行补偿，算法简单，适应于高 动态环境。
捷联惯导系统
b 直线拟合：ω nb (t k + τ ) = a + 2bτ
ωb 抛物线拟合： nb (tk + τ ) = a + 2bτ + 3cτ 2
b ωnb (tk + τ ) = a + 2bτ + 3cτ 2 + 4dτ 3 三次抛物线：
Φ(h) = ∆θ1 + ∆θ2 + ∆θ3 + ∆θ4 +
捷联惯导系统
2.4 等效旋转矢量法
四元数法求解中用到了角速度矢量的积分。 当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生误 差，称为转动不可交换性误差。 为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是采用 等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的 概念将四元数微分方程转化为等效旋转矢量微分方程(即Bortz方程)：
2 2 2 q0 + q12 − q2 − q3 CbR = 2(q1q2 + q0 q3 ) 2(q1q3 − q0 q2 )
2(q1q2 − q0 q3 ) 2 2 q0 − q12 + q22 − q3 2(q2 q3 + q0 q1 )
2( q1q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) 2 2 q0 − q12 − q2 + q32
4q1 q0 = T32 − T23 4q 2 q0 = T13 − T31 4 q q = T − T 21 12 3 0
sign(q1 ) = sign(q 0 )[sign(T32 − T23 )] sign(q 2 ) = sign(q 0 )[sign(T13 − T31 )] sign(q ) = sign(q )[ sign(T − T )] 3 0 21 12
∆Θ
2
Q (tk +1 ) = ( I +
)Q (tk )
捷联惯导系统
2.3.3 四元数初值的确定与归一化
q1 q2 q3 q0 = 1 2 1 = 2 1 = 2 1 = 2 1 + T11 − T22 − T33 1 − T11 + T22 − T33 1 − T11 − T22 + T33 1 + T11 + T22 + T33
Cbn
一个动坐标系相对参考坐标系的方位， 一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三 个不同的轴的三个角度来确定。把载坐标系作动坐标系， 个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为参 γ 即为一组欧拉角。 考系则 θ 、 和 ψ 即为一组欧拉角。
& ψ sin γ cos θ θ& = − sin θ γ& − cos γ cos θ cos γ 0 sin γ 0 1 0
-Q = − cos
θ
2
− u sin
θ
= cos(π − ) − u sin(π − ) = cos 2 2 2
θ
θ
2π − θ 2π − θ − u sin 2 2
表征旋转的四元数应该是规范四元数； Q = 1 计算误差，失去规范性，需归一化处理；