捷联惯导系统原理框图
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
捷联惯导系统
捷联惯导系统原理框图
捷联惯导系统
• • • • 姿态更新算法 速度更新算法 位置更新算法 系统误差方程
捷联惯导系统
2. 姿态更新算法（核心）
基本思想：刚体的定点转动 2.1 欧拉角法（三参数法）
（ - ）
b nb b ib b in
Cbn
一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三 个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为参 和 即为一组欧拉角。 考系则 、
b nb x b 0 nby b 1 cos tan nb z 0
cos cos sin
当 90 时，方程退化，故不能全姿态工作。
捷联惯导系统
2.2 方向余弦法（九参数法）
n C n ωbk C b b nb
欧拉角法：概念直观；只适应水平姿态角变化不大的情况，不能全姿态 解算。 方向余弦法：可全姿态工作；但计算量大，不实用。 四元数法：算法简单，计算量小；存在不可交换误差，适应于低动态运 载体。（等效旋转矢量的单子样） 等效旋转矢量法：可对不可交换性误差进行补偿，算法简单，适应于高 动态环境。
捷联惯导系统
b 直线拟合：ωnb (t k ) a 2b
b ωnb (tk ) a 2b 3c 2 抛物线拟合：
b 2 3 ω ( t ) a 2 b 3 c 4 d nb k 三次抛物线：
Φ (h) θ1 θ2 θ3 θ4
2 2 2 q0 q12 q2 q3 CbR 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )
Q cos
u sin
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
位置误差方程： L
VN
RM h
VE
RN h
h
VN ( RM h) 2
VE V sec L tan L sec L h E RN h ( RN h) 2
sec L L
V h U
MATLAB仿真
1、轨迹生成仿真 2、惯导器件输出信息的仿真 3、捷联惯导解算仿真 4、基本函数
捷联惯导中的姿态更新实质上是如何计算四元数。
捷联惯导系统
2.3.2 四元数微分方程
n qb
1 n b qb ωnb 2
n n ( m) n b ( m) qb ( m) qn ( m1) qb ( m1) qb ( m1)
毕卡求解法（角增量） 1）定时采样增量法：采样时间间隔相同； 2）定量采样增量法：角增量达到一固定值时才更新；
2. 速度更新算法
基础：比力方程
n n V n Cbn f b 2ωie ωen V n g n
n n b n 数字递推形式： Vm Vm1 C m1Vsfm Vg / corm n n Vm 1 C m 1 ( Vm Vrotm Vsculm ) Vg / corm
Vrotm 旋转效应：rotation 载体线运动在空间的旋转，角速度与线速度不共线； Vsculm 划桨效应：scull 绕一轴做线振动同时绕另一轴做同频角振动； （根本原因：更新周期内姿态角的变化引起） n Vg /corm 有害加速度：g/Coriolis
捷联惯导系统
2. 位置更新算法
n n n b in in Cb ([ KG ] [ G])ωib εn
姿态误差方程：
N N’
E in U
U
N in U
E’ E
捷联惯导系统
捷联惯导系统误差方程
n n n 速度误差方程： V n n f n Cb ([ K A ] [ A]) f b V n (2ωie ωen ) n n V n (2 ωie ωen ) n
真值表判断
捷联惯导系统
2.4 等效旋转矢量法
四元数法求解中用到了角速度矢量的积分。 当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生误 差，称为转动不可交换性误差。 为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是采用 等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的 概念将四元数微分方程转化为等效旋转矢量微分方程(即Bortz方程)：
736 (θ1 θ2 θ3 θ4 ) 945 334 526 654 (θ1 θ3 θ2 θ4 ) θ1 θ4 θ2 θ3 945 945 945
捷联惯导系统
四元数法与等效旋转矢量法的区别： • 原理相同：计算姿态四元数完成姿态更新； • 四元数算法 等效旋转矢量的单子样算法；
L arcsin P33
0 sin L sin cos L cos L sin sin L
主 arctg
P32 P31
cos
捷联惯导系统
4. 捷联惯导系统误差方差
捷联惯导系统误差源 • 惯性仪表的安装误差和刻度因子误差 b b • 陀螺漂移 ε 和加速度计零位 • 初始条件误差 • 计算误差
[ , v ]
[ (t ), a (t )]
t
[ , v ]
[att , v , pos ]
捷联解算
航迹仿真
加误差
[att , v, pos ]
MATLAB仿真
1、轨迹生成仿真
目的：航迹仿真的目的是生成惯性器件信息源（比力和角速度） ，并给出
相应航迹点的航行参数（姿态、速度和位置） 1）航行轨迹微分方程 姿态角微分方程：
-Q cos
2
u sin
源自文库
cos( ) u sin( ) cos 2 2 2
2 2 u sin 2 2
表征旋转的四元数应该是规范四元数； Q 1 计算误差，失去规范性，需归一化处理；
qi ˆi q
2 2 2 ˆ0 ˆ12 q ˆ2 ˆ3 q q q
θ
t t
t
dt Φ
t t
t
( )dt
q cos Φ 2 Φ Φ sin Φ 2
表征旋转的另一种形式：
Φ u
1 1 b Φ nb (t ) Φ ωb ( t ) Φ (Φ ωb nb nb (t )) 2 12
捷联惯导系统
4q1 q 0 T32 T23 4q 2 q 0 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
Φ(h) θ1 θ2
2 Φ (h) θ1 θ2 θ1 θ2 3
• 算法思路不同； 等效旋转矢量法思路：
n n ( m) n b ( m) qb ( m) qn ( m1) qb ( m1) qb ( m1)
n in
b ib
捷联惯导系统
2.4 几种姿态算法的比较
捷联惯导系统
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
2 2 2 Q q0 q12 q2 q3
四元数表达方式 三角式
Q cos
2
u sin
2
基本运算
捷联惯导系统
动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转 轴转动一个角度（θ，u）
四元数描述转动：
2 2 四元数是刚体转动的一种描述形式。 结论： • 四元数可以描述刚体的定点转动，Q包含了等 效旋转的全部信息； • 四元数与姿态矩阵的关系； • 描述刚体转动的四元数是规范化四元数；
Θ
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
ω(t )
cos ω(t ) 0 sin sin cos b 1 sin ω nb (t ) 0 cos cos 0
矢量的方向余弦表示姿态矩阵的方法； 可全姿态工作，但需要解含有九个未知量的线性方程组，计算量大， 工程上不实用。
捷联惯导系统
2.3 四元数法（四参数法）
2.3.1 四元数基本概念 四元数是由一个实数单位1和一个虚数单位i、j、k组成的含有四个 元的数。（超复数） Q q0 , q1, q2 , q3 q0 q1i q2 j q3k 四元数的大小——范数
sin cos sin cos cos cos 0 sin 0 1 0
1
b nbx b nby b nbz
sin cos cos sin tan
泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）
0 h
b (tk ) a 常数拟合：ωnb
Φ(h) θ
2 Φ (h) θ1 θ2 θ1 θ2 3
Φ (h) θ1 θ2 θ3 33 57 θ1 θ3 θ2 (θ3 θ1 ) 80 80
T11 T21 T31 C bn T T T 12 22 32 T13 T23 T33
1 sin (T32 ) T31 1 tan ( ) 主 T 33 1 T 主 tan ( 12 ) T22
数字递推形式：
n (l ) n(l 1) Cen(l ) Cn C (l 1) e
n n n (l ) en F (t )V n (t ) l en dt F R n C n ( l 1)
sin C en sin L cos cos L cos
0 [ G ] Gz Gy
Gz
0 Gx
Gy Gx 0
K x [ K ] 0 0
0
Ky
0
0 0 Kz
捷联惯导系统
捷联惯导系统误差方程
b b b n n ωnb ωib Cn Cnωin
捷联惯导系统原理框图
捷联惯导系统
• • • • 姿态更新算法 速度更新算法 位置更新算法 系统误差方程
捷联惯导系统
2. 姿态更新算法（核心）
基本思想：刚体的定点转动 2.1 欧拉角法（三参数法）
（ - ）
b nb b ib b in
Cbn
一个动坐标系相对参考坐标系的方位，可以完全由动坐标系一次绕三 个不同的轴的三个角度来确定。把载坐标系作动坐标系，导航系为参 和 即为一组欧拉角。 考系则 、
b nb x b 0 nby b 1 cos tan nb z 0
cos cos sin
当 90 时，方程退化，故不能全姿态工作。
捷联惯导系统
2.2 方向余弦法（九参数法）
n C n ωbk C b b nb
欧拉角法：概念直观；只适应水平姿态角变化不大的情况，不能全姿态 解算。 方向余弦法：可全姿态工作；但计算量大，不实用。 四元数法：算法简单，计算量小；存在不可交换误差，适应于低动态运 载体。（等效旋转矢量的单子样） 等效旋转矢量法：可对不可交换性误差进行补偿，算法简单，适应于高 动态环境。
捷联惯导系统
b 直线拟合：ωnb (t k ) a 2b
b ωnb (tk ) a 2b 3c 2 抛物线拟合：
b 2 3 ω ( t ) a 2 b 3 c 4 d nb k 三次抛物线：
Φ (h) θ1 θ2 θ3 θ4
2 2 2 q0 q12 q2 q3 CbR 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )
Q cos
u sin
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
位置误差方程： L
VN
RM h
VE
RN h
h
VN ( RM h) 2
VE V sec L tan L sec L h E RN h ( RN h) 2
sec L L
V h U
MATLAB仿真
1、轨迹生成仿真 2、惯导器件输出信息的仿真 3、捷联惯导解算仿真 4、基本函数
捷联惯导中的姿态更新实质上是如何计算四元数。
捷联惯导系统
2.3.2 四元数微分方程
n qb
1 n b qb ωnb 2
n n ( m) n b ( m) qb ( m) qn ( m1) qb ( m1) qb ( m1)
毕卡求解法（角增量） 1）定时采样增量法：采样时间间隔相同； 2）定量采样增量法：角增量达到一固定值时才更新；
2. 速度更新算法
基础：比力方程
n n V n Cbn f b 2ωie ωen V n g n
n n b n 数字递推形式： Vm Vm1 C m1Vsfm Vg / corm n n Vm 1 C m 1 ( Vm Vrotm Vsculm ) Vg / corm
Vrotm 旋转效应：rotation 载体线运动在空间的旋转，角速度与线速度不共线； Vsculm 划桨效应：scull 绕一轴做线振动同时绕另一轴做同频角振动； （根本原因：更新周期内姿态角的变化引起） n Vg /corm 有害加速度：g/Coriolis
捷联惯导系统
2. 位置更新算法
n n n b in in Cb ([ KG ] [ G])ωib εn
姿态误差方程：
N N’
E in U
U
N in U
E’ E
捷联惯导系统
捷联惯导系统误差方程
n n n 速度误差方程： V n n f n Cb ([ K A ] [ A]) f b V n (2ωie ωen ) n n V n (2 ωie ωen ) n
真值表判断
捷联惯导系统
2.4 等效旋转矢量法
四元数法求解中用到了角速度矢量的积分。 当不是定轴转动时，即角速度矢量的方向在空间变化时，将使计算产生误 差，称为转动不可交换性误差。 为了消除不可交换性误差，必须对角速度矢量积分修正，修正的方法是采用 等效旋转矢量算法把角速度矢量积分等效为等效旋转矢量，利用等效旋转矢量的 概念将四元数微分方程转化为等效旋转矢量微分方程(即Bortz方程)：
736 (θ1 θ2 θ3 θ4 ) 945 334 526 654 (θ1 θ3 θ2 θ4 ) θ1 θ4 θ2 θ3 945 945 945
捷联惯导系统
四元数法与等效旋转矢量法的区别： • 原理相同：计算姿态四元数完成姿态更新； • 四元数算法 等效旋转矢量的单子样算法；
L arcsin P33
0 sin L sin cos L cos L sin sin L
主 arctg
P32 P31
cos
捷联惯导系统
4. 捷联惯导系统误差方差
捷联惯导系统误差源 • 惯性仪表的安装误差和刻度因子误差 b b • 陀螺漂移 ε 和加速度计零位 • 初始条件误差 • 计算误差
[ , v ]
[ (t ), a (t )]
t
[ , v ]
[att , v , pos ]
捷联解算
航迹仿真
加误差
[att , v, pos ]
MATLAB仿真
1、轨迹生成仿真
目的：航迹仿真的目的是生成惯性器件信息源（比力和角速度） ，并给出
相应航迹点的航行参数（姿态、速度和位置） 1）航行轨迹微分方程 姿态角微分方程：
-Q cos
2
u sin
源自文库
cos( ) u sin( ) cos 2 2 2
2 2 u sin 2 2
表征旋转的四元数应该是规范四元数； Q 1 计算误差，失去规范性，需归一化处理；
qi ˆi q
2 2 2 ˆ0 ˆ12 q ˆ2 ˆ3 q q q
θ
t t
t
dt Φ
t t
t
( )dt
q cos Φ 2 Φ Φ sin Φ 2
表征旋转的另一种形式：
Φ u
1 1 b Φ nb (t ) Φ ωb ( t ) Φ (Φ ωb nb nb (t )) 2 12
捷联惯导系统
4q1 q 0 T32 T23 4q 2 q 0 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
Φ(h) θ1 θ2
2 Φ (h) θ1 θ2 θ1 θ2 3
• 算法思路不同； 等效旋转矢量法思路：
n n ( m) n b ( m) qb ( m) qn ( m1) qb ( m1) qb ( m1)
n in
b ib
捷联惯导系统
2.4 几种姿态算法的比较
捷联惯导系统
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
2 2 2 Q q0 q12 q2 q3
四元数表达方式 三角式
Q cos
2
u sin
2
基本运算
捷联惯导系统
动坐标系相对于参考坐标系的转动，等效于动坐标系绕某一个等效转 轴转动一个角度（θ，u）
四元数描述转动：
2 2 四元数是刚体转动的一种描述形式。 结论： • 四元数可以描述刚体的定点转动，Q包含了等 效旋转的全部信息； • 四元数与姿态矩阵的关系； • 描述刚体转动的四元数是规范化四元数；
Θ
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
ω(t )
cos ω(t ) 0 sin sin cos b 1 sin ω nb (t ) 0 cos cos 0
矢量的方向余弦表示姿态矩阵的方法； 可全姿态工作，但需要解含有九个未知量的线性方程组，计算量大， 工程上不实用。
捷联惯导系统
2.3 四元数法（四参数法）
2.3.1 四元数基本概念 四元数是由一个实数单位1和一个虚数单位i、j、k组成的含有四个 元的数。（超复数） Q q0 , q1, q2 , q3 q0 q1i q2 j q3k 四元数的大小——范数
sin cos sin cos cos cos 0 sin 0 1 0
1
b nbx b nby b nbz
sin cos cos sin tan
泰勒级数展开、曲线拟合的方法（几个采样角就为几子样算法）
0 h
b (tk ) a 常数拟合：ωnb
Φ(h) θ
2 Φ (h) θ1 θ2 θ1 θ2 3
Φ (h) θ1 θ2 θ3 33 57 θ1 θ3 θ2 (θ3 θ1 ) 80 80
T11 T21 T31 C bn T T T 12 22 32 T13 T23 T33
1 sin (T32 ) T31 1 tan ( ) 主 T 33 1 T 主 tan ( 12 ) T22
数字递推形式：
n (l ) n(l 1) Cen(l ) Cn C (l 1) e
n n n (l ) en F (t )V n (t ) l en dt F R n C n ( l 1)
sin C en sin L cos cos L cos
0 [ G ] Gz Gy
Gz
0 Gx
Gy Gx 0
K x [ K ] 0 0
0
Ky
0
0 0 Kz
捷联惯导系统
捷联惯导系统误差方程
b b b n n ωnb ωib Cn Cnωin