惯性导航系统_牛小骥_课件3

1 0 q = q⋅ b 2 ωnb
0 q0 ω q 1 = 1 x q2 2 ω y q3 ω z
− ωx 0 − ωz
− ωy
ωz
0 − ωx
ωy
姿态变换表示为旋转矢量 Bortz方程理论严密 极大的降低了计算时间和和所需存储 空间，精度几乎没有损失 分离出了有限旋转中的不可交换误差
e i
d 2r dv d = e + (ω ie × r ) dt 2 i dt i dt i
dv e = f − ω ie × v e + (g − ω ie × (ω ie × r )) dt i = f − ω ie × v e + g l
v 对比i系的机械编排： ddt
= f − ω ie × v e + g l
(∫
tk
tk −1
Ωb ab dt
)
(∆θb ×) i Cib = Cb lim ∆t → 0 ∆t (∆θb ×) lim = Ωb ib ∆t →0 ∆t i i b Cb = Cb Ωib
∫
tk +1
tk
Ωdt = [σ ×]
0 Ωb ib = ω z −ω y
−ωz 0
ωx
牛小骥
第二步：离散化之后的解算公式
算法在数字计算机上的实现 离散化造成的误差（Coning，Sculling）
2
第三步：误差微分方程
向量的表示
数学符号及概念
x
p rb
坐标变换 角速度 斜 / 反对称 矩阵
i b r i = Cb r
ωib = ωin + ω nb
b ωb ib ⇒ Ω ib
满足矢量的加法
等效旋转矢量——John E. Bortz
− ω z q0 − ωy ⋅ q1 ω x q2 0 q3
1 − cos σ 2 sin σ a a Cb (tk ) = Cb (tk −1 ) I + [σ ×] + 2 [σ ×] σ σ
14
Bortz 方程
微分方程 DCM转换公式
DCM
R B CR B = C B Ω RB
CR B =I+
sin σ
思路：根据以上2方程构造2个恒等的向量 推出旋转矢量微分方程
σ
[ σ ×] +
1 − cos σ
σ
2
[ σ ×]
2
由等效旋转矢量方程得到旋转矢量，用于 DCM微分方程或者四元数微分方程，进行姿 态更新和INS机械编排。
DCM的微分方程的求解
dx = Ax dt x(t) |t =t0 = x 0 ⇓ x = e A (t − t 0 ) x(t 0 )
a a Cb = Cb Ωb ab a a Cb (t k ) = C b (tk −1 ) exp
Cib = lim
∆t →0
∆θ
b b ( t ) b ( t +∆t )
重力定义！
21
dv e = f − 2ω ie × v e + g l dt e
dv e e e = f e − 2ω e ie × v e + g l dt e
e b e e = Cb f − 2ω ie × ve e + gl e
dv e i = f i − ωie × v ie + g li dt i
n
dv e dv = e + ω in × v e dt i dt n
dv e = f − ω ie × v e + g l dt i
= f − ω ie × v e + g l − ω in × v e
n
0 1 ( RN + h) cos ϕ 0
e
= f − (2ω ie + ω en )× v e + g l
无论是DCM还是四元数都涉及到了
13
1 tk q(tk ) = exp ∫ Ω dt q(tk −1 ) 2 tk −1
“Before his work in the early 1970s, strapdown was widely considered as something with possible promise “maybe, if only it could ever come out of the lab-&-theory realm” and into operation. Technological capabilities we take for granted today were far less advanced then; among the many state-of-the-art limitations of that time, processing speed is a glaringly obvious example. To make a long story short, John Bortz made it all happen anyway.”----James Farrell
q = cos µ 2 + µ µ ⋅ sin µ 2
2 [ σ ×]
σ
σ
1 − cos σ 2 sin σ a a Cb (tk ) = Cb (tk −1 ) I + [σ ×] + 2 [σ ×] σ σ
DCM
变换公式
11
12
姿态微分方程——四元数形式
n n Cb = Cb ⋅ Ωb nb
16
姿态更新各种算法比较 欧拉角法，直接计算姿态角。简单明了，概念直观，容易理解， 但方程中包含有三角运算，这给实时计算带来一定困难。而且当 俯仰角接近90度时方程出现退化现象，所以这种方法只适用于水 平姿态变化不大的情况，而不适用于全姿态运载体的姿态确定。 方向余弦法，避免了方程退化的问题，可全姿态工作。但包含了 九个未知量的线性微分方程组，计算量大，实时计算困难。 四元数法，只需求解四个未知量的线性方程组，计算量比方向余 弦法小，且算法简单，易于操作。较实用的工程方法。实质上是 旋转矢量法中的单子样算法，对有限转动引起的不可交换误差的 补偿程度不够，所以只适用于低动态运载体的姿态解算 旋转矢量法，采用多子样算法，对不可交换误差做有效补偿，算 法关系简单，易于操作，并且通过对系数的优化处理使算法漂移 在相同子样算法中达到最小，因此特别适用于角机动频繁激烈或 存在严重角振动的运载体的姿态更新。
方向余弦矩阵(DCM) 四元数 等效旋转矢量
5
6
方向余弦(DCM)
正交的小角变换
思考： 思考： 1.为什么绕 y 轴转动时的 sin 函 数和其他两轴符号相反？ 数和其他两轴符号相反？ 2.小角度下DCM是？
思考陀螺的输出是否可以直接套用上面的公式？ 思考陀螺的输出是否可以直接套用上面的公式？
∆θb (tk −1 )b ( tk ) = ∫ ωb ib dt
ωy −ω x 0
b, 载体坐标系 r, 参考坐标系 p, 投影坐标系
ω x 0 ω ⇒ ω y z ω z −ω y
−ω z 0
ωx
i b Ωiib = Cb Ωib Cib
3
4
为啥姿态很重要
姿态表达式
欧拉角
从n系到b系：航向-俯仰-横滚（Yaw-Pitch-Roll） 不可交换 在俯仰+/-90度时奇异 三个欧拉角定义
ωy −ω x 0
9
10
DCM的微分方程的求解(续)
exp
四元数（ 四元数（Quaternion）
(∫
tk
tk −1
Ωdt
)
2
= I + [ σ ×] +
+ 3! C 1 σ2 σ4 [σ ×] + 2 − 4! + 6! − sin σ 1 − cos σ 2 =I+ [ σ ×] + 2 [ σ ×] 2! σ2 σ4 = I + 1 − + − 3! 5!
dv e n n n = f n − ( 2ωie + ωn en ) × v e + g l dt n
n b n n = Cb f − ( 2ωie + ωen ) × ven + gln
投影到n系：
已知？未知？
23
24
惯导微分算法总结
主要研究的是姿态、速度、位置的微分方程 （连续时间）。 导航状态的时间微分注意区分是从哪个坐标 系中观察的；这与投影到哪个坐标系不同。
主要内容 Outline
惯性导航算法
Algorithm of INS
目标：IMU输出 导航状态（PVA） 第一步：连续时间微分方程
建立理论公式，打好基础 进行误差分析，推导误差微分方程 惯导误差分析
(GNSS Center of Wuhan University) 2014
武汉大学卫星导航定位技术研究中心 年春季学期
C = C (ω ×) = C ( (ω − ω ) × ) = C (ω ×) − (ω ×)C
n b n b b nb n b b ib b in n b b ib n in n n v n = Cbn f b − ( 2ωie + ωen ) × vn + g n n b
d 2r =f +g dt 2 i 不同坐标系下建立：i系、e系、n系
导航方程
地速在不同系下变化不同；已知量
19
20
惯性坐标系机械编排
由哥氏方程 和地速定义： 对i系求导： 经推导得： 投影到i系：