CN102721417B - 一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法 - Google Patents

Abstract

本发明涉及一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，属于惯性导航技术领域。根据载体所在的经度、纬度，地球坐标系到导航坐标系的转换；使旋转式惯导系统的双轴按一定旋转方案周期性地旋转；利用粗对准过程中陀螺输出的角速度信息，通过求矩阵微分方程数值解，更新IMU坐标系相对于p0坐标系姿态变化的方向余弦矩阵；并依次求解矩阵从而得到载体姿态矩阵

Description

一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法

技术领域

本发明涉及一种捷联惯导系统在震荡、摇摆基座条件下凝固惯性系粗对准方法的误差抑制方法，属于惯性导航技术领域。

背景技术

捷联惯导系统的初始对准技术就是在较短的时间内以一定的精度确定载体的初始位置、初始速度以及从载体坐标系到导航坐标系的初始姿态角和方向余弦矩阵或初始四元数。对于未进行线运动的载体，其位置可通过载体外部信息(如GNSS定位数据)获取，直接装订入导航计算机，而载体没有线运动时地速为零，也可直接装订入导航计算机，因而初始姿态角的获取成为初始对准的主要任务。通常，把捷联惯性导航系统的对准分为两个阶段：粗对准阶段和精对准阶段。在粗对准阶段，依靠重力矢量和地球速率矢量的测量值，直接估算出载体坐标系到导航参考坐标系的变换矩阵，它是一个粗略值，与真实值存在偏差，但应为小量。而精对准解算可在粗对准基础上采用最小二乘法或卡尔曼滤波法完成，因此粗对准精度对于精对准阶段的速度和精度有影响。

现在捷联式惯导系统中常用的粗对准方法为解析式粗对准法。其中解析式粗对准法在稳定的静基座条件下能够满足粗对准的要求，但是对于舰船在风浪作用下摇摆、震荡运动的对准环境，由于摇摆引起的干扰角速度远大于地球自转角速度，信噪比降低，而且干扰角速度频带很宽，无法用滤波器从陀螺输出中提取地球自转角速度。这时陀螺测得的角速度已不是地球自转角速度向量ω_ie；如果载体有垂荡、纵荡、横荡或随机干扰比较大的时候，加速度计测量的也不是重力加速度g，因此粗对准效果不理想。根据地球自转角速度ω_ie和粗对准期间所经历的时间，结合重力加速度g在该段时间内在惯性空间中方向的变化可以推算出地球的北向信息，这就是应用凝固惯性系粗对准方案的基本原理。

在普通捷联式惯导系统中，采用凝固惯性系的粗对准方法将求解载体的姿态矩阵过程转化为求解一组方向余弦矩阵的乘积，其中为从地球坐标系e到导航坐标系n的变换矩阵，可由舰船所在位置的经度λ和纬度L确定；为从地心惯性坐标系i到地球坐标系e之间的方向余弦矩阵，可由地球自转角速度和经历的时间确定；为从当前载体坐标系b到初始时刻载体姿态惯性坐标系b0的方向余弦矩阵，可以利用陀螺输出的角速度信息通过四元数算法或等效旋转矢量算法进行姿态更新获得，其中初始时刻载体姿态惯性坐标系b0的定义来源于惯性系凝固假设，即与载体坐标系b在粗对准的起始时刻完全重合的惯性坐标系。

是地心惯性坐标系i和初始时刻载体姿态惯性坐标系b0之间的方向余弦矩阵，求得粗对准过程中两个时刻的加速度计的输出的积分值及其叉积，以及地心惯性坐标系下重力加速度的积分值及其叉积，可以组成两个矩阵通过矩阵相乘可以求得对加速度计输出的积分，可以使加速度计输出中由摇摆、震荡引起的分量被抵消，从而使粗对准获得更高的精度，这是凝固惯性系粗对准方法的优势。

加速度计输出中，还包含有加速度计的零偏误差，在没有正确标定补偿的情况下，加速度计零偏将随时间积累，因此凝固惯性系粗对准方法的精度将随时间积累而降低，而在很短的时间内完成凝固惯性系粗对准又难以保证加速度计输出中由摇摆、震荡引起的误差分量通过积分得到补偿，所以凝固惯性系粗对准方法效果存在局限性。

旋转式捷联惯导系统是将惯导系统惯性测量单元(IMU)基座安装在与载体固连的旋转平台上，使得IMU坐标系可以绕平台转轴(一般与IMU的惯性器件的敏感轴重合)相对于载体坐标系进行旋转角运动的捷联式惯导系统，通过旋转可以调制惯性器件的误差从而抑制导航误差积累。在旋转式捷联惯导系统中，加速度计的零偏误差可以通过旋转调制技术加以补偿，因此将凝固惯性系粗对准技术引入旋转式惯导系统可以有效改进其性能，本发明将给出旋转式惯导系统的凝固惯性系粗对准方案。

发明内容

本发明的目的在于解决凝固惯性系粗对准方法中加速度计输出的零偏误差分量随时间积累引起粗对准精度下降的技术问题，提供一种旋转式捷联惯导系统通过旋转惯性测量单元(IMU)调制加速度计零偏误差，从而提高凝固惯性系粗对准方法精度的方法。

一种惯性导航系统凝固惯性系粗对准误差抑制方法的具体技术方案如下：

步骤一、通过全球导航卫星系统(GNSS)或其他途径获得载体所在位置的经度λ、纬度L。

步骤二、根据步骤一得到的载体所在的经度、纬度，计算从地球坐标系e到导航坐标系n的方向余弦矩阵

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right]$

步骤三、使旋转式惯导系统的双轴按一定旋转方案周期性地旋转。

旋转方案选择下述方案之一：

a.内环轴、外环轴单向连续旋转；

b.内环轴、外环轴连续旋转，每旋转一周改变转向；

c.内环轴、外环轴单向交替旋转，每个轴旋转一周则停止同时开始旋转另一轴，如此循环往复；

d.内环轴、外环轴变向交替旋转，第一轴旋转一周后停止，然后由第二轴旋转一周，然后再由第一轴在反向旋转一周，然后再由第二轴反向旋转一周，如此循环往复；

e.内环轴、外环轴变向交替旋转，第一轴旋转一周后再反向旋转一周，然后停止，然后由第二轴旋转一周后再反向旋转一周，如此循环往复；

其中方案a、c只能在旋转惯导系统的旋转平台含有导电滑环的情况下使用，而且当IMU存在标度因数误差和安装误差的情况下因耦合产生新误差而不宜采用。

上述各方案中内环轴、外环轴分别以恒定角速率ω₁、ω₂旋转，ω₁和ω₂的范围为0.6°/s～60°/s。

步骤四、在步骤三确定旋转方案的同时，利用粗对准过程中陀螺输出的角速度信息，通过求矩阵微分方程数值解，更新IMU坐标系相对于p0坐标系姿态变化的方向余弦矩阵

其中，p0坐标系为与起始时刻t₀时的IMU坐标系重合的惯性坐标系，即凝固惯性系，粗对准起始时刻为单位矩阵I。为IMU坐标系相对于p0坐标系的旋转角速度的反对称矩阵形式。

${\mathrm{\ω}}_{p0,p}^p={\left[\begin{array}{ccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]}^T$

${\mathrm{\Ω}}_{p0,p}^p=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$

其中，ω_x、ω_y、ω_z分别为沿IMU坐标系x，y，z轴的分量。

矩阵微分方程数值解用毕卡逼近算法(适合输出为角增量的陀螺)或龙格—库塔算法(适合输出为角速度的陀螺)，或者直接利用惯导系统的姿态更新算法程序模块求解，常用的算法有四元数算法和等效旋转矢量算法。上述算法均为惯导领域的公知技术，不是本发明的主要特征，所以不做过多描述。

同时根据更新的积分求解(积分的数值解法可采用矩形公式或梯形公式)

式中t₀为步骤三或者步骤四的起始时刻。

步骤五、在完成步骤三确定的双轴旋转方案的一个完整旋转周期后，选取两个时刻t₁，t₂(其中t₂为一次粗对准结果求解的时刻)分别计算两个时刻的Vⁱ：

$V^i\left(t_1\right)=\left[\begin{array}{c}\frac{g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_1)-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[\cos \mathrm{\λ}-\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_1)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·\mathrm{\Δ}{t}_1\\ \end{array}\right]$ $V^i\left(t_2\right)=\left[\begin{array}{c}\frac{g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2)-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[\cos \mathrm{\λ}-\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·\mathrm{\Δ}{t}_2\\ \end{array}\right]$

式中Δt₁＝t₁-t₀，Δt₂＝t₂-t₀，为粗对准从起始时刻t₀到当前时刻所经历的时间，ω_ie为地球自转角速度。

步骤六、求解矩阵

$C_i^e=\left[\begin{array}{ccc}{\cos \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& {\sin \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& 0\\ -\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& \cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& 0\\ 0& 0& 1\end{array}\right]$

为从地心惯性坐标系i到地球坐标系e之间的变换矩阵；为地心惯性坐标系i到与起始时刻IMU坐标系重合的惯性坐标系p0(即凝固惯性系)的坐标变换矩阵。为从IMU坐标系p到载体坐标系b的坐标变换矩阵，简称旋转矩阵。

旋转矩阵可由双轴旋转惯导系统转轴的旋转角度传感器输出值计算获得，不同结构的IMU旋转系统对应不同的旋转矩阵，的值可根据t₂时刻内、外环转轴角位置传感器输出α_i，α_j(i，j＝x，y，z)求解，需要根据旋转式系统具体结构从以下公式中选择：

(1)z轴为内环轴，x轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& {\sin \mathrm{\α}}_z& 0\\ -{\sin \mathrm{\α}}_z\cos {\mathrm{\α}}_z& {\cos \mathrm{\α}}_z{\cos \mathrm{\α}}_x& {\sin \mathrm{\α}}_x\\ \sin {\mathrm{\α}}_z{\sin \mathrm{\α}}_x& -\cos {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& {\cos \mathrm{\α}}_x\end{array}\right]$

(2)x轴为内环轴，z轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x{\sin \mathrm{\α}}_z& \sin {\mathrm{\α}}_x{\sin \mathrm{\α}}_z\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x{\cos \mathrm{\α}}_z& {\sin \mathrm{\α}}_x\cos {\mathrm{\α}}_z\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]$

(3)z轴为内环轴，y轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_y{\cos \mathrm{\α}}_z& {\cos \mathrm{\α}}_y{\sin \mathrm{\α}}_z& -\sin {\mathrm{\α}}_y\\ -\sin {\mathrm{\α}}_z& {\cos \mathrm{\α}}_z& 0\\ \sin {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_y\end{array}\right]$

(4)y轴为内环轴，z轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& {-\sin \mathrm{\α}}_y\cos {\mathrm{\α}}_z\\ -\cos {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z\\ {\sin \mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]$

(5)y轴为内环轴，x轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_y& 0& {-\sin \mathrm{\α}}_y\\ \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\\ {\cos \mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x& {\cos \mathrm{\α}}_x{\cos \mathrm{\α}}_y\end{array}\right]$

(6)x轴为内环轴，y轴为外环轴：

$C_p^b=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_y& \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\cos {\mathrm{\α}}_x{\sin \mathrm{\α}}_y\\ 0& {\cos \mathrm{\α}}_x& {\sin \mathrm{\α}}_x\\ \sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x{\cos \mathrm{\α}}_y& \cos {\mathrm{\α}}_x{\cos \mathrm{\α}}_y\end{array}\right]$

步骤七、根据步骤二，以及步骤四到步骤六得到的t₂时刻的各矩阵值求解载体姿态矩阵完成粗对准。

有益效果

本发明方法通过粗对准过程中的积分运算补偿舰船在风浪作用下摇摆、震荡运动的对准环境下加速度计输出中由摇摆、震荡引起的误差分量，同时通过IMU双轴旋转调制以抑制加速度计零偏随时间积累而引起的误差，从而提高了凝固惯性系粗对准方法的精度。

附图说明

图1是不同旋转环架结构的旋转式惯导系统中从IMU坐标系到载体坐标系的方向余弦矩阵。其中各式所对应的旋转式系统的结构分别为：A.z轴为内环轴，x轴为外环轴；B.x轴为内环轴，z轴为外环轴；C.z轴为内环轴，y轴为外环轴；D.y轴为内环轴，z轴为外环轴；E.y轴为内环轴，x轴为外环轴；F.x轴为内环轴，y轴为外环轴。

图2是对于图1中所列出的各种结构的旋转式惯导系统，在本发明的凝固惯性系粗对准方法中加速度计输出误差经过旋转调制后误差项δV^p0的表达式，其中A～F式分别与图1中A～F式对应。

图3是matlab仿真实验①中，普通捷联惯导系统与应用基于IMU旋转的粗对准误差抑制方法的旋转式惯导系统在凝固惯性系粗对准中误差δV^p0的比较，图3(a)～(c)分别是误差δV^p0在IMU坐标系x，y，z方向的分量随时间的变化情况。

图4是matlab仿真实验②中，普通捷联惯导系统与应用基于IMU旋转的粗对准误差抑制方法的旋转式惯导系统在凝固惯性系粗对准中误差δV^p0的比较，图4(a)～(c)分别是误差δV^p0在IMU坐标系x，y，z方向的分量随时间的变化情况。

具体实施方式

为了进一步说明本发明的目的和优点，下面结合附图和实施例详细描述本发明的具体实施方式。

旋转式捷联惯导姿态矩阵可以通过坐标变换矩阵等效变换为以下形式：

$C_b^n=C_e^n{C}_i^e{C}_{p0}^i{C}_p^{p0}{C}_b^p---\left(1\right)$

式中各角标含义如下：b——载体坐标系；n——东北天地理坐标系；e——地球坐标系；i——地心惯性坐标系；p——IMU坐标系，其三坐标轴分别与IMU惯性器件的敏感轴重合；p0——与起始时刻t₀的IMU坐标系重合的惯性坐标系，即凝固惯性系。

式(1)中各坐标变换矩阵的含义和计算表达式如下：

为从地球坐标系e到导航坐标系n的方向余弦矩阵，可由载体所在位置的经度λ和纬度L确定：

$C_e^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin L& \cos L\\ 0& -\cos L& \sin L\end{array}\right]\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}& -\sin \mathrm{\λ}& 0\\ 0& 0& 1\end{array}\right]$

(2)

$=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right]$

为从地心惯性坐标系i到地球坐标系e之间的变换矩阵：

$C_i^e=\left[\begin{array}{ccc}{\cos \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& \sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& 0\\ -{\sin \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& \cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

式中，Δt＝t-t₀，为粗对准从起始时刻到当前时刻所经历的时间。

为从IMU坐标系p到载体坐标系b的坐标变换矩阵，简称旋转矩阵。旋转矩阵可由双轴旋转惯导系统转轴的旋转角度传感器输出值计算获得，不同结构的IMU旋转系统对应不同的旋转矩阵，具体的双轴旋转系统IMU结构所对应的矩阵如图1所示。在以x轴为外环轴，z轴为内环轴的IMU旋转系统中，的表达式如下：

$C_p^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

(4)

$=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z\cos {\mathrm{\α}}_x& \cos {\mathrm{\α}}_z\cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ \sin {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& -\cos {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]$

式中，α_x，α_z分别为外环、内环的实时旋转角位移，由角度传感器输出。

为当前IMU坐标系与p0坐标系之间的方向余弦矩阵，粗对准起始时刻为单位矩阵I，可以利用粗对准过程中陀螺输出的角速度信息求解：

式中，为IMU坐标系相对于p0坐标系的旋转角速度的反对称矩阵形式：

${\mathrm{\ω}}_{p0,p}^p={\left[\begin{array}{ccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]}^T$

${\mathrm{\Ω}}_{p0,p}^p=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]---\left(6\right)$

在粗对准阶段，陀螺的输出即式(5)的微分方程可以用毕卡逼近算法(适合输出为角增量的陀螺)或龙格—库塔算法(适合输出为角速度的陀螺)。

在此给出采用二阶龙格—库塔算法求解的实现方法：

$C_p^{p0}(t+T)=C_p^{p0}\left(t\right)+\frac{T}{2}\{C_p^{p0}\left(t\right){\mathrm{\Ω}}_{p0,p}^p\left(t\right)+[C_p^{p0}\left(t\right)+{\mathrm{TC}}_p^{p0}\left(t\right){\mathrm{\Ω}}_{p0,p}^p\left(t\right)\left]{\mathrm{\Ω}}_{p0,p}^p(t+T)\right\}$

式中T为姿态惯性器件采样周期。

又因为在一般惯导系统中，载体姿态更新算法可以归结为求解以下微分方程：

因此的求解可以直接利用惯导系统的姿态更新算法程序模块求解，常用的算法有四元数算法和等效旋转矢量算法，上述算法均为惯导领域的公知技术，不是本发明的主要特征，所以不做过多描述。

为从p0坐标系到地心惯性坐标系i的坐标变换矩阵，在一次粗对准过程中为待定常值矩阵，在式(1)中其他方向余弦矩阵的求法都已给出，求出即可代入(1)求得姿态矩阵从而完成粗对准。

求解的方法如下：

在摇摆基座下，IMU输出的比力信息可由式(8)表示：

式中，Δ为加速度计零偏，a_D为干扰加速度，主要来源为沿载体坐标系三轴方向的高频振荡。

在p0坐标系下有：

对(9)式积分，得到：

(10)

将(10)式中第一项和第二项的积分分别记作Vⁱ和δV^p0，由于δV^p0为周期性振荡，近似认为δV^p0≈0，则有

又因为

$g^i=\left[\begin{array}{c}-g\cos L\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})\\ -g\cos L\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt})\\ -g\sin L\end{array}\right]---\left(12\right)$

所以

$V^i=\left[\begin{array}{c}\frac{g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}t)-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[\cos \mathrm{\λ}-\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}t)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·\mathrm{\Δ}t\\ \end{array}\right]---\left(13\right)$

在t₁、t₂两个时刻，p0坐标系和惯性坐标系i下的载体速度具有以下关系：

由于为正交矩阵，可以证明：

按(10)式可求得的计算值，按(13)式可求得Vⁱ(t)，则矩阵可按如下方法计算：

求得后，代入(1)式，进而可以求得

相对于捷联惯导系统，旋转式惯导系统更适合采用基于凝固惯性系的粗对准方法，下面将具体说明。(10)式的误差积分项δV^p0，在结构如(4)式所描述的旋转式捷联惯导系统中，其表达式可以进一步写成：

${\mathrm{\δV}}^{p0}={\∫}_{t_0}^t{C}_b^{p0}{C}_p^b(a_D^p+{\mathrm{\Δ}}^p)\mathrm{dt}$

$={\∫}_{t_0}^t{C}_b^{p0}{C}_p^b{a}_D^p\mathrm{dt}+{\∫}_{t_0}^t{C}_b^{p0}{C}_p^b{\mathrm{\Δ}}^p\mathrm{dt}$

$={\∫}_{t_0}^t{C}_b^{p0}{a}_D^b\mathrm{dt}+{\∫}_{t_0}^t{C}_b^{p0}\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& {\sin \mathrm{\α}}_z& 0\\ -{\sin \mathrm{\α}}_z\cos {\mathrm{\α}}_z& {\cos \mathrm{\α}}_z{\cos \mathrm{\α}}_x& {\sin \mathrm{\α}}_x\\ \sin {\mathrm{\α}}_z{\sin \mathrm{\α}}_x& -\cos {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& {\cos \mathrm{\α}}_x\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\end{array}\right]\mathrm{dt}---\left(17\right)$

$={\∫}_{t_0}^t{C}_b^{p0}{a}_D^b\mathrm{dt}+{\∫}_{t_0}^t{C}_b^{p0}\left[\begin{array}{c}\cos {\mathrm{\α}}_z{\mathrm{\Δ}}_x+{\sin \mathrm{\α}}_z{\mathrm{\Δ}}_y\\ -\sin {\mathrm{\α}}_z{\cos \mathrm{\α}}_x{\mathrm{\Δ}}_x+\cos {\mathrm{\α}}_z\cos {\mathrm{\α}}_x{\mathrm{\Δ}}_y+{\sin \mathrm{\α}}_x{\mathrm{\Δ}}_z\\ \sin {\mathrm{\α}}_z{\sin \mathrm{\α}}_x{\mathrm{\Δ}}_x-{\cos \mathrm{\α}}_z{\sin \mathrm{\α}}_x{\mathrm{\Δ}}_y+{\cos \mathrm{\α}}_x{\mathrm{\Δ}}_z\end{array}\right]\mathrm{dt}$

其中，第一积分项中为高频振荡项，所以经过积分可认为近似为零，而第二项中加速度计零偏为常值，在旋转式惯导系统中，周期性地旋转IMU可将加速度计零偏调制成周期性正弦函数，使其积分近似等于零，在无旋转机构的捷联式惯导系统中(即(17)式中α_z、α_x为0或其他常值的情况下)，零偏向量仍为常值，积分值随时间而增加，所以误差较大。

对于图1中所列出的各种结构的旋转式惯导系统，在凝固惯性系粗对准方法中的误差项δV^p0经过旋转调制后的表达式如图2所示。(图2中A～F式分别与图1中A～F式对应。)可知，各种结构的旋转式捷联惯导系统中，δV^p0中的加速度计零偏积分项都被调制成了周期性的正弦函数，因此都可以通过积分得到补偿，从而提高粗对准精度。

为了使正弦函数的积分为零，旋转式惯导系统的双轴必须周期性地旋转，在此给出典型旋转方案，各方案中内环轴、外环轴分别以恒定角速率ω₁、ω₂旋转，ω₁和ω₂应大于惯导系统舒拉频率(约0.0711°/s)数倍，又因为转速过高会使IMU的动态范围大大提高，对惯性器件的参数稳定性也随之提高，将增加系统的成本，所以转速不宜过高，ω₁和ω₂的范围应在0.6°/s～60°/s之间。基本旋转方案如下：

(1)内环轴、外环轴单向连续旋转。(此方法只有在旋转惯导系统的旋转平台含有导电滑环的情况下才可使用，且当IMU存在标度因数误差和安装误差的情况下IMU旋转将导致新的误差产生和积累，此时不宜采用。)

(2)内环轴、外环轴连续旋转，每旋转一周改变转向。

(3)内环轴、外环轴单向交替旋转，每个轴旋转一周则停止同时开始旋转另一轴，如此循环往复。(此方法只有在旋转惯导系统的旋转平台含有导电滑环的情况下才可使用，且IMU部件存在标度因数误差和安装误差的情况下IMU旋转将导致新的误差产生，此时不宜采用。)

(4)内环轴、外环轴变向交替旋转，第一轴旋转一周后停止，然后由第二轴旋转一周，然后再由第一轴在反向旋转一周，然后再由第二轴反向旋转一周，如此循环往复。

(5)内环轴、外环轴变向交替旋转，第一轴旋转一周后再反向旋转一周，然后停止，然后由第二轴旋转一周后再反向旋转一周，如此循环往复。

下面通过matlab软件仿真实验说明基于IMU旋转的惯性导航系统凝固惯性系粗对准误差抑制方法的效果。

设载体位于北纬30度，东经120度，各陀螺、加速度计零偏分别为0.01°/h和g×10^-4；惯性器件白噪声标准差分别为0.005°/h和0.5g×10^-4；载体俯仰、横滚、方位姿态角按正弦函数变化，幅值分别为10°，11°，12°，频率分别为0.2Hz，0.21Hz，0.22Hz；载体横荡、纵荡、垂荡速度按正弦函数变化，幅值分别为0.1m/s，0.11m/s，0.12m/s，频率分别为0.15Hz，0.16Hz，0.17Hz。

①在如图1中A式所描述的结构的旋转式惯导系统中，按照基本旋转方案(2)，令ω₁＝12°/s，ω₂＝6°/s。捷联惯导系统和旋转惯导系统的δV^p0的加速度计零偏积分分量和δV^p0如图3所示。实施旋转调制方案的旋转式惯导系统中凝固惯性系粗对准方法中的误差δV^p0的幅度小于捷联式惯导系统，而且捷联式系统δV^p0的上升趋势在旋转式系统中得到了抑制。其中图(3)a～c分别表示各沿IMU坐标系x，y，z轴各分量的误差。

②在如图1中A式所描述的结构的旋转式惯导系统中，按照基本旋转方案(4)，令ω₁＝6°/s，ω₂＝6°/s。捷联惯导系统和旋转惯导系统的δV^p0的加速度计零偏积分分量和δV^p0如图4所示。

仿真实验表明：采用使IMU双轴旋转的方法调制零偏误差可以有效抑制求解矩阵的误差，提高凝固惯性系粗对准方法的精度。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。

Claims (8)

1.一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：具体实现过程如下：

步骤一、通过全球导航卫星系统或其他途径获得载体所在位置的经度λ、纬度L；

步骤二、根据步骤一得到的载体所在的经度、纬度，计算从地球坐标系e到导航坐标系n的方向余弦矩阵

步骤三、使旋转式惯导系统的双轴按一定旋转方案周期性地旋转；

旋转方案选择下述方案之一：

a.内环轴、外环轴单向连续旋转；

b.内环轴、外环轴连续旋转，每旋转一周改变转向；

c.内环轴、外环轴单向交替旋转，每个轴旋转一周则停止同时开始旋转另一轴，如此循环往复；

d.内环轴、外环轴变向交替旋转，第一轴旋转一周后停止，然后由第二轴旋转一周，然后再由第一轴在反向旋转一周，然后再由第二轴反向旋转一周，如此循环往复；

e.内环轴、外环轴变向交替旋转，第一轴旋转一周后再反向旋转一周，然后停止，然后由第二轴旋转一周后再反向旋转一周，如此循环往复；

其中，方案a、c只在旋转惯导系统的旋转平台含有导电滑环的情况下使用，且当IMU存在标度因数误差和安装误差的情况下不采用；

所述内环轴、外环轴分别以恒定角速率ω₁、ω₂旋转，且ω₁和ω₂的范围为0.6°/s～60°/s；

步骤四、在步骤三确定旋转方案的同时，利用粗对准过程中陀螺输出的角速度信息，通过求矩阵微分方程数值解，更新IMU坐标系相对于p0坐标系姿态变化的方向余弦矩阵

${\stackrel{\·}{C}}_p^{p0}=C_p^{p0}{\mathrm{\Ω}}_{p0,p}^p$

其中，p0坐标系为与起始时刻t₀时的IMU坐标系重合的惯性坐标系，粗对准起始时刻为单位矩阵I；为IMU坐标系相对于p0坐标系的旋转角速度的反对称矩阵形式；

${\mathrm{\ω}}_{p0,p}^p={\left[\begin{array}{ccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]}^T$

${\mathrm{\Ω}}_{p0,p}^p=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$

其中，ω_x、ω_y、ω_z分别为沿IMU坐标系x,y,z轴的分量；

同时根据更新的积分求解

${\tilde{V}}^{p0}={\∫}_{t_0}^t{C}_p^{p0}{\tilde{f}}^p\mathrm{dt}$

式中t₀为步骤三或者步骤四的起始时刻；

步骤五、在完成步骤三确定的双轴旋转方案的一个完整旋转周期后，选取两个时刻t₁,t₂，其中t₂为一次粗对准结果求解的时刻，分别计算两个时刻的Vⁱ：

$V^i\left(t_1\right)=\left[\begin{array}{c}\frac{g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_1)-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[\cos \mathrm{\λ}-\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_1)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·\mathrm{\Δ}{t}_1\\ \end{array}\right]$ $V^i\left(t_2\right)=\left[\begin{array}{c}\frac{g\cos L[\sin (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2)-\sin \mathrm{\λ}]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ \frac{g\cos L[\cos \mathrm{\λ}-\cos (\mathrm{\λ}+{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2)]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L\·\mathrm{\Δ}{t}_2\\ \end{array}\right]$

式中Δt₁＝t₁-t₀,Δt₂＝t₂-t₀，为粗对准从起始时刻t₀到当前时刻所经历的时间，ω_ie为地球自转角速度；

步骤六、求解矩阵

$C_i^e=\left[\begin{array}{ccc}\cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& \sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& 0\\ -\sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& \cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δ}{t}_2& 0\\ 0& 0& 1\end{array}\right]$

$C_{p0}^i={\left(\begin{array}{cc}{\left[\begin{array}{c}{\left[V^i\left(t_1\right)\right]}^T\\ {\left[V^i\left(t_2\right)\right]}^T\\ {[V^i\left(t_1\right)\×V^i\left(t_2\right)]}^{T\left[\begin{array}{c}\\ \\ \end{array}\right]}\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[{\tilde{V}}^{p0}\left(t_1\right)\right]}^T\\ {\left[{\tilde{V}}^{p0}\left(t_2\right)\right]}^T\\ {[{\tilde{V}}^{p0}\left(t_1\right)\×{\tilde{V}}^{p0}\left(t_2\right)]}^T\end{array}\right]& \end{array}\right)}^T$

为从地心惯性坐标系i到地球坐标系e之间的变换矩阵；为地心惯性坐标系i到与起始时刻IMU坐标系重合的惯性坐标系p0的坐标变换矩阵；为从IMU坐标系p到载体坐标系b的坐标变换矩阵，简称旋转矩阵；

旋转矩阵可由双轴旋转惯导系统转轴的旋转角度传感器输出值计算获得，不同结构的IMU旋转系统对应不同的旋转矩阵，的值可根据t₂时刻内、外环转轴角位置传感器输出α_i,α_j(i,j＝x,y,z)求解，当z轴为内环轴，x轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z\cos {\mathrm{\α}}_x& \cos {\mathrm{\α}}_z\cos {\mathrm{\α}}_x& \mathrm{si}n{\mathrm{\α}}_x\\ \sin {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& -\cos {\mathrm{\α}}_z\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right];$

步骤七、根据步骤二，以及步骤四到步骤六得到的t₂时刻的各矩阵值求解载体姿态矩阵 $C_b^n=C_e^n{C}_i^e{C}_{p0}^i{C}_p^{p0}{C}_b^p,$ 完成粗对准。

2.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：步骤二所述的方向余弦矩阵为：

$C_e^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right].$

3.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：矩阵微分方程数值解用毕卡逼近算法或龙格－库塔算法，或者直接利用惯导系统的姿态更新算法程序模块求解。

4.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：所述步骤六中，当x轴为内环轴，z轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_z& \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_z\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_z\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right].$

5.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：所述步骤六中，当z轴为内环轴，y轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& -\sin {\mathrm{\α}}_y\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ \sin {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_y\end{array}\right].$

6.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：所述步骤六中，当y轴为内环轴，z轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& -\sin {\mathrm{\α}}_y\cos {\mathrm{\α}}_z\\ -\cos {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right].$

7.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：所述步骤六中，当y轴为内环轴，x轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\\ \cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\end{array}\right].$

8.根据权利要求1所述的一种捷联惯性导航系统凝固惯性系粗对准误差抑制方法，其特征在于：所述步骤六中，当x轴为内环轴，y轴为外环轴时:

$C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_y\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ \sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_y& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\end{array}\right].$