CN103076026B - 一种捷联惯导系统中确定多普勒计程仪测速误差的方法 - Google Patents

Abstract

本发明提供了一种捷联惯导系统中确定多普勒计程仪测速误差的方法。该方法通过GPS确定载体的初始位置参数，采集光纤陀螺仪输出和加速度计输出数据，数据处理进行初始对准，确定初始捷联矩阵；然后采集惯性组件测量的载体的角运动和线运动信息，分别采用罗经法和惯导法进行导航解算，其中罗经法解算中引入DVL测量的载体运动速度信息；将两方法解算得到的两组姿态信息做差，进行转换得到两组解算姿态的方位失准角差值；最后将方位失准角差值换算得到DVL测速误差。本发明方法能够在载体航行过程中估算DVL测速误差，将结果补偿给DVL后，提高DVL测速精度，且方法简单，易操作。

Description

一种捷联惯导系统中确定多普勒计程仪测速误差的方法

技术领域

本发明涉及捷联惯导系统的导航解算技术领域，具体是一种基于捷联惯导系统双解算程序的多普勒计程仪(DVL)测速误差的确定方法。

背景技术

捷联惯导系统SINS是一种能够连续输出载体速度、位置和姿态信息的全自主导航系统，它将惯性组件（陀螺仪和加速度计）直接安固连在载体上，通过测量载体运动的线速度和角速度信息，经导航解算后得到导航信息。由于其具有隐蔽性好、不受天气气候条件限制、体积小、重量轻、成本低、易维护等优点，被广泛应用于航空、航天、航海等领域。但是，根据SINS基本原理可知，由于惯性组件输出误差、初始对准失准角等干扰因素导致系统各导航误差振荡、发散是制约捷联惯导系统适用性的重要因素之一。

为了抑制导航误差，各种导航技术穿插在导航解算中：例如，为了消除舒勒、地球周期振荡误差，系统解算过程中引入阻尼技术，该技术作为一种修正方法，通过系统自身解算速度与多普勒计程仪（DVL）测速做差得到速度误差，再经过阻尼网络后消除振荡误差，以此提高惯导系统的长时间导航能力；例如，动基座对准技术，该技术通过载体航行过程中，以系统解算速度与DVL测速做差作为Kalman滤波的观测量，将估算失准角补偿后提高捷联矩阵精度，进而降低失准角对导航影响。以上提及各项导航技术的共同点是都采用了DVL提供的速度信息作为速度基准。但在实际工程应用中，DVL提供速度不会绝对准确，必然存在误差，导致DVL速度误差被引入到导航解算中，最终影响系统导航精度。如何有效的估算DVL测速误差是增强各类导航技术适用性、提高导航精度是一项非常重要的课题。

传统的导航解算方法有罗经法解算和惯导法解算：基于罗经原理的罗经法导航解算是利用惯性器件测量值解算载体姿态信息，不进行速度和位置计算。因此，在载体航行过程中，需要DVL提供速度信息以更新罗经参数。该解算方法中，DVL测速误差对解算姿态影响等效于陀螺漂移，影响其稳态误差；不同于罗经法解算，惯导法导航解算速度、位置信息，因此惯导法解算姿态的稳态误差中不包含DVL测速误差影响。

《中国惯性技术学报》1999年第7卷第2期中由黄一河等人撰写的《周期性阻尼信号对外全阻尼惯导系统位置误差影响研究（海流修正）》，该文章研究了外全阻尼惯导系统的阻尼理论问题，即外测速度包含周期性误差对惯导系统误差影响的规律、特点；《大连海运学报》1985年第11卷第4期中由任茂东撰写的《多普勒计程仪某些误差的分析》，该文章利用数学解析法分析了双波束多普勒计程仪的某些误差，即声速误差、信号起伏误差、换能器安装误差以及波束发射角变化引起的误差等。《现代雷达》1999年第16卷第4期由孙元鸿撰写的《适应消除多普勒导航测速误差》，该文章主要研究海上多普勒导航测速回波频谱偏移畸变的特征参数、非参量实时处理频谱畸变和恢复高斯谱的原理，以及信号起伏和信噪比对修正的影响及实时处理的准则。以上文献都分析了DVL误差的影响，说明确定DVL误差具有必要性，但现有技术都没有给出如何估算DVL测速误差的方法。

发明内容

本发明的目的在于提供一种当载体处于航行状态下，基于捷联惯导系统双解算程序的DVL测速误差估算方法。为了得到DVL测速误差，本发明利用一组惯性组件测量值同时进行两种解算方法解算，将两组解算姿态进一步计算估算DVL测速误差，具体本发明提供的一种捷联惯导系统中确定多普勒计程仪测速误差的方法，包括如下步骤：

步骤1：利用全球定位GPS系统提供载体位置信息，并装订至导航计算机中。

步骤2：光纤陀螺捷联惯导系统进行预热后，采集光纤陀螺仪和石英加速度计输出的数据；根据加速度计输出与重力加速度的关系，以及陀螺仪输出与地球自转角速度的关系，确定载体姿态角，完成系统初始对准，建立惯导系统初始捷联矩阵

$C_{b0}^n=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}-\sin {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}\sin {\mathrm{\φ}}_{z0}& -\cos {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{z0}& \sin {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}+\sin {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\\ \sin {\mathrm{\φ}}_{z0}\cos {\mathrm{\φ}}_{y0}+\sin {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}& \cos {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{z0}& \sin {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{y0}+\cos {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\\ -\cos {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}& \sin {\mathrm{\φ}}_{x0}& \cos {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\end{array}\right]---\left(1\right)$

其中，φ_x0、φ_y0、φ_z0分别表示初始时刻载体纵摇角、横滚角、航向角。

步骤3：利用惯性组件测量载体角运动和线运动信息，采用罗经法进行导航解算，更新捷联矩阵并解算得到载体的纵摇角φ_x(C)、横滚角φ_y(C)和航向角φ_z(C)，下角标(C)表示罗经法解算。罗经解算过程中，引入DVL测量的载体运动速度信息来更新罗经参数。

步骤4：进行步骤3的同时，利用步骤3同一组惯性组件测量的载体的角运动和线运动信息，进行惯导法导航解算，更新捷联矩阵并解算得到载体的纵摇角φ_x(I)、横滚角φ_y(I)和航向角φ_z(I)，下角标(I)表示惯导法解算。

步骤5：将罗经法和惯导法解算姿态角做差，建立方位失准角差值：

γ_(I)-γ_(C)＝(φ_y(C)-φ_y(I))sinφ_x(C)+(φ_z(C)-φ_z(I))（2）

其中，γ_(I)、γ_(C)分别表示惯导法和罗经法解算的方位失准角。

步骤6：利用步骤5中方位失准角差值，确定DVL测速误差δV_D：

其中，R表示地球半径；Ω表示地球自转角速度；表示利用DVL测速推位计算得到的载体所在位置的纬度。

本发明与现有技术相比的优点在于：本发明提出的确定DVL测速误差的方法，使用一组捷联惯性测量组件的测量值同时支持惯导法和罗经法两个解算程序的计算，其中罗经法引入DVL测量的速度信息更新罗经参数，两个解算程序解算的姿态信息进一步耦合计算得到DVL测速误差，该方法具有如下特点，（1）本发明利用捷联惯性导航系统作为辅助系统估算DVL测速误差，将该估算结果补偿给DVL后，提高DVL测速精度。（2）利用来源于同一组测量组件的测量值，输出结果具有相关性；（3）测量信息完全同步，且不存在任何安装偏差；（4）本发明不需任何外界信息，利用捷联惯导系统导航解算中，不同解算程序解算的导航信息误差形式不同这一特点，将两组捷联惯导系统解算姿态进一步耦合计算得到DVL测速误差，方法简单，易操作。

附图说明

图1为本发明的确定DVL测速误差方法的流程图；

图2为本发明利用VisualC++仿真得到十次估算DVL测速误差曲线；

图3为本发明利用VisualC++仿真估算测速误差补偿前后系统解算姿态误差比较曲线；

图4为利用本发明方法对海上试验估算DVL测速误差补偿前后系统解算姿态比较曲线；

图5为利用海上试验载体航行轨迹。

具体实施方式

下面结合附图对本发明的具体实施方式进行详细描述。

如图1所示，本发明提供的一种捷联惯导系统中确定多普勒计程仪(DVL)测速误差的方法，具体包括如下步骤。

步骤1：利用全球定位GPS系统提供载体初始的位置信息，并装订至导航计算机中。所述的位置信息是指载体初始位置的纬度和经度。

步骤2：光纤陀螺捷联惯导系统进行预热后,采集光纤陀螺仪和石英加速度计输出的数据；根据加速度计输出与重力加速度的关系，以及陀螺仪输出与地球自转角速度的关系，确定载体姿态角，完成系统初始对准，建立惯导系统初始捷联矩阵

$C_{b0}^n=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}-\sin {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}\sin {\mathrm{\φ}}_{z0}& -\cos {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{z0}& \sin {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}+\sin {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\\ \sin {\mathrm{\φ}}_{z0}\cos {\mathrm{\φ}}_{y0}+\sin {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}\cos {\mathrm{\φ}}_{z0}& \cos {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{z0}& \sin {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{y0}+\cos {\mathrm{\φ}}_{z0}\sin {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\\ -\cos {\mathrm{\φ}}_{x0}\sin {\mathrm{\φ}}_{y0}& \sin {\mathrm{\φ}}_{x0}& \cos {\mathrm{\φ}}_{x0}\cos {\mathrm{\φ}}_{y0}\end{array}\right]---\left(4\right)$

其中，φ_x0、φ_y0、φ_z0分别表示初始时刻载体纵摇角、横滚角、航向角。

步骤3：利用惯性组件测量载体角运动和线运动信息，进行罗经法导航解算。罗经解算过程中，为了更新罗经参数，引入DVL测量载体运动速度信息。下面公式中下角标(C)表示罗经法解算结果，小脚标(I)表示惯导法解算结果。

采集陀螺仪测量数据更新捷联矩阵具体是：

首先更新角速度：

${\mathrm{\ω}}_{\mathrm{nb}\left(C\right)}^b={\mathrm{\ω}}_{\mathrm{ib}\left(C\right)}^b-{\left(C_{b\left(C\right)}^n\right)}^T({\mathrm{\ω}}_{\mathrm{ie}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{en}\left(C\right)}^n)-{\left(C_{b\left(C\right)}^n\right)}^T{\mathrm{\ω}}_{c\left(C\right)}^n---\left(5\right)$

其中，i表示地心惯性系，e表示地球坐标系，b表示载体坐标系，n表示导航坐标系，本发明中导航坐标系采用当地地理坐标系；表示b系到n系的捷联矩阵，导航解算初始时刻，采用步骤2中利用初始对准得到的初始捷联矩阵后面解算过程中，采用更新后的捷联矩阵；表示p系相对m系的旋转角速度在q系投影；为控制角速率在n系上的投影；为地球自转角速度在n系投影；·^T表示矩阵转置。地球自转角速度在导航系投影更新：

Ω＝0004167°/s，表示地球自转角速度；表示利用DVL测速推位计算得到的载体所在位置的纬度，其计算方法为：

其中，表示步骤1中利用GPS获得的初始时刻载体所在位置的纬度，v_y(D)表示DVL测速在导航坐标系oy_n轴上的分量；t表示当前的载体航行时间；R表示地球半径。

由于载体运动导致导航坐标系相对地球坐标系变化的旋转角速度在导航坐标系投影根据下式更新：

其中，v_x(D)表示DVL测速在导航坐标系ox_n轴上的分量。

控制角速率在导航坐标系ox_n轴、oy_n轴、oz_n轴上的分量和更新为：

${\mathrm{\ω}}_{\mathrm{cx}\left(C\right)}^n=\frac{k_N}{s+k_1}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

${\mathrm{\ω}}_{\mathrm{cz}\left(C\right)}^n=\frac{k_U}{(s+k_1)(s+k_2)}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

其中，v_x(C)和v_y(C)分别为罗经法解算载体沿导航坐标系ox_n轴和oy_n轴的运动速度；k₁、k₂、k_E、k_N、k_U为罗经参数，本发明实施例中设置k₁＝0.002828、k₂＝0.002828、k₃＝6.1218×10^-7、k_E＝k_N＝k_U＝8.0429×10^-9。s表示复数域参变量。

然后采用更新四元数法更新捷联矩阵

设载体坐标系相对导航坐标系的转动四元数Q为：

Q_(C)＝q_0(C)+q_1(C)i_b+q_2(C)j_b+q_3(C)k_b（10）

其中，q_0(C)、q_1(C)、q_2(C)和q_3(C)为罗经法解算中四元数的四个实数；i_b、j_b和k_b分别表示载体坐标系ox_b轴、oy_b轴和oz_b轴上的单位方向向量。

四元数Q的及时修正：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(C\right)}\\ {\stackrel{\·}{q}}_{1\left(C\right)}\\ {\stackrel{\·}{q}}_{2\left(C\right)}\\ {\stackrel{\·}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& {\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(C\right)}\\ q_{1\left(C\right)}\\ q_{2\left(C\right)}\\ q_{3\left(C\right)}\end{array}\right]---\left(11\right)$

其中，分别表示罗经算法中载体坐标系相对导航坐标系的旋转角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量。分别表示q_0(C)、q_1(C)、q_2(C)、q_3(C)的微分量。

利用求出的q_0(C)、q_1(C)、q_2(C)、q_3(C)更新罗经算法中的捷联矩阵

$C_{b\left(C\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(C\right)}^2+q_{1\left(C\right)}^2-q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{1\left(C\right)}{q}_{2\left(C\right)}-q_{0\left(C\right)}{q}_{3\left(C\right)})& 2(q_{1\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{2\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{2\left(C\right)}+q_{0\left(C\right)}{q}_{3\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2+q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{1\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{2\left(C\right)})& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{1\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2-q_{2\left(C\right)}^2+q_{3\left(C\right)}^2\end{array}\right]---\left(12\right)$

更新载体姿态信息，具体经罗经法解算得到的载体的纵摇角φ_x(C)、横滚角φ_y(C)和航向角φ_z(C)分别为：

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(C\right)}=\arcsin \left(c_{33\left(C\right)}\right)\\ {\mathrm{\φ}}_{y\left(C\right)}=\arctan (c_{32\left(C\right)}/c_{31\left(C\right)})\\ {\mathrm{\φ}}_{z\left(C\right)}=\arctan (c_{13\left(C\right)}/c_{23\left(C\right)})\end{array}\right.---\left(13\right)$

其中，c_ij(C)（i＝1,2,3，j＝1,2,3）表示中第i行第j列矩阵元素。

利用加速度计测量比力通过捷联矩阵转换：

$f_{\left(C\right)}^n=C_{b\left(C\right)}^n{f}_{\left(C\right)}^s---\left(14\right)$

其中，fⁿ、f^s分别表示加速度计测量比力在n系和s系投影。

利用下列微分方程求解载体运动速度：

$\left[\begin{array}{c}{\stackrel{\·}{v}}_{x\left(C\right)}\\ {\stackrel{\·}{v}}_{y\left(C\right)}\\ {\stackrel{\·}{v}}_{z\left(C\right)}\end{array}\right]=\left[\begin{array}{c}{f}_{x\left(C\right)}^n\\ f_{y\left(C\right)}^n\\ f_{z\left(C\right)}^n\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\ω}}_{\mathrm{iez}\left(C\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(C\right)}^n)\\ -{\mathrm{\ω}}_{\mathrm{iez}\left(C\right)}^n& 0& 2{\mathrm{\ω}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(C\right)}^n\\ 2{\mathrm{\ω}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(C\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(C\right)}^n)& 0\end{array}\right]\left[\begin{array}{c}{v}_{x\left(C\right)}\\ v_{y\left(C\right)}\\ v_{z\left(C\right)}\end{array}\right]---\left(15\right)$

其中，分别表示加速度计测量比力在导航坐标系ox_n轴、oy_n轴、oz_n轴上的分量；g为重力加速度。和分别表示地球自转角速度在导航坐标系ox_n轴、oy_n轴、oz_n轴上的分量。分别表示由于载体运动导致导航坐标系相对地球坐标系变化的旋转角速度在导航坐标系ox_n轴、oy_n轴上的投影。分别表示v_x(C)、v_y(C)、v_z(C)的微分量。

步骤4：进行步骤3的同时，利用步骤3中惯性组件测量的同一组载体的角运动和线运动信息，采用惯导法进行导航解算。具体过程为：

采集陀螺仪测量数据更新捷联矩阵首先更新角速度：

${\mathrm{\ω}}_{\mathrm{nb}\left(I\right)}^b={\mathrm{\ω}}_{\mathrm{ib}\left(I\right)}^b-{\left(C_{b\left(I\right)}^n\right)}^T({\mathrm{\ω}}_{\mathrm{ie}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{en}\left(I\right)}^n)---\left(16\right)$

其中，下角标(I)表示惯导法解算结果。

更新公式如下：

其中，表示惯导法解算得到的载体所在位置的纬度。

更新公式如下：

其中，表示由于载体运动导致导航坐标系相对地球坐标系变化的旋转角速度在导航坐标系的投影；v_x(I)、v_y(I)分别惯导法解算的载体沿导航系ox_n轴、oy_n轴上的运动速度。R表示地球半径。

设载体坐标系相对导航坐标系的转动四元数Q_(I)为

Q_(I)＝q_0(I)+q_1(I)i_b+q_2(I)j_b+q_3(I)k_b（19）

其中，q_0(I)、q_1(I)、q_2(I)、q_3(I)为四元数的四个实数。

四元数Q的及时修正：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(I\right)}\\ {\stackrel{\·}{q}}_{1\left(I\right)}\\ {\stackrel{\·}{q}}_{2\left(I\right)}\\ {\stackrel{\·}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& {\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(I\right)}\\ q_{1\left(I\right)}\\ q_{2\left(I\right)}\\ q_{3\left(I\right)}\end{array}\right]---\left(20\right)$

其中，分别表示惯导法中载体坐标系相对导航坐标系的运动角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量。分别表示q_0(I)、q_1(I)、q_2(I)、q_3(I)的微分量。

利用求出的q_0(I)、q_1(I)、q_2(I)、q_3(I)更新捷联矩阵

$C_{b\left(I\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(I\right)}^2+q_{1\left(I\right)}^2-q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{1\left(I\right)}{q}_{2\left(I\right)}-q_{0\left(I\right)}{q}_{3\left(I\right)})& 2(q_{1\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{2\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{2\left(I\right)}+q_{0\left(I\right)}{q}_{3\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2+q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{1\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{2\left(I\right)})& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{1\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2-q_{2\left(I\right)}^2+q_{3\left(I\right)}^2\end{array}\right]---\left(21\right)$

更新载体姿态信息，具体采用惯导法解算得到的载体纵摇角φ_x(I)、横摇角φ_y(I)和航向角φ_z(I)分别为：

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(I\right)}=\arcsin \left(c_{33\left(I\right)}\right)\\ {\mathrm{\φ}}_{y\left(I\right)}=\arctan (c_{32\left(I\right)}/c_{31\left(I\right)})\\ {\mathrm{\φ}}_{z\left(I\right)}=\arctan (c_{13\left(I\right)}/c_{23\left(I\right)})\end{array}\right.---\left(22\right)$

其中，c_ij(I)（i＝1,2,3，j＝1,2,3）表示中第i行第j列矩阵元素。

利用加速度计测量比力通过捷联矩阵转换：

$f_{\left(I\right)}^n=C_{b\left(I\right)}^n{f}_{\left(I\right)}^s---\left(23\right)$

利用下列微分方程求解载体运动速度：

$\left[\begin{array}{c}{\stackrel{\·}{v}}_{x\left(I\right)}\\ {\stackrel{\·}{v}}_{y\left(I\right)}\\ {\stackrel{\·}{v}}_{z\left(I\right)}\end{array}\right]=\left[\begin{array}{c}{f}_{x\left(I\right)}^n\\ f_{y\left(I\right)}^n\\ f_{z\left(I\right)}^n\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\ω}}_{\mathrm{iez}\left(I\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(I\right)}^n)\\ -{\mathrm{\ω}}_{\mathrm{iez}\left(I\right)}^n& 0& 2{\mathrm{\ω}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(I\right)}^n\\ 2{\mathrm{\ω}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(I\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(I\right)}^n)& 0\end{array}\right]\left[\begin{array}{c}{v}_{x\left(I\right)}\\ v_{y\left(I\right)}\\ v_{z\left(I\right)}\end{array}\right]---\left(24\right)$

步骤5：将罗经法和惯导法解算的姿态角做差，建立方位失准角差值：

γ_(I)-γ_(C)＝(φ_y(C)-φ_y(I))sinφ_x(C)+(φ_z(C)-φ_z(I))（25）

其中，γ_(I)、γ_(C)分别表示惯导法和罗经法解算的方位失准角。

步骤6：根据步骤5得到的方位失准角差值，进一步得到DVL测速误差δV_D：

对本发明的有益效果进行验证如下：

（1）在VisualC++仿真条件下，对该方法进行仿真实验：

载体做三轴摇摆运动。载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：

其中，φ_x、φ_y、φ_z分别表示载体的纵摇角、横摇角和航向角；φ_xm、π_ym、π_zm分别表示相应的摇摆角度幅值，φ_xm＝φ_ym＝φ_zm＝5°；分别表示纵摇角、横摇角和航向角摇摆的初始相位，T_x、T_y、T_z分别表示相应摇摆轴的摇摆周期T_x＝T_y＝T_z＝4s；k为真航迹，k＝30°。

载体初始位置：北纬45.7796°，东经126.6705°；

载体匀速直航运动，运动速度为v＝15m/s；

DVL测速误差：δv_D＝1m/s；

赤道半径：R＝6378393.0m；

由万有引力可得的地球表面重力加速度：g＝9.78049m/s²；

地球自转角速度：Ω＝7.2921158×10^-5rad/s；

常数：π＝3.1415926535；

光纤陀螺常值漂移：0.01°/h；

光纤陀螺白噪声误差：0.005°/h；

光纤陀螺刻度因数误差：10ppm；

光纤陀螺安装误差：1×10^-3rad；

加速度计零偏：10^-4g₀；

加速度计白噪声误差：5×10^-5g₀；

加速度计刻度因数误差：10ppm；

加速度计安装误差：1×10^-3rad；

罗经参数：k₁＝0.002828、k₂＝0.002828、k₃＝6.1218×10^-7、k_E＝k_N＝k_U＝8.0429×10^-9；

惯导解算中，系统工作在内全阻尼状态，阻尼系数0.5；

仿真时间：t＝48h；

采样频率：Hn＝0.01s；

利用发明所述方法，得到十次估算DVL测速误差结果如图2所示，十次估算结果统计如表1，如图3所示为十次估算均值补偿前后系统解算姿态的比较曲线。结果表明本发明能够较好估计DVL测速误差。

表1DVL测速误差估算结果统计

（2）光纤陀螺捷联惯导系统海上航行试验

本试验是采用光纤陀螺捷联惯导系统进行的海上航行试验，本次试验利用安装在船艏的DVL提供其测量载体运动速度，所用光纤陀螺惯导系统主要技术指标如下：

动态范围：±100°/s；

零偏稳定性：≤0.005°/h；

随机游走：

标度因数非线性度：≤5ppm。

利用本发明方法，得到估算DVL测速误差补偿前后系统解算姿态比较曲线如图4所示，载体航行轨迹如图5所示。结果表明本发明能够较好估计DVL测速误差，提高系统解算姿态精度。

Claims (3)

1.一种捷联惯导系统中确定多普勒计程仪(DVL)测速误差的方法，其特征在于，包括以下步骤：

步骤1：利用全球定位GPS系统提供载体初始时刻的位置信息，并装订至导航计算机中；所述的位置信息包括：载体初始时刻所在位置的纬度和经度；

步骤2：光纤陀螺捷联惯导系统进行预热后，采集光纤陀螺仪和石英加速度计输出的数据，然后确定载体姿态角，完成系统初始对准，建立捷联惯导系统的初始捷联矩阵

$C_{b0}^n=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_{y0}{\mathrm{cos\φ}}_{z0}-{\mathrm{sin\φ}}_{x0}{\mathrm{sin\φ}}_{y0}{\mathrm{sin\φ}}_{z0}& -{\mathrm{cos\φ}}_{x0}{\mathrm{sin\φ}}_{z0}& {\mathrm{sin\φ}}_{y0}{\mathrm{cos\φ}}_{z0}+{\mathrm{sin\φ}}_{z0}{\mathrm{sin\φ}}_{x0}{\mathrm{cos\φ}}_{y0}\\ {\mathrm{sin\φ}}_{z0}{\mathrm{cos\φ}}_{y0}+{\mathrm{sin\φ}}_{x0}{\mathrm{sin\φ}}_{y0}{\mathrm{cos\φ}}_{z0}& {\mathrm{cos\φ}}_{x0}{\mathrm{cos\φ}}_{z0}& {\mathrm{sin\φ}}_{z0}{\mathrm{sin\φ}}_{y0}+{\mathrm{cos\φ}}_{z0}{\mathrm{sin\φ}}_{x0}{\mathrm{cos\φ}}_{y0}\\ -{\mathrm{cos\φ}}_{x0}{\mathrm{sin\φ}}_{y0}& {\mathrm{sin\φ}}_{x0}& {\mathrm{cos\φ}}_{x0}{\mathrm{cos\φ}}_{y0}\end{array}\right]$

其中，φ_x0、φ_y0和φ_z0分别表示初始时刻载体的纵摇角、横滚角和航向角，b表示载体坐标系，n表示导航坐标系；

步骤3：利用惯性组件测量载体的角运动和线运动信息，进行罗经法导航解算，更新捷联矩阵并解算得到载体的纵摇角φ_x(C)、横滚角φ_y(C)和航向角φ_z(C)，下角标(C)表示罗经法解算；

步骤4：利用步骤3中惯性组件测量的载体的角运动和线运动信息，采用惯导法进行导航解算，更新捷联矩阵并解算得到载体的纵摇角φ_x(I)、横滚角φ_y(I)和航向角φ_z(I)，下角标(I)表示惯导法解算；

步骤5：将罗经法和惯导法解算的姿态角做差，建立方位失准角差值：

γ_(I)-γ_(C)＝(φ_y(C)-φ_y(I))sinφ_x(C)+(φ_z(C)-φ_z(I))

其中，γ_(I)、γ_(C)分别表示惯导法和罗经法解算的方位失准角；

步骤6：利用步骤5中方位失准角差值，确定DVL测速误差δV_D：

其中，R表示地球半径；Ω为地球自转角速度，值为0.004167°/s；表示利用DVL测速推位计算得到的载体所在位置的纬度。

2.根据权利要求1所述的一种捷联惯导系统中确定多普勒计程仪测速误差的方法，其特征在于，步骤3中所述的进行罗经法导航解算，具体过程为：

首先采用下面公式进行角速度更新：

${\mathrm{\ω}}_{nb\left(C\right)}^b={\mathrm{\ω}}_{ib\left(C\right)}^b-{\left(C_{b\left(C\right)}^n\right)}^T({\mathrm{\ω}}_{ie\left(C\right)}^n+{\mathrm{\ω}}_{en\left(C\right)}^n)-{\left(C_{b\left(C\right)}^n\right)}^T{\mathrm{\ω}}_{c\left(C\right)}^n$

i表示地心惯性系，e表示地球坐标系；表示p系相对m系的旋转角速度在q系的投影，m＝n,i,e，p＝b,e,n，q＝b,n；是地球自转角速度在n系的投影；为控制角速率在n系的

投影；·^T表示矩阵转置；初始采用步骤2中得到的初始捷联矩阵

的更新公式为： 表示利用DVL测速推位计算得到的载体所在位置的纬度， 表示载体初始时刻所在位置的纬度，v_y(D)表示DVL测速在导航坐标系oy_n轴上的分量；t表示当前的载体航行时间；R表示地球半径；

更新公式为：其中，v_x(D)表示DVL测速在导航坐标系ox_n轴上的分量；

控制角速率在导航坐标系ox_n轴、oy_n轴和oz_n轴上的分量和更新为：

${\mathrm{\ω}}_{cx\left(C\right)}^n=\frac{k_N}{s+k_1}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

${\mathrm{\ω}}_{cz\left(C\right)}^n=\frac{k_U}{(s+k_1)(s+k_2)}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

其中，v_x(C)和v_y(C)分别为罗经法解算载体沿导航坐标系ox_n轴和oy_n轴的运动速度；s表示复数域参变量；罗经参数k₁、k₂、k_E、k_N和k_U设置为：k₁＝0.002828，k₂＝0.002828，k_E＝k_N＝k_U＝8.0429×10^-9；

然后采用更新四元数法更新捷联矩阵

设载体坐标系相对导航坐标系的转动四元数Q为：

Q_(C)＝q_0(C)+q_1(C)i_b+q_2(C)j_b+q_3(C)k_b

其中，q_0(C)、q_1(C)、q_2(C)和q_3(C)为罗经法解算中四元数的四个实数；i_b、j_b和k_b分别表示载体坐标系ox_b轴、oy_b轴和oz_b轴上的单位方向向量；

四元数Q的及时修正：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(C\right)}\\ {\stackrel{\·}{q}}_{1\left(C\right)}\\ {\stackrel{\·}{q}}_{2\left(C\right)}\\ {\stackrel{\·}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx\left(C\right)}^b& -{\mathrm{\ω}}_{nby\left(C\right)}^b& -{\mathrm{\ω}}_{nbz\left(C\right)}^b\\ {\mathrm{\ω}}_{nbx\left(C\right)}^b& 0& {\mathrm{\ω}}_{nbz\left(C\right)}^b& -{\mathrm{\ω}}_{nby\left(C\right)}^b\\ {\mathrm{\ω}}_{nby\left(C\right)}^b& -{\mathrm{\ω}}_{nbz\left(C\right)}^b& 0& {\mathrm{\ω}}_{nbx\left(C\right)}^b\\ {\mathrm{\ω}}_{nbz\left(C\right)}^b& {\mathrm{\ω}}_{nby\left(C\right)}^b& -{\mathrm{\ω}}_{nbx\left(C\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(C\right)}\\ q_{1\left(C\right)}\\ q_{2\left(C\right)}\\ q_{3\left(C\right)}\end{array}\right]$

其中，分别表示载体坐标系相对导航坐标系的旋转角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量；分别表示q_0(C)、q_1(C)、q_2(C)、q_3(C)的微分量；

利用求出的q_0(C)、q_1(C)、q_2(C)、q_3(C)更新罗经算法中的捷联矩阵

$C_{b\left(C\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(C\right)}^2+q_{1\left(C\right)}^2-q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{1\left(C\right)}{q}_{2\left(C\right)}-q_{0\left(C\right)}{q}_{3\left(C\right)})& 2(q_{1\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{2\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{2\left(C\right)}+q_{0\left(C\right)}{q}_{3\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2+q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{1\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{2\left(C\right)})& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{1\left(C\right)})& q_{0\left(C\right)}^2-q_{2\left(C\right)}^2-q_{2\left(C\right)}^2+q_{3\left(C\right)}^2\end{array}\right]$

经罗经法解算得到的载体的纵摇角φ_x(C)、横滚角φ_y(C)和航向角φ_z(C)为：

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(C\right)}=\arcsin \left(c_{33\left(C\right)}\right)\\ {\mathrm{\φ}}_{y\left(C\right)}=arctan(c_{32\left(C\right)}/c_{31\left(C\right)})\\ {\mathrm{\φ}}_{z\left(C\right)}=arctan(c_{13\left(C\right)}/c_{23\left(C\right)})\end{array}\right.$

其中，c_ij(C)表示中第i行第j列矩阵元素，i＝1,2,3，j＝1,2,3。

3.根据权利要求1所述的一种捷联惯导系统中确定多普勒计程仪测速误差的方法，其特征在于，步骤4中所述的采用惯导法进行导航解算，具体过程为：

首先更新角速度：

${\mathrm{\ω}}_{nb\left(I\right)}^b={\mathrm{\ω}}_{ib\left(I\right)}^b-{\left(C_{b\left(I\right)}^n\right)}^T({\mathrm{\ω}}_{ie\left(I\right)}^n+{\mathrm{\ω}}_{en\left(I\right)}^n)$

其中，i表示地心惯性系，e表示地球坐标系；初始采用步骤2中得到的初始捷联矩阵表示p系相对m系的旋转角速度在q系的投影，m＝n,i,e，p＝b,e,n，q＝b,n；是地球自转角速度在n系的投影；

更新公式为： 表示惯导法解算得到的载体所在位置的纬度；

更新公式为：其中，v_x(I)、v_y(I)分别惯导法解算的载体沿导航系ox_n轴、oy_n轴上的运动速度，R表示地球半径；

然后更新捷联矩阵

设载体坐标系相对平台坐标系的转动四元数Q为：

Q_(I)＝q_0(I)+q_1(I)i_b+q_2(I)j_b+q_3(I)k_b

其中，q_0(I)、q_1(I)、q_2(I)、q_3(I)为惯导法解算中四元数的四个实数，i_b、j_b和k_b分别表示载体坐标系ox_b轴、oy_b轴和oz_b轴上的单位方向向量；

四元数Q的及时修正：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(I\right)}\\ {\stackrel{\·}{q}}_{1\left(I\right)}\\ {\stackrel{\·}{q}}_{2\left(I\right)}\\ {\stackrel{\·}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx\left(I\right)}^b& -{\mathrm{\ω}}_{nby\left(I\right)}^b& -{\mathrm{\ω}}_{nbz\left(I\right)}^b\\ {\mathrm{\ω}}_{nbx\left(I\right)}^b& 0& {\mathrm{\ω}}_{nbz\left(I\right)}^b& -{\mathrm{\ω}}_{nby\left(I\right)}^b\\ {\mathrm{\ω}}_{nby\left(I\right)}^b& -{\mathrm{\ω}}_{nbz\left(I\right)}^b& 0& {\mathrm{\ω}}_{nbx\left(I\right)}^b\\ {\mathrm{\ω}}_{nbz\left(I\right)}^b& {\mathrm{\ω}}_{nby\left(I\right)}^b& -{\mathrm{\ω}}_{nbx\left(I\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(I\right)}\\ q_{1\left(I\right)}\\ q_{2\left(I\right)}\\ q_{3\left(I\right)}\end{array}\right]$

其中，分别表示载体坐标系相对导航坐标系的运动角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量；分别表示q_0(I)、q_1(I)、q_2(I)、q_3(I)的微分量；

利用求出的q_0(I)、q_1(I)、q_2(I)、q_3(I)更新捷联矩阵

$C_{b\left(I\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(I\right)}^2+q_{1\left(I\right)}^2-q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{1\left(I\right)}{q}_{2\left(I\right)}-q_{0\left(I\right)}{q}_{3\left(I\right)})& 2(q_{1\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{2\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{2\left(I\right)}+q_{0\left(I\right)}{q}_{3\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2+q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{1\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{2\left(I\right)})& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{1\left(I\right)})& q_{0\left(I\right)}^2-q_{2\left(I\right)}^2-q_{2\left(I\right)}^2+q_{3\left(I\right)}^2\end{array}\right]$

采用惯导法解算得到的载体的纵摇角φ_x(I)、横摇角φ_y(I)和航向角φ_z(I)为：

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(I\right)}=\arcsin \left(c_{33\left(I\right)}\right)\\ {\mathrm{\φ}}_{y\left(I\right)}=arctan(c_{32\left(I\right)}/c_{31\left(I\right)})\\ {\mathrm{\φ}}_{z\left(I\right)}=arctan(c_{13\left(I\right)}/c_{23\left(I\right)})\end{array}\right.$

其中，c_ij(I)表示中第i行第j列矩阵元素，i＝1,2,3，j＝1,2,3。