水下航行器导航系统原理 课件 第4讲 导航基础(一)

导航基础（一）HARBINENGINEERING

UNIVERSITY目录/Contents010203坐标系03方向余弦欧拉角惯性器件标定01坐标系导航的任务就是确定载体的运动参数，即确定载体在某个坐标系的位置、速度以及姿态等。什么是运动物体的姿态？词典:

指物体呈现的样子。惯性技术:

指载体坐标系与参考坐标系相对角位移关系。1.

坐标系5常用坐标系如下：惯性坐标系i地球坐标系e地理坐标系t载体坐标系b平台坐标系p1.

坐标系惯性坐标系（i）：相对恒星所确定的参考系称为惯性空间，相对惯性空间静止或作匀速直线运动的参考坐标系。日心惯性坐标系：原点取在日心。地心惯性坐标系：原点取在地心，Z 轴与地球自转轴一致，X、Y 轴在赤道平面内，指向某个恒星,与Z构成右手直角坐标系。地心惯性坐标系不参与地球的自转运动，即其三根坐标轴在惯性空间的方向保持不变。1.

坐标系地球坐标系（e）：原点取在地心，Z 轴沿极轴（地轴）e方向，Xe

轴在赤道平面与本初子午面的交线上，Ye

轴也在赤道平面内并与Xe

、Ze

轴构成右手直角坐标系。地球坐标系随地球自转而变化。RR

o

hxeyeez

极轴

ie本初子午面赤道平面1.

坐标系地理坐标系（t）原点位于运载体所在的点，Xtt轴沿当地纬线指东，Y

轴沿当地子午线指北；

Z

轴垂直于tXt,Yt 轴构成的平面，构成右手直角坐标系。其中Xt

、Yt

轴构成的平面即为当地水平面。地理坐标系随地球自转、载体运动而变化。R

o

h极轴NExtytzt1.

坐标系载体坐标系（b）：载体坐标系zb轴垂直于甲板，

yb轴沿载体纵轴，xb轴沿载体横轴。载体姿态即为载体坐标系相对于地理坐标系角位移关系，对于船舶，可由横摇角、纵摇角和航向角三个角度描述。1.

坐标系平台坐标系（p）：在平台式惯性导航系统里，惯性元件陀螺仪和加速度计安装在与运载体姿态运动相隔离的平台上，原点在平台质心，zp轴垂直于平台台面，yp轴指向平台北，

xp轴指向东。构成右手坐标系。平台坐标系和地理坐标系一致么？1.

坐标系02方向余弦问题：已知一个矢量在某个坐标系下的表示，如何求其在其他坐标系下的表示？方向余弦法（九参数法）欧拉角法（三参数法）四元数法（四参数法）2.

方向余弦方向余弦定义设取直角坐标系OXYZ，沿各坐标轴的单位矢量分别为i、j、k；设过原点有一矢量R，它在各坐标轴上的投影分别为Rx，Ry，Rz。矢量R的投影表示为𝑹=𝑅𝑥𝒊+

𝑅𝑦𝒋+𝑅𝑧k投影Rx，Ry，Rz分别表示为𝑅

∙

𝑐𝑜𝑠 𝑅෢,

𝑥 ,𝑅

∙

𝑐𝑜𝑠 𝑅෢,

𝑦 ,

𝑅

∙

𝑐𝑜𝑠 𝑅෢,

𝑧其中𝑐𝑜𝑠 𝑅෢,

𝑥 ,

𝑐𝑜𝑠 𝑅෢,

𝑦 ,

𝑐𝑜𝑠 𝑅෢,

𝑧 是矢量R与坐标轴X,

Y,

Z正向之间夹角的余弦，称为方向余弦。XYZORR

xR

zR

yijk2.

方向余弦坐标系各坐标轴方向余弦直角坐标系Oxryrzr与载体固连（简称r系）Ox0y0z0为参考坐标系（简称0系）要确定载体在空间的角位置，只要确定载体坐标系在参考坐标系的角位置即可。而要做到这一点，只需要知道xr、yr、zr这三个轴的九个方向余弦。Z0X

00YZrX

rYrRj0k0irrjkri02.

方向余弦Cr00r或

C

来表示，即

23

21 220

C

C

C

C11 C12 C13

C

r

32

2212

C

C

CC21 C31

C11C

0r方向余弦矩阵把上述九个方向余弦组成一个3*3阶矩阵，用Cr0

C31 C32 C33

则称这种矩阵为方向余弦矩阵。其中

C13 C23 C33

为0系到r系的方向余弦矩阵，为r系到0系的方向余弦矩阵。0rC2.

方向余弦基于方向余弦的坐标变换设过坐标原点O有一矢量R，矢量端点为M。现直接用Xr,

Yr,

Zr代表R在刚体坐标系OXrYrZr上的投影，并直接用X0、Y0、Z0代表R在参考坐标系OX0Y0Z0上的投影，矢

量

R

在

刚

体

坐

标

系

和

参

考

坐

标

系OX0Y0Z0中可分别表示为：𝑹=𝑥0𝒊𝟎+𝑦0𝒋𝟎+

𝑧0𝒌𝟎𝑹=𝑥𝑟𝒊𝒓+𝑦𝑟𝒋𝒓+

𝑧𝑟𝒌𝒓Z0X

00YZrX

rYrRk0irjrj0kri02.

方向余弦基于方向余弦的坐标变换如果用方向余弦表示R在刚体坐标系OXrYrZr上的投影，则有:𝑥𝑟=𝑥0

∙cos 𝑥𝑟,

𝑥0൞𝑦𝑟=𝑥0

∙

cos 𝑦𝑟,

𝑥0𝑧𝑟=𝑥0

∙

cos 𝑧𝑟,

𝑥0+𝑦0

∙

cos 𝑥𝑟,

𝑦0+𝑦0

∙

cos 𝑦𝑟,

𝑦0+𝑦0

∙

cos 𝑧𝑟,

𝑦0Z0X

00YZrX

r+𝑧0

∙

cos 𝑥𝑟,

𝑧0+𝑧0

∙

cos 𝑦𝑟,

𝑧0+𝑧0

∙

cos 𝑧𝑟,

𝑧0YrRj0k0rirjkri02.

方向余弦写成矩阵形式：𝑥𝑟𝑦𝑟𝑧𝑟=𝐶11𝐶21𝐶31𝐶12𝐶22𝐶32𝐶13𝐶23𝐶33∙𝑥0𝑦0𝑧00=𝐶𝑟

∙𝑥0𝑦0𝑧0按照类似的方法，R在参考坐标系OX0Y0Z0上的投影可表示为：𝑥0𝑦0𝑧0𝐶11𝐶21𝐶31𝐶12𝐶22𝐶32𝐶13𝐶23𝐶33= ∙𝑥𝑟𝑦𝑟𝑧𝑟𝑟=𝐶0

∙𝑥𝑟𝑦𝑟𝑧𝑟对于任意一个确定矢量，利用方向余弦矩阵就可以在两个坐标系之间进行坐标变换。因此方向余弦矩阵又称为坐标变换矩阵。2.

方向余弦方向余弦矩阵性质根据方向余弦矩阵的正交性质，方向余弦矩阵具有如下性质：（1）两个方向余弦矩阵互为转置矩阵0 𝑟𝐶𝑟

𝑇

𝐶0

𝑇𝑟 0=

𝐶0 =

𝐶𝑟（2）两个方向余弦矩阵互为逆矩阵𝐶𝑟 −1=

𝐶0 𝐶0 −1=

𝐶𝑟0 𝑟 𝑟 0（3）各个方向余弦矩阵的转置矩阵与逆矩阵相等=0 0𝐶𝑟

𝑇

𝐶𝑟

−1=𝑟 𝑟𝐶0

𝑇

𝐶0

−1𝐶𝑟

𝐶𝑟

𝑇0 0=

𝐶𝑟

𝐶𝑟

−1=I0 02.

方向余弦方向余弦间关系式11𝐶2121321𝐶222+

𝐶223+

𝐶2𝐶231

32

33+

𝐶2 +

𝐶2+

𝐶2 +

𝐶2 =

1=

1=

1𝐶11𝐶21+𝐶12𝐶22+𝐶13𝐶23=

0𝐶21𝐶31+𝐶22𝐶32+𝐶23𝐶33=

0𝐶31𝐶11+𝐶32𝐶12+𝐶33𝐶33=

0𝐶11𝐶21𝐶31𝐶12𝐶22𝐶32𝐶13𝐶23𝐶33𝐶11𝐶12𝐶13𝐶21𝐶22𝐶23𝐶31𝐶32𝐶33= 01 0 01 00 0 1九个方向余弦之间存在六个约束条件，因而实际上有三个方向余弦是独立的。通常采用三个独立的转角即欧拉角来求出九个方向余弦的数值，这样便能唯一的确定两个坐标系之间的相对角位置。2.

方向余弦03欧拉角刚体坐标系相对参考坐标系的角位置，可以用三次独立转动的三个转角来表示，这就是欧拉法，三个独立的转角称为欧拉角。第一次旋转可绕任一轴进行第二次旋转绕其余两轴中任一轴第三次旋转绕除第二次外任一轴3.

欧拉角绕

𝑦𝑎正向𝜃绕

𝑧0正向 绕

𝑧𝑏正向𝜓 𝜑𝑥0𝑦0𝑧0−−−−⟶𝑥𝑎𝑦𝑎𝑧𝑎

−−−−⟶𝑥𝑏𝑦𝑏𝑧𝑏−−−−⟶𝑥𝑟𝑦𝑟𝑧𝑟𝑥𝑎=𝑥0∙cos𝜓+𝑦0∙sin

𝜓𝑦𝑎=

𝑥0

∙ −sin

𝜓 +𝑦0∙cos

𝜓𝑧𝑎=

𝑧00x0yxaay

ayy01.

绕Z轴z0(za

)

x0

xa

𝑥𝑎𝑦𝑎𝑧𝑎0=𝑪𝑎

∙𝑥0𝑦0𝑧0=cos

𝜓−sin

𝜓0sin

𝜓 0cos

𝜓 00 1∙𝑥0𝑦0𝑧0将三维旋转表示为二维旋转更容易确定坐标轴之间的方向余弦角3.

欧拉角𝑎=𝑪𝑏

∙𝑥𝑎𝑦𝑎𝑧𝑎=cos

𝜃 0 −sin

𝜃0 1 0sin

𝜃 0 cos

𝜃∙𝑥𝑎𝑦𝑎𝑧𝑎2.

绕Y轴za

bz

ax𝑥𝑏xb

𝑦𝑏𝑧𝑏𝑥𝑏 =𝑥𝑎∙cos𝜃−𝑧𝑎∙sin

𝜃𝑦𝑏 =

𝑦𝑎𝑧𝑏 =𝑧𝑎∙cos𝜃+𝑥𝑎∙sin

𝜃3.

欧拉角𝑥𝑟𝑦𝑟𝑧𝑟𝑏=𝑪𝑟

∙𝑥𝑏𝑦𝑏𝑧𝑏=cos

𝜑sin

𝜑0−sin

𝜑cos

𝜑0001∙𝑥𝑏𝑦𝑏𝑧𝑏xbbyrx3.

绕Z轴yr

𝑥𝑟 =𝑥𝑏∙cos𝜑+𝑦𝑏∙sin

𝜑𝑦𝑟=𝑥𝑏

∙−sin

𝜑+𝑦𝑏∙cos

𝜑𝑧𝑟=

𝑧03.

欧拉角𝑥𝑟𝑦𝑟𝑧𝑟=

𝑪𝑟 ∙𝑪𝑏∙𝑪𝑎

∙𝑏 𝑎 0𝑥0𝑦0𝑧00=𝑪𝑟

∙𝑥0𝑦0𝑧0综合三次旋转：𝑪𝑟 =

𝑪𝑟 ∙𝑪𝑏∙

𝑪𝑎0 𝑏 𝑎 0cos𝜑cos𝜃cos𝜓−

sin𝜑sin𝜓cos𝜑cos𝜃sin𝜓+

sin𝜑cos𝜓−cos𝜑sin𝜃=−sin𝜑cos𝜃cos𝜓−

cos𝜑sin𝜓sin𝜃𝑐𝑜𝑠𝜓−sin𝜑cos𝜃sin𝜓+

cos𝜑cos𝜓sin𝜃sin𝜓sin𝜑sin𝜃cos𝜃3.

欧拉角欧拉角与姿态载体坐标系oxbybzb与导航坐标系oxnynzn之间的关系，可以用三个转动欧拉角来表示，对于船舶，定义如下：oxnyn

zn

oxbyb

zbox1

y1z1绕ox1

绕oy2

ox2y2z2绕

ozn航向角纵摇角横摇角3.

欧拉角04惯性器件标定4.

惯性器件标定安装误差示意图陀螺仪、加速度计的输出是什么？以陀螺仪为例：1、单位是什么？标度因数2、是否表征载体系？安装误差3、测量是否准确？零偏、标度因数误差

Gx

zx

Gz

g gzygyxgxygxzgzx

Gy

gzyyzg ggxzgzygyzgyxgxyg

bz

by

bx

cos

cos

yxsin

cos

sin

cos

sin

cos

cos

xzsin

sin

cos

cos

cos

sin

Gz

Gx

gyxgxyzx

Gy

gxzgzygyz

bz

by

bx

11

g

1

小角度4.

惯性器件标定

Gy

gygy

D

Kgx

Ngx

0y

D0

x

0

Ngz

0

N

0Kgz

Gz

D0z

K 0

0 0

Gx

标度因数：输出量与输入量的比值零偏：输入量为0时的输出量

gygyGy

D

N

0y

0 1/

K 00 0 1/

K

gz

N

gz

D0

z

0

Ngx

D0x

1/

K

gx

0

Gz

Gx

移项

b