第二章 捷联惯导系统的初试对准
2.1引言
惯导系统是一种自主式导航系统。它不需要任何人为的外部信息，只要给定导航的初始条件（例如初始速度、位置等），便可根据系统中的惯性敏感元件测量的比力和角速率通过计算机实时地计算出各种导航参数。由于“平台”是测量比力的基准，因此“平台”的初始对准就非常重要。对于平台惯导系统，初试对准的任务就是要将平台调整在给定的导航坐标系的方向上。若采用游动方位系统，则需要将平台调水平---称为水平对准，并将平台的方位角调至某个方位角处---称为方位对准。对于捷联惯导系统，由于捷联矩阵T 起到了平台的作用，因此导航工作一开始就需要获得捷联矩阵T 的初始值，以便完成导航的任务。显然捷联惯导系统的初始对准就是确定捷联矩阵的初始值。在静基座条件下，捷联惯导系统的加速度计的输入量为---g ，陀螺的输入量为地球自转角速率ωie 。因此g b 与b ωie 就成为初始对准的基准。将陀螺与加速度计的输入引出计算机，通过计算机b b 就可以计算出捷联矩阵T 的初始值。
由以上的分析可以看出，陀螺与加速度计的误差会导致对准误差；对准飞行器的干扰运动也是产生对准误差的重要因素。因此滤波技术对捷联系统尤其重要。由于初始对准的误差将会对捷联惯导系统的工作造成难以消除的影响，因此研究初始对准的误差传播方程也是非常必要的。
2.2 捷联惯导系统的基本工作原理
捷联式惯性导航系统，陀螺仪和加速度计直接与载体固联，加速度计测量是载体坐标系轴向比力，只要把这个比力转换到导航坐标系上，则其它计算就与平台式惯性导航系统一样，而比力转换的关键就是要实时地进行姿态基准计算来提供数学平台，即实时更新姿态矩阵C b n ，姿态矩阵也称为捷联矩阵。一般选择地
t 理坐标系为导航坐标系，那么捷联矩阵C b n 也可表示为C b ， 其导航原理图如图
2.1所示。
由惯导系统的工作原理可以看出，捷联式惯性导航系统有以下几个主要优点:
1. 惯性敏感器便于安装、维修和更换。 2.惯性敏感器可以直接给出舰船坐标系轴向的线加速度、线速度，供给舰船稳定控制系统和武备控制系统。角速度以提供给舰船稳定控制系统和武 备控制系统。 3.便于将惯性敏感器重复布置，从而易在惯性敏感器的级别上实现冗余 技术，这对提高系统的性能和可靠性十分有利。 4.由于去掉了具有常平架的平台，一则消除了稳定平台稳定过程中的各 种误差; 二则由于不存在机电结合的常平架装置，使整个系统可以做得小而 轻，并易于维护。 当然，由于惯性敏感器直接固接于船体上也带来新的问题，即导致惯性 敏感器的工作环境恶化了。由于惯性敏感器直接承受舰船的振动、冲击及温 度波动等环境条件，惯性敏感器的输出信息将会产生严重的动态误差。为保 证惯性敏感器的参数和性能有很高的稳定性，则要求在系统中必须对惯性敏 感器采取误差补偿措施。
2.3 常用坐标系与捷联矩阵
2.3.1常用坐标系
惯性导航中常用的坐标系有以下几种:
1. 地心惯性坐标系（下标为i ）——O e x i y i z i
惯性坐标系是符合牛顿力学定律的坐标系，即是绝对静止或只做匀速直线运动的坐标系。由于宇宙空间中的万物都处于运动之中，因此想寻找绝对的惯性坐标系是不可能的，我们只能根据导航的需要来选取惯性坐标系。对于在地球附近运动的飞行器选取地心惯性坐标系是合适的。地心惯性坐标系不考虑地球绕太阳的公转运动，当然更略去了太阳相对于宇宙空间的运动。地心惯性坐标系的原点O e 选在地球的中心，它不参与地球的自转。惯性坐标系是惯性敏感元件测量的
基准。由于在进行导航计算时无需再这个坐标系中分解任何向量，因此惯性坐标系的坐标轴的定向本无关紧要，但习惯上我们可以将z i 轴选在沿地轴指向北极的方向上，而x i 、y i 轴则在地球的赤道平面内，并指向空间的两颗恒星。
2. 地球坐标系（下标为e ）——O e x e y e z e
地球坐标系是固连在在地球上的坐标系，它相对于惯性坐标系以地球自转角
O e z e 轴与O e z i ωe =15.04107 /h 。速率ωe 旋转，地球坐标系的原点在地球中心O e ，
O e x e y e 在赤道平面内，x e 轴指向格林威治经线，y e 轴指向东经90方向。 轴重合，
3. 地理坐标系（下标为t ）——Ox t y t z t
地理坐标系是在飞行器上用来表示飞行器所在位置的东向、北向和垂线方向的坐标系。地理坐标系的原点O 选在飞行器的重心处，x t 指向东，y t 指向北，z t 沿垂线方向指向天（东北天）。对于地理坐标系，在不同的惯导文献中往往有不同的取法。所不同之处仅在于坐标轴的正向的指向不同，如还有北西天、北东地等取法。坐标轴指向不同于仅使向量在坐标系中取投影分量时的正负号有所不同，并不影响导航基本原理的阐述及导航参数计算结果的正确性。
4. 导航坐标系（下标为n ）——Ox n y n z n
导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。当把导航坐标系选得与地理坐标系相重合时，可将这种导航坐标系称为指北方位系统；为了适应在极区附近导航的需要往往将导航坐标系的z n 轴仍选得与z t 重合，而使x n 与x t 及y n 与y t 之间相差一个自由方位角或游动方位角α，这种导航坐标系可称为自由方位系统或游动自由方位系统。
5. 平台坐标系（下标为p ）——Ox p y p z p
平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系。平台坐标系的坐标原点O 位于飞行器的重心处。当惯导系统不存在误差时，平台坐标系与导
航坐标系相重合；当惯导系统出现误差时，平台坐标系就要相对导航坐标系出现误差角。对于平台惯导系统，平台坐标系是通过平台台体来实现的；对于捷联惯导系统，平台坐标系则是通过存储在计算机中的方向余弦矩阵来实现的，因此又叫做“数学平台”。对于平台惯导系统，平台坐标系与导航坐标系之间的误差是由平台的加工、装配工艺不完善、敏感元件误差以及初始对准误差等因素造成的；而对于捷联惯导系统，该误差则是由算法误差、敏感元件误差以及初始对准误差造成的。
6. 机体坐标系（下标为b ）——Ox b y b z b
机体坐标系是固连在机体上的坐标系。机体坐标系的坐标原点O 位于飞行器的重心处，x b 沿机体横轴指向右，y b 沿机体纵轴指向前，z b 垂直于Ox b y b ，并沿飞行器的数轴指向上。
2.3.2捷联矩阵的定义
对于捷联惯导系统，加速度计时沿机体坐标系Ox b y b z b 安装的，它只能测量
b b b b p 沿机体坐标系的比力分量f x ，f y b ，f z ，因此需要将f x ，f y b ，f z 转换为f x ，f y p ，
p f z p 。实现由机体坐标系到平台坐标系的坐标转换的方向余弦矩阵C b 又叫做捷联
矩阵，本章用T 来表示；由于根据捷联矩阵的元素可以单值地确定飞行器的姿态角，因此又可以叫做飞行器状态矩阵；由于捷联矩阵起到了平台的作用（借助于
p p 它可以获得f x ，f y p ，f z ），所以又可以叫做“数学平台”。
设机体坐标系Ox b y b z b 固连在机体上，其Ox b 、Oy b 、Oz b 轴分别沿飞机的横轴、纵轴与竖轴，实现由机体坐标系到平台坐标系的坐标转换的捷联矩阵T 应满足如下的矩阵方程
⎡x p ⎤ ⎡x b ⎤ （3-1）⎢⎥⎢⎥⎢y p ⎥=T ⎢y b ⎥⎢z ⎥⎢⎣z b ⎥⎦⎣p ⎦
式中
⎡T 11T 12T 13⎤ （3-2） ⎥T =⎢T T T 23⎥⎢2122
⎢⎣T 31T 32T 33⎥⎦
当矩阵T 求得后，沿机体坐标系测量的比力f 就可以转换到平台坐标系上，得到f 。
由平台坐标系至机体坐标系的转换关系，可以通过下述顺序的三种旋转来表示： p b
''''''x p y p z p −−−→x '→x ''→x b y b z b p y p z p −−−p y p z p −−−ψθγ
其中ψ、θ、γ分别为机体的航向角、俯仰角和倾斜角。
根据上述的旋转顺序，可以得到由平台坐标系到机体坐标系的转换关系，即 ⎡x b ⎤⎡cos γ⎢y ⎥=⎢0⎢b ⎥⎢⎢⎣sin γ⎣z b ⎥⎦⎢0-sin γ⎤⎡10⎢0cos θ10⎥⎥⎢0cos γ⎥⎦⎢⎣0-sin θ0⎤⎡cos ψ⎢-sin ψsin θ⎥⎥⎢cos θ⎥⎦⎢⎣0sin ψcos ψ00⎤⎡x p ⎤⎢⎥0⎥⎥⎢y p ⎥=
⎢⎥1⎥⎦⎣z p ⎦绕z p 轴绕x 'p 轴绕y ''p 轴
⎡cos γcos ψ-sin γsin θsin ψ⎢-cos θsin ψ⎢⎢⎣sin γcos ψ+cos γsin θsin ψcos γsin ψ+sin γsin θcos ψcos θcos ψsin γsin ψ-cos γsin θcos ψ-sin γcos θ⎤⎡x p ⎤⎥⎢y ⎥si n θ⎥⎢p ⎥⎢⎥cos γcos θ⎥⎦⎣z p ⎦（3-3）
由式（3-1）可得
⎡x b ⎤⎡x p ⎤ ⎢y ⎥=T -1⎢y ⎥⎢p ⎥⎢b ⎥⎢z ⎥⎢⎣z b ⎥⎦⎣p ⎦
（3-4）
考虑到捷联矩阵T 为正交矩阵，T -1=T t （t 表示转置），于是
⎡cos γcos ψ-sin γsin θsin ψ
T =⎢⎢cos γsin ψ+sin γsin θcos ψ
⎢-sin γcos θ⎣-cos θsin ψcos θcos ψsin θsin γcos ψ+cos γsin θsin ψ⎤sin γsin ψ-cos γsin θcos ψ⎥⎥
⎥cos γcos θ⎦
(3-5)
2.3 姿态的描述及更新算法
物体（刚体）的姿态是物体相对观察者的几何角度关系的统称。姿态的数学描述是运动体进行姿态运动建模的基础。一般来讲，刚体的运动要用刚体位置移动和绕自身重心的转动来描述。其位置移动可用船体上下起伏（heave ）、前后涌动（surge ）、左右晃动（sway ）来描述，而转动则要用纵摇（pitch ）、横摇（roll ）、和偏航（yaw ）来描述。纵摇是用来描述船在穿过连续的浪头和波谷时，船头的船尾忽上忽下而引起的船体绕横摇轴转动的振荡运动；横摇则是用来描述船体绕纵轴转动的振荡运动。因此，舰船和飞机一类的运载体的姿态参数就是其纵摇角、横摇角和航向角。
为了描述载体的姿态，至少需要建立两个坐标系，即空间参考坐标系（Reference Coordinate）和固连于载体的本体坐标系（Body Reference）。载体坐标系的三个坐标轴在参考坐标系中的方向确定了载体的姿态，描述这些方向的物理量就称为姿态参数。姿态参数有多种形式，最常用的如欧拉角、四元数等。采用不同的姿态参数，可以构成不同的坐标转换矩阵，称为姿态矩阵。因为载体姿态是唯一确定的，因此各种姿态参数之间可以互相转换。
2.3.1欧拉角
欧拉角（Euler Angles ）发源于欧拉定理，即刚体绕固定点的角位移可以由绕该点的若干次有限转动合成。因此，可将参考坐标系绕不同坐标轴连续旋转三次得到载体坐标系，每次的旋转轴取为被旋转坐标系的某一轴，每次的旋转角就称为欧拉角。姿态矩阵与三次转动的顺序有关，但描述三轴控制的飞机、导弹、舰船等方面，一般采用非对称旋转。
由坐标系的定义三个欧拉角的几何意义为：
纵摇角θ（φp ）：载体绕纵摇轴X b 转动的角度；
横摇角γ（φγ）：载体绕横摇轴Y b 转动的角度；
航向角ψ（φψ）：载体绕航向轴Z b 转动的角度。
根据转动规律得到姿态矩阵：
⎡cos φγcos φψ+sin φγsin φp sin φψ⎢C b n =⎢-cos φp sin φψ
⎢sin φγcos φψ+cos φγsin φp sin φψ⎣cos φγsin φψ-sin φγsin φp cos φψcos φp cos φψsin φγsin φψ-cos φγsin φp cos φψ-sin φγcos φp ⎤⎥sin φp ⎥cos φγcos φp ⎥⎦
=⎡T 11T 12T 13⎤ (5-14) ⎢T T ⎥T 23⎥⎢2122
⎢⎣T 31T 32T 33⎥⎦
由(5-14)式可得：
θ=sin -1(T 23) ， γ=tg -1(-T 13T ) ，ψ=tg -1(-21) (5-15) T 33T 22
00纵摇角定义在θ∈[-90,90]区间，和反正弦函数主值一样，不存在多值问题。
0000横摇角定义在γ∈[-180,180]区间，航向角定义在ψ∈[0,360]区间，都存在多
值问题，求姿态角时应设法判断γ和ψ的真值，以确定载体落在哪一个象限。
欧拉角是经典的、应用最为广泛的一种姿态参数，具有直接、明显的几何意义，维数最小（三维），并且往往可以由姿态敏感器直接测量。在某些应用方面，欧拉角描述法具有明显的优势。但是，由于方向余弦矩阵具有九个元素，所以，解算矩阵微分方程时，实际上解算九个联立微分方程，一般说来，计算量比较大。此外，它还有一个固有缺陷，即对于某一个角度存在奇异问题。不同的旋转顺序，奇异角度也不同。因此，要准确描述任意姿态，至少需要两组欧拉角。为了解决这些问题，可以采用四元数法。下面介绍四元数法。
2.3.2四元数
（1） 四元数定义及其姿态矩阵
设坐标系S a (Ox a y a z a ) 绕某轴ON 转动某个角度φ，就与坐标系S b (Ox b y b z b ) 重合，如图5.2所示。
图5.2 四元数示意图
定义如下四个元素： x a ⎧q 0=cos(φ/2) (5-16) ⎪q =C sin(φ/2) ⎪11⎨⎪q 2=C 2sin(φ/2)
⎪⎩q 3=C 3sin(φ/2)
其中：C 1, C 2, C 3分别为旋转欧拉轴ON 在坐标系S a (Ox a y a z a ) 中的方向余弦。 显然，用q 0-q 3这四个元素就可以完全描述这两个坐标系之间的关系，则定义四元数为：
(5-17)
q =q 0+q 1i +q 2j +q 3k =q 0+q
四元数满足以下的约束条件：
222q 0+q 12+q 2+q 3=1 (5-18)
所以，四元数四个变量之中只有三个是独立的。
根据欧拉旋转公式(5-14)及四元数定义，四元数姿态矩阵可表示为：
T +(q 0I -[q ⨯])2]R 0 R b =[qq
2 (5-19) ⎡2(q 0+q 12) -12(q 1q 2+q 0q 3) 2(q 1q 3-q 0q 2) ⎤⎢⎥22=⎢2(q 1q 2-q 0q 3) 2(q 0+q 2) -12(q 2q 3+q 0q 1) ⎥R 0=T b 0(q ) R 0
22⎢2(q 1q 3+q 0q 2) 2(q 2q 3-q 0q 1) 2(q 0+q 3) -1⎥⎣⎦
T b 0() 为导航坐标系与载体坐标系之间的坐标转换矩阵。
注意到四元数姿态矩阵是由四元数q 的二项式组成，因此对于四元数-q =[-q 0 T ]T ，也可以得到(5-19)所示的姿态矩阵。四元数的这种非唯一性-q
是与欧拉角的非唯一性相一致的。物理意义上这两者实际代表同一种姿态、同一种旋转，并无区别。一般我们取四元数正值进行计算。
上述分析说明，如果表征n 系到b 系的旋转四元数Q 已经确定，那么就可以确定出运载体的航向角、俯仰角和横滚角，因此，四元数Q 包含了所有的姿态信息，捷联惯导中的姿态更新实质上是如何计算四元数Q 。
2.3.3四元数与欧拉角的关系
四元数和欧拉角的关系可由四元数的乘积公式求出。欧拉角载体坐标系按三次旋转到导航坐标系，则每次旋转所对应的四元数分别为：
q 1(ψ) =cos ψ
2-k sin ψ
2
q 2(θ) =cos
q 3(γ) =cos θ2+i sin θ (5-20) 2γ
2+j sin γ
2
根据四元数的定义及乘法公式，可得：
q b 0=q 1⊗q 2⊗q 3 (5-21)
因此可解出：
ψθγψθγ (5-22) ⎧q =cos cos cos +sin sin sin ⎪0222222⎪⎪q =cos ψsin θcos γ+sin ψcos θsin γ
⎪1222222⎨⎪q =cos ψcos θsin γ-sin ψsin θcos γ
⎪2222222⎪ψθγψθγ⎪q 3=cos sin sin -sin cos cos ⎪⎩222222
或 ⎧ (5-23) ⎪-1θ=sin [2(q 3q 2+q 1q 0)]⎪⎪⎪-12(q 1q 3-q 2q 0) ]⎨γ=-tg [222(q 0+q 3) -1⎪⎪-12(q q -q q ) ⎪ψ=-tg [1
22230]2(q 0+q 2) -1⎪⎩
与欧拉角相比，四元数避免了奇异问题，没有复杂的三角运算，更有利于存储及计算。但由于计算误差等，较难保证四元数的规范化条件，因此往往需要重新规范化或降阶。
2.3.4 姿态更新的四元数算法
设由运载体的机体轴确定的坐标系为b ，惯导系统所采用的导航坐标系为n ，则由b 系到n 系的坐标变换矩阵C b 称为运载体的姿态矩阵。姿态更新是指根据惯性器件的输出实时计算出C b 矩阵。由于n 系和b 系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可对一个坐标系作平移，使其原点与另一个坐标系的原点重合。因此，
n n
两坐标系间的空间角位置可理解成刚体的定点转动。从这一基本思想出发，可获得姿态更新的四元数算法及旋转矢量算法。
一、四元数微分方程
假设表征n 系至b 系的旋转四元数为
对（1）式求导可得
Q =cos +u R sin （1）
22
θθ
d Q 1R
=ωRb ⊗Q （2） dt 2
R u R 。 刚体绕u 旋转θ，ωRb =θ
在捷联惯导系统中，角速度信息是捷联陀螺提供的，而捷联陀螺是在机体坐标系内测量的，所以（2）式中的ωRb 还需换成ωRb 。根据四元数向量之间的变换关系（r R =Q ⊗r b ⊗Q *）可得，
R b ωRb =Q ⊗ωRb ⊗Q * （3）
R
b
将（3）式代入（2）式可得
d Q 11b b
=Q ⊗ωRb ⊗Q *⊗Q =Q ⊗ωRb （4） dt 22
记
b
ωRb
⎡ωx ⎤ （5）
⎥=⎢ω⎢y ⎥⎢⎣ωz ⎥⎦
即得到
0⎤⎡0⎡q ⎢q ω 1⎥1⎢⎢⎥=⎢x ⎢q 2⎥2⎢ωy
⎢⎢⎥ q ⎢⎣3⎦⎣ωz
b
-ωx 0-ωz
-ωy
ωz
0-ωx
ωy
-ωz ⎤⎡q 0⎤ （6）
⎢q ⎥-ωy ⎥⎥⎢1⎥ωx ⎥⎢q 2⎥⎥⎢⎥0⎥⎦⎣q 3⎦
其中, ωnb 的获取按照下式进行
b b b n n
ωnb =ωib -C n (ωie +ωen ) （7）
b b n
是捷联陀螺的输出；C n 由姿态更新的最新值确定；ωen 和ωie 分别是未知式中，ωib
n
速率和地球自转速率，对于导航坐标系取地理坐标系的情况有
⎡V N ⎤
-⎢⎥R ⎢M ⎥⎢⎥V n n
ωie +ωen =⎢ωie cos L +E ⎥ （8）
R N ⎢⎥
⎢⎥V E
tan L ⎥⎢ωie sin L +R N ⎣⎦
式中，V N 、V E 、L 为导航计算所得的最新值, 分别是北向速度、东向速度、纬度；R N
为地球沿卯酉圈的曲率半径，R M 为地球子午圈的曲率半径，在不精确计算的情况下，也可以视地球为圆球形状，则（8）式中R N 、R M 可用地球半径R e 代替。
二、四元数的初值确定和规范化处理
四元数的初值Q (0)由捷联惯导的初始对准确定。设初始对准确定的姿态阵
n
为C b =[T ij ]，根据式（5-19）及描述刚体旋转的四元数为规范化四元数的结论，有如下方
程成立
222⎧q 0+q 12-q 2-q 3=T 11⎪2
222
⎪q 0-q 1+q 2-q 3=T 22⎪q 2-q 2-q 2+q 2=T
2333⎪01
222⎪q 0+q 12+q 2+q 3=1⎪
⎪2(q 1q 2-q 0q 3) =T 12⎨ （9） ⎪2(q 1q 3+q 0q 2) =T 13⎪2(q q +q q ) =T
0321⎪12
⎪2(q 2q 3-q 0q 1) =T 23⎪
⎪2(q 1q 3-q 0q 2) =T 31⎪2(q q +q q ) =T
32⎩2310
从上述方程解得
⎧
q =⎪0⎪⎪4q q =T -T
3223⎨10
（10）
⎪4q 2q 0=T 13-T 31⎪⎪⎩4q 3q 0=T 21-T 12
q 0的符号可以任选。
由于表征旋转的四元数是规范化四元数，即Q =1，但是由于计算误差等因素，计算过程中四元数会逐渐失去规范化特性，因此若干次更新后，必须对四元数做规范化处理：
q i =
ˆi =0,1, 2,3
ˆ0、q ˆ1、q ˆ2、q ˆ3是四元数更新所得的值。 其中，q
2.4捷联惯导系统的误差模型
对于工作在非极区的捷联惯导系统，为了简化计算，导航坐标系一般选取地理坐标系，这样，捷联惯导完全等效于指北方位系统。由于陀螺在捷联系统中起测量器件作用，陀螺漂移引起的数学漂移与陀螺漂移的方向相反，刻度系数误差引起对运载体角速度的测量误差，
经姿态更新计算引入系统。
2．4．1 速度误差方程和位置误差方程
根据比力方程，当不考虑任何误差时，速度的理想值由下式确定：
=C n f b -(2ωn +ωn ) ⨯V +g n （3.6.1） V n b ie en n
而实际系统中总存在各种误差，所以实际的速度计算值应由下述方程确定：
ˆn f b -2(ωc +ωc ) ⨯V c +g c （3.6.2） =C V c b ie en
式中
V c =V n +δV n
c n n
ωie =ωie +δωie c n n ωen =ωen +δωen
g c =g n +δg
ˆn =C n 'C n =(I -φn ⨯) C n C
b
n
b
b
b =(I +[δK ])(I +[δA ])f b +∇b f A -φU φN ⎤⎡0
⎥φn ⨯=⎢φ0-φU E ⎢⎥
⎢0⎥⎣-φN φE ⎦[δK A ]=diag [δK Ax δK Ay
δK Az ]
⎡0⎢
[δA ]=⎢-δA z
⎢δA y ⎣
δA z
0-δA x
-δA y ⎤
⎥δA x ⎥0⎥⎦
其中，φE 、φN 、φU 为姿态误差角，δK Ai 和δA zi (i =x , y , z ) 分别为加速度计的刻度系数和安装误差角。
用式（3.6.1）减去式（3.6.2），忽略δg 的影响，并略去二阶小量，得
n =-φn ⨯f n +C n ([δK ]+[δA ])f b δV b A
n n n n
+δV n ⨯(2ωie +ωen ) +V n ⨯(2δωie +δωen ) +∇n （3.6.3）
当取地理坐标系为导航坐标系时，
⎡0⎤⎡0⎤
⎥n ⎢-δL ωsin L ⎥n
ωie =⎢ωcos L δω=ie ⎢ie ⎥，ie ⎢⎥
⎢⎢⎣ωie sin L ⎥⎦⎣δL ωie cos L ⎥⎦
⎡⎤V N
-⎢⎥R +h M ⎢⎥⎢V ⎥n
ωen =⎢E ⎥
R +h ⎢N ⎥ ⎢V E ⎥
tan L ⎥⎢
⎣R N +h ⎦
⎡δV N ⎤V N
-+δh ⎢⎥2R +h (R +h ) M M ⎢⎥⎢δV E ⎥V E n
δωen =⎢-δh ⎥ 2
R +h (R +h ) N ⎢N ⎥⎢δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢2⎥R N +h (R N +h ) ⎦⎣R N +h
记
⎡T 11T 12T 13⎤
⎥C b n =⎢T T T 212223⎢⎥
⎢⎣T 31T 32T 33⎥⎦
将上述式子代入式（3.6.3），可以得到速度误差方程：
⎤⎡0⎡δV E
⎢ ⎥⎢⎢δV N ⎥=⎢φU ⎢δV ⎥⎢-φ⎣U ⎦⎣N
-φU 0
φE
φN ⎤⎡f E ⎤⎡T 11T 12T 13⎤⎡δK Ax δA z
⎢f ⎥+⎢T T ⎥⨯⎢-δA δK -φE ⎥T z Ay ⎥⎢N ⎥⎢212223⎥⎢0⎥⎦⎢⎣T 31T 32T 33⎥⎦⎢⎣f U ⎥⎦⎢⎣δA y -δA x
-δA y ⎤⎡f x ⎤
⎥⎢⎥δA x ⎥⎢f y b ⎥
⎢b ⎥δK Az ⎥⎦⎣f z ⎦
b
⎡0
+⎢⎢δV U ⎢⎣-δV N
-δV U 0
δV E
⎡⎤V N
-⎢⎥R +h M ⎥⎡∇E ⎤δV N ⎤⎢
⎢⎥⎢⎥V E ⎥-δV E ⎥⨯⎢2ωie cos L +⎥+⎢∇N ⎥
R N +h ⎥
⎢⎥⎢0⎦⎣∇U ⎥⎦⎢⎥V E
tan L ⎥⎢2ωie sin L +
R N +h ⎣⎦
⎡0
+⎢⎢V U ⎢⎣-V N
-V U 0V E
⎡⎤δV N V N
-+δh ⎢⎥2R +h (R +h ) M M ⎥V N ⎤⎢
⎢⎥δV E V E ⎥-V E ⎥⨯⎢-2δL ωie sin L +-δh ⎥2
R +h (R +h ) N N ⎢⎥0⎥⎦⎢
δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢2δL ωie cos L +2⎥R +h R +h (R +h ) N N N ⎣⎦
式（3.6.4）
其中，
b b
∇E =T 11∇b x +T 12∇y +T 13∇z
b b ∇N =T 21∇b x +T 22∇y +T 23∇z
b b
∇U =T 31∇b x +T 32∇y +T 33∇z
位置误差方程为：
δV N V N ⎧
δL =-δh ⎪R M +h (R M +h ) 2⎪
δV E V E V E sec L ⎪ δλ=sec L +δL tan L sec L -δh ⎨R N +h R N +h (R N +h ) 2 （3.6.5） ⎪⎪δh =δV
U
⎪⎩
2．4．2 姿态误差方程
根据式（2），姿态四元数满足如下微分方程：
=1Q ⊗ωb Q nb
2
其中，姿态速率ωnb 视为零标量四元数。
如果求取的姿态速率不含任何误差，即
b b b ωnb =ωib -ωin
b
则无误差的理想姿态四元数由下式确定：
=1Q ⊗(ωb -ωb ) Q ib in （3.6.6）
2
b
b
ib 和对数学平台的指令角速度ωˆin 确定 但实际系统中，姿态速率由陀螺的输出角速度ω
b b b
ˆnb ib in ω=ω-ω
b
in 根据系统解算出的导航解确定，带有一定的误差。所以实际解算其中，指令角速度ω
的四元数由下式确定：
b b ˆ=1Q ˆ⊗(ω ib ˆin Q -ω) （3.6.7）
2
n '
设与Q 相对应的姿态阵为C b ，根据姿态阵与姿态四元数之间的等价关系，与
n 'n 'n C b =C n C b 相对应的四元数为
即
ˆ=δQ *⊗Q Q
ˆ*δQ =Q ⊗Q （3.6.8）
ˆ引起的误差四元数。 其中，δQ 为Q
对式（3.6.8）两边对时间求导，用式（3.6.6）和式（3.6.7）代入之，可以得到,
=Q ⊗Q ˆ*+Q ⊗Q ˆ*δQ
11b b b b ˆ* ib ˆin =Q ⊗(ωib -ωin ) ⊗Q *+Q ⊗(-ω+ω) ⊗Q 221 b b b ˆ*-1Q ⊗ωb ⊗Q ˆ*+1Q ⊗ωˆ* ib ˆ=-Q ⊗(ω-ωib ) ⊗Q ⊗Q in in 2221b b ˆ*-1Q ⊗ωb ⊗Q *⊗Q ⊗Q ˆ*+1Q ⊗Q ˆ*⊗Q ˆ⊗ωˆ* ib ˆin =-Q ⊗δω⊗Q *⊗Q ⊗Q ⊗Q in 222
其中
由四元数的变化关系有
n b
ib ib δω=Q ⊗δω⊗Q *
b b b
ib ib δω=ω-ωib
n b ωin =Q ⊗ωin ⊗Q *n b ˆ⊗ωˆ*ˆin ˆin ω=Q ⊗Q
所以
n n =-δωˆ*-ωn ⊗Q ⊗Q ˆ*+Q ⊗Q ˆ*⊗ω ib ˆin δQ ⊗Q ⊗Q in
1
21212
将式（3.6.8）代入上式，得
n n n n =-δω ib δQ ⊗δQ -ωin ⊗δQ +δQ ⊗(ωin +δωin )
（3.6.9）
121212
将δQ 写成三角形式
φφφ
δQ =cos +sin
2φ2
ˆ确定的导航坐标系n '相对于由Q 确定的导航坐标系n 的偏差，φ是由Q
其中，φ=
角矢量，即姿态误差角矢量。由于φ是小角，所以δQ 可写成
上式两边对t 求导后得
φ
δQ =1+ （3.6.10）
2
φ δQ = （3.6.11） 2
将式（3.6.10）和式（3.6.11）代入式（9.8.9），略去二阶小量后 其中
n b
ib δω=C b n ([δK G ]+[δG ])ωib +εn
n n n =-δω ib φ+δωin +φ⨯ωin
（3.6.12）
[δK G ]=diag ⎡⎣δK Gx δK Gy ⎡0
⎢
[δG ]=⎢-δG z
⎢δG y ⎣
δK Gz ⎤⎦
δG z
0-δG x
-δG y ⎤⎥δG x ⎥0⎥⎦
δK Gi ，δG i , (i =x , y , z ) , 分别为陀螺的刻度系数误差和安装角误差角。所以式（3.6.12）
可写成
=φ⨯ωn +δωn -C n ([δK ]+[δG ])ωb -εn φin in b G ib （3.6.13）
上式即为捷联惯导的姿态误差方程的矢量形式。 当取地理坐标系为导航坐标系时，式（3.6.13）可写成
⎤⎡0⎡φE
⎢ ⎥⎢⎢φN ⎥=⎢φU ⎥⎢-φ⎢φ
⎣U ⎦⎣N
-φU 0
φE
⎡⎤V N
-⎢⎥R +h M ⎥φN ⎤⎢
⎢⎥V ⎥-φE ⎥⎢ωie cos L +⎥
R +h N ⎢⎥0⎥⎦⎢⎥V E
tan L ⎥⎢ωie sin L +
R N +h ⎣⎦
⎡⎤δV N V N
-+δh ⎢⎥2R +h (R +h ) M M ⎢⎥
⎢⎥δV E V E +⎢-δL ωie sin L +-δh ⎥2
R +h (R +h ) N N ⎢⎥
⎢δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢δL ωie cos L +2⎥R +h R +h (R +h ) N N N ⎣⎦⎡T 11T 12T 13⎤⎡δK Gx
⎥⎢-δG -⎢T T T z ⎢212223⎥⎢
⎢⎣T 31T 32T 33⎥⎦⎢⎣δG y
（3.6.14）
式中
b b
εE =T 11εx +T 12εy +T 13εz b
δG z
δK Gy -δG x
b
⎤⎡εE ⎤-δG y ⎤⎡ωibx
⎥⎢b ⎥⎢⎥δG x ⎥⎢ωiby
⎥-⎢εN ⎥b ⎥⎢ωibz δK Gz ⎥⎣εU ⎥⎦⎦⎣⎦⎢
b b
εN =T 21εx +T 22εy +T 23εz b b b εU =T 31εx +T 32εy +T 33εz b
第二章 捷联惯导系统的初试对准
2.1引言
惯导系统是一种自主式导航系统。它不需要任何人为的外部信息，只要给定导航的初始条件（例如初始速度、位置等），便可根据系统中的惯性敏感元件测量的比力和角速率通过计算机实时地计算出各种导航参数。由于“平台”是测量比力的基准，因此“平台”的初始对准就非常重要。对于平台惯导系统，初试对准的任务就是要将平台调整在给定的导航坐标系的方向上。若采用游动方位系统，则需要将平台调水平---称为水平对准，并将平台的方位角调至某个方位角处---称为方位对准。对于捷联惯导系统，由于捷联矩阵T 起到了平台的作用，因此导航工作一开始就需要获得捷联矩阵T 的初始值，以便完成导航的任务。显然捷联惯导系统的初始对准就是确定捷联矩阵的初始值。在静基座条件下，捷联惯导系统的加速度计的输入量为---g ，陀螺的输入量为地球自转角速率ωie 。因此g b 与b ωie 就成为初始对准的基准。将陀螺与加速度计的输入引出计算机，通过计算机b b 就可以计算出捷联矩阵T 的初始值。
由以上的分析可以看出，陀螺与加速度计的误差会导致对准误差；对准飞行器的干扰运动也是产生对准误差的重要因素。因此滤波技术对捷联系统尤其重要。由于初始对准的误差将会对捷联惯导系统的工作造成难以消除的影响，因此研究初始对准的误差传播方程也是非常必要的。
2.2 捷联惯导系统的基本工作原理
捷联式惯性导航系统，陀螺仪和加速度计直接与载体固联，加速度计测量是载体坐标系轴向比力，只要把这个比力转换到导航坐标系上，则其它计算就与平台式惯性导航系统一样，而比力转换的关键就是要实时地进行姿态基准计算来提供数学平台，即实时更新姿态矩阵C b n ，姿态矩阵也称为捷联矩阵。一般选择地
t 理坐标系为导航坐标系，那么捷联矩阵C b n 也可表示为C b ， 其导航原理图如图
2.1所示。
由惯导系统的工作原理可以看出，捷联式惯性导航系统有以下几个主要优点:
1. 惯性敏感器便于安装、维修和更换。 2.惯性敏感器可以直接给出舰船坐标系轴向的线加速度、线速度，供给舰船稳定控制系统和武备控制系统。角速度以提供给舰船稳定控制系统和武 备控制系统。 3.便于将惯性敏感器重复布置，从而易在惯性敏感器的级别上实现冗余 技术，这对提高系统的性能和可靠性十分有利。 4.由于去掉了具有常平架的平台，一则消除了稳定平台稳定过程中的各 种误差; 二则由于不存在机电结合的常平架装置，使整个系统可以做得小而 轻，并易于维护。 当然，由于惯性敏感器直接固接于船体上也带来新的问题，即导致惯性 敏感器的工作环境恶化了。由于惯性敏感器直接承受舰船的振动、冲击及温 度波动等环境条件，惯性敏感器的输出信息将会产生严重的动态误差。为保 证惯性敏感器的参数和性能有很高的稳定性，则要求在系统中必须对惯性敏 感器采取误差补偿措施。
2.3 常用坐标系与捷联矩阵
2.3.1常用坐标系
惯性导航中常用的坐标系有以下几种:
1. 地心惯性坐标系（下标为i ）——O e x i y i z i
惯性坐标系是符合牛顿力学定律的坐标系，即是绝对静止或只做匀速直线运动的坐标系。由于宇宙空间中的万物都处于运动之中，因此想寻找绝对的惯性坐标系是不可能的，我们只能根据导航的需要来选取惯性坐标系。对于在地球附近运动的飞行器选取地心惯性坐标系是合适的。地心惯性坐标系不考虑地球绕太阳的公转运动，当然更略去了太阳相对于宇宙空间的运动。地心惯性坐标系的原点O e 选在地球的中心，它不参与地球的自转。惯性坐标系是惯性敏感元件测量的
基准。由于在进行导航计算时无需再这个坐标系中分解任何向量，因此惯性坐标系的坐标轴的定向本无关紧要，但习惯上我们可以将z i 轴选在沿地轴指向北极的方向上，而x i 、y i 轴则在地球的赤道平面内，并指向空间的两颗恒星。
2. 地球坐标系（下标为e ）——O e x e y e z e
地球坐标系是固连在在地球上的坐标系，它相对于惯性坐标系以地球自转角
O e z e 轴与O e z i ωe =15.04107 /h 。速率ωe 旋转，地球坐标系的原点在地球中心O e ，
O e x e y e 在赤道平面内，x e 轴指向格林威治经线，y e 轴指向东经90方向。 轴重合，
3. 地理坐标系（下标为t ）——Ox t y t z t
地理坐标系是在飞行器上用来表示飞行器所在位置的东向、北向和垂线方向的坐标系。地理坐标系的原点O 选在飞行器的重心处，x t 指向东，y t 指向北，z t 沿垂线方向指向天（东北天）。对于地理坐标系，在不同的惯导文献中往往有不同的取法。所不同之处仅在于坐标轴的正向的指向不同，如还有北西天、北东地等取法。坐标轴指向不同于仅使向量在坐标系中取投影分量时的正负号有所不同，并不影响导航基本原理的阐述及导航参数计算结果的正确性。
4. 导航坐标系（下标为n ）——Ox n y n z n
导航坐标系是在导航时根据导航系统工作的需要而选取的作为导航基准的坐标系。当把导航坐标系选得与地理坐标系相重合时，可将这种导航坐标系称为指北方位系统；为了适应在极区附近导航的需要往往将导航坐标系的z n 轴仍选得与z t 重合，而使x n 与x t 及y n 与y t 之间相差一个自由方位角或游动方位角α，这种导航坐标系可称为自由方位系统或游动自由方位系统。
5. 平台坐标系（下标为p ）——Ox p y p z p
平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系。平台坐标系的坐标原点O 位于飞行器的重心处。当惯导系统不存在误差时，平台坐标系与导
航坐标系相重合；当惯导系统出现误差时，平台坐标系就要相对导航坐标系出现误差角。对于平台惯导系统，平台坐标系是通过平台台体来实现的；对于捷联惯导系统，平台坐标系则是通过存储在计算机中的方向余弦矩阵来实现的，因此又叫做“数学平台”。对于平台惯导系统，平台坐标系与导航坐标系之间的误差是由平台的加工、装配工艺不完善、敏感元件误差以及初始对准误差等因素造成的；而对于捷联惯导系统，该误差则是由算法误差、敏感元件误差以及初始对准误差造成的。
6. 机体坐标系（下标为b ）——Ox b y b z b
机体坐标系是固连在机体上的坐标系。机体坐标系的坐标原点O 位于飞行器的重心处，x b 沿机体横轴指向右，y b 沿机体纵轴指向前，z b 垂直于Ox b y b ，并沿飞行器的数轴指向上。
2.3.2捷联矩阵的定义
对于捷联惯导系统，加速度计时沿机体坐标系Ox b y b z b 安装的，它只能测量
b b b b p 沿机体坐标系的比力分量f x ，f y b ，f z ，因此需要将f x ，f y b ，f z 转换为f x ，f y p ，
p f z p 。实现由机体坐标系到平台坐标系的坐标转换的方向余弦矩阵C b 又叫做捷联
矩阵，本章用T 来表示；由于根据捷联矩阵的元素可以单值地确定飞行器的姿态角，因此又可以叫做飞行器状态矩阵；由于捷联矩阵起到了平台的作用（借助于
p p 它可以获得f x ，f y p ，f z ），所以又可以叫做“数学平台”。
设机体坐标系Ox b y b z b 固连在机体上，其Ox b 、Oy b 、Oz b 轴分别沿飞机的横轴、纵轴与竖轴，实现由机体坐标系到平台坐标系的坐标转换的捷联矩阵T 应满足如下的矩阵方程
⎡x p ⎤ ⎡x b ⎤ （3-1）⎢⎥⎢⎥⎢y p ⎥=T ⎢y b ⎥⎢z ⎥⎢⎣z b ⎥⎦⎣p ⎦
式中
⎡T 11T 12T 13⎤ （3-2） ⎥T =⎢T T T 23⎥⎢2122
⎢⎣T 31T 32T 33⎥⎦
当矩阵T 求得后，沿机体坐标系测量的比力f 就可以转换到平台坐标系上，得到f 。
由平台坐标系至机体坐标系的转换关系，可以通过下述顺序的三种旋转来表示： p b
''''''x p y p z p −−−→x '→x ''→x b y b z b p y p z p −−−p y p z p −−−ψθγ
其中ψ、θ、γ分别为机体的航向角、俯仰角和倾斜角。
根据上述的旋转顺序，可以得到由平台坐标系到机体坐标系的转换关系，即 ⎡x b ⎤⎡cos γ⎢y ⎥=⎢0⎢b ⎥⎢⎢⎣sin γ⎣z b ⎥⎦⎢0-sin γ⎤⎡10⎢0cos θ10⎥⎥⎢0cos γ⎥⎦⎢⎣0-sin θ0⎤⎡cos ψ⎢-sin ψsin θ⎥⎥⎢cos θ⎥⎦⎢⎣0sin ψcos ψ00⎤⎡x p ⎤⎢⎥0⎥⎥⎢y p ⎥=
⎢⎥1⎥⎦⎣z p ⎦绕z p 轴绕x 'p 轴绕y ''p 轴
⎡cos γcos ψ-sin γsin θsin ψ⎢-cos θsin ψ⎢⎢⎣sin γcos ψ+cos γsin θsin ψcos γsin ψ+sin γsin θcos ψcos θcos ψsin γsin ψ-cos γsin θcos ψ-sin γcos θ⎤⎡x p ⎤⎥⎢y ⎥si n θ⎥⎢p ⎥⎢⎥cos γcos θ⎥⎦⎣z p ⎦（3-3）
由式（3-1）可得
⎡x b ⎤⎡x p ⎤ ⎢y ⎥=T -1⎢y ⎥⎢p ⎥⎢b ⎥⎢z ⎥⎢⎣z b ⎥⎦⎣p ⎦
（3-4）
考虑到捷联矩阵T 为正交矩阵，T -1=T t （t 表示转置），于是
⎡cos γcos ψ-sin γsin θsin ψ
T =⎢⎢cos γsin ψ+sin γsin θcos ψ
⎢-sin γcos θ⎣-cos θsin ψcos θcos ψsin θsin γcos ψ+cos γsin θsin ψ⎤sin γsin ψ-cos γsin θcos ψ⎥⎥
⎥cos γcos θ⎦
(3-5)
2.3 姿态的描述及更新算法
物体（刚体）的姿态是物体相对观察者的几何角度关系的统称。姿态的数学描述是运动体进行姿态运动建模的基础。一般来讲，刚体的运动要用刚体位置移动和绕自身重心的转动来描述。其位置移动可用船体上下起伏（heave ）、前后涌动（surge ）、左右晃动（sway ）来描述，而转动则要用纵摇（pitch ）、横摇（roll ）、和偏航（yaw ）来描述。纵摇是用来描述船在穿过连续的浪头和波谷时，船头的船尾忽上忽下而引起的船体绕横摇轴转动的振荡运动；横摇则是用来描述船体绕纵轴转动的振荡运动。因此，舰船和飞机一类的运载体的姿态参数就是其纵摇角、横摇角和航向角。
为了描述载体的姿态，至少需要建立两个坐标系，即空间参考坐标系（Reference Coordinate）和固连于载体的本体坐标系（Body Reference）。载体坐标系的三个坐标轴在参考坐标系中的方向确定了载体的姿态，描述这些方向的物理量就称为姿态参数。姿态参数有多种形式，最常用的如欧拉角、四元数等。采用不同的姿态参数，可以构成不同的坐标转换矩阵，称为姿态矩阵。因为载体姿态是唯一确定的，因此各种姿态参数之间可以互相转换。
2.3.1欧拉角
欧拉角（Euler Angles ）发源于欧拉定理，即刚体绕固定点的角位移可以由绕该点的若干次有限转动合成。因此，可将参考坐标系绕不同坐标轴连续旋转三次得到载体坐标系，每次的旋转轴取为被旋转坐标系的某一轴，每次的旋转角就称为欧拉角。姿态矩阵与三次转动的顺序有关，但描述三轴控制的飞机、导弹、舰船等方面，一般采用非对称旋转。
由坐标系的定义三个欧拉角的几何意义为：
纵摇角θ（φp ）：载体绕纵摇轴X b 转动的角度；
横摇角γ（φγ）：载体绕横摇轴Y b 转动的角度；
航向角ψ（φψ）：载体绕航向轴Z b 转动的角度。
根据转动规律得到姿态矩阵：
⎡cos φγcos φψ+sin φγsin φp sin φψ⎢C b n =⎢-cos φp sin φψ
⎢sin φγcos φψ+cos φγsin φp sin φψ⎣cos φγsin φψ-sin φγsin φp cos φψcos φp cos φψsin φγsin φψ-cos φγsin φp cos φψ-sin φγcos φp ⎤⎥sin φp ⎥cos φγcos φp ⎥⎦
=⎡T 11T 12T 13⎤ (5-14) ⎢T T ⎥T 23⎥⎢2122
⎢⎣T 31T 32T 33⎥⎦
由(5-14)式可得：
θ=sin -1(T 23) ， γ=tg -1(-T 13T ) ，ψ=tg -1(-21) (5-15) T 33T 22
00纵摇角定义在θ∈[-90,90]区间，和反正弦函数主值一样，不存在多值问题。
0000横摇角定义在γ∈[-180,180]区间，航向角定义在ψ∈[0,360]区间，都存在多
值问题，求姿态角时应设法判断γ和ψ的真值，以确定载体落在哪一个象限。
欧拉角是经典的、应用最为广泛的一种姿态参数，具有直接、明显的几何意义，维数最小（三维），并且往往可以由姿态敏感器直接测量。在某些应用方面，欧拉角描述法具有明显的优势。但是，由于方向余弦矩阵具有九个元素，所以，解算矩阵微分方程时，实际上解算九个联立微分方程，一般说来，计算量比较大。此外，它还有一个固有缺陷，即对于某一个角度存在奇异问题。不同的旋转顺序，奇异角度也不同。因此，要准确描述任意姿态，至少需要两组欧拉角。为了解决这些问题，可以采用四元数法。下面介绍四元数法。
2.3.2四元数
（1） 四元数定义及其姿态矩阵
设坐标系S a (Ox a y a z a ) 绕某轴ON 转动某个角度φ，就与坐标系S b (Ox b y b z b ) 重合，如图5.2所示。
图5.2 四元数示意图
定义如下四个元素： x a ⎧q 0=cos(φ/2) (5-16) ⎪q =C sin(φ/2) ⎪11⎨⎪q 2=C 2sin(φ/2)
⎪⎩q 3=C 3sin(φ/2)
其中：C 1, C 2, C 3分别为旋转欧拉轴ON 在坐标系S a (Ox a y a z a ) 中的方向余弦。 显然，用q 0-q 3这四个元素就可以完全描述这两个坐标系之间的关系，则定义四元数为：
(5-17)
q =q 0+q 1i +q 2j +q 3k =q 0+q
四元数满足以下的约束条件：
222q 0+q 12+q 2+q 3=1 (5-18)
所以，四元数四个变量之中只有三个是独立的。
根据欧拉旋转公式(5-14)及四元数定义，四元数姿态矩阵可表示为：
T +(q 0I -[q ⨯])2]R 0 R b =[qq
2 (5-19) ⎡2(q 0+q 12) -12(q 1q 2+q 0q 3) 2(q 1q 3-q 0q 2) ⎤⎢⎥22=⎢2(q 1q 2-q 0q 3) 2(q 0+q 2) -12(q 2q 3+q 0q 1) ⎥R 0=T b 0(q ) R 0
22⎢2(q 1q 3+q 0q 2) 2(q 2q 3-q 0q 1) 2(q 0+q 3) -1⎥⎣⎦
T b 0() 为导航坐标系与载体坐标系之间的坐标转换矩阵。
注意到四元数姿态矩阵是由四元数q 的二项式组成，因此对于四元数-q =[-q 0 T ]T ，也可以得到(5-19)所示的姿态矩阵。四元数的这种非唯一性-q
是与欧拉角的非唯一性相一致的。物理意义上这两者实际代表同一种姿态、同一种旋转，并无区别。一般我们取四元数正值进行计算。
上述分析说明，如果表征n 系到b 系的旋转四元数Q 已经确定，那么就可以确定出运载体的航向角、俯仰角和横滚角，因此，四元数Q 包含了所有的姿态信息，捷联惯导中的姿态更新实质上是如何计算四元数Q 。
2.3.3四元数与欧拉角的关系
四元数和欧拉角的关系可由四元数的乘积公式求出。欧拉角载体坐标系按三次旋转到导航坐标系，则每次旋转所对应的四元数分别为：
q 1(ψ) =cos ψ
2-k sin ψ
2
q 2(θ) =cos
q 3(γ) =cos θ2+i sin θ (5-20) 2γ
2+j sin γ
2
根据四元数的定义及乘法公式，可得：
q b 0=q 1⊗q 2⊗q 3 (5-21)
因此可解出：
ψθγψθγ (5-22) ⎧q =cos cos cos +sin sin sin ⎪0222222⎪⎪q =cos ψsin θcos γ+sin ψcos θsin γ
⎪1222222⎨⎪q =cos ψcos θsin γ-sin ψsin θcos γ
⎪2222222⎪ψθγψθγ⎪q 3=cos sin sin -sin cos cos ⎪⎩222222
或 ⎧ (5-23) ⎪-1θ=sin [2(q 3q 2+q 1q 0)]⎪⎪⎪-12(q 1q 3-q 2q 0) ]⎨γ=-tg [222(q 0+q 3) -1⎪⎪-12(q q -q q ) ⎪ψ=-tg [1
22230]2(q 0+q 2) -1⎪⎩
与欧拉角相比，四元数避免了奇异问题，没有复杂的三角运算，更有利于存储及计算。但由于计算误差等，较难保证四元数的规范化条件，因此往往需要重新规范化或降阶。
2.3.4 姿态更新的四元数算法
设由运载体的机体轴确定的坐标系为b ，惯导系统所采用的导航坐标系为n ，则由b 系到n 系的坐标变换矩阵C b 称为运载体的姿态矩阵。姿态更新是指根据惯性器件的输出实时计算出C b 矩阵。由于n 系和b 系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可对一个坐标系作平移，使其原点与另一个坐标系的原点重合。因此，
n n
两坐标系间的空间角位置可理解成刚体的定点转动。从这一基本思想出发，可获得姿态更新的四元数算法及旋转矢量算法。
一、四元数微分方程
假设表征n 系至b 系的旋转四元数为
对（1）式求导可得
Q =cos +u R sin （1）
22
θθ
d Q 1R
=ωRb ⊗Q （2） dt 2
R u R 。 刚体绕u 旋转θ，ωRb =θ
在捷联惯导系统中，角速度信息是捷联陀螺提供的，而捷联陀螺是在机体坐标系内测量的，所以（2）式中的ωRb 还需换成ωRb 。根据四元数向量之间的变换关系（r R =Q ⊗r b ⊗Q *）可得，
R b ωRb =Q ⊗ωRb ⊗Q * （3）
R
b
将（3）式代入（2）式可得
d Q 11b b
=Q ⊗ωRb ⊗Q *⊗Q =Q ⊗ωRb （4） dt 22
记
b
ωRb
⎡ωx ⎤ （5）
⎥=⎢ω⎢y ⎥⎢⎣ωz ⎥⎦
即得到
0⎤⎡0⎡q ⎢q ω 1⎥1⎢⎢⎥=⎢x ⎢q 2⎥2⎢ωy
⎢⎢⎥ q ⎢⎣3⎦⎣ωz
b
-ωx 0-ωz
-ωy
ωz
0-ωx
ωy
-ωz ⎤⎡q 0⎤ （6）
⎢q ⎥-ωy ⎥⎥⎢1⎥ωx ⎥⎢q 2⎥⎥⎢⎥0⎥⎦⎣q 3⎦
其中, ωnb 的获取按照下式进行
b b b n n
ωnb =ωib -C n (ωie +ωen ) （7）
b b n
是捷联陀螺的输出；C n 由姿态更新的最新值确定；ωen 和ωie 分别是未知式中，ωib
n
速率和地球自转速率，对于导航坐标系取地理坐标系的情况有
⎡V N ⎤
-⎢⎥R ⎢M ⎥⎢⎥V n n
ωie +ωen =⎢ωie cos L +E ⎥ （8）
R N ⎢⎥
⎢⎥V E
tan L ⎥⎢ωie sin L +R N ⎣⎦
式中，V N 、V E 、L 为导航计算所得的最新值, 分别是北向速度、东向速度、纬度；R N
为地球沿卯酉圈的曲率半径，R M 为地球子午圈的曲率半径，在不精确计算的情况下，也可以视地球为圆球形状，则（8）式中R N 、R M 可用地球半径R e 代替。
二、四元数的初值确定和规范化处理
四元数的初值Q (0)由捷联惯导的初始对准确定。设初始对准确定的姿态阵
n
为C b =[T ij ]，根据式（5-19）及描述刚体旋转的四元数为规范化四元数的结论，有如下方
程成立
222⎧q 0+q 12-q 2-q 3=T 11⎪2
222
⎪q 0-q 1+q 2-q 3=T 22⎪q 2-q 2-q 2+q 2=T
2333⎪01
222⎪q 0+q 12+q 2+q 3=1⎪
⎪2(q 1q 2-q 0q 3) =T 12⎨ （9） ⎪2(q 1q 3+q 0q 2) =T 13⎪2(q q +q q ) =T
0321⎪12
⎪2(q 2q 3-q 0q 1) =T 23⎪
⎪2(q 1q 3-q 0q 2) =T 31⎪2(q q +q q ) =T
32⎩2310
从上述方程解得
⎧
q =⎪0⎪⎪4q q =T -T
3223⎨10
（10）
⎪4q 2q 0=T 13-T 31⎪⎪⎩4q 3q 0=T 21-T 12
q 0的符号可以任选。
由于表征旋转的四元数是规范化四元数，即Q =1，但是由于计算误差等因素，计算过程中四元数会逐渐失去规范化特性，因此若干次更新后，必须对四元数做规范化处理：
q i =
ˆi =0,1, 2,3
ˆ0、q ˆ1、q ˆ2、q ˆ3是四元数更新所得的值。 其中，q
2.4捷联惯导系统的误差模型
对于工作在非极区的捷联惯导系统，为了简化计算，导航坐标系一般选取地理坐标系，这样，捷联惯导完全等效于指北方位系统。由于陀螺在捷联系统中起测量器件作用，陀螺漂移引起的数学漂移与陀螺漂移的方向相反，刻度系数误差引起对运载体角速度的测量误差，
经姿态更新计算引入系统。
2．4．1 速度误差方程和位置误差方程
根据比力方程，当不考虑任何误差时，速度的理想值由下式确定：
=C n f b -(2ωn +ωn ) ⨯V +g n （3.6.1） V n b ie en n
而实际系统中总存在各种误差，所以实际的速度计算值应由下述方程确定：
ˆn f b -2(ωc +ωc ) ⨯V c +g c （3.6.2） =C V c b ie en
式中
V c =V n +δV n
c n n
ωie =ωie +δωie c n n ωen =ωen +δωen
g c =g n +δg
ˆn =C n 'C n =(I -φn ⨯) C n C
b
n
b
b
b =(I +[δK ])(I +[δA ])f b +∇b f A -φU φN ⎤⎡0
⎥φn ⨯=⎢φ0-φU E ⎢⎥
⎢0⎥⎣-φN φE ⎦[δK A ]=diag [δK Ax δK Ay
δK Az ]
⎡0⎢
[δA ]=⎢-δA z
⎢δA y ⎣
δA z
0-δA x
-δA y ⎤
⎥δA x ⎥0⎥⎦
其中，φE 、φN 、φU 为姿态误差角，δK Ai 和δA zi (i =x , y , z ) 分别为加速度计的刻度系数和安装误差角。
用式（3.6.1）减去式（3.6.2），忽略δg 的影响，并略去二阶小量，得
n =-φn ⨯f n +C n ([δK ]+[δA ])f b δV b A
n n n n
+δV n ⨯(2ωie +ωen ) +V n ⨯(2δωie +δωen ) +∇n （3.6.3）
当取地理坐标系为导航坐标系时，
⎡0⎤⎡0⎤
⎥n ⎢-δL ωsin L ⎥n
ωie =⎢ωcos L δω=ie ⎢ie ⎥，ie ⎢⎥
⎢⎢⎣ωie sin L ⎥⎦⎣δL ωie cos L ⎥⎦
⎡⎤V N
-⎢⎥R +h M ⎢⎥⎢V ⎥n
ωen =⎢E ⎥
R +h ⎢N ⎥ ⎢V E ⎥
tan L ⎥⎢
⎣R N +h ⎦
⎡δV N ⎤V N
-+δh ⎢⎥2R +h (R +h ) M M ⎢⎥⎢δV E ⎥V E n
δωen =⎢-δh ⎥ 2
R +h (R +h ) N ⎢N ⎥⎢δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢2⎥R N +h (R N +h ) ⎦⎣R N +h
记
⎡T 11T 12T 13⎤
⎥C b n =⎢T T T 212223⎢⎥
⎢⎣T 31T 32T 33⎥⎦
将上述式子代入式（3.6.3），可以得到速度误差方程：
⎤⎡0⎡δV E
⎢ ⎥⎢⎢δV N ⎥=⎢φU ⎢δV ⎥⎢-φ⎣U ⎦⎣N
-φU 0
φE
φN ⎤⎡f E ⎤⎡T 11T 12T 13⎤⎡δK Ax δA z
⎢f ⎥+⎢T T ⎥⨯⎢-δA δK -φE ⎥T z Ay ⎥⎢N ⎥⎢212223⎥⎢0⎥⎦⎢⎣T 31T 32T 33⎥⎦⎢⎣f U ⎥⎦⎢⎣δA y -δA x
-δA y ⎤⎡f x ⎤
⎥⎢⎥δA x ⎥⎢f y b ⎥
⎢b ⎥δK Az ⎥⎦⎣f z ⎦
b
⎡0
+⎢⎢δV U ⎢⎣-δV N
-δV U 0
δV E
⎡⎤V N
-⎢⎥R +h M ⎥⎡∇E ⎤δV N ⎤⎢
⎢⎥⎢⎥V E ⎥-δV E ⎥⨯⎢2ωie cos L +⎥+⎢∇N ⎥
R N +h ⎥
⎢⎥⎢0⎦⎣∇U ⎥⎦⎢⎥V E
tan L ⎥⎢2ωie sin L +
R N +h ⎣⎦
⎡0
+⎢⎢V U ⎢⎣-V N
-V U 0V E
⎡⎤δV N V N
-+δh ⎢⎥2R +h (R +h ) M M ⎥V N ⎤⎢
⎢⎥δV E V E ⎥-V E ⎥⨯⎢-2δL ωie sin L +-δh ⎥2
R +h (R +h ) N N ⎢⎥0⎥⎦⎢
δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢2δL ωie cos L +2⎥R +h R +h (R +h ) N N N ⎣⎦
式（3.6.4）
其中，
b b
∇E =T 11∇b x +T 12∇y +T 13∇z
b b ∇N =T 21∇b x +T 22∇y +T 23∇z
b b
∇U =T 31∇b x +T 32∇y +T 33∇z
位置误差方程为：
δV N V N ⎧
δL =-δh ⎪R M +h (R M +h ) 2⎪
δV E V E V E sec L ⎪ δλ=sec L +δL tan L sec L -δh ⎨R N +h R N +h (R N +h ) 2 （3.6.5） ⎪⎪δh =δV
U
⎪⎩
2．4．2 姿态误差方程
根据式（2），姿态四元数满足如下微分方程：
=1Q ⊗ωb Q nb
2
其中，姿态速率ωnb 视为零标量四元数。
如果求取的姿态速率不含任何误差，即
b b b ωnb =ωib -ωin
b
则无误差的理想姿态四元数由下式确定：
=1Q ⊗(ωb -ωb ) Q ib in （3.6.6）
2
b
b
ib 和对数学平台的指令角速度ωˆin 确定 但实际系统中，姿态速率由陀螺的输出角速度ω
b b b
ˆnb ib in ω=ω-ω
b
in 根据系统解算出的导航解确定，带有一定的误差。所以实际解算其中，指令角速度ω
的四元数由下式确定：
b b ˆ=1Q ˆ⊗(ω ib ˆin Q -ω) （3.6.7）
2
n '
设与Q 相对应的姿态阵为C b ，根据姿态阵与姿态四元数之间的等价关系，与
n 'n 'n C b =C n C b 相对应的四元数为
即
ˆ=δQ *⊗Q Q
ˆ*δQ =Q ⊗Q （3.6.8）
ˆ引起的误差四元数。 其中，δQ 为Q
对式（3.6.8）两边对时间求导，用式（3.6.6）和式（3.6.7）代入之，可以得到,
=Q ⊗Q ˆ*+Q ⊗Q ˆ*δQ
11b b b b ˆ* ib ˆin =Q ⊗(ωib -ωin ) ⊗Q *+Q ⊗(-ω+ω) ⊗Q 221 b b b ˆ*-1Q ⊗ωb ⊗Q ˆ*+1Q ⊗ωˆ* ib ˆ=-Q ⊗(ω-ωib ) ⊗Q ⊗Q in in 2221b b ˆ*-1Q ⊗ωb ⊗Q *⊗Q ⊗Q ˆ*+1Q ⊗Q ˆ*⊗Q ˆ⊗ωˆ* ib ˆin =-Q ⊗δω⊗Q *⊗Q ⊗Q ⊗Q in 222
其中
由四元数的变化关系有
n b
ib ib δω=Q ⊗δω⊗Q *
b b b
ib ib δω=ω-ωib
n b ωin =Q ⊗ωin ⊗Q *n b ˆ⊗ωˆ*ˆin ˆin ω=Q ⊗Q
所以
n n =-δωˆ*-ωn ⊗Q ⊗Q ˆ*+Q ⊗Q ˆ*⊗ω ib ˆin δQ ⊗Q ⊗Q in
1
21212
将式（3.6.8）代入上式，得
n n n n =-δω ib δQ ⊗δQ -ωin ⊗δQ +δQ ⊗(ωin +δωin )
（3.6.9）
121212
将δQ 写成三角形式
φφφ
δQ =cos +sin
2φ2
ˆ确定的导航坐标系n '相对于由Q 确定的导航坐标系n 的偏差，φ是由Q
其中，φ=
角矢量，即姿态误差角矢量。由于φ是小角，所以δQ 可写成
上式两边对t 求导后得
φ
δQ =1+ （3.6.10）
2
φ δQ = （3.6.11） 2
将式（3.6.10）和式（3.6.11）代入式（9.8.9），略去二阶小量后 其中
n b
ib δω=C b n ([δK G ]+[δG ])ωib +εn
n n n =-δω ib φ+δωin +φ⨯ωin
（3.6.12）
[δK G ]=diag ⎡⎣δK Gx δK Gy ⎡0
⎢
[δG ]=⎢-δG z
⎢δG y ⎣
δK Gz ⎤⎦
δG z
0-δG x
-δG y ⎤⎥δG x ⎥0⎥⎦
δK Gi ，δG i , (i =x , y , z ) , 分别为陀螺的刻度系数误差和安装角误差角。所以式（3.6.12）
可写成
=φ⨯ωn +δωn -C n ([δK ]+[δG ])ωb -εn φin in b G ib （3.6.13）
上式即为捷联惯导的姿态误差方程的矢量形式。 当取地理坐标系为导航坐标系时，式（3.6.13）可写成
⎤⎡0⎡φE
⎢ ⎥⎢⎢φN ⎥=⎢φU ⎥⎢-φ⎢φ
⎣U ⎦⎣N
-φU 0
φE
⎡⎤V N
-⎢⎥R +h M ⎥φN ⎤⎢
⎢⎥V ⎥-φE ⎥⎢ωie cos L +⎥
R +h N ⎢⎥0⎥⎦⎢⎥V E
tan L ⎥⎢ωie sin L +
R N +h ⎣⎦
⎡⎤δV N V N
-+δh ⎢⎥2R +h (R +h ) M M ⎢⎥
⎢⎥δV E V E +⎢-δL ωie sin L +-δh ⎥2
R +h (R +h ) N N ⎢⎥
⎢δV E V E V E tan L ⎥2
tan L +δL sec L -δh ⎢δL ωie cos L +2⎥R +h R +h (R +h ) N N N ⎣⎦⎡T 11T 12T 13⎤⎡δK Gx
⎥⎢-δG -⎢T T T z ⎢212223⎥⎢
⎢⎣T 31T 32T 33⎥⎦⎢⎣δG y
（3.6.14）
式中
b b
εE =T 11εx +T 12εy +T 13εz b
δG z
δK Gy -δG x
b
⎤⎡εE ⎤-δG y ⎤⎡ωibx
⎥⎢b ⎥⎢⎥δG x ⎥⎢ωiby
⎥-⎢εN ⎥b ⎥⎢ωibz δK Gz ⎥⎣εU ⎥⎦⎦⎣⎦⎢
b b
εN =T 21εx +T 22εy +T 23εz b b b εU =T 31εx +T 32εy +T 33εz b