CN1325932C - 载人潜水器的组合导航定位方法 - Google Patents

Abstract

一种载人潜水器的组合导航定位方法，取运动传感器和多普勒测速声纳的速度值，通过第一级卡尔曼滤波器综合，得最优载人潜水器速度值；由卡尔曼滤波器综合出载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值；取超短基线定位声纳测量位置值，将超短基线定位声纳测量的位置值和推算出的载人潜水器位置值通过第二级卡尔曼滤波器综合，得最优载人潜水器位置值；采用延时超短基线测量数据融合方法，实现用过去的超短基线测量载人潜水器位置数据，对现在载人潜水器位置推算数据的矫正；不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标。本发明可把多路导航设备测量得到的载人潜水器的位置和姿态信息，进行综合处理，得最优导航信息。

Description

载人潜水器的组合导航定位方法

技术领域

本发明涉及载人潜水器在海洋中定位技术，具体的说是把多种导航设备的信号进行综合处理后，估算出一个平滑的最接近真实的载人潜水器位置信息的载人潜水器的组合导航定位方法。

技术背景

载人潜水器在海洋中航行，实时需要知道自己当前的位置和姿态信息。目前，载人潜水器导航定位***的功能包括：确定载人潜水器相对作业母船的位置、载人潜水器在惯性导航坐标系中的位置、载人潜水器的姿态(包括航向、深度、纵倾、高度)。

载人潜水器和自治水下机器人(AUV)统称为无缆水下机器人，现在的无缆水下机器人导航定位有多种办法，也有取多路导航信号的导航方法。但是，他们往往是简单的导航信号备份，而不是进行信号的融合。

发明内容

本发明的目的是提供一种载人潜水器的组合导航定位方法，把多路导航设备测量得到的载人潜水器的位置和姿态信息，进行综合处理，得到最优的导航信息。

为了实现上述目的，本发明的技术方案是按如下步骤操作：1)取运动传感器和多普勒测速声纳的速度值，通过第一级卡尔曼滤波器综合，得到最优的载人潜水器速度值；2)由卡尔曼滤波器综合出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值；3)取超短基线定位声纳测量的位置值，将超短基线定位声纳测量的位置值和推算出的载人潜水器位置值通过第二级卡尔曼滤波器综合，得到最优的载人潜水器位置值；4)采用延时超短基线测量数据融合方法，实现用过去的超短基线测量载人潜水器位置数据，对现在载人潜水器位置推算数据的矫正；5)不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标；

所述由推算出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值的步骤具体如下；

先确定载人潜水器起始位置的经纬度坐标，采用固定坐标系(E-XYZ)，其中以起始点为原点E，EZ指向地心，水平面采用北东坐标系；再定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起，将ox轴与载人潜水器的主对称轴取向一致，而oy轴与辅助对称轴取向一致，u、v表示载人潜水器沿动坐标系两维速度；载人潜水器在下一时刻的位置值推算公式为：

X_k+1＝X_k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts

Y_k+1＝Y_k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts  (12)

所述θ为载人潜水器在固定坐标系中的航向角，ψ为载人潜水器在固定坐标系中的纵倾角，k时刻的载人潜水器的位置为X_k，Y_k，运动坐标系中的速度值为u(k)，v(k)，T_s为位置时延；

所述航向角θ、纵倾角ψ、两维速度值u，v是载人潜水器的导航设备数据通过卡尔曼滤波器的数据综合得到；其中所述超短基线定位声纳测量到的载人潜水器的位置值为卡尔曼滤波器的新值；

步骤4)所述延时超短基线测量数据融合方法具体为：

设超短基线定位声纳测量的载人潜水器位置信息中的声波在水中的传播时间长度为t＝d*T_s，在t＝k*T_s时刻水下超短基线水声信标向超短基线定位声纳的收发换能器发出应答信号，载人潜水器在声波在水中传输的时延t＝(k+d)*T_s接收到k时刻的载人潜水器位置信息X_km，Y_km；在t＝(k+d)*T_s时刻载人潜水器的位置估计式为：

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1){\]T}_s$

$Y_{k+d}=Y_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (13)；

其中，d为时间间隔，T_s为时间计量单位；k为第k次测量；X_k，Y_k为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，ψ是载人潜水器的纵倾角；令：

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (14)；

则根据k时刻测量值X_km，Y_km和k时刻估计值X_k，Y_k，应用卡尔曼滤波公式，得到k时刻的最优估计

，进一步获得(k+d)时刻载人潜水器位置的最优估计值为：

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)+\mathrm{\Δ}{X}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+\mathrm{\Δ}{Y}_d$ (15)；

其中X，Y每隔d*T_s秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔T_s秒刷新一次；所述不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标的确定方法是将载人潜水器的固定坐标转化到惯性导航坐标系中，用经纬度表示载人潜水器的坐标。具体为：确定固定坐标系(E-XYZ)，其中以母船为原点E，EZ指向地心，水平面采用北东坐标系；将固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标；载人潜水器在惯性坐标系中的经纬度坐标值计算式为：

(16)；

其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，X，Y是载人潜水器在固定坐标系中的坐标值，R为地球半径。

本发明提出的载人潜水器组合导航定位方法是选取多路的导航信号，然后把多路导航信号进行数据综合，得到新的比每一路的导航信号都优的导航数据估算值，估算出的导航数据即平滑又更接近实际值。

本发明具有如下优点：

1、本发明提出了一种载人潜水器的组合导航方法。这种方法是把各路的导航信号通过数字滤波器进行综合，而不是导航信号的简单的备份。这种组合导航方法得到的载人潜水器的导航信号，形成连续、稳定的导航数据，即平滑又具有界性，最大的接近载人潜水器的实际航行位置值。

2、本发明提出了延时超短基线测量数据融合方法。

3、本发明使用一种新型的确定载人潜水器在惯性坐标系中坐标的方法，免去在海中投放固定水声信标的麻烦。

附图说明

图1载人潜水器导航定位***组成示意图。

图2导航***数据流结构图。

图3卡尔曼滤波器结构图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

载人潜水器是无缆潜水器，载人潜水器与作业母船通过水声通讯机联系。载人潜水器的导航定位***由水上、水下两部分组成，水上部分包括：水面监控计算机(含计算机数据处理***，载人潜水器状态信息显示***)，超短基线定位声纳数据处理***和超短基线定位声纳的收发换能器，GPS(全球定位***)接收机，水声通讯机的水上***。GPS接收机可以确定母船在惯性坐标系中的坐标，通过水声通讯机将母船在惯性坐标系中的坐标传送给水下载人潜水器。超短基线定位声纳数据处理***、超短基线定位声纳的收发换能器和水下载人潜水器上的水声信标组成独立水声定位***，可以测量到载人潜水器与母船相对位置。导航定位***的水下部分包括：水下导航计算机和各种水下导航设备和传感器，导航设备以运动传感器为主，辅助以多普勒测速声纳、数字罗盘、光纤陀螺、超短基线定位声纳的水下信标、深度计、避碰及测高声纳、倾角仪等组成组合导航***。各路导航数据输入到水下导航计算机，通过卡尔曼滤波器(为本发明计算机程序)进行最优数据综合，为载人潜水器驾驶员提供高精度、连续的导航数据。

载人潜水器导航定位***的水上和水下两部分组成如图1所示，其中：

运动传感器是惯性导航设备，用于测量载体的三维运动参数，提供潜水器3个轴向速度和加速度分量，提供艏摇、横摇、纵摇3个角速度分量。

所述载人潜水器导航定位***所获得的导航数据以运动传感器数据为主，辅助以多普勒测速声纳、数字罗盘、GPS接收机、超短基线定位声纳(含超短基线定位声纳数据处理***、超短基线定位声纳的收发换能器及水声信标)、光纤陀螺、倾角仪、深度计、避碰及测高声纳等导航设备数据。为了进行准确的载人潜水器的位置估计，本发明对上述数据进行筛选和综合。图2载人潜水器导航***的数据流结构图。

本发明载人潜水器的组合导航定位方法：1)取运动传感器和多普勒测速声纳的速度值，通过第一级卡尔曼滤波器综合，得到最优的载人潜水器速度值；2)由卡尔曼滤波器综合出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值；3)取超短基线定位声纳测量的位置值，将超短基线定位声纳测量的位置值和推算出的载人潜水器位置值通过第二级卡尔曼滤波器综合，得到最优的载人潜水器位置值；4)采用延时超短基线测量数据融合方法，实现用过去的超短基线测量载人潜水器位置数据，对现在载人潜水器位置推算数据的矫正；5)不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标。具体为：

1)取运动传感器和多普勒测速声纳的速度值，通过第一级卡尔曼滤波器综合，得到最优的载人潜水器速度值；

运动传感器的输出值的精度与所在的纬度有关系，需要把运动传感器所在的纬度值输入到运动传感器中。运动传感器可以输出载人潜水器的三维加速度和速度值，但是速度值的精度不是很高，而多普勒测速声纳由于环境的影响不是总可以测到载人潜水器速度数据，而且在一些情况下，其数据的精度不是很好。需要把运动传感器与多普勒测速声纳的测量速度值通过第一级卡尔曼滤波器进行数据综合。综合后的载人潜水器的速度值和载人潜水器的水平航向角与纵倾角一起用于载人潜水器的航迹推算。

2)由卡尔曼滤波器综合出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值；

载人潜水器在航位推算之前，先确定起始位置，即确定载人潜水器起始位置的经纬度坐标，采用固定坐标系(E-XYZ)，其中以载人潜水器起始点为原点E，EZ指向地心，水平面采用北东坐标系；再定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起；将ox轴与载人潜水器的主对称轴取向一致，而oy轴与辅助对称轴取向一致，u、v表示载人潜水器沿动坐标系的两维速度(即两轴速度)，载人潜水器的导航设备数据通过卡尔曼滤波器的数据综合，可以得到载人潜水器的航向角θ，纵倾角ψ，及在运动坐标系中载人潜水器的两维速度值u，v；如果知道k时刻的载人潜水器的位置为X_k，Y_k，运动坐标系中的速度值为u(k)，v(k)，载人潜水器在固定坐标系中的航向角为θ，纵倾角为ψ，位置时延为T_s，由公式(12)可得下一时刻的位置X_k+1，Y_k+1为：

X_k+1＝X_k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts    (12)

Y_k+1＝Y_k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts

由公式(12)就可以推算出载人潜水器在每时刻的位置值。

3)取超短基线定位声纳测量的位置值，将超短基线定位声纳测量的位置值和推算出的载人潜水器位置值通过第二级卡尔曼滤波器综合，得到最优的载人潜水器位置值；

把超短基线定位***测量到的载人潜水器的位置值作为卡尔曼滤波器的新值，通过卡尔曼滤波器综合超短基线定位声纳的测量值和载人潜水器的航位推算值，可以得到载人潜水器的航位最优估算值。

超短基线定位声纳直接进行位置测量可以保证测量误差有界，但是其测量噪声很大。只根据公式(12)来推测位置数据，推测数据平滑，但是由于测量误差的存在，位置估计误差呈现增大趋势，并随着时间增长，最终使得位置估计误差趋于无穷大。把超短基线定位声纳测量数据，通过卡尔曼滤波器融合到载人潜水器位置推算中，综合了间接测量(推算值)的数据平滑性和直接测量(水声)过程误差的有界性，获得了最佳的估计效果。

卡尔曼滤波是一种基于迭代和递推过程的最优线性估计器，它可以分解成三个子过程：预测、测量、校正。设离散时间***的状态方程和输出方程分别为：

x(k+1)＝Ф(k+1，k)x(k)+Г(k+1，k)w(k)    (2)

y(k)＝C(k)x(k)+v(k)

其中，Ф(k+1，k)为状态转移矩阵，C(k)为观测矩阵，w(k)，v(k)分别为模型噪声和观测噪声，且有：

E[w(k)]＝0，Cov[w(k)，w(j)]＝Q(k)δ_kj，k，j≥0    (3)

E[v(k)]＝0，Cov[w(k)，w(j)]＝R(k)δ_kj，k，j≥0    (4)

假设根据观测数据y＝{y(1)，y(2)，...，y(k-1)}已经求得k-1时刻状态x(k-1)的最优滤波估计

，则在没有获得新的观测数据y(k)以前，根据y(k-1)求得k时刻状态x(k)的预测值为：

$\hat{x}\left(k\right|k-1)=\mathrm{\Φ}(k,k-1)\hat{x}(k-1|k-1)---\left(5\right)$

观测值的预测估计为：

$\hat{y}\left(k\right|k-1)=C\left(k\right)\hat{x}\left(k\right|k-1)---\left(6\right)$

当获得新的观测数据y(k)时，可得出由(5)式求得的预测误差为：

$\tilde{y}\left(k\right|k-1)=y\left(k\right)-\hat{y}\left(k\right|k-1)---\left(7\right)$

可以证明，当式(2)(3)满足时， 是一个白噪声序列，即后来的与先前的无关，因此， 就可以视为k时刻获得的新值，可以用来对可状态预测值 进行校正，则k时刻状态的估计值则变为：

$\hat{x}\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)\tilde{y}\left(k\right|k-1)---\left(8\right)$

其中K(k)为滤波增益矩阵，它根据估计误差的方差达到极小的准则来确定的。表达式为：

K(k)＝P(k|k-1)C^T(k)[C(k)P(k|k-1)C^T(k)+R(k)]^-1    (9)

其中，P(k|k-1)为预测估计误差的方差，表示为：

P(k|k-1)＝Ф(k，k-1)P(k-1|k-1)Ф^T(k，k-1)+Q(k)    (10)

P(k|k)＝[I-K(k)C(k)]P(k|k-1)[I-K(k)C(k)]^T+K(k)R(k)K^T(k)  (11)

在载人潜水器导航中，上述卡尔曼滤波算法实际上可理解为一种数据融合过程。卡尔曼滤波器的计算过程参见图3。

4)采用延时超短基线测量数据融合方法，实现用过去的超短基线测量载人潜水器位置数据，对现在载人潜水器位置推算数据的矫正；

从载人潜水器获得超短基线定位***的位置信息过程角度讲：首先超短基线定位声纳的超短基线定位声纳的收发换能器定时地发出声脉冲询问信号，载人潜水器上的超短基线水声信标接到询问信号后，发射应答声脉冲信号。短基线定位的收发换能器收到脉冲信号后，超短基线定位声纳数据处理***计算出载人潜水器相对与母船的位置值，然后通过水上的水声通讯机将获得的载人潜水器的位置信息发送给安装在水下载人潜水器上的水声通讯机。这一过程由于载人潜水器与水上母船的距离不同，需要几秒甚至几十秒的时间，因此每次载人潜水器收到的超短基线定位声纳测量的载人潜水器位置信息都将是过时的信息。

本发明将所述超短基线定位声纳测量的载人潜水器位置值和推算的位置值进行数据综合，通过当前得到的过时超短基线定位声纳位置测量信息，实现对当前的载人潜水器位置信息进行优化估计，解决位置测量信息滞后造成的估计误差问题。把超短基线定位声纳的测量值融合到载人潜水器的位置推算中，避免长时间的载人潜水器位置推算，使载人潜水器位置数据偏离实际位置值。具体实施过程为：

设超短基线定位声纳测量的载人潜水器位置信息中的声波在水中的传播时间长度为t＝d*T_s，在t＝k*T_s时刻水下超短基线水声信标向超短基线定位声纳的收发换能器发出应答信号，由于声波在水中传输的时延，载人潜水器要在t＝(k+d)*T_s才能接收到k时刻的载人潜水器位置信息X_km，Y_km。此时，在t＝(k+d)*T_s时刻，载人潜水器的位置估计式为：

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)T_s$

$Y_{k+d}=Y_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (13)；

其中，d为时间间隔，T_s为时间计量单位；k为第k次测量；X_k，Y_k为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，ψ是载人潜水器的纵倾角。令：

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\]T_s$ (14)；

则根据k时刻测量值X_km，Y_km和k时刻估计值X_k，Y_k，应用(8)～(11)的卡尔曼滤波公式，得到k时刻的最优估计

，则可得(k+d)时刻载人潜水器位置的最优估计值为：

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)={\mathrm{\ΔX}}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+{\mathrm{\ΔY}}_d$ (15)；

其中，X，Y每隔d*T_s秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔T_s秒刷新一次。

5)不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标。

载人潜水器，在海中航行时不仅需要确定载人潜水器和作业母船之间的相对位置，还需要确定载人潜水器在惯性坐标系中的坐标。由于长基线定位***使用中布置水声基阵的过程复杂，而且在作业完成后，回收水声基阵容易丢失，造成很大的浪费。使用固定信标法的超短基线定位***可以确定惯性坐标的范围小，所以载人潜水器的可活动范围也有限。超短基线定位***如果投放固定信标，也有回收水声信标时丢失的水声信标的可能。

本发明采用超短基线定位声纳确定载人潜水器在惯性坐标系中的坐标，不用在海面或海底投放固定信标。这样载人潜水器的活动范围很大，并且没有回收水声信标的麻烦。

考虑到载人潜水器作业过程中母船是随机移动的，没有相对固定的参考点，只能计算出每一时刻载人潜水器相对与母船的相对位置。所以本发明需要将载人潜水器的固定坐标转化到惯性导航坐标系中，用经纬度表示载人潜水器的坐标。具体为：

确定固定坐标系(E-XYZ)，其中以母船为原点E，EZ指向地心，水平面采用北东坐标系；将固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标；载人潜水器在惯性坐标系中的经纬度坐标值计算式为：

(16)；

公式(16)是由载人潜水器相对作业母船的位置值求载人潜水器在惯性导航坐标系中的经纬度坐标值(λ，L)。其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，X，Y是载人潜水器在固定坐标系中的坐标值，R为地球半径。

载人潜水器在惯性导航坐标系中经纬度坐标(λ，L)的计算过程：

首先由母船上的GPS接收机接收到母船某一个时刻的经纬度坐标(λ₀，L₀)。母船这一个时刻的位置作为固定坐标系中的原点，由卡尔曼滤波器综合出的载人潜水器在固定坐标系中的坐标值(X，Y)用公式(16)计算出载人潜水器这个时刻的经纬度坐标(λ₁，L₁)，以这点为新的固定坐标系原点(0，0)，同时作为载人潜水器航迹推算的初始值。在确定了载人潜水器的航迹推算初始值后，开始用载人潜水器的坐标变换式(12)进行航迹推算，求出载人潜水器在新的固定坐标中坐标值(x，y)。由公式(16)可推算出每一时刻的载人潜水器经纬度坐标(λ，L)。这时的坐标(λ₀，L₀)用(λ₁，L₁)代替，(X，Y)用(x，y)代替。在载人潜水器确定初始值或复位值时，载人潜水器要处于相对静止的状态，以减少由于超短基线和水声通讯等延时产生的误差。母船在接收到某时刻可靠的母船经纬度(λ₀，L₀)和载人潜水器在固定坐标系中的坐标值(X，Y)后，就可以移动了。

本发明提出的载人潜水器位置测量延时处理方法，同样适用于其它的水下机器人，包括自治水下机器人(AUV)和遥控水下机器人(ROV)。

Claims (7)

1.一种载人潜水器的组合导航定位方法，其特征在于按如下步骤操作：1)取运动传感器和多普勒测速声纳的速度值，通过第一级卡尔曼滤波器综合，得到最优的载人潜水器速度值；2)由卡尔曼滤波器综合出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值；3)取超短基线定位声纳测量的位置值，将超短基线定位声纳测量的位置值和推算出的载人潜水器位置值通过第二级卡尔曼滤波器综合，得到最优的载人潜水器位置值；4)采用延时超短基线测量数据融合方法，实现用过去的超短基线测量载人潜水器位置数据，对现在载人潜水器位置推算数据的矫正；5)不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标。

2.按权利要求1所述载人潜水器的组合导航定位方法，其特征在于：所述由推算出的载人潜水器最优速度值和航向角、纵倾角推算载人潜水器位置值的步骤具体如下；

先确定载人潜水器起始位置的经纬度坐标，采用固定坐标系(E-XYZ)，其中以起始点为原点E，EZ指向地心，水平面采用北东坐标系；再定义动坐标系(o-xyz)，动坐标系与载人潜水器固联在一起，将ox轴与载人潜水器的主对称轴取向一致，而oy轴与辅助对称轴取向一致，u、v表示载人潜水器沿动坐标系两维速度；载人潜水器在下一时刻的位置值推算公式为：

X_k+1＝X_k+[u(k)cosθ(k)cosψ(k)-v(k)sinθ]Ts

Y_k+1＝Y_k+[u(k)sinθ(k)cosψ(k)+v(k)cosθ]Ts  (12)

所述θ为载人潜水器在固定坐标系中的航向角，ψ为载人潜水器在固定坐标系中的纵倾角，k时刻的载人潜水器的位置为X_k，Y_k，运动坐标系中的速度值为u(k)，v(k)，T_s为位置时延。

3.按权利要求2所述载人潜水器的组合导航定位方法，其特征在于：所述航向角θ、纵倾角ψ、两维速度值u，v是载人潜水器的导航设备数据通过卡尔曼滤波器的数据综合得到。

4.按权利要求1所述载人潜水器的组合导航定位方法，其特征在于：其中所述超短基线定位声纳测量到的载人潜水器的位置值为卡尔曼滤波器的新值。

5.按权利要求1所述载人潜水器的组合导航定位方法，其特征在于：步骤4)所述延时超短基线测量数据融合方法具体为：

设超短基线定位声纳测量的载人潜水器位置信息中的声波在水中的传播时间长度为t＝d*T_s，在t＝k*T_s时刻水下超短基线水声信标向超短基线定位声纳的收发换能器发出应答信号，载人潜水器在声波在水中传输的时延t＝(k+d)*T_s接收到k时刻的载人潜水器位置信息X_km，Y_km；在t＝(k+d)*T_s时刻载人潜水器的位置估计式为：

$X_{k+d}=X_k+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1\right]T_s$

$Y_{k+d}=Y_s+\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s---\left(13\right);$

其中，d为时间间隔，T_s为时间计量单位；k为第k次测量；X_k，Y_k为k时刻载人潜水器位置的估计，u、v是载人潜水器在水平面的两维运动速度，θ是载人潜水器的航向角，ψ是载人潜水器的纵倾角；令：

$\mathrm{\Δ}{X}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)-v(k+i-1)\sin (k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s$

$\mathrm{\Δ}{Y}_d=\underset{i=1}{\overset{d}{\mathrm{\Σ}}}\left[u(k+i-1)\sin \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)+v(k+i-1)\cos \mathrm{\θ}(k+i-1)\cos \mathrm{\ψ}(k+i-1)\right]T_s---\left(14\right);$

则根据k时刻测量值X_km，Y_km和k时刻估计值X_k，Y_k，应用卡尔曼滤波公式，得到K时刻的最优估计 ，进一步获得(k+d)时刻载人潜水器位置的最优估计值为：

$\hat{X}(k+d|k)=\hat{X}\left(k\right|k)+\mathrm{\Δ}{X}_d$

$\hat{Y}(k+d|k)=\hat{Y}\left(k\right|k)+\mathrm{\Δ}{Y}_d---\left(15\right).$

6.按权利要求5所述载人潜水器的组合导航定位方法，其特征在于：其中X，Y每隔d*T_s秒刷新一次，其余变量如载人潜水器的运动速度和航向角，每隔T_s秒刷新一次。

7.按权利要求1所述载人潜水器的组合导航定位方法，其特征在于：所述不用投放固定水声信标确定载人潜水器在惯性坐标系中坐标的确定方法是将载人潜水器的固定坐标转化到惯性导航坐标系中，用经纬度表示载人潜水器的坐标；具体为：确定固定坐标系(E-XYZ)，其中以母船为原点E，EZ指向地心，水平面采用北东坐标系；将固定坐标转化到惯性坐标系中，用经纬度表示载人潜水器的坐标；载人潜水器在惯性坐标系中的经纬度坐标值计算式为：

其中，(λ₀，L₀)是作业母船起始位置的经纬度坐标，X，Y是载人潜水器在固定坐标系中的坐标值，R为地球半径。

Images