[发明专利]一种基于几何法的无人艇动态避障方法和系统

权利要求书

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。