P09-捷联惯导系统-姿态算法(矩阵)

19
Strap-down Inertial Navigation System
Introduction and Algorithms
捷联惯导系统——介绍及算法（DCM）
Lecture 10 -- Algorithms for SINS
1
Outline
捷联惯导系统概述
姿态矩阵（DCM）微分方程的推导
x
G0 B
y x
0
T
where i i dt
(角增量)
i x, y, z
and denote
B IB ( t ) dt
则
C (T ) C (0) e 0
G0 B
0 x y z
2 2
2
(向量的模)
Lecture 10 -- Algorithms for SINS
Lecture 10 -- Algorithms for SINS
10
Outline
捷联惯导系统概述
姿态矩阵（DCM）微分方程的推导
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
11
3.1 方向余弦矩阵微分方程
G C G B C B B GB
C I
1 C I 2 2 ………..
许多类型的陀螺仪可用内部 硬件完成对角速率的积分， 从而直接输出角增量.
Lecture 10 -- Algorithms for SINS
14
3.6 地理坐标系的修正
G0 G0 CB (T ) CB (0)C
G0 G0 B0 CB C B0 CBT T
Lecture 10 -- Algorithms for SINS
3
1.2 姿态变换
z, 方位轴
加速度需要变换:
fE f bx f f C N by f f bz
x, 滚动轴 y, 俯仰轴
e
N

E
C 包含姿态信息.
G0 B
可记为: 近似计算:
order S C
G0 G0 CB (T ) CB (0) I S C 2


1 1
2 1
3
4
0
1 2
02 1 6 1 2
02 1 6 1 02 2 24
Lecture 10 -- Algorithms for SINS
C12 z C13 y C 22 z C 23 y C 32 z C 33 y C11 C 21 C 31 C12 C 22 C 32
C13 x C11 z C 23 x C 21 z C 33 x C 31 z z 0
类似的
dj ' (k ' x i ' z ) dt
dk ' (i ' y j ' x ) dt
Lecture 10 -- Algorithms for SINS
9
2.3 方向余弦矩阵微分方程
则
i ( j ' z k ' y ) i (k ' x i ' z ) i (i ' y j ' x ) j ( j ' k ' ) j (k ' i ' ) j (i ' j ' ) C z y x z y x k ( j ' z k ' y ) k (k ' x i ' z ) k (i ' y j ' x )
C(n) I3
0 z y z 0 x y 0.006 0.004 0 x 0.006 0 0 . 002 0 0 0.004 0.002
C(n 1) C(n)I 3
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
2
1.1*捷联惯导系统: 特点
陀螺和加速度计被直接“捆绑 (strapped)”到载体上 没有物理的平台 对陀螺仪的角速度输出进行 积分，获取载体的姿态信息.
滚动轴

E
方位轴
俯仰轴
N
加速度计的输出需要变换到导航坐标系中. 对导航坐标系中的加速度分量进行补偿并积分，获取载体的速 度和位置信息.
C 为从 S2 到 S1 的方向余弦 矩阵，即:
S2
k'
j'
AS1 CAS 2
其中 C11 C C 21 C 31
i'
C12 C 22 C 32 C13 i i' i j ' i k ' j i ' j j ' j k ' C 23 C 33 k i ' k j ' k k '
j
k
i
R
载体的旋转导致 C 发生变化

S1
Lecture 10 -- Algorithms for SINS
8
2.2 方向余弦矩阵的导数
dj' dk' di' i dt i dt i dt i i' i j ' i k ' di' dj ' dk ' C j i ' j j ' j k ' j so C j j dt dt dt k i ' k j ' k k ' k di' k dj' k dk' dt dt dt 设载体坐标系相对于导航坐标系的角速度为 X Y Z T 表示在载体坐标系中 i' j' k ' di ' 则 i' x y z ( j ' z k ' y ) dt 1 0 0
C13 0 C 23 z C 33 y
x
y x C
0
C11 y C12 x C 21 y C 22 x C 31 y C 32 x
-- 斜
对称矩阵
C --- 其解取决于 C(0) 和载体的转动角速度的变化规律 C
12
3.2*毕-卡解
IB ( t ) dt G0 0 C (T ) C (0) e CB (0) e G0 B G0 B
B T
sin 0 1 cos 0 2 C (0) I 2 0 0 ------ 毕-卡形式的解
C I I TG IG
0
N
GT
T
G0 1 G0 B0 GT CB ( C GT ) CB0 CBT T
Lecture 10 -- Algorithms for SINS 15
3.7 算例
某捷联惯导系统在 n 时刻, 其载体坐标系和惯性坐标系重合. 然 后从时刻 n 到时刻 n+1, 沿着载体三个轴的三个陀螺仪 X, Y, Z 的角增量输出分别为 0.002, 0.004 和 0.006 (rad); 请利用基于 Peano-Paker 解的一阶角增量算法计算时刻 n+1 载体和惯性坐 标系之间的方向余弦矩阵.
记初始时刻的地理坐标系为 G0 , 则有 G0 C G0 B C B B IB
记
B IB dt
0
Twenku.baidu.com
其中
0 B IB z y z 0
0 z y
T 0
z 0 x
y x 0
Lecture 10 -- Algorithms for SINS
17
Summary
捷联惯导系统概述
方向余弦矩阵微分方程的推导
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
18
End
Lecture 10 -- Algorithms for SINS
0.006 0.004 0.006 0.004 1 1 0 0 1 0.006 0.006 1 0 . 002 0 1 0 1 0 . 002 1 1 0.004 0.002 0 0 1 0.004 0.002
Lecture 10 -- Algorithms for SINS
16
3.7 算例
0.006 0.004 1 0 0 0 I 0.006 0 1 0 0 0 . 002 C(n 1) C(n)I 3 3 0 0 1 0 . 004 0 . 002 0
6
2.1 方向余弦矩阵
z, 方位
C C
S2
x, 滚动 y, 俯仰
咋来的？ 设 S1 为导航坐标系，其单 位坐标矢量为 i, j 和 k S2 为载体坐标系, 其单位坐标矢量 为 i’, j’ 和 k’
N

E
R

S1
Lecture 10 -- Algorithms for SINS
7
2.1 方向余弦矩阵
13
3.3*角增量算法
order S C
1 1 2 1
3
4
0
1 2
02 1 6 1 2
02 1 6 1 02 2 24
方程的解 各阶近似:
G0 G0 G0 CB (T ) CB (0)[I S C 2 ] CB (0)C
1st
2nd
:
:
惯性元件
对地理坐 标系进行
修正 姿态
沿载体轴的 角速率输出
计算载体的 姿态信息 数学平台 计算机
Lecture 10 -- Algorithms for SINS
5
Outline
捷联惯导系统概述
姿态矩阵（DCM）微分方程的推导
方向余弦矩阵的求解: 角增量算法
Lecture 10 -- Algorithms for SINS
C C
R

也可用欧拉角、四元数等表示姿态
Lecture 10 -- Algorithms for SINS
4
1.3 SINS的示意框图
AX AY AZ
姿态信息 沿载体轴的 加速度输出 对加速度分量 进行坐标变换 沿地理坐标 系各轴的加 速度 导 航 计 算 位置 速度 显 示
GX GY GZ
G IG
VN R V E e cos R VE t an sin e R
B0
C
B0 BT
BT
C
G0 C G0 G C G G IG
G0 B0
ζ
GT CB T
C
G0 GT
C C
G0 G0
G0
E
G0 CG T