一种双智能水下机器人水下航行地形匹配导航方法

摘要

本发明公开了一种双智能水下机器人水下航行地形匹配导航方法，包括第一智能水下机器人和第二智能水下机器人，其将所述第一智能水下机器人和所述第二智能水下机器人进行水下定深航行，并进行地形匹配导航，通过间隔一定时间进行一次的水声通信，获得两个智能水下机器人之间的相对估计位置与距离；根据两个智能水下机器人的实际位置，通过公式得到两个智能水下机器人的导航精度；根据两个智能水下机器人之间的估计距离与实际测量距离，得到两者之间的距离误差；根据多源信息的融合原理，采用混合式融合的方式重新估计两个智能水下机器人的位置。

权利要求

1.一种双智能水下机器人水下航行地形匹配导航方法，其特征在于，包括第一智能水下机器人和第二智能水下机器人，其将所述第一智能水下机器人和所述第二智能水下机器人进行水下定深航行，并进行地形匹配导航，通过间隔一定时间进行一次的水声通信，获得所述第一智能水下机器人和所述第二智能水下机器人之间的相对估计位置与距离；

根据所述第一智能水下机器人和所述第二智能水下机器人的实际位置，通过公式得到所述第一智能水下机器人和所述第二智能水下机器人的导航精度；

根据所述第一智能水下机器人和所述第二智能水下机器人之间的估计距离与实际测量距离，得到两者之间的距离误差；

根据多源信息的融合原理，采用混合式融合的方式重新估计所述第一智能水下机器人和所述第二智能水下机器人的位置；

以相对估计位置与距离为依据，根据地形匹配导航方法得到的第一智能水下机器人的估计位置A(x1'，y1',z1'),第二智能水下机器人估计位置B(x2'，y2',z2')，两者之间的估计距离依据下式计算：

其中，L1为两个机器人之间的估计距离，Δx＝x1'‑x2'为两个机器人x方向的相对距离，Δy＝y1'‑y2'为两个机器人y方向相对的距离；

两者之间的水声通信距离依据下式计算：L＝cΔt

其中，c为水下声音传播的速度，Δt为水下声音传播的时间；

导航精度是根据两个水下机器人的实际位置进行计算的，智能水下机器人的导航误差可依据下式计算：

其中，(x1，y1,z1)为所述第一智能水下机器人的实际位置,(x2,y2,z2)为所述第二智能水下机器人的实际位置；

根据所述第一智能水下机器人和所述第二智能水下机器人导航误差的大小，智能水下机器人的导航精度可通过下式计算：其中，η1为第一智能水下机器人的导航精度，η2为第二智能水下机器人的导航精度。

2.根据权利要求1所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述距离误差是根据第一智能水下机器人和第二智能水下机器人之间的估计距离与实际测量距离进行计算的，误差根据下式进行计算：Vl＝l‑l1

其中，Vl为两个水下机器人之间的误差，l为实际测量距离，l1为两个水下机器人之间的估计距离；

基于多源信息的融合原理得出所述第一智能水下机器人和所述第二智能水下机器人的更新位置，所述更新位置可根据下式计算：A'＝A+η2Vl

B'＝B+η1Vl

其中，A'为所述第一智能水下机器人更新后的位置，A为所述第一智能水下机器人的估计位置，B'为所述第二智能水下机器人更新后的位置，B为所述第二智能水下机器人的估计位置；其双智能水下机器人水声通信的位置更新，其将每隔一定的时间进行一次水声通信，并根据A'和B'数值对两个水下机器人的位置进行更新，再进行下一步的预测和更新，从而更新智能水下机器人位置信息；

实际应用过程中，针对上述重新估计所述第一智能水下机器人和所述第二智能水下机器人的位置，对所述第一智能水下机器人和所述第二智能水下机器人的具体操作分七个步骤进行，包括：

步骤一，粒子初始化；

步骤二，重要性权值更新；

步骤三，计算有效样本容量判断是否进行重采样；

步骤四，输出状态估计；

步骤五，基于双水下机器人水声通信的位置更新；

步骤六，位置参数预估；

步骤七，新位置测定；

操作步骤循环进行后，其将得到所述第一智能水下机器人和所述第二智能水下机器人在水下的位置信息。

3.根据权利要求2所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤一中的所述粒子初始化，其在t＝0时刻，根据先验概率p(x0)分布采样产生粒子集合 且所有粒子的权值

4.根据权利要求2所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤二中的所述重要性权值更新包括在t时刻更新粒子权值为 其中，i＝1,2,…,N,归一化权值 得到t时刻位置参数x的最小均方差估计为

5.根据权利要求2所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤三中的所述计算有效样本容量判断是否进行重采样，由式计算有效样本容量，若Neff＜Nth，进行重采样，其中，Nth为设定阈值。

6.根据权利要求2所述双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤四中的所述输出状态估计，为输出地形匹配的状态估计，状态估计：

7.根据权利要求2所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤五中的所述基于双水下机器人水声通信的位置更新为：每隔一定的时间进行一次水声通信，并根据公式

A'＝A+η2Vl

B'＝B+η1Vl

对两个水下机器人的位置进行更新。

8.根据权利要求2所述的双智能水下机器人水下航行地形匹配导航方法，其特征在于，所述步骤六中的所述位置参数预估为由状态方程对位置参数xt+1进行预估，其中，u为水下机器人的运动速度，v为系统噪声；

所述步骤七中的所述新位置测定包括：在t＝t+1时刻,转到所述步骤中的所述重要性权值更新，对粒子的权值进行更新和归一化，进行新一轮的测定。

说明书

一种双智能水下机器人水下航行地形匹配导航方法

技术领域

[0001] 本发明属于水下导航技术领域，具体涉及一种双智能水下机器人水下航 行地形匹配导航方法。

背景技术

[0002] 智能水下机器人水下地形匹配导航是一种依据地理信息进行的自主导航 方式，利用智能水下机器人自身搭载的测深设备进行实时的地形测量和地形 重构，最后通过匹
配定位或定位信息融合得到智能水下机器人相对于先验地 形图的位置估计，但是在长航
时特别是极地冰下数千公里航程的航行应用中， 水下机器人存在声学基线定位的高昂成
本以及冰区无法上浮进行卫星定位的 局限，但同时地形匹配定位的误差又与局部地形特
征和地形测量误差密切相 关，且各匹配点的测量地形之间几乎没有相关性，这就意味着每
一个地形匹 配定位点的定位误差和定位点置信区间各不相同且不存在相关性，地形匹配 
定位的误差和置信区间不能以样本统计的方式获得。此外，单个机器人获取 地形信息较
少，误差较大。本发明提供了一种双智能水下机器人水下航行地 形匹配导航方法。

发明内容

[0003] 本发明的目的在于提供一种双智能水下机器人水下航行地形匹配导航方 法，以解决上述背景技术中提出的问题。

[0004] 为实现上述目的，本发明提供如下技术方案：一种双智能水下机器人水 下航行地形匹配导航方法，包括：第一智能水下机器人和第二智能水下机器 人，其将所述第一智能
水下机器人和所述第二智能水下机器人进行水下定深 航行，并进行地形匹配导航，通过间
隔一定时间进行一次的水声通信，获得 所述第一智能水下机器人和所述第二智能水下机
器人之间的相对估计位置与 距离；

[0005] 根据所述第一智能水下机器人和所述第二智能水下机器人的实际位置， 通过公式得到所述第一智能水下机器人和所述第二智能水下机器人的导航精 度；

[0006] 根据所述第一智能水下机器人和所述第二智能水下机器人之间的估计距 离与实际测量距离，得到两者之间的距离误差；

[0007] 根据多源信息的融合原理，采用混合式融合的方式重新估计所述第一智 能水下机器人和所述第二智能水下机器人的位置；

[0008] 实际应用过程中，对所述第一智能水下机器人和所述第二智能水下机器 人的具体操作步骤包括：粒子初始化、重要性权值更新、计算有效样本容量 判断是否进行重采样、
输出状态估计、基于双水下机器人水声通信的位置更 新、位置参数预估和新位置测定，操
作步骤循环进行后，其将得到所述第一 智能水下机器人和所述第二智能水下机器人在水
下的位置信息。

[0009] 进一步地，以相对估计位置与距离为依据，根据地形匹配导航方法得到 的第一智能水下机器人的估计位置A(x′1,y′1,z′1),第二智能水下机器人估计 位置B(x′2,y′2,z′2)，
两者之间的估计距离依据下式计算：

[0010]

[0011] 其中，L1为两个机器人之间的估计距离，Δx＝x′1‑x′2为两个机器人x方向 的相对距离，Δy＝y′1‑y′2为两个机器人y方向相对的距离；

[0012] 两者之间的水声通信距离依据下式计算：

[0013] L＝cΔt

[0014] 其中，c为水下声音传播的速度，Δt为水下声音传播的时间。

[0015] 进一步地，所述导航精度是根据两个水下机器人的实际位置进行计算的， 智能水下机器人的导航误差可依据下式计算：

[0016]

[0017]

[0018] 其中，(x1,y1,z1)为所述第一智能水下机器人的实际位置,(x2,y2,z2) 为所述第二智能水下机器人的实际位置；

[0019] 根据所述第一智能水下机器人和所述第二智能水下机器人导航误差的大 小，智能水下机器人的导航精度可通过下式计算：

[0020]

[0021]

[0022] 其中，η1为第一智能水下机器人的导航精度，η2为第二智能水下机器人的 导航精度。

[0023] 进一步地，所述距离误差是根据第一智能水下机器人和第二智能水下机 器人之间的估计距离与实际测量距离进行计算的，误差根据下式进行计算：

[0024] Vl＝l‑l1

[0025] 其中，Vl为两个水下机器人之间的误差，l为实际测量距离，l1为两个水 下机器人之间的估计距离；

[0026] 基于多源信息的融合原理得出所述第一智能水下机器人和所述第二智能 水下机器人的更新位置，所述更新位置可根据下式计算：

[0027] A'＝A+η2Vl

[0028] B'＝B+η1Vl

[0029] 其中，A′为所述第一智能水下机器人更新后的位置，A为所述第一智能 水下机器人的估计位置，B′为所述第二智能水下机器人更新后的位置，B为所 述第二智能水下机器
人的估计位置；其双智能水下机器人水声通信的位置更 新，其将每隔一定的时间进行一次
水声通信，并根据A′和B′数值对两个水下机 器人的位置进行更新，再进行下一步的预测和
更新，从而更新智能水下机器 人位置信息；

[0030] 实际应用过程中，针对上述重新估计所述第一智能水下机器人和所述第 二智能水下机器人的位置，对所述第一智能水下机器人和所述第二智能水下 机器人的具体操作
分七个步骤进行，包括：

[0031] 步骤一，粒子初始化；

[0032] 步骤二，重要性权值更新；

[0033] 步骤三，计算有效样本容量判断是否进行重采样；

[0034] 步骤四，输出状态估计；

[0035] 步骤五，基于双水下机器人水声通信的位置更新；

[0036] 步骤六，位置参数预估；

[0037] 步骤七，新位置测定；

[0038] 操作步骤循环进行后，其将得到所述第一智能水下机器人和所述第二智 能水下机器人在水下的位置信息。

[0039] 进一步地，所述步骤一中的所述粒子初始化，其在t＝0时刻，根据先验概 率p(x0)分布采样产生粒子集合 且所有粒子的权值

[0040] 进一步地，所述步骤二中的所述重要性权值更新包括在t时刻更新粒子权 值为归一化权值  可得到t时刻位置参数x
的最小均方差估计为

[0041] 进一步地，所述步骤三中的所述计算有效样本容量判断是否进行重采样， 由式

[0042]

[0043] 计算有效样本容量，若Neff＜Nth，进行重采样，其中，Nth为设定阈值。

[0044] 进一步地，所述步骤四中的所述输出状态估计，为输出地形匹配的状态 估计，状态估计：

[0045] 进一步地，所述步骤五中的所述基于双水下机器人水声通信的位置更新 为：每隔一定的时间进行一次水声通信，并根据公式

[0046] A'＝A+η2Vl

[0047] B'＝B+η1Vl

[0048] 对两个水下机器人的位置进行更新。

[0049] 进一步地，所述步骤六中的所述位置参数预估为由状态方程

[0050]

[0051] 对位置参数xt+1进行预估，其中，u为水下机器人的运动速度，v为系统噪声； 所述步骤七中的所述新位置测定包括：在t＝t+1时刻,转到所述步骤中的所述 重要性权值更
新，对粒子的权值进行更新和归一化，进行新一轮的测定。

[0052] 与现有技术相比，本发明的有益效果是，本发明的双智能水下机器人水 下航行地形匹配导航方法，是利用两个水下机器人的地形匹配信息以及两者 之间的相互水声通信
来获取更多的信息，来更新水下机器人的位置，提高地 形匹配导航的精度。考虑到耗能和
在冰区或水下长时间航行的问题，采用单 波束测量地形数据，利用粒子滤波算法对两个水
下机器人位置进行估计，并 根据两者之间的距离，根据多源信息融合原理进行数据融合对
水下机器人的 位置进行更新重新估算位置，使地形匹配导航定位的精度更高。该方法实用 
性强，能够综合考虑在复杂冰区环境下的匹配导航问题，解决了单一水下机 器人匹配误差
大的问题，具有较高的匹配导航精度，并且能够消除长时间推 算导航或惯性的导航累积偏
差而使导航误差有界，在长航时特别是极地冰下 数千公里航程的航行应用中，可以使水下
机器人脱离成本高昂的声学基线定 位以及上浮修正导航偏差导致的隐蔽性丧失和冰区无
法上浮进行卫星定位的 局限。另外这种方法设备简单，造价低廉，较为经济实用。

附图说明

[0053] 图1为本发明实施例双智能水下机器人水下航行地形匹配导航方法的仿 真实验运动图；

[0054] 图2为本发明实施例中重新估计两个智能水下机器人位置的步骤图。

具体实施方式

[0055] 下面结合实施例对本发明做进一步的描述。

[0056] 以下实施例用于说明本发明，但不能用来限制本发明的保护范围。实施 例中的条件可以根据具体条件做进一步的调整，在本发明的构思前提下对本 发明的方法简单改进
都属于本发明要求保护的范围。

[0057] 本发明实施例提供了一种双智能水下机器人水下航行地形匹配导航方法， 本实施例是利用两个设定的智能水下机器人在水下定深航行，结合智能水下 机器人测量信息
和先验数字地图的特征，对水下机器人航行位置进行匹配， 并在此基础上考虑两个水下机
器人之间的水声通信，根据两个水下机器人之 间的估计位置和相对距离信息，根据多源信
息融合原理对水下机器人的位置 进行更新，最后得到一条与实际航行曲线较为接近的匹
配导航曲线。

[0058] 本实施例先做仿真实验，将两个智能水下机器人放置在定深的冰区水下 进行航行，每个智能水下机器人均带有推算导航传感器、水听器等，其中推 算导航传感器包括罗
经、加速度计、多普勒测速仪和深度计。智能水下机器 人航行过程中存在过程噪声和测量
噪声，为了便于计算设定该噪声为高斯白 噪声，并且考虑到在水下或冰下长时间航行耗能
的问题，采用单波束测量数 据。

[0059] 请参阅图1所示，由于第一智能水下机器人和第二智能水下机器人是在 定深的水下航行，可以把两个智能水下机器人的三维平面的运动简化为二维 平面的运动，通过间隔
一定时间进行一次的水声通信，获得两个智能水下机 器人之间的相对估计位置，第一智能
水下机器人的估计位置A(x＇1,y′1,z′1),第 二智能水下机器人的估计位置B(x′2,y′2,z′2)，
以及两者之间的距离L,其中 L＝cΔt，c为水下声音传播的速度，Δt为水下声音传播的时
间。根据两个智能 水下机器人的位置，通过

[0060]

[0061] 其中L1为两个智能水下机器人之间的估计距离，Δx＝x′1‑x′2为两个智能水下机 器人x方向的相对距离，y′1‑y′2为两个机器人y方向相对的距离，得到两个智 能水下机器人
之间的相对估计距离，根据两个智能水下机器人的实际位置， 可以得到智能水下机器人的
导航误差为，

[0062]

[0063]

[0064] 其中(x1,y1,z1)为第一智能水下机器人的实际位置,(x2,y2,z2)为第 二智能水下机器人的实际位置，根据两个智能水下机器人导航误差的大小得 到第一智能水下机器人
和第二智能水下机器人的导航精度分别为：

[0065]

[0066]

[0067] 根据两个智能水下机器人之间的估计距离与实际测量距离，可以得到两 者之间的误差为：

[0068] Vl＝l‑l1

[0069] 其中Vl为两个水下机器人之间的误差，l为实际测量距离，l1为两个智能水下 机器人之间的估计距离。

[0070] 根据多源信息的融合原理，采用混合式融合的方式重新估计两个智能水 下机器人的位置，其中第一智能水下机器人的位置为A'＝A+η2Vl，A′为第一 智能水下机器人更新
后的位置，A为第一智能水下机器人的估计位置，第二水 下机器人的位置为B'＝B+η1Vl，，
B′为第二智能水下机器人更新后的位置，B 为第二智能水下机器人的估计位置。对水下机
器人的位置更新成功后，进行 下一步的预测和更新，得到两个较为精确的水下机器人导航
曲线。

[0071] 请参阅图2所示，在实际应用过程中，针对上述重新估计两个智能水下机 器人的位置，具体操作步骤分七步,包括：粒子初始化、重要性权值更新、计 算有效样本容量判断
是否进行重采样、输出状态估计、基于双水下机器人水 声通信的位置更新、位置参数预估
和新位置测定。

[0072] 第一步是粒子初始化，在t＝0时刻，根据先验概率p(x0)分布采样产生粒子 集合且所有粒子的权值

[0073] 第二步是重要性权值更新，在t时刻更新粒子权值为   (i＝1,2,…,N),归一化权值 可得t时刻位置参数x的最小均方差 估计为

[0074] 第三步是计算有效样本容量判断是否进行重采样，由式

[0075]

[0076] 计算有效样本容量，若Neff＜Nth，进行重采样，式中，Nth是设定阈值。

[0077] 第四步是输出状态估计，在输出地形匹配的状态估计时，状态估计： 

[0078] 第五步是基于双水下智能机器人水声通信的位置更新，每隔一定的时间 进行一次水声通信，并根据公式

[0079] A'＝A+η2Vl

[0080] B'＝B+η1Vl

[0081] 对两个智能水下机器人的位置进行更新。

[0082] 第六步是位置参数预估，由系统状态方程

[0083]

[0084] 对位置参数xt+1进行预估，其中，u表示水下智能机器人的运动速度，v为系统 噪声。

[0085] 第七步是新位置测定，前六步进行完之后，在t＝t+1时刻,转到第二步重 要性权值更新，对粒子的权值进行更新和归一化，进行新一轮的测定。

[0086] 在实际应用过程中，上述这七步循环进行。

[0087] 这样可以得到两个智能水下机器人在水下的航行轨迹，把基于两个智能水 下机器人粒子滤波的地形匹配导航的估计轨迹与推算导航轨迹智能水下机器 人的真实位置轨
迹相比，可以得到基于两个水下智能机器人的地形匹配导航 精度较高，有效的减小了地形
匹配导航过程中的误差。

[0088] 尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而 言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行 多种变化、修改、替换
和变型，本发明的范围由所附权利要求及其等同物限 定。