捷联惯性导航原理

2.捷联惯导力学编排方程
姿态角定义： ψ航向角----载体纵轴在水平面的投影与地理子午线之间 的夹角，用ψ表示，规定以地理北向为起点，偏东方向 为正，定义域0~360°。 θ俯仰角----载体纵轴与纵向水平轴之间的夹角，用θ表 示，规定以纵向水轴为起点，向上为正，向下为负，定 义域-90 ° ~+90 ° 。 γ横滚角----载体纵向对称面与纵向铅垂面之间的夹角， 用γ表示，规定从铅垂面算起，右倾为正，左倾为负， 定义域-180 ° ~+180° 。（载体纵向对称面和 纵轴空 间 铅垂面）
捷联惯性导航原理
2010.11.30 北航通信导航与自动测试实验室
如果载体真实地理位置以纬度、经度、高度 表示，则与此对应的载体在地球坐标系中的 真实位置(x，y，z)可通过下式求得:
地球各点重力加速度近似计算公式: g=g0(1-0.00265cos&)/1+(2h/R) g0:地球标准重力加速度9.80665(m/平方秒) &:测量点的地球纬度 h:测量点的海拔高度 R: 地球的平均半径(R=6370km) s:时间 ？？？？？？？？？？？？？？？？？？？？
f 为地球椭球模型的椭圆度，f= 1/298.257223563
R1 RN h R2 RM h
注意从瞬时速度过来那条线，用来计算w（enn）
3、捷联惯导系统的算法
3.1 姿态更新算法 四元数法：
Q(q0 , q1 , q2 , q3 ) q0 q1i q2 j q3k
1. 惯性导航中的常用坐标系
yt
yb
z e zi
北
xb
zb
zt
xt
O
东
Oe
xi
xe
ye
yi
地球坐标系到地理坐标系转换矩阵
Ce-g=
若为地理坐标系转为地球坐标系则为转置阵
2.捷联惯导力学编排方程
上图理解
上图理解：由陀螺仪的角速度（以及地球自转 等角速度 得到四元数微分方程，求解出 姿态 矩阵：一方面提取姿态角，一方面 把加速度计 比力转化为导航坐标系；再由比力方程得到 速 度，由速度得到位置。）
四元数的三角式为：
2 四元数与姿态矩阵之间的关系 ：
q0 2 q12 q2 2 q32 Cbn 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 )
Q cos

u sin

2
2(q2 q3 q0 q1 ) q0 2 q12 q2 2 q32 2(q1q3 q0 q2 )
1. 惯性导航中的常用坐标系
导航坐标系(下标为n)— Oxn yn zn 导航坐标系是在导航时根据导航系统工作的需要而选取 的作为导航基准的坐标系。 指北方位系统，游离方位系统；
载体坐标系（下标为b）— Oxb yb zb yb 轴沿载体纵轴指向前， xb 坐标原点位于载体的重心， 轴沿载体横轴指向右， zb 轴垂直于平面指向上。
n wie
航相对于地球，地球相对于惯性，相加=导航相对于惯性在n上 投影。再经姿态矩阵，得到相对于b上。（如后面一页中图所示）
0 n wie w cos L ie wie sin L
n wen
VN R h M VE R h N VE sin L R h cos L N
•ψ为航向角，θ为俯仰角，γ为横滚角 •程序中转换采用这一矩阵形式
导航坐标系绕三轴（zxy）依次旋转ψ 角θ角γ角，则得 到机体坐标系。由此，导航坐标系和机体坐标系之间的 转换矩阵为（和前面的不一样？）
0 0 cos 0 1 0 1 Cnb 0 cos sin 0 sin cos sin 0 cos cos sin cos sin sin cos sin cos cos sin sin
R1 RN h R2 RM h
2.捷联惯导力学编排方程
C11 C12 b Cn C C 21 22 C31 C32
C22
C13 C23 C33

90° -90°
C12
→0 →0 + + -
+ + +
主
+ + -
C33

主 主
主+180 主 -180

bz T 构 nb
2.捷联惯导力学编排方程
w w w w w C (w w )
b nb b ib b ie b en b ib b n n ie n en
也就是wnbb=wibb- winb式中： 为地球（坐标系相 对于惯性坐标系的）自转角速率在导航坐标系中的投 n 影；wen 为导航坐标系中相对地球坐标系的角速率 n 在导航坐标系上的投影。（由瞬时速度 Ven 求得） 导
k 4 1/ 2{[q(t ) k3 h] * (t h)}
1. 惯性导航中的常用坐标系
地球坐标系(下标为e)— Oe xe ye ze 地球坐标系的原点在地球中心 Oe ， Oe ze 轴与 Oe zi 轴重合， ye 轴指向东经 xe 轴指向格林威治经线， Oe xe ye 在赤道平面内， 90°方向。又称为空间直角坐标系或地心地固坐标系。 （地球-x轴指向0子午线） 地理坐标系(东北天)(下标为t)— Oxt yt zt 原点选在载体重心处 ， xt 指向东，yt 指向北， z t 沿垂线 方向指向天。 是在载体上用来表示载体所在位置的东向、北向和垂线 方向的坐标系。
2.捷联惯导力学编排方程
载体坐标系与地理坐标系之间的关系-----姿态矩阵 地理坐标系 Oxt yt zt 绕 zt 轴负向（拇指指z负旋转）转 角ψ得ox’y’z’ ， ox’y’z’绕 x’轴转角θ得ox’’y’’’z’’， ox’’y’’’z’’再绕 y’’轴转角γ 则得到载体坐标 Oxb 。（地理到载体即是 yb zb 系 z负xy正正， ψ θ γ 形 式）
2(q1q2 q0 q3 ) q0 2 q12 q2 2 q32 2(q2 q3 q0 q1 )
3、捷联惯导系统的算法
3.1 姿态更新算法 四元数微分方程及计算 n 表征n系至b系的旋转四元数为 Q cos 2 u sin 2 通过求导变换可得四元数微分方程（姿态角速率在变 （更新），但是每一值都知道）
q(t h) q(t ) h / 6(k1 2k 2 2k3 k 4 )
k1 b k 2 1 / 2{[ q(t ) h] * tb (t h / 2)} k 2
2
k1 1/ 2[q(t ) * (t )]
b tb
k 3 1 / 2{[ q(t )
b (t ) 1 q q ( t )* nb (t ) 2
表示成矩阵形式，则有：
q0 0 b q 1 1 wnbx 2 wb nby q2 b wnbz q3
b wnbx 0 b wnbz b wnby
2.捷联惯导力学编排方程
姿态矩阵：从导航坐标系(n系)到载体坐标系(b系)的变 换矩阵；
sin sin sin cos cos b Cn sin cos cos sin sin sin cos cos sin sin sin cos cos cos sin sin cos sin cos cos sin sin cos cos
0

ve R1 cos L
R cos L
e 0 1 t 0
t
L L0
vn dt R2
vn L R2
h h0 vu dt
0
t
h vu
RM Re(1 2 f 3 f sin 2 L)

式中：
RM
RN
为载体所在的子午圈的曲率半径，
RN Re(1 f sin 2 L) 为载体所在的卯酉圈的曲率半径， Re 为地球椭球模型的半长轴， Re 6378137m
b wnby b wnbz 0 b wnbx
b q0 wnbz b q wnby 1 b q2 wnbx 0 q3
3、捷联惯导系统的算法
四元数微分方程 的解法---四阶龙格-库塔法—(适用于陀 螺输出为角速度）-----应将(h/6) （1/2）等括起来
+ -
主
主 -180 主+180
+
-
横滚角真值表
航向角真值表（多值问题）
2.捷联惯导力学编排方程
速度更新计算 比力方程：
n b n n n n V Cb f wen 2wie V g n
里面的值在前面都已列出
有害加速度（即上式中所提到的加速度） 哥氏加速度— 2wn V n ie n 向心加速度— wen V n 重力加速度— g 速度方程： f (2 sin L Ve tgL)V (2 cos L Ve )V V e e ie n ie u 比例方程展开 R1 R1
内容
1
惯性导航中的常用坐标系 捷联惯导力学编排方程
2
3
捷联惯导系统的算法
4
捷联惯导系统的误差分析
1. 惯性导航中的常用坐标系
地心惯性坐标系(下标为i) --- Oe xi yi zi 惯性坐标系是符合牛顿力学定律的坐标系，即是 绝对静止或只做匀速直线运动的坐标系。 以地心 Oe为原点作右手坐标系，Oe zi轴沿地轴指 Oe yi 轴在地球赤道平面内与 向地球的北极，Oe xi ， 地轴垂直并不随地球自转，其中，Oe xi 轴指向春 分点。（惯性-不随地球自转，所以指向春分点） 春分点是天文测量中确定恒星时的起始点，因 此 Oe xi 、Oe yi 、 Oe zi 均指向惯性空间某一方向不变。