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

sin sin cos cos cos
求解欧拉角速率, 得到
0 1 0 cos 0 sin
sin sin cos cos cos
dj ' (k ' x i ' z ) dt
dk ' (i ' y j ' x ) dt
Lecture 10 -- Algorithms for SINS
14
3. 方向余弦矩阵微分方程
则
i ( j ' z k ' y ) i (k ' x i ' z ) i (i ' y j ' x ) C j ( j ' z k ' y ) j (k ' x i ' z ) j (i ' y j ' x ) k ( j ' z k ' y ) k (k ' x i ' z ) k (i ' y j ' x )
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
8
2.2 欧拉角速率
b Ebx 0 0 0 1 b Eby C C 0 C 0 0 cos b Ebz 0 0 0 sin
C C C E C E b
b b
Eb
其中

b Eb
0 z y
z 0
x
y x
0
陀螺仪敏感到的是载体的绝 对角速度 b , 因此 ib
第二项可相对较慢更新
Lecture 10 -- Algorithms for SINS
b ( t ) dt ib
t t t
b dt ib
b ibdt
如果
b 的方向不变，则上述解是精确的 Eb
Lecture 10 -- Algorithms for SINS
需要解9个微分方程, 计算量较大
18
4.3*角增量算法
sin 0 1 cos 0 b b 2 C (t t ) C (t ) I ib (ib ) 2 0 0 激光陀螺一般是脉冲输出, 每个脉冲代表 一个单位的角增量.
i'
dj' di' i dt i dt di' dj' so C j j dt dt k di' k dj' dt dt j' k ' y z ( j ' z k ' y ) 0 0
dk' i dt dk' j dt dk' k dt
E b E b
在每个采样时间间隔Δt 内, 角增量输出可以近似表示成:

b ib
t t
0 b b ib ibZ b ibY
t

b ib
dt
b ibX

b ibY

b T ibZ

b ibZ
当θ= 90 度, 方程是奇异的，意味着载体的滚动角受限
Lecture 10 -- Algorithms for SINS
10
Outline
捷联惯导系统概述
使用欧拉角的姿态算法
方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法
数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
Strap-down Inertial Navigation System
Introduction and Algorithms
捷联惯导系统——介绍及算法（DCM）
Lecture 10 -- Algorithms for SINS
1
Outline
捷联惯导系统概述
使用欧拉角的姿态算法
方向余弦矩阵微ຫໍສະໝຸດ Baidu方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法
数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
2
1.1*捷联惯导系统: 特点
没有物理的平台
方位轴
陀螺和加速度和载体固联
对陀螺仪的角速度输出进行 积分，获取载体的姿态信息.
滚动轴 俯仰轴
加速度计的输出需要投影到导航坐标系中. 对导航坐标系中的加速度分量进行积分，获取载体的速度 和位置信息.
Lecture 10 -- Algorithms for SINS
15
Outline
捷联惯导系统概述
使用欧拉角的姿态算法
方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法
数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
修正 姿态 数学平台
沿载体轴的 角速率输出
计算载体的 姿态 计算机
Lecture 10 -- Algorithms for SINS
5
Outline
捷联惯导系统概述
使用欧拉角的姿态算法
方向余弦矩阵微分方程的推导 方向余弦矩阵微分方程的求解
基于Peano-Baker 解的角增量算法
数值积分 角速率提取
Lecture 10 -- Algorithms for SINS
3
1.2 姿态变换
方位轴
加速度需要变换:
AE Abx AN C Aby A Abz
C C
滚动轴 俯仰轴
S1
C 包含姿态信息. 也可利用欧拉角、四元数等表示.
sin sin cos cos cos
sin sin cos cos sin
1
b Ebx b Eby b Ebz
b cos sin Ebx b sin cos Eby b Ebz cos
16
4.1 方向余弦矩阵微分方程
方向余弦矩阵微分方程
b b b Eb ib iE
导航计算可以得到 E IE
b E E 并且 b CE iE Cb iE b E E 因此 b b CE iE Cb Eb ib E E E E 所以 Cb Cb b iE Cb ib
Lecture 10 -- Algorithms for SINS
7
2.1 载体转动的表示
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
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 --- 关于方向余弦矩阵的微分方程
11
3. 方向余弦矩阵
方向余弦矩阵(DCM)微分方程:
滚动 俯仰 方位
S2 设 S1 为导航坐标系，其单 位坐标矢量为 i, j 和 k S1 S2 为载体坐标系, 其单位坐标矢量 为 i’, j’ 和 k’
Lecture 10 -- Algorithms for SINS
12
3. 方向余弦矩阵
17
4.2*毕-卡解
解方程：
E CbE CbE b iECbE ib
t t
记

b ib
b ib
t t
t CbE (t t ) CbE (t ) e t b b b b 0 0 ibZ ibY ibZ ibY t t b b b b b ib ibZ 0 ibX dt ibZ 0 ibX t b b b b ibY ibX ibY ibX 0 0 2 b b b 0 (ibX )2 (ibY )2 (ibZ )2 sin 0 1 cos 0 E E b b 2 Cb (t t ) Cb (t ) I ib (ib ) 2 0 0
S1
Lecture 10 -- Algorithms for SINS
13
3. 方向余弦矩阵的导数
i i' i j ' i k ' C j i ' j j ' j k ' k i ' k j ' k k '
其中
di' i' x dt 1
假设 为载体坐标系 S2 相对于导航坐 标系 S1 的角速度, 表示在 S2 中 C 为 S2 和 S1 之间的方向余 弦矩阵，即: 其中
C11 C C 21 C 31
X
Y Z T
方位
S2
滚动 俯仰
AS1 CAS 2
C12 C 22 C 32 C13 i i' i j ' i k ' C 23 j i ' j j ' j k ' C 33 k i ' k j ' k k '
1
b Ebx b Eby b Ebz
Lecture 10 -- Algorithms for SINS
9
2.2 欧拉角速率
0 1 0 cos 0 sin
cos 1 0 or cos 0
载体相对地理坐标系的转动角速度 (表 示在地理坐标系中):

b Eb


b Ebx

b Eby

T b Ebz

0 0 C C 0 C 0 0 0
Lecture 10 -- Algorithms for SINS
Lecture 10 -- Algorithms for SINS
4
1.3 SINS的示意框图
AX AY AZ
沿载体轴的 加速度输出 利用方向余弦 矩阵进行坐标 变换 方向余弦矩阵 的元素 沿地理坐标 系各轴的加 速度 导 航 计 算 机 位置 速度
显 示
GX GY GZ
惯性元件
对地理坐 标系进行
Lecture 10 -- Algorithms for SINS
6
2.0 姿态计算
计算姿态矩阵: 欧拉角法 利用顺序的三次旋转描述载体相对 地理坐标系的姿态: azimuth --ψ pitching --θ rolling ----γ
方向余弦矩阵：
b CE C C C