CN110908387A - 一种动态环境水面无人艇路径规划方法、介质及电子设备 - Google Patents

Abstract

本申请公开了一种动态环境水面无人艇路径规划方法、介质及电子设备；简单基于毫米波雷达传感器的反应技术，以确定在导航过程中阻碍水面无人艇路径的障碍物的方向，将毫米波雷达传感器探测范围均分成三个区域角度划定三个均等探测范围，在生成路径前，利用形态学膨胀技术对障碍物膨胀处理，从而通过水面无人艇的当前位置生成新路径来得到规避障碍物后的路径。

Description

一种动态环境水面无人艇路径规划方法、介质及电子设备

技术领域

本申请涉及一种动态环境水面无人艇路径规划方法、介质及电子设备。

背景技术

无人艇是一种无人驾驶、能够自主航行的舰艇，可以通过无线通讯设备对其下达指令或通过自身的控制***实现操纵，多用在一些相对危险的环境或执行一般有人船无法去完成的任务。与无人机、无人车和无人水下航行器共同组成了我们通常所说的四大无人载体***。与其他无人***相比，无人艇发展相对滞后，作为新的科学技术领域，在许多方面都具有极大的发展潜力，如无人艇外型设计、推进***的改进、自动化与自主化操控等。无人艇再动态环境下路径规划是无人艇实现自主化作业的中重要组成部分。

发明人发现，在水面无人艇导航过程中，随机障碍物会阻碍优化生成的路径。障碍物可能是运动的，也可能是静止的；如果正在运行，则需要其方向来确定路径重新规划操作，目前具有利用水面无人艇的角度位置和障碍物的角度位置计算了水面无人艇的方向和障碍物的位置，将这些角度的差异与角度阈值进行比较确定障碍物的方向，然后重新规划行动，虽然在计算中很容易获得水面无人艇的位置，但是比较困难在实时环境中获取障碍物的坐标，因此现有的无人艇避障方法难以满足无人艇巡检作业时的避障需求。

发明内容

本申请的目的是针对现有技术存在的缺陷，提供一种动态环境水面无人艇路径规划方法、介质及电子设备；利用雷达传感器对前方进行探测，在生成路径前，利用形态学膨胀技术对障碍物膨胀处理，从而通过水面无人艇的当前位置生成新路径来得到规避障碍物后的路径。

本申请的第一目的是提供一种动态环境水面无人艇路径规划方法，包括以下步骤：

无人艇通过雷达传感器对前方区域进行扫描，判断安全距离内是否有障碍物；

若安全距离内有障碍物，则通过相机采集地图，将地图图像二值化处理；

判断获取障碍物位置，对障碍物进行形态学膨胀技术处理；

修改并使用目标偏向rrt生成路线图，并使生成的树朝向目标；

应用A*启发式算法对路线图进行优化并得到最短路径；

采用CSI对最短路径进行平滑性处理；

执行路径并继续扫描。

进一步地，进行形态学膨胀技术处理包括：

P代表地图，Q代表膨胀的结构元素，r代表地图上所有点的集合，Qi表示Q集合的映射；

(bmap⊕q)R＝max{bmap(R-x)+q(x)|(R-x)，∈Sbmap&∈Sq}

在生成树之前，将地图上的障碍物进行膨胀；bmap为给定膨胀二值图像矩阵，q(x,y)为结构元素，S为地图维数，R为水面无人艇所需安全空间的半径。

进一步地，在生成路线图的过程中，将随机值rand与随机阈值rt进行比较：

当rand<rt时，则按等式计算上述随机样本方程；当rand≥rt偏置时，使树的生长偏向目标。

进一步地，最近的节点是使用k最近邻计算得出：

其中，urand＝sample；

方向角θ扩展样本以创建新的节点由下式给出：

θ＝atan2(urand-unearest)；

其中，unearest＝Nearest(T，urand)；

新节点由下式给出：

sz1和sz2分别表示不同的步长。

进一步地，A*启发式代价算法的计算如下：

f(p)＝g(p)+h(p)；

g(p)＝c(s，p)是指从初始点s到节点p的代价，h(p)是成本函数的启发式组成部分，具体为：

其中p＝(p0，p1，p2.......,pn)代表路径上的点。

进一步地，应用A*算法后获得的路径为三次样条曲线，对其进行平滑处理：

其中，这里的Pi表示三次函数，

Pi(x)＝a_i+b_ix+c_ix²+d_ix³

这里的ai、bi、ci、di为每个i的系数。

进一步地，在雷达传感器探测的过程中，根据探测障碍物的情况进行判定：

其中，Pcross为移动中的障碍物通过水面无人艇的路径，Psame为障碍物是静止的、在与水面无人艇相同的方向上或在水面无人艇的相同方向上向前移动；如果条件是Pcross，则等待障碍物通过后继续导航，如果条件是Psame，则对路径进行重新规划。

进一步地，所述雷达传感器的探测范围均分为三个探测区域，用于探测障碍物的移动情况；所述的雷达传感器选用毫米波雷达传感器。

本申请的第二目的是提供一种介质，包括以下技术方案：

其上存储有程序，该程序被处理器执行时实现如上所述的动态环境水面无人艇路径规划方法中的步骤。

本申请的第三目的是提供一种电子设备，包括以下技术方案：

包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如上所述的动态环境水面无人艇路径规划方法中的步骤。

与现有技术相比，本申请具有的优点和积极效果是：

在生成路径之前，采用形态学膨胀(MD)技术对障碍物膨胀，并采用三次样条(CSI)的方法对路径进行平滑处理，当随机障碍物阻碍了水面无人艇的导航路径时，通过从水面无人艇的当前位置生成新路径来重新规划路径。改进的RRT-A*方法解决了局部最小问题，并在部分已知环境中以最低的时间代价生成了安全和最优的路径。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是本发明实施例1中规划方法的流程示意图；

图2为本发明实施例1中雷达传感器探测示意图。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步地说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；

正如背景技术中所介绍的，现有技术中利用水面无人艇的角度位置和障碍物的角度位置计算了水面无人艇的方向和障碍物的位置，将这些角度的差异与角度阈值进行比较确定障碍物的方向，然后重新规划行动，虽然在计算中很容易获得水面无人艇的位置，但是比较困难在实时环境中获取障碍物的坐标，因此现有的无人艇避障方法难以满足无人艇巡检作业时的避障需求，针对上述技术问题，本申请提出了一种动态环境水面无人艇路径规划方法、介质及电子设备。

实施例1

本申请的一种典型的实施方式中，如图1-图2所示，提出了一种动态环境水面无人艇路径规划方法。

方法分为5步：

步骤一：获取和处理地图；

步骤二：计算RRT路线图；

步骤三：路径查询与优化；

步骤四：路径平滑；

步骤五：路径重规划。

具体的：在生成路径之前，采用形态学膨胀(MD)技术对障碍物膨胀，并采用三次样条(CSI)的方法对路径进行平滑处理，当随机障碍物阻碍了水面无人艇的导航路径时，通过从水面无人艇的当前位置生成新路径来重新规划路径。改进的RRT-A*方法解决了局部最小问题，并在部分已知环境中以最低的时间代价生成了安全和最优的路径。

步骤一：获取和处理地图

1.相机来采集地图并将地图二值化；

2.为了避免碰撞，在构建RRT路线前，先对障碍物MD处理。

MD表达式：

P代表地图，Q代表膨胀的结构元素，r代表地图上所有点的集合，Qi表示Q集合的映射；

(bmap⊕q)R＝max{bmap(R-x)+q(x)|(R-x)，∈Sbmap&∈Sq} (2)

bmap为给定膨胀二值图像矩阵，q(x,y)为结构元素S为地图维数，R为水面无人艇所需安全空间的半径。

方程(2)用于在生成树之前将地图上的障碍物充气，以确保航行安全。

步骤二：计算RRT路线图

本发明修改并使用目标偏向rrt来生成路线图，以确保快速生成的树朝向目标。当确定样本计算的随机值rand小于随机阈值rt时，生成的树偏向于目标。两种不同的步骤大小，sz1和sz2(被用于分配比sz1更高的值)。当生长偏向目标时，sz2用于树的生长。这是为了减少在到达目标之前生成的节点数，从而减少时间和算法的空间复杂度。

将生成的随机值rand与随机阈值(rt)进行比较。如果rand<rt，则按等式计算随机样本方程(3)。当rand≥rt偏置时，使树的生长偏向目标。

最近的节点是使用k最近邻计算得来。公式如下：

在这里得urand＝sample由等式(3)给出。

方向角θ扩展样本以创建新的节点由等式(5)给出。

θ＝atan2(urand-unearest) (5)

这里得unearest＝Nearest(T，urand)。

新节点的计算公式由等式(6)给出。

sz1和sz2分别表示不同的步长，如果urand＝target，使用sz2计算新节点unew具有较高步长朝向目标的位置。这是为了促进树朝着目标以更快的速度生长并节点数减少。执行碰撞检查以确保树是在CSfree的范围内生成的。一旦到达目标点或迭代尝试终止，树的增长就会结束。CS表示重构空间，包括CSobs障碍物空间和CSfree空闲空间。

步骤三：路径查询与优化

本发明先利用改进的目标偏置RRT算法生成路径之后应用A*启发式算法对路径图进行优化并得到最短路径。

A*启发式代价算法的公式如下：

f(p)＝g(p)+h(p) (7)

这里g(p)＝c(s，p)是指从初始点s到节点p的代价。

h(p)是成本函数的启发式组成部分，估计从节点p到目标点的最低成本。

h(p)的公式如下：

这里p＝(p0，p1，p2.......,pn)代表路径上的点。

A*之所以被考虑，是因为它既是完全的，也是最优的。它是完整的，因为一旦一个路径存在于CSfree中，A*就可以找到该路径。A*是可接受和连续的。可接受性和连续性是最优性的性质。如果g(P)是到达点p的实际成本，那么f(P)就不会高估达到目标的成本，这使得A*是可以接受。考虑c(p，p’)作为从点p到p’的代价，连续性是从h(p)≤c(p,p’)+h(p’)实现的，而可接受性是从路径的所有弧c(p，p’)>ε≥0实现的。

步骤四：路径平滑

在应用A*算法后获得的路径不够平滑，无法实现水面无人艇的轻松导航。采用CSI来提高优化路径的平滑性。对于三次样条曲线，使用针对每个数据点间隔的不同三次函数来表示曲线的函数。考虑到m数据点，三次样条曲线函数如下表示：

这里的Pi表示三次函数。通常，三次样条定义为

Pi(x)＝a_i+b_ix+c_ix²+d_ix³ (10)

这里的ai、bi、ci、di为每个i的系数，因此对于m个节点间隔需要计算4m个系数。在计算道路之前，提出了MD技术将地图中的障碍物膨胀，在本发明中，降低了发生碰撞的可能性。

步骤五：路径重规划

本发明提出一种简单基于毫米波雷达传感器的反应技术，以确定在导航过程中阻碍水面无人艇路径的障碍物的方向。将毫米波雷达传感器探测范围均分成三个区域角度划定三个均等探测范围。作出决定需要障碍物的两个主要条件：(1)移动中的障碍物是否通过水面无人艇的路径,Pcross；(2)障碍物是否是静止的，在与水面无人艇相同的方向上或在水面无人艇的相同方向上向前移动,Psame。给出了水面无人艇的预期重计划动作Ra:

如果条件是PCross,水面无人艇等待障碍物通过，则继续导航。如果条件是Psame，则执行路径重新规划。

参照图2，sensor为水面无人艇上的毫米波雷达传感器，(s1，s2，s3)为毫米波雷达传感器均分的三个区域，(θs1，θs2，θs3)为毫米波雷达传感器均分的三个区域角度，(p1，p2，p3)为同一个障碍物的三个状态点，d1，d2分别表示不同的距离阈值，sd是水面无人艇与障碍物的安全距离。

毫米波雷达传感器在导航过程中跟踪障碍物。当毫米波雷达传感器的最小距离dist在水面无人艇和障碍物之间的距离小于d1时，这表明水面无人艇在较远的距离探测到了一个障碍物，对水面无人艇的碰撞威胁较小,探测障碍物的毫米波雷达传感器被记录下来。为了检查障碍物的方向，毫米波雷达传感器读数被记录到dist≤d2，并记录带有读数的毫米波雷达传感器。

如果同一个区域的毫米波雷达传感器在dist≤d2探测到障碍物，Psame条件满足,并进行了调用，重新规划和计算了一条新的可行导航路径,路径重新规划任务包括前四步：

步骤一：获取和处理地图

步骤二：计算RRT路线图

步骤三：路径查询与优化

步骤四：路径平滑

水面无人艇的当前位置被用作重新规划的新节点。

但是，如果在dist≤d2,毫米波雷达传感器的其他区域检测到障碍物，则满足Pcross条件。水面无人艇停下来，等待dist>d1。

如图2所示：有一个障碍物obs处于分别在P1和P2点处在毫米波雷达传感器的s3和s2区域被侦察到，由于毫米波雷达传感器的最小距离dist处在d2和d1的中间区域，对水面无人艇的碰撞威胁较小，探测障碍物的毫米波雷达传感器的s3和s2被记录下来。之后障碍物obs在P3点毫米波雷达传感器的s1区域被侦察到，由于毫米波雷达传感器的最小距离dist处在d2之内，对水面无人艇有碰撞威胁，记录带有读数的毫米波雷达传感器。该状态满足Pcross，因此在dist≤d1，水面无人艇停止运动并等待障碍物通过，否则将会重新规划。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种动态环境水面无人艇路径规划方法，其特征在于，包括以下步骤：

无人艇通过雷达传感器对前方区域进行扫描，判断安全距离内是否有障碍物；

若安全距离内有障碍物，则通过相机采集地图，将地图图像二值化处理；

判断获取障碍物位置，对障碍物进行形态学膨胀技术处理；

修改并使用目标偏向rrt生成路线图，并使生成的树朝向目标；

应用A*启发式算法对路线图进行优化并得到最短路径；

采用CSI对最短路径进行平滑性处理；

执行路径并继续扫描。

2.如权利要求1所述的动态环境水面无人艇路径规划方法，其特征在于，所述雷达传感器的探测范围均分为三个探测区域，用于探测障碍物的移动情况。

3.如权利要求1所述的动态环境水面无人艇路径规划方法，其特征在于，进行形态学膨胀技术处理包括：

P代表地图，Q代表膨胀的结构元素，r代表地图上所有点的集合，Qi表示Q集合的映射；

(bmap⊕q)R＝max{bmap(R-x)+q(x)|(R-x)，∈Sbmap&∈Sq}；

在生成树之前，将地图上的障碍物进行膨胀；bmap为给定膨胀二值图像矩阵，q(x,y)为结构元素，S为地图维数，R为水面无人艇所需安全空间的半径。

4.如权利要求3所述的动态环境水面无人艇路径规划方法，其特征在于，在生成路线图的过程中，将随机值rand与随机阈值rt进行比较：

当rand<rt时，则按等式计算上述随机样本方程；当rand≥rt偏置时，使树的生长偏向目标。

5.如权利要求4所述的动态环境水面无人艇路径规划方法，其特征在于，最近的节点是使用k最近邻计算得出：

其中，urand＝sample；

方向角θ扩展样本以创建新的节点由下式给出：

θ＝atan2(urand-unearest)；

其中，unearest＝Nearest(T，urand)；

新节点由下式给出：

；sz1和sz2分别表示不同的步长。

6.如权利要求5所述的动态环境水面无人艇路径规划方法，其特征在于，A*启发式代价算法的计算如下：

f(p)＝g(p)+h(p)；

g(p)＝c(s，p)是指从初始点s到节点p的代价，h(p)是成本函数的启发式组成部分，具体为：

其中p＝(p0，p1，p2.......,pn)代表路径上的点。

7.如权利要求6所述的动态环境下水面无人艇路径规划方法，其特征在于，应用A*算法后获得的路径为三次样条曲线，对其进行平滑处理：

其中，这里的Pi表示三次函数，

Pi(x)＝a_i+b_ix+c_ix²+d_ix³

这里的ai、bi、ci、di为每个i的系数。

8.如权利要求1所述的动态环境水面无人艇路径规划方法，其特征在于，在雷达传感器探测的过程中，根据探测障碍物的情况进行判定：

其中，Pcross为移动中的障碍物通过水面无人艇的路径，Psame为障碍物是静止的、在与水面无人艇相同的方向上或在水面无人艇的相同方向上向前移动；如果条件是Pcross，则等待障碍物通过后继续导航，如果条件是Psame，则对路径进行重新规划。

9.一种介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-7任一项所述的动态环境水面无人艇路径规划方法中的步骤。

10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7任一项所述的动态环境水面无人艇路径规划方法中的步骤。

Images