一种复杂环境下基于改进人工势场法的船舶编队避障方法

本发明涉及船舶编队避障方法，尤其涉及一种复杂环境下基于改进人工势场法的船舶编队避障方法，属于船舶编队控制及路径规划。

背景技术：

1、随着船舶技术的发展和日益成熟，国内外的研究趋势也从单一船体向多船集群协同控制方向发展，作为单无人船的发展和延伸，使用多艘船舶组成编队共同完成任务，能够在发挥个体作用的同时协同合作、提高工作效率、适应复杂环境，因此编队控制是当前研究的热点和难点。船舶编队避障主要包含三部分内容：(1)编队队形的形成和保持：多艘船舶从初始位置开始运动，跟随船舶和领航船舶之间保持期望距离和角度，进而形成并保持船舶编队队形；(2)编队避障：在船舶编队航行过程中，航行水域出现障碍物阻碍船舶正常行驶，船舶编队需改变原队形躲避障碍物；(3)队形恢复：在编队避障任务结束后，编队的队形发生改变，各船舶需恢复并保持原始队形继续航行。

2、船舶编队控制结构包括领导跟随结构、虚拟结构、基于行为结构和图论结构等，其中领导跟随结构设计简单、易实现，是最常用的一种船舶编队控制结构。船舶编队执行任务的环境通常是较为复杂的受限水域，船舶编队在航道内航行时需要考虑航道边界对船舶的限制，但现有技术中传统的人工势场法在执行避障任务时，存在目标不可达、避障动作过大导致船舶转角变化剧烈的问题。

3、综上所述，需要一种保证船舶编队航行至目标点且不发生碰壁和碰撞的同时达到船舶航向角变化较小的船舶编队避障方法。

技术实现思路

1、在下文中给出了关于本发明的简要概述，以便提供关于本发明的某些方面的基本理解。应当理解，这个概述并不是关于本发明的穷举性概述。它并不是意图确定本发明的关键或重要部分，也不是意图限定本发明的范围。其目的仅仅是以简化的形式给出某些概念，以此作为稍后论述的更详细描述的前序。

2、鉴于此，为解决现有技术中船舶避障采用传统的人工势场法存在的目标不可达和转角变化剧烈问题，同时考虑航道边界对船舶航行路线的限制，本发明提供一种复杂环境下基于改进人工势场法的船舶编队避障方法。

3、技术方案如下：一种复杂环境下基于改进人工势场法的船舶编队避障方法，包括以下步骤：

4、s1.设计各跟随船舶与领航船舶之间的期望距离和期望角度，结合领航船舶的位置信息确定跟随船舶的位置，进而确定船舶编队的队形；

5、s2.根据船舶位置所在区域，采用指数函数和正弦函数分别构建边界斥力势场，整合为航道边界势场，根据航道边界势场计算出船舶所受的航道边界斥力；

6、s3.设计引力势场并计算出船舶所受的引力，构建改进后的斥力势场并计算出船舶所受的障碍物斥力，整合得到船舶所受合力；

7、s4.根据船舶最大转艏角度设定船舶最大转角约束，结合船舶所受合力，计算出船舶当前转艏角和下一时刻船舶位置；

8、s5.根据船舶当前转艏角和下一时刻船舶位置，结合船舶到达目标点判定条件判断出船舶是否到达目标点，到达目标点则任务结束，否则返回步骤s4直至船舶到达目标点。

9、进一步地，所述s1中，跟随船舶的位置表示为：

10、

11、其中，xi为第i个跟随船舶的横向位置坐标，yi为第i个跟随船舶的纵向位置坐标，xl为领航船舶的横向位置坐标，yl为领航船舶的纵向位置坐标，ld为跟随船舶和领航船舶之间的期望距离，ψd为跟随船舶和领航船舶之间的期望角度。

12、进一步地，所述s2中，航道边界斥力势场uedge表示为：

13、

14、其中，γ1和γ2为道路边界斥力增益系数，l为航道的宽度，yl为左航道中线的位置坐标，yr为右航道中线的位置坐标，y为各船舶的纵向位置坐标；

15、航道边界斥力fedge表示为：

16、

17、其中，v为船舶当前的航速，航道边界斥力fedge方向由航道边界指向船舶当前时刻位置。

18、进一步地，所述s3中，根据当前时刻船舶位置与目标点位置之间的相对距离，设计引力势场uatt，根据当前时刻船舶位置与障碍物位置之间的相对距离，构建改进后的斥力势场urep；

19、引力势场uatt表示为：

20、

21、ρrg＝||q-qg||

22、q＝(x,y)

23、qg＝(xg,yg)

24、其中，katt为引力增益系数，ρrg为当前时刻船舶位置q与目标点位置qg之间的相对距离，(x,y)为当前时刻船舶位置q的坐标，(xg,yg)为目标点位置qg的坐标；

25、引力fatt为引力势场uatt的负梯度；

26、引力fatt表示为：

27、

28、其中，引力fatt的方向由船舶位置q指向目标点位置qg；

29、改进后的斥力势场urep表示为：

30、

31、其中，krep为斥力增益系数，ρro为当前时刻船舶位置q与障碍物位置qo之间的相对距离，(xo,yo)为障碍物位置qo的坐标，ρ0为常量，其表示障碍物斥力作用的最大范围，为船舶与目标点的距离，n为常数；

32、障碍物斥力frep为改进后的斥力势场urep的负梯度；

33、障碍物斥力frep表示为：

34、

35、

36、其中，第一斥力frep1的方向由障碍物位置qo指向船舶位置q，第二斥力frep2的方向由船舶位置q指向目标点位置qg；

37、船舶所受合力fsum表示为：

38、fsum＝fatt+frep1+frep2+fedge。

39、进一步地，所述s4中，船舶最大转艏角度ψmax表示为：

40、ψmax＝rmaxδt

41、其中，rmax为船舶转艏角速度的最大值，δt为采样间隔；

42、船舶最大转角约束表示为：

43、ψ＜ψmax

44、其中，ψ为船舶当前转艏角度；

45、将船舶所受合力fsum分解为船舶横向方向所受合力fsumx和船舶纵向方向所受合力fsumy，根据船舶最大转角约束和船舶所受合力，计算出船舶当前转艏角度和船舶下一时刻位置；

46、船舶当前转艏角度ψ表示为：

47、ψ＝sign(fsumy)arccos(fsumx/fsum)

48、船舶下一时刻位置qn表示为：

49、qn＝(xnext,ynext)

50、

51、其中，(xnext,ynext)为船舶下一时刻位置qn的坐标，l表示仿真步长。

52、进一步地，所述s5中，船舶到达目标点判定条件表示为：计算出船舶当前时刻位置q与目标点位置qg之间的相对距离ρrg，当相对距离ρrg小于设定值时，则判定船舶到达目标点。

53、本发明的有益效果如下：本发明在船舶靠近航道两侧的区域采用变化趋势较快的指数函数构建边界斥力势场，在航道中间区域采用变化较为平缓的正弦函数构建边界斥力势场，通过采用变化趋势快慢程度不同的函数类型构建航道边界斥力势场，考虑了航道边界产生的斥力影响的情况，提高了船舶避障的安全性；本发明通过在传统人工势场法的斥力势场中加入调节因子即船舶与目标点的距离，解决了船舶避障后目标不可达的问题；本发明通过引入船舶最大转角约束使船舶当前转艏角度始终控制在船舶最大转艏角度内，解决了船舶避障过程中转角变化剧烈的问题。