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

摘要

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

主权项

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.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角。