CN105717923B - 基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制算法 - Google Patents

Abstract

本发明提出一种基于椭圆聚类‑碰撞锥推演的无人艇海洋动态避障控制方法。在海洋环境下，无人艇所面对的动态障碍主要是船只，船只的长轴与短轴比例悬殊，因此本发明对动态船只障碍物进行椭圆聚类；在此基础上，根据无人艇自身航向、速度和聚类椭圆体线速度、公转速度和自转速度计算无人艇和障碍物船只的碰撞的运动角度，迭代所有可能碰撞的角度生成碰撞锥来对无人艇进行避障规划。

Description

基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制算法

技术领域

本发明涉及一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障方法，本发明所涉及到的无人艇避障方法不仅适用于静态避障，尤其适用并主要运用于动态环境下的避障。

背景技术

在海洋强国战略中，海图是军事和民用海洋安全航行不可或缺的基本保障，但是我国海图序列不完整，原因是目前海测船的吃水深，无法靠近岛礁岸线等复杂海域进行海图测绘；另外，海洋环境监测船也由于吃水深的问题，很难到海岸线、海岛、油气平台附近进行环境监测。而无人艇具有吃水浅、自主航行、自主避障、任务载荷配置灵活的特点，是解决近海岛礁海域海图序列不完整问题、复杂海域环境监测问题的有效科技手段，同时也是提升我国海洋军事装备能力，维护我国海洋权益、处理南海等热点海域局势的重要海洋装备。

随着世界各国对水面无人艇的应用研究越来越广泛，对水面无人艇智能化水平的要求也在不断提高，要求水面无人艇在运动过程中具有与周围环境交互的能力，即要求水面无人艇具有从周围环境中收集有用信息，构造符号化的环境模型，并利用模型来规划并执行上层任务的能力，其中路径规划问题就是核心问题之一。水面无人艇的路径规划是指在静动态障碍物并存的海洋环境中，寻找一条从给定起点到终点的满足一定评价标准的运动路径，使水面无人艇在航行过程中能安全可靠地避开所有障碍物。

路径规划的分类方法很多，按层次关系可分为全局路径规划和局部路径规划；按照对环境的了解程度，分为环境已知，环境部分已知，环境未知三种规划；同时还有基于功能、行为的规划等等。全局路径规划和局部路径规划是建立在机器人对其环境了解程度的基础上。水面无人艇的全局规划着眼于规划出一条从起始点到目标点的无碰撞路径，综合考虑上层模块的任务要求和水面无人艇本身的运动性能，确立某种最优性能函数，通过一定的搜索方式，就能在水面无人艇的运动空间中找到相应的优化路径。水面无人艇的局部规划立足于实时性，根据传感器信息确定水面无人艇在地图中的当前位置以及周围局部范围内障碍物的分布情况，规划下一时刻的运动，从而避开动态障碍物。

全局路径规划的方法主要有：几何法，栅格法、构形空间法、可视图法、拓扑法和概率路径图法。局部路径规划的主要方法有：人工势场法、模糊逻辑算法、神经网络方法。很多适用于全局路径规划的方法经过改进都可以用于局部路径规划，而适用于局部路径规划的方法也适用于全局路径规划。

每一种路径规划方法都具有各自的优点，但总的看来，以上方法都或多或少地存在着一些问题：如栅格法的规范性和一致性较好，易于实现计算机的建模、存储、处理、更新于分析，适合于A星算法或Dijkstra算法搜索路径。但对于栅格法及其演化而来的单元树法，前者在复杂的环境下易引起组合爆炸，后者需要计算单元间的邻接关系，增加了计算量。分层单元分解法是上述两种环境建模的糅合，可谓是取其所长，避其所短；Khatib提出的人工势场法，是一种虚拟力法。势场法搜索路径具有启发信息，势场法无需大量的预计算，就可以自动生成较光滑的路径，但人工势场法把所有信息压缩为单个合力，容易丢弃有关障碍物分布的有价值信息，故具有陷入局部最小而不能使机器人到达最终目标位置的缺陷；模糊控制算法将模糊控制本身所具有的鲁棒性与基于生理学上的“感知，动作”行为结合起来，适用于未知时变环境下的路径规划，实时性较好，但对于必要因素：障碍物的速度大小和方向，在实际应用中很难得到；基于行为的神经网络法，虽然在真实世界里表现出较高的鲁棒性、实时性，但当任务和环境变得复杂时，要完全依靠程序员的手工编程实现其基本行为的设计和组织工作，就变得非常繁重，甚至是不可能的。

随着智能计算方法的迅速发展，将智能算法应用到多目标决策问题——路径规划中成为一种趋势。智能算法主要包括进化算法，粒子群优化算法和蚁群算法。

发明内容

本发明针对已有技术存在的不足公开了一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法。在海洋环境下，无人艇所面对的动态障碍主要是船只，船只的长轴与短轴比例悬殊，因此本发明对动态船只障碍物进行椭圆聚类；在此基础上，根据无人艇自身航向、速度和聚类椭圆体线速度、公转速度和自转速度计算无人艇和障碍物船只的碰撞的运动角度，迭代所有可能碰撞的角度生成碰撞锥，由此计算两者如按照当前运动状态是否会发生碰撞并进一步采取相应措施。

为达到上述目的，本发明采用下述技术方案：

一种基于碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于操作步骤如下：

第1步在软件中对动态船只障碍物按照其尺寸形态进行聚类成动态椭圆障碍物，并且采集其运动状态；

第2步规划系统初始参数，包括目标距离、目标之间的方位角、质点速度--无人艇速度、椭圆速度--动态障碍物速度、椭圆长轴、椭圆短轴、质点速度方向--无人艇速度方向、椭圆速度方向--动态障碍物速度方向；

目标距离：无人艇质点与动态船只障碍物中心的距离；

目标之间的方位角：动态船只障碍物中心点相对于无人艇质点的方位角；

质点速度：无人艇当前的运动速度大小；

椭圆速度：动态船只障碍物当前的运动速度大小；

椭圆长、短轴：动态船只障碍物聚类为椭圆的长短轴参数；

质点速度方向：无人艇的运动速度方向；

椭圆速度方向：动态船只障碍物的运动速度方向。

第3步根据椭圆的相关参数确定椭圆的两个焦点位置--焦点C1和焦点C2，并且计算两个焦点与无人艇质点之间的方位角；

第4步根据椭圆两个焦点与无人艇质点之间的位置关系分别计算出两个焦点与无人艇质点之间的相对距离；

第5步根据椭圆和无人艇质点的运动状态以及位置关系分别计算出无人艇质点与椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度；

第6步通过以上得出的无人艇质点和椭圆动态障碍物之间的相对参数，利用推演的点和椭圆之间的碰撞锥计算方法对两者进行碰撞检测，并计算出碰撞锥区间。

椭圆边界上的点具有以下特性：边界上的点到达两个焦点的距离之和为椭圆长轴的两倍。根据椭圆的这个特性，我们可以利用这一点计算无人艇质点和椭圆障碍物在运动过程中无人艇质点与两个焦点之间距离和的最小值。如果该距离和的最小值大于该椭圆障碍物的长轴的两倍，则说明无人艇质点在任何时刻都不会与椭圆障碍物相遇而碰撞，相反，如果该最小值小于该椭圆障碍物的长轴的两倍，则说明无人艇质点会在此刻运动到椭圆内部，即无人艇会与动态障碍物发生碰撞。

第7步通过第6步计算的碰撞结果，依据所得出的碰撞锥区间，无人艇采取相应的措施：

如检测到无人艇和动态障碍物不会发生碰撞，则可不采取措施；

如检测到无人艇和动态障碍物会发生碰撞，则可采取以下措施：

(1)改变无人艇的速度方向，不改变速度大小；依据所计算出的碰撞锥区间，无人艇的速度方向只要取在该区间之外就可完成避障行为；

(2)改变无人艇的速度大小，不改变速度方向；无人艇的速度增大或减小可进行有效的 避障；改变速度大小后进行碰撞检测，如检测到不会发生碰撞则说明采取了有效的避障措施；

(3)即改变无人艇的速度方向，也改变速度大小；这种措施能更加有效迅速的进行有效的避障行为。

第8步按照动态障碍物的运动状态重复第2步到第7步；当无人艇所搭载的传感器检测到动态船只障碍物的运动状态发生改变，则应当重新进行碰撞计算。

本发明相对于现有的其他无人艇海洋避障方法，具有如下显而易见的突出实质性特点和显著技术进步：

(1)本发明公开的无人艇避障方法可以预测无人艇对于动态障碍物的避障行为。

(2)本发明公开的无人艇避障方法用椭圆对动态障碍物进行聚类，更加适用于无人艇在海洋环境下对于大型船只的动态避障，并且通过椭圆聚类减少了无人艇的规划路径，可以实现精确避障行为。

(3)本发明公开的无人艇避障方法充分考虑了动态船只障碍物的自转--自身姿态的改变和公转--运动轨迹的变化。

附图说明

图1为基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法流程框图。

图2为软件仿真无人艇质点与动态船只障碍的仿真状态图。

图3为在动态障碍物作匀速运动下计算出的碰撞锥区间示意图。

图4为在动态障碍物作变速运动下的运动仿真图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明优选实施方式作进一步的详细描述。显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有创造性劳动前提下所获得的所有其他实施例，都属于本发明保护范围。

实施例一：

本基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于操作步骤如下：

第1步在软件中对动态船只障碍物按照其尺寸形态进行聚类成动态椭圆障碍物，并且采集其运动状态；

第2步规划系统初始参数，包括目标距离、目标之间的方位角、质点速度--无人艇速度、椭圆速度--动态障碍物速度、椭圆长轴、椭圆短轴、质点速度方向--无人艇速度方向、椭圆速度方向--动态障碍物速度方向；

第3步根据椭圆的相关参数确定椭圆的两个焦点位置--焦点C1和焦点C2，并且计算两个焦点与无人艇质点之间的方位角；

第4步根据椭圆两个焦点与无人艇质点之间的位置关系分别计算出两个焦点与无人艇质点之间的相对距离；

第5步根据椭圆和无人艇质点的运动状态以及位置关系分别计算出无人艇质点与椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度；

第6步通过以上得出的无人艇质点和椭圆动态障碍物之间的相对参数，利用推演的点和椭圆之间的碰撞锥计算方法对两者进行碰撞检测，并计算出碰撞锥区间；

第7步通过第6步计算的碰撞结果，依据所得出的碰撞锥区间，无人艇采取相应的措施；

第8步按照动态障碍物的运动状态重复第2步到第7步；当无人艇所搭载的传感器检测到动态船只障碍物的运动状态发生改变，则应当重新进行碰撞计算。

实施例二：

本实施例与实施例一基本相同，特别之处如下：

所述第3步中，椭圆的两个焦点与无人艇质点之间的位置关系为：

焦点C1的相对坐标位置为(r·cosα-c·cosβ,r·sinα-c·sinβ)，焦点C2的相对坐标位置为：(r·cosα-c·cosβ,r·sinα-c·sinβ)，

其中r为无人艇质点与动态船只障碍物中心的距离、α为无人艇质点的运动方向、β为动态障碍物的运动方向。

所述第4步中两个焦点与无人艇质点之间的相对距离为：

其中r₀₁为焦点1与无人艇质点的距离、r₀₂为焦点2与无人艇质点的距离、θ为动态船只障碍物中心点相对于无人艇质点的方位角。

所述第5步中无人艇质点与椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度为：

其中，无人艇质点与椭圆中心的相对距离变化速度为V_r0、相对方位角变化速度为V_θ0；无人艇质点与椭圆的焦点1之间的相对距离变化速度为V_r01、相对方位角变化速度为V_θ01；无人艇质点与椭圆的焦点2之间的相对距离变化速度为V_r02、相对方位角变化速度为V_θ02。

所述第7步中无人艇采取相应的措施为：

如检测到无人艇和动态障碍物不会发生碰撞，则可不采取措施；

如检测到无人艇和动态障碍物会发生碰撞，则可采取以下措施：

改变无人艇的速度方向，不改变速度大小；依据所计算出的碰撞锥区间，无人艇的速度方向只要取在该区间之外就可完成避障行为；

改变无人艇的速度大小，不改变速度方向；无人艇的速度增大或减小可进行有效的避障；改变速度大小后进行碰撞检测，如检测到不会发生碰撞则说明采取了有效的避障措施；

即改变无人艇的速度方向，也改变速度大小；这种措施能更加有效迅速的进行有效的避障行为。

实施例三：

图1所示为本实施例系统程序框图。其中规定了一些系统的初始参数：质点速度Va、速度方向α；椭圆速度Vb、速度方向β；目标距离r、目标之间的方位角θ、椭圆的长轴a和短轴b。目标距离r：无人艇质点与动态船只障碍物中心的距离。

目标之间的方位角θ：动态船只障碍物中心点相对于无人艇质点的方位角。

质点速度Va：无人艇当前的运动速度大小。

椭圆速度Vb：动态船只障碍物当前的运动速度大小。

椭圆长轴a、短轴b：动态船只障碍物聚类为椭圆的长短轴参数。

质点速度方向α：无人艇的运动速度方向。

椭圆速度方向β：动态船只障碍物的运动速度方向。

无人艇在进行碰撞检测之前所采集到的动态船只的运动参数主要有：动态船只速度Vb和运动方向β、无人艇速度Va和运动方向α。得到无人艇和动态产值的运动参数后，进行碰撞检测，得到相关的碰撞锥区间：(α₁-α₂)，并采取相应的措施进行有效避障。当动态船只的运动状态发生变化，再次进行碰撞检测。

由于动态船只障碍物不是作匀速运动，其速度大小和速度方向均会发生变化，因此我们用以下式子来表示其运动速度大小的变化：

V_b(i)＝V_b(i-1)+a_bt (1)

其中i＝0,1,2,...表示动态船只速度大小在某一时刻的速度大小，a_b表示发生速度变化该时刻的加速度，t表示速度变化的时长。

对于动态船只速度方向的变化我们考虑到两种情况：公转和自转；

1.船只速度方向的变化--公转

我们在这所提到的公转是把动态船只看作一个刚体，考虑其速度方向的变化。对于这种速度方向的变化我们称之为公转，速度方向β的表达式如下：

β_i＝β_i-1+ω₁t (2)

ω₁表示速度方向改变的角速度的大小。

2.船只自身姿态追随运动方向的变化(自转)

β_i′＝β_i-1′+ω₂t (3)

ω₂表示船只自转的角速度的大小。

在正常情况下，根据船只的运动特点，船只自身的姿态应该与其运动方向保持一致，即当船只聚类成椭圆，椭圆中心与焦点的连线应当与椭圆运动方向一致。因此，在一般情况下ω₂＝ω₁。

在无人艇对当前运动状态进行碰撞检测的时候，所有采集到的数据是按照常数来进行处理，因此当动态船只障碍物的运动发生改变的时候，应当对实时数据再次进行碰撞检测，如图1所示。

本发明所涉及的无人艇质点与动态椭圆障碍物之间的相关参数的计算如下：

无人艇质点与动态椭圆障碍物之间的相关参数包括：无人艇质点与椭圆中心的距离变化速度V_r0、方位角变化速度V_θ0；无人艇质点与椭圆的焦点1之间的距离r₀₁、相对距离变化速度V_r01、相对方位角变化速度V_θ01；无人艇质点与椭圆的焦点2之间的距离r₀₂、相对距离变化速度V_r02、相对方位角变化速度V_θ02；

无人艇质点与椭圆中心的距离变化速度V_r0、方位角变化速度V_θ0的计算公式如下：

根据椭圆的长短轴参数可以确定两个焦点相对于无人艇质点A的位置，以无人艇质点为原点。由于椭圆参数根据三角关系，焦点C1的相对坐标位置为(r·cosα-c·cosβ,r·sinα-c·sinβ)，焦点C2的相对坐标位置为：(r·cosα-c·cosβ,r·sinα-c·sinβ)。所以：

θ₁＝arc tan[(r sinθ-c sinβ)/(r cosθ-c cosβ)] (6)

θ₂＝arc tan[(r sinθ+c sinβ)/(r cosθ+c cosβ)] (7)

其中，θ₁和θ₂分别表示椭圆焦点C1和C2相对于无人艇质点的方位角。

无人艇质点与椭圆的焦点1之间的距离r₀₁、相对距离变化速度V_r01、相对方位角变化速度V_θ01、无人艇质点与椭圆的焦点2之间的距离r₀₂、相对距离变化速度V_r02、相对方位角变化速度V_θ02的计算公式如下：

我们在得到相关参数后，带入到碰撞锥检测条件中进行验证，碰撞检测条件如下：

V_r02V_θ01+V_r01V_θ02≤0 (15)

如果满足以上两个检测条件(11)(12)，说明无人艇在当前的运动状态下，将会在接下来的某一时刻与动态船只障碍物发生碰撞，应当采取必要的措施来进行避障。反之如果不满足检测条件，则说明无人艇在当前的运动状态下是安全航行，与动态船只障碍物不会发生碰撞，不需要采取避障措施。

图2为本实施例无人艇质点与动态船只障碍的初始状态图。在进行碰撞检测之前，首先我们在软件中根据动态船只的尺寸特征对其进行椭圆聚类。

图3为本实施例在动态障碍物作匀速运动下计算出的碰撞锥区间示意图。依据初始的系统参数进行出碰撞锥计算得到的碰撞区间，其中横坐标为无人艇的运动方向(0～360度)，纵坐标为计算结果，1代表会发生碰撞，0表示不会发生碰撞。当无人艇所搭载的传感器检测到动态船只障碍物的运动状态发生改变，则应当重新进行碰撞计算。

图4为本实施例在动态障碍物作变速运动下的运动仿真图。其中，无人艇和动态船只分别以图中所示轨迹方向运动。我们在仿真图中可以看出，在初始状态无人艇依据采集到的信息发现当前运动不会与船只障碍物发生碰撞，故保持原有的运动轨迹不变。而当横坐标时间T＝70的时候，无人艇采集到船只信息后，并通过本发明提出的一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法计算得出，当前运动状态会与船只发生碰撞，因此果断采取变化运动方向，使无人艇驶入安全轨道，成功避开障碍物，在避开障碍物后又重新回归原航线。

Claims (5)

1.一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于操作步骤如下：

第1步在软件中对动态船只障碍物按照其尺寸形态进行聚类成动态椭圆障碍物，并且采集其运动状态；

第2步规划系统初始参数，包括目标距离、动态船只障碍物中心点相对于无人艇质点的方位角、质点速度--无人艇速度、椭圆速度--动态障碍物速度、椭圆长轴、椭圆短轴、质点速度方向--无人艇速度方向、椭圆速度方向--动态障碍物速度方向；

第3步根据椭圆的相关参数确定椭圆的两个焦点位置--焦点C1和焦点C2，并且计算两个焦点与无人艇质点之间的方位角；

第4步根据椭圆两个焦点与无人艇质点之间的位置关系分别计算出两个焦点与无人艇质点之间的相对距离；

第5步根据椭圆和无人艇质点的运动状态以及位置关系分别计算出无人艇质点与椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度；

第6步通过以上得出的无人艇质点和椭圆动态障碍物之间的相对参数，利用推演的点和椭圆之间的碰撞锥计算方法对两者进行碰撞检测，并计算出碰撞锥区间；

第7步通过第6步计算的碰撞结果，依据所得出的碰撞锥区间，无人艇采取相应的措施；

第8步按照动态障碍物的运动状态重复第2步到第7步；当无人艇所搭载的传感器检测到动态船只障碍物的运动状态发生改变，则应当重新进行碰撞计算。

2.根据权利要求1所述的一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于，在第3步中，椭圆的两个焦点与无人艇质点之间的位置关系为：

焦点C1的相对坐标位置为(r·cosα-c·cosβ，r·sinα-c·sinβ)，焦点C2的相对坐标位置为：(r·cosα-c·cosβ，r·sinα-c·sinβ)，

其中r为无人艇质点与动态船只障碍物中心的距离、α为无人艇质点的运动方向、β为动态障碍物的运动方向，其中a为椭圆障碍物的长轴长、b为椭圆障碍物的短轴长、c为椭圆障碍物焦点到椭圆中心点的距离。

3.根据权利要求2所述的一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于，在第4步中两个焦点(焦点C1和焦点C2)与无人艇质点之间的相对距离为：

其中r₀₁为焦点C1与无人艇质点的距离、r₀₂为焦点C2与无人艇质点的距离、θ为动态船只障碍物中心点相对于无人艇质点的方位角。

4.根据权利要求1所述的一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于，在第5步中无人艇质点与椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度为：

其中，无人艇质点与椭圆中心的相对距离变化速度为V_r0、相对方位角变化速度为V_θ0；无人艇质点与椭圆的焦点1之间的相对距离变化速度为V_r01、相对方位角变化速度为V_θ01；无人艇质点与椭圆的焦点2之间的相对距离变化速度为V_r02、相对方位角变化速度为V_θ02；r为无人艇质点与动态船只障碍物中心的距离、α为无人艇质点的运动方向、β为动态障碍物的运动方向、V_a为无人艇质点的运动速度、V_b为动态障碍物的运动速度、r₀₁为焦点C1与无人艇质点的距离、r₀₂为焦点C2与无人艇质点的距离、θ为动态船只障碍物中心点相对于无人艇质点的方位角、θ₁为动态船只障碍物焦点C1相对于无人艇质点的方位角、θ₂为动态船只障碍物焦点C2相对于无人艇质点的方位角。

5.根据权利要求1所述的一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于，在第7步中无人艇采取相应的措施为：

如检测到无人艇和动态障碍物不会发生碰撞，则可不采取措施；

如检测到无人艇和动态障碍物会发生碰撞，则可采取以下措施：

(1)改变无人艇的速度方向，不改变速度大小；依据所计算出的碰撞锥区间，无人艇的速度方向只要取在该区间之外就可完成避障行为；

(2)改变无人艇的速度大小，不改变速度方向；无人艇的速度增大或减小可进行有效的避障；改变速度大小后进行碰撞检测，如检测到不会发生碰撞则说明采取了有效的避障措施；

(3)既 改变无人艇的速度方向，也改变速度大小；这种措施能更加有效迅速的进行有效的避障行为。