捷联惯性导航总结

z
0 x
y
姿态矩阵计算方法
3 四元数微分方程及其解
1 qk 1 exp( )qk Mq k qk rk 2
rk ac as x i as y j as z k
(0.5 )2 (0.5 )4 ac cos( ) 1 2 2! 4!
tk b nb
t
v f dt
b tk
t
速度微分方程求解
2 速度积分算法
v f dt [2ω ωen ] v dt g1 dt
n n 0 0 n ie n n n 0
t
t
t
v
n k 1
v u gt
n k n
b
v
n l 1
[ I 2Ωie tl Ωen tl ]v
v f dt
b
t
1 tk 1 αk 1 α ωdt 2 tk
α ωdt
tk
tk
t
捷联导航算法
捷联惯导误差分析
主要误差源

1 初始对准误差
2 惯性器件误差
3 计算误差
误差传播方程：误差源与系统性能的相互关系的 方程，主要用来阐述各种类型误差随时间的传播 过程。
捷联惯导误差分析
sin( / 2) (0.5 ) (0.5 ) as 0.5(1 ) 3! 5!
2 4
姿态矩阵计算方法
4 圆锥误差
σ αk 1 αk 1
α ωdt
tk t
1 tk 1 αk 1 α ωdt 2 tk
姿态矩阵计算方法
5 姿态角计算
b nbz

姿态矩阵计算方法
3 四元数微分方程及其解
1 tk 1 qk 1 [exp( W dt )]qk 2 tk
1 qk 1 exp( )qk 2
0 t k 1 x W dt tk y z x 0 z y z y x 0
优点：不存在正交误差
缺点：实时计算难；有奇点， 不能用于全姿态飞行器
(sin y cos z ) sec
cos sin y z
姿态矩阵计算方法
2 方向余弦矩阵微分方程及其解
dC b C bn Ωnb dt
n b
Ω Ω Ω
b nb b ib
b in
0 b b Ωnb nbz b nby
b nbz
0

n b
b b
nby
b
nbx
0
nbx
由相似变换定理： 最终有：
Ω C Ω C
bБайду номын сангаасin b n n in
dC bn n b n b C b Ωib ΩinC n dt
广义误差方程:
n ψ ωin ψ ωin C b ωib n [ f n ]ψ C b v f b
v p
ψ
T
捷联惯导误差分析
广义误差方程的实例——当地地理坐标系导航 误差传播方程
X FX Gu
X [ vN vE vD L l h]T
速度微分方程求解
1 比力矢量变换
f [2ω ωen ] v g1 v
n n n ie n n
n
n
1 1 tk 1 u C k [v k 1 αk 1 v k 1 (α f b ωb v )dt ] 2 2 tk
旋转修正 划摇修正
α ω dt

------偏航角
俯 仰 角
c13 c 23 c33
滚 转 角
c11 c12 C bn c 21 c 22 c31 c32
arcsin(c31 )
arctan( c32 / c33 )
arctan( c21 / c11 )
捷联惯导机械编排
姿态矩阵计算方法
1 欧拉角微分方程式
cos cos b T C bn (C n ) cos sin sin sin cos cos sin sin sin cos cos sin sin cos cos sin sin sin sin sin cos cos sin cos sin cos cos
b nbx 0 0 b nby 0 C C C 0 z 0 nbz 0
C C C C
b n
(sin cos )tg x y z
t t
t
Ω (t )dt
b nb
0 z y
2
z 0
x
2
y x
0
( x ) ( y ) ( z )
2 2
姿态矩阵计算方法
2 方向余弦矩阵微分方程及其解
C (t Δt ) C (t )[ Ι
姿态矩阵计算方法
2 方向余弦矩阵微分方程及其解
其精确解为：
n n b C b C b Ωnb t n b C b (t ) C (t0 ) exp( Ωnb (t )dt )
t0
t t
或： C
n b
(t t) C (t) exp(
t
Ω (t)dt)
b nb
令：

cos cos C bn cos sin sin sin cos cos sin sin sin cos cos sin sin cos cos sin sin sin sin sin cos cos sin cos sin cos cos
捷联惯导计算总结
2011.10.
捷联式惯导计算流程
确定机械编排（选定导航坐标系，列写导航方程）
系统初始化——初始对准，获得初始姿态、初始位置、
初始速度） 姿态微分方程求解（方向余弦、欧拉角、四元数、等效 旋转矢量），姿态矩阵求取，姿态提取 加速度矢量变换，速度微分方程器求解，速度获取 位置微分方程求解，位置获取
n n
n l
Ck
t k 1
tk
1 1 tk 1 A(t ) f dt C k [v k 1 αk 1 v k 1 (α f b ωb v )dt ] 2 2 tk
C k 1 C k Ak
Ak Ι
σ αk 1 αk 1
sin 1 cos ( ) ( ) 2
n b n b
sin

( )
1 cos

( ) 2 ]
b ω 使用条件：要求在 Δt 时间内，对应的角速度矢量 nb
的方向不变。 优点：避免欧拉方程退化现象，可全姿态工作；
缺点：需解9个一阶微分方程，计算量大。
姿态矩阵计算方法
3 四元数微分方程及其解
dq 1 Wq dt 2
u [x y z fx f y fz ]T
捷联惯导误差分析
捷联惯导误差分析
大作业
1）阅读现有捷联惯导MATLAB程序 2）以某导弹为背景，以发射点固定北-东-地坐标系 为导航坐标系，不考虑地球旋转，设计捷联惯导方 案，编写捷联惯导MATLAB仿真程序，通过仿真给出 陀螺、加速度传感器误差对姿态、速度、位置的影 响。
0 b P1 1 nbx b 2 nby P 2 b P3 nbz 0
b nbx
b nbz b nby
0
b nby b nbz

b nbx
b nby P1 b P2 nbx 0 P3