捷联惯性导航总结

v f dt
b
t
1 tk 1 αk 1 α ωdt 2 tk
α ωdt
tk
tk
t
捷联导航算法
捷联惯导误差分析
主要误差源

1 初始对准误差
2 惯性器件误差
3 计算误差
误差传播方程：误差源与系统性能的相互关系的 方程，主要用来阐述各种类型误差随时间的传播 过程。
捷联惯导误差分析
捷联惯导机械编排
姿态矩阵计算方法
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 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

------偏航角
俯 仰 角
c13 c 23 c33
滚 转 角
c11 c12 C bn c 21 c 22 c31 c32
arcsin(c31 )
arctan( c32 / c33 )
arctan( c21 / c11 )
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
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
速度微分方程求解
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
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 姿态角计算
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 )[ Ι
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
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
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

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!
优点：不存在正交误差
缺点：实时计算难；有奇点， 不能用于全姿态飞行器
(sin y cos z ) sec
cos sin y z
姿态矩阵计算方法
2 方向余弦矩阵微分方程及其解
dC b C bn Ωnb dt
n b
Ω Ω Ω
b nb b ib
广义误差方程:
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
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
u [x y z fx f y fz ]T
捷联惯导误差分析
捷联惯导误差分析
大作业
1）阅读现有捷联惯导MATLAB程序 2）以某导弹为背景，以发射点固定北-东-地坐标系 为导航坐标系，不考虑地球旋转，设计捷联惯导方 案，编写捷联惯导MATLAB仿真程序，通过仿真给出 陀螺、加速度传感器误差对姿态、速度、位置的影 响。
n b n b
sin

( )
1 cos

( ) 2 ]
b ω 使用条件：要求在 Δt 时间内，对应的角速度矢量 nb
的方向不变。 优点：避免欧拉方程退化现象，可全姿态工作；
缺点：需解9个一阶微分方程，计算量大。
姿态矩阵计Leabharlann Baidu方法
3 四元数微分方程及其解
dq 1 Wq dt 2
捷联惯导计算总结
2011.10.
捷联式惯导计算流程
确定机械编排（选定导航坐标系，列写导航方程）
系统初始化——初始对准，获得初始姿态、初始位置、
初始速度） 姿态微分方程求解（方向余弦、欧拉角、四元数、等效 旋转矢量），姿态矩阵求取，姿态提取 加速度矢量变换，速度微分方程器求解，速度获取 位置微分方程求解，位置获取
姿态矩阵计算方法
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
令：