一种深潜载人潜水器ADCP速度估计与修正算法

摘要

一种深潜载人潜水器ADCP速度估计与修正算法，包括以下步骤：1)在海面上，利用SINS/GPS组合导航提供载人潜水器在初始时刻的对地速度vb；2)ADCP工作在对流模式中，测量出载人潜水器相对水流的速度利用多次ADCP扫描的重复测量推导出载人潜水器的对地速度vb和水流速度3)下潜至近海底，利用ADCP底跟踪功能获取的载人潜水器的对地速度与重叠的ADCP测量推导出载人潜水器的对地速度的误差，提出一种对各深度单元层中载人潜水器对地速度和水流速度实现误差修正的方法。本发明解决了下潜过程载人潜水器的对地速度和洋流速度难以估算并且速度误差随时间累计的问题，能够实现深潜载人潜水器下潜/上浮过程中获取对地、洋流速度以及完成误差修正得到可靠准确的速度信息。

权利要求

1.一种深潜载人潜水器ADCP速度估计与修正算法，其特征在于，具体步骤如下：b

(1)在海面上，利用SINS/GPS组合导航提供载人潜水器在初始时刻的对地速度v；

所述步骤(1)中通过SINS/GPS组合导航提供载人潜水器在初始时刻的对地速度方法如下：

S1.1：采用SINS导航解算算法，惯性测量单元输出角速度和比力信息，经过惯导机械编排方程求解，输出导航信息，即姿态，速度，位置；导航坐标系下导航解算方程为：其中，选择当地地理坐标系为导航坐标系n，为“北东地”坐标系；载体坐标系b，为“前右下”坐标系；惯性、地球坐标系分别记为i、e， 表示载体坐标系到导航坐标系的姿态矩阵；

分别是载体坐标系对于导航坐标系的角速率在载体坐标系下的投影、地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影、导航坐标系相对于地球坐标系的n

角速率在导航坐标系下的投影；V表示对地速度在导航坐标系下的投影； 表示加速度计n

输出的比力信息；g表示重力加速度在n系下的投影；L、λ、h分别表示纬度、经度、高度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；

S1.2：通过GPS导航解算，基于测量站星距离就能够进行载体速度、位置的测量：式中，Xj、Yj、Zj是卫星位置，载体位置Xu、Yu、Zu是GPS实时检测到的；c是电磁波的传播速度； 为信号接收机时钟相对GPS时系的偏差； 则是第k颗GPS卫星时钟相对于GPS时系的偏差； 分别是电离层、对流层所引起的距离偏差；

根据速度和距离变化率的关系，得出三维速度表达式如下：GPS接收机可以测得站星距离ρj及其变化率在进行定位计算时，式(5)只有四个未知数，载体的速度 和 只要测4颗卫星，就能够解出来所有未知数；

式中

n

最终，依据四个方程式就可以解算出载体的三维速度v及位置信息；

S1.3：在海面上，GPS正常工作时，将SINS和GPS的位置，速度之差作为量测信息送入卡尔曼滤波器中，获得离散化的kalman滤波方程和量测方程，对SINS误差进行校正，输出更为准确可靠的导航信息；

n

S1.4：根据SINS/GPS提供的 V和位置(L、λ、h)信息，由以下公式得到载人潜水器在b

初始时刻的对地速度V；

(2)ADCP工作在对流模式中，测量出载人潜水器相对水流的速度 利用多次ADCP扫描b

的重复测量推导出载人潜水器的对地速度v和水流速度所述步骤(2)中通过ADCP测量载人潜水器的对流速度：ADCP先向海水中发射一定频率的声波信号，由于水体中存在散射粒子，一部分散射回波信号被接收换能器接收，因为发射声波和散射回波频率之间存在多普勒频移，所以通过频移计算得到波束向的相对速度，再经过波束向夹角把它转换成载体坐标系下的速度(3)下潜至近海底，利用ADCP底跟踪功能获取的载人潜水器的对地速度与重叠的ADCP测量推导出载人潜水器的对地速度的误差，提出一种对各深度单元层中载人潜水器对地速度和水流速度实现误差修正的方法；

所述步骤(3)中下潜至近海底，利用ADCP底跟踪功能获取的载人潜水器的对地速度与重叠的ADCP测量推导出载人潜水器的对地速度的误差，实现对各深度单元层中载人潜水器对地速度和水流速度实现误差修正的方法如下：S3.1：当深潜载人潜水器到达近海底时，可以使用底跟踪方法使得海底对信号的反射b

得到回波信号，通过反射信号的多普勒频移进行计算，获取载人潜水器的对地速度v(track)，由此得该深度单元的洋流速度b

S3.2：将利用重叠的ADCP测量推导出载人潜水器的对地速度v (binN)和使用ADCP底跟b

踪方法获取的载人潜水器的对地速度v (trackN)进行作差，得出估算和测量对地速度误差b

为δν(N)；同样将重叠ADCP测量推导出的洋流速度和底跟踪方法下计算出的洋流速度作差得

S3.3：根据载人潜水器、洋流速度的估算误差随下潜时间和深度呈线性关系的特性，假设从下潜初始时刻至近海底深度为h米,已知ADCP各单元深度为d米，深度单元总数即为N＝h/d，依次排序，根据步骤2已知每层深度单元的对地、洋流速度可求，载人潜水器的对地速b b

度在每一层深度单元的误差为Δν (n)＝(δν (N)/N)·binn；洋流速度误差为由于第一层深度单元的对地速度可由导航系统求解，不需要重叠ADCP测量估算，所以n＞1；

S3.4：由上述的误差估算方法，将各层深度单元的速度误差作为参考量，对重叠的ADCP测量推导出的速度进行误差修正，得载人潜水器对地速度：载人潜水器所在深度单元处的洋流速度：

2.根据权利要求1所述的一种深潜载人潜水器ADCP速度估计与修正算法，其特征在于，所述步骤S1.3具体步骤如下：S1.3.1：选取GPS位置误差、速度误差、姿态角误差、陀螺仪常值漂移误差、陀螺一阶马尔科夫漂移误差、加速度计一阶马尔科夫漂移误差作为量测变量，状态变量X表示如下：b T b T

X＝[δp δν φ ε ε ▽]  (10)δp＝[δL δλ δh]表示位置误差，δν＝[δνN δνE δνD]表示速度误差，φ＝[φN φE φD]b b b b T T T T b表示姿态角误差，ε＝[εx εy εz]表示陀螺仪常值漂移误差，ε＝[εx εy εz ]和▽ ＝[▽x ▽y ▽z]分别是陀螺仪、加速度计的一阶马尔科夫漂移误差，得到状态方程如下：其中，状态转移矩阵F为：

S1.3.2：将SINS和GPS的位置，速度之差作为系统的量测信息，则SINS/GPS系统的量测方程为：

式中,Vk为高斯白噪声，H矩阵如下：根据SINS解算的误差传播方程和获得的位置、速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：

Xk+1＝Φk+1∣kXk+Γk+1∣kWk(15)Zk+1＝HkXk+Vk(16)其中，Xk为k时刻的状态估计，Xk+1为k+1时刻的状态估计，Zk+1为k+1时刻的观测值，Φk+1∣k为非奇异状态一步转移矩阵，Hk为观测矩阵，Wk是系统随机过程噪声序列，Vk是系统随机量测噪声序列；

根据状态方程和观测方程得到卡尔曼滤波方程：状态预测：

一步预测误差方差矩阵：

滤波增益矩阵：

状态估计：

估计误差方差矩阵：

其中，I表示单位矩阵；

S1.3.3：利用步骤1.3.2中得到的状态估计对SINS输出的导航参数进行修正，此处以速度ν为例：

其中，为SINS输出结果，δν为卡尔曼滤波估计结果，ν表示SINS/GPS组合导航输出结果。

3.根据权利要求1所述的一种深潜载人潜水器ADCP速度估计与修正算法，其特征在于，所述步骤(2)中在对流模式中，利用重叠的ADCP测量推导载人潜水器的速度和水流速度方法如下：

S2.1：将ADCP的扫描范围，从水面至海洋深处，人为的分成若干个相同深度的深度单元层，设海面至海底深度为h米，ADCP扫描范围Q米，深度单元层深度q米，则ADCP每次可以测量K＝Q/q个深度单元层中载人潜水器相对水流的速度S2.2：由步骤1的S1.3可知，GPS可以测得载人潜水器在第一个深度单元层的对地速度b

为v (bin1)，根据ADCP的工作原理，可以测得扫描范围内K个深度单元层中载人潜水器相对b

水流的速度 此时，已知第一个深度单元层中的v (bin1)和 根据载体坐标系下载人潜水器对地速度表达式： 能够推出 从而求解出深度单元层1中的水流速度

S2.3：当载人潜水器下潜到第2个深度单元层的时候，ADCP第二次扫描范围变成2～(K+

1)深度单元层，可测得2～(K+1)深度单元层的载人潜水器相对水流速度是 其中ADCP进行重复观测的是2～K的深度单元；

S2.4：假定一定限制范围内的洋流速度近似相同，即从第一个深度单元下潜到第2个深度单元过程中的水流速度 不变，根据 可以得到 所以有其中， 和 分别表示重叠深度单元的第二次测量和第一次测量，第二次测量和第一次测量重叠的深度单元是2～K，式中 由b b

式(6)，通过v (bin1)和两次重叠测量的深度单元的误差值 求得v (bin2)，由步骤2.3，已知 又由公式

获取

S2.5：载人潜水器继续下潜，过程中扫描次数也不断累加，重叠的深度单元从2～K，到3～(K+1),4～(K+2)…重复执行步骤S2.3和步骤S2.4，得到所有深度单元载人潜水器的对地速度 和水流速度

说明书

一种深潜载人潜水器ADCP速度估计与修正算法

技术领域

[0001] 本发明属于高精度水下导航定位技术领域，涉及一种深潜载人潜水器ADCP速度估计与修正算法，特别适用于在深海作业中对载人潜水器的对地速度和洋流速度的信息测
量。

背景技术

[0002] 海洋中蕴含着大量的矿产资源、生物资源、化学资源等，所以深海探索有重要意义。而在深海领域科学研究调查作业中，载人潜水器可以搭载科学家、技术人员精确的到达
深海复杂环境，为深海考察提供了重要的研究基础，所以重视发展载人潜水器的科学研究
目前也成为海洋强国的共识。

[0003] 载人潜水器的对地速度和洋流速度在深海复杂环境中科学考察、水下潜艇作战等都是非常重要的支持信息。从海洋交通角度出发，高精度导航定位离不开对载人潜水器的
对地速度监测，为航行控制等提供参考；从军事角度出发，观测潜水器所在水域的洋流速度
以防异变需要进行流速测量，为国防建设提供帮助。在水面上，可以通过全球导航卫星系统
(Global Navigation Satellite System，GNSS)或者捷联惯性导航系统(Strapdown 
Inertial Navigation System，SINS)等系统提供载体的速度信息；而在下潜后至远离海底
的区域，信号无法穿透海洋深处，所以GNSS无法测量载人潜水器的对地速度，DVL也无法测
量准确的洋流速度信息，这使得如何获取准确可靠的载体、洋流速度信息获取仍是载人潜
水器在下潜过程中面对的难题。

[0004] 专利文献(公告号CN110274591A)：一种载人潜水器在水下的导航定位方法。基于声学多普勒流速剖面仪(Acoustic Doppler Current Profiler，ADCP)测量载体相对水流
的速度并推导出载人潜水器的对地速度，然后与SINS导航解算输出的对地速度进行融合，
构成SINS/ADCP组合导航。该方法核心是基于ADCP的测量及速度估计，设计了中层水域的组
合导航算法，解决了在水下实现难以实现自主导航定位的问题。但是载人潜水器从开始下
潜至近海底的期间：载人潜水器在初始时刻的对地速度仅依靠SINS导航解算输出不能保证
高精度；基于ADCP测量的估计速度会出现随下潜里程成正比的误差，该方法没有考虑到速
度估计误差累计的问题，会出现导航定位失准的情况，也没有办法获取到准确可靠的洋流
速度信息实现水域观测。

发明内容

[0005] 针对以上问题，本发明提供一种深潜载人潜水器ADCP速度估计与修正算法。针对在深海区域的下潜过程中，GPS和DVL都无法直接测量载体对地速度、洋流速度信息的现状，
本发明提出通过GPS/SINS组合导航提供载人潜水器初始时刻的对地速度，然后利用深潜载
人潜水器ADCP估算载体对地速度及洋流速度，并且基于速度估算误差随下潜深度累计的问
题，本专利提供一种深潜载人潜水器ADCP速度估计与修正算法，其特征在于，具体步骤如
下：

[0006] (1)在海面上，利用SINS/GPS组合导航提供载人潜水器在初始时刻的对地速度vb；

[0007] 所述步骤1中通过SINS/GPS组合导航提供载人潜水器在初始时刻的对地速度方法如下：

[0008] S1.1：采用SINS导航解算算法，惯性测量单元输出角速度和比力信息，经过惯导机械编排方程求解，输出导航信息，即姿态，速度，位置；导航坐标系下导航解算方程为：

[0009]

[0010]

[0011]

[0012] 其中，选择当地地理坐标系为导航坐标系n，为“北东地”坐标系；载体坐标系b，为“前右下”坐标系；惯性、地球坐标系分别记为i、e， 表示载体坐标系到导航坐标系的姿态
矩阵； 分别是载体坐标系对于导航坐标系的角速率在载体坐标系下的投影、
地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影、导航坐标系相对于地球坐
n
标系的角速率在导航坐标系下的投影；V表示对地速度在导航坐标系下的投影； 表示加
n
速度计输出的比力信息；g 表示重力加速度在n系下的投影；L、λ、h分别表示纬度、经度、高
度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；

[0013] S1.2：通过GPS导航解算，基于测量站星距离就能够进行载体速度、位置的测量：

[0014]

[0015] 式中，Xj、Yj、Zj是卫星位置，载体位置Xu、Yu、Zu是GPS实时检测到的；c是电磁波的传播速度； 为信号接收机时钟相对GPS时系的偏差； 则是第k颗GPS卫星时钟相对于GPS时
系的偏差； 分别是电离层、对流层所引起的距离偏差；

[0016] 根据速度和距离变化率的关系，得出三维速度表达式如下：

[0017]

[0018] GPS接收机可以测得站星距离ρj及其变化率

[0019]

[0020] 在进行定位计算时，式(5)只有四个未知数，载体的速度 和 只要测4颗卫星，就能够解出来所有未知数；

[0021]

[0022] 式中

[0023]

[0024] 最终，依据四个方程式就可以解算出载体的三维速度vn及位置信息；

[0025] S1.3：在海面上，GPS正常工作时，将SINS和GPS的位置，速度之差作为量测信息送入卡尔曼滤波器中，获得离散化的kalman滤波方程和量测方程，对SINS误差进行校正，输出
更为准确可靠的导航信息；

[0026] S1.4：进一步地，根据SINS/GPS提供的 Vn和位置(L、λ、h)等信息，由以下公式得b
到载人潜水器在初始时刻的对地速度V；

[0027]

[0028] (2)ADCP工作在对流模式中，测量出载人潜水器相对水流的速度 利用多次ADCPb
扫描的重复测量推导出载人潜水器的对地速度v和水流速度

[0029] 所述步骤2中通过ADCP测量载人潜水器的对流速度：ADCP先向海水中发射一定频率的声波信号，由于水体中存在散射粒子，一部分散射回波信号被接收换能器接收，因为发
射声波和散射回波频率之间存在多普勒频移，所以通过频移计算得到波束向的相对速度，
再经过波束向夹角把它转换成载体坐标系下的速度

[0030] (3)下潜至近海底，利用ADCP底跟踪功能获取的载人潜水器的对地速度与重叠的ADCP测量推导出载人潜水器的对地速度的误差，提出一种对各深度单元层中载人潜水器对
地速度和水流速度实现误差修正的方法；

[0031] 所述步骤3中下潜至近海底，利用ADCP底跟踪功能获取的载人潜水器的对地速度与重叠的ADCP测量推导出载人潜水器的对地速度的误差，实现对各深度单元层中载人潜水
器对地速度和水流速度实现误差修正的方法如下：

[0032] S3.1：当深潜载人潜水器到达近海底时，可以使用底跟踪方法使得海底对信号的b
反射得到回波信号，通过反射信号的多普勒频移进行计算，获取载人潜水器的对地速度v
(track)，由此得该深度单元的洋流速度

[0033] S3.2：将利用重叠的ADCP测量推导出载人潜水器的对地速度vb(binN)和使用ADCPb
底跟踪方法获取的载人潜水器的对地速度v(trackN)进行作差，得出估算和测量对地速度
b
误差为δν(N)；同样将重叠ADCP测量推导出的洋流速度和底跟踪方法下计算出的洋流速度
作差得

[0034] S3.3：根据载人潜水器、洋流速度的估算误差随下潜时间和深度呈线性关系的特性，假设从下潜初始时刻至近海底深度为h米,已知ADCP各单元深度为d米，深度单元总数即
为N＝h/d，依次排序，根据步骤2已知每层深度单元的对地、洋流速度可求，载人潜水器的对
b b
地速度在每一层深度单元的误差为Δν(n)＝(δν(N)/N)·binn；洋流速度误差为
由于第一层深度单元的对地速度可由导航系统求解，不需要重
叠ADCP测量估算，所以n＞1；

[0035] S3.4：由上述的误差估算方法，将各层深度单元的速度误差作为参考量，对重叠的A DC P 测 量 推 导 出的 速 度 进 行 误 差修 正 ，得 载 人 潜水 器 对 地 速 度 ：
载人潜水器所在深度单元处的洋流速度：

[0036] 作为本发明进一步改进，所述步骤S1.3具体步骤如下：

[0037] S1.3.1：选取GPS位置误差、速度误差、姿态角误差、陀螺仪常值漂移误差、陀螺一阶马尔科夫漂移误差、加速度计一阶马尔科夫漂移误差作为量测变量，状态变量X表示如
下：

[0038] X＝[δp δν φ εb εT ▽b]T    (10)

[0039] δp＝[δL δλ δh]表示位置误差，δν＝[δνN δνE δνD]表示速度误差，φ＝[φN φE b b b b T T T T b
φD]表示姿态角误差，ε＝[εx εy εz ]表示陀螺仪常值漂移误差，ε＝[εx εy εz]和▽
＝[▽x ▽y ▽z]分别是陀螺仪、加速度计的一阶马尔科夫漂移误差，得到状态方程如下：

[0040]

[0041] 其中，状态转移矩阵F为：

[0042]

[0043] S1.3.2：将SINS和GPS的位置，速度之差作为系统的量测信息，则SINS/GPS系统的量测方程为：

[0044]

[0045] 式中,Vk为高斯白噪声，H矩阵如下：

[0046]

[0047] 根据SINS解算的误差传播方程和获得的位置、速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：

[0048] Xk+1＝Φk+1∣kXk+Γk+1∣kW      k(15)

[0049] Zk+1＝HkXk+Vk(16)

[0050] 其中，Xk为k时刻的状态估计，Xk+1为k+1时刻的状态估计，Zk+1为k+1时刻的观测值，Φk+1∣k为非奇异状态一步转移矩阵，Hk为观测矩阵，Wk是系统随机过程噪声序列，Vk是系统随
机量测噪声序列；

[0051] 根据状态方程和观测方程得到卡尔曼滤波方程：

[0052] 状态进一步预测：

[0053] 一步预测误差方差矩阵：

[0054] 滤波增益矩阵：

[0055] 状态估计：

[0056] 估计误差方差矩阵：

[0057] 其中，I表示单位矩阵；

[0058] S1.3.3：利用步骤1.3.2中得到的状态估计对SINS输出的导航参数进行修正，此处以速度ν为例：

[0059]

[0060] 其中，为SINS输出结果，δν为卡尔曼滤波估计结果，ν表示SINS/GPS组合导航输出结果。

[0061] 作为本发明进一步改进，所述步骤2中在对流模式中，利用重叠的ADCP测量推导载人潜水器的速度和水流速度方法如下：

[0062] S2.1：将ADCP的扫描范围(从水面至海洋深处)，人为的分成若干个相同深度的深度单元层，设海面至海底深度为h米，ADCP扫描范围Q米，深度单元层深度q米，则ADCP每次可
以测量K＝Q/q个深度单元层中载人潜水器相对水流的速度

[0063] S2.2：由步骤1的S1.3可知，GPS可以测得载人潜水器在第一个深度单元层的对地b
速度为v (bin1)，根据ADCP的工作原理，可以测得扫描范围内K个深度单元层中载人潜水器
b
相对水流的速度 此时，已知第一个深度单元层中的v (bin1)和 根据
载体坐标系下载人潜水器对地速度表达式： 能够推出 从而求解出
深度单元层1中的水流速度

[0064] S2.3：当载人潜水器下潜到第2个深度单元层的时候，ADCP第二次扫描范围变成2～(K+1)深度单元层，可测得2～(K+1)深度单元层的载人潜水器相对水流速度是
其中ADCP进行重复观测的是2～K的深度单元；

[0065] S2.4：假定一定限制范围内的洋流速度近似相同，即从第一个深度单元下潜到第2个深度单元过程中的水流速度 不变，根据 可以得到 所以有

[0066]

[0067] 其中， 和 分别表示重叠深度单元的第二次测量和第一次测量，第二次测量和第一次测量重叠的深度单元是2～K，式中
b b
由式(6)，通过v (bin1)和两次重叠测量的深度单元的误差值 求得v (bin2)，由步骤2.3，
已知 又由公式

[0068]

[0069] 获取

[0070] S2.5：载人潜水器继续下潜，过程中扫描次数也不断累加，重叠的深度单元从2～K，到3～(K+1),4～(K+2)…重复执行步骤S2.3和步骤S2.4，得到所有深度单元载人潜水器
的对地速度 和水流速度

[0071] 有益效果：与现有技术相比，本发明的技术方案具有以下有益的技术效果：

[0072] 1、本发明利用ADCP重叠的测量来推导载人潜水器的对地速度和水流速度，使得不仅可以获取载体下潜/上浮过程中的对地速度，而且还能获得不同深度的水流速度剖面，无
需姿态传感器实现ADCP的测速功能。

[0073] 2、本发明提出的速度估计与修正算法，能有效的补偿采用ADCP重叠测量推导出的载人潜水器的对地速度和洋流速度，使得速度信息准确可靠，能为科学考察、水下作战等提
供重要的信息支持。

附图说明

[0074] 图1为根据本发明实施例的ADCP速度估计示意图；

[0075] 图2为根据本发明实施例的误差修正示意图；

[0076] 图3为本发明的摘要附图。

具体实施方式

[0077] 下面结合附图与具体实施方式对本发明作进一步详细描述：

[0078] 本发明提供一种深潜载人潜水器ADCP速度估计与修正算法。针对在深海区域的下潜过程中，GPS和DVL都无法直接测量载体对地速度、洋流速度信息的现状，本发明提出通过
GPS/SINS组合导航提供载人潜水器初始时刻的对地速度，然后利用深潜载人潜水器ADCP估
算载体对地速度及洋流速度，并且基于速度估算误差随下潜深度累计的问题。

[0079] 本发明在不考虑声学定位系统的条件下，通过SINS/GPS组合导航提供载人潜水器在下潜初始时刻的对地速度，然后通过安装在载人潜水器底部的ADCP测量载人潜水器相对
水流的速度，进一步利用重叠的ADCP测量推导出对地速度和不同深度的水流速度剖面；进
一步根据底跟踪功能获取载人潜水器的对地速度；最后，通过提出的一种速度误差修正方
法，对载人潜水器在下潜过程中采用重叠测量的ADCP推导得到的不同深度单元中的对地速
度和洋流速度进行有效准确的误差修正。

[0080] 步骤1：通过SINS/GPS组合导航提供载人潜水器在下潜初始时刻的对地速度νb，作为ADCP第一次测量时所处深度单元层洋流速度估计的基础。

[0081] 步骤1.1：采用SINS导航解算算法，惯性测量单元输出角速度和比力信息，经过惯导机械编排方程求解，输出导航信息，即姿态，速度，位置；导航坐标系下导航解算方程为：

[0082]

[0083]

[0084]

[0085] 其中，选择当地地理坐标系为导航坐标系n，为“北东地”坐标系；载体坐标系b，为“前右下”坐标系；惯性、地球坐标系分别记为i、e， 表示载体坐标系到导航坐标系的姿态
矩阵； 分别是载体坐标系对于导航坐标系的角速率在载体坐标系下的投影、
地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影、导航坐标系相对于地球坐
n
标系的角速率在导航坐标系下的投影；V表示对地速度在导航坐标系下的投影； 表示加
n
速度计输出的比力信息；g 表示重力加速度在n系下的投影；L、λ、h分别表示纬度、经度、高
度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；

[0086] 步骤1.2：通过GPS导航解算，基于测量站星距离就能够进行载体速度、位置的测量：

[0087]

[0088] 式中，Xj、Yj、Zj是卫星位置，载体位置Xu、Yu、Zu是GPS实时检测到的；c是电磁波的传播速度； 为信号接收机时钟相对GPS时系的偏差； 则是第k颗GPS卫星时钟相对于GPS时
系的偏差； 分别是电离层、对流层所引起的距离偏差；

[0089] 根据速度和距离变化率的关系，得出三维速度表达式如下：

[0090]

[0091] GPS接收机可以测得站星距离ρj及其变化率

[0092]

[0093] 在进行定位计算时，式(5)只有四个未知数，载体的速度 和 只要测4颗卫星，就能够解出来所有未知数。

[0094]

[0095] 式中

[0096]

[0097] 最终，依据四个方程式就可以解算出载体的三维速度vn及位置信息。

[0098] 步骤1.3：在海面上，GPS正常工作时，将SINS和GPS的位置，速度之差作为量测信息送入卡尔曼滤波器中，获得离散化的kalman滤波方程和量测方程，对SINS误差进行校正，输
出更为准确可靠的导航信息。

[0099] 具体方法如下：

[0100] 步骤1.3.1：选取GPS位置误差、速度误差、姿态角误差、陀螺仪常值漂移误差、陀螺一阶马尔科夫漂移误差、加速度计一阶马尔科夫漂移误差作为量测变量，状态变量X表示如
下：

[0101] X＝[δp δν φ εb εT ▽b]T   (9)

[0102] δp＝[δL δλ δh]表示位置误差，δν＝[δνN δνE δνD]表示速度误差，φ＝[φN φE b b b b T T T T b
φD]表示姿态角误差，ε＝[εx εy εz ]表示陀螺仪常值漂移误差，ε＝[εx εy εz]和▽
＝[▽x ▽y ▽z]分别是陀螺仪、加速度计的一阶马尔科夫漂移误差。得到状态方程如下：

[0103]

[0104] 其中，状态转移矩阵F为：

[0105]

[0106] 步骤1.3.2：将SINS和GPS的位置，速度之差作为系统的量测信息，则SINS/GPS系统的量测方程为：

[0107]

[0108] 式中,Vk为高斯白噪声，H矩阵如下：

[0109]

[0110] 根据SINS解算的误差传播方程和获得的位置、速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：

[0111] Xk+1＝Φk+1∣kXk+Γk+1∣kWk   (14)

[0112] Zk+1＝HkXk+Vk  (15)

[0113] 其中，Xk为k时刻的状态估计，Xk+1为k+1时刻的状态估计，Zk+1为k+1时刻的观测值，Φk+1∣k为非奇异状态一步转移矩阵，Hk为观测矩阵，Wk是系统随机过程噪声序列，Vk是系统随
机量测噪声序列；

[0114] 根据状态方程和观测方程得到卡尔曼滤波方程：

[0115] 状态进一步预测：

[0116] 一步预测误差方差矩阵：

[0117] 滤波增益矩阵：

[0118] 状态估计：

[0119] 估计误差方差矩阵：

[0120] 其中，I表示单位矩阵。

[0121] 步骤1.3.3：利用步骤1.3.2中得到的状态估计对SINS输出的导航参数进行修正，此处以速度ν为例：

[0122]

[0123] 其中，为SINS输出结果，δν为卡尔曼滤波估计结果，ν表示SINS/GPS组合导航输出结果。

[0124] 步骤1.4：根据SINS/GPS组合导航解算提供的 νn和位置等信息，由以下公式得b
到载人潜水器在初始时刻的对地速度ν：

[0125]

[0126] 步骤2：将安装在载人潜水器底部的声学多普勒测速仪(ADCP)用于在下潜过程中测量载人潜水器相对水流的速度，并利用不断的ADCP扫描中的重复测量，推导出载人潜水
器的对地速度和水流速度：

[0127]

[0128] 其中， 表示载人潜水器相对水流速度， 表示水流速度。

[0129] 步骤2.1：通过ADCP测量载人潜水器的相对水流速度

[0130] 图1为ADCP测速示意图，潜水器正下方安装ADCP，使用JANUS波束结构(由4个换能器组成)，如图1所示：实施例中每个换能器的发射声线与剖面仪轴线成投射角30°。测速的
工作流程：先向海水中发射一定频率的声波信号，由于水体中存在散射粒子，一部分散射回
波信号被接收换能器接收，因为发射声波和散射回波频率之间存在多普勒频移，所以通过
频移计算得到波束向的相对速度，再经过波束向夹角把它转换成载体坐标系下的速度

[0131] 步骤2.2：利用ADCP不同次扫描工作中载人潜水器对流速度的重复测量，推导出对地速度和不同深度的水流速度剖面。

[0132] 实施例中：如图1所示，设置ADCP扫描范围Q＝10米，深度单元层深度q＝1米，所以ADCP每次扫描的范围是10米，共有10个深度单元层的深度。(此处实施例中的数字设置，仅
为举例说明，其他实施例中可以进行实际情况进行设置)
b

[0133] 步骤2.2.1：未下潜之前，初始化第一个深度单元层，获取v (bin1)、 和

[0134] 假设载人潜水器在海面上，还未下潜，这时候可以利用高精度的全球定位系统b
(GPS)测得载人潜水器在第一个深度单元层的对地速度为v (bin1)，根据ADCP的工作原理，
可以测得扫描范围内K个深度单元层中载人潜水器相对水流的速度 此时，已
b
知第一个深度单元层中的v (bin1)和 根据载体坐标系下载人潜水器对地速度表
达式： 能够推出 从而求解出深度单元层1中的水流速度

[0135] 步骤2.2.2：下潜至第2个深度单元层，获取

[0136] 当载人潜水器下潜到第2个深度单元层的时候，ADCP第二次扫描范围变成2～(K+1)深度单元层，可测得2～(K+1)深度单元层的载人潜水器相对水流速度是 其
中ADCP进行重复观测的是2～K的深度单元。

[0137] 步骤2.2.3：根据第二次和第一次的ADCP扫描中的重复测量，获取vb(bin2)、

[0138] 假定一定限制范围内的洋流速度近似相同，即从第一个深度单元下潜到第2个深度单元过程中的水流速度 不变，根据 可以得到 所以有

[0139]

[0140] 其中， 和 分别表示重叠深度单元的第二次测量和第一次测量，第二次测量和第一次测量重叠的深度单元是2～K，式中
b b
由式(6)，通过v (bin1)和两次重叠测量的深度单元的误差值 求得v(bin2)，由步骤2.4，
已知 又由公式 得到

[0141] 步骤2.2.4：载人潜水器继续下潜，过程中扫描次数也不断累加，依次迭代下去，重复步骤2.2.2和步骤2.2.3操作，获取所有深度单元载人潜水器的对地速度 和水流速度

[0142] 步骤3：载人潜水器下潜到了近海底，可以利用ADCP底跟踪功能直接获取载人潜水器的对地速度，进一步与之前重叠的ADCP测量推导出载人潜水器的对地速度作误差对比，
基于估算误差随下潜深度成正比的线性关系，对不同深度单元层的对地速度、水流速度进
行修正。具体方法如下：

[0143] 步骤3.1：ADCP使用底跟踪方法获取载人潜水器的对地速度。

[0144] 图3为ADCP底跟踪测速图，如图3所示，当深潜载人潜水器到达近海底时，可以使用底跟踪方法使得海底对信号的反射得到回波信号，通过反射信号的多普勒频移进行计算，
b
获取载人潜水器的对地速度v(track)，由此得该深度单元的洋流速度

[0145] 步骤3.2：将步骤2推导出的载人潜水器的对地速度、水流速度和步骤3.1得出的对地速度和洋流速度作差。

[0146] 将利用重叠的ADCP测量推导出载人潜水器的对地速度vb(binn)和使用ADCP底跟踪b
方法获取的载人潜水器的对地速度v (trackn)进行作差，得出估算和测量对地速度误差为δ
b
ν(n)；同样将重叠ADCP测量推导出的洋流速度和底跟踪方法下计算出的洋流速度作差得

[0147] 步骤3.3：利用ADCP底跟踪方法获取的载人潜水器的对地速度和由此计算得出的洋流速度，对步骤2中ADCP多次扫描的重叠推导出的速度信息进行误差修正。

[0148] 根据载人潜水器、洋流速度的估算误差随下潜时间和深度呈线性关系的特性，假设从下潜初始时刻至近海底深度为h米,已知ADCP各单元深度为d米，深度单元总数即为N＝
h/d，，依次排序(由步骤2已知每层深度单元的对地、洋流速度可求)。载人潜水器的对地速
b b
度在每一层深度单元的误差为Δν (n)＝(δν (n)/n)·binn；洋流速度误差为
由于第一层深度单元的对地速度可由导航系统求解，不需要重
叠ADCP测量估算，所以n＞1。

[0149] 由上述的误差估算方法，将各层深度单元的速度误差作为参考量，对重叠的ADCP测量推导出的速度进行误差修正：

[0150] 得载人潜水器对地速度误差修正：

[0151]

[0152] 载人潜水器所在深度单元处的洋流速度误差修正：

[0153]

[0154] 在本发明中，一方面，可以利用重叠ADCP的测量估算出载人潜水器的对地速度和水流速度信息；另一方面，可以利用ADCP的底跟踪功能测量的速度信息对之前估算的速度
信息进行修正，不需要额外配备其他传感器。

[0155] 以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范
围。