CN103777639B

CN103777639B - 运动障碍环境中uuv三维航路规划方法

Info

Publication number: CN103777639B
Application number: CN201410011297.1A
Authority: CN
Inventors: 严浙平; 邓超; 李聪聪; 赵玉飞; 徐健; 陈涛
Original assignee: Harbin Engineering University
Current assignee: Harbin ship Intelligent Equipment Technology Co., Ltd.
Priority date: 2014-01-10
Filing date: 2014-01-10
Publication date: 2015-06-24
Anticipated expiration: 2034-01-10
Also published as: CN103777639A

Abstract

本发明涉及一种运动障碍环境中UUV三维航路规划方法，其特征在于：步骤一：通过计算UUV遭遇固定障碍与运动障碍的时间，建立UUV三维避障代价函数；步骤二：通过计算UUV艏向与目标方向角度关系，建立UUV三维趋向目标代价函数；步骤三：结合UUV三维避障代价函数和三维趋向目标代价函数，建立UUV三维航路规划代价函数。

Description

运动障碍环境中UUV三维航路规划方法

技术领域

本发明涉及一种运动障碍环境中无人水下航行器(Unmanned underwatervehicle,UUV)三维航路规划方法。

背景技术

随着海洋经济的发展，科学技术的进步，UUV(Unmanned underwater vehicle)在海洋环境探测、资源开采、军事干扰与侦察等方面的作用越来越重要，逐渐成为研究热点。海洋环境十分复杂，既有海底山脉、焦岩、沉船、暗桩等固定障碍，又有舰艇等运动障碍威胁。如何寻找一条从起点到目标点优化的航路，既能避开障碍与威胁又能高效的完成勘测与侦察任务，成为UUV安全、高效作业的首要问题。

国内外众多学者对航路规划问题进行了深入的研究，提出了许多有效的航路规划方法，比如A*算法、D*算法、人工势场算法等，获得了较好的应用效果。大多数的传统航路规划算法，需要对地图进行栅格化，栅格大小的选择会影响到解的质量，随着地图复杂度的增加，需要较大的存储空间。传统的人工势场算法容易陷入局部极小值，当机器人试图穿过两相邻很近的障碍物时，两障碍物产生的斥力分别为F′_r1和F′_r2，斥力的合力F_r方向为背离障碍物方向。如果此时引力小于斥力，机器人将远离目标点运动；另一种情况，当引力大于或者近似等于斥力时，当机器人靠近障碍物时，机器人与障碍物可能发生碰撞或者停滞不前。机器人无法到达目标点。当目标点在障碍物产生的斥力作用范围内时，如果斥力大于引力时，合力方向会背离目标点，机器人远离目标点运动。在这种情况下，目标点不是总势场最小的点，即使机器人和目标点之间没有障碍物，机器人也无法运动到目标点，只会在实际势场最小的点位置附近振荡。

发明内容

本发明目的在于提供一种运动障碍环境中UUV三维航路规划方法，能够有效避开障碍，实现到达目标点的优化航路规划。

实现本发明目的技术方案：

一种运动障碍环境中UUV三维航路规划方法，其特征在于：

步骤一：通过计算UUV遭遇固定障碍与运动障碍的时间，建立UUV三维避障代价函数；

步骤二：通过计算UUV艏向与目标方向角度关系，建立UUV三维趋向目标代价函数；

步骤三：结合UUV三维避障代价函数和三维趋向目标代价函数，建立UUV三维航路规划代价函数。

步骤一中，避碰固定障碍代价函数为：

f_{1} = \frac{n * S}{L (ψ, θ)}

式中，n为常系数，以调节避障灵敏度；S为规划步长，亦即当前点到规划下一点的距离;L(ψ,θ)为当前点与下一规划点连线前进方向上遭遇的障碍物与当前点的直线距离，假设当前点P_i在规划下一点候选点P′_i+1方向上到障碍物的直线距离为L(ψ′_i,θ′_i)，则有该候选点的代价函数参数L(ψ,θ)＝L(ψ′_i,θ′_i)。

步骤一中，假设，当前点P_i在候选点P′_i+1方向延长线与运动障碍方向延长线的最近交互点为M，运动障碍前行延长线上的最近交互点为M′，h为M与M′点的距离，则

g (h) = \{\begin{matrix} 1 & 0 < h \leq c \\ {[1 + {(\frac{h - c}{m})}^{2}]}^{- 1} & h > c \end{matrix}

其中，c为安全距离，m为衰减调节常系数

假设，UUV从P_i点运动到M点的时间为t₁，运动障碍运动到M′点的时间为t₂，

避碰运动障碍代价函数为：

其中α、β为调节系数。

步骤二中，三维趋向目标代价函数为，

f_{3} = \frac{1}{\sqrt{2 π} σ_{1} σ_{2}} e^{- \frac{{(θ - α)}^{2}}{2 {σ_{1}}^{2}} - \frac{{(ψ - β)}^{2}}{2 {σ_{2}}^{2}}}

其中，当前点与目标点连线的角度为(β,α)，当前点与下一规划点连线的角度为(ψ,θ)，σ₁、σ₂为调节系数。

步骤三中，三维航路规划代价函数为，

f=k₁f₁+k₂f₂-k₃f₃

其中k_i为权重系数（k_i>0）。

本发明具有的有益效果：

本发明通过计算UUV遭遇固定障碍与运动障碍的时间，建立UUV三维避障代价函数；

通过计算UUV艏向与目标方向角度关系，建立UUV三维趋向目标代价函数；结合UUV三维避障代价函数和三维趋向目标代价函数，建立UUV三维航路规划代价函数，能够有效避开障碍，实现到达目标点的优化航路规划。

附图说明

图1是运动障碍环境中UUV航路规划流程图；

图2是UUV遭遇固定障碍示意图；

图3是UUV遭遇运动障碍示意图；

图4是UUV趋近目标示意图；

图5是运动障碍环境下UUV三维航路规划仿真结果。

具体实施方式

UUV在前行方向上离障碍物越近，代价函数越大，反之代价函数越小。避碰固定障碍代价函数为：

f_{1} = \frac{n * S}{L (ψ, θ)} - - - (1)

式中，n为常系数，以调节避障灵敏度。S为规划步长，亦即当前点到规划下一点的距离。L(ψ,θ)为当前点与下一规划点连线前进方向上遭遇的障碍物与当前点的直线距离。假设当前点P_i在规划下一点候选点P′_i+1方向上到障碍物的直线距离为L(ψ′_i,θ′_i)，则有该候选点的代价函数参数L(ψ,θ)＝L(ψ′_i,θ′_i)。UUV前行过程中始终寻找f₁小的点，从而实现对固定障碍物的规避。

UUV作业过程中会遇到运动障碍。假设，当前点P_i在候选点P′_i+1方向延长线与运动障碍方向延长线的最近交互点为M，运动障碍前行延长线上的最近交互点为M′。h为M与M′点的距离。h越大，UUV越安全。从而有：

g (h) = \{\begin{matrix} 1 & 0 < h \leq c \\ {[1 + {(\frac{h - c}{m})}^{2}]}^{- 1} & h > c \end{matrix} - - - (2)

其中，c为安全距离，m为衰减调节常系数。

假设，UUV从P_i点运动到M点的时间为t₁，运动障碍运动到M′点的时间为t₂。如果t₂>t₁则说明UUV先于运动障碍到达，t₂-t₁越大，UUV越安全，这里取函数为避碰动态势场函数，当UUV距离交点越近时避碰行为越强，UUV距离交点越远时避碰行为越弱，因此避碰运动障碍代价函数调整为：如果t₂<t₁则说明运动障碍先于UUV到达，UUV可以安全通过。最终有避碰运动障碍代价函数：

如果两条线不存在交互，则说明UUV与运动障碍运动航线不相交，此时同样有f₂=0。

为实现UUV朝向目标运动，需要建立趋向目标代价函数。假设，当前点p_i与目标点P_g连线的角度为(β_i,α_i)，当前点p_i到候选点P′_i+1的角度为(ψ′_i,θ′_i)，UUV朝向目标点运动，即有(β_i,α_i)与(ψ′_i,θ′_i)越相近时趋近目标代价函数越大，(β_i,α_i)与(ψ′_i,θ′_i)差值越大，趋近目标代价函数越小。从而有趋近目标代价函数：

f_{3} = \frac{1}{\sqrt{2 π} σ_{1} σ_{2}} e^{- \frac{{(θ - α)}^{2}}{2 {σ_{1}}^{2}} - \frac{{(ψ - β)}^{2}}{2 {σ_{2}}^{2}}} - - - (4)

UUV运动障碍环境下三维航路规划总势场函数为各代价函数的叠加。从而，得到UUV航路规划总代价函数：

f=k₁f₁+k₂f₂-k₃f₃ (5)

其中k_i为权重系数（k_i>0）。UUV航路规划过程中，UUV以一定步长前进，在UUV当前航路点，利用粒子群算法寻找特定步长范围内代价函数最小点，作为下一航路点，逐步实现UUV航路规划。

下面结合具体实施例对本发明进一步说明。

取海底地形图的一部分区域{[0,14000],[0,16000],[0,-300]}，UUV起始点坐标为，（2000,2000,-170）目标点为(10000,14000,-100)，步长为80，最大转角约束为ψ≤60°,θ≤60°。在UUV航路规划过程中，假设当前点P_i在规划下一点候选点P′_i+1方向上到障碍物的直线距离为L(ψ′_i,θ′_i)。利用公式(1)得到避碰固定障碍代价函数，利用公式(3)得到避碰运动障碍代价函数，利用公式(4)得到趋近目标代价函数，利用公式(5)最终得到UUV运动障碍环境下三维航路规划总代价函数。然后，利用粒子群算法寻找特定步长范围内代价函数值最小点，作为下一航路点，逐步实现UUV航路规划。

为使规划的航路具有一定安全裕度，UUV不是紧贴地形航行。进行障碍物扩展，将海底地形进行了30m的扩展，为简化问题此处将任意区域海底的深度减小了30m。在此基础上进行规划。UUV航行过程中遭遇纵坐标为4200m，平行于x轴的运动障碍。部分参数设置为n=1，c=10，m=5，k₁=1，k₂=1，k₃=1。

Claims

1.一种运动障碍环境中无人水下航行器三维航路规划方法，其特征在于：

步骤一：通过计算无人水下航行器遭遇固定障碍与运动障碍的时间，建立无人水下航行器三维避碰固定障碍和避碰运动障碍的代价函数；

避碰固定障碍代价函数f₁为：

f_{1} = \frac{n * S}{L (ψ, θ)}

式中，n为常系数，以调节避障灵敏度；S为规划步长，亦即当前点到规划下一点的距离；L(ψ,θ)为当前点与下一规划点连线前进方向上遭遇的障碍物与当前点的直线距离,假设当前点P_i在规划下一点候选点P′_i+1方向上到障碍物的直线距离为L(ψ′_i,θ′_i)，则有该候选点的代价函数参数L(ψ,θ)＝L(ψ′_i,θ′_i)；

设，当前点P_i在候选点P′_i+1方向延长线与运动障碍方向延长线的最近交互点为M，运动障碍前行延长线上的最近交互点为M′，h为M与M′点的距离，则

g (h) = \{\begin{matrix} 1 & 0 < h \leq c \\ {[1 + {(\frac{h - c}{m})}^{2}]}^{- 1} & h > c \end{matrix}

其中，c为安全距离，m为衰减调节常系数

设，无人水下航行器从P_i点运动到M点的时间为t₁，运动障碍运动到M′点的时间为t₂，

避碰运动障碍代价函数f₂为：

其中α、β为调节系数；

步骤二：通过计算无人水下航行器艏向与目标方向角度关系，建立无人水下航行器三维趋向目标代价函数f₃，

f_{3} = \frac{1}{\sqrt{2 π} σ_{1} σ_{2}} e^{- \frac{{(θ - α)}^{2}}{2 {σ_{1}}^{2}} - \frac{{(ψ - β)}^{2}}{2 {σ_{2}}^{2}}}

其中，当前点与目标点连线的角度为(β,α)，当前点与下一规划点连线的角度为(ψ,θ)，σ₁、σ₂为调节系数；

步骤三：结合无人水下航行器三维避障代价函数和三维趋向目标代价函数，建立无人水下航行器三维航路规划代价函数f，

f＝k₁f₁+k₂f₂-k₃f₃

其中k_i为权重系数,k_i>0。