一种基于人工势场的双艇协同系统避障方法

本发明涉及无人艇避障，特别涉及一种基于人工势场的双艇协同系统避障方法。

背景技术：

1、海上发生溢油事故具有危害时间长、影响程度大等特点，会对海洋环境和生物健康造成严重的危害。有人船舶搭载围油栏对溢油进行围捕和回收，不仅会消耗大量的人力和物力，还会对船上工作人员造成严重的伤害。利用无人艇进行海上溢油回收任务有很多优势，包括可以避免人的参与、降低回收成本、提高任务执行的效率、避免回收时间过长导致溢油进一步挥发和扩展等。双艇溢油回收系统是由两艘无人艇拖曳一条围油栏组成的，经常被用于海上溢油事故的处理。合理规划无人艇回收溢油过程中的路径，可以保障无人艇安全高效地完成任务，具有实际应用价值。

2、目前人工势场法因为具有计算简单、易于实现的优点而被广泛应用于路径规划中。传统的人工势场法是由khatib提出的一种模拟自然场的虚拟力法。该方法把障碍物、目标点、机器人等看做一个点进行处理，机器人在一个由引力势场和斥力势场组成的虚拟力场中运动。其中，目标点的周围存在对机器人有引力作用的引力势场，障碍物周围存在对机器人有排斥作用的斥力势场。该方法原理简单、计算方便且易于实现，因此应用广泛。但也存在一些缺陷：1.当机器人距离障碍物较近而目标点较远时，机器人所受引力远大于斥力，有可能与障碍物相撞；2.当目标点在障碍物的影响距离之内且距离障碍物较近时，障碍物对机器人的斥力大于机器人所受引力，此时机器人不能到达目标点；3.在某一位置机器人所受斥力和引力的合力为零，则会陷入局部极小值点而停止运动。

3、基于改进的人工势场方法进行路径规划主要采用以下方法实现，专利号为cn111176272b，名称为“一种基于运动约束的人工势场轨迹规划方法及系统”，提供了一种基于人工势场的车辆轨迹规划方法。该方法通过增加运动约束，解决车的轨迹不顺滑问题；同时设定最小引力，当计算出的引力小于最小引力时，使用设定的最小引力，以解决目标不可达问题。但是该方法并未解决传统人工势场法存在的局部极小值导致的机器人停止运动和可能与障碍物碰撞的问题，规划出的轨迹不能完全满足实际需求。专利号为cn112099501b，名称为“一种基于势场参数优化的无人艇路径规划方法”，提出了一种基于人工势场的无人艇路径规划方法。该方法通过多个引力和斥力增益系数进行路径规划，同时考虑避碰系数、长度系数和转角系数三个评价系数，借鉴经典的遗传算法，使规划出来的路径既避免了局部极小点，又能够满足无人艇的运动约束。但该方法并未解决传统人工势场法存在的目标不可达和可能与障碍物碰撞的问题，仍存在改进空间。专利号为cn113110604b，名称为“一种基于人工势场的路径动态规划方法”，提供了一种结合模拟退火法的改进人工势场法。该方法基于目标位置对模拟退火法的邻域函数进行优化，解决了无人机陷入局部最优状态后导致路径规划任务失败的问题，提高了路径规划的整体运行效率与成功率。但该方法仅针对传统人工势场法存在的局部极小值这一缺陷改进，障碍物与无人艇容易相撞、无人艇无法达到目标点的问题并未解决。因此现有技术中基于改进人工势场法的无人艇避障方法还存在无法同时解决传统人工势场法存在当机器人所受引力远大于斥力时机器人易与障碍物相撞、当机器人所受斥力远大于引力时机器人无法到达目标点、机器人所受引力和斥力的合力为零时机器人容易陷入局部极小值而停止的问题。

技术实现思路

1、本发明目的是为了解决现有无人艇避障方法还存在无法同时解决无人艇易与障碍物相撞、目标不可达、局部最小值的问题，而提出了一种基于人工势场的双艇协同系统避障方法。

2、一种基于人工势场的双艇协同系统避障方法具体过程为：

3、步骤一、将双无人艇队形的中心位置设置为虚拟领航者，并获取虚拟领航者信息；

4、所述虚拟领航者信息包括：虚拟领航者的坐标v(xv,yv)、航向ψv和速度vv；

5、步骤二、获取虚拟领航者在当前位置所受合力f，使虚拟领航者沿着合力f方向航行；

6、步骤三、判断虚拟领航者在当前位置是否陷入缺陷，若虚拟领航者陷入缺陷，则利用虚拟领航者信息和步骤二获得的虚拟领航者所受合力，对虚拟领航者的航向进行调整，然后执行步骤四；若虚拟领航者在当前位置没有陷入缺陷则虚拟领航者沿着合力f方向航行，然后执行步骤四；

7、步骤四、虚拟领航者前进至下一位置，并更新虚拟领航者当前位置，根据更新后的虚拟领航者当前位置判断虚拟领航者是否到达目标点，若到达目标点则结束，若没有到达目标点则返回步骤二。

8、进一步地，所述步骤一中的获取虚拟领航者信息，如下式：

9、

10、其中，(x1,y1)、(x2,y2)分别是两艘水面无人艇的坐标，ψ1、ψ2分别是两艘水面无人艇的航向，v1、v2分别是两艘水面无人艇的速度，(xv,yv)是虚拟领航者v的坐标。

11、进一步地，所述步骤二中的虚拟领航者在当前位置所受合力f，如下式：

12、f＝fatt+frep

13、fatt＝-katt(v-t)

14、

15、其中，fatt是目标点对虚拟领航者的引力，frep是障碍物对虚拟领航者的斥力，katt是引力增益常数，t是目标点的位置，krep是斥力增益常数，ρobs是虚拟领航者与障碍物之间的距离，ρ0是障碍物的影响半径。

16、进一步地，所述步骤三中判断虚拟领航者在当前位置是否陷入缺陷，具体为：

17、当ρot＞ρ0且且l＜robs+rs+γ，则表示虚拟领航者在当前位置陷入缺陷1；

18、其中，μ是vr与f方向的夹角，vr是虚拟领航者和障碍物连线，l是障碍物到f所在直线的垂直距离，robs是障碍物的实际半径，γ是针对缺陷1的安全补偿距离，rs是针对双无人艇队形的安全补偿半径，ρot为目标点与障碍物之间的距离；

19、当ρot＜ρ0，则表示虚拟领航者在当前位置陷入缺陷2；

20、若f＝0，则表示虚拟领航者在当前位置陷入缺陷3。

21、进一步地，所述步骤三中若虚拟领航者陷入缺陷，则利用虚拟领航者信息和步骤二获得的虚拟领航者所受合力，对虚拟领航者的航向进行调整，具体为：

22、若虚拟领航者陷入缺陷1，则令使虚拟领航者向远离障碍物的方向航行；

23、其中，是虚拟领航者航行方向与大地坐标系x方向的夹角，β是vr与大地坐标系x方向的夹角，γ是vr与vs构成的夹角，vs是虚拟领航者与s点的连线，s是沿着障碍物到f所在直线的垂直方向对障碍物等效半径ro延长γ获得的点；

24、所述障碍物等效半径ro，如下式：

25、ro＝robs+rs

26、若虚拟领航者陷入缺陷2，则当d≥robs+rs+ω或时，令当d＜robs+rs+ω且时，令

27、

28、其中，η是vt与大地坐标系x方向的夹角，(xt,yt)是目标点t的位置坐标，d是障碍物到vt的垂直距离，vt是虚拟领航者与目标点的连线，ω是针对缺陷2的安全补偿距离，λ是vr与vt构成的夹角，β是vr与大地坐标系x方向的夹角，γ1是vr与vq构成的夹角，vq是虚拟领航者与q的连线，q是沿着障碍物到vt的方向对障碍物等效半径ro延长ω获得的点；

29、若虚拟领航者陷入缺陷3，则获取虚拟领航者的下一位置，使虚拟领航者在下一位置时所受合力f不为0。

30、进一步地，障碍物到f所在直线的垂直距离通过以下公式获得：

31、l＝ρobs cosε

32、

33、

34、其中，ε为vr与rs构成的夹角，rs是为障碍物与s点的连线，θ是f方向与大地坐标系x方向的夹角，(xobs,yobs)是障碍物r的位置坐标，(xo,yo)是大地坐标系原点。

35、进一步地，vr与vt构成的夹角通过以下公式获得：

36、

37、其中，ρvt是虚拟领航者到目标点的距离。

38、进一步地，vr与vs构成的夹角，通过以下公式获得：

39、

40、其中，γ是vr与vs构成的夹角。

41、进一步地，vr与vq构成的夹角，如下式：

42、

43、

44、其中，σ是vr与rq构成的夹角，rq是障碍物与q之间的连线。

45、进一步地，所述若虚拟领航者陷入缺陷3，则获取虚拟领航者的下一位置，使虚拟领航者在下一位置时所受合力f不为0，具体为：

46、s1、对初始温度t0、终止温度tf、随机概率阈值ξ、双艇协同系统最大航向变化角度初始化；

47、

48、其中，δt为时间步长，rs为当前时刻双艇协同系统中位于外侧的无人艇的回转半径；

49、s2、选择一个随机点x1；

50、s3、获取位置v的势场u(v)和位置x1的势场u(x1)差，若u(x1)-u(v)＜0，则执行s4，若u(x1)-u(v)≥0，则根据metropolis准则利用t0和tf计算接受x1为虚拟领航者下一位置的概率p，若p＞ξ，则执行s4；若p≤ξ，则返回s2重新选取随机点；

51、位置v的势场u(v)和位置x1的势场u(x1)，通过以下公式获取：

52、u＝uatt+urep

53、

54、

55、其中，u为合势场，uatt为引力势场，urep为斥力势场；

56、s4、获取虚拟领航者当前位置航向与下一位置航向差将与进行比较，若则接受x1作为虚拟领航者下一位置；若返回s2重新选择随机点；

57、s5、虚拟领航者到达位置x1后，判断当前虚拟领航者所在位置所受引力和斥力的合力是否为0，若合力不为0表示摆脱了局部极小值点，则继续追踪目标点；若当前虚拟领航者所在位置所受引力和斥力的合力仍为0，则返回s2重新选择随机点。

58、本发明的有益效果为：

59、针对现有基于传统人工势场法的无人艇路径规划问题，人工势场法存在的可能与障碍物相撞、目标不可达、局部最小值问题，本发明引入了安全补偿距离，并对模拟退火法进行了改进，同时本发明结合了双艇溢油回收系统的最大航向变化角，对改进后的模拟退火算法选择的虚拟领航者下一位置进行了筛选，保证了无人艇路径不会出现较大的振荡，本发明首先判断了是否有陷入三种缺陷的风险，然后提出了针对三种缺陷的解决方案，本发明提出的apf-1、apf-2、apf-3能够避免由于无人机所受引力远大于斥力导致的无人艇易与障碍物相撞、无人艇所受斥力远大于引力导致的无人艇无法到达目标点、无人艇所受引力和斥力的合力为零导致的机器人容易陷入局部极小值而停止的问题，本发明能够安全的避开障碍，最终保证无人艇安全地追踪到目标溢油。