方向余弦矩阵和姿态角提取算法(矩阵)

陀螺和加速度计被直接“捆绑 (strapped)”到载体上
方位轴
没有物理的平台
对陀螺仪的角速度输出进行 积分，获取载体的姿态信息.
滚动轴
加速度计的输出需要变换到导航坐标系中.
俯仰轴
EN
对导航坐标系中的加速度分量进行补偿并积分，获取载体的速 度和位置信息.
A
3
1.2 姿态变换
z, 方位轴
加速度需要变换:
C11 C21
y y C C1 22 2xx
C32zC33y C33xC31z C31yC32x
C11 C21
C12 C22
C C12330z
C31 C32 C33y
z
0
x
yx C
0
-- 斜
对称矩阵
CC --- 其解取决于 C(0) 和载体的转动角速度的变化规律
A
10
Outline
dt
k
di' dt
i dj' dt
j dj' dt
k dj' dt
i j
dk '
dt dk '
dt
k
dk dt
'
设载体坐标系相对于导航坐标系的角速度为 X Y ZT
表示在载体坐标系中 i' j' k'
则 di' i' x y z (j'z k'y)
dt
100
类似的 dd'jt(k'x i'z) ddk't(i'y j'x)
C C C G0
G0 B0
BT
B0 BT
VN
G IG
R VE
R
e
cos
VE R
tan
e
sin
C G G0 CG G0G IG
CG0 GT
CG G00C
C
B0
C B0 BT
C G0 B0
ζ
G0
N
E0
C G0 GT
BT
C GT BT
GT T
II TGIG
C(C) CC G T B T
G 0 1 G 0 B 0
A
9源自文库
2.3 方向余弦矩阵微分方程
则
C ij((jj'' zz k k'' yy))
i(k'xi'z) j(k'xi'z)
ij((ii'' yy jj'' xx))
k(j'zk'y) k(k'xi'z) k(i'yj'x)
C C2 12 2
z C13 y z C23 y
C13xC11z C23xC21z
S2 为载体坐标系, 其单位坐标矢量 为 i’, j’ 和 k’
z, 方位
S2
x, 滚动
y, 俯仰
N
E
R S1
A
7
2.1 方向余弦矩阵
C 为从 S2 到 S1 的方向余弦 矩阵，即:
AS1 CAS2
其中
C11 C12 C13 i i' i j' i k'
C C21 C22
C23
j
捷联惯导系统概述 姿态矩阵（DCM）微分方程的推导 方向余弦矩阵的求解: 角增量算法
A
11
3.1 方向余弦矩阵微分方程
CBG CBGG B B
记初始时刻的地理坐标系为 G0 ,
则有
C B G0 CB G0B IB
其中
0
BIB
z
z
0
y x
y x 0
记
T 0
BIBdt
0
z
y
z
0
x
y
G T
B 0 B T
A
15
3.7 算例
某捷联惯导系统在 n 时刻, 其载体坐标系和惯性坐标系重合. 然 后从时刻 n 到时刻 n+1, 沿着载体三个轴的三个陀螺仪 X, Y, Z 的角增量输出分别为 0.002, 0.004 和 0.006 (rad); 请利用基于 Peano-Paker 解的一阶角增量算法计算时刻 n+1 载体和惯性坐 标系之间的方向余弦矩阵.
f E fbx
fN
C
f by
f f bz
C 包含姿态信息.
CC
也可用欧拉角、四元数等表示姿态
x, 滚动轴
y, 俯仰轴
e
N
E R
A
4
1.3 SINS的示意框图
AX
沿载体轴的 加速度输出
AY
AZ
GX
沿载体轴的
G Y 角速率输出 GZ
惯性元件
对加速度分量 进行坐标变换
姿态信息
2 24
方程的解 C B G 0 ( T ) C B G 0 (0 )I[ S C 2 ] CBG0(0)C
各阶近似:
1st : CI 2nd : CI12
2 ………..
许多类型的陀螺仪可用内部 硬件完成对角速率的积分， 从而直接输出角增量.
A
14
3.6 地理坐标系的修正
CB G 0(T)CB G 0(0)C
x
0
T
where i 0 idt ix,y,z
(角增量)
and denote
则
CB G 0(T)CB G 0(0)e0 T B IB (t)dt
0 x2y2z2
(向量的模)
A
12
3.2*毕-卡解
CB G 0(T)CB G 0(0)e0 T B IB (t)dtCB G0(0)e
C B G 0(0) Is i n 0 0 1c o 0 2 s02
C(n) I3
0
z y
z
0
x
yx
0 0.006
0.006 0.004 0 0.002
0 0.004 0.002 0
C (n 1 ) C (n )I3
A
16
3.7 算例
i'
j j'
j k'
C31 C32 C33 k i' k j' k k'
载体的旋转导致 C 发生变化
k'
S2
j'
i'
j k
i R S1
A
8
2.2 方向余弦矩阵的导数
i i' i j' i k'
C
j
i'
j j'
j k'
k i' k j' k k'
so
C
i j
di'
dt di'
Strap-down Inertial Navigation System
Introduction and Algorithms
捷联惯导系统——介绍及算法（DCM）
A
1
Outline
捷联惯导系统概述 姿态矩阵（DCM）微分方程的推导 方向余弦矩阵的求解: 角增量算法
A
2
1.1*捷联惯导系统: 特点
计算载体的 姿态信息 计数算学机平台
沿地理坐标 系各轴的加 速度
导 位置
航
计 速度 显
算
示
对地理坐 标系进行 修正
姿态
A
5
Outline
捷联惯导系统概述 姿态矩阵（DCM）微分方程的推导 方向余弦矩阵的求解: 角增量算法
A
6
2.1 方向余弦矩阵
CC 咋来的？
设 S1 为导航坐标系，其单 位坐标矢量为 i, j 和 k
------ 毕-卡形式的解
可记为:
C B G 0 ( T ) C B G 0 ( 0 ) I S C 2
近似计算:
order 1
2
S
1
1
1
C
0
2
3
1
2 0
6
1
2
4
1
2 0
6
1
2 0
2 24
A
13
3.3*角增量算法
order
1
2
S
1
1
1
C
0
2
3
1
2 0
6
1
2
4
1
2 0
6
1
2 0