0 引言
水面无人艇在航行中会遇到未知的障碍物,例如电子海图未及时更新的静态障碍物,还有大型鱼类、移动船舶等动态障碍物。在复杂障碍物环境和动态环境下水面无人艇路径规划的主要目标不是路径长度和能量消耗,而是无人艇应对危险规避障碍物的能力。传感器探测的范围作为局部路径规划的环境,此环境通常是未知且动态变化的,故局部路径规划需要快速且高效的环境建模,对路径搜索和避障的动态性能要求更高。无人艇动态避障算法通常有快速扩展随机树算法[1](rapidly-exploring random tree,RRT)、速度障碍法[2](reciprocal velocity obstacle,RVO)、粒子群算法(PSO)[3]、动态窗口法[4-5](dynamic window approach,DWA)、人工势场法[6](artificial potential field,APF)等。
人工势场法(APF)根据无人艇当前位置、障碍物和目标点的相对态势引导水面无人艇安全航行,因其在未知环境和动态环境下的良好适应性而得到广泛应用。然而,人工势场法存在一定的局限性,例如规划的路径不平滑,出现目标不可达等问题。因此,国内外学者提出大量的改进办法。易先军等[7]对斥力势场划分成不同的梯度等级,同时采用逃逸力,解决了局部最小的问题;陈劲峰等[8]对机器人受到的无效的斥力进行忽略,并采用逃离的方法解决局部最小值问题;徐小强等[9]为了解决路径过长的问题,采用安全距离的概念,并且在航行中进行距离预测解决局部最小值的问题;程志等[10]对斥力进行改进,并在陷入局部最小值后通过设置虚拟目标点引导机器人逃离该区域;罗强等[11]在机器人行驶中仅考虑一定范围内受到的斥力,其余斥力不考虑,并对不同类型障碍物形成的局部最小问题采用不同的解决策略。
本文中采用改进的人工势场法对水面无人艇的局部路径进行规划。针对人工势场法出现的局部最小值和路径不平滑的问题,对其斥力系数进行改进,并引入逃逸力。为了防止由于船舶随意航行造成海上事故,在避碰过程中需遵守相关的海上船舶航行的规范。因此,在动态避障中结合《避碰规则》引入转向力。通过仿真实验,得到的路径更加平滑且解决了局部最小值问题,在与动态障碍物会遇时,均能在符合避碰规则的前提下安全避碰。
1 局部路径规划问题分析
1.1 动态避碰问题分析
水面无人艇在未知环境中航行时,其只知道当前位置和终点坐标,依据自身传感器探测环境信息,获得障碍物的大小、形状、方位以及动态障碍物的移动速度。根据障碍物信息进行局部范围的调整,避开障碍物后回到原有航线继续航行,局部避碰流程如图1所示。
图1 局部避碰流程
Fig.1 Local collision avoidance process
1.2 基于USV特征的避碰规则
无人艇作为海上无人驾驶平台,避碰决策时为了保证本艇与障碍船的安全,在航行中也需遵守国际海上避碰规则(international regulations for preventing collisions at sea,COLREGS)[12],按照规定的要求进行避让,防止在海面上任意航行导致与会遇船的行动不协调,从而对它船及人员造成伤害。
COLREGs对机动船只在海洋航行过程中发生碰撞的情形做出了相应的规定。由于海上避碰规则是为有人船的船员航行中提供操纵船舶进行避碰的参考策略,并不能完全适用于USV避碰。USV船体较小、雷达反射面积小、探测信号易被海波干扰,会使得其他船舶在探测USV位置等运动信息时存在困难。但水面无人艇机动性强,因此,在实际会遇局面中,无人艇作为主动避让方执行规避动作将更加安全。由于USV快速灵活,方位角度划分过小易造成会遇局势频繁改变,不利于避碰策略的计算,所以结合USV的运动特点将会遇局势划分为四个部分,分别为追越、对遇、左交叉相遇、右交叉相遇,如图2所示。其中USV表示本艇,Obs表示障碍船。
图2 会遇态势划分
Fig.2 Division of encounter situation
在USV与障碍物发生以上4种会遇态势时,USV均为让路船,其他障碍物定义为直行船。综上所述,根据USV的特性和COLREGS的相关规则,USV在海洋中航行时对于移动障碍物的避碰规则被设计如下:
1) 追越:当无人艇与障碍船处于追越局面时,无人艇应从障碍船左舷方向通过,如图3(a)所示。
图3 USV在不同情况下的海事规则
Fig.3 USV maritime rules in different situations
2) 对遇:当无人艇与障碍船对遇,两者的航向相反或近似相反并存在发生碰撞的危险时,无人艇应从障碍船左舷,即己方右舷方向通过,如图3(b)所示。
3) 交叉相遇:当无人艇与障碍船处于交叉局面时,无人艇应从障碍船船尾方向通过,避免从他船的前方通过,如图3(c)、图3(d)所示。
2 人工势场法
2.1 原理
人工势场法最早由Khatib[13]提出,其原理是将无人艇与障碍物和目标点之间的相互影响视为一个虚拟力场,无人艇的出发点在高势场处,终点在低势场处,在航行中受到障碍物的斥力作用和目标点对其吸引力作用,最终在合力的作用下避开障碍物安全航行至目标点。
引力势场函数随着无人艇与目标点的距离增加而增加,公式为
(1)
式中:Uatt为引力势场;Xp为当前位置;Xg为目标点位置; ρ(Xp,Xg)为水面无人艇离目标点距离,katt>0为引力势场增益因子,由式(1)得出,引力势场由引力势场因子和距离ρ(Xp,Xg)共同决定,其大小与ρ(Xp,Xg)的平方成正比,在目标点处Uatt最小。由于无人艇从势能高的位置移向势能低的位置,因此,其在引力场的作用下向目标点移动。
Fatt为Uatt的负梯度,也是Uatt下降最快的方向,公式为:
kattρ(Xp,Xg)Frep(Oi)
(2)
Fatt(X)的大小与距离ρ(Xp,Xg)成正比,距离越大,引力越大,到达目的点时引力为0,方向为从无人艇当前位置指向目标点。引力对无人艇的作用是全局的,只要无人艇存在于人工势场范围内,就受到目标点引力的影响,因此,引力场函数是连续变化的。
每个障碍物的斥力影响是有范围的,无人艇进入障碍物的斥力影响区域内,才受到斥力作用,斥力的大小与二者相对距离成反比,当无人艇与障碍物无限靠近时,受到的斥力接近无穷大,无人艇所受到的斥力是分段的非连续。
斥力势场函数公式为
(3)
式中:ρ(Xp,X0)为水面无人艇到障碍物的距离; ρ0为障碍物影响范围的半径; krep>0为斥力势场的增益因子。
障碍物对无人艇的斥力为斥力势场函数的负梯度,斥力函数公式为:
Frep=-▽Urep=
(4)
Frep的作用下使得无人艇远离障碍物。Frep与ρ(Xp,X0)近似成反比。
无人艇航行中会遇到多个障碍物,其在某位置最终受到的合力为受到的所有斥力与目标点的引力之和,合力势场函数为
(5)
式中:Uall为无人艇受到的总势场;N为障碍物的个数;Urep(Oi)为第i个障碍物产生的斥力势场。所受合力为
(6)
式中:Fall为合力; Frep(Oi)为第i个障碍物的斥力。
2.2 存在问题
人工势场法通过势场函数反映了无人艇、障碍物、目标点三者之间的实时状态信息,通过由障碍物的斥力与目标点的引力组成的合力引导无人艇行驶。该算法实现简单,实效性高,适用于局部路径规划,但也存在着许多问题。
1) 目标不可达
无人艇在行驶中受到引力场和斥力场的共同作用,在理想情况下,会在合力的作用下远离障碍物最终到达目标点。由于斥力随着无人艇与障碍物的距离的减小而增大,而引力随着无人艇与目标点的距离减小而减小,则在航行过程遇到障碍物时,会存在斥力大于引力的区域。当目标点位于这些区域时,无人艇接近目标点,引力在不断减小,同时离障碍物的距离也在减小,对其产生的斥力不断增加,其所受斥力大于引力,无人艇将无法到目标点,导致局部路径规划失败,如图4所示。
图4 目标点不可达示意图
Fig.4 Schematic diagram of target point unreachable
2) 局部极小值
当某点的引力势场等于斥力势场时,无人艇受到的合力为0,该点称为局部极小值点。陷入局部极小值点的无人艇会停止运动或者在局部极小值点附近来回振动无法脱离,无人艇的航向、航速就会快速变化,处于振荡状态,在实际航行中,会导致无人艇频繁操舵,陷入局部最小值。此情况下无人艇、障碍物与目标点相对位置如图5所示。
图5 局部极小值状态示意图
Fig.5 Schematic diagram of the local minimum state
3 人工势场法的改进
3.1 斥力系数的改进
传统APF方法中,krep>0为斥力势场的增益系数,其固定不变。由于离障碍物越近其危险度越高,将krep设为随距离动态调整,刚进入障碍物作用区域内,瞬间触发排斥力,随着离障碍物的距离接近,krep的值不断增大,其设置如下
式中:ρ0为障碍物作用距离;d为无人艇距离障碍物距离;k0为常量;amax和amin为斥力系数条件参数;dgoal为无人艇。
3.2 逃逸力的提出
由3.2节可知,当斥力与引力大小相等,方向相反时,无人艇所受合力为0,出现局部最优的情况。针对此问题,提出逃逸力表达式为
(8)
式中:dgoal为无人艇与目标点距离;dob-g为障碍物与目标点的距离,障碍物对无人艇的影响不仅与障碍物和无人艇的距离有关,还与无人艇与目标点两者间的相对角度有关。ω为无人艇与障碍物之间夹角的系数,其公式如下
(9)
式中:ω0为常数; θ为无人艇行驶方向与障碍物的夹角,当其小于
时,ω随着夹角的减小而增大。图6为逃逸力的设计框图。
图6 逃逸力的设计
Fig.6 The design of Escape power
3.3 转向力的提出
为了使无人艇在遇到动态船舶时,按照《避碰规则》进行避碰,根据会遇时无人艇与障碍船速度的夹角情况,引入转向力,使得无人艇向正确的方向进行避让。图7为USV与障碍物的方位图。
图7 USV与障碍物的方位图
Fig.7 Azimuth map of USV and obstacles
转向力的定义如下:
(10)
式中:k为常数;α为会遇船与无人艇航向之间的夹角;φ为无人艇当前航向; β为会遇船航行方向与无人艇航向的夹角,当β∈(-67.5°,67.5°)为追越状态,无人艇需向左转向,当β∈(67.5°,165°)时,为右交叉相遇,无人艇需向右转向,当β∈(-165°,-67.5°)为左交叉相遇,无人艇需向左转向,当为对遇状态时无人艇需向右转向,此时β∈(-180°,-165°)∪(165°,180°)。
4 仿真实验
为验证改进APF算法的有效性,分别对静态未知障碍物以及动态未知障碍物进行仿真实验。
4.1 静态未知障碍物实验
水面无人艇在航行中,除了已知障碍物外,还会遇沉船、礁石、抛锚船舶等静态障碍物。由于这些障碍物提前并不知道,在航行中传感器获得到的障碍物信息通常是不规则的形状,常见的方式是将障碍物用圆形表示,通过雷达扫描获得其地理位置,并以此为圆心,设定一定距离的安全半径。
1) 局部极小点仿真实验
将无人艇起始点坐标设为(5,5),目标点坐标为(48,48),障碍物1半径r1为3 m,坐标为(20,20),障碍物2半径r2为2 m,坐标为(35,40),斥力影响范围ρ0=3r。此时障碍物正好出现在无人艇与目标点连线上,引力和斥力的方向相反,采用传统的人工势场法函数会出现无人艇停止在障碍物前方,如图8(a)所示。
图8 局部最小问题解决示意图
Fig.8 Schematic diagram of solving the local minimum problem
针对局部最小的问题,对APF算法的斥力系数进行调整,使得离障碍物越近斥力系数越大,并且在无人艇行驶方向接近障碍物时,引入逃逸力,与无人艇所受的斥力方向垂直,在其作用下无人艇远离障碍物,解决局部最小的问题,如图8(b)所示。
2) 静态多障碍物实验
在60 m×60 m的区域内,设定无人艇的起始点为(5,5),速度为3 m/s,目标点为(48,48),障碍物个数为 5个,障碍物坐标分别为{(20,19.8);(35,40);(32,40);(30,40);(42,35)},障碍物半径分别为{3;2;1;1;2 }。
传统APF算法与改进APF算法、文献[7]中算法规划的路径对比如图9所示。传统APF算法距离障碍物最近距离0.77 m,总路径长度65.0 m,文献[7]中算法距离障碍物最近距离为1.58 m。总路径长度64.5 m,改进APF算法距离障碍物最近距离2.05 m,总路径长度64.0 m,改进后的算法路径缩短,距离障碍物更远,安全性得到提高,路径更加平滑,更符合无人艇的动力性能。
图9 静态多个未知障碍物路径对比
Fig.9 Comparison of path planning for multiple obstacles
4.2 动态未知障碍物实验
在同一环境下设置了具有不同运动态势的多个障碍物,以此来验证算法在多障碍局面下的避碰效果。
设定无人艇起始位置为(5,5),目标点为(78,48),无人艇航速为 3 m/s,障碍物数量为 4,障碍物半径r为5,障碍物斥力的影响范围ρ0=3r,USV的禁航区用一个红色圆圈表示,4个障碍物起始坐标分别为{(15,9);(70,20);(80,44);(85,87)},障碍物速度大小和速度方向为{(1,20°);(3,160°);(2,200°);(2.8,250°)}。4个动态障碍的参数设置如表1所示。
表1 障碍物运动参数
Table 1 Motion parameters of obstacles
障碍物编号起点/m航速/(m·s-1)航向/(°)会遇态势障碍物A(15,9)120追越障碍物B(70,20)3160右交叉相遇障碍物C(80,44)2200对遇障碍物D(85,87)2.8250左交叉相遇
无人艇与多个障碍物的避碰过程如图10所示。图10(a)中无人艇在航行中首先与障碍物1呈现追越态势,根据COLREGs规则通过向左转向超越障碍物A,图10(b)中与障碍物2呈现右交叉相遇,根据COLREGs规则无人艇向右进行转向,图10(c)中与障碍物3呈现对遇态势,根据COLREGs规则无人艇向右转向,图10(d)中与障碍物4呈现左交叉相遇局面,根据COLREGs规则向左转向,随后安全驶向目标点。
图10 多船会遇场景仿真
Fig.10 Simulation of multi-ship encounter scenarios
仿真结果显示,改进APF算法在复杂环境下,能引导无人艇对多个动态障碍进行安全规避。避碰轨迹具有较好的平滑性和连续性,满足无人艇的运动能力约束,同时能很好地兼顾COLREGs的避碰要求。
5 结论
针对人工势场法的原理与缺点进行介绍,结合《避碰规则》设计了无人艇动态避碰策略,并进行了仿真实验。通过对静态未知障碍物避碰实验结果进行分析,局部最优问题得到解决且获得更加安全平滑的避碰路径。通过对移动未知障碍物会遇情形的仿真实验,验证了USV能够在遵守《避碰规则》的前提下进行安全避碰。
[1] MART
NEZ-ROZAS S,ALEJO D,CABALLERO F,et al.Path and trajectory planning of a tethered UAV-UGV marsupial robotics system[J].2022.DOI:10.48550/arXiv.2204.01828.
[2] 王伟,薛华,周睿,等.基于RVO算法的复杂场景人群疏散仿真算法[J].物理学杂志:系列会议,2021.
WANG Wei,XUE Hua,ZHOU Rui,et al.Simulation algorithm for crowd evacuation in complex scene based on RVO algorithm[J].Journal of Physics:Conference Series,2021.
[3] ABAAS T F,SHABEEB A H.Autonomous mobile robot navigation based on PSO algorithm with inertia weight variants for optimal path planning[J].IOP Conference Series:Materials Science and Engineering,2020,928:022005-.
[4] KASHYAP A K,PARHI D R,MUNI M K,et al.A hybrid technique for path planning of humanoid robot NAO in static and dynamic terrains[J].Applied Soft Computing,2020,96.
[5] 王斌,李雪,袁S.一种基于滑移轮式车辆运动学分析的改进局部路径规划算法[C]//ISCSIC 2020:2020第四届计算机科学与智能控制国际研讨会.出版地不详:[出版社不详],2020.
WANG Bin,LI Xue,YUAN S.An improved local path planning algorithm based on kinematics analysis of the skid-steered wheeled vehicle[C]//ISCSIC 2020:2020 4th International Symposium on Computer Science and Intelligent Control.[S.l.]:[s.n.],2020.
[6] 齐北康,李敏,杨宇,等.基于改进人工势场法的无人机路径规划避障算法研究[J].物理学杂志:系列会议,2021,1948(1):012060.
QI Beikang,LI Min,YANG Yu,et al.Research on UAV path planning obstacle avoidance algorithm based on improved artificial potential field method[J].Journal of Physics:Conference Series,2021,1948(1):012060.
[7] 易先军,耿翰夫,付龙,等.模糊改进人工势场法移动机器人路径规划[J].组合机床与自动化加工技术,2021(5):65-68.
YI Xianjun,GENG Hanfu,FU Long,et al.Fuzzy improved artificial potential field method for mobile robot path planning[J].Combined Machine Tool and Automated Machining Technology,2021(5):65-68.
[8] 陈劲峰,黄卫华,章政,等.动态环境下基于改进人工势场法的路径规划算法[J].组合机床与自动化加工技术,2020(12):6-9,14.
CHEN Jinfeng,HUANG Weihua,ZHANG Zheng,et al.Path planning algorithm based on improved artificial potential field method in dynamic environment[J].Combined Machine Tool and Automated Machining Technology,2020(12):6-9,14.
[9] 徐小强,王明勇,冒燕.基于改进人工势场法的移动机器人路径规划[J].计算机应用,2020,40(12):3508-3512.
XU Xiaoqiang,WANG Mingyong,MAO Yan.Path planning of mobile robots based on improved artificial potential field method[J].Computer Applications,2020,40(12):3508-3512.
[10] 程志,张志安,李金芝,等.改进人工势场法的移动机器人路径规划[J].计算机工程与应用,2019,55(23):29-34.
CHENG Zhi,ZHANG Zhian,LI Jinzhi,et al.Path planning of mobile robots based on improved artificial potential field method[J].Computer Engineering and Applications,2019,55(23):29-34.
[11] 罗强,王海宝,崔小劲,等.改进人工势场法自主移动机器人路径规划[J].控制工程,2019,26(6):1091-1098.
LUO Qiang,WANG Haibao,CUI Xiaojin,et al.Improved artificial potential field method for autonomous mobile robot path planning[J].Control Engineering,2019,26(6):1091-1098.
[12] 吴汉华,项华峰,端木玉.无人船艇适用《国际海上避碰规则》若干问题的研究[J].产业与科技论坛,2021,20(19):27-29.
WU Hanhua,XIANG Huafeng,DUAN Muyu.Research on several issues concerning the application of “International Regulations for Preventing Collisions at Sea” to unmanned vessels[J].Industry and Technology Forum,2021,20(19):27-29.
[13] KATHIB O.Real-time obstacle avoidance for manipulators and mobile robots[J].The International Journal of Robotics Research,1986,5(1):90-98.