CN111721296B - 一种水下无人航行器数据驱动路径规划方法 - Google Patents

Abstract

本发明提出一种水下无人航行器数据驱动路径规划方法，主要应用于水下区域遍历任务的路径规划；首先，对于任务区域，利用栅格法进行环境建模，读取先验知识以更新建模地图，并利用初始地图信息生成初始全局路径；执行任务期间，实时检测并接收观测数据，进行在线路径规划分析，基于规划模式决策评价函数F对全局规划模式和局部规划模式进行决策，如果切换至局部规划模式，依据启发规则集进行局部规划，局部特征密集区遍历完成后，回归初始全局规划路径；本方案根据水下实时高维空间观测数据进行在线指导决策，基于实时探测情况进行全局与局部规划的决策，通过规划模式切换，选择适应的规划策略，能够适用于更复杂的环境，并达到时间效率上的显著提升。

Description

一种水下无人航行器数据驱动路径规划方法

技术领域

本发明涉及水下导航领域，具体涉及一种水下无人航行器数据驱动路径规划方法。

背景技术

由于深海环境可能存在许多无法预知的危险且比较难以接近，而在实际的水下作业中，自主式水下机器人能够在无需人工干预时就可以独立运行并完成许多任务，比如自主导航、自主避障以及自主作业。因此，在不适合潜水员作业的场合，水下无人航行器无论是军事上，还是商业上都发挥着重要的作用，例如作为深海工作平台、海洋勘探取样、水下测量以及其它研究应用，无人水下机器人就能够体现出巨大的优越性。

路径规划技术是水下机器人完成海洋作业任务的重要技术，国内外在路径规划技术方面均有很多显著的成果，当前在机器人导航研究中遇到的难点和热点是在未知动态环境中所进行的路径规划。所谓路径规划通常是指按照某一性能指标(最小代价、最短路线或最短时间等)在工作空间中根据任务需求搜索一条从给定起始点到终止点的最优或者近似最优的无碰撞路径，避开环境中的障碍物。其中一种是完全遍历路径规划，即根据设定的评价函数，在规划区域内寻找一条从起始点到目标点的无障碍连续路径，使得评价函数最优。

在遍历任务中，目前常用的方法有除草机算法(lawnmoweralgorithm)、螺旋式覆盖算法(spiralalgorithm)、反弹算法(bouncealgorithm)等；但是，这些算法策略单一、缺少智能性，不能根据目标区域的不同进行智能调节，时间效率偏低。启发式搜索算法是路径规划中常见的方法，主要通过启发函数来指引算法的搜索方向，大多定义的启发函数与距离函数相关，启发式搜索算法的优劣与启发函数直接相关，而一般只能通过试凑法来尝试获取更好的启发函数。而且，在未知环境中，因为障碍物陷阱的存在，可能会导致搜索失败。

本方案为实现未知区域下的目标物探测与遍历，以最短时间为指标实现未知区域的完全探测，提出一种水下无人航行器数据驱动路径规划方法。

发明内容

本发明为解决现存的区域遍历算法普遍缺乏决策能力、效率不高的问题，提出一种水下无人航行器数据驱动路径规划方法，能够利用实时高维空间观测数据，进行全局与局部路径规划模式切换，并根据目标物形状特征，选择最合适的局部遍历方法，参照启发式搜索规则，优化局部路径规划，从而提升效率。

本发明是采用以下的技术方案实现的：一种水下无人航行器数据驱动路径规划方法，包括以下步骤：

步骤A、对于任务区域，利用栅格法进行环境建模，读取先验知识以更新建模地图，并生成初始全局路径；

步骤B、实时检测并接收观测数据，进行在线路径规划分析；

步骤B1、任务执行初始时刻，基于初始全局路径进行遍历导航，并实时接收高维空间观测分析数据，更新相应栅格内的信息素；

步骤B2、基于规划模式决策评价函数F对全局规划模式和局部规划模式进行决策，若F＜0，则执行步骤B3，由全局规划模式转化为局部规划模式；若F≥0，即计算结果为全局规划，则继续按照初始全局路径进行遍历导航；

其中，决策评价函数F表示如下：

F＝c₁G₁+c₂G₂

其中，G₁表示当前遵循全局规划获得的信息量，G₂表示当前遵循局部规划获得的信息量，c₁、c₂为权重，c₁＞0，c₂＜0，G₁、G₂表达式如下：

其中，n’表示当前未知栅格数，P_i(i＝1,2,...,n')＝cnt/cs，cnt表示目前地图中未知或非目标物的栅格数，cs表示地图中栅格总数，P'_i(i＝1,2,...,m)＝ct/cs，ct表示目前地图中已知目标物的栅格数，m表示待扫描目标物栅格数；

步骤B3、切换为局部路径规划模式，具体的：

步骤B31、依据启发规则集进行局部规划，局部特征密集区遍历完成后，回归初始全局规划路径，所述启发规则集包括目标物区域几何特征判定策略及其对应的导航策略；

步骤B32、继续按照初始全局路径遍历导航，以局部规划结束的位置，回归到初始全局路径，按照初始全局路径行进；

步骤C、重复执行步骤B2和步骤B3，直至完成对任务区域的遍历。

进一步的，所述步骤A具体包括以下步骤：

步骤A1、确定最终任务区域并将其划分为若干区块：

首先确定初始任务区域顶点以及栅格大小，栅格大小与细扫的单边扫描宽度相同；计算初始任务区域的最小外接矩形，并对最小外接矩形进行栅格大小整数倍扩建，确定最终任务区域；

以粗扫双边扫描范围为单位，沿最终任务区域宽边进行单元分解，划分若干区块，实验区域区块数＝ceil(栅格地图的宽边栅格数/粗扫双边扫描范围栅格数)；

所述粗扫相对于细扫拥有大的扫描范围，粗扫用以将未知空间最大程度转化为已知，细扫用以对目标物区域遍历，粗扫、细扫的扫描范围比例为n”:1，n”为整数；

步骤A2、读取先验知识，更新建模地图：

读入先验知识，所述先验知识包括已知的任务区域先验数据库，读取先验知识中的坐标信息与状态值，更新栅格地图的栅格状态，得到初始地图信息；

步骤A3、基于初始地图信息生成初始全局路径。

进一步的，所述步骤B31中，不同的目标物区域几何特征所对应的导航策略如下：

对于多块矩形区域，采用除草机算法；

对于正方形区域，采用螺旋式扫描算法；

对于不规则多边形区域，采用改进除草机算法，通过分析计算目标物区域多边形的最短弦长，并沿最短弦长方向用传统除草机算法进行扫描，选定扫描方法后，基于启发规则集，确定转弯临界点。

进一步的，所述步骤B31中：在进行局部路径规划确定启发规则集时，首先判断当前目标物区域几何特征，对目标物区域的几何特征进行判断时，采用以下方式：

(1)设定UUV当前所在行为cur_row，最近的目标物区域所在的一行为ob_row_1，确定探测方向为

(2)在目标物区域第一行ob_row_1，沿列方向确定目标物区域左侧列号left_col_1以及右侧列号right_col_1；

(3)继续对目标物区域第二行进行操作，ob_row_2＝ob_row_1-dir，并确定第二行中目标物区域两端列号left_col_2和right_col_2；

(4)重复第(3)步，直到目标物区域每一行两端列号都确定，组成点集pt＝{(ob_row_i,left_col_i),(ob_row_i,right_col_i)}(i＝1,2,...,n)，将pt中的点按逆时针或顺时针排序，记为ver＝{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，依次连接ver中的点，组成多边形；

(5)计算每两个点组成的向量，向量集记为vector＝{(x_i-x_i-1,y_i-y_i-1)＝{p_i}，计算每两个向量间的夹角α_i：

其中，当i＝n时，取p_i+1＝p_n+1＝p₁；

(6)若α_i＝0或α_i＝π，说明p_i和p_i+1共线；若

说明p_i⊥p_i+1；若向量夹角α_i(i＝1,2,...,n)中，只有4个元素值为

则说明多边形为矩形；

(7)若多边形为矩形，则找出ver中x坐标最小的条件下y坐标最小的点，设该点坐标为(x_A,y_A)，同理分别找到x坐标最小的条件下y最大的点(x_B,y_B)，y坐标最小的条件下x最大的点(x_C,y_C)，y坐标最大的条件下x最大的点(x_D,y_D)，该四点则为矩形四个顶点，计算四顶点两两间的距离，若相等，则说明为正方形，反之为非正方形；

进而可以判断出目标物区域为多块矩形区域、正方形区域或不规则多边形区域。

进一步的，所述步骤B31中，计算目标物区域多边形最短弦长包括以下步骤：

步骤B311、判断区域凸凹性，将非凸区域转化为凸区域；

步骤B312、通过旋转计算凸多边形最短弦长。

进一步的，所述步骤B311中，判断凸凹性方式如下：

(1)将多边形顶点按逆时针或顺时针顺序整理为集合ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}，计算多边形每相邻三个顶点A(x_i-1,y_i-1),B(x_i,y_i),C(x_i+1,y_i+1)组成的两个向量p，q；

p＝(x_i-x_i-1,y_i-y_i-1)，

q＝(x_i+1-x_i,y_i+1-y_i)，

如果i＝n，则令C(x₁,y₁),q＝(x₁-x_n,y₁-y_n)，计算p×q，记录叉积符号s_i；

(2)重复上述过程，直到计算完多边形所有相邻三个顶点组成向量的叉积，将符号集合记为symbol＝{s₁,...,s_i,...,s_n}，s_i＝±1；

(3)如果所有叉积的符号相同，则该多边形为凸多边形，反之，则为凹多边形；如果区域为凹多边形，则将其转化为凸多边形。

进一步的，所述步骤B311中，转化为凸多边形的具体方式如下：

(1)将多边形顶点按照逆时针排序，记为ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}；

(2)按序号取顶点，当前点记为cur＝(x_i,y_i)，上一顶点记为pre＝(x_i-1,y_i-1)，下一顶点记为next＝(x_i+1,y_i+1)，如果i＝n,则令next＝(x₁,y₁)，计算向量p＝cur-pre和向量q＝next-cur的叉积，记为Cross＝p×q；

(3)如果Cross＜0，代表q在p顺时针方向，此时在ver中删除cur，如果Cross＞0，则令cur＝next；重复该过程，直到所有顶点遍历完成，最后的ver即为转化后的凸多边形顶点。

进一步的，所述步骤B312中，在确定了凸多边形后，采用如下方法，计算该多边形最短弦长，确定扫描方向：

(1)凸多边形顶点为ver，选择ver中y_i最小的顶点为旋转中心,记为O(x_i,y_i)；

(2)绕该点进行逆时针旋转，旋转角度为θ，θ取值范围为[0,α]，α为使旋转中心所在一条边旋转到水平的角度，旋转后的顶点集为ver'；

(3)每旋转一次，记录ver'中y_j'最大的顶点I'(x'_j,y'_j)位置以及最低点位置(即旋转中心坐标)O(x_i,y_i)，计算两点间距离d_j作为弦长,

(4)旋转中心所在一条边转至水平后，将该边另一顶点作为新旋转中心，重复步骤(1)到步骤(3)，直到所有顶点作为旋转中心完成旋转；

(5)将记录下的弦长记为集合D＝{d₁,..d_i,...,d_n},找到最小弦长min{D}，并确定此时对应的旋转中心以及旋转角度θ，以垂直于该弦长的方向,即按

进行遍历。

进一步的，所述步骤A3中生成初始全局路径时，如果任务区域完全未知或已知程度小，则基于粗扫双边扫描范围生成初始全局路径；如果任务区域已知程度高、目标物位置明确，则基于蚁群算法、A*算法或贪婪策略确定初始全局路径。

进一步的，所述步骤B3中，在局部路径搜索中设计贪心策略，在局部规划开始时，选择获得最大信息量的方向为初始扫描方向。

与现有技术相比，本发明的优点和积极效果在于：

本方案主要应用于水下区域遍历任务的路径规划，根据水下实时高维空间观测数据进行在线指导决策，基于实时探测情况进行全局与局部规划的决策，在切换至局部路径规划时，利用启发式搜索算法的思想，参照启发规则集进行智能局部规划，使用启发规则集，避免了去寻找启发函数的麻烦，使局部路径规划能达到很好的效果；本方案通过规划模式切换，选择适应的规划策略，最终在实现区域全覆盖的前提下，相比传统除草机算法，平均时间效率提升不低于25％，能够适用于更复杂的环境，并达到时间效率上的显著提升。

附图说明

图1为本发明实施例所述路径规划方法的整体流程示意图；

图2为本发明实施例所述数据驱动算法的原理示意图；

图3为传统除草机算法原理示意图；

图4为本发明实施例所述粗扫细扫示意图；

图5为本发明实施例生成的初始全局路径示意图；

图6为本发明实施例局部规划采用除草机算法示意图；

图7为本发明实施例局部规划采用螺旋算法示意图；

图8为本发明实施例局部规划采用改进除草机算法示意图；

图9为本发明实施例启发规则-初始局部规划扫描方向选择策略-1示意图；

图10为本发明实施例启发规则-初始局部规划扫描方向选择策略-2示意图；

图11为本发明实施例启发规则-局部规划计算最合适转弯列示意图；

图12为本发明实施例启发规则-局部规划计算最合适转弯行示意图；

图13为本发明实施例启发规则-计算结束局部规划转弯行示意图；

图14为本发明实施例实验中实际规划路径图-1示意图；

图15为本发明实施例实验中实际规划路径图-2示意图；

图16为本发明实施例实验中实际规划路径图-3示意图；

图17为本发明实施例实验中实际规划路径图-4示意图。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用不同于在此描述的其他方式来实施，因此，本发明并不限于下面公开的具体实施例。

UUV(Underwaterunmannedvehicle，水下无人航行器)在水底工作，姿态受海流影响较大，可能会影响探测效果。因此，为提高目标物探测效果，对应全局规划与局部规划，本方案提出粗扫与细扫的概念，粗扫也可称之为最大扫描，拥有最大的扫描范围，用以将未知空间最大程度转化为已知；细扫也可称之为精细扫描，拥有较小的扫描范围，但有着更高的扫描精度，用以对目标物区域遍历。粗扫、细扫可视为两个同时工作的滚动窗口，二者窗口宽度有差别，扫描范围比例为n”:1，n”为整数，当任务区域遍历完成且所有目标物区域均被细扫过，任务结束。为方便表述，本实施例中以粗扫:细扫扫描范围为4:1为例，即粗扫单边扫描范围为4个栅格，细扫单边扫描范围为1个栅格。

本发明提出的方法为数据驱动路径规划算法，整体架构如图1所示，主要包括以下步骤：

步骤1、对于任务区域，利用栅格法进行环境建模，读取先验知识以更新建模地图，并利用初始地图信息生成初始全局路径，具体的：

(1)确定最终任务区域并将其划分为若干区块：

首先确定初始任务区域顶点以及栅格大小，栅格大小与细扫的单边扫描宽度相同；计算初始任务区域的最小外接矩形，并对最小外接矩形进行栅格大小整数倍化扩建，确定最终任务区域；以粗扫双边扫描范围为单位，沿最终任务区域宽边进行单元分解，划分若干区块，区块数量的计算方法为，实验区域区块数＝ceil(栅格地图的宽边栅格数/粗扫双边扫描范围栅格数)；

(2)读取先验知识，更新建模地图：

读入先验知识，所述先验知识包括已知的任务区域先验数据库，将先验知识按照规定的格式(每一行内容为经度、纬度、状态值；其中状态值为整数，表示该处有无目标物或有无障碍物)写入指定文本文件中，读取文本文件中的坐标信息与状态值，更新栅格地图的栅格状态，得到初始地图信息，尽可能将未知区域转化成已知区域，以优化路径规划搜索空间；

(3)基于初始地图信息生成初始全局路径：

环境的表达是机器人认知领域的重要内容，对路径规划方法的选取具有决定性的影响，对于水下机器人来说，环境建模就是将真实的水下空间环境信息以抽象的方式加以表达，通过构建初始全局路径，以对整体遍历扫描提供支撑。如果区域完全未知或已知程度较小，则可根据粗扫双边扫描范围，生成全局除草机式路径；如果区域已知程度较高，目标物区域位置明确，则可以根据蚁群算法、A*算法以及贪婪策略计算全局路径。

步骤2、实时检测并接收观测数据，进行在线路径规划分析；

步骤21、任务执行初始状态，基于初始全局路径进行遍历导航，并实时接收高维空间观测分析数据，更新相应栅格内的信息素(信息素表示栅格状态)；

步骤22、基于规划模式决策评价函数F对全局规划模式和局部规划模式进行决策，若F＜0，则执行步骤23，由全局规划模式转化为局部规划模式；若F≥0，即计算结果为全局规划，则继续按照初始全局路径进行遍历导航；

其中，决策评价函数F表示如下：

F＝c₁G₁+c₂G₂

其中，F≥0代表选择全局规划，F＜0表示选择局部规划，G₁表示当前遵循全局规划获得的信息量，G₂表示当前遵循局部规划获得的信息量，c₁、c₂为权重，其中，c₁＞0，c₂＜0，G₁、G₂表达式如下：

其中，n’表示当前未知栅格数，P_i(i＝1,2,...,n')＝cnt/cs，cnt表示目前地图中未知或非目标物的栅格数，cs表示地图中栅格总数，运用蒙特卡罗思想，用出现的频次去代替概率，P'_i(i＝1,2,...,m)＝ct/cs，ct表示目前地图中已知目标物的栅格数，m表示待扫描目标物栅格数，通过修改c₁、c₂大小，调整全局与局部规划的评价值，c₁与c₂可由神经网络训练、强化学习等方法进行调整；

步骤23、切换为局部规划模式(说明存在目标物)，当探测到目标物且目标物区域最小占一个栅格时，进行局部路径规划，具体的：

步骤231、依据启发规则集进行局部规划，局部特征密集区遍历完成后，回归初始全局规划路径，所述启发规则集包括目标物区域几何特征判定及其对应的导航策略；

对于多块矩形区域，采用除草机算法；

对于正方形区域，采用螺旋式扫描算法；

对于不规则多边形区域，采用改进除草机算法，通过计算目标物区域多边形的最短弦长，并沿最短弦长方向用传统除草机算法进行扫描，选定扫描方法后，基于启发规则集，找寻转弯临界点，以改进局部规划效果，节省路径点数，以提升效率。

其中，对目标物区域的几何特征进行判断时，采用以下方式：

目标物区域顶点的提取其实是目标物区域在栅格地图中位置的提取，因为栅格为正方形，所以最终形成的目标物区域的形状为多边形：

(1)设定从UUV当前所在行为cur_row，最近的目标物区域所在的一行为ob_row_1，确定探测方向为

(2)在目标物区域第一行ob_row_1，沿列方向确定目标物区域左侧列号left_col_1以及右侧列号right_col_1；

(3)继续对目标物区域第二行进行操作，ob_row_2＝ob_row_1-dir，并确定第二行中目标物区域两端列号left_col_2和right_col_2；

(4)重复第(3)步，直到目标物区域每一行两端列号都确定，组成点集pt＝{(ob_row_i,left_col_i),(ob_row_i,right_col_i)}(i＝1,2,...,n)，将pt中的点按逆时针或顺时针排序，记为ver＝{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，依次连接ver中的点，组成多边形；

(5)计算每两个点组成的向量，向量集记为vector＝{(x_i-x_i-1,y_i-y_i-1)＝{p_i}，计算每两个向量间的夹角α_i，计算公式为：

其中当i＝n时，取p_i+1＝p_n+1＝p₁；

(6)若α_i＝0或α_i＝π，说明p_i和p_i+1共线；若

说明p_i⊥p_i+1；若向量夹角α_i(i＝1,2,...,n)中，只有4个元素值为

则说明多边形为矩形；

(7)若多边形为矩形，则找出ver中x坐标(即栅格地图内的列号)最小的条件下y坐标(即栅格地图内的行号)最小的点，设该点坐标为(x_A,y_A)，同理分别找到x坐标最小的条件下y最大的点(x_B,y_B)，y坐标最小的条件下x最大的点(x_C,y_C)，y坐标最大的条件下x最大的点(x_D,y_D)，该四点则为矩形四个顶点，两两计算四顶点间的距离，若相等，则说明为正方形，反之为非正方形；

另外，计算目标物区域多边形最短弦长的方法分为两步：第一步、判断区域凸凹性，将非凸区域转化为凸区域；第二步、通过旋转计算凸多边形最短弦长。

其中判断凸凹性，转化为凸区域方式如下：

(1)将多边形顶点按逆时针或顺时针顺序整理为集合ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}，计算多边形每相邻三个顶点A(x_i-1,y_i-1),B(x_i,y_i),C(x_i+1,y_i+1)组成的两个向量p，q，其中p＝(x_i-x_i-1,y_i-y_i-1)，q＝(x_i+1-x_i,y_i+1-y_i)，如果i＝n，则令C(x₁,y₁),q＝(x₁-x_n,y₁-y_n)，计算p×q，记录叉积符号s_i；

(2)重复上述过程，直到计算完多边形所有相邻三个顶点组成向量的叉积，将符号集合记为symbol＝{s₁,...,s_i,...,s_n}，s_i＝±1；

(3)如果所有叉积的符号相同，则该多边形为凸多边形，反之，则为凹多边形；如果区域为凹多边形，则将其转化为凸多边形。

转化为凸多边形的具体方式如下：

(1)将多边形顶点按照逆时针排序，记为ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}；

(2)按序号取顶点，当前点记为cur＝(x_i,y_i)，上一顶点记为pre＝(x_i-1,y_i-1)，下一顶点记为next＝(x_i+1,y_i+1)，如果i＝n,则令next＝(x₁,y₁)，计算向量p＝cur-pre和向量q＝next-cur的叉积，记为Cross＝p×q；

(3)如果Cross＜0，代表q在p顺时针方向，此时在ver中删除cur，如果Cross＞0，则令cur＝next；重复该过程，直到所有顶点遍历完成，最后的ver即为转化后的凸多边形顶点。

在确定了凸多边形后，采用如下方法，计算该多边形最短弦长，确定扫描方向：

(1)凸多边形顶点为ver，选择ver中y_i最小的顶点为旋转中心,记为O(x_i,y_i)；

(2)绕该点进行逆时针旋转，旋转角度为θ，θ取值范围为[0,α]，α为使旋转中心所在一条边旋转到水平的角度，旋转后的顶点集为ver'；

(3)每旋转一次，记录ver'中y_j'最大的顶点I'(x'_j,y'_j)位置以及最低点位置(即旋转中心坐标)O(x_i,y_i)，计算两点间距离d_j作为弦长,

(4)旋转中心所在一条边转至水平后，将该边另一顶点作为新旋转中心，重复步骤(1)到步骤(3)，直到所有顶点作为旋转中心完成旋转；

(5)将记录下的弦长记为集合D＝{d₁,..d_i,...,d_n},找到最小弦长min{D}，并确定此时对应的旋转中心以及旋转角度θ，以垂直于该弦长的方向，即按

进行遍历。

本实施例中，在局部路径搜索中还设计贪心策略，用以在选择局部规划初始扫描方向、确定临界节点等过程中发挥作用。对于目标物区域遍历，根据其几何特征选择合适的导航策略更有利于生成一个最优解，利用栅格地图栅格的几何特性，目标物区域在经过路径规划抽象建模后，区域的边只有可能是直线而非曲线，因此几何特征的判定问题大大简化，选择局部策略也更加快速有效；对于局部路径规划而言，最优选择之一总是遵循贪心策略做出的，因此贪心策略是相对安全的，结合本发明应用目的—遍历目标物区域，以该目的为基础的全局最优解是可以通过局部最优(贪心)选择来得到，可以大大降低解决问题的成本，提高实时规划的效率，更好的应对动态未知环境下对规划效率的要求；

贪心策略总是会做出当前最优的选择，流程如下：

在贪心策略选择器中，s表示所有可行解，f表示最优解，n表示可行解的个数，A表示所有尝试过的解，a_i(i＝1,2,...,n)表示第i个尝试的解，s[i]表示当前解的表现，如果s[i]的表现比之前最好的解f[k]效果还好，则选择a_i为当前最优解；重复这个过程，直到选出s中最优解；利用这个策略，在局部规划开始时，选择获得最大信息量的方向为初始扫描方向，在局部规划中，优化搜索转弯行列。

步骤232、继续按照初始全局路径遍历导航，以局部规划结束的位置，重规划回归到初始全局路径，按照初始全局路径行进，重复执行步骤22和步骤23，直至完成对任务区域的遍历。

下面结合具体的路径规划实例对本发明方案进行详细的介绍，如图2和图3所示，图2为数据驱动路径规划算法原理图，图3为传统除草机扫描算法原理示意图，其中S点为起点,E点为终点；在图2中，当沿着初始全局路径行进时，若探测到目标物，则切换到局部规划，并由图6、图7中所示的策略选择扫描方向，其中，黑色虚线表示期望路径，灰色实线表示实际已走路径，灰色栅格表示目标物栅格(下同)。

图4-图13为路径规划原理图，如图4所示，对区域进行栅格法建模，灰色实心圆所在的栅格为原点栅格，灰色实心箭头代表UUV，箭头的方向表示航行器当前行进方向。为方便表述，本实施例选用16*16栅格地图，根据粗扫范围，将任务区域分为两个分区。UUV(Underwater unmannedvehicle，水下无人航行器)沿行方向与列方向行进时的粗扫细扫范围如图4所示。

如图5所示，读取先验知识文本文件后更新地图，白色栅格表示该栅格内无目标物或未经遍历。通过搜索初始的栅格地图信息，形成初始全局路径，如图上灰色虚线所示。正如图2所示，当沿着初始全局路径行进时，若探测到目标物，则由规划器切换到局部规划。局部规划器进行扫描策略选择以及启发规则查阅。

局部扫描策略如图6-图8所示，图上灰色实线表示实际已走路径，灰色虚线表示期望路径。图6为传统除草机算法，适用于在已知空间内存在多块矩形形状的目标物区域的情况；图7为螺旋算法，在目标物区域为正方形形状时，应用该算法效果较好；图8为改进式的除草机算法，适用于目标物区域为不规则多边形的情况，根据目标区域所占栅格组成的多边形的几何特征，计算最短的弦长，垂直与弦长的方向进行除草机遍历。

为方便表述启发规则，以局部算法为除草机算法为例，启发规则集示意图如图9到图13所示，图9与图10中所示的启发规则为，在局部规划开始时，利用贪婪策略选择扫描方向，因为粗扫拥有更大的探测范围，因此从起点到UUV所在位置的第一区块的栅格状态均被探测到，因此根据贪婪策略，选择目标物分布较多的方向作为局部规划初始扫描方向前进，其中，灰色虚线表示期望路径，灰色实线表示实际已走路径，灰色栅格表示目标物栅格(下同)。图11与图12中所示的启发规则为局部规划未完成时的指导规则，结合航行器当前位置(图上灰色箭头状标志)，以当前已知空间为解空间，分析解空间内目标物区域的位置，计算航行器在行方向与列方向上的转弯临界点。

在局部规划中，粗扫也在发挥作用，在沿列扫描时，UUV能够同时探测到粗扫范围内栅格的状态信息，扩大已知空间。如在图11中，通过列扫描，获取了未来前进方向四列栅格的状态信息，搜索得到转弯列，在该列转弯，能够恰好细扫到目标物区域，并且节省路径点；如图12所示，在进行列扫描时，结合在上一次列扫描时提前扫到的内容，计算出转弯行，在该行转弯，能够恰好细扫完目标物区域，不漏扫，并且尽量不重复扫描，节省目标点。如在图13中，航行器所在位置在完成扫扫描后，前进方向的四列栅格均为已知，分析这四列栅格中的状态信息，发现无目标物，因此向前前行四个栅格后转弯沿列方向扫描至该区块中线可达到最优扫描效果，既可以实现不漏扫，也可以节省路径点，转弯点称为转弯临界点。图13中所示的启发规则用以指导局部规划完成恢复至全局规划，搜索最近未知区域的转弯临界点，并在到达该点后回归全局路径规划。重复这个过程，直到区域遍历完成且区域内所有目标物全部经过细扫，此时任务完成。

为了进一步说明本方案的有效性，如图14到图17所示，使用MATLAB对UUV日志数据中存储的路径信息进行处理，绘制出路径图，路径方向如箭头方向所示，其中灰色栅格为目标物栅格，可见可以实现目标区域的细扫全覆盖。综上可知，本发明具备更高的智能性，更强的决策能力，能够基于当前执行任务情况选择全局规划与局部规划，并在局部规划中，能够选择多种扫描方法以及参照启发规则进行优化，相比传统除草机算法，同一张海图，遍历效率有显著提升。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (10)

1.一种水下无人航行器数据驱动路径规划方法，其特征在于，包括以下步骤：

步骤A、对于任务区域，利用栅格法进行环境建模，读取先验知识以更新建模地图，并生成初始全局路径；

步骤B、实时检测并接收观测数据，进行在线路径规划分析；

步骤B1、任务执行初始时刻，基于初始全局路径进行遍历导航，并实时接收高维空间观测分析数据，更新相应栅格内的信息素；

步骤B2、基于规划模式决策评价函数F对全局规划模式和局部规划模式进行决策，若F＜0，则执行步骤B3，由全局规划模式转化为局部规划模式；若F≥0，即计算结果为全局规划，则继续按照初始全局路径进行遍历导航；

其中，决策评价函数F表示如下：

F＝c₁G₁+c₂G₂

其中，G₁表示当前遵循全局规划获得的信息量，G₂表示当前遵循局部规划获得的信息量，c₁、c₂为权重，c₁＞0，c₂＜0，G₁、G₂表达式如下：

其中，n’表示当前未知栅格数，P_i(i＝1,2,...,n')＝cnt/cs，cnt表示目前地图中未知或非目标物的栅格数，cs表示地图中栅格总数，P'_i(i＝1,2,...,m)＝ct/cs，ct表示目前地图中已知目标物的栅格数，m表示待扫描目标物栅格数；

步骤B3、切换为局部路径规划模式，具体的：

步骤B31、依据启发规则集进行局部规划，局部特征密集区遍历完成后，回归初始全局规划路径，所述启发规则集包括目标物区域几何特征判定策略及其对应的导航策略；

步骤B32、继续按照初始全局路径遍历导航，以局部规划结束的位置，回归到初始全局路径，按照初始全局路径行进；

步骤C、重复执行步骤B2和步骤B3，直至完成对任务区域的遍历。

2.根据权利要求1所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤A具体包括以下步骤：

步骤A1、确定最终任务区域并将其划分为若干区块：

首先确定初始任务区域顶点以及栅格大小，栅格大小与细扫的单边扫描宽度相同；计算初始任务区域的最小外接矩形，并对最小外接矩形进行栅格大小整数倍扩建，确定最终任务区域；

以粗扫双边扫描范围为单位，沿最终任务区域宽边进行单元分解，划分若干区块，实验区域区块数＝ceil(栅格地图的宽边栅格数/粗扫双边扫描范围栅格数)；

所述粗扫相对于细扫拥有大的扫描范围，粗扫用以将未知空间最大程度转化为已知，细扫用以对目标物区域遍历，粗扫、细扫的扫描范围比例为n”:1，n”为整数；

步骤A2、读取先验知识，更新建模地图：

读入先验知识，所述先验知识包括已知的任务区域先验数据库，读取先验知识中的坐标信息与状态值，更新栅格地图的栅格状态，得到初始地图信息；

步骤A3、基于初始地图信息生成初始全局路径。

3.根据权利要求1所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B31中，不同的目标物区域几何特征所对应的导航策略如下：

对于多块矩形区域，采用除草机算法；

对于正方形区域，采用螺旋式扫描算法；

对于不规则多边形区域，采用改进除草机算法，通过分析计算目标物区域多边形的最短弦长，并沿最短弦长方向用传统除草机算法进行扫描，选定扫描方法后，基于启发规则集，确定转弯临界点。

4.根据权利要求3所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B31中：在进行局部路径规划确定启发规则集时，首先判断当前目标物区域几何特征，对目标物区域的几何特征进行判断时，采用以下方式：

(1)设定UUV当前所在行为cur_row，最近的目标物区域所在的一行为ob_row_1，确定探测方向为

(2)在目标物区域第一行ob_row_1，沿列方向确定目标物区域左侧列号left_col_1以及右侧列号right_col_1；

(3)继续对目标物区域第二行进行操作，ob_row_2＝ob_row_1-dir，并确定第二行中目标物区域两端列号left_col_2和right_col_2；

(4)重复第(3)步，直到目标物区域每一行两端列号都确定，组成点集pt＝{(ob_row_i,left_col_i),(ob_row_i,right_col_i)}(i＝1,2,...,n)，将pt中的点按逆时针或顺时针排序，记为ver＝{(x₁,y₁),(x₂,y₂),...,(x_n,y_n)}，依次连接ver中的点，组成多边形；

(5)计算每两个点组成的向量，向量集记为vector＝{(x_i-x_i-1,y_i-y_i-1)＝{p_i}，计算每两个向量间的夹角α_i：

其中，当i＝n时，取p_i+1＝p_n+1＝p₁；

(6)若α_i＝0或α_i＝π，说明p_i和p_i+1共线；若

说明p_i⊥p_i+1；若向量夹角α_i(i＝1,2,...,n)中，只有4个元素值为

则说明多边形为矩形；

(7)若多边形为矩形，则找出ver中x坐标最小的条件下y坐标最小的点，设该点坐标为(x_A,y_A)，同理分别找到x坐标最小的条件下y最大的点(x_B,y_B)，y坐标最小的条件下x最大的点(x_C,y_C)，y坐标最大的条件下x最大的点(x_D,y_D)，该四点则为矩形四个顶点，计算四顶点两两间的距离，若相等，则说明为正方形，反之为非正方形；

进而判断出目标物区域为多块矩形区域、正方形区域或不规则多边形区域。

5.根据权利要求3所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B31中，计算目标物区域多边形最短弦长包括以下步骤：

步骤B311、判断区域凸凹性，将非凸区域转化为凸区域；

步骤B312、通过旋转计算凸多边形最短弦长。

6.根据权利要求5所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B311中，判断凸凹性方式如下：

(1)将多边形顶点按逆时针或顺时针顺序整理为集合ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}，计算多边形每相邻三个顶点A(x_i-1,y_i-1),B(x_i,y_i),C(x_i+1,y_i+1)组成的两个向量p，q；

p＝(x_i-x_i-1,y_i-y_i-1)，

q＝(x_i+1-x_i,y_i+1-y_i)，

如果i＝n，则令C(x₁,y₁),q＝(x₁-x_n,y₁-y_n)，计算p×q，记录叉积符号s_i；

(2)重复上述过程，直到计算完多边形所有相邻三个顶点组成向量的叉积，将符号集合记为symbol＝{s₁,...,s_i,...,s_n}，s_i＝±1；

(3)如果所有叉积的符号相同，则该多边形为凸多边形，反之，则为凹多边形；如果区域为凹多边形，则将其转化为凸多边形。

7.根据权利要求6所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B311中，转化为凸多边形的具体方式如下：

(1)将多边形顶点按照逆时针排序，记为ver＝{(x₁,y₁),...,(x_i,y_i),...,(x_n,y_n)}；

(2)按序号取顶点，当前点记为cur＝(x_i,y_i)，上一顶点记为pre＝(x_i-1,y_i-1)，下一顶点记为next＝(x_i+1,y_i+1)，如果i＝n,则令next＝(x₁,y₁)，计算向量p＝cur-pre和向量q＝next-cur的叉积，记为Cross＝p×q；

(3)如果Cross＜0，代表q在p顺时针方向，此时在ver中删除cur，如果Cross＞0，则令cur＝next；重复该过程，直到所有顶点遍历完成，最后的ver即为转化后的凸多边形顶点。

8.根据权利要求6所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B312中，在确定了凸多边形后，采用如下方法，计算该多边形最短弦长，确定扫描方向：

(1)凸多边形顶点为ver，选择ver中y_i最小的顶点为旋转中心,记为O(x_i,y_i)；

(2)绕该点进行逆时针旋转，旋转角度为θ，θ取值范围为[0,α]，α为使旋转中心所在一条边旋转到水平的角度，旋转后的顶点集为ver'；

(3)每旋转一次，记录ver'中y_j'最大的顶点I'(x'_j,y'_j)位置以及最低点位置，最低点位置即旋转中心坐标O(x_i,y_i)，计算两点间距离d_j作为弦长,

(4)旋转中心所在一条边转至水平后，将该边另一顶点作为新旋转中心，重复步骤(1)到步骤(3)，直到所有顶点作为旋转中心完成旋转；

(5)将记录下的弦长记为集合D＝{d₁,..d_i,...,d_n},找到最小弦长min{D}，并确定此时对应的旋转中心以及旋转角度θ，以垂直于该弦长的方向,即按

进行遍历。

9.根据权利要求2所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤A3中生成初始全局路径时，如果任务区域完全未知或已知程度小，则基于粗扫双边扫描范围生成初始全局路径；如果任务区域已知程度高、目标物位置明确，则基于蚁群算法、A*算法或贪婪策略确定初始全局路径。

10.根据权利要求2所述的水下无人航行器数据驱动路径规划方法，其特征在于：所述步骤B3中，在局部路径搜索中设计贪心策略，在局部规划开始时，选择获得最大信息量的方向为初始扫描方向。

Images