CN109960262B - 一种基于几何法的无人艇动态避障方法和系统 - Google Patents

Abstract

本发明公开了一种基于几何法的无人艇动态避障方法和系统，属于路径规划领域。本发明在无人艇感知传感器的检测范围内，计算无人艇和每个障碍物的碰撞风险，对于存在碰撞风险的障碍物，提前进行避障动作，从障碍物运动的后方避开障碍物，保障无人艇的安全，在避障过程中，无人艇基本保持直线航向，在避障点处进行航向调整，缩短无人艇的避障航程，该避障方法对静止障碍物和动态障碍物都具有良好可靠的避障效果。本发明基于几何原理计算每个障碍物的避障点，采用LOS跟踪方法对避障点进行跟踪，间接控制无人艇的航向，降低了路径规划对无人艇运动模型的依赖。

Description

一种基于几何法的无人艇动态避障方法和系统

技术领域

本发明属于路径规划领域，更具体地，涉及一种基于几何法的无人艇动态避障方法和系统。

背景技术

水面无人艇(Unmanned Surface Vessel，USV)，简称无人艇，是一种水面智能机器人，能够实现自主航行、自主路径规划以及自主作业，并且能够自主感知环境信息。无人艇无论是在军事上还是在民用上都具有广泛的应用，可以完成一定的海上作战任务，对制定区域的探测、搜索、监视任务，以及对特定目标的围捕等和海岛之间物资投送任务等军事任务，还可以完成河湖水资源的检测、堰塞湖探测和水上救援等民用功能。海上环境复杂，不仅存在暗礁、岛屿、灯塔等静止障碍物，同时也存在其他船只和大型漂浮物等动态障碍物，要求无人艇能够在自主完成航行完成任务，则无人艇必须能够独立自主完成路径规划，因此路径规划是无人艇能够安全自主航行的关键。

路径规划分为基于已知地理信息全局路径规划和基于传感器信息的局部路径规划：基于已知地理信息全局路径规划时，全局路径规划在与任务下达给无人艇的时候，无人艇根据出发点和目标点的位置以及已知的地理信息，规划出一条能够避开已知静止障碍物的路径；基于传感器信息的局部路径规划时，在无人艇执行任务的过程中，为了保证无人艇的安全航行，根据艇上装备的实时环境感知传感器信息，在检测新的动态或静态障碍物时，进行局部路径规划，避开新出现的障碍物。

然而，无人艇与其他机器人相比，具有大惯性、长时延、非线性、模型复杂等运动特性，目前无人艇局部路径规划技术极少结合无人艇的运动特性，实际的避障路径不理想；另外，目前无人艇局部路径规划算法基本采用直接控制无人艇航向的方向，直接控制无人艇航向的方法则需要基于无人艇的运动学模型，对不同运动学模型的无人艇不具备通用性。

发明内容

针对现有技术的缺陷，本发明的目的在于解决现有技术无人艇局部路径规划方法避障路径不理想、不具备通用性的技术问题。

为实现上述目的，第一方面，本发明实施例提供了一种基于几何法的无人艇动态避障方法，该方法包括以下步骤：

S0.获取无人艇的目标点位置信息，初始化避障点集合为{目标点}，判断检测到的障碍物个数n是否大于0，若是，初始化t为1，进入步骤S1，否则，直接进入步骤S7；

S1.判断t是否大于n，若是，进入步骤S7，否则，进入步骤S2；

S2.获取无人艇与第t个障碍物的位置信息和速度信息；

S3.根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数；

S4.根据无人艇和目标点之间位置关系参数，判断无人艇是否到达目标点，若是，停止动态避障，否则，进入步骤S5；

S5.根据无人艇与第t个障碍物之间位置关系参数、无人艇与第t个障碍物的速度信息、无人艇与目标点之间位置关系参数，判断无人艇与第t个障碍物是否存在碰撞风险，若是，进入步骤S6，否则，t加1，进入步骤S1；

S6.计算该障碍物对应的避障点，加入避障点集合，t加1，进入步骤S1；

S7.对避障点集合的每个避障点，计算其到无人艇的距离，找出距离无人艇最近的避障点；

S8.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角。

具体地，所述根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数，具体如下：

无人艇坐标到障碍物圆心坐标方向与正东方向的夹角

根据无人艇坐标(x_a,y_a)与障碍物圆心坐标(x_o,y_o)采用夹角公式计算得到；

无人艇与障碍物圆心和无人艇与目标点连线的夹角

由

和

计算得到，计算公式如下：

其中，

为无人艇坐标到目标点坐标方向与正东方向的夹角；

上一个计算周期

的值；

无人艇与障碍物圆心和无人艇与避障点连线的夹角

计算公式如下：

其中，r₀为障碍物安全范围圆的半径，d₃为无人艇与障碍物圆心的距离；

无人艇与避障点连线和正东方向的夹角

由

和

计算得到：

当选择障碍物安全范围圆右边切点为避障点时，计算公式如下：

当选择障碍物安全范围圆左边切点为避障点时，计算公式如下：

无人艇与目标点之间的距离d₀，根据无人艇坐标(x_a,y_a)与目标点坐标(x_t,y_t)采用距离公式计算得到；

障碍物圆心到无人艇与目标点连线的距离d₁，由d₃和

计算得到，计算公式如下：

d′₁：上一个计算周期d₁的值；

障碍物到避障点的距离d₂，也就是障碍物安全范围圆的半径，d₂＝r₀；

无人艇与障碍物圆心的距离d₃，可根据无人艇与障碍物圆心坐标采用距离公式计算得到；

无人艇到避障点的距离d₄，由d₃和

计算得到，计算公式如下：

无人艇在保持当前速度航向情况下，与障碍物之间的出现的最短距离d_min，计算公式如下：

m₁＝X_o-X_a

m₂＝y_o-y_a

m₁、n₁、m₂、n₂为中间常量。

具体地，圆形障碍物半径加上无人艇与障碍物的安全距离，作为该障碍物的安全范围圆的半径r_o。

具体地，无人艇与障碍物的安全距离为5L，L为无人艇的长度。

具体地，若d₀≤L，即无人艇与目标点的距离小于一倍船长，判断无人艇已到达目标点；若d₀>L，则无人艇未达到目标点。

具体地，判断以下三个条件任一个是否满足：(1)障碍物的速度大小v_o＝0；(2)v_o≠0且

且v_o<v_a且

若满足，根据障碍物圆心到无人艇与目标点连线的距离d₁和

判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

否则，根据d_min和无人艇与障碍物之间的安全距离r_o，判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

具体地，判断以下三个条件任一个是否满足：(1)障碍物的速度大小v_o＝0；(2)v_o≠0且

且v_o<v_a且

若满足，避障点的选择依据为：

时，选取安全范围圆右边切点为避障点，

时，选取安全范围圆左边切点为避障点；

否则，避障点的选择依据为：

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆左切点为避障点；

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆右切点为避障点。

具体地，选择避障点之后，计算避障点坐标(x_b,y_b)，计算公式如下：

第二方面，本发明实施例提供了一种基于几何法的无人艇动态避障系统，所述无人艇动态避障系统采用上述第一方面所述的无人艇动态避障方法。

第三方面，本发明实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述第一方面所述的无人艇动态避障方法。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

1.本发明基于数学几何的原理计算避障点，采用LOS跟踪方法对避障点进行跟踪，通过跟踪避障点的方法间接控制无人艇的航向，而不是直接控制无人艇航，降低了路径规划对无人艇运动模型的依赖。

2.本发明在无人艇感知传感器的检测范围内，计算碰撞风险，提前进行避障动作，从障碍物运动的后方避开障碍物，保障无人艇的安全，在避障过程中，无人艇基本保持直线航向，在避障点处进行航向调整，缩短无人艇的避障航程。

附图说明

图1为本发明实施例提供的一种基于几何法的无人艇动态避障方法流程图；

图2为本发明实施例提供的障碍物、无人艇、目标点与避障点位置示意图；

图3为本发明实施例提供的避障点在障碍物安全范围圆右边切点示意图；

图4为本发明实施例提供的避障点在障碍物安全范围圆左边切点示意图；

图5为本发明实施例提供的MATLAB平台上对静止障碍物的仿真结果图；

图6为本发明实施例提供的MATLAB平台上对动态障碍物的仿真结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

夹角公式

夹角

表示坐标(x₁,y₁)到坐标(x₂,y₂)方向与x轴正方向的夹角，s表示两个坐标点的距离公式。

如图1所示，一种基于几何法的无人艇动态避障方法，该方法包括以下步骤：

S0.获取无人艇的目标点位置信息，初始化避障点集合为{目标点}，判断检测到的障碍物个数n是否大于0，若是，初始化t为1，进入步骤S1，否则，直接进入步骤S7；

S1.判断t是否大于n，若是，进入步骤S7，否则，进入步骤S2；

S2.获取无人艇与第t个障碍物的位置信息和速度信息；

S3.根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数；

S4.根据无人艇和目标点之间位置关系参数，判断无人艇是否到达目标点，若是，停止动态避障，否则，进入步骤S5；

S5.根据无人艇与第t个障碍物之间位置关系参数、无人艇与第t个障碍物的速度信息、无人艇与目标点之间位置关系参数，判断无人艇与第t个障碍物是否存在碰撞风险，若是，进入步骤S6，否则，t加1，进入步骤S1；

S6.计算该障碍物对应的避障点，加入避障点集合，t加1，进入步骤S1；

S7.对避障点集合的每个避障点，计算其到无人艇的距离，找出距离无人艇最近的避障点；

S8.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角。

步骤S0.获取无人艇的目标点位置信息，初始化避障点集合为{目标点}，判断检测到的障碍物个数n是否大于0，若是，初始化t为1，进入步骤S1，否则，直接进入步骤S7。

目标点是指无人艇执行任务的目的地，在整个动态避障过程是固定不变的。获取无人艇的目标点位置信息，目标点位置坐标表示为(x_t,y_t)。无人艇在避障过程中，遇到的可能不止存在一个障碍物，因此，需要计算每一个障碍物的避障点，最后会输出一个避障点的集合{x_b1,y_b1；x_b2,y_b2…x_bn,y_bn}，n为障碍物的数量。如果n＝0，即不存在障碍物。

步骤S1.判断t是否大于n，若是，进入步骤S7，否则，进入步骤S2。

步骤S2.获取无人艇与第t个障碍物的位置信息和速度信息。

如图2所示，障碍物用字母O表示，无人艇用字母A表示，目标点用字母T表示，避障点用字母B表示。

根据差分GPS，获得无人艇的经纬度坐标和速度。将无人艇经纬度坐标信息转化为北东地坐标系(x,y)下的位置信息，正东方向为x轴正方向，正北方向为y轴正方向，选取差分GPS基站为坐标原点，障碍物运动方向角以x轴正方向逆时针旋转为正，范围为0～2π。无人艇位置和速度信息用

表示，(x_a,y_a)表示无人艇的位置坐标，v_a表示无人艇速度大小，

表示无人艇坐标到目标点坐标方向与x轴正方向的夹角，可根据无人艇坐标和目标点坐标采用夹角公式计算得到。

障碍物包括静止障碍物和动态障碍物，静态障碍物如小岛、灯塔和暗礁等，动态障碍物如其他运动船只和其他大型漂浮物等。基于视觉或雷达传感器实时检测障碍物信息，获取在传感器检测范围内的所有障碍物的尺寸、位置以及速度信息。障碍物位置和速度信息用

表示，(x_o,y_o)表示障碍物圆心坐标，v_o表示障碍物速度大小，

表示障碍物运动方向与x轴正方向的夹角，r_o表示障碍物安全范围圆的半径。

根据障碍物尺寸的大小，将障碍物建模为大小不同圆形障碍物。根据无人艇的长度L，设定无人艇与障碍物的安全距离，一般设定为5L。圆形障碍物半径加上无人艇与障碍物的安全距离，作为该障碍物的安全范围圆的半径r_o。如果两个障碍物的安全范围圆存在相交或者相切的情况，则这两个障碍物合并为一个尺寸更大的障碍物。

步骤S3.根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数。

输入(x_t,y_t)、

计算以下无人艇与障碍物之间位置关系参数、无人艇和目标点之间位置关系参数。

无人艇坐标到障碍物圆心坐标方向与x轴正方向的夹角

可根据无人艇坐标(x_a,y_a)与障碍物圆心坐标(x_o,y_o)采用夹角公式计算得到。

无人艇与障碍物圆心和无人艇与目标点连线的夹角

由

和

计算得到，计算公式如下：

其中，

为无人艇坐标到目标点坐标方向与x轴正方向的夹角，

为无人艇坐标到障碍物圆心坐标方向与x轴正方向的夹角。

具有以下作用：

(1)用于计算障碍物圆心到无人艇与目标点连线的距离d₁。

(2)根据

的正负可以判断障碍物在无人艇与目标点连线的左右。

(3)作为判断障碍物与无人艇是否存在碰撞风险参数之一。

上一个计算周期

的值。

无人艇与障碍物圆心和无人艇与避障点连线的夹角

计算公式如下：

其中，r₀为障碍物安全范围圆的半径，d₃为无人艇与障碍物圆心的距离。

无人艇与避障点连线和x轴正方向的夹角

用于计算避障点的坐标，由

和

计算得到，计算公式如下：

如图3所示，当选择障碍物安全范围圆右边切点为避障点时：

如图4所示，当选择障碍物安全范围圆左边切点为避障点时：

无人艇与目标点之间的距离d₀，根据无人艇(x_a,y_a)与目标点的坐标(x_t,y_t)采用距离公式计算得到，用于判断是否已经到达目标点。

障碍物圆心到无人艇与目标点连线的距离d₁，由d₃和

计算得到，计算公式如下：

d′₁：上一个计算周期d₁的值。

障碍物圆到避障点的距离d₂，避障点为障碍物安全范围圆切点时d₂＝r₀。

无人艇与障碍物圆心的距离d₃，可根据无人艇与障碍物圆心坐标采用距离公式计算得到。

无人艇到避障点的距离d₄，由d₃和

计算得到，用于计算避障点的坐标，计算公式如下：

无人艇在保持当前速度航向情况下，与障碍物之间的出现的最短距离d_min，计算公式如下：

m₁＝X_o-X_a

m₂＝y_o-y_a

m₁、n₁、m₂、n₂为中间常量。

步骤S4.根据无人艇和目标点之间位置关系参数，判断无人艇是否到达目标点，若是，停止动态避障，否则，进入步骤S5。

如果当d₀≤L，即无人艇与目标点的距离小于一倍船长，判断无人艇已到达目标点；如果d₀>L，则无人艇没有达到目标点。该判断可有效避免检测到的障碍物位于目标点之后的情形。

步骤S5.根据无人艇与第t个障碍物之间位置关系参数、无人艇与第t个障碍物的速度信息、无人艇与目标点之间位置关系参数，判断无人艇与第t个障碍物是否存在碰撞风险，若是，进入步骤S6，否则，t加1，进入步骤S1。

根据障碍物的位置信息和速度信息，将障碍物分为静止、对遇、被超越和其他动态避障模式四种情况。在判断无人艇与障碍物是否存在碰撞风险时，根据障碍物的运动情况，需要分四种情况进行讨论，分别为障碍物在静态障碍物、“对遇型”动态障碍物、“超越型”动态障碍物和其他动态障碍物四种类型。

(1)静态障碍物

根据环境感知传感器输出的信息，如果障碍物的速度大小v_o＝0，则该障碍物为静止障碍物。对于静止障碍物，根据障碍物圆心到无人艇与目标点连线的距离d₁和

进行判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

(2)“对遇型”动态障碍物

根据环境感知传感器输出的信息，v_o≠0且

时，无人艇与障碍物为“对遇型”情况。在对遇情况下，根据障碍物圆心到无人艇与目标点连线的距离d₁和

进行判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

(3)“超越型”动态障碍物

根据环境感知传感器输出的信息，满足以下情况时，为无人艇超越障碍物情况。当v_o≠0且v_o<v_a且

时，为“超越型”动态障碍物，在无人艇超越障碍物情况下，根据障碍物圆心到无人艇与目标点连线的距离d₁和

进行判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

(4)其他动态障碍物

在其他动态障碍物情况下，根据无人艇与障碍物当前的位置信息和速度信息计算得到无人艇和障碍保持目前航行状态会出现的最短距离d_min，根据d_min和无人艇与障碍物之间的安全距离r_o可以判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

检测到存在碰撞风险之后，通过跟踪避障点开始避障动作。

步骤S6.计算该障碍物对应的避障点，加入避障点集合，t加1，进入步骤S1。

下面为存在碰撞风险的4个类型障碍物避障点的选择依据。

1)静止障碍物、“对遇型”障碍物、“超越型”障碍物的避障点的选择依据为：

2)其他动态障碍物避障点的选择依据为：

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆左切点避障点；

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆右切点避障点。

对于其他动态障碍物，选择的避障点位于障碍物运动的后方。

选择避障点之后，计算避障点坐标(x_b,y_b)，计算公式如下：

步骤S7.对避障点集合的每个避障点，计算其到无人艇的距离，找出距离无人艇最近的避障点。

输出的避障点的集合{x_b1,y_b1；x_b2,y_b2…x_bn,y_bn}，n为障碍物的数量。采用距离公式计算每个避障点(x_bi,y_bi)到无人艇(x_a,y_a)之间的距离{s₁,s₂…s_n}，n为障碍物的数量。

选出距离无人艇最近的避障点(x_bmin,y_bmin)，即对应的距离最小。

步骤S8.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角。

采用夹角公式计算出无人艇坐标(x_a,y_a)到距离无人艇最近的避障点坐标(x_bmin,y_bmin)方向与x轴正方向的夹角

角作为无人艇的航向角。无人艇就根据这个航向角航行就可以顺利避障。

根据LOS(line-of-sight，视线)导航算法，如果使被控无人艇的航向保持对准视线角(LOS角)，那么经过适当的控制，就能使被控船舶达到预期的位置。无人艇基本保持直线航向，在避障点处进行航向调整，缩短无人艇的避障航程。

MATLAB平台上对静止障碍物的仿真结果如图5所示，MATLAB平台上对动态障碍物的仿真结果如图6所示。本算法根据无人艇的运动特性进行实时避障点计算和跟踪，通过仿真实验证明，对静止障碍物和动态障碍物都具有良好可靠的避障效果。

以上，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (9)

1.一种基于几何法的无人艇动态避障方法，其特征在于，该方法包括以下步骤：

S0.获取无人艇的目标点位置信息，初始化避障点集合为{目标点}，判断检测到的障碍物个数n是否大于0，若是，初始化t为1，进入步骤S1，否则，直接进入步骤S7；

S1.判断t是否大于n，若是，进入步骤S7，否则，进入步骤S2；

S2.获取无人艇与第t个障碍物的位置信息和速度信息；

S3.根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数；

S4.根据无人艇和目标点之间位置关系参数，判断无人艇是否到达目标点，若是，停止动态避障，否则，进入步骤S5；

S5.根据无人艇与第t个障碍物之间位置关系参数、无人艇与第t个障碍物的速度信息、无人艇与目标点之间位置关系参数，判断无人艇与第t个障碍物是否存在碰撞风险，若是，进入步骤S6，否则，t加1，进入步骤S1；

S6.计算该障碍物对应的避障点，加入避障点集合，t加1，进入步骤S1；

S7.对避障点集合的每个避障点，计算其到无人艇的距离，找出距离无人艇最近的避障点；

S8.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角；

所述根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数，具体如下：

无人艇坐标到障碍物圆心坐标方向与正东方向的夹角

根据无人艇坐标(x_a,y_a)与障碍物圆心坐标(x_o,y_o)采用夹角公式计算得到；

无人艇与障碍物圆心和无人艇与目标点连线的夹角

由

和

计算得到，计算公式如下：

其中，

为无人艇坐标到目标点坐标方向与正东方向的夹角；

无人艇与障碍物圆心和无人艇与避障点连线的夹角

计算公式如下：

其中，r₀为障碍物安全范围圆的半径，d₃为无人艇与障碍物圆心的距离；

无人艇与避障点连线和正东方向的夹角

由

和

计算得到：

当选择障碍物安全范围圆右边切点为避障点时，计算公式如下：

当选择障碍物安全范围圆左边切点为避障点时，计算公式如下：

无人艇与目标点之间的距离d₀，根据无人艇坐标(x_a,y_a)与目标点坐标(x_t,y_t)采用距离公式计算得到；

障碍物圆心到无人艇与目标点连线的距离d₁，由d₃和

计算得到，计算公式如下：

障碍物到避障点的距离d₂，也就是障碍物安全范围圆的半径，d₂＝r₀；

无人艇与障碍物圆心的距离d₃，可根据无人艇与障碍物圆心坐标采用距离公式计算得到；

无人艇到避障点的距离d₄，由d₃和

计算得到，计算公式如下：

无人艇在保持当前速度航向情况下，与障碍物之间的出现的最短距离d_min，计算公式如下：

m₁＝x_o-x_a

m₂＝y_o-y_a

m₁、n₁、m₂、n₂为中间常量，v_o为障碍物速度大小，v_a为无人艇速度大小，

为障碍物运动方向与X轴正方向的夹角。

2.如权利要求1所述的无人艇动态避障方法，其特征在于，圆形障碍物半径加上无人艇与障碍物的安全距离，作为该障碍物的安全范围圆的半径r_o。

3.如权利要求2所述的无人艇动态避障方法，其特征在于，无人艇与障碍物的安全距离为5L，L为无人艇的长度。

4.如权利要求1所述的无人艇动态避障方法，其特征在于，若d₀≤L，即无人艇与目标点的距离小于一倍船长，判断无人艇已到达目标点；若d₀>L，则无人艇未达到目标点。

5.如权利要求1所述的无人艇动态避障方法，其特征在于，判断以下三个条件任一个是否满足：(1)障碍物的速度大小v_o＝0；(2)v_o≠0且

(3)v_o≠0且v_o<v_a且

若满足，根据障碍物圆心到无人艇与目标点连线的距离d₁和

判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

否则，根据d_min和无人艇与障碍物之间的安全距离r_o，判断无人艇与障碍物是否存在碰撞风险，判断方式如下：

6.如权利要求1所述的无人艇动态避障方法，其特征在于，判断以下三个条件任一个是否满足：(1)障碍物的速度大小v_o＝0；(2)v_o≠0且

(3)v_o≠0且v_o<v_a且

若满足，避障点的选择依据为：

时，选取安全范围圆右边切点为避障点，

时，选取安全范围圆左边切点为避障点；

否则，避障点的选择依据为：

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆左切点为避障点，

为上一个计算周期

的值，d′₁为上一个计算周期d₁的值；

当(

且d₁-d′₁<0)或者(

且

)时，选择安全范围圆右切点为避障点。

7.如权利要求6所述的无人艇动态避障方法，其特征在于，选择避障点之后，计算避障点坐标(x_b,y_b)，计算公式如下：

8.一种基于几何法的无人艇动态避障系统，其特征在于，所述无人艇动态避障系统采用如权利要求1至7任一项所述的无人艇动态避障方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的无人艇动态避障方法。

Images