Loading [MathJax]/jax/output/HTML-CSS/jax.js
  舰船科学技术  2025, Vol. 47 Issue (5): 126-131    DOI: 10.3404/j.issn.1672-7649.2025.05.019   PDF    
引用本文
顾潮宏, 冯友兵, 仲伟波, 蔡立涛. 基于优化人工势场法的无人水面艇路径规划. 舰船科学技术, 2025, 47(5): 126-131  
GU Chaohong, FENG Youbing, ZHONG Weibo, CAI Litao. Path planning of unmanned surface vehicles based on optimized artificial potential field method. Ship Science and Technology, 2025, 47(5): 126-131  
基于优化人工势场法的无人水面艇路径规划
顾潮宏, 冯友兵, 仲伟波, 蔡立涛     
江苏科技大学 海洋学院，江苏 镇江 212003
收稿日期: 2024-03-30.
基金项目: 江苏省研究生实践创新计划（SJCX23_2140）
作者简介: 顾潮宏（1999 – ），男，硕士研究生，研究方向为船舶综合控制技术
摘要: 人工势场法因其平滑的路径被广泛应用于解决水面无人艇路径规划问题，但人工势场法与无人艇的结合会出现转向角过大、航迹不可控、局部最小值问题。本文研究一种基于视线法的人工势力场法，利用视线法确定出各期望航迹点的位置，通过目标点引力与障碍物斥力实现避障；修正新定义的引力点函数和障碍物斥力函数，使其更加适用于期望航迹点的避障。仿真结果表明，改进后的算法在多障碍物与大型障碍物环境中的航迹偏差分别降低了85.61%、13.2%，艏向角变化分别降低了59.52%、17.3%，且能够引导无人艇摆脱局部最小值环境。优化后的人工势场法有着更强的路径保持能力，更小的避障角度需求，对无人艇的操控性能要求也更低，大大提高了无人艇的行驶安全性，具有一定的实际应用价值。
关键词: 人工势场法     避障     无人水面艇    
Path planning of unmanned surface vehicles based on optimized artificial potential field method
GU Chaohong, FENG Youbing, ZHONG Weibo, CAI Litao     
Ocean College, Jiangsu University of Science and Technology, Zhenjiang 212003, China
Abstract: The artificial potential field method is widely used to solve the path planning problem of unmanned surface vehicle (USV) because of its smooth path. However, the combination of artificial potential field method and USV will cause problems such as excessive steering angle, uncontrollable track and local minimum. In this paper, an artificial force field method based on line of sight method is studied. The position of each desired track point is determined by line of sight method, and obstacle avoidance is realized by the attraction of target point and the repulsion of obstacles. The newly defined attraction point function and obstacle repulsion function are modified to make them more suitable for the obstacle avoidance of the desired track point. The simulation results show that the track deviation of the improved algorithm is reduced by 85.61% and 13.2%, and the change of the heading angle is reduced by 59.52% and 17.3% respectively in the environment of multiple obstacles and large obstacles, and the improved algorithm can guide the USV to get rid of the local minimum environment. The optimized artificial potential field method has stronger path maintaining ability, smaller obstacle avoidance angle demand, and lower control performance requirements for USV, which greatly improves the driving safety of USV and has certain practical application value.
Key words: artificial potential field     obstacle avoidance     unmanned surface vehicle    
0 引　言

水面无人艇（Surface Unmanned Vehicle，USV）配备先进的环境感知设备，其体型相较与传统船舶更加小巧，应用更加广泛。USV能够代替人类完成水面危险任务，提高水面作业的安全性[1]。为使USV在行驶过程中能无碰撞到达目的地，路径规划算法已经被部署到USV上。路径规划包括全局路径规划与局部路径规划，全局路径规划负责从起点到终点的整体路径规划，而局部路径规划负责USV当前位置附近的最佳行驶路径以避开障碍物[2]。不同的路径规划算法应用在USV上会受到船艇本身的约束，如动力系统约束、航行规则约束、环境约束等[3]。

人工势场法（Artificial Potential Field，APF）以其平滑的避障路线被广泛应用于USV路径规划中。APF算法依靠虚拟的引力势场和斥力势场引导机器人在势场中运动[4]，传统APF算法存在局部最小值、目标点不可达、路径不可控等问题。刘涛等[5]针对APF算法在未知环境中规划效果差的问题，采用了长短记忆循环神经网络和强化学习算法结合对APF算法进行改进，结果表明改进后的算法对动态障碍物的规划具有更大优势。Fan等[6]通过构建虚拟六边形势场来摆脱局部最小值的困境，并验证了其可行性。为解决USV在传统PAF算法下的转向过大问题与局部最小值问题，Song等[7]通过设置转向角约束、速度限制与预测势场来生成适合USV的平滑路径。APF算法应用于USV上还要考虑USV实际航行的要求等限制。为解决USV在传统APF算法下的航迹偏移量过大的问题，Zhu等[8]通过引入动态移动目标点来引导避障后的USV返回路线，实验证明改进后的算法更加适合于工程应用中。为了克服USV在形式过程中受到风浪的影响，Chu等[9]通过设置虚拟足点作为引力点，USV在虚拟足点的引力作用下能够保持高精度的路径保持。

本文针对传统APF算法中的转向过大、航迹不可控、局部最小值3个问题，研究了将视线制导算法（Line of Sight，LOS）与APF相互结合的方法，实现航迹可控的避障路径。通过切线选取障碍物边界点，设定动态避障引力点来实现避障，避免USV陷入局部最小值环境。改进算法考虑到了USV自身的艏向约束与航向控制需求，在实际应用中具有参考价值。

1 避障方案分析与改进 1.1 航迹保持方案设计

为使USV的轨迹能够完全贴合期望航迹线，选择LOS算法对期望航迹进行跟随。LOS算法主要根据几何原理计算出LOS期望角，控制器调节USV当前的艏向角Ψ，使当前艏向角不断接近期望行驶角度[10]。设定USV当前的位置坐标为PUSV=[x,y]T，在二维水平运动视角下，USV的速度可以定义如下：

U(t)=√x2(t)+y2(t)。 (1)

USV随时间t改变的艏向角计算公式为：

Ψ=atan2(y(t)x(t))∈[−π,π]。 (2)

如图1所示，设起点Pk，终点Pk+1，e表示USV距离期望航迹线PkPk+1的距离，主要体现出USV对于期望航迹的跟随性能。αk表示大地坐标系正北方向与期望航迹线PkPk+1的夹角，公式定义为：

图 1 LOS期望角的判断 Fig. 1 LOS expectation angle judgment
αk=atan2(yk+1+ykxk+1−xk)∈[−π,π]。 (3)

式中：xk+1与yk+1为Pk+1的横纵坐标；xk与yk为Pk的横纵坐标。在固定路径参考系上USV坐标可以计算得到：

ε(t)=R(αk)T[P(t)−Pk]。 (4)

式中：R(αk)为旋转矩阵，表达式为

R(αk)=[cosαk−sinαksinαkcosαk]。 (5)

USV当前目标点Plos可以通过计算χd得到：

χd(e)=χp+χr(e)。 (6)

式中：χp即为初始航迹线的角度αk，χr(e)表达式如下：

χr(e)=arctan(−eΔ)。 (7)

式中：Δ为USV可视距离，利用式(6)可确定下个期望航迹点Plos的位置，控制艏向角度Ψ趋近于χd即可实现对航迹线的跟踪。当USV与期望航迹点Plos的距离小于设定值时，即完成了Plos的跟踪，USV开始更新下个期望航迹点Plos2的位置坐标，以此类推，USV逐个到达期望航迹点Plos(1⋯n)，最终到达目的地。

根据APF算法的目标点引力原理，对每个期望航迹点构建引力函数。当USV与Plos1的距离小于引力函数的交界距离时，USV不受到该目标点引力作用，由LOS计算出下个期望航迹点。如图2所示，当USV到达Plos3后艏向角Ψ=χd，当USV按照式(6)计算得到的艏向角度与航向角αk一致，表明已实现路径跟踪。

图 2 LOS轨迹图 Fig. 2 LOS trajectory diagram

传统LOS算法在到达指定路径后，调节艏向与航迹线角度一致即完成路径跟踪任务，但尚未考虑到USV在APF算法的引导下，航迹以外的障碍物仍然会对USV有斥力的作用。为解决这个问题，在此基础上沿着USV当前的艏向向量Ψ方向延长Δ作为下个期望航迹点Plos4，以此类推。对每个Plos进行判断，若期望航迹点Plos与终点Pk+1的距离小于Δ，则以Pk+1作为下个航迹点，直到到达目的地。

1.2 避障方案设计

APF通过假设目标点产生吸引力、障碍物产生排斥力来实现局部避障，其中的斥力公式与引力公式由Oussama在1986年提出[11]。1.1节将传统的单一目标点改进为LOS期望航迹点，但当航迹线上出现障碍物时，期望航迹点可能会处于障碍物内部或后部，由于障碍物的斥力作用，USV会出现避障失败或无法到达期望航迹点的情况。

如图3所示，当USV中心点与下个期望航迹点的连线上存在障碍物，则需要舍弃当前Plos并切换为障碍物避障引力点。以USV中心点作为原点，以USV当前的速度向量v作射线绕原点旋转一周，与障碍物边界相切可获得2个交点POL和POR，切线相对正北角度分别为ΨOL与ΨOR。点POL和POR在期望航迹线的投影点分别为dOL和dOR。若dOL⩽dOR，选取POL作为下个Plosn，若dOL>dOR，选取POR作为下个Plosn。

图 3 避障引力点示意图 Fig. 3 Schematic diagram of obstacle avoidance gravitational points

USV通过改进LOS算法获取到下个期望航迹点坐标，当障碍物出现在路径中时，USV不仅会受到传统APF算法下的障碍物斥力作用，还会受到障引力点的作用，向障碍物边界行驶。当USV与避障引力点的距离小于设定值时，计算下个期望航迹点的坐标位置，并再次判断是否需要切换为避障引力点。最后USV逐个受到避障引力点与期望航迹点的引力作用，避开障碍物到达目的地。

2 改进LOS-APF算法

为提高无人艇避障的路径保持能力，同时解决传统人工势场法中存在的局部最小值问题，提出基于LOS的APF算法（LOS-APF）。

2.1 航迹引力点函数设定

LOS-APF通过计算得到各期望航迹引力点，将目标航迹分为若干线段，无人艇依次受到各航迹引力点的引力作用，如图4所示。

图 4 航迹引力点示意图 Fig. 4 Schematic diagram of track gravitational points

当USV行驶到期望航迹点Plos1的引力函数分界点半径范围内，立刻切换计算下个Plos2，定义第n个期望航迹点的引力函数如下式：

Uatt(q)={0,‖q−qlosn‖<Rlosn，η⋅ln(‖q−qlosn‖Rlosn),‖q−qlosn‖⩾Rlosn。 (8)

式中：‖q−qlosn‖为第n个目标点与USV的欧氏距离，η为引力增益因数。在LOS引导下的USV进入到LOS航迹引力点的影响范围半径后，将受到下个航迹引力点的引力作用。考虑到无人艇控制过程中很难进行紧急的制动与转向，这里Rlosn大小选取符合公式：

Rlosn=3√L2+B2。 (9)

式中：L为USV的长；B为USV的宽，对期望航迹点的引力函数求负梯度可得引力公式如下：

Fatt(q)={0,‖q−qlosn‖<Rlosn，η1‖q−qlosn‖1Rlosn⋅∇‖q−qlosn‖,‖q−qlosn‖⩾Rlosn。 (10)
∇‖q−qlosn‖=(x−xlosn‖q−qlosn‖,y−ylosn‖q−qlosn‖)。 (11)

终点作为航迹线上最后一个期望航迹点，需要考虑到USV距离终点过远会产生极大的吸引力，定义终点引力函数如下式：

Uattg(q)={12η‖q−qg‖2,‖q−qg‖<Rlosn，Rlosnη‖q−qg‖−12ηRlosn2,‖q−qg‖⩾Rlosn。 (12)

式中：Rlosn为终点的引力势场函数的切换半径；η为引力增益因数；‖q−qg‖为终点与USV之间的欧氏距离，对式(12)求取负梯度可得终点的引力公式：

Fattg(q)={η‖q−qg‖,‖q−qg‖<Rlosn，Rlosnη∇‖q−qg‖,‖q−qg‖⩾Rlosn。 (13)
∇‖q−qg‖=(x−xg‖q−qg‖,y−yg‖q−qg‖)。 (14)

USV最终受到所有期望航迹点的引力到达终点，遵循航迹点引力函数选择如下式：

F′att(q)={Fatt(q),‖qlosn−qg‖>Rlosn+δg，Fattg(q),‖qlosn−qg‖⩽Rlosn+δg。 (15)

式中：δg为终点势场半径增益系数，若下个期望航迹点与终点距离满足‖qlosn−qg‖⩽Rlosn+δg，则切换终点作为下个期望航迹点。

2.2 避障引力点设定

避障引力点的设定不仅为USV提供一个避障方向，还能够在USV陷入局部最小值时帮助USV摆脱当前环境。避障引力点的引力函数定义如下式：

Uatto(q)={0,‖q−qlosn‖<Rlosn+δatto，A⋅e−‖q−qlosn‖22σ2,‖q−qlosn‖⩾Rlosn+δatto。 (16)

式中：A为避障引力点的引力系数；σ为高斯函数的标准差，越小则引力越强大，越大则引力场越平滑；‖q−qlosn‖为USV与Plos点的欧氏距离；δatto为避障引力点势场增益系数，对Uatto(q)求负梯度如下式：

Fatto(q)={0,‖q−qlosn‖<Rlosn+δatto，−Aδ2⋅e−‖q−qlosn‖22σ2⋅∇‖q−qlosn‖，‖q−qlosn‖⩾Rlosn+δatto。 (17)
∇‖q−qlosn‖=(x−xlosn‖q−qlosn‖,y−ylosn‖q−qlosn‖)。 (18)

由式(17)可看出，当USV进入到以Rlosn+δatto为半径的引力函数分界点后，USV不受到避障引力点的引力作用，并开始计算下个航迹点位置。为避免USV与障碍物碰撞，δatto的选取要大于USV的最小转向半径。

2.3 障碍物斥力函数设定

避障引力点的设定能够提供障碍物边界点的引力作用，由于切点过于贴近障碍物，除了避障引力点引力函数的Rlosn+δatto选取不可过小以外，障碍物的斥力函数在传统的引力场的基础上，添加USV艏向角与障碍物相对位置角度进行改进，如下式：

Urep(x,y)={12k(1‖q−qo‖−1ρo)2cos(Ψ−Ψo),‖q−qo‖<ρo∩Ψo∈(Ψ−π2,Ψ−π2)，0,other。 (19)

式中：Ψ为当前艏向角；Ψo为障碍物相对角度；‖q−qo‖为USV与障碍物的欧氏距离；k为斥力增益系数；ρo为传统斥力函数中的斥力影响范围，对斥力场函数求负梯度后可得，如下式：

Frep1(x,y)=kr1(1‖q−qo‖−1ρo)1‖q−qo‖2×cos(Ψ−Ψo)⋅∇‖q−qo‖， (20)
Frep2(x,y)=12kr1(1‖q−qo‖−1ρo)2∇Ψo。 (21)

∇Ψo计算如下式：

∇Ψo=sin(Ψ−Ψo)(yo−y)i−(xo−x)j(xo−x)2+(yo−y)2。 (22)

如图5所示，USV受到的斥力函数可视为Frep1和Frep2叠加完成。斥力函数的大小与障碍物的相对位置角度有关联，相对角度越大，计算得到的斥力就会越小，则障碍物避障角度越小，从而减小不必要的避障动作，USV完成避障后能够更快返回期望航迹线。

图 5 合力力示意图 Fig. 5 Resultant force diagram
3 仿真结果及分析 3.1 多障碍物避障实验仿真

构造多障碍物环境进行仿真。图6为传统APF算法的避障效果图，避障后的USV产生了过度转向，并且受到了航迹线附近障碍物斥力的影响。图7为改进后的LOS-APF算法，由于避障引力点的存在，USV以贴近障碍物边缘的路径进行避障，在未到达避障引力点时立刻切换为下个引力点，从而避免与障碍物发生碰撞。由于期望航迹点的存在，LOS-APF算法与传统算法相比，路径更加平滑。

图 6 多障碍物传统APF算法 Fig. 6 Multi-obstacle traditional APFalgorithm

图 7 多障碍物改进LOS-APF算法 Fig. 7 Multi-obstacle improved LOS-APF algorithm

图8(a)为改进前后2种算法的航迹偏移距离，2种算法均在210 ms处达到航迹偏移的最大值，传统APF的偏移达到82.6 m，而LOS-APF算法的偏移为30.8 m，最大航迹偏移量减少了62.71%。从整体上看来，400 ms后的LOS-APF算法完全贴合航迹线行驶，APF和LOS-APF算法整体的偏移量分别为2001.142 ms和287.887 ms，改进后的算法整体偏移量减少了85.61%。

图 8 多障碍物实验数据对比图 Fig. 8 Comparison of experimental data of obstacles

图8(b)为传统APF算法与改进后LOS-APF算法的USV艏向角对比图，APF算法由于在初始位置就受到了斥力作用开始了转向操作，偏离45°的最大艏向角度为62.9°。LOS-APF算法由于受到避障引力点作用稍晚转向，最大角度为37.44°，相比减少了59.52%。在多障碍物环境中，改进算法无论是航迹保持还是艏向变化都远小于传统算法。

3.2 大型障碍物避障实验仿真

为验证改进LOS-APF算法避障的效果，设定大型障碍物环境进行算法对比仿真实验。图9为传统APF算法的大型障碍物避障路线，当USV进入到障碍物的斥力势场范围后，USV由于受到障碍物斥力的作用产生艏向角的急转，甚至出现一定的后退情况，随后USV受到障碍物斥力的作用平滑避开障碍物并到达终点。

图 9 大型碍物传统APF算法 Fig. 9 Traditional APF algorithm for large obstacles

图10为改进LOS-APF算法在相同的障碍物地图下的避障路线，包括3个航迹引力点Plos2、Plos3、Plos4与3个障碍物引力Plos1、Plos5、Plos6。USV到达避障引力点Plos2范围内，则立刻进行航迹点切换，此时USV受到障碍物斥力作用远离障碍物，驶向下个避障引力点Plos3。与传统算法相比，避障动作结束后以贴近航迹线的路径到达终点。

图 10 改进LOS-APF算法大型障碍物避障 Fig. 10 Improved LOS-APF algorithm for large obstacle avoidance

图11(a)为传统APF算法与改进后LOS-APF算法在大型障碍物地图中的偏移量对比图，当USV行驶时间达到675 ms时，USV在2种算法下都到达了最大航迹偏移的位置，LOS-APF算法在最大航迹偏移量上比传统APF算法减小了26 m，USV在LOS-APF算法下整体的偏移总量减少了13.2%。

图 11 大型障碍物实验数据对比图 Fig. 11 Comparison of experimental data for large obstacles

图11(b)为传统APF算法与改进后LOS-APF算法的USV在大型障碍物避障中的艏向角对比图。传统APF与LOS-APF算法最大正转向角的位置分别为67°与49°，最大负转向角分别为−36°与−34°，整体艏向角度上改进算法减少了17.3%。在大型障碍物避障过程中由于环境的限制，LOS-APF算法的偏移量与艏向角变化都有一定的下限。从仿真结果可以看出，在大型障碍物地形中，USV在LOS-APF算法下具有更加短航迹偏移与更小的艏向角变化，避障效果更加优越。

3.3 局部最小值避障实验仿真

传统APF算法在避障过程中存在局部最小值的问题。图12为传统APF算法在陷入局部最小值情况下的避障路线图，APF算法理论上的全局梯度最低点为目标点，但由于环境与USV本身转向角度的限制，陷入无法摆脱的环境位置点。此时USV受到的障碍物斥力与引力相互抵消，陷入局部最小值状态，停留在原地。

图 12 传统APF算法局部最小值避障 Fig. 12 Traditional APF algorithm local minimum obstacle avoidance

改进LOS-APF算法在同样的局部最小值环境中，由于设置了避障引力点，USV逐个到达引力点并最终到达目的地。图13中的USV在既定的局部最小值环境下，通过改进后的LOS-APF算法动态切换引力点摆脱了局部最小值的状态，相较于传统APF算法，改进算法能够成功摆脱局部最小值环境。

图 13 改进LOS-APF算法局部最小值避障 Fig. 13 Improved LOS-APF algorithm for local minimum obstacle avoidance
4 结　语

本文针对传统APF在无人艇路径规划中存在航迹不可控和局部最小值问题，经研究改进提出一种LOS-APF算法。算法通过LOS原理确定了路径上的每个期望航迹点坐标，将位于障碍物内部的航迹点动态切换为避障点引力点。利用APF算法作为规划方案，采用自然对数构建航迹引力公式，并采用幂函数构建终点引力公式；在斥力函数中添加相对角度变量，随着角度的变化，障碍物对USV的斥力作用也逐渐变化，适用于航迹要求更高的场景。最后从仿真实验结果中可以得出，LOS-APF算法的路径偏移量远低于传统APF的路径偏移量，USV的艏向变化也更小，避障引力点的设计帮助USV摆脱局部最小值环境。在实际应用中LOS-APF算法既保留了APF算法的航迹平滑的特点，还能够驱动USV在稳定的航线行驶，有一定的实际应用价值。

参考文献
[1]
王磊, 刘晶晶, 齐俊艳, 等. 基于改进人工势场法的AUV全局路径规划[J]. 河南理工大学学报(自然科学版), 2024, 43(1): 132-139.
[2]
MARIN-PLAZA P, HUSSEIN A, MARTIN D, et al. Global and local path planning study in a ROS-Based research platform for autonomous vehicles[J]. Journal of Advanced Transportation, 2018, 20181−10.
[3]
葛甜, 岳佳豪. 改进人工势场法的避障轨迹规划[J]. 汽车实用技术, 2023, 48(23): 50-55.
[4]
张扬, 彭鹏菲, 曹杰. 基于改进APF算法的水面无人艇局部路径规划[J]. 兵器装备工程学报, 2023, 44(9): 42-48.
[5]
刘涛, 贾立校, 曹翔. 动态人工势场法的无人船避障路径规划[J]. 舰船科学技术, 2023, 45(5): 89-92.
[6]
FAN X, GUO Y, LIU H, et al. Improved artificial potential field method applied for AUV path planning[J]. Mathematical Problems in Engineering, 2020(1): 1−21.
[7]
SONG J, HAO C, SU J. Path planning for unmanned surface vehicle based on predictive artificial potential field[J]. International Journal of Advanced Robotic Systems, 2020, 17(2): 255689146.
[8]
ZHU Z, YIN Y, LYU H. Automatic collision avoidance algorithm based on route-plan-guided artificial potential field method[J]. Ocean Engineering, 2023, 271(3): 1-23.
[9]
CHU Y, WU Z, YUE Y, et al. PK-APF: path-keeping algorithm for USVs based on artificial potential field[J]. Applied Sciences, 2022, 12(16): 8201. DOI:10.3390/app12168201
[10]
LIU Z, SONG S, YUAN S, et al. ALOS-based USV path-following control with obstacle avoidance strategy[J]. Journal of Marine Science and Engineering, 2022, 10(9): 1203. DOI:10.3390/jmse10091203
[11]
WANG X, LIU J, PENG H, et al. A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments[J]. Journal of Intelligent & Robotic Systems, 2022, 105(2): 36-52.
基于优化人工势场法的无人水面艇路径规划
顾潮宏, 冯友兵, 仲伟波, 蔡立涛  &n...