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

20
4.5 1阶~ 4阶 角增量算法
n
1
2
3
4
Sn
1
1
1
Cn
0
2
1 02 6
1 2
1
2 0
6
1
2 0
2 24
CbE (n
1)
CbE (n)
I
S
b ib
C
(
b ib
)
2
1阶:
CbE
(n
1)
CbE
(n)
I
b ib
2阶: 3阶: 4阶:
CbE
(n
1)
CbE
(n)I
b ib
1 2
(
b ib
)2
i j
dk'
dt dk'
dt
k
dk' dt
z ( j'z k'y )
0
dk' dt
(i'y
j'x )
Lecture 10 -- Algorithms for SINS
14
3. 方向余弦矩阵微分方程
则
C
i ( j'z j ( j'z
k'y ) k'y )
k ( j'z k'y )
其中 近似计算:
S sin 0 0
C 1 cos 0
2 0
order
1
2
S
1
1
1
C
0
2
3
1 02 6
1 2
4
1
2 0
6
1
2 0
2 24
解的结果: CbE (t t) CbE (t)C
或 CbE (n 1) CbE (n)C
对 C-S 参数不同程度的近似可以得到不同阶次的算法.
Lecture 10 -- Algorithms for SINS
m主ajor 1800 (900 ,1800 )
m主ajor 1800 (1800 ,2700 )
Lecture 10 -- Algorithms for SINS
23
Outline
捷联惯导系统概述 使用欧拉角的姿态算法 方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法 数值积分 角速率提取
cos cos
cos sin sin sin cos
cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
载体相对地理坐标系的转动角速度 (表 示在地理坐标系中):
b
b
Eb
Ebx
b Eby
bT Ebz
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
sin
cos
sin
cos cos
T11 T12 T13
C
b E
T21
T22
T23
T31 T32 T33
C11 y C21 y
C12 C22
x x
C31 y C32 x
C11 C12 C13 0
C21
C 22
C
23
z
z y
0
x
C
C31 C32 C33 y x
0
C C --- 关于方向余弦矩阵的微分方程
-- 斜
对称矩阵
Lecture 10 -- Algorithms for SINS
Lecture 10 -- Algorithms for SINS
24
5.1 数值积分: 一阶算法
Lecture 10 -- Algorithms for SINS
17
4.2*毕-卡解
解方程：
C bE
C
E b
b ib
E iE
C
E b
记
CbE
(t
t)
CbE
(t
)
t t
e t
bib (t
) dt
0
b ib
t t
b
t ibZ
b ibY
b ibZ
0
b ibX
b ibY
b ibX
dt
0
0
13
3. 方向余弦矩阵的导数
i i' i j' i k'
C
j
i'
j j'
j k'
k i' k j' k k'
i' j'
其中
di' dt
i'
x
y
10
dj' dt
(k'x
i'z
)
so
C
i j
di'
dt di'
dt
k'
k
di' dt
i dj' dt
j dj' dt
k dj' dt
ibb
dt tt b
t
ib
ibbX
ibbY
ibbZ T
0
b ib
b ibZ
ibbZ
0
b ibY
ibbX
ibbY
b ibX
0
Lecture 10 -- Algorithms for SINS
19
4.4 C-S 参数
毕-卡解可以改写为 : CbE (t t) CbE (t) I S bib C (bib )2
r真eal /2
象限象限
0 /2
0 3 / 2
m主ajor
(00 ,900 )
m主ajor
(00 ,900 )
m主ajor 1800 (900 ,1800 )
m主ajor 1800 (1800 ,2700 )
m主ajor
(00 ,900 )
m主ajor 3600 (900 ,00 )
i (k'x i'z ) j (k'x i'z ) k (k'x i'z )
i (i'y j (i'y
jj''xx))
k (i'y j'x )
CC2122
z z
C13 y C23 y
C32 z C33 y
C13 x C11 z C23 x C21 z C33 x C31 z
15
Outline
捷联惯导系统概述 使用欧拉角的姿态算法 方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法 数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
16
4.1 方向余弦矩阵微分方程
方向余弦矩阵微分方程
C C
sin 1 (T13 ) θ 介于 [-90, +90]
唯一
tg
1
T23 T33
γ 介于 [-180, +180] 不唯一
tg
1
T12 T11
ψ 介于 [0, 360] 不唯一
Lecture 10 -- Algorithms for SINS
22
4.6从方向余弦矩阵到欧拉角
cos cos
CEb cos sin sin sin cos
b Ebx
b Eby
cos cos
b Ebz
cos
or
1
cos
0 0
sin sin cos cos
sin
cos sin
s in cos
EEbbbbyx
cos Ebbz
当θ= 90 度, 方程是奇异的，意味着载体的滚动角受限
Lecture 10 -- Algorithms for SINS
方位
S2
滚动
C 为 S2 和 S1 之间的方向余 弦矩阵，即:
其中
AS1 CAS 2
C11 C12 C13 i i'
C C21
C22
C
23
j
i'
C31 C32 C33 k i'
i j' j j' k j'
i k' j k' k k'
俯仰
S1
Lecture 10 -- Algorithms for SINS
基于Peano-Baker 解的角增量算法 数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
2
1.1*捷联惯导系统: 特点
没有物理的平台 陀螺和加速度和载体固联
对陀螺仪的角速度输出进行 积分，获取载体的姿态信息.
方位轴
滚动轴
俯仰轴
加速度计的输出需要投影到导航坐标系中.
俯仰轴
S1
Lecture 10 -- Algorithms for SINS
4
1.3 SINS的示意框图
AX
沿载体轴的 加速度输出
AY
AZ
GX
沿载体轴的Baidu Nhomakorabea
GY 角速率输出 GZ
惯性元件
利用方向余弦 矩阵进行坐标 变换
方向余弦矩阵 的元素
计算载体的 姿态
计算机
沿地理坐标 系各轴的加 速度
导 位置
航
计 算
cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
sin
cos
sin
cos cos
tg
1
T23 T33
注意 cos 0
tg
1
T12 T11
T33 T23 0
r真eal /2
象限象限
T11 T12 0
求解欧拉角速率, 得到
1
0
0
0
cos sin
s
sin in cos
1
b Ebx
b Eby
cos cos
b Ebz
Lecture 10 -- Algorithms for SINS
9
2.2 欧拉角速率
1
0
0
0
cos sin
s
sin in cos
1
C
E b
C
E b
b Eb
其中
0
b Eb
z
z
0
y
x
y x
0
陀螺仪敏感到的是载体的绝 对角速度 bib , 因此
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
所以
C bE
C
E b
b ib
E iE
C
E b
第二项可相对较慢更新
Strap-down Inertial Navigation System
Introduction and Algorithms
捷联惯导系统——介绍及算法（DCM）
Lecture 10 -- Algorithms for SINS
1
Outline
捷联惯导系统概述 使用欧拉角的姿态算法 方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
CbE
(n
1)
CbE
(n)I
(1
02
6
)bib
1 2
(bib
)2
CbE
(n
1)
CbE
(n)I
(1
2 0
6
)bib
1 ( 2
2 0
24
)(bib
)2
Lecture 10 -- Algorithms for SINS
21
4.6 从方向余弦矩阵到欧拉角
根据载体坐标系和地理坐标系之间的方向余弦矩阵，可以算出载 体的姿态角
6
2.0 姿态计算
计算姿态矩阵: 欧拉角法
利用顺序的三次旋转描述载体相对 地理坐标系的姿态: azimuth --ψ pitching --θ rolling ----γ
方向余弦矩阵：
CEb C C C
Lecture 10 -- Algorithms for SINS
7
2.1 载体转动的表示
速度
显
机
示
对地理坐 标系进行 修正
姿态
数学平台
Lecture 10 -- Algorithms for SINS
5
Outline
捷联惯导系统概述 使用欧拉角的姿态算法 方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法 数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
b ibZ
ibbY
b ib
t t
t
bib
dt
ibb
dt tt b
t
ib
ibbZ
0
ibbX
ibbY
b ibX
0
2 0
(
b ibX
)2
(
b ibY
)2
(
b ibZ
)2
CbE (t
t)
CbE
(t)I
sin 0 0
bib
1
cos0
2 0
(bib )2
如果
b Eb
的方向不变，则上述解是精确的
需要解9个微分方程, 计算量较大
Lecture 10 -- Algorithms for SINS
18
4.3*角增量算法
CbE
(t
t)
CbE
(t)I
sin 0 0
bib
1 cos0 02
(bib
)2
激光陀螺一般是脉冲输出, 每个脉冲代表 一个单位的角增量.
在每个采样时间间隔Δt 内, 角增量输出可以近似表示成:
10
Outline
捷联惯导系统概述 使用欧拉角的姿态算法 方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法 数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
11
3. 方向余弦矩阵
方向余弦矩阵(DCM)微分方程:
0 0
C
C
0
C
0
0 0
sin
cos
sin
cos cos
Lecture 10 -- Algorithms for SINS
8
2.2 欧拉角速率
b Ebx
b Eby
C
C
0 0
C
0
0
1 0
b Ebz
0 0 0
0
c os sin
sin
sin
c os
cos cos
对导航坐标系中的加速度分量进行积分，获取载体的速度 和位置信息.
Lecture 10 -- Algorithms for SINS
3
1.2 姿态变换
加速度需要变换:
AE AN
C
Abx Aby
A Abz
C C
C 包含姿态信息.
也可利用欧拉角、四元数等表示.
方位轴
滚动轴
设 S1 为导航坐标系，其单 位坐标矢量为 i, j 和 k S2 为载体坐标系, 其单位坐标矢量 为 i’, j’ 和 k’
方位
S2
滚动 俯仰
S1
Lecture 10 -- Algorithms for SINS
12
3. 方向余弦矩阵
假设 X Y Z T
为载体坐标系 S2 相对于导航坐 标系 S1 的角速度, 表示在 S2 中