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

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

A Dynamic Obstacle Avoidance Method and System for Unmanned Vehicle Based on Geometric Method

【技术实现步骤摘要】
一种基于几何法的无人艇动态避障方法和系统
本专利技术属于路径规划领域，更具体地，涉及一种基于几何法的无人艇动态避障方法和系统。
技术介绍
水面无人艇(UnmannedSurfaceVessel，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个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数，具体如下：无人艇坐标到障碍物圆心坐标方向与正东方向的夹角根据无人艇坐标(xa,ya)与障碍物圆心坐标(xo,yo)采用夹角公式计算得到；无人艇与障碍物圆心和无人艇与目标点连线的夹角由和计算得到，计算公式如下：其中，为无人艇坐标到目标点坐标方向与正东方向的夹角；上一个计算周期的值；无人艇与障碍物圆心和无人艇与避障点连线的夹角计算公式如下：其中，r0为障碍物安全范围圆的半径，d3为无人艇与障碍物圆心的距离；无人艇与避障点连线和正东方向的夹角由和计算得到：当选择障碍物安全范围圆右边切点为避障点时，计算公式如下：当选择障碍物安全范围圆左边切点为避障点时，计算公式如下：无人艇与目标点之间的距离d0，根据无人艇坐标(xa,ya)与目标点坐标(xt,yt)采用距离公式计算得到；障碍物圆心到无人艇与目标点连线的距离d1，由d3和计算得到，计算公式如下：d′1：上一个计算周期d1的值；障碍物到避障点的距离d2，也就是障碍物安全范围圆的半径，d2＝r0；无人艇与障碍物圆心的距离d3，可根据无人艇与障碍物圆心坐标采用距离公式计算得到；无人艇到避障点的距离d4，由d3和计算得到，计算公式如下：无人艇在保持当前速度航向情况下，与障碍物之间的出现的最短距离dmin，计算公式如下：m1＝Xo-Xam2＝yo-yam1、n1、m2、n2为中间常量。具体地，圆形障碍物半径加上无人艇与障碍物的安全距离，作为该障碍物的安全范围圆的半径ro。具体地，无人艇与障碍物的安全距离为5L，L为无人艇的长度。具体地，若d0≤L，即无人艇与目标点的距离小于一倍船长，判断无人艇已到达目标点；若d0>L，则无人艇未达到目标点。具体地，判断以下三个条件任一个是否满足：(1)障碍物的速度大小vo＝0；(2)vo≠0且且vo<va且若满足，根据障碍物圆心到无人艇与目标点连线的距离d1和判断无人艇与障碍物是否存在碰撞风险，判断方式如下：否则，根据dmin和无人艇与障碍物之间的安全距离ro，判断无人艇与障碍物是否存在碰撞风险，判断方式如下：具体地，判断以下三个条件任一个是否满足：(1)障碍物的速度大小vo＝0；(2)vo≠0且且vo<va且若满足，避障点的选择依据为：时，选取安全范围圆右边切点为避障点，时，选取安全范围圆左边切点为避障点；否则，避障点的选择依据为：当(且d1-d′1<0)或者(且)时，选择安全范围圆左切点为避障点；当(且d1-d′1<0)或者(且)时，选择安全范围圆右切点为避障点。具体地，选择避障点之后，计算避障点坐标(xb,yb)，计算公式如下：第二方面，本专利技术实施例提供了一种基于几何法的无人艇动态避障系统，所述无人艇动态避障系统采用上述第一方面所述的无人艇动态避障方法。第三方面，本专利技术实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述第一方面所述的无人艇动态避障方法。总体而言，通过本专利技术所构思的以上技术方案与现有技术相比，具有以下有益效果：1.本专利技术基于数学几何的原理计算避障点，采用LOS跟踪方法对避障点进行跟踪，通过跟踪避障点的方法间接控制无人艇的航向，而不是直接控制无人艇航，降低了路径规划对无人艇运动模型的依赖。2.本专利技术在无人艇感知传感器的检测范围内，计算碰撞风险，提前进行避障动作，从障碍物运动的后方避开障碍物，保障无人艇的安全，在避障过程中，无人艇基本保持直线航向，在避障点处进行航向调整，缩短无人艇的避障航程。附图说明图1为本专利技术实施例提供的一种基于几何法的无人艇动态避障方法流程图；图2为本专利技术实施例提供的障碍物、无人艇、目标点与避障点位置示意图；图3为本专利技术实施例提供的避障点在障碍物安全范围圆右边切点示意图；图4为本专利技术实施例提供的避障点在障碍物安全范围圆左边切点示意本文档来自技高网...

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

【技术特征摘要】
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.计算出无人艇坐标到距离无人艇最近的避障点坐标方向与正东方向的夹角，作为无人艇的航向角。2.如权利要求1所述的无人艇动态避障方法，其特征在于，所述根据目标点位置信息、无人艇与第t个障碍物的位置信息和速度信息，计算无人艇与第t个障碍物之间位置关系参数、无人艇和目标点之间位置关系参数，具体如下：无人艇坐标到障碍物圆心坐标方向与正东方向的夹角根据无人艇坐标(xa,ya)与障碍物圆心坐标(xo,yo)采用夹角公式计算得到；无人艇与障碍物圆心和无人艇与目标点连线的夹角由和计算得到，计算公式如下：其中，为无人艇坐标到目标点坐标方向与正东方向的夹角；上一个计算周期的值；无人艇与障碍物圆心和无人艇与避障点连线的夹角计算公式如下：其中，r0为障碍物安全范围圆的半径，d3为无人艇与障碍物圆心的距离；无人艇与避障点连线和正东方向的夹角由和计算得到：当选择障碍物安全范围圆右边切点为避障点时，计算公式如下：当选择障碍物安全范围圆左边切点为避障点时，计算公式如下：无人艇与目标点之间的距离d0，根据无人艇坐标(xa,ya)与目标点坐标(xt,yt)采用距离公式计算得到；障碍物圆心到无人艇与目标点连线的距离d1，由d3和计算得到，计算公式如下：d′1：上一个计...

【专利技术属性】
技术研发人员：赵金，陈佐鹏，王旭东，耿涛，刘洋，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：湖北,42