一种基于改进人工势场法的无人船路径规划方法
本发明涉及一种基于改进人工势场法的无人船路径规划方法，属于海洋无人器运动操纵及路径规划领域。
背景技术：
目前，以无人船为代表的无人系统正在快速发展，大量无人系统被研制出来投入使用。但是无人船运动操纵的路径规划研究尚不成熟。
常用的路径规划方法有模拟退火算法、模糊逻辑算法、禁忌搜索算法、a*算法、人工势场法等。人工势场法较其他算法而言，具有运算时间短、实时性强、硬件适配性好等方面的优势。但是传统人工势场法在无人船运动操纵及路径规划领域的应用有如下问题：
(1)障碍物建模问题，无人船在复杂的海面航行，将障碍物视为质点，规划出的路径难以达到最优；
(2)局部极小点问题，容易让无人船陷入局部震荡，导致目标不可达；
(3)转角变化剧烈问题，无人船在实际航行过程中，转角变化要连续，不能突变，且不能超过无人船的最大转角。
综上所述，如何解决传统人工势场法在考虑无人船实际航行情况下的应用成为亟待解决的难点。
技术实现要素：
本发明的目的是提出一种基于改进人工势场法的无人船路径规划方法，解决传统人工势场法应用在无人船路径规划过程中的障碍物建模理想化、易陷入局部极小点和转角变化剧烈等问题。
本发明为了解决上述问题采用以下技术方案：设计了一种基于改进人工势场法的无人船路径规划方法，在传统人工势场法基础上，对障碍物进行膨胀化处理并预留出转向区；当传统人工势场法构建的引力与斥力共线且方向相反时，让无人船提前在转向区内进行转向，解决无人船陷入局部极小点问题；加入无人船自身最大转角和最大角加速度限制，解决无人船转角变化剧烈问题。具体包括以下步骤：
步骤1：
建立地图中障碍物的圆形等效模型，将障碍物膨胀处理，预留出转向区，进行相关参数设置，然后进入步骤2；
步骤2：
判断无人船是否到达目标点，是则结束，否则进入步骤3；
步骤3：
构建引力函数：
fg(x)＝k·d(x,xgoal)(1)
式中：fg(x)为引力函数，k为引力系数，x为无人船当前位置，xgoal为目标点位置，d(x,xgoal)是一个矢量，模长为无人船当前位置与目标点之间的距离，方向为无人船指向目标点。引力函数fg(x)对无人船产生引力，在引力作用下，无人船前往目标点。
构建斥力函数：
式中：fo(x)为斥力函数，n为斥力系数，xobs为障碍物位置，d0为障碍物的影响半径，d(x,xobs)是一个矢量，模长为无人船当前位置与障碍物之间的距离，方向为障碍物指向无人船，|d(x,xobs)|为d(x,xobs)的模长。当无人船在障碍物的影响半径d0内，斥力函数fo(x)对其产生斥力，让其远离障碍物。然后进入步骤4；
步骤4：
判断引力和斥力是否共线且方向相反，是则进入步骤5，否则进入步骤8；
步骤5：
此时无人船在转向区内，为了摆脱局部极小点，躲避障碍物，构建转角公式：
式中：θ为无人船在转向区的转角，dact是障碍物膨胀过后的实际半径，θmax表示无人船自身最大转角。转角公式让无人船转动最大转角θmax，达到摆脱局部极小点，躲避障碍物的效果。然后进入步骤6；
步骤6：
为了避免角度θ变换过大，导致路径突变，构建角加速度公式：
式中：θcurr(t)表示当前无人船的转角角度，θcurr(t-δt)表示前一时刻无人船的转角角度，δt表示采样间隔，
步骤7：
判断无人船是否离开转向区，是则进入步骤2，否则进入步骤5；
步骤8：
构建合力公式：
fs(x)＝fo(x)+fg(x)(5)
式中：fs(x)为合力。
构建合力单位向量公式：
式中：θs(t)为合力单位向量，|fs(x)|为合力fs(x)的模长。合力单位向量公式求合力单位向量θs(t)，θs(t)为无人船下一步的运动方向。然后进入步骤9；
步骤9：
判断无人船下一步运动方向与无人船当前方向差值的绝对值是否大于最大角加速度，是则进入步骤11，否则进入步骤10；
步骤10：
计算无人船下一步运动，然后进入步骤2；
步骤11：
根据公式(4)计算无人船的转角角度，计算无人船下一步运动，然后进入步骤9。
本发明具有如下有益效果：
1.本发明所述方法在不改变传统势场函数的基础上，逃脱局部极小点，方法简单，计算量小，实时性能好；
2.相较于论文《基于改进人工势场法的移动机器人避障研究》提供的改进方法，有以下优点：
(1)考虑自身最大转角和角加速度的限制；
(2)所规划出的躲避局部极小点的路径短而且光滑；
(3)躲避障碍物时无人船与障碍物保持的距离远，更为安全。
3.相较于论文《基于改进人工势场法的无人船路径规划算法》提供的改进方法，有以下优点：
(1)该论文摆脱局部极小点时，随机选取0～90°的角度进行转向，转角发生突变，而本发明设计转角公式和角加速度公式，让转角变化光滑；
(2)该论文判断无人船陷入局部极小点的情况是根据无人船陷入局部极小点后路径出现三次震荡，而本发明在引力和斥力共线且方向相反时，提前进行转向，摆脱局部极小点效率高。
附图说明
图1为基于改进人工势场法的无人船路径规划方法的流程图；
图2为障碍物的圆形等效模型；
图3为无人船路径规划受力分析图；
图4为无人船自身最大转角图；
图5为无人船路径规划引力和斥力共线时受力分析图；
图6为无人船路径规划图；
图7为无人船路径规划的角度变化图；
图8为无人船路径规划的角加速度变化图。
具体实施方式
图1为本发明基于改进人工势场法的无人船路径规划流程图，包括以下步骤：
步骤1：
建立地图中障碍物的圆形等效模型，将障碍物膨胀处理，预留出转向区，如图2所示，根据障碍物边缘对障碍物进行膨胀化处理，图中xobs为障碍物的位置，dact为障碍物膨胀处理后的半径长度，图中实线圆包围的部分是障碍物膨胀后的结果，d0为构建障碍物斥力函数的影响半径，图中虚线圆包围的部分是障碍物斥力的影响范围，即无人船在此范围内会受到斥力。图中虚线圆与实线圆中间的圆环部分定义为转向区，若无人船受到的引力和斥力共线且方向相反时，再往前走，就有可能陷入局部极小点，因此，无人船进入转向区进行转向。进行相关参数设置，然后进入步骤2；
步骤2：
判断无人船是否到达目标点，是则结束，否则进入步骤3；
步骤3：
构建引力函数：
fg(x)＝k·d(x,xgoal)(1)
式中：fg(x)为引力函数，k为引力系数，x为无人船当前位置，xgoal为目标点位置，d(x,xgoal)是一个矢量，模长为无人船当前位置与目标点之间的距离，方向为无人船指向目标点。引力函数fg(x)对无人船产生引力，在引力作用下，无人船前往目标点。
构建斥力函数：
式中：fo(x)为斥力函数，n为斥力系数，xobs为障碍物位置，d0为障碍物的影响半径，d(x,xobs)是一个矢量，模长为无人船当前位置与障碍物之间的距离，方向为障碍物指向无人船，|d(x,xobs)|为d(x,xobs)的模长。当无人船在障碍物的影响半径d0内，斥力函数fo(x)对其产生斥力，让其远离障碍物。然后进入步骤4；
步骤4：
判断引力fg(x)和斥力fo(x)是否共线且方向相反，若如图5所示，无人船受到的引力fg(x)和斥力fo(x)共线且方向相反，此时则进入步骤5，否则进入步骤8；
步骤5：
此时无人船在转向区内，为了摆脱局部极小点，躲避障碍物，构建转角公式：
式中：θ为无人船在转向区的转角，dact是障碍物膨胀过后的实际半径，θmax表示无人船自身最大转角，如图4所示，无人船可以向左或向右转动最大转角θmax。转角公式让无人船转动最大转角θmax，达到摆脱局部极小点，躲避障碍物的效果。然后进入步骤6；
步骤6：
为了避免角度θ变换过大，导致路径突变，构建角加速度公式：
式中：θcurr(t)表示当前无人船的转角角度，θcurr(t-δt)表示前一时刻无人船的转角角度，δt表示采样间隔，
步骤7：
判断无人船是否离开转向区，是则进入步骤2，否则进入步骤5；
步骤8：
构建合力公式：
fs(x)＝fo(x)+fg(x)(5)
式中：fs(x)为合力。
构建合力单位向量公式：
式中：θs(t)为合力单位向量，|fs(x)|为合力fs(x)的模长。合力单位向量公式求合力单位向量θs(t)，θs(t)为无人船下一步的运动方向。如图3所示，图中x为无人船当前位置，xobs为障碍物位置，xgoal为目标点位置，fg(x)为无人船受到目标点的引力，fo(x)为无人船受到障碍物的斥力，fs(x)为无人船在引力fg(x)和斥力fo(x)作用下的合力，合力fs(x)单位向量θs(t)为无人船下一步的运动方向。然后进入步骤9；
步骤9：
判断无人船下一步运动方向与无人船当前方向差值的绝对值是否大于最大角加速度，是则进入步骤11，否则进入步骤10；
步骤10：
计算无人船下一步运动，然后进入步骤2；
步骤11：
根据公式(4)计算无人船的转角角度，计算无人船下一步运动，然后进入步骤9。
利用matlab根据流程图对改进人工势场法进行仿真，得到图6，图7，图8所示的结果。图6为无人船路径规划图，设置地图长度为5km，宽度为5km，无人船的出发点坐标x为(3，0)，目标点的坐标xgoal为(3，5)，引力系数k为9，障碍物的坐标xobs为(3，2.5)，障碍物膨胀化半径dobs为0.48km，障碍物影响半径d0为0.8km，斥力系数n为0.3，无人船最大转角θmax为0.550rad，最大角加速度
- 还没有人留言评论。精彩留言会获得点赞！