惯导原理捷联惯导基本算法与误差精

0 3 / 2
主
(00 ,900 )
主 3600 (900 ,00 )
主 1800 (900 ,1800 )
主 1800 (1800 ,2700 )
T23 2(P2 P3 P1 ) T33 2 P32 P12 P22 T13 2(P1P3 P2 )
姿态实时计算 概述
其中
CbEBiblioteka Baidu
C
E b
b Eb
0
b Eb
z
z
0
y
x
y x
0
由于陀螺仪直接测得的是载体 相对惯性空间的角速度，所以：
b Eb
b ib
b iE
导航计算可以得到
E IE
有
b iE
C
b E
E iE
CbE
因此
b Eb
b ib
C
b E
E iE
CbE
得
CbE
CbE
b ib
E iE
CbE
姿态计算 矩阵方程精确解2
CbE
CbE
b Eb
的精确解（毕卡逼近）：
C
E b
(t
t)
C
E b
(t)
I
sin 0 0
b Eb
1
c os 0
2 0
(
b Eb
)
2
其中
b Eb
dt tn1 b
tn
Eb
0
b Eb
b EbZ
b EbY
b EbZ
0
b EbX
b EbY
b EbX
0
2 0
(
b EbX
)
2
(
b EbY
)
2
(
y
/
2
P1
x /2
0
PP32
精确解： q(t)
c
os
2
0
I
sin 0
2
0
q(0)
其中：
t2 t1
b
dt
姿态计算 四元数精确解2
q(t)
c
os
2
0
I
sin 0
2
0
q(0)
其中：
t2 t1
b
dt
0
x
y z
x
0
z y
y z
0
x
z
y
x
惯性器件误差补偿 惯性器件误差补偿对捷联惯导的重要性
一、陀螺仪误差模型
静态误差模型
dx1 K 0 K x Ax K y Ay K z Az K xx Ax2 K y Ay2
K zz Az2 K xy Ax Ay K yz Ay Az K zx Az Ax
动态误差模型
dx2 D1x D2y D3z D4 x D5 y D6 z
姿态矩阵的实时计算
因假定“数学平台”跟踪地理坐标系，因
b Eb
b ib
b iE
此 所以可得相应的姿态矩阵微分方程（6-12）：
CbE
CbE
b ib
E iE
CbE
或四元数微分方程：
q(t)
(
b ib
b iE
)q(t)
注意事项： 1、上述两个方程中的角速度表达式不一样 2、方程第二项较小，计算时速度可以低一些
捷联惯导基本算法与误差
捷联惯导系统算法概述 算法：从惯性仪表输出到导航与控制信息 捷联惯导算法的基本内容： 一、系统初始化： 1、给定飞行器初始位置、速度等 2、数学平台的初始对准 3、惯性仪表的校准 二、惯性仪表的误差补偿 三、姿态矩阵的计算 四、导航计算 五、导航控制信息的提取
惯性器件误差 陀螺
象限
(00 ,900 ) (00 ,900 ) (900 ,1800 ) (1800 ,2700 )
如利用四元数微分方程求解， 则先利用四元数求解结果计算 方向余弦矩阵的元素(1-58)：
T11 2 P12 P22 P32 T12 2(P1P2 P3 )
T11 T12 0
真 /2
象限
b
b
Eb
Ebx
b Eby
bT Ebz
0 0
C
C
0
C
0
0 0
姿态计算 欧拉角微分方程3
0 0 1 0
C
C
0
C
0
0
c os
0 0 0 sin
sin
sin
c
os
cos cos
求解欧拉角速率得
1 0 0 cos 0 sin
sin sin cos
b EbZ
)
2
b Eb
方向不变时的精确解
九个微分方程求解，计算量大
姿态计算 四元数精确解1
三、四元数微分方程式及其解
由第一章，四元数微分方程式：
q b q
对 b 的处理类似上一节
0
P1
PP32
x
y z
/ / /
2 2 2
x /2
0
z /2 y /2
y /2 z /2
0
x /2
z / 2
D7
2 x
D8
2 y
D9
2 z
D10 x y
D11 y z
D12 z x
惯性器件误差 加速度计
二、加速度计误差模型
静态误差模型 A1 K0 K1Ai K2 Ai2 K3 Ai3 K4 AP K5 AO
K6 AP Ai K7 AO Ai
动态误差模型
A2
D1i
D2O
D3P
D4i2
D5
1
b Ebx
b Eby
cos cos
b Ebz
cos
1
cos
0
0
sin sin cos cos
sin
cos sin
s in cos
b Ebx
b Eby
cos
b Ebz
注意事项：当 θ= 90 度时，方程出现奇点
姿态计算 矩阵方程精确解1
二、方向余弦矩阵微分方程及其解 C C
0
2 0
2 X
2 Y
2 Z
姿态计算 姿态航向角计算1
四、姿态和航向角的计算
根据载体和地理坐标系之间的方向余弦矩阵可确定姿态、航向角
cos cos
CEb cos sin sin sin cos
cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
姿态计算 欧拉角微分方程2
cos cos
cos sin sin sin cos
cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
sin
cos
sin
cos cos
飞行器相对地理坐标系的角速度：
sin
cos
sin
cos cos
T11 T12 T13
C
b E
T21
T22
T23
T31 T32 T33
sin 1 (T13 )
tg
1
T23 T33
tg
1
T12 T11
姿态、航向角 真值的判断
姿态计算 姿态航向角计算2
T33 T23 0 0
真 /2
/2
主 主 主 1800 主 1800
2 P
D6iO
D7i P
D8O P
D9O
2 i
D10O
2 P
三、惯性器件的误差补偿
姿态计算 欧拉角微分方程1
姿态矩阵的计算 假设数学坐标系模拟地理坐标系 飞行器姿态的描述：
航向角ψ、俯仰角θ、滚动角γ 一、欧拉微分方程
从地理坐标系到载体坐标系 的旋转顺序：
Ψ →θ →γ
方向余弦矩阵： CEb C C C