一种用于水下INS和DVL组合导航系统的DVL参数标定方法

摘要

一种用于水下INS和DVL组合导航系统的DVL参数标定方法，其流程为：(1)水下航行器完成初始对准后，进行第一次GNSS校准；(2)匀速直航一定距离后进行第二次GNSS校准；(3)根据两次校准记录的GNSS和组合导航位置信息，计算DVL参数；(4)航行器折返并匀速直航一定距离后进行第三次GNSS校准，计算DVL参数，根据上次DVL参数迭代计算总的DVL参数；(5)航行器折返并直航一定距离时，进行第四次GNSS校准，根据第四次校准组合导航位置和GNSS位置信息，计算组合导航的位置误差，判断精度是否满足要求，若满足则标定完成，否则迭代计算以完成标定。本发明具有计算简单、精度高、操作简单等优点。

说明书

技术领域

本发明主要涉及到导航系统领域，特指一种主要适用于远程水下航行器惯性导航系统 (Inertial Navigation System，INS)与多普勒测速仪(Doppler Velocity Log，DVL)组合导航 中的DVL参数标定方法。

背景技术

由于远程水下航行器具有远航程的特点，单一的导航系统都无法满足需求。惯性导航具 有自主、短时定位精度高的优势，但存在位置误差随时间累积的缺点；GNSS(Global Navigation  Satellite System)导航定位精度高，定位误差不随时间累积，但存在抗干扰能力弱，且无法进 行水下导航等不足。SINS/DVL组合导航则是利用DVL提供的高精度速度信息抑制SINS定 位误差的累积，其具有自主性和定位精度高的优点，成为了远程水下潜航器导航技术研究的 热点。

而DVL的参数(即：DVL刻度因子、DVL与INS基准之间安装角偏差)是影响自主导 航精度的关键因素，因此在组合导航时必须对它们进行标定和补偿。目前，在水下INS/DVL 组合导航中主要采用较高的加工工艺以保证DVL与INS基准之间的安装关系，或者采用全 程使用GNSS方式事先标定DVL的参数。对于上述采用搞精度加工工艺的办法而言，其成本 高，并且很难保证DVL与INS之间的安装精度，从而导致INS/DVL组合导航定位精度下降； 而对上述全程使用GNSS信号来标定DVL参数的方式而言，则必须使水下航行器浮出水面接 受GNSS信号，因此也存在操作复杂等缺点。

总而言之，现有远程水下航行器INS/DVL组合导航中的DVL参数标定方法均存在成本 高、操作复杂等不足，难以满足远程水下潜航器高精度INS/DVL组合导航日益迫切的高精度 要求。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种计算 简单、精度高、操作简单的用于水下INS和DVL组合导航系统的DVL参数标定方法，其可 广泛应用于远程水下航行器组合导航中的DVL参数标定，从而提高INS/DVL组合导航系统 的精度。

为解决上述技术问题，本发明采用以下技术方案：

一种用于水下INS和DVL组合导航系统的DVL参数标定方法，其流程为：

(1)第一次GNSS校准：水下航行器完成初始对准后，进入SINS/DVL组合导航状态， 浮出水面接收GNSS信号，进行第一次GNSS校准，并记录当GNSS最后一次位置有效时的 组合导航的基于地心地固坐标系下的位置(x_I0，u_I0，z_I0)和GNSS的基于地心地固坐标系下的 位置信息(x_G0，y_G0，z_G0)，作为校准点1的信息；

(2)第二次GNSS校准：水下航行器匀速直航，并以INS/DVL组合导航状态工作，航 行一段距离后水下航行器上浮开始接收GNSS信号，进行第二次校准，记录从第一个校准点 开始到第二个校准点DVL天向投影速度的积分H_DVL，记录当GNSS位置第一次有效时且没 有校准的组合导航位置(x_I1，y_I1，z_I1)和GNSS位置(x_G1，y_G1，z_G1)，作为校准点2的信息；

(3)计算DVL参数：根据校准点1和校准点2记录的GNSS位置信息和组合导航位置 信息，计算DVL参数；即：DVL刻度因子K₀、DVL与INS的方位安装角偏差α₀和俯仰安 装角偏差β₀，然后代入INS/DVL组合导航进行计算，并记录当GNSS最后一次位置有效时 的组合导航的基于地心地固坐标系下的位置(x_I2，y_I2，z_I2)和GNSS的基于地心地固坐标系下 的位置信息(x_G2，y_G2，z_G2)，作为校准点3的信息；

(4)第三次GNSS校准：航行器折返并匀速直航，航行一定距离时，进行第三次GNSS 校准，记录从校准点2开始到校准点3DVL速度在天向投影的积分H_DVL，记录当GNSS位置 第一次有效时且没有校准的组合导航位置(x_I3，y_I3，z_I3)和GNSS的位置信息(x_G3，y_G3，z_G3)，作 为校准点4的信息，根据校准点3和校准点4的信息，计算DVL刻度因子K₁、DVL与INS 的方位安装角偏差α₁和俯仰安装角偏差β₁；

(5)根据上次DVL参数迭代计算总的DVL参数：总DVL刻度因子K、DVL与INS 的总方位安装角偏差α和总俯仰安装角偏差β，迭代计算公式如下：

K＝K₀×K₁  α＝α₀+α₁  β＝β₀+β₁

然后代入INS/DVL组合进行计算，记录当GNSS最后一次位置有效时的组合导航位置 (x_I4，y_I4，z_I4)和GNSS的位置信息(x_G4，y_G4，z_G4)，作为校准点5的信息；

(6)第四次GNSS校准：航行器开始下潜折返并匀速直航，当航行一定距离(如5Km) 时，进行第四次校准。当GNSS信息有效时，记录当GNSS位置第一次有效时且没有校准的 组合导航位置(x_I5，y_I5，z_I5)和GNSS位置(x_G5，y_G5，z_G5)作为校准点6的信息。根据校准点6组 合导航位置(x_I5，y_I5，z_I5)和GNSS位置(x_G5，y_G5，z_G5)信息，计算组合导航的位置误差，判断精 度是否满足要求，若满足要求，保存DVL参数，标定完成，否则根据校准点5和校准点6 记录的GNSS位置信息和组合导航位置信息，计算DVL参数，根据上次DVL参数计算总的 DVL参数，并保存DVL参数，标定完成。

作为本发明的进一步改进：

所述步骤(3)的具体流程为：

(a)分别计算组合导航和GNSS的校准点1到校准点2位置矢量：

R_INS/DVL＝(x_I1，y_I1，z_I1)^T-(x_I0，y_I0，z_I0)^T    (1)

R_GNss＝(x_G1，y_G1，z_G1)^T-(x_G0，y_G0，z_G0)^T    (2)

(b)根据位置矢量计算方位安装角偏差α₀和DVL刻度因子K₀：

$\sin {\alpha }_0=\frac{|R_{\mathrm{INS}/\mathrm{DVL}}\times R_{\mathrm{GNSS}}|}{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|\left|R_{\mathrm{GNSS}}\right|}---\left(3\right)$

从而可以得到方位角安装偏差α₀为：

${\alpha }_0=a\sin \left(\frac{|R_{\mathrm{INS}/\mathrm{DVL}}\times R_{\mathrm{GNSS}}|}{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|\left|R_{\mathrm{GNSS}}\right|}\right)---\left(4\right)$

DVL刻度因子K₀计算公式为：

$K_0=\frac{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|}{\left|R_{\mathrm{GNSS}}\right|}---\left(5\right)$

(c)根据DVL速度在天向投影的积分H_DVL，可以求得俯仰安装角偏差β₀如下：

${\beta }_0=a\tan \left(\frac{H_{\mathrm{DVL}}}{\left|R_{\mathrm{GNSS}}\right|}\right)---\left(6\right).$

所述步骤(6)的具体流程为：

(a)计算校准点6位置误差P_error：

$P_{\mathrm{error}}=\sqrt{{(x_{G5}-x_{I5})}^2+{(y_{G5}-y_{I5})}^2+{(z_{G5}-z_{I5})}^2}---\left(7\right)$

(b)误差的判断：如果P_error＜10m，则保存标定参数，标定完成。否则根据校准点5和 校准点6记录的GNSS位置信息和组合导航位置信息，按照公式(1)～(6)计算DVL参数， 根据上次DVL参数计算总的DVL参数，并保存DVL参数，标定完成。

与现有技术相比，本发明的优点在于：

1、本发明的DVL参数标定方法，不需要全程提供GNSS卫星信息，只需用开始点潜航 器上浮接收GNSS信号，然后下潜航行一段距离后，再上浮接收GNSS信息，根据一段距离 的首尾两点组合导航位置信息和GNSS位置信息计算DVL相关的标定参数，具有操作简单、 计算简单等优点。

2、本发明采用迭代方法求解惯导系统与DVL之间安装角偏差及DVL刻度因子，这样可 以提高安装角偏差及刻度因子估计精度，提高自主导航定位的精度。

附图说明

图1是本发明方法的流程示意图。

图2是本发明在执行时的原理示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1和图2所示，本发明的用于水下INS和DVL组合导航系统的DVL参数标定方法， 其流程为：

(1)第一次GNSS校准：水下航行器完成初始对准后，进入SINS/DVL组合导航状态， 浮出水面接收GNSS信号，进行第一次GNSS校准，并记录当GNSS最后一次位置有效时的 组合导航的基于地心地固坐标系下的位置(x_I0，y_I0，z_I0)和GNSS的基于地心地固坐标系下的 位置信息(x_G0，y_G0，z_G0)，作为的信息，即图2所示的校准点1；

(2)第二次GNSS校准：水下航行器匀速直航，并以INS/DVL组合导航状态工作，航 行一段距离(如大约8Km)后水下航行器上浮开始接收GNSS信号，进行第二次校准，记录 从第一个校准点开始到第二个校准点DVL天向投影速度的积分H_DVL，记录当GNSS位置第 一次有效时且没有校准的组合导航位置(x_I1，y_I1，z_I1)和GNSS位置(x_G1，y_G1，z_G1)，作为校准点2 的信息，即图2所示的校准点2；

(3)计算DVL参数：根据校准点1和校准点2记录的GNSS位置信息和组合导航位置 信息，计算DVL参数；即：DVL刻度因子K₀、DVL与INS的方位安装角偏差α₀和俯仰安 装角偏差β₀，然后代入INS/DVL组合导航进行计算，并记录当GNSS最后一次位置有效时 的组合导航的基于地心地固坐标系下的位置(x_I2，y_I2，z_I2)和GNSS的基于地心地固坐标系下 的位置信息(x_G2，y_G2，z_G2)，作为校准点3的信息，即图2所示的校准点3；

(a)分别计算组合导航和GNSS的校准点1到校准点2位置矢量：

R_INS/DVL＝(x_I1，y_I1，z_I1)^T-(x_I0，y_I0，z_I0)^T    (1)

R_GNSS＝(x_G1，y_G1，z_G1)^T-(x_G0，y_G0，z_G0)^T    (2)

(b)根据位置矢量计算方位安装角偏差α₀和DVL刻度因子K₀：

$\sin {\alpha }_0=\frac{|R_{\mathrm{INS}/\mathrm{DVL}}\times R_{\mathrm{GNSS}}|}{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|\left|R_{\mathrm{GNSS}}\right|}---\left(3\right)$

从而可以得到方位角安装偏差α₀为：

${\alpha }_0=a\sin \left(\frac{|R_{\mathrm{INS}/\mathrm{DVL}}\times R_{\mathrm{GNSS}}|}{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|\left|R_{\mathrm{GNSS}}\right|}\right)---\left(4\right)$

DVL刻度因子K₀计算公式为：

$K_0=\frac{\left|R_{\mathrm{INS}/\mathrm{DVL}}\right|}{\left|R_{\mathrm{GNSS}}\right|}---\left(5\right)$

(c)根据DVL速度在天向投影的积分H_DVL，可以求得俯仰安装角偏差β₀如下：

${\beta }_0=a\tan \left(\frac{H_{\mathrm{DVL}}}{\left|R_{\mathrm{GNSS}}\right|}\right)---\left(6\right)$

(4)按照图2进行第三次GNSS校准，并依照公式(1)～(6)计算DVL刻度因子K₁、 DVL与INS的方位安装角偏差α₁和俯仰安装角偏差β₁；即根据校准点3组合导航位置 (x_I2，y_I2，z_I2)和GNSS的位置信息(x_G2，y_G2，z_G2)及校准点4记录的组合导航位置(x_I3，y_I3，z_I3) 和GNSS的位置信息(x_G3，y_G3，z_G3)，校准点3开始到校准点4DVL速度在天向投影的积分 H_DVL，根据上述公式(1)至(6)计算DVL参数：DVL刻度因子K₁、DVL与INS的方位安 装角偏差α₁和俯仰安装角偏差β₁。

(5)根据上次DVL参数迭代计算总的DVL参数：总DVL刻度因子K、DVL与INS 的总方位安装角偏差α和总俯仰安装角偏差β，迭代计算公式如下：

K＝K₀×K₁  α＝α₀+α₁  β＝β₀+β₁    (7)

然后代入INS/DVL组合进行计算，记录当GNSS最后一次位置有效时的组合导航位置 (x_I4，y_I4，z_I4)和GNSS的位置信息(x_G4，y_G4，z_G4)，作为校准点5的信息；

(6)按照图2进行第四次GNSS校准，根据校准点6组合导航位置(x_I5，y_I5，z_I5)和GNSS 位置(x_G5，y_G5，z_G5)信息，计算组合导航的位置误差，判断精度是否满足要求，若满足要求， 保存DVL参数，标定完成，否则根据校准点5和校准点6记录的GNSS位置信息和组合导航 位置信息，计算DVL参数，根据上次DVL参数计算总的DVL参数，并保存DVL参数，标 定完成。

(a)计算校准点6位置误差P_error：

$P_{\mathrm{error}}=\sqrt{{(x_{G5}-x_{I5})}^2+{(y_{G5}-y_{I5})}^2+{(z_{G5}-z_{I5})}^2}---\left(8\right)$

(b)误差的判断：如果P_error＜10m，则保存标定参数，标定完成。否则根据校准点5和 校准点6记录的GNSS位置信息和组合导航位置信息，按照公式(1)～(6)计算DVL参数， 根据上次DVL参数按照公式(7)计算总的DVL参数，并保存DVL参数，标定完成。

由于惯导的基准坐标系和DVL基准坐标系的不统一及DVL速度存在刻度因子误差，从 而使得SINS/DVL组合的位置和真实的位置存在偏差，本发明则是根据位置偏差计算惯导系 统与DVL之间安装角偏差及DVL刻度因子。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于 本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术 人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。