[发明专利]基于人工势场栅格法的无人潜器三维路径规划方法

权利要求书

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。