多水下机器人最大探测覆盖率的快速梳型路径规划方法

1.本发明涉及到多水下机器人路径规划技术领域，尤其设计一种多自主水下机器人基于探测区域覆盖率的路径规划方法。


背景技术：

2.自主水下机器人是进行海洋探索和海洋科学研究的有效工具。因为自主水下机器人有着航行控制精确、位置定位准确、探测能力优秀、下潜深大、不需要操纵员常驻值守带来的便利性，无需人下潜操纵带来的安全性等特点，因此海洋探测领域，无人自主水下机器人发挥越来越重要的作用。并随着无人自主水下机器人越来越广泛的应用，无人自主水下机器人承担任务的工作量和复杂性越来越强。但载荷功能复杂、体型巨大的自主水下机器人增加了工程的成本，降低了设备应用的灵活性和环境适应性。因此，多自主水下机器人越来越受到广泛的关注。与单体自主水下机器人相比，多自主水下机器有很多优点，例如：
3.(1)提高探测效率，降低探测成本。大洋上的天气以及海况极为复杂，多个自主水下机器执行任务可以大大地节省时间，保证任务在短时间完成。单体自主水下机器人，无法很快完成任务，需要母船不断地避风避险等等，不断地进行补给并前往目标领域执行任务，大大地增加了探测成本。
4.(2)多个机器人功能上有所冗余，整个系统有很高的鲁棒性，不会受到单体自主水下机器人失效造成的影响。
5.(3)制造多个功能相对单一的自主水下机器人，与一个功能复杂的自主水下机器人相比，前者更容易实现且某种意义上更经济。
6.(4)有些任务比较复杂，只能由多个自主水下机器人完成。
7.因此研发多自主水下机器人作业技术是非常必要的。多自主水下机器人的研究包括很多方面，其中多自主水下机器人的路径规划算法就是最关键的技术之一。在海洋工程应用中，自主水下机器人多用于探测任务。科学家根据海洋地质学或船只飞机事故前飞行路线，预先估计某片海域可能存一定的科学价值或者疑似目标。为了在海洋环境比较好、适宜母船及水下机器人工作的有限时间内，尽可能多地执行探测任务，科学家们需要凭借经验对自主水下机器人进行合理的布置，保证自主水下机器人更多地探测。但是多自主水下机器人的探测率，受到探测区域地形的限制，这些通常很难准确的预估，导致覆盖率较差：比如：1.地形坡度变化比较大，自主水下机器人工作受到安全挑战，会自动停止任务，不再按照预设的探测线航行；2.因为地形复杂性，为避开悬崖等极端地形，科学家不会将每台自主水下机器人按照从左到右顺次，同探测方向布置，因此，为达到更大的探测效率，各个自主水下机器人间各自的探测区域不可避免地会重合，这样大大降低了探测效率。因此需要更精确的布置方法。


技术实现要素：

8.本发明目的是提供一种多自主水下机器人最大探测区域覆盖率路径规划方法，充
分考虑了探测区域的地形，自主水下机器人的携带能源等因素，保证探测区域覆盖率近似最大。
9.本发明为实现上述目的所采用的技术方案是：
10.多水下机器人最大探测覆盖率的快速梳型路径规划方法，包括以下步骤：
11.基于待探测区域的地形条件以及自主水下机器人的探测能力，将待探测区域进行栅格化处理，构建二维栅格模型；
12.基于二维栅格模型，采用分步迭代的方式确定单体自主水下机器人的探测轨迹，并根据单体自主水下机器人的探测轨迹即航行路径，得到单体自主水下机器人进行梳型探测时的探测面积；
13.利用单体自主水下机器人进行梳型探测时的探测面积，结合粒子群优化算法，得到满足多自主水下机器人最大探测覆盖率时的路径规划。
14.所述构建二维栅格模型包括以下步骤：
15.将待探测区域进行栅格化处理；
16.在栅格中标记危险栅格。
17.所述将待探测区域进行栅格化处理，具体为：设定待探测区域中两条相邻等高线之间的高度差为a，自主水下机器人满足航行安全的最大纵倾角为φ
max
，格栅化后每个小格的宽度为w，则宽度w满足：
[0018][0019]
其中，kh为栅格内最大等高线数量，ds为梳型路径中长测线的长度，n为栅格宽度。
[0020]
所述在栅格中标记危险栅格，具体为：
[0021]
将待探测区域中等高线间的坡度超过自主水下机器人满足航行安全的最大纵倾角为φ
max
的区域称为危险区，将待探测区域分成宽度为w的栅格，如果单个栅格内等高线数量大于栅格内最大等高线数量kh，则将该栅格标记为危险栅格。
[0022]
所述基于二维栅格模型，采用分步迭代的方式确定单体自主水下机器人的探测轨迹，具体为：
[0023]
1)通过自主水下机器人的布置角度即在第一条测线的航行方向以及栅格信息，确定其可能经过的栅格，将自主水下机器人航行方向上与栅格横纵边界的距离l
x
与ly，表示如下：
[0024][0025]
其中，θ为布置角度，即梳型路径中第一条测线与正北方向的夹角，area[i][j].x
表示第i行j列栅格的左下定点x坐标，area[i][j].y表示第i行j列栅格的左下定点y坐标，p0.x表示自主水下机器人当前位置的x坐标，p0.y表示自主水下机器人当前位置的y坐标；
[0026]
2)利用l
x
，ly，θ，将栅格行列i与j的变化先后顺序以及变化的方向分为八种情况：
[0027][0028]
3)根据栅格行列i与j的变化先后顺序以及变化的方向，采用分步迭代的方式计算自主水下机器人进行梳型探测中的航行路径及航行路径经过的栅格，当自主水下机器人遇到危险栅格时，自主水下机器人结束探测任务。
[0029]
所述步骤3)具体为：
[0030]
3.1)计算栅格变化时对应自主水下机器人的单位航行距离：自主水下机器人在起始点p0时，x与y方向上分别距离s
0x
与s
0y
到达当前所在栅格边界，x与y方向上自主水下机器人航行单位栅格距离的航行路程分别为s
x
与sy，s
0x
、s
0y
、s
x
和sy计算式如下：
[0031][0032]
其中，w为单位栅格宽度；
[0033]
3.2)计算自主水下机器人进行梳型探测中的航行路径，并确定上浮位置。
[0034]
所述步骤3.2)包括以下步骤：
[0035]
a)确定自主水下机器人航行路径上下一个经过栅格的i与j变化顺序以及变化方向；
[0036]
b)自主水下机器人每航行单位航行距离s
x
或sy，都会经过新的栅格，因此不断迭代s
x
与sy，更新自主水下机器人经过的栅格的信息；如果新的栅格是危险栅格则上浮；如果新
的栅格不是危险栅格则继续航行，直到航行到该条测线总长度，转入下一条测线；
[0037]
c)自主水下机器人进入新测线后，更新起始测线起始位置以及测线航行角，不断重复步骤a)和步骤b)，直到自主水下机器人能量耗尽，规划梳型轨迹结束或遇到危险栅格，航行结束。
[0038]
所述根据单体自主水下机器人的探测轨迹即航行路径，得到单体自主水下机器人进行梳型探测时的探测面积，具体为：在已经确定的自主水下机器人的探测轨迹的两侧各选取n条间距为w的平行测线，w为栅格宽度，测线间的面积即为自主水下机器人进行梳型探测时的探测面积。
[0039]
所述结合粒子群优化算法，得到满足多自主水下机器人最大探测覆盖率时的路径规划，具体为：将起始点的横坐标x、纵坐标y、布置角度θ、探测长测线长度d
l
，作为粒子群优化算法的位置矩阵，将多自主水下机器人覆盖率总和的大小作为粒子群优化算法的判断条件。
[0040]
本发明具有以下有益效果及优点：
[0041]
1.速度快。该方法不拘泥于时刻计算当前航行位置与具体某条等高线的相交情况，以及当前航行位置附近地形变化情况。该方法避免了算法陷入高时间复杂度，而是基于无人自主水下机器人航行安全坡度，探测能力范围，地形的等高线分布等条件，将探测区域地形进行栅格化并标记危险栅格。这种方法很大地节约了时间，方便根据海上不断变化的实际情况，进行具体规划。
[0042]
2.鲁棒性高。采用多自主水下机器人探测的方法。整个系统有一定的冗余性，不会受到单体自主水下机器人失效造成的影响，整个系统有很高的鲁棒性。
[0043]
3.效率高。多自主水下机器人探测覆盖率最大路径规划方法具有效率高的优点，主要体现在：a.多自主水下机器人同时合作完成探测任务极大地提高效率；b.通过本发明对自主水下机器人的布置，使得它们之间的覆盖区域的重叠率降低，无论是由多自主水下机器人或单体自主水下机器人多次下潜完成探测任务，都能获得最大的覆盖率，最高的效率。
附图说明
[0044]
图1为本发明的系统组成图；
[0045]
图2为本发明的梳型测线示意图；
[0046]
图3a为海底环境坡度图—上坡图；
[0047]
图3b为海底环境坡度图—下坡图；
[0048]
图4为等高线间距与等高线间平均距离图；
[0049]
图5为探测区域栅格化处理及危险区域标记图；
[0050]
图6为自主水下机器人测线起始位置，航行方向与栅格关系图；
[0051]
图7为测线两侧探测区域标记图；
[0052]
图8为方法流程图。
具体实施方式
[0053]
下面结合附图及实施例对本发明做进一步的详细说明。
[0054]
本发明涉及到多水下机器人路径规划技术领域，尤其设计一种多自主水下机器人基于探测区域覆盖率的路径规划方法。针对待探测的领域，结合探测区域的地形特点，自主机器人能源等特点完成对该领域最大的探测。本发明也涉及到一个自主水下机器人多潜次进行基于探测区域覆盖率的路径规划方法。本发明包括：(1)基于待探测区域的地形条件、自主水下机器人航行安全纵倾角及其探测能力，合理地对划定的探测区域进行栅格化处理，并将所有危险栅格进行标记。由此将待探测海底区域抽象为二维栅格模型。(2)探测区域栅格信息，完成自主水下机器人轨迹与栅格相交方法。并采用分步迭代的方式确定自主水下机器人的轨迹，如果通过危险栅格信息确定其避障(超过安全纵倾角)上浮坐标。采用建立多条平行于测线且间隔为w直线的方式，获得单体自主水下机器人进行梳型探测时的探测面积。(3)体自主水下机器人探测覆盖率相加的总覆盖率作为判断条件，以粒子群优化算法为基础，计算多自主水下机器人最大探测覆盖率。
[0055]
整个系统组成包括母船、自主水下机器人。母船的作用是，根据任务对通过算法对每个自主水下机器人工作的起始位置、探测测线的长度、角度进行设计，并将各个自主水下机器人进行布放。自主水下机器人的作用是在水下执行探测任务，尽可能多地探测待探测区域。系统组成如图1所示。
[0056]
各个系统搭载的设备如表一所示。其中，深度计、多普勒计程仪和惯导系统是实现自主水下机器人导航定位的关键设备。高度计是实现自主水下机器人定高航行探测的设备。水声通讯机是保证自主水下机器人与母船进行水下通讯的设备。多波束前视声呐(或者温盐仪、磁力计等探测设备)是实现目标探测的设备。短基线定位设备能够提升自主水下机器人在水底的定位精度。
[0057]
表一各系统搭载设备
[0058][0059]
梳型测线是探测型自主水下机器人最常用的探测路线，梳型测线的布置与梳子形似，如二所示，由一条条等间距的测线组成。梳型测线具有布置灵活，适应性强，光学探测时成图效果好的特点。梳型测线的间距与自主水下机器人的探测能力有关。如图2所示，梳型测线由长测线与短测线间隔组成。短测线长度是由潜水器的探测能力决定的，长测线长度要大于短测线，且要足够长才能有更好的探测效果，长测线与短测线长度表示为分别为d
l
，ds。其中ds与自主水下机器人的探测能力相关为固定值，d
l
为需要根据任务进行设置。图中第一条测线与正北方向的夹角，在这里称作布置角度，下文中我们用θ表示。需要说明的是，在实际布置中可以对梳型测线进行逆时针或顺时针布置，但是不影响覆盖效果，因此默认
为顺时针。
[0060]
危险区是指给自主水下机器人航行带来危险的区域。如图3a和图3b所示，自主水下机器人进行梳型探测时，是沿着海底定高航行，地形坡度即为自主水下机器人的实际航行时的纵倾角。当区域向上坡度φ较大超过阈值φ
max
(坡度阈值φ
max
即为满足自主水下机器人航行安全最大航行纵倾角)，或者向下的坡度φ超过阈值φ
max
，会给潜水器带来危险：(一)自主水下机器人在海底航行，经过向上坡度较大的区域，类似山包的区域。自主水下机器人由于躲避不及时造成剐蹭，甚至相撞，威胁自主水下机器人的安全。(2)向下的坡度很大，形似低谷，自主水下机器人可能无法航行出低谷，造成自主水下机器人的丢失。(3)目前自主水下机器人是为了水平航行探测设计的，其无法应对大纵倾的航行需求，进行大纵倾航行时，推力无法满足控制大纵倾航行，惯导设备精度受到很大影响，甚至发生姿态角“奇异现象”等等。因此，我们在进行轨迹规划时尽量避免自主水下机器人有过大的航行纵倾角。在这里，我们将向上坡度较大，或者向下坡度较大超过阈值φ
max
(φ
max
为正整数)的区域称为危险区。这些危险区域的特点就是等高线密度较大。
[0061]
整个系统的工作流程如下：首先船上的科学家划定探测区域，并利用本发明计算每个自主水下机器人工作的起始位置、探测测线的长度、角度等信息。然后，母船航行到指定位置对自主水下机器人进行布放，自主水下机器人下水后立即开展预置的探测任务。
[0062]
整个路径规划方法流程图如图8所示。包括以下步骤：
[0063]
a)基于待探测区域的地形条件、自主水下机器人航行安全纵倾角及其探测能力，合理地对划定的探测区域进行栅格化处理，并将所有危险栅格进行标记。由此将待探测海底区域抽象为二维栅格模型。
[0064]
b)基于探测区域栅格信息，完成自主水下机器人轨迹与栅格相交方法。并采用分步迭代的方式确定自主水下机器人的轨迹，通过危险栅格信息确定其避障(超过安全纵倾角)上浮坐标。采用建立多条平行于测线且间隔为w的直线的方式，获得单体自主水下机器人进行梳型探测时的探测面积。
[0065]
c)利用单体自主水下机器人探测覆盖率计算方法，结合粒子群优化算法，计算满足多自主水下机器人最大探测覆盖率时，路径规划方法。
[0066]
第一步，基于区域等高先分布、自主水下机器人航行安全纵倾角及其探测能力，合理地对划定的探测区域进行栅格化处理，并将所有危险栅格进行标记。由此将待探测海底区域抽象为二维栅格模型。
[0067]
(一)待探测区域栅格化。
[0068]
自主水下机器人航行的探测区域上两条相邻等高线之间的高度差为a，格栅化后每个小格的宽度为w。如图4所示，如果等高线间平均坡度为φ'，那么在二维平面图上，两等高线的间距应为a/tanφ'。自主水下机器人满足航行安全的最大纵倾角为φ
max
(正数，向上或者向下)。该栅格模型内单位栅格沿横轴方向由左向右，纵轴方向由下到上排布，其高与长相等为正方形，栅格的横轴方向为东西方向，纵轴方向为南北方向。
[0069]
那么，栅格宽度的取值满足：(1)为了让坡度过大的区域的判定更具有一般性且又不致使栅格的宽度过大影响计算精度。单位栅格宽度w值应满足：w》5
·
(a/tanφ
max
)，且w尽量接近(a/tanφ
max
)的整数倍。(2)为了便于对自主水下机器人航行轨迹两侧的探测区域进行栅格法标记，我们取w值应为自主水下机器人探测单侧探测距离的约数。我们已知短测线
长度取决于自主水下机器人双侧探测距离ds。那么，单位栅格宽度w取值应满足：综上，单位栅格宽度取值应满足：
[0070][0071]
其中kh为栅格内最大等高线数量。
[0072]
(二)划分栅格，标记危险栅格。
[0073]
如果栅格内等高线密度过大，即该区域内地形向上向下的坡度角度较大。在上一个步骤中，我们将kh作为决定栅格宽度的标准，即如果栅格内等高线数量大于kh，那么我们认为该栅格内地形坡度变化比较大，是不满足自主水下机器人安全航行的危险区域。
[0074]
已知等高线集合为hm，探测区域四个点坐标为等高线间距a，自主水下机器人最大航行倾角为φ
max
。我们使用将整个探测区域进行栅格化并标记危险区域的方法用算法grid(hm,s(x,y),a,φ
max
)来表示。该算法输入为等高线集合hm，探测区域s(x,y)，等高线间距a，自主水下机器人最大航行倾角φ
max
；输出为area[i][j]，其中i为行数，j为列数。将待探测区域分成宽度为w的栅格，如果单个栅格内等高线数量大于kh，则将该栅格标记为危险栅格。为了更方便理解，图5是该算法将探测区域栅格化处理后的可视化显示。
[0075]
第二步，计算无人自主水下机器人航行轨迹以及探测面积。基于探测区域栅格信息，完成自主水下机器人轨迹与栅格相交方法。并采用分步迭代的方式确定自主水下机器人的轨迹，通过危险栅格信息确定其避障(超过安全纵倾角)上浮坐标。采用建立多条平行于测线且间隔为w直线的方式，获得单体自主水下机器人进行梳型探测时的探测面积。
[0076]
(一)通过自主水下机器人的布置角度，栅格信息，确定其可能经过的栅格，即area[i][j]中i与j的变化趋势。可能经过的栅格我们又称为预期栅格。如图6所示，自主水下机器人航行方向不同，经过的栅格也不同，为了方便表示，我们将自主水下机器人航行方向上与栅格边界的距离，l
x
与ly，表示如下：
[0077][0078]
以第一行为例，area[i+1][j+1].x代表第i+1行j+1列栅格的左下定点x坐标，p0.x代表自主水下机器人当前位置的x坐标。
[0079]
利用l
x
，ly，θ，我们又将栅格横纵i与j的变化先后顺序以及变化的方向分为了八种情况：
[0080][0081]
以第二种情况为例，如图6自主水下机器人航行轨迹所示，当其航行方向为(且tanθ＜(l
x
/ly))θ时，此时，i++，j++，即自主水下机器人从栅格area[i][j]出发，经过area[j][i+1](i先变化，为加一)，再经过area[j+1][i+1](j后变化为加一)。为了方便描述，我们将上述过程概括为方法函数direction(p0,area,θ)。即输入是此时自主水下机器人的当前的位置p0，栅格信息area，航向角θ。输出为direction类变量，l
x
、ly。
[0082]
(二)根据上一步方法函数获取的信息，采用分步迭代的方式计算自主水下机器人进行梳型探测中的轨迹。遇到危险栅格时，自主水下机器人抛载上浮，结束探测任务。计算此时自主水下机器人的航行距离及探测面积。
[0083]
根据上面步骤的计算，我们获得了自主水下机器人可能经过栅格的变化规律及距预期栅格边界的距离l
x
与ly。
[0084]
(1)计算栅格变化时对应自主水下机器人的单位航行距离，单位航行距离即为分步迭代的单位长度，也是引起栅格i或j变化的单位长度。自主水下机器人在起始点p0时，x与y方向上分别航距离s
0x
与s
0y
到达栅格边界。x与y方向上自主水下机器人航行单位栅格距离的航行路程为s
x
与sy。s
0x
、s
0y
、s
x
和sy计算式如下：
[0085][0086]
(2)计算探测梳型航行路径，并确定上浮位置。该方法模拟自主水下机器人航行过程，不断迭代单位航行距离，直到航行结束。步骤如下：
[0087]
a.调用direction(p0,area,θ)方法确定自主水下机器人航行路径上下一个经过栅格的i与j变化顺序以及变化方向。以direction结果中的第二种情况为例，自主水下机器人在p0位置时先通过航行s
0x
的距离，自主水下机器人航行到位置p1，同时也到达第二个栅格area[i+1][j]，然后，自主水下机器人在p1位置时通过航行s
0x-s
0y
的距离，到达位置p2，同时也到达第三个栅格area[i+1][j+1]。
[0088]
b.在此基础上，其每航行单位航行距离s
x
或sy，都会经过新的栅格，因此我们不断迭代s
x
与sy，更新经过的栅格的信息。如果是危险栅格则上浮。如果不是危险栅格则继续航行，直到航行到该条测线总长度，转入下一条测线(由长测线转为短测线，或由短测线转为长测线)。
[0089]
同样以direction结果中的第二种情况为例，自主水下机器人在pn位置时先通过航行s
x
的距离，到达新的栅格，这个栅格行数i增加1。然后，自主水下机器人在pm位置时通过航行sy的距离，到达第新的栅格栅格的列数j增加1。
[0090]
c.进入新测线后，更新起始测线起始位置，更行测线航行角。不断重复上面a与b的步骤，直到能量耗尽或遇到危险栅格，航行结束。需要说明的是：根据梳型测线的特点，四条测线一个循环，即：第一条长测线的航向角是θ，第二条短测线航向角是第三条长测线的航向角是-θ，第四条短测线航向角是a与b表述的自主水下机器人单测线轨迹确定算法我们用dtctln(p0,area,θ)表示。该方法的输入为梳型测线起点坐标p0，探测区域栅格信息area，起始点航向角θ，测线长度为d。c步骤较容易理解不再赘述。
[0091]
(三)确定自主水下机器人航行探测覆盖面积，对探测区域进行栅格标记。上文中阐述，自主水下机器人的短测线长度可以看作其有效的探测能力，即探测范围宽度。为了将所有自主水下机器人航行经过的测线两侧所有探测的栅格进行标记，我们在梳型测线中该条探测线两侧各选取n条间距为w的平行直线(注：此处的n即为第一步中选取栅格宽度公式中的n)，如图7所示。使用步骤(二)中的方法对探测区域进行标记。因为栅格的宽度取值为因此，我们两侧选取n条间距为w的平行直线，既满足了探测区域全覆盖的需要，又避免了直线间距选取的不合适，造成栅格的遗漏。(图中为示意简便，测线两侧只各取了两条平行间距为w的直线，实际情况中，两侧应该有更多的直线)。
[0092]
第三歩，使用pso算法，寻找多自主水下机器人探测覆盖率最大的最优解。pso算法(particlesearm optimization)即粒子群优化算法。该算法通过模拟鸟类捕食行为，通过群体中个体之间的协作和信息共享来寻找最优解。
[0093]
设一共有u台自主水下机器人，影响每台自主水下机器人探测率的因素有起始点的横坐标x、纵坐标y、起始航行角度θ、探测长测线长度l。经过t次迭代后，第i个粒子的位置矩阵为：xi(t)＝(x
i,1
,x
i,2
,x
i,3
,x
i,4
)，在这里x
i,1
,x
i,2
,x
i,3
,x
i,4
分别代表自主水下机器人的xi,yi,θi,li。定义粒子运动速度矩阵vi(t)＝(v
i,1
,v
i,2
,v
i,3
,v
i,4
)，分别代表xi,yi,θi,li方向上的速度。然后，通过调用第二步中对自主水下机器人测线轨迹确定及探测面积计算方法，计算所有自主水下机器人在该状态下的覆盖率(即所有自主水下机器人经过的栅格总和)，该覆盖率即为i例子的解，并获得i粒子的最优pbesti＝(p
i,1
,p
i,2
,p
i,3
,p
i,4
)，与全局最优
gbest＝(p1,p2,p3,p4)。由i粒子最优pbesti与全局最优gbest，更新速度变化：
[0094]
vi(t)＝w
·
vi(t-1)+c1·
random[0,1]
·
[pbest
i-xi(t-1)]+c2·
random[0,1]
·
[gbest-xi(t-1)]
[0095]
其中w、c1、c2是权重系数。由此不断迭代，可以得到能达到最大覆盖率的各个自主水下机器人的起始的横坐标x、纵坐标y、起始航行角度θ、探测长测线长度l。