CN108445879B - 一种基于预测碰撞危险区域的无人艇避障方法 - Google Patents

Abstract

本发明公开了一种基于预测碰撞危险区域的无人艇避障方法，根据无人艇当前时刻的位置和速度，以及障碍物的位置和速度，判断碰撞风险，计算会发生碰撞的区域，然后把此区域投影到栅格地图上，通过让无人艇避让此区域（危险区域）来达到避开障碍物的目的。本发明方法结合海上船只避障规则，运用Theta*搜索算法进行路径规划，最终得到无人艇行驶路径即与障碍物不发生碰撞，又能符合海上避障规则避开障碍物。通过仿真实验证明，本方法具有较好的避障效果。

Description

一种基于预测碰撞危险区域的无人艇避障方法

技术领域

本发明涉及一种基于预测碰撞危险区域的无人艇避障方法。

背景技术

随着科技的进步，模块化、智能化、无人化的武器装备已经得到长足的发展。水面无人艇(unmanned surface vehicle，USV)，简称无人艇，在海洋作战中越来越多的得到试用，并且很可能成为越来越重要的作战手段。除了在海洋军事方面的使用，无人艇还可以用于民用方面，如用于海洋的监测，预警和海上搜救等领域。八十年代以来，西方国家逐渐重视无人艇的研究，投入了大量的人力物力。其中以美国和以色列为代表，在无人艇研究方面取得较大突破，已经运用于军事方面。最近几年来，我国也对无人艇进行了研究，取得了一定的成果。但是大多是在理论领域，实用实验相对较少，好多关键技术的研发方面，与国外特别是西方国家差距还很大，加上西方国家的技术封锁。因此，我国加强对无人艇的研发是势在必行的。

随着世界各国对水面无人艇的应用研究越来越广泛，对水面无人艇智能化水平的要求也在不断提高，要求水面无人艇在运动过程中具有与周围环境交互的能力，即要求水面无人艇具有从周围环境中收集有用信息，进行环境建摸，并利用模型来规划并执行上层任务的能力，其中路径规划问题就是核心问题之一。水面无人艇的路径规划是指在静动态障碍物并存的海洋环境中，寻找一条从给定起点到终点的满足一定评价标准的运动路径，使水面无人艇在航行过程中能安全可靠地避开所有障碍物，并且在避开障碍物的同时，又能满足无人艇海上行驶规则。路径规划的分类方法很多，按层次关系可分为全局路径规划和局部路径规划；按照对环境的了解程度，分为环境已知，环境部分已知，环境未知三种规划；同时还有基于功能、行为的规划等等。全局路径规划和局部路径规划是建立在无人艇对其环境了解程度的基础上。水面无人艇的全局规划着眼于规划出一条从起始点到目标点的无碰撞路径，综合考虑上层模块的任务要求和水面无人艇本身的运动性能，确立某种最优性能函数，通过一定的搜索方式，就能在水面无人艇的运动空间中找到相应的优化路径。水面无人艇的局部规划立足于实时性，根据传感器信息确定水面无人艇在地图中的当前位置以及周围局部范围内障碍物的分布情况，规划下一时刻的运动，从而避开动态障碍物。在现有的避障方法中，输出的直接是控制无人艇的速度和方向来完成避障，并且有极少部分研究结合海事规则，只是单纯的为避开障碍物而设计的，避障效果并不理想。

发明内容

针对现有技术存在的问题，本发明提出一种基于预测碰撞危险区域的无人艇避障方法，能完成动静态障碍物实时的避障，并且符合海上避障规则。

为达到上次目的，本发明的构思是：

根据无人艇当前时刻的位置和速度，以及障碍物的位置和速度，判断碰撞风险，计算会发生碰撞的区域，然后把此区域投影到栅格地图上，通过让无人艇避让此区域(危险区域)来达到避开障碍物的目的。

根据上述构思，本发明采用如下技术方案：

一种基于预测碰撞危险区域的无人艇避障方法，包括以下步骤：

步骤一，获取无人艇和障碍物当前的相关信息：

通过GPS或北斗和惯导传感器获取无人艇的位置，运动和姿态信息，再通过无人艇自身携带的多传感器数据融合进行环境建模得到障碍物的位置，运动和尺寸信息，根据障碍物的不同尺寸将障碍物建模为大小不同的圆形状障碍物；

步骤二，通过无人艇以及障碍物当前位置速度信息来判断是否具有碰撞风险：

判断无人艇与障碍物是否有碰撞风险是通过计算无人艇与障碍物相距最近点(Closet Point of Approach，CPA)方法实现的，即通过计算无人艇到障碍物最近点所需要的时间t_CPA，和无人艇到达障碍物最接近点两者相距距离d_CPA来判断；当两者同时满足t_CPA≤t _max和d_CPA≤d_min，其中t_max，d_min均为已知的提前设定好的参数(通过这两个参数的设定来控制无人艇与障碍物的碰撞范围)，此时才会有碰撞风险，如果不能同时满足上述条件，即无碰撞风险；当无人艇与障碍物有碰撞风险时，进行以下步骤；

步骤三，将预测得到的碰撞危险区域投影到栅格地图上：

经过步骤二判断无人艇与障碍物有碰撞风险的情况下，计算障碍物与无人艇相距最近时刻障碍物的具体位置，即在预计发生碰撞时，障碍物的具体位置，根据障碍物的尺寸信息，把障碍物区域投影到栅格地图上，简称POA(Projected obstacle area)区域；

步骤四，判断无人艇与障碍物所处的海事场景：

在单一障碍物的情况下，一般存在四种较为简单的海事场景：“相遇”、“追击”、“障碍物从左侧交叉穿过”、“障碍物从右侧交叉穿过”；在完成投影POA区域后，需判断障碍物与无人艇处在何种海事场景下，而要判断处于何种海事场景，则通过无人艇与障碍物的相对位置方向和相对速度来判断；

步骤五，在判断海事场景后，采用海事规则进行额外障碍物区域的添加：

针对不同的海事场景，采用不同的海事规则，在POA区域周围添加不同的附加障碍物区域；额外障碍物的添加的目的是让Theta*算法(一种路径规划算法，A*算法的衍生)搜寻出一条既符合海事规则，又能避开障碍物的较为合理的路径；

步骤六，用路径规划算法Theta*算法进行路径规划：

用Theta*算法在栅格地图内进行路径规划，最终找到一条符合无人艇避障规则，又能与障碍物不发生碰撞的路径。

与现有技术相比，本发明具有如下的突出的优点：

本发明方法是一种无人艇局部路径规划方法，在建立局部栅格地图的前提下，根据传感器确定分布在无人艇周围的障碍物的位置和速度信息，进而求出投影在栅格地图上的危险区域，然后再根据海事规则，用Theta*路径搜索算法规划一条比较合理的路径。本方法和以往避障方法的区别在于，结合了海事规则，输出比较合理的规划路径。通过仿真实验证明，本方法具有较好的避障效果。

附图说明

图1是本发明基于预测碰撞危险区域的无人艇避障方法流程框图。

图2是CPA方法原理示意图。

图3是基于障碍物的POA区域的路径规划效果图。

图4是海事规则场景示意图。

图5是不同海事场景下所添加的额外障碍物示意图。

图6是“相遇”场景下避障效果图。

图7是“追击”场景下避障效果图。

图8是“障碍物从左侧交叉穿过”场景下避障效果图。

图9是“障碍物从右侧交叉穿过”场景下避障效果图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明优选实施方式作进一步的详细描述。

如图1所示，一种基于预测碰撞危险区域的无人艇避障方法，包括以下步骤：

步骤一，获取无人艇和障碍物当前的位置、运动以及尺寸信息：

通过GPS或北斗传感器获得无人艇的经纬度位置信息，将无人艇的经纬度位置信息转为北斗地坐标系下的(x,y)坐标，北斗地坐标系以正北方向为x轴，正东方向为y轴，地球表面一点为坐标原点。通过惯导传感器获得无人艇的运动与姿态信息，运动信息用速度、加速度、角速度和角加速度表示；姿态信息主要为无人艇艏向角。速度方向与艏向角的单位为度，范围为[0.0,360.0)，角速度的单位为度/秒，角加速度的单位为度/秒²，都是顺时针方向为正值。

再通过无人艇自身携带的雷达、视觉、激光雷达、声呐传感器获得环境信息，进行数据融合与环境建模得到障碍物的形状与位置和运动信息，障碍物的位置信息在船体坐标系下，用距离与角度来表示，距离为无人艇中心到障碍物中心的距离，角度为障碍物方向相对于无人艇艏向的角度，角度范围为[0.0,360.0)，单位为度，顺时针方向角度为正值；通过跟踪算法计算出障碍物的运动信息，障碍物的运动信息用速度大小与速度方向表示，速度方向以正北方向为基准，方向范围为[0.0,360.0)，单位为度，顺时针方向为正值。根据障碍物的不同尺寸将障碍物建模为大小不同的圆形状障碍物。

步骤二，通过无人艇以及障碍物当前位置速度信息来判断是否具有碰撞风险：

判断无人艇与障碍物是否有碰撞风险是通过计算无人艇与障碍物相距最近点(Closet Point of Approach，CPA)方法实现的，即通过计算无人艇到障碍物最近点所需要的时间t_CPA，和无人艇到达障碍物最接近点两者相距距离d_CPA来判断。

如图2所示，P_A为无人艇的当前位置，P_B为障碍物的当前位置，V_A、V_B分别为他们的速度矢量，因此到达最近点的时间为：

并且在此时两者的距离为：

d_CPA＝(P_A+v_At_CPA)-(P_B+v_Bt_CPA)

如果V_A-V_B→0，则，其物理意义是t_CPA→+∞，两者以相同的速度矢量运动时，其之间的距离始终保持不变。

当两者同时满足t_CPA≤t_max和d_CPA≤d_min(t_max，d_min均为已知的参数)，此时才会有碰撞风险，如果不能同时满足上诉条件，即无碰撞风险。当无人艇与障碍物有碰撞风险时，进行以下步骤。

步骤三，将预测得到的碰撞危险区域投影到栅格地图上：

经过步骤二判断无人艇与障碍物有碰撞风险的情况下，计算障碍物与无人艇相距最近时刻障碍物的具体位置，即在预计发生碰撞时，障碍物的具体位置，根据障碍物的尺寸信息，把障碍物区域投影到栅格地图上，简称POA区域；

P_CPA＝P_B+v_Bt_CPA

如图3所示，根据障碍物尺寸把障碍物作为圆形障碍物来处理，投影到栅格地图上去，根据路径搜索算法进行规划路径如图所示。

步骤四，判断无人艇与障碍物所处的海事场景：

在单一障碍物的情况下，一般存在四种较为简单的海事场景：“相遇”、“追击”、“障碍物从左侧交叉穿过”、“障碍物从右侧交叉穿过”；在完成投影POA区域后，需判断障碍物与无人艇处在何种海事场景下，而要判断处于何种海事场景，则通过无人艇与障碍物的相对位置方向和相对速度来判断。

当判断两者有碰撞的危险后，则判断它们属于哪种海事规则，一种高效的并且简单的方法来判断碰撞的场景，是通过计算两船只相对方位角β，其被定义为

β＝atan2(y_A-y_B,x_A-x_B)-ψ_B

其中(x_A,y_A)和(x_B,y_B)为无人艇和障碍物所在位置坐标，ψ_B为障碍物的艏向角。通过计算相对方位角，就可以确定是哪种海事规则场景。

如图4所示，分为四种不同的扇形区域，分别对应四种海事规则场景：“相遇”、“追击”、“障碍物从左侧交叉穿过”、“障碍物从右侧交叉穿过”。“相遇”这种场景就是无人艇与障碍物船只相向而行，如果两船只不采取任何避障措施，两船只将会迎头相撞；“追击”这种场景即是无人艇在障碍物船只后面追击并且意图超越障碍物船只；“障碍物从左侧交叉穿过”这种场景就是障碍物船只从无人艇左侧过来，并且如果不采取避障措施，两船只会发生碰撞的场景；“障碍物从右侧交叉穿过”这种场景就是障碍物船只从无人艇右侧过来，并且如果不采取避障措施，两船只会发生碰撞的场景。

步骤五，在判断海事场景后，采用海事规则进行额外障碍物区域的添加：

针对不同的海事场景，采用不同的海事规则，在POA区域周围添加不同的附加障碍物区域；额外障碍物的添加的目的是让Theta*算法(一种路径规划算法，A*算法的衍生)搜寻出一条既符合海事规则，又能避开障碍物的较为合理的路径。

如图5所示，针对不同的海事场景添加不同的额外障碍物区域，所添加障碍物的方向是基于CPA点为参考点，在不同方向上进行障碍物的添加，方向的选择是根据所处的海事场景和障碍物的速度方向进行判断的。针对不同的海事场景，以障碍物速度方向为基准，在不同的方向上添加额外的障碍物，可以让路径搜索算法规划出一条合理的避障路径。

步骤六，用路径规划算法Theta*算法进行路径规划：

用Theta*算法在栅格地图内进行路径规划，最终找到一条符合无人艇避障规则，又能与障碍物不发生碰撞的路径。

如图6，图7，图8，图9分别表示在四种不同的海事场景，分别是“相遇”，“追击”，“障碍物从左侧交叉穿过”和“障碍物从右侧交叉穿过”四种海事场景下的仿真效果展示，对每种场景分别运用一组(3副)图片来展示其避障过程，从图中可以看出，无人艇都能较好的避开障碍物船只。充分说明了运用本发明方法，无人艇能较好的完成与障碍物船只的避障。

Claims (1)

1.一种基于预测碰撞危险区域的无人艇避障方法，其特征在于，包括以下步骤：

步骤一，获取无人艇和障碍物当前的相关信息：

通过GPS或北斗和惯导传感器获取无人艇的位置，运动和姿态信息，再通过无人艇自身携带的多传感器数据融合进行环境建模得到障碍物的位置，运动和尺寸信息，根据障碍物的不同尺寸将障碍物建模为大小不同的圆形状障碍物；

步骤二，通过无人艇以及障碍物当前位置速度信息来判断是否具有碰撞风险：

判断无人艇与障碍物是否有碰撞风险是通过计算无人艇与障碍物相距最近点的方法实现的，即通过计算无人艇到障碍物最近点所需要的时间t_CPA，和无人艇到达障碍物最接近点两者相距距离d_CPA来判断；当两者同时满足t_CPA≤t_max和d_CPA≤d_min，其中t_max，d_min均为已知的参数，此时才会有碰撞风险，如果不能同时满足上述条件，即无碰撞风险；当无人艇与障碍物有碰撞风险时，进行以下步骤；

步骤三，将预测得到的碰撞危险区域投影到栅格地图上：

经过步骤二判断无人艇与障碍物有碰撞风险的情况下，计算障碍物与无人艇相距最近时刻障碍物的具体位置，即在预计发生碰撞时，障碍物的具体位置，根据障碍物的尺寸信息，把障碍物区域投影到栅格地图上，简称POA区域；

步骤四，判断无人艇与障碍物所处的海事场景：

在单一障碍物的情况下，一般存在四种较为简单的海事场景：“相遇”、“追击”、“障碍物从左侧交叉穿过”、“障碍物从右侧交叉穿过”；在完成投影POA区域后，需判断障碍物与无人艇处在何种海事场景下，而要判断处于何种海事场景，则通过无人艇与障碍物的相对位置方向和相对速度来判断；

步骤五，在判断海事场景后，采用海事规则进行额外障碍物区域的添加：

针对不同的海事场景，采用不同的海事规则，在POA区域周围添加不同的附加障碍物区域；额外障碍物的添加的目的是让Theta*算法搜寻出一条既符合海事规则，又能避开障碍物的较为合理的路径；

步骤六，用路径规划算法Theta*算法进行路径规划：

用Theta*算法在栅格地图内进行路径规划，最终找到一条符合无人艇避障规则，又能与障碍物不发生碰撞的路径。

Images