CN108871364A - 一种基于结点算法的水下机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于结点算法的水下机器人路径规划方法，通过构建水下机器人起始点和目标点之间的周围区域的水下二维网格地图，判断周围区域是否存在障碍物；根据结点算法寻找最优路径。本发明基于结点算法路径规划算法运用于水下机器人的自主避障，能够在没有障碍物时，快速的到达目标位置；在遇到障碍物时，找到一条最优的路径，同时能够减少搜索的时间，提高效率。从而让水下机器人在水底能够快速、准确的到达指定位置。

Description

一种基于结点算法的水下机器人路径规划方法

技术领域

本发明属于水下机器智能自主移动领域。涉及一种路径规划算法来实现水下机器人的自主避障能力，主要基于优化的路径搜索算法，通过对起点和目标点之间的区域进行路径搜索，在进入障碍物区域之前，规划好最优路径，实现自主避障。

背景技术

随着人类社会的发展，对于海洋资源的探索和海底的探测日益增加，但是人类自身的局限性无法到达一些危险海域，相关工作也无法完成。水下机器人具有灵活性高，机动性强等优势，能够在复杂的海洋环境下，完成各种作业使命避免造成生命损失。由于水下的地形复杂多变，视线范围较窄，依靠摄像头观察水下地形，图像可能拍摄不清，人为远程遥控水下机器人，难以及时提前的给出指令，因此需要水下机器人搭载声呐，摄像和导航等装置，对行驶周围的相关环境进行提前的扫描，形成一个搜索区域，进行路径的规划，提早对周围环境可能造成的碰撞危险进行感知并做出相应的响应，来实现自主避障的功能。

发明内容

本发明的目的是为了解决现有技术中存在的缺陷，提供一种能够有效规划最优路径、并排除周围障碍物影响的方法。

为了达到上述目的，本发明提供了一种基于结点算法的水下机器人路径规划方法，包括以下步骤：

(1)构建水下机器人起始点和目标点之间的周围区域的水下二维网格地图，并显示出周围区域存在的障碍物；每个网格的中心点定义为结点；

(2)初始设置：确定起始结点和目标结点的位置；初始化代价函数、开启列表和关闭列表；将起始结点加入开启列表中；

(3)优化路径选择：判断水下机器人当前位置结点周围是否存在障碍物；

当存在障碍物时，从水下机器人的起始结点开始，向周围访问二维网格地图中的结点，迭代检测开启列表中的结点，并把当前检测结点最靠近的未检测的结点加入开启列表中，已检测的结点移至关闭列表中；该开启列表从初始结点向外扩展，直至达到目标结点；

当不存在障碍物时，选择离目标点最近的结点，寻找最快路径。

具体地，本发明水下机器人路径规划方法如下：

水下机器人在航行之前，利用声呐等探测系统，对起始点和目标点之间的周围区域进行扫描，形成周围地形的轮廓图，显示出周围的障碍物。假设把它想象成一张二维网格图，起始点和目标点分别为图中的一个网格，整个扫描区域定义为搜索区域，每个网格的中心点定义为搜索结点(n)。把待检测的网格结点放在开启列表(openlist)中，已检测过的结点放在关闭列表(closelist)中，在有障碍物的情况下，算法从水下机器人的起始点开始，向周围访问图中的结点，迭代检测开启列表中的结点，并把该结点最靠近的未检测的结点加入开启列表，该开启列表从初始结点向外扩展，直到到达目标结点；当水下机器人周围没有障碍物时，运用一个启发函数快速的导向目标结点的方位，例如目标点在水下机器人的右边，它将优先的趋向于右边结点的路径，评估任意结点到达目标结点的代价，此时它选择的是离目标结点最近的结点进行比较分析，从而迅速的找到一条通往目标结点的路径。

水下机器人具体该往哪个方向运动由以下公式确定：

f(n)＝g(n)+h(n) 式(1)

g(n)指从初始结点A沿着已经生成的路径到指定待检测结点形成的代价；

h(n)指待检测结点到目标结点B的代价；

h(n)又被称为启发函数，它告诉水下机器人从任意结点到目标结点B的最小代价估计值，选择一个好的启发函数十分重要。

启发函数可以控制水下机器人路径规划的整体行为：

(1)当h(n)＝0时，只有g(n)起作用，水下机器人从起始点向周围搜索寻找，能够保证一条最短的到达路径；

(2)如果h(n)经常比你移动到目标结点的实际代价小，结点算法扩展的多，能够帮助水下机器人规划一条最短的到达路径，但花费的时间会较长。

(3)如果h(n)精确的等于任意结点n到达目标，结点算法会仅仅扩展最短的路径上的结点，

(4)如果h(n)从n移动到目标点的实际代价很高，算法的运行速度很快，却不能找到一条最短的路。

标准的启发式函数是曼哈顿距离(两点南北方向上的距离加上东西方向上的距离，D(x，y)＝|x1-x2|+|y1-y2|)。选择你的代价函数从一个位置移动到邻近位置的最小代价D，实际中的启发式函数应该是曼哈顿距离的D倍：

h(n)＝D*(|(n.x-goal.x)|)+|(n.y-goal.y)|

水下机器人的运动是各个方向上的，假设运动地图为一个网格，不需要直上直下运动，可以沿着对角线进行快速移动，它的移动代价是D2＝sqrt(2)*D

h_diagonal(n)＝min(|(n.x-goal.x)|,|(n.y-goal.y)|)

h_straight(n)＝(|(n.x-goal.x)|+|(n.y-goal.y)|)

h(n)＝D2*h_diagonal(n)+D*(h_staright(n)-2*diagonal(n))

这里的diagonal(n)：沿着斜线可以移动到步数；staright(n)：曼哈顿距离；让所有的斜线步都乘以D2，剩下的直线步乘以D。

之前的结点搜索有时会导致较低的效率，因为当某些f(n)的值相同时，他们都会被加入搜索，尽管我们只需要搜索其中的一条，这时我们需要给启发函数添加一个附加值，这个值是确定的，保证f值不同只有一条路径会被检测。

一个有效的添加附加值的方法是稍微改变h的衡量单位，计算初始-目标向量和当前-目标向量的向量叉积，算法的结果是搜索很少的区域，并且路径很短。

dx1＝current.x-goal.x

dy1＝current.y-goal.y

dx2＝start.x-goal.x

dy2＝start.y-goal.y

cross＝|(dx 1*dy 2-dx 2*dy 1)|

h*＝cross*0.01

本发明相比现有技术具有以下优点：

本发明基于结点算法路径规划算法运用于水下机器人的自主避障，能够在没有障碍物时，快速的到达目标位置；在遇到障碍物时，找到一条最优的路径，同时能够减少搜索的时间，提高效率。从而让水下机器人在水底能够快速、准确的到达指定位置。

附图说明

图1为本发明基于结点算法的水下机器人路径规划方法的流程图；

图2为采用本发明路径规划方法的系统算法流程图；

图3为采用本发明规划方法与传统方法规划路径的比较示意图；

图4为采用本发明规划方法的水下机器人具体行进路线。

图3中，A为起始点，B为目标点，3为障碍物，1为本发明规划路径，2为传统方法规划路径；

图4中，A为起始点，B为目标点，C为障碍物。

具体实施方式

下面结合附图对本发明进行详细说明。

如图1所示，本发明基于结点算法的水下机器人路径规划方法具体步骤如下：

机器人进入水下环境后，先通过声呐和相关传感器等对水下情况进行扫描和分析，构建水下的基本地形结构，确定周围的环境，以及水下机器人的初始位置(初始结点)和目标位置(目标结点)。姿态和定位传感器通过采集数据，制定初步的路径反馈给控制器。判断前方是否有障碍物，如果有障碍物，就从起始结点向外扩散搜索，选择最优的到达路径；如果没有障碍物，则偏向搜索靠近目标结点方向的区域，最快到达。通过算法程序的选择，控制推进器进行相应运动，最终到达目标点。

如图2所示，具体算法如下：

1、首先初始化相关函数、开启列表和关闭列表的值

起始结点(0,0)；

并设f(n)＝g(n)+h(n) 式(1)，

g(n)指从初始结点A沿着已经生成的路径到指定待检测结点形成的代价，起始值为0，向周围网格结点移动时：沿网格线正向移动一个相邻结点时，g(n)就加10；斜向移动一个网格结点时，g(n)就加14。

h(n)指待检测结点到目标结点B的代价g(n)，

假如当前待检测结点的坐标(x,y),目标结点B的坐标(x1,y1),则h(n)＝|x1-x|+|y1-y|。

从起始结点开始，将其作为待检测的结点加入开启列表中。

2、从起始结点开始，将起始结点作为当前待检测结点，寻找起始结点周围可以到达的结点(即沿网格线正向移动的上、下、左、右四个结点，以及斜向移动的左上、右上、右下、左下四个结点)，并把起始结点设置为这些结点的“父结点”，然后把周围的8个结点加入到“开启列表中”，根据列表中结点的f(n)值进行升序排序。同时将起始结点从开启列表中移至关闭列表中。

3、检查更新

寻找开启列表中的f值最小的结点，作为当前待检测结点为n，也就是n到目标结点的最小代价，同时在开启列表中删除n结点，加入到关闭列表中。检查这个待检测结点周围能到达的结点(障碍物和关闭列表的结点不予考虑)，如果周围能达到的结点均在关闭列表中，则忽略，否则进行如下操作：

a、如果周围能到达的结点不在开启列表中则把该结点加入开启列表，并将该待检测结点设置为父结点，并计算相应的f值；

b、如果某个周围结点已经在开启列表中，检查如果用新的路径也就是说先经过当前待检测结点，再到达这个相邻结点的g值是否更小(简单来说就是当前结点的g+1>这个相邻结点的g，则不经过当前结点才是一条更好的路径)，若是则因为这个相邻结点已经有父结点，应该把这个相邻结点的父结点改为当前待检测结点，并且这个相邻结点的g＝当前待检测结点的g+1；若否则保持原来的g(n)和h(n)。如此反复，直到目标结点在开启列表中了，程序结束。

附图3表明了全局上水下机器人的路径选择，A是起始点，B是目标点，3是障碍物。

在没有使用本发明规划方法时，机器人会按照2的路径到达一定位置再进行路径规划，这样的效率会比较低，多走了很多路。当使用了本发明结点算法时，机器人先是通过声呐扫描的地图进行一个区域搜索，搜索最优到达路径1，在一开始就按照规划的路径前进，提高了前进效率。

附图4是机器人的具体行进路线，假设把机器人周围的地图想象成一个二维网图，中间是障碍物C，机器人会根据计算的最优代价值进行行进。

Claims (4)

1.一种基于结点算法的水下机器人路径规划方法，其特征在于：包括以下步骤：

（1）构建水下机器人起始点和目标点之间的周围区域的水下二维网格地图，并显示出周围区域存在的障碍物；每个网格的中心点定义为结点；

（2）初始设置：确定起始结点和目标结点的位置；初始化代价函数、开启列表和关闭列表；将起始结点加入开启列表中；

（3）优化路径选择：判断水下机器人当前位置结点周围是否存在障碍物；

当存在障碍物时，从水下机器人的起始结点开始，向周围访问二维网格地图中的结点，迭代检测开启列表中的结点，并把当前检测结点最靠近的未检测的结点加入开启列表中，已检测的结点移至关闭列表中；该开启列表从初始结点向外扩展，直至达到目标结点；

当不存在障碍物时，选择离目标点最近的结点，寻找最快路径。

2.根据权利要求1所述的水下机器人路径规划方法，其特征在于：所述步骤（2）的初始设置中，起始结点位置设置为（0,0）；代价函数f（n）为f（n）=g(n)+h（n）；其中，g(n)为从初始位置（0,0）沿着已经生产的路径到当前检测结点（x1,y1）形成的代价，起始值为0，每移动一个网格结点，进行更新；h(n)为当前检测结点（x1,y1）到目标结点（x,y）的最小代价。

3.根据权利要求2所述的水下机器人路径规划方法，其特征在于：所述g(n)的更新方式为：向相邻网格结点移动时，增加10；向相对网格结点斜向移动时，增加14。

4.根据权利要求2所述的水下机器人路径规划方法，其特征在于：所述步骤（3）中当存在障碍物时的优化路径选择采用以下方法：寻找当前检测结点可以达到的周围结点，将这些周围邻结点加入开启列表中，并使得开启列表中的结点按照f(n)值的大小升序排序；如果这些周围结点中的某个结点已经存在开启列表中，则比较：先经当前检测结点再到达该周围结点的新路径形成的代价与不经过当前检测结点直接到达该周围结点形成的代价大小，若新路径的形成的代价小，则更新g(n)、h(n)；并将已检测的结点移至关闭列表中；如此反复，直至目标结点出现在开启列表中。