CN114111840B - 一种基于组合导航的dvl误差参数在线标定方法 - Google Patents

Abstract

一种基于组合导航的DVL误差参数在线标定方法，涉及导航系统技术领域，针对现有技术中标定方法需要离线解算导致不能实时标定的问题，本申请可以在水下运载体正常运行过程中较为精确地标定DVL的标度因数和安装角度误差，同时可以补偿该误差为运载体提供较高精度的导航位置信息，可以广泛应用于具有DVL标定需求的水下运载体上。与其它标定方法相比，本申请具备以下4个优点。(1)无需固定航行轨迹，对水下运载体的控制要求低；(2)无需复杂的标定步骤，在水下运载体正常行驶过程中即可进行标定工作；(3)实时标定，无需离线解算，且标定时间极大缩短；(4)标定与导航同时进行，误差参数实时反馈组合导航系统，进而获得更高精度位置信息。

Description

一种基于组合导航的DVL误差参数在线标定方法

技术领域

本发明涉及导航系统技术领域，具体为一种基于组合导航的DVL误差参数在线标定方法。

背景技术

随着科学技术和社会文明的飞速发展，人类对海洋资源开发与利用的需求日益高涨。作为开发利用海洋资源、勘探监测海洋环境、保障执行海洋军事任务的关键技术，高精度水下导航与定位技术已经成为各国研究的热点。

目前在水下导航领域主要依靠基于加速度计和陀螺的惯性导航(INS)，基于声学的多普勒测速仪(DVL)，基于声学信标的长基线导航(LBL)和超短基线导航(USBL)等方式。惯性导航具有隐蔽性强、输出频率高、不受外界干扰等优点，但导航精度会随时间降低，需要定期修正；基于声学信标的长基线和超短基线导航不会随着时间漂移，但输出频率较低且精度较差。而采用罗经、DVL、LBL和深度计相结合的组合导航方式，能够结合各导航方式的优点，为运载体提供输出频率高、导航结果不漂移的高精.度定位信息。

水下组合导航由于结合了多种传感器设备，则必然存在各传感器量测数据如何有效融合的问题。其中，DVL在装配过程中，受限于载体结构与加工精度，其量测坐标系无法与运载体坐标系完全重合，需要标定并补偿安装偏角和测速标度因数等误差。

目前对于DVL的传统标定方法主要采用直线或圆形等固定航行轨迹，并以GPS量测的速度信息作为参考，在航行结束之后进行安装偏角和标度因数的解算。该方法对航行轨迹有较高要求，增加了运载体的控制难度。最近提出的无模型多普勒计程仪DVL误差标定方法无需固定航行轨迹，但需要庞大的数据样本对预测器参数进行训练。以上方法标定所需的时间较长，操作步骤繁琐，且均为离线解算，在水下导航应用中仍然存在很大局限性。

发明内容

本发明的目的是：针对现有技术中标定方法需要离线解算导致不能实时标定的问题，提出一种基于组合导航的DVL误差参数在线标定方法。

本发明为了解决上述技术问题采取的技术方案是：

一种基于组合导航的DVL误差参数在线标定方法，包括以下步骤：

步骤一：基于罗经的姿态角量测信息以及DVL的速度量测信息进行运载体航位推算，所述运载体航位推算包括坐标变换和位置解算；

步骤二：以X＝[δk″ θ′_dz δL δλ δh]^T作为状态变量、以LBL的位置量测信息和深度计的深度量测信息为观测值进行滤波，

其中，

δk″为DVL的标度因数误差的变体，θ′_dz为DVL的z轴安装角度误差的变体，δL、δλ、δh分别为运载体的纬度误差、运载体的经度误差和运载体的高度误差，δk为DVL的标度因数误差，θ_dz为DVL的z轴安装角度误差；

步骤三：利用滤波结果矫正航位推算，进而得到运载体的精确定位以及DVL误差参数。

进一步的，所述滤波为卡尔曼滤波。

进一步的，所述步骤一的具体步骤为；

步骤一一：获取来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差；

步骤一二：利用来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差进行航位推算，得到包含DVL误差变量的位置递推信息。

进一步的，所述滤波的具体步骤为；

步骤二一：获取来自LBL的位置量测信息、来自深度计的深度量测信息以及来自运载体航位推算的包含DVL误差变量的位置递推信息；

步骤二二：将来自LBL的位置量测信息、来自深度计的深度量测信息以及来自航位推算的包含DVL误差变量的位置递推信息输入卡尔曼滤波器中，得到DVL标度因数、安装角度误差以及位置误差信息。

进一步的，所述步骤三的具体步骤为；

步骤三一：利用DVL标度因数、安装角度误差以及位置误差信息对包含DVL误差变量的位置递推信息矫正运载体航位推算，进而得到运载体的精确定位以及DVL误差参数。

进一步的，所述运载体航位推算具体包括以下步骤：

步骤1：获取运载体上一时刻所处的经度信息、纬度信息以及高度信息；

步骤2：由罗经得到当前的量测姿态角，表示为：

其中，

θ(k)、γ(k)分别为k时刻的航向角、俯仰角和滚转角，

根据当前的量测姿态角得到由机体坐标系到导航坐标系的坐标转换矩阵，由机体坐标系到导航坐标系的坐标转换矩阵表示为：

其中，γ为滚转角、k为k时刻、θ为俯仰角、

为航向角；

由DVL得到当前的量测速度，表示为：

其中，

分别为k时刻DVL量测坐标系下x轴速度、y轴速度和z轴速度，

根据当前的量测速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为：

V^nd(k)＝[v_E(k) v_N(k) v_U(k)]^T＝C(k)·V^d(k)

其中，v_E(k)、v_N(k)、v_U(k)分别为k时刻导航坐标系下的东向速度、北向速度和天向速度；

步骤3：根据运载体在导航坐标系下速度以及经纬高数据的转化矩阵得到当前时刻导航系下位置增量，

所述经纬高数据的转化矩阵表示为：

其中，T_s为Ts采样时间、R_n为子午圈曲率半径、R_m为卯酉圈曲率半径、secL为正割运算、L(k-1)为k-1时刻纬度，

当前时刻导航系下位置增量表示为：

d_Pose(k)＝[d_L(k) d_λ(k) d_h(k)]^T＝M·V^nd(k)

其中，d_L(k)、d_λ(k)、d_h(k)分别为k时刻的纬度增量、经度增量和高度增量；

步骤4：由运载体上一时刻所处的经度、纬度、高度和当前时刻导航系下位置增量得到当前的航位推算结果，当前的航位推算结果表示为：

其中，

分别为k时刻航位推算纬度、经度和高度。

进一步的，所述卡尔曼滤波的具体步骤为：

步骤1：将状态变量X和协方差矩阵P的初值进行设定，状态变量X和协方差矩阵P表示为：

X(0)＝[δk″(0) θ′_dz(0) δL(0) δλ(0) δh(0)]^T＝0_5×1

P(0)＝I_5×5

其中，I_5×5为5维单位矩阵；

步骤2：获取上一时刻的状态变量，上一时刻的状态变量表示为：

X(k-1)＝[δk″(k-1) θ′_dz(k-1) δL(k-1) δλ(k-1) δh(k-1)]^T

同时获取上一时刻的协方差矩阵P(k-1)；

步骤3：获取上一时刻的DVL的安装误差角和标度因数误差，并根据上一时刻的DVL的安装误差角和标度因数误差得到DVL误差矩阵，DVL误差矩阵表示为：

步骤4：由DVL得到当前的量测速度，当前的量测速度表示为：

由当前的量测速度和DVL误差矩阵得到当前的载体坐标系速度，当前的载体坐标系速度表示为：

其中，

分别为k时刻载体坐标系下x轴速度、y轴速度和z轴速度；

步骤5：根据当前的载体坐标系速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为

步骤6：对当前的量测速度V^d(k)进行矩阵化，表示为：

然后结合当前罗经量测结果得到速度误差转移矩阵，速度误差转移矩阵表示为：

D_tk＝C(k)·M_V^d(k)；

步骤7：根据速度误差转移矩阵得到当前系统矩阵F_k/k-1，当前系统矩阵F_k/k-1表示为：

其中，L为纬度；

步骤8：根据当前系统矩阵F_k/k-1进行状态一步预测，得到状态变量的估计值，状态变量的估计值表示为：

步骤9：根据运载体上一时刻所处的经度信息得到当前噪声矩阵G_k-1，当前噪声矩阵G_k-1表示为：

其中，

为导航坐标系速度误差转换矩阵，

N₁矩阵表示为：

矩阵表示为：

步骤10：根据上一时刻的协方差矩阵P(k-1)、当前噪声矩阵G_k-1以及当前系统矩阵F_k/k-1得到当前时刻的协方差矩阵估计值P_k/k-1，当前时刻的协方差矩阵估计值P_k/k-1表示为：

其中Q为DVL量测噪声协方差矩阵，σ_DVLx、σ_DVLy、σ_DVLz分别为DVL速度测量过程中的噪声标准差；

步骤11：获取当前时刻的长基线和深度计的测量值，并根据当前时刻的长基线和深度计的测量值得到当前时刻的位置量测值，当前时刻的位置量测值表示为：

其中，

分别为LBL量测的纬度、LBL量测的经度和深度计量测的深度，

根据当前时刻的位置量测值和航位推算结果得到观测值，观测值表示为：

步骤12：根据当前时刻的协方差矩阵估计值P_k/k-1得到滤波增益K_k，滤波增益K_k表示为：

其中，量测矩阵H_k表示为：

量测噪声方差矩阵R表示为：

其中σ_LBLL、σ_LBLλ和σ_DEP分别为长基线的纬度量测噪声标准差、长基线的经度量测噪声标准差和深度计量测标准差；

步骤13：根据状态变量的估计值、滤波增益K_k和观测值进行状态修正，得到当前时刻的状态变量，当前时刻的状态变量表示为：

步骤14：根据滤波增益K_k、量测矩阵H_k以及当前时刻的协方差矩阵估计值P_k/k-1进行协方差阵修正，得到当前时刻的协方差矩阵，当前时刻的协方差矩阵表示为：

P_k＝(I-K_k·H_k)P_k/k-1。

进一步的，所述步骤三中利用滤波结果矫正航位推算表示为：

其中δ_Pose(k)表示为：

δ_Pose(k)＝[δL(k) δλ(k) δh(k)]^T＝X(k)_3:5

其中，L(k)、λ(k)、h(k)分别为k时刻校正后的航位纬度、经度和高度，δL(k)、δλ(k)、δh(k)分别为k时刻的纬度误差、经度误差和高度误差。

进一步的，所述DVL误差参数表示为：

本发明的有益效果是：

本申请可以在水下运载体正常运行过程中较为精确地标定DVL的标度因数和安装角度误差，同时可以补偿该误差为运载体提供较高精度的导航位置信息，可以广泛应用于具有DVL标定需求的水下运载体上。与其它标定方法相比，本申请具备以下4个优点。

(1)无需固定航行轨迹，对水下运载体的控制要求低；

(2)无需复杂的标定步骤，在水下运载体正常行驶过程中即可进行标定工作；

(3)实时标定，无需离线解算，且标定时间极大缩短；

(4)标定与导航同时进行，误差参数实时反馈组合导航系统，进而获得更高精度位置信息。

附图说明

图1为本申请结构示意图；

图2为本申请具体流程图；

图3为仿真轨迹示意图；

图4为航位推算示意图1；

图5为航位推算示意图2；

图6为航位推算示意图3；

图7为定位误差示意图1；

图8为定位误差示意图2；

图9为定位误差示意图3；

图10为DVL误差参数示意图1；

图11为DVL误差参数示意图2；

图12为KF、DR定位误差对比图1；

图13为KF、DR定位误差对比图2；

图14为KF、DR定位误差对比图3。

具体实施方式

需要特别说明的是，在不冲突的情况下，本申请公开的各个实施方式之间可以相互组合。

具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种基于组合导航的DVL误差参数在线标定方法，包括以下步骤：

步骤一：基于罗经的姿态角量测信息以及DVL的速度量测信息进行运载体航位推算，所述运载体航位推算包括坐标变换和位置解算；

步骤二：以X＝[δk″ θ′_dz δL δλ δh]^T作为状态变量、以LBL的位置量测信息和深度计的深度量测信息为观测值进行滤波，

其中，

δk″为DVL的标度因数误差的变体，θ′_dz为DVL的z轴安装角度误差的变体，δL、δλ、δh分别为运载体的纬度误差、运载体的经度误差和运载体的高度误差，δk为DVL的标度因数误差，θ_dz为DVL的z轴安装角度误差；

步骤三：利用滤波结果矫正航位推算，进而得到运载体的精确定位以及DVL误差参数。

本申请公开了一种罗经、DVL、LBL和深度计的组合导航系统，并基于此提出了DVL的标度因数和z轴安装角度误差的在线标定方法。组合导航与标定系统由航位推算和滤波器两部分组成。航位推算部分由罗经提供姿态解算，由DVL提供带误差变量的速度量测，经过坐标变换与积分解算得到航位推算位置信息；滤波器部分由LBL和深度计共同提供位置量测信息，结合航位推算位置信息设计卡尔曼滤波器解算位置误差，同时估计DVL的误差参数。该标定方法无需确定的运动模型，无需特定的航行轨迹，无需繁琐的操作步骤，无需庞大的数据样本，运载体在水下正常航行的过程中即可完成DVL相关误差参数的标定，并且误差参数可以实时解算并反馈给导航系统从而为运载体提供更高精度的定位信息。

组合导航系统由航位推算和卡尔曼滤波两部分组成，结构示意图如图1所示：

航位推算的输入主要由三部分组成，分别为来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差。航位推算的输出为包含DVL误差变量的位置递推信息。

卡尔曼滤波的输入主要由三部分组成，分别为来自LBL的位置量测信息、来自深度计的深度量测信息以及来自航位推算的位置递推信息。卡尔曼滤波的输出为DVL标度因数和安装角度误差以及位置误差信息。

航位推算输出的位置递推信息经过卡尔曼滤波输出的位置误差信息补偿后作为组合导航系统的导航位置输出，该步骤即为航位校准。航位推算和卡尔曼滤波通过航位校准形成闭环，从而修正航位推算过程中的当前位置。

以X＝[δk″ θ′_dz δL δλ δh]^T作为系统状态进行滤波，同时进行基于DVL量测速度的航位推算，最终通过滤波结果矫正航位推算，获取精确定位和DVL误差参数。

滤波算法设计

系统原理框图如图1所示，由罗经和DVL组成的航位推算系统进行水下无人设备的位置更新与迭代，由LBL和深度计组成的量测系统进行水下无人设备的位置量测，以上两个系统通过KF获得位置信息校准，并最终对航位推算结果进行校准与更新，从而获得水下无人设备的长时间高精度定位。

系统方程

根据前面所分析的误差方程，选取状态为X＝[δk″ θ′_dz δL δλ δh]^T，则有：

其中，d_ij为D(i,j)，则系统的状态方程为：

对该系统矩阵进行离散化则有：

X(k)＝F_k/k-1X(k-1)

简化之后的系统矩阵F为：

其中d_ij为

简化之后的等效离散化系统矩阵为：

考虑系统噪声W则有：

X(k)＝F_k/k-1·X(k-1)+G_k-1·W_k-1

W_k-1＝[W_DVLx W_DVLy W_DVLz]^T

量测方程

其中，深度计的δh_DEP几乎为0；V为经纬高环境下的LBL与深度计测量噪声。

具体流程如图2所示：

由于卡尔曼滤波器的状态选择为经度、纬度和高度的误差，因此需要进行实时航位推算，以航位推算计算得到的经度、纬度和高度信息为基准，以卡尔曼滤波估计得到的经度、纬度和高度误差为修正量，最终得到最优的经度、纬度和高度信息以及DVL的安装误差角和标度因数误差。对流程图右侧蓝色部分进行进一步解释如下所示：

(1)航位推算初始化

航位推算初始化是指将水下无人设备的初始位置、地球曲率半径和DVL误差参数进行设定，即：

Pose(0)＝[L(0) λ(0) h(0)]^T

Re(0)＝[R_n(0) R_m(0)]^T

ε(0)＝[δk″(0) θ′_dz(0)]^T

(2)状态更新

状态更新是指将导航系统上一时刻所处的经度、纬度和高度，地球曲率半径以及DVL的安装误差角和标度因数误差进行更新，用以进行当前时刻的推算：

Pose(k-1)＝[L(k-1) λ(k-1) h(k-1)]^T

ε(k-1)＝[δk″(k-1) θ′_dz(k-1)]^T

计算当前时刻地球曲率半径如下所示：

Re(k)＝[R_m(k) R_n(k)]^T

(3)基于DVL的导航系速度计算

由罗经得到当前的量测姿态角为：

则可以得到由机体坐标系到导航坐标系的坐标转换矩阵如下所示：

由DVL得到当前的量测速度为：

则进行导航系速度推算如下：

V^nd(k)＝[v_E(k) v_N(k) v_U(k)]⁾＝C(k)·V^d(k)

(4)位置变化量解算

计算基于DVL的导航系速度到经纬高数据的转化矩阵如下所示：

由上一步计算得到的当前导航系速度可以计算得到当前时刻导航系下位置增量：d_Pose(k)＝[d_L(k) d_λ(k) d_h(k)]^T＝M·V^nd(k)

(5)航位推算

由上一时刻的位置信息和计算得到的当前位置变化量可以计算得到当前的航位推算结果：

选取DVL标度因数误差和安装角误差以及经度、纬度和高度的误差作为卡尔曼滤波器的状态，进行一步预测与量测更新。对流程图左侧橙色部分进行进一步解释如下所示：

(1)卡尔曼滤波初始化

卡尔曼滤波初始化是指将状态变量X和协方差矩阵P的初值进行设定：

X(0)＝[δk″(0) θ′_dz(0) δL(0) δλ(0) δh(0)]^T＝0_5×1

P(0)＝I_5×5

(2)状态更新

获取上一时刻的状态变量：

X(k-1)＝[δk″(k-1) θ′_dz(k-1) δL(k-1) δλ(k-1) δh(k-1)]^T

同时获取协方差矩阵P(k-1)，为一步预测进行数据准备。

(3)一步预测

一步预测是指根据状态方程进行状态变量由k-1时刻到k时刻的估计，以时序先后顺序依次进行：

DVL误差矩阵计算

根据上一时刻的DVL的安装误差角和标度因数误差求得DVL误差矩阵的逆矩阵如下所示：

机体坐标系速度解算

由DVL得到当前的量测速：

再由DVL误差逆矩阵可得到当前的载体坐标系速度：

导航坐标系速度解算

根据当前的量测姿态角信息C(k)可进一步获得当前的导航坐标系速度：

计算速度误差转移矩阵

对DVL得到当前的量测速度V^d(k)进行矩阵化如下所示：

则根据当前罗经量测结果可以计算得到：

计算当前系统矩阵F_k/k-1

其中d_ij为

L(t_k-1)、R_m(k)、R_n(k)在航位推算中已经进行了计算。

状态一步预测

计算当前噪声矩阵G_k-1

计算N₁矩阵：

计算

矩阵：

计算噪声矩阵G_k-1：

1)协方差矩阵P_k/k-1

其中Q为DVL量测噪声协方差矩阵，σ_DVLx、σ_DVLy、σ_DVLz分别为DVL速度测量过程中的噪声标准差。

量测更新

获取观测值Z_k

当前时刻长基线和深度计的测量值为：

当前航位推算结果为：

将两者的差值作为观测值则有：

计算量测矩阵H_k

计算量测噪声方差矩阵R

其中σ_LBLL、σ_LBLλ和σ_DEP分别为长基线的纬度量测噪声标准差、长基线的经度量测噪声标准差和深度计量测标准差。

计算滤波增益K_k

状态修正

协方差阵修正

P_k＝(I-K_k·H_k)P_k/k-1

在获得修正后的状态变量X(k)之后，经度、纬度和高度误差数据δ_Pose(k)为：

δ_Pose(k)＝[δL(k) δλ(k) δh(k)]^T＝X(k)_3:5

则对航位推算的结果进行校准如下所示：

在滤波器收敛之后可以最终得到DVL误差参数ε为：

ε＝[δk″ θ′_dz]^T＝X(n)_1:2

则对误差参数进行解算有：

仿真图如图3所示，初始条件为：

航位推算仿真结果如图4-6所示，定位误差如图7-9所示，DVL误差参数如图10和图11所示。

由仿真结果可以得出：

组合导航定位误差在水平面内保持在10^-3量级，基本满足定位精度。

DVL标度因数估计值为0.0152，设定值为0.015；DVL安装误差角估计值为0.872°，设定值为0.9°。基本满足标定精度。

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。

Claims (7)

1.一种基于组合导航的DVL误差参数在线标定方法，其特征在于包括以下步骤：

步骤一：基于罗经的姿态角量测信息以及DVL的速度量测信息进行运载体航位推算，所述运载体航位推算包括坐标变换和位置解算；

步骤二：以X＝[δk″ θ′_dz δL δλ δh]^T作为状态变量、以LBL的位置量测信息和深度计的深度量测信息为观测值进行滤波，

其中，

δk″为DVL的标度因数误差的变体，θ′_dz为DVL的z轴安装角度误差的变体，δL、δλ、δh分别为运载体的纬度误差、运载体的经度误差和运载体的高度误差，δk为DVL的标度因数误差，θ_dz为DVL的z轴安装角度误差；

步骤三：利用滤波结果矫正航位推算，进而得到运载体的精确定位以及DVL误差参数；

所述滤波为卡尔曼滤波；

所述卡尔曼滤波的具体步骤为：

步骤1：将状态变量X和协方差矩阵P的初值进行设定，状态变量X和协方差矩阵P表示为：

X(0)＝[δk″(0) θ′_dz(0) δL(0) δλ(0) δh(0)]^T＝0_5×1

P(0)＝I_5×5

其中，I_5×5为5维单位矩阵；

步骤2：获取上一时刻的状态变量，上一时刻的状态变量表示为：

X(k-1)＝[δk″(k-1) θ′_dz(k-1) δL(k-1) δλ(k-1) δh(k-1)]^T

同时获取上一时刻的协方差矩阵P(k-1)；

步骤3：获取上一时刻的DVL的安装误差角和标度因数误差，并根据上一时刻的DVL的安装误差角和标度因数误差得到DVL误差矩阵，DVL误差矩阵表示为：

步骤4：由DVL得到当前的量测速度，当前的量测速度表示为：

由当前的量测速度和DVL误差矩阵得到当前的载体坐标系速度，当前的载体坐标系速度表示为：

其中，

分别为k时刻载体坐标系下x轴速度、y轴速度和z轴速度，

分别为k时刻DVL量测坐标系下x轴速度、y轴速度和z轴速度；

步骤5：根据当前的载体坐标系速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为

v_E(k)、v_N(k)、v_U(k)分别为k时刻导航坐标系下的东向速度、北向速度和天向速度；

步骤6：对当前的量测速度V^d(k)进行矩阵化，表示为：

然后结合当前罗经量测结果得到速度误差转移矩阵，速度误差转移矩阵表示为：

步骤7：根据速度误差转移矩阵得到当前系统矩阵F_k/k-1，当前系统矩阵F_k/k-1表示为：

其中，L为纬度，T_s为采样时间、R_n为子午圈曲率半径、R_m为卯酉圈曲率半径、secL为正割运算、L(k-1)为k-1时刻纬度，d_ij为

步骤8：根据当前系统矩阵F_k/k-1进行状态一步预测，得到状态变量的估计值，状态变量的估计值表示为：

步骤9：根据运载体上一时刻所处的经度信息得到当前噪声矩阵G_k-1，当前噪声矩阵G_k-1表示为：

其中，

为导航坐标系速度误差转换矩阵，

N₁矩阵表示为：

矩阵表示为：

步骤10：根据上一时刻的协方差矩阵P(k-1)、当前噪声矩阵G_k-1以及当前系统矩阵F_k/k-1得到当前时刻的协方差矩阵估计值P_k/k-1，当前时刻的协方差矩阵估计值P_k/k-1表示为：

其中Q为DVL量测噪声协方差矩阵，σ_DVLx、σ_DVLy、σ_DVLz分别为DVL速度测量过程中的噪声标准差；

步骤11：获取当前时刻的长基线和深度计的测量值，并根据当前时刻的长基线和深度计的测量值得到当前时刻的位置量测值，当前时刻的位置量测值表示为：

其中，

分别为LBL量测的纬度、LBL量测的经度和深度计量测的深度，

根据当前时刻的位置量测值和航位推算结果得到观测值，观测值表示为：

为当前的航位推算结果；

步骤12：根据当前时刻的协方差矩阵估计值P_k/k-1得到滤波增益K_k，滤波增益K_k表示为：

其中，量测矩阵H_k表示为：

量测噪声方差矩阵R表示为：

其中σ_LBLL、σ_LBLλ和σ_DEP分别为长基线的纬度量测噪声标准差、长基线的经度量测噪声标准差和深度计量测标准差；

步骤13：根据状态变量的估计值、滤波增益K_k和观测值进行状态修正，得到当前时刻的状态变量，当前时刻的状态变量表示为：

步骤14：根据滤波增益K_k、量测矩阵H_k以及当前时刻的协方差矩阵估计值P_k/k-1进行协方差阵修正，得到当前时刻的协方差矩阵，当前时刻的协方差矩阵表示为：

P_k＝(I-K_k·H_k)P_k/k-1。

2.根据权利要求1所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述步骤一的具体步骤为；

步骤一一：获取来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差；

步骤一二：利用来自罗经的姿态角量测信息、来自DVL的速度量测信息以及来自KF的估计结果标度因数和安装角度误差进行航位推算，得到包含DVL误差变量的位置递推信息。

3.根据权利要求2所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述滤波的具体步骤为；

步骤二一：获取来自LBL的位置量测信息、来自深度计的深度量测信息以及来自运载体航位推算的包含DVL误差变量的位置递推信息；

步骤二二：将来自LBL的位置量测信息、来自深度计的深度量测信息以及来自航位推算的包含DVL误差变量的位置递推信息输入卡尔曼滤波器中，得到DVL标度因数、安装角度误差以及位置误差信息。

4.根据权利要求3所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述步骤三的具体步骤为；

步骤三一：利用DVL标度因数、安装角度误差以及位置误差信息对包含DVL误差变量的位置递推信息矫正运载体航位推算，进而得到运载体的精确定位以及DVL误差参数。

5.根据权利要求4所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述运载体航位推算具体包括以下步骤：

步骤1：获取运载体上一时刻所处的经度信息、纬度信息以及高度信息；

步骤2：由罗经得到当前的量测姿态角，表示为：

其中，

θ(k)、γ(k)分别为k时刻的航向角、俯仰角和滚转角，

根据当前的量测姿态角得到由机体坐标系到导航坐标系的坐标转换矩阵，由机体坐标系到导航坐标系的坐标转换矩阵表示为：

其中，γ为滚转角、k为k时刻、θ为俯仰角、

为航向角；

由DVL得到当前的量测速度，表示为：

其中，

分别为k时刻DVL量测坐标系下x轴速度、y轴速度和z轴速度，

根据当前的量测速度以及由机体坐标系到导航坐标系的坐标转换矩阵得到运载体在导航坐标系下速度，运载体在导航坐标系下速度表示为：

V^nd(k)＝[v_E(k) v_N(k) v_U(k)]^T＝C(k)·V^d(k)

其中，v_E(k)、v_N(k)、v_U(k)分别为k时刻导航坐标系下的东向速度、北向速度和天向速度；

步骤3：根据运载体在导航坐标系下速度以及经纬高数据的转化矩阵得到当前时刻导航系下位置增量，

所述经纬高数据的转化矩阵表示为：

其中，T_s为Ts采样时间、R_n为子午圈曲率半径、R_m为卯酉圈曲率半径、secL为正割运算、L(k-1)为k-1时刻纬度，

当前时刻导航系下位置增量表示为：

d_Pose(k)＝[d_L(k) d_λ(k) d_h(k)]^T＝M·V^nd(k)

其中，d_L(k)、d_λ(k)、d_h(k)分别为k时刻的纬度增量、经度增量和高度增量；

步骤4：由运载体上一时刻所处的经度、纬度、高度和当前时刻导航系下位置增量得到当前的航位推算结果，当前的航位推算结果表示为：

其中，

分别为k时刻航位推算纬度、经度和高度。

6.根据权利要求1所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述步骤三中利用滤波结果矫正航位推算表示为：

其中δ_Pose(k)表示为：

δ_Pose(k)＝[δL(k) δλ(k) δh(k)]^T＝X(k)_3：5

其中，L(k)、λ(k)、h(k)分别为k时刻校正后的航位纬度、经度和高度，δL(k)、δλ(k)、δh(k)分别为k时刻的纬度误差、经度误差和高度误差。

7.根据权利要求6所述的一种基于组合导航的DVL误差参数在线标定方法，其特征在于所述DVL误差参数表示为：

Images