CN112379672B - 一种基于改进人工势场的智能无人船路径规划方法 - Google Patents

Abstract

本发明涉及一种无人船路径规划方法，特别是一种基于改进人工势场的智能无人船路径规划方法，其基于改进的势场构造函数，使用梯度下降法计算出无人船从当前位置到目标点的路径，从而指导无人船的运动；针对无人船在自动避障过程中存在的局部极小值问题，引入了扰动势能的概念，打破无人船在局部受力平衡的状态，从而促进无人船的自动避障导航。本发明有效的解决了传统人工势场算法在无人船自动驾驶避障领域存在四点不足，即无人船距离目标较远而距离障碍物较近时可能与障碍物碰撞的问题、目标不可达问题、无效避障规划导致的能耗问题以及局部极小值问题。本发明了考虑多种相关因素，提出改进的引力势场构造函数和改进的斥力势场构造函数。

Description

一种基于改进人工势场的智能无人船路径规划方法

技术领域

本发明涉及一种无人船路径规划方法，特别是一种基于改进人工势场的智能无人船路径规划方法。

背景技术

由于海洋环境的复杂性，在无人船航行过程中，海面上可能会有动态的漂浮障碍物或者在电子海图中无法显示的静态障碍物，这都使得无人船无法按照预期规划的路径行驶。为了避免无人船与障碍物相撞，需要规划出一条可以及时更新的航行路径，以实现局部避障。

人工势场法是在自主导航研究中常用的动态避障算法。它是在1986年由Khatib在他的博士论文中提出来的，最初是用来实现机器人操作臂的避障规划。人工势场算法的基本思想是对目标点构造一个引力势场，对障碍物构造一个斥力势场。构造出的两个势场在运动物体周围共同作用形成了一个虚拟的叠加人工势场。最终，物体会根据势函数下降的方向，搜索出一条无碰撞的最优运动轨迹，这便是人工势场避障算法的基本思想。

使用人工势场法进行路径规划灵活快速、实时性强，所计算的路径较为平滑，安全性较高。然而，将人工势场法应用到无人船的路径规划时，仍然存在着以下亟需解决的问题：

(1)无人船距离目标较远而距离障碍物较近时可能与障碍物碰撞的问题。当无人船航行初始位置距离目标点比较远时，无人船将受到特别大的引力，以至于其附近障碍物产生的斥力势场几乎可以忽略，那么此时无人船与障碍物相撞的风险也会大大增加。

(2)目标不可达问题。当无人船接近目标位置时，根据目标位置产生的引力势场特点，此时无人船所受到的引力十分微弱。假如此时有动态障碍物靠近目标，其所产生的斥力势场作用于无人船的斥力会大于目标作用于无人船的引力，那么无人船将无法到达目标点位置。

(3)无效避障规划导致的能耗问题。当动态障碍物靠近无人船移动时，由于无人船受到障碍物所产生斥力势场的影响，会改变航行线路。倘若动态障碍物的运动方向是背离或偏离无人船的，又或者障碍物的停留位置与无人船的相对距离并没有威胁到无人船的安全，那么在该情况下无人船受到斥力势场影响所计算和规划的局部弧线避障路径不仅无效，反而会带来一些不必要的能耗损失。

(4)局部极小值问题。理想情况下，我们希望无人船在只有到达目标点时，其所拥有的势能为零。然而，当无人船与障碍物、目标位置共处于一条直线上时，在引力势场和斥力势场的共同作用下，无人船可能会处于受力平衡的状态，那么无人船将会在没有到达目标点的情况下，停止不前或者局部往复移动。除了三者共线状态之外，当无人船周围有多个障碍物时，其会同时受到来自不同方向的斥力，若斥力总和与引力相等，那么无人船将会陷入局部受力平衡状态。

发明内容

本发明的目的是为了解决上述现有技术的不足而提供一种基于改进人工势场的智能无人船路径规划方法。

为了实现上述目的，本发明所设计的一种基于改进人工势场的智能无人船路径规划方法，其具体包括以下的步骤：

步骤1：将已知电子海图中的环境信息、无人船起始点坐标和目标点坐标输入到路径规划***。

步骤2：对目标点建立引力势场，使用如下引力势场构造函数：

式中，k_a为引力势场的增益系数；ρ(X,X_g)为无人船当前位置到目标点位置的距离；ρ_g为引力势场影响的距离阈值；k_g为势场影响系数；η为势场调节参数。

对电子海图中已知的静态障碍物建立斥力势场，使用如下斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离。

步骤3：使用梯度下降法解算出一条从无人船的起始点到目标点的路径。

步骤4：使用解算出的路径指导无人船向目标点运动。

步骤5：在无人船向目标点运动的途中，使用障碍物检测装置检测无人船周围的动态障碍物。

步骤6：若检测到动态障碍物，则实时计算无人船与动态障碍物之间的距离，并在动态障碍物处建立动态障碍物斥力势场，使用如下斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离；ρ_u为以无人船为中心的圆形威胁区域半径，ρ_u＜ρ_o。

步骤7：根据更新的人工势场，不停地利用梯度下降法实时地解算出从当前位置到目标点的路径。

步骤8：重复步骤4至步骤7直到无人船停止运动，此时计算ρ(X，X_g)是否为0，若为0跳转到步骤10，若不为0跳转到步骤9。

步骤9：若ρ(X，X_g)不为0，则表示无人船陷入了局部极小值点，此时在局部极小点处添加一个扰动势场ΔU＝k_disρ(X，X_g)²，让无人船离开该点；

式中，ΔU为添加的扰动势能；k_dis为可调节的扰动势能随机系数；ρ(X，X_g)为无人船当前位置到目标点的距离。

步骤10：无人船到达目标点，路径规划结束。

当然，在实际路径规划过程中除了排除动态障碍物不在威胁区域内的干扰外，还要考虑动态障碍物与无人船之间的相对位置、相对运行速度对无人船避障路径规划的影响；

因此，作为本发明的一种技术改进方案，上述步骤6中动态障碍物处建立的动态障碍物斥力势场函数的改进如下：

式中，f为在斥力势场函数中引入的一个斥力势场系数。对f作如下定义：

式中，k_f为可调的影响系数；v_u和v_o分别为无人船和障碍物的运动速度；α为无人船运动速度v_u方向与无人船和障碍物连线l_uo的夹角；θ为障碍物运动速度v_o方向与无人船和障碍物连线l_uo的夹角。

当动态障碍物和无人船相向运动时，v_o cosθ的值为负数，那么斥力势场系数f将会大幅增加，从而使斥力势场的影响增强，使得无人船可以及时地调整路径规划，有效地避免和障碍物碰撞。

与现有技术相比，本发明旨在使用改进的人工势场法来指导无人船自动地做出路径规划。针对传统人工势场存在的技术问题，本发明在传统人工势场的基础上做出改进，构造出新的势场函数，从而有效地解决背景技术中提到的几个问题，规划出行之有效的路径。具体而言即：

传统的引力势场函数是一个关于船只到目标点距离的二次函数，这就使得势场强度对距离因素的变化十分敏感。仅仅通过增益系数去调整引力势场构造函数往往达不到想要的目的。在对无人船从起始点到目标点做全局路径规划时，往往会遇到船只距离目标点很远而距离障碍物较近的情况，此时如果还是使用传统的引力势场构造函数，那么可能会造成船只引力过大而斥力过小的结果，从而可能会发生船只还是沿着目标方向行驶以致碰撞障碍物的危险。为此，考虑修改传统的引力势场构造函数。我们的目标是当无人船与目标点之间的距离超出一定范围后，引力势场的强度的增长幅度有所减缓，不至于距离船只较近的障碍物产生的斥力势场与距离船只很远的目标点产生的引力势场相比可以忽略不计，从而使得无人船在此种情况下遇到障碍物时仍可以调整路径、避免碰撞。为此，需要为引力势场构造函数添加距离阈值，当距离大于此阈值时，使用对数函数来代替原有的二次函数，以此来降低势场强度变化的幅度。同时，考虑到阈值处势场强度的变化应该连续，综合以上考虑，本发明所提出引力势场构造函数如下：

式中，k_a为引力势场的增益系数；ρ(X,X_g)为无人船当前位置到目标点位置的距离；ρ_g为引力势场影响的距离阈值；k_g为势场影响系数；η为势场调节参数。

除了引力势场构造函数不适用于无人船的路径规划及避障外，传统人工势场构造函数还存在目标不可达的问题。为了解决这一问题，本发明提出修改的斥力势场构造函数。根据传统人工势场的特点，当动态障碍物移动到目标位置附近时，若目标点和无人船都在障碍物势场的影响范围之内，此时障碍物对无人船产生的斥力要远大于目标点对无人船产生的引力，这会使得无人船无法到达目标点而只能在障碍物的某一范围之外运动。这就是目标不可达问题。为解决此问题，在传统斥力势场构造函数中引入无人船与目标之间的距离ρ(X，X_g)作为斥力势场影响因子，改进后的斥力势场如下：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离。与传统的斥力势场构造函数相比，改进的斥力势场构造函数由于引入了ρ(X，X_g)，使得目标点位置的斥力势场和引力势场均为0，从而解决了目标不可达问题。

另外，传统的斥力势场构造函数并没有考虑无人船与障碍物的相对运动，因此规划出的避障路径可能是无效的，这会造成无人船能耗的增加。例如，如果某动态障碍物相对于无人船的运动方向是远离无人船的，那么即使无人船此时处在该障碍物斥力势场的影响范围内，也无需考虑该障碍物对无人船产生的斥力作用，从而减少无效的避障路径，提高无人船航行的能效。

对于无人船，定义一个以无人船为中心的圆形威胁区域，半径为ρ_u，若障碍物不在圆形威胁区域内则不考虑其斥力势场，这样一来就可以避免一些停留在原规划路径周围但不处在圆形威胁区域内的障碍物的影响，减少了无效的避障路径。为此，设计出如下斥力势场构造函数：

即当无人船与障碍物之间的距离小于ρ_u时，障碍物对无人船产生斥力势场；反之不存在斥力势场。一般而言，ρ_u要小于ρ_o，即无人船受威胁区域要小于斥力势场影响范围。

除了排除动态障碍物不在威胁区域内的干扰外，还要考虑动态障碍物与无人船之间的相对位置、相对运行速度对无人船避障路径规划的影响。当动态障碍物在无人船的威胁区域内运动时，需要根据两者之间相对位置的变化来对斥力势场模型进行改进。对斥力势场函数的改进如下：

式中，f为在斥力势场函数中引入的一个斥力势场系数。对f作如下定义：

式中，k_f为可调的影响系数；v_u和v_o分别为无人船和障碍物的运动速度；α为无人船运动速度v_u方向与无人船和障碍物连线l_uo的夹角；θ为障碍物运动速度v_o方向与无人船和障碍物连线l_uo的夹角。

当障碍物和无人船相向运动时，v_o cosθ的值为负数，那么斥力势场系数f将会大幅增加，从而使斥力势场的影响增强，使得无人船可以及时地调整路径规划，有效地避免和障碍物碰撞。

在使用人工势场进行路径规划时，无法避免的一个问题就是存在局部极小值点，即无人船在该点所受的引力势场和总斥力势场平衡，合势场为0的情况。当无人船陷入局部极小值点时就无法主动挣脱，误以为到达了目标点。此时，通过判断无人船当前位置到目标点的距离ρ(X，X_g)是否为0来判断无人船是否到达目标点；若ρ(X，X_g)为0，则表示无人船到达了目标点；若ρ(X，X_g)不为0，则表示无人船陷入了局部极小值点，此时应在无人船当前位置增加一个扰动势场，扰动势场的定义如下：

ΔU＝k_disρ(X,X_g)²

式中，ΔU为添加的扰动势能；k_dis为可调节的扰动势能随机系数；ρ(X，X_g)为无人船当前位置到目标点的距离。

附图说明

图1是本发明一种智能无人船路径规划方法的逻辑流程图；

图2为传统引力势场函数和改进引力势场函数作用效果对比图；

图3为不对无人船产生威胁的动态障碍物；

图4为对无人船产生威胁的动态障碍物。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员所获得的所有其他实施例，都应该属于本发明保护的范围。

作为一种实施方式，本发明提供的一种基于改进人工势场的智能无人船路径规划方法，具体工作步骤如下：

步骤1：获知电子海图中显示的静态障碍物的位置信息，通过船载北斗卫星导航***获知无人船起始点位置信息，在无人船地面站计算机中指定目标点位置，将电子海图中静态障碍物的位置信息、无人船起始点位置信息和目标点位置信息发送给无人船路径规划***；

步骤2：在无人船路径规划***中对目标点建立引力势场，使用如下引力势场构造函数：

式中，k_a为引力势场的增益系数；ρ(X,X_g)为无人船当前位置到目标点位置的距离；ρ_g为引力势场影响的距离阈值；k_g为势场影响系数；η为势场调节参数。

对电子海图中已知的静态障碍物建立斥力势场，使用如下斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离。

步骤3：利用MATLAB软件通过梯度下降法解算出从起始点到目标点的路径。

步骤4：利用解算出的路径指导无人船向目标点运动，通过船载北斗卫星导航***实时获得无人船行驶的坐标位置，得到无人船行驶的真实轨迹并反馈给无人船路径规划***，由无人船路径规划***计算真实轨迹与规划路径的偏差，进而调整无人船的运动方向。

步骤5：在无人船行驶的途中利用双目摄像头/激光雷达传感器实时检测无人船周围是否存在动态的障碍物。

步骤6：若发现动态障碍物，就将实时计算出的无人船与动态障碍物之间的距离、动态障碍物的速度以及动态障碍物的位置信息反馈给无人船路径规划***，在无人船路径规划***中建立该动态障碍物实时变化的斥力势场，使用如下动态障碍物斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离；ρ_u为以无人船为中心的圆形威胁区域半径，ρ_u＜ρ_o。

步骤7：根据更新的人工势场，在无人船路径规划***中不停地利用梯度下降法实时地解算出从当前位置到目标点的路径。

步骤8：重复步骤4至步骤7直到无人船停止运动，此时无人船路径规划***计算ρ(X，X_g)是否为0，若为0跳转到步骤10，若不为0跳转到步骤9。

步骤9：若ρ(X，X_g)不为0，则表示无人船陷入了局部极小值点，此时无人船路径规划***在局部极小点处添加一个扰动势场ΔU＝k_disρ(X，X_g)²，让无人船离开该点。

式中，ΔU为添加的扰动势能；k_dis为可调节的扰动势能的随机系数，X_u表示无人船处在局部极小值点的位置，X_g为目标点位置；ρ(X_u，X_g)代表处在局部极小值点的无人船与目标点的距离。

步骤10：无人船到达目标点，路径规划结束。

上述实施方式中除了排除动态障碍物不在威胁区域内的干扰外，还要考虑动态障碍物与无人船之间的相对位置、相对运行速度对无人船避障路径规划的影响。v_u和v_o分别为无人船和障碍物的运动速度；l_ug为无人船至目标的直线路径，方向与v_u相同；l_uo为无人船到障碍物的直线路径，方向由无人船指向障碍物；v_ou和v_og分别为障碍物的运动速度v_o的分量，其中v_ou表示v_o在垂直l_ug方向上的分量，v_og表示v_o在v_u方向上的分量；α、β、θ分别表示v_u方向与无人船和障碍物连线l_uo的夹角、v_og方向与v_o方向的夹角、v_o方向与无人船和障碍物连线l_uo的夹角，规定顺时针为正方向，标注如附图3、附图4所示。当α＜β时，动态障碍物的运行方向与无人船的运行方向是向外偏离的。实际上，当0＜β＜π时，障碍物的速度方向是向外偏离无人船速度方向的，并且障碍物相对于无人船依然是向后方运动的，所以这种情况下，障碍物对无人船没有构成威胁，如附图3所示可以不考虑障碍物斥力势场的影响。当-π＜β＜0时，障碍物的速度分量v_ou则指向了l_ug，这意味着障碍物的运动方向对沿着l_ug方向行驶的无人船存在一定威胁，如附图4所示，并且威胁程度与障碍物和无人船的相对位置、相对运动速度有一定的关系。当动态障碍物向着无人船的方向运动时，为了避免无人船与动态障碍物发生碰撞，应考虑两者之间相对位置的变化。当动态障碍物在无人船的威胁区域内运动时，需要根据两者之间相对位置的变化来对斥力势场模型进行改进。即本发明的第二种实施方式，其大体内容与前述实施例一致，但是本实施例中对步骤6中动态障碍物斥力势场函数的改进如下：

式中，f为在斥力势场函数中引入的一个斥力势场系数。对f作如下定义：

式中，k_f为可调的影响系数；v_u和v_o分别为无人船和障碍物的运动速度；α为无人船运动速度v_u方向与无人船和障碍物连线l_uo的夹角；θ为障碍物运动速度v_o方向与无人船和障碍物连线l_uo的夹角。

当障碍物和无人船相向运动时，v_o cosθ的值为负数，那么斥力势场系数f将会大幅增加，从而使斥力势场的影响增强，使得无人船可以及时地调整路径规划，有效地避免和障碍物碰撞。

本发明不局限于上述最佳实施方式，任何人在本发明的启示下都可得出其他各种形式的产品，但不论在其形状或结构上作任何变化，凡是具有与本申请相同或相近似的技术方案，均应该落在本发明的保护范围之内。

Claims (1)

1.一种基于改进人工势场的智能无人船路径规划方法，其特征是包括以下的步骤：

步骤1：将已知电子海图中的环境信息、无人船起始点坐标和目标点坐标输入到路径规划***；

步骤2：对目标点建立引力势场，使用如下引力势场构造函数：

式中，k_a为引力势场的增益系数；ρ(X,X_g)为无人船当前位置到目标点位置的距离；ρ_g为引力势场影响的距离阈值；k_g为势场影响系数；η为势场调节参数；

对电子海图中已知的静态障碍物建立斥力势场，使用如下斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；P(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离；

步骤3：使用梯度下降法解算出一条从无人船的起始点到目标点的路径；

步骤4：使用解算出的路径指导无人船向目标点运动；

步骤5：在无人船向目标点运动的途中，使用障碍物检测装置检测无人船周围的动态障碍物；

步骤6：若检测到动态障碍物，则实时计算无人船与动态障碍物之间的距离，并在动态障碍物处建立动态障碍物斥力势场，使用如下斥力势场构造函数：

式中，k_r为斥力势场的增益系数；X为无人船当前位置坐标；X_o为障碍物的位置坐标；ρ(X，X_o)为无人船当前位置距离最近障碍物的欧几里德距离；ρ_o为障碍物的斥力势场覆盖的最大半径；ρ(X，X_g)为无人船当前位置到目标位置的欧几里德距离；ρ_u为以无人船为中心的圆形威胁区域半径，ρ_u＜ρ_o；

式中，f为在斥力势场函数中引入的一个斥力势场系数，对f作如下定义：

式中，k_f为可调的影响系数；v_u和v_o分别为无人船和障碍物的运动速度；α为无人船运动速度v_u方向与无人船和障碍物连线l_uo的夹角；θ为障碍物运动速度v_o方向与无人船和障碍物连线l_uo的夹角；

步骤7：根据更新的人工势场，不停地利用梯度下降法实时地解算出从当前位置到目标点的路径；

步骤8：重复步骤4至步骤7直到无人船停止运动，此时计算ρ(X，X_g)是否为0，若为0跳转到步骤10，若不为0跳转到步骤9；

步骤9：若ρ(X，X_g)不为0，则表示无人船陷入了局部极小值点，此时在局部极小点处添加一个扰动势场ΔU＝k_disρ(X，X_g)²，让无人船离开该点；

式中，ΔU为添加的扰动势能；k_dis为可调节的扰动势能随机系数；ρ(X，X_g)为无人船当前位置到目标点的距离；

步骤10：无人船到达目标点，路径规划结束。

Images