CN115167465A - 基于人工势场栅格法的无人潜器三维路径规划方法 - Google Patents

Abstract

本发明针对人工势场法(artificial potential field approach,APFA)在无人潜器三维路径规划应用存在局部震荡、易陷入局部极小值的问题，公开了基于人工势场栅格法的无人潜器三维路径规划方法，具体包括：在三维地图下将障碍物进行栅格化处理，采用斥力点附着在栅格体表面的方法，弥补传统APFA建模理想化导致规划路径变长的缺点；改变栅格体表面的斥力函数，使栅格体表面势场平行栅格体表面形成环绕势场，解决无人潜器路径规划局部震荡的问题；通过引入虚拟目标点，避免无人潜器陷入局部最优；最后对路径点进行插值平滑处理，使得路径光滑。仿真结果表明，本发明提出改进APFA方法有效克服了无人潜器在三维路径规划下局部震荡及易陷入局部极小值的不足，规划路径平滑，时间效率高。

Description

基于人工势场栅格法的无人潜器三维路径规划方法

技术领域

本发明涉及无人潜器路径规划领域，尤其涉及基于人工势场栅格法的无人潜器三维路径规划方法。

背景技术

无人潜器是一种能在水下浮游或在海底行走，具有观察能力和使用机械手或其他工具进行水下作业的装置。从机器人学的角度看，无人潜器属于特种机器人范畴，在海洋工程界，无人潜器通常也称为水下机器人。路径规划技术直接影响着无人潜器平稳、安全、自主的完成水下作业的能力。在现有的路径规划算法中，人工势场法是一种常见的局部路径规划算法，具有计算时间短、易实现、路径平滑的优点。

但传统人工势场法存在局部极小值和局部震荡的问题，而且对无人潜器路径规划时，将障碍物视为质点，使得障碍物建模过于简单，导致无人潜器无法准确完成水下作业。专利CN111481933A《一种基于改进势场栅格法的游戏路径规划方法》重点在二维空间下将整个地图进行栅格化处理，角色根据栅格进行移动，同时该专利采用模拟水流法逃离局部极小值，适用于二维空间，但具有局限性，上升到三维空间该方法还有待改善；论文《融栅格法和人工势场法的机器人三维路径规划》将三维地图进行栅格化处理，但把机器人和障碍物都看成质点，机器人在三维空间中按栅格块进行移动，路径较远且不具有普适性。

发明内容

本发明的目的是提供基于人工势场栅格法的无人潜器三维路径规划方法，能够有效解决人工势场法在无人潜器三维路径应用存在局部震荡和易陷入局部最小值问题。

本发明为了解决上述问题采用以下技术方案：设计基于人工势场栅格法的无人潜器三维路径规划方法，提出在三维空间中对障碍物进行栅格化处理，同时采用斥力点附着在栅格体表面的方法，改变斥力势场函数，引入虚拟目标点，避免无人潜器局部震荡和陷入局部最优。具体包括以下步骤：

1.基于人工势场栅格法的无人潜器三维路径规划方法，其特征在于，包括以下步骤：

步骤1：

在三维地图下将障碍物进行栅格化处理，将斥力点附着在栅格体表面，同时改变斥力函数，使斥力垂直栅格体表面且大小与引力在该方向分量相同，根据无人潜器的步长设置合适的引力系数，执行步骤2；

步骤2：

判断无人潜器是否到达目标点，是则对路径点进行插值平滑处理，然后结束，否则执行步骤3；

步骤3：

判断无人潜器是否在栅格体的斥力影响范围内，是则执行步骤5，否则执行步骤4；

步骤4：

无人潜器在引力作用下向目标点移动，构建传统引力函数，目标点对无人潜器产生引力，在引力作用下，无人潜器前往目标点，执行步骤2；

步骤5：

无人潜器进入到栅格体影响范围内，将障碍物分为普通障碍物和特殊障碍物，其中将目标点进行x轴、y轴和z轴三个方向进行投影，其中与之相交的障碍物为特殊障碍物，此类障碍物表面存在局部最优的风险，判断当前障碍物是否为特殊障碍物，是则执行步骤7，否则执行步骤6；

步骤6：

栅格体对无人潜器产生斥力作用，其斥力函数如下：

式中：F_r为斥力函数，F_aobs是引力沿垂直栅格表面的分量，ρ_obs是一个矢量，模长为1，方向垂直障碍物指向无人潜器，R为障碍物斥力的影响范围，ρ(x_goal,x_obs)是一个矢量，模长为无人潜器当前位置与障碍物之间的距离，方向为垂直表面指向无人潜器，|ρ(x_goal,x_obs)|为ρ(x_goal,x_obs)的模长，当无人船在障碍物斥力的影响范围R内，斥力函数F_r对其产生斥力，让其停止靠近障碍物，无人潜器在势场中受的合力为：

F_res＝F_a+F_r (2)

式中：F_a为引力，F_res为合力，无人潜器沿合力方向移动，然后执行步骤2；

步骤7：

当前障碍物为特殊障碍物，即在当前障碍物斥力影响范围内无人潜器有陷入局部极小值的风险，当无人潜器进入斥力影响范围，判断目标点与各个面的距离，以最近的面为对称轴，设置虚拟目标点，虚拟目标点与当前特殊障碍物面水平一致，虚拟目标点对无人潜器产生引力，无人潜器逃离当前障碍物影响范围以后，取消虚拟目标点的引力，返回步骤2。

本发明具有如下有益效果：

1.本发明所述方法将栅格法与人工势场法相结合，能有效解决局部震荡，摆脱局部极小点，方法规划的路径短，效率高，实时性能好，普适性强；

2.本发明将障碍物进行栅格化处理，采用斥力点附着在栅格体表面的方法，使无人潜器在移动过程中不再局部震荡；

3.本发明将障碍物分为普通障碍物和特殊障碍物，在遇到特殊障碍物时采用虚拟目标点，使无人潜器在避免陷入局部最优的前提下规划路径尽可能最短；

4.在相同障碍物建模条件下与传统人工势场法相比，在传统人工势场法陷入局部极小的情况下，本发明所述方法均能避免局部最优顺利到达目标点，在相同障碍物下的全局路径规划本发明效率较传统方法提升4.17％。

附图说明

图1为基于人工势场栅格法的无人潜器路径规划方法的流程图；

图2为栅格体障碍物模型；

图3为斥力点附着在栅格体表面的示意图；

图4为特殊障碍物的示意图；

图5为构建虚拟目标点的示意图；

图6为特殊障碍物下避免局部最优的结果图；

图7为传统人工势场法与本发明在特殊障碍物下结果对比图；

图8为传统人工势场法与本发明在普通障碍物下结果对比图；

图9为传统人工势场法与本发明在全局路径下结果对比图。

具体实施方式

图1为本发明基于人工势场栅格法的无人潜器三维路径规划方法流程图，包括以下步骤：

步骤1：

在三维地图下将障碍物进行栅格化处理，障碍物模型如图2所示，*为目标点，·为起始点，将斥力点附着在栅格体表面如图3所示，实际应用中斥力点远多于图中所示且斥力影响范围需覆盖整个栅格体表面，同时改变斥力函数，使斥力垂直栅格体表面且大小与引力在该方向分量相同，根据无人潜器的步长设置合适的引力系数，执行步骤2；

步骤2：

判断无人潜器是否到达目标点，是则对路径点进行插值平滑处理，然后结束，否则执行步骤3；

步骤3：

判断无人潜器是否在栅格体的斥力影响范围内，是则执行步骤5，否则执行步骤4；

步骤4：

无人潜器在引力作用下向目标点移动，其引力函数如下：

F_a＝K_a·ρ(x_goal,x₀) (1)

式中：F_a为引力函数，K_a为引力系数，x₀为无人潜器当前位置，x_goal为目标点位置，ρ(x_goal,x₀)是一个矢量，模长为无人潜器当前位置与目标点之间的距离，方向为无人潜器指向目标点，引力函数F_a对无人潜器产生引力，在引力作用下，无人潜器前往目标点，执行步骤2；

步骤5：

无人潜器进入到栅格体影响范围内，将障碍物分为普通障碍物和特殊障碍物，其中将目标点进行x轴、y轴和z轴三个方向进行投影，其中与之相交的障碍物为特殊障碍物，此类障碍物表面存在局部最优的风险，如图4所示，当前障碍物W面和H面与目标点投影线相交，属于特殊障碍物，相交点为危险极小值点，判断当前障碍物是否为特殊障碍物，是则执行步骤7，否则执行步骤6；

步骤6：

栅格体对无人潜器产生斥力作用，其斥力函数如下：

式中：F_r为斥力函数，F_aobs是无人潜器受到的引力沿垂直栅格表面的分量，ρ_obs是一个矢量，模长为1，方向垂直障碍物指向无人潜器，R为障碍物斥力的影响范围，ρ(x_goal,x_obs)是一个矢量，模长为无人潜器当前位置与障碍物之间的距离，方向为垂直表面指向无人潜器，|ρ(x_goal,x_obs)|为ρ(x_goal,x_obs)的模长，当无人船在障碍物斥力的影响范围R内，斥力函数F_r对其产生斥力，大小与引力在该方向的分量相同，让其停止靠近障碍物，平行当前障碍物表面移动且不会震荡，无人潜器在势场中受的合力为：

F_res＝F_a+F_r (3)

式中：F_a为引力，F_res为合力，无人潜器沿合力方向移动，然后执行步骤2；

步骤7：

当前障碍物为特殊障碍物，即在当前障碍物斥力影响范围内无人潜器有陷入局部极小值的风险，如图5所示，当无人潜器进入Hf面的斥力影响范围，在当前特殊障碍物面的主视图中判断目标点与各个面的距离，以最近的Wr面为对称轴，设置虚拟目标点，★为虚拟目标点，y轴坐标与当前特殊障碍物面一致，虚拟目标点产生引力，无人潜器逃离当前障碍物影响范围以后，取消虚拟目标点的引力，如图6所示，返回步骤2。

通过Matlab/Simulink软件对本发明方法进行仿真，图7、图8、图9分别为本发明与传统人工势场法在不同障碍物下的路径规划，坐标轴单位为km，步长为0.15km。图7为特殊障碍物，传统人工势场法陷入局部最优且存在局部震荡，本发明能够提前预测并产生虚拟目标点，成功避免局部最优到达目标点。图8为普通障碍物，对于本发明而言不存在局部极小的问题，可以顺利到达目标点，但传统人工势场法会陷入局部最优。

图9为全局路径规划，传统人工势场法在栅格体中心设置斥力点，斥力影响范围为球形且半径大于栅格体顶点与中心点的距离，在路径规划时会导致路径偏长，如图所示本发明步数为207步，传统人工势场法步数为216步，效率提升4.17％。

以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发明的保护范围内。

Claims (1)

1.基于人工势场栅格法的无人潜器三维路径规划方法，其特征在于，包括以下步骤：

步骤1：

在三维地图下将障碍物进行栅格化处理，将斥力点附着在栅格体表面，同时改变斥力函数，使斥力垂直栅格体表面且大小与引力在该方向分量相同，根据无人潜器的步长设置合适的引力系数，执行步骤2；

步骤2：

判断无人潜器是否到达目标点，是则对路径点进行插值平滑处理，然后结束，否则执行步骤3；

步骤3：

判断无人潜器是否在栅格体的斥力影响范围内，是则执行步骤5，否则执行步骤4；

步骤4：

无人潜器在引力作用下向目标点移动，构建传统引力函数，目标点对无人潜器产生引力，在引力作用下，无人潜器前往目标点，执行步骤2；

步骤5：

无人潜器进入到栅格体影响范围内，将障碍物分为普通障碍物和特殊障碍物，其中将目标点进行x轴、y轴和z轴三个方向进行投影，其中与之相交的障碍物为特殊障碍物，此类障碍物表面存在局部最优的风险，判断当前障碍物是否为特殊障碍物，是则执行步骤7，否则执行步骤6；

步骤6：

栅格体对无人潜器产生斥力作用，其斥力函数如下：

式中：F_r为斥力函数，F_aobs是引力沿垂直栅格表面的分量，ρ_obs是一个矢量，模长为1，方向垂直障碍物指向无人潜器，R为障碍物斥力的影响范围，ρ(x_goal,x_obs)是一个矢量，模长为无人潜器当前位置与障碍物之间的距离，方向为垂直表面指向无人潜器，|ρ(x_goal,x_obs)|为ρ(x_goal,x_obs)的模长，当无人船在障碍物斥力的影响范围R内，斥力函数F_r对其产生斥力，让其停止靠近障碍物，无人潜器在势场中受的合力为：

F_res＝F_a+F_r (2)

式中：F_a为引力，F_res为合力，无人潜器沿合力方向移动，然后执行步骤2；

步骤7：

当前障碍物为特殊障碍物，即在当前障碍物斥力影响范围内无人潜器有陷入局部极小值的风险，当无人潜器进入斥力影响范围，判断目标点与各个面的水平距离，以最近的面为对称轴，设置虚拟目标点，虚拟目标点与当前特殊障碍物面水平一致，虚拟目标点对无人潜器产生引力，无人潜器逃离当前障碍物影响范围以后，取消虚拟目标点的引力，返回步骤2。

Images