基于动态领域势场法的船舶避碰路径规划

孙 硕,杨少龙,向先波,范 雪

(华中科技大学 船舶与海洋工程学院,湖北 武汉,430074)

0 引言

随着智能船舶技术的蓬勃兴起,船舶路径规划作为重要支撑技术,在商船远洋运输航行、海洋区域探测搜索等场景中均有重要意义[1-3]。船舶路径规划是指在海洋环境中设计出一条合理路径使船舶能安全、经济地从出发点到达目标点的规划算法。常用的规划算法有遗传算法[4-5]、蚁群算法[6]、A*算法[7]和人工势场法[8]。

人工势场法因其计算量小、实时性高等特点成为路径规划领域有望用于实际场景的避碰规划算法之一,在船舶路径规划方面也有着诸多报道。文献[9]～[11]通过在斥力势场函数中引入船舶与障碍物相对距离解决目标不可达问题,其中张琪等[9]通过引入逃逸力的方法来帮助船舶逃离局部最小区域。陈超等[12]针对无人艇在势场中运行可能遇到的二力平衡造成的局部最小值问题给引力加入振荡函数F(x)协助避免二力平衡。马小轩等[13]针对水下复杂环境进行了基于改进人工势场法的水下自主航行器的路径规划。Yu 等[14]采用全局和局部路径规划切换方式,先用全局规划算法设计跟踪路径,再在局部复杂障碍环境中用人工势场法进行局部路径规划,完成复杂环境的路径规划任务。然而,现有研究在人工势场法斥力模型的作用距离设定仍有显著的局限性,固定的障碍物斥力势场范围使得避碰时机在整个路径规划中全程固化,过大障碍物斥力势场范围导致出现不必要的碰撞威胁以及路程的额外耗费;过小的障碍物斥力势场范围则会导致航行安全难以保证。在实际航海中,针对不同航行任务或某一航行任务的不同路段,有经验的驾驶员也会精心考量避碰时机,动态调整避让距离,以实现既安全又经济的航行。因此,根据航行状况和任务要求,动态调整避碰时机和避让距离将是少人化智能船舶实现安全经济航行任务的重要能力。实现像有经验驾驶员一样的优化避碰考量是避碰路径规划算法面临的重大挑战之一。

文中针对传统人工势场法在避碰时机、避让距离方面的局限性,提出一种基于船舶动态安全领域的改进人工势场法优化船舶的避碰路径规划。通过构建动态的船舶避让领域,实现船舶避让距离的自适应,从而实现更具针对性的航行危险感知。并考虑在复杂的真实海图场景中对大型障碍物避碰面临的局部最小值和路径抖动问题,提出改进的子目标点设立方法,完成复杂海图场景的船舶避碰路径规划。通过模拟海图场景中无人艇的仿真试验,验证了算法合理性。

1 传统人工势场法

人工势场法是最早由Khatib[15]提出的一种虚拟势场法,该方法将整个环境设想为抽象的势场空间。在研究船舶避碰路径规划过程中,把船舶假设在由目标点产生的引力势场和障碍物产生的斥力势场组成的环境中,如图1 所示,整个环境就是由障碍物和目标点分别引起的势场相互叠加,并共同影响的一个虚拟势场。船舶在目标点引力和障碍物斥力的共同作用下向目标点行进,从而完成船舶的避碰路径规划。

图1 人工势场避碰路径规划示意图Fig.1 Artificial potential field collision avoidance path planning

势场力定义为势场的负梯度,势能下降最快的方向,通过对引力势场函数求导得出引力,对斥力势场函数求导得出斥力。将船舶简化为质点,其实时位置p(x,y),则引力势场函数为

式中:k1为引力势场系数;d(p,pg)为船舶和目标点的距离;n1为引力势场因子;pg为目标点位置。

引力Fatt的方向为势能下降最快的方向,且引力大小为

斥力势场函数为

式中:k2表示斥力势场系数;n2为斥力势场因子;d(p,po)表示船舶和障碍物的距离;po为障碍物位置;d0为障碍物斥力势场范围。

船舶在运行到障碍物斥力势场范围以内才会受到斥力影响,否则斥力为0。则斥力为

合力由受到的引力和斥力矢量叠加求得

2 改进人工势场避碰路径规划

2.1 人工势场法固定避让距离的改进

设定航行全程固定的障碍物斥力势场范围对航行过程中不同避碰场景的适配性欠佳,会出现不必要的转弯,或没有及时采取避碰措施而导致距离障碍物过近,进而影响航行安全。受四元安全领域理论启发,引入动态领域,改进斥力势场函数。构造与速度有关的动态船舶避让领域,当检测到障碍物进入避让领域后,根据与障碍物的距离计算所受斥力,避让领域以外船舶不受斥力势场影响,实现船舶与障碍物的避让距离在航行过程中的自适应调整。

Wang[16]提出以前后左右4 个半径Rfore、Raft、Rport和Rstarb为四元的安全领域,即

四元安全领域模型为

式中:k取1,2,3,···,N,k的取值不同导致领域形状不同;L为本船船长;kAD和kDT为船舶进距和旋回初径的系数,其不仅可以通过进距AD和旋回初径DT的数据计算求取,还可以利用船速估算得出

式中,v为本船船速。

基于四元安全领域的启发,进行相应简化。前方是避碰时最需要考虑的方向,前半径Rfore也是4 个方向半径中最大的,利用Rfore可构建1 个足够安全的自适应一元圆形船舶安全领域。安全领域是保证航行安全不被侵入的区域,为避免或减少对安全领域的入侵,应有一个范围更大的避让领域,当障碍物进入到避让领域(即船舶与障碍物距离小于避让距离)时可认为存在碰撞危险,船舶应采取相应避碰措施来减少或者避免对安全领域的入侵。障碍物斥力势场范围d0实际上为避让距离,将安全领域半径放大1 倍作为避让领域半径,在利用动态避让领域改进后避让领域半径d0=2×Rfore。为避免或减少对安全领域的入侵,当船舶与障碍物距离小于避让距离时,障碍物会有斥力产生,引导船舶在路径规划时进行避碰。安全领域与避让领域如图2 所示。

图2 船舶安全领域与避让领域Fig.2 Ship safety domain and avoidance domain

构建的一元船舶安全领域边界为

对应的船舶动态避让领域边界为

当障碍物进入到外层边界范围后,船舶会受到障碍物的斥力影响,此时,d0不再是一个定值,而是会依据船舶速度在航行中的变化进行自适应调整。简化避让距离的设定调试过程,优化了对航行状况变化的适应性,有助于改善在固定障碍物斥力势场范围下较低速度时不必要的障碍物斥力影响、不必要的路径抖动以及较高速度时距障碍物过近等问题。

2.2 局部最小值和路径抖动的改进

针对传统人工势场算法在面对有大型障碍物的复杂场景时易出现局部最小值和路径抖动问题(如图3 所示),通过设立子目标点的方法进行优化。主要流程是检测是否需要设立子目标点,搜索设立子目标点的方向,确定设定半径并设立子目标点,最后通过设立的子目标点引导无人船逃离受限区域来改善上述航行规划问题,完成逃离后则向目标点行进继续规划路径。

图3 局部最小值和路径抖动问题Fig.3 Local minimum and path jitter problem

检测是否陷入局部最小或路径抖动问题,采取以下判定式[17]

式(13)表示合力为零。式(14)表示位移小于一段时间内移动的距离,其中 |Pa-Pb|表示a,b点的直线长度,也就是从其中一点到另一点的位移;Sab表示两点之间行进的距离;α为0～1 的值。当满足式(13)和(14)其中之一时,可认为陷入了局部最小或路径抖动问题。

分析α的取值可知,较小的取值可更精确地检测是否陷入局部最小值问题,但α值越小需要越长的判断时间,改善路径抖动的效果也会很差。α值越大,可越快判断是否陷入局部最小值或路径是否出现抖动,但较大的取值会导致到达子目标点以后由于切换目标点引起的转弯等现象也可能被式(14)判断为真,这样会向设立的多余的子目标点驶去导致偏航,或根据算法无法正常规划子目标点。所以,为获得更快更合理的判断局部最小值的方法和面对大型障碍物时实现路径波动问题优化,在式(13)和式(14)(α取较大值)组成的判定式后加上一道判断程序,排除不是由于障碍物引起的路径抖动问题。当满足任意式(13)或式(14)后判断在船舶避让领域内是否有障碍物存在,船舶距障碍物的最近距离为

上式如果结果为真,表示虽然满足了式(13)与式(14),但这并不是由障碍物引起的路径偏转或波动,故不规划新的子目标点;如果结果为假,则继续规划子目标点的位置。

图4为确定子目标点位置的示意图,首先确定设立子目标点的方向,从引力方向开始向两侧不断地扫描,初始探测方向即为引力方向。探测距离为l,若探测到障碍物就向两侧各偏转Δθ角直到一侧探测不到障碍物,则此方向为首次探测到没有障碍物的方向θnew。图中:rmin为θnew角度搜索线上从无人船到障碍物的最近距离;θa为调整角。

图4 子目标点规划示意图Fig.4 Planning sub-target points

在θnew方向上设立子目标点虽不会设立到障碍物上,但可能造成子目标点距障碍物过近,故需增加调整角θa,即

式中:c为常量;rmin也可用作设定子目标点的距离半径,在θ方向上rmin远处设立子目标点,以实现θa随设定半径大小的变化。通过rmin和θ实现了设定半径的自适应,并保证子目标点距障碍物有适当的距离。改进后算法流程如图5 所示。

图5 改进后算法流程图Fig.5 Flow chart of improved algorithm

3 仿真试验

3.1 试验系统

文中的仿真试验系统如图6 所示。

图6 仿真试验系统示意图Fig.6 Simulation test system

1) 船舶模型参数见表1[18]。

表1 船舶模型参数Table 1 Ship model parameters

2) 感知系统主要用来获取自身状态和周围环境信息,输送给导航系统障碍物信息。

3) 制导系统根据周围环境指导如何进行下一步航行规划。

4) 底层控制器将输入偏差转化为控制力和力矩输出至船舶模型,主要由分别用于速度和航向的2 个控制器组成,且

式中:Kp,u,Kp,ψ分别为速度控制和航向控制的比例增益;Td,ψ为微分系数,其取值分别为1.0、5.0 和3.2;其余参数为船舶模型相关参数[18]。

3.2 试验结果

3.2.1 传统人工势场避碰路径规划

针对传统人工势场原理,设参数n1=2,n2=2;k1=10、k2=1×1010;d0=100 m,在适当的仿真环境下,制定1 000 m×1 000 m 的地图,起点在(0,0)位置,终点(1 000,1 000)位置,障碍物由黑色椭圆表示。

如图7 所示,采用传统人工势场法在简化的设定避碰环境中进行避碰路径规划。红色线条为船舶船位组成的轨迹,可以看出在经过障碍物附近时会出现一些偏转,躲避障碍物,完成避碰路径规划任务。

图7 传统人工势场避碰路径仿真结果Fig.7 Simulation results of collision avoidance path of traditional artificial potential field

3.2.2 动态领域改进的人工势场避碰路径规划

为检验基于动态领域改进的人工势场法的合理性,分别采用不同的控制速度进行试验,仿真环境方面,在(100,150)、(400,450)、(350,150)、(650,500)点设立4 个大小不一的椭圆障碍物,起始点和目标点仍分别位于(0,0)、(1 000,1 000),如图8 所示。从图中可见,速度分别为1、5、10、15 m/s 时路径规划和避碰状况存在明显差异。在虚线转弯处,速度越大,船舶越远离障碍物,避碰路径规划越安全。

图8 基于动态领域改进的人工势场法仿真结果Fig.8 Simulation results of improved artificial potential field method based on dynamic domain

图9为速度在1、5、10、15 m/s 时固定d0(d0=30、100、200 m)和改进后动态d0的避碰试验路径对比。图中也展示出动态d0时船舶安全领域和避让领域。图9(a)中,当速度为1 m/s 时,只有d0=30 m和动态d0的仿真试验在全程安全航行的前提下减少或减弱了避让、偏转行为和路径抖动。然而,在控制速度分别提高至5、10、15 m/s 的试验中,过小的d0(d0=30 m)则出现明显的距离障碍物过近的现象,难以保证足够的安全。

图9 不同速度下避碰试验路径对比Fig.9 Comparison of collision avoidance paths at different speeds

图10 展示了图9 中各航次试验的总航程数据,可知改进后算法构造的动态避让距离d0在航行速度较低时保留了小避让距离的优势,减少或减弱避让、偏转行为和路径抖动,使行驶路径更加平缓,减少了航程消耗。在速度为1 m/s 时,动态d0和d0=30 m 的总航程耗费明显少于d0=100 m 和d0=200 m 时。动态d0相对于d0=100 m 和d0=200 m时分别节省航程8%和9%。结合图8 可见改进后的动态d0能够很好地完成各速度工况下避碰路径规划任务试验,适应不同的航行状况。

图10 不同速度下避碰试验航程比较Fig.10 Comparison of collision avoidance voyages at different speeds

基于四元安全领域简化的船舶安全领域和船舶避让领域,考虑船舶速度实时计算避让距离,调整避碰时机。在高速时保证足够的避让距离减小碰撞危险;在低速时通过较小的避让距离筛掉不必要的碰撞危险,在安全航行前提下减少避让次数和因避让造成的航程浪费。

3.2.3 真实海图场景的改进人工势场避碰路径规划

进一步在真实海图场景中验证改进后人工势场避碰路径规划的局部避障能力。截取一段真实海图上的复杂海陆环境作为避碰检验场景,范围为2 800 m×6 360 m,对地图进行二值化处理,生成二值化图像,用于构建仿真场景。针对更大范围的试验环境,设置k1=10,k2=1×1016,速度为10 m/s(19.44 kn)。

图11为动态领域势场法在真实海图的仿真试验结果。图12为采用子目标法后的试验结果,其中α=0.3,探索可设立子目标点的位置时,每次的偏转角度为3°。如图11 所示,在真实航海环境下,存在着各种岛屿等大型障碍物,改进动态领域势场法尽管能完成路径规划任务,但在靠近大型岛屿障碍物时仍有明显路径徘徊和抖动,最终轨迹十分复杂。相比之下,如图12 所示,根据2.2 节改进子目标点设立方法可有效解决陷入受限区域后的航行指引,减少航行轨迹的徘徊和抖动。

图11 真实海图环境船舶避碰路径规划仿真结果Fig.11 Simulation results of ship collision avoidance path planning in real chart environment

图12 采取子目标点法的真实海图环境仿真结果Fig.12 Simulation results of real chart environment with sub-target point method

为了更加优化逃脱局部最小值问题和轨迹抖动问题,设定α=0.8,在判定式(13)和式(14)之后采用式(15),滤掉不是由障碍物引起的路径抖动,再次进行同参数设定的仿真试验,由图13 可见改进后人工势场避碰路径规划算法可更简洁快速地改善局部最小值问题,在面对大型障碍物时出现的路径抖动得以优化。

图13 优化后的真实海图环境仿真结果Fig.13 Real chart environment simulation results after optimization

4 结论

针对传统人工势场避碰路径规划面临的避让距离固化、避碰易陷入局部最小值等问题,提出一种基于改进人工势场的船舶避碰路径规划方法,通过考虑航速动态调整避让距离,实现更加符合航海实践的运动规划,解决复杂海图场景中避碰路径规划。主要结论如下:

1) 考虑到避让距离d0对避障场景的适应需求,基于四元安全领域构造一元船舶安全领域和动态避让领域,通过避让领域动态计算受障碍物影响的避让距离,实现不同航行状况的避碰时机自适应调整;

2) 针对大型障碍物存在的真实海图场景下传统人工势场法避碰规划易出现的局部最小值和路径抖动问题,提出改进的子目标点规划方法,通过设定子目标点实现逃离局部路径规划困难区域。

文中尚未考虑动态环境避障,后续会深入拓展研究动态领域势场法的移动目标避碰,并进一步优化改进后无人船面对大型障碍物进行路径规划时所存在的些许轨迹抖动问题。