基于改进人工势场法的船舶路径规划与跟踪控制

宁君， 马昊冉， 李铁山

(1.大连海事大学 航海学院，辽宁 大连 116026； 2.电子科技大学 自动化工程学院，四川 成都 611730)

近年来，随着物联网、大数据、云计算等技术的快速发展，智能航海受到广泛关注。面对复杂的海洋通航环境，在满足海上航行安全，保证经济性的前提下，如何使船舶在航行海域短时间内快速地规划出一条由起始点到终点并可以安全避开所有障碍物的路径最短航线，并使船舶能够达到良好的路径跟踪效果，成为学者研究的热点问题[1]。

在关于船舶路径规划的研究中，关于人工势场法，刘琨等[2]提出用指数函数代替二次函数构造势场函数并在斥力势场函数中增加无人船与目标点的相对位置的一个因子从而解决目标不可达问题；陈鹏[3]在斥力函数中引入了碰撞危险度因子，同时在引力函数中加入了原航线对本船的引力，实现了对静态障碍物进行避碰仿真实验；张鸣鸣[4]提出一种“人工干预船舶航向”的方法来解决局部极小值问题。Ge等[5]构造了新的人工势场法势场函数以解决静态情况下船舶避障出现目标不可达的问题。Song等[6]将速度障碍算法与人工势场法相结合，引入了一种非紧急情况下的2级动态避障算法；通过结合粒子群优化算法提高计算效率以及计算精度，以此来解决欠驱动无人水面船舶 (under-actuated unmanned surface vehicles, USV)动态环境下紧急情况的避碰问题。Yu等[7]利用网格法建立未知区域的地表水环境模型，再将改进斥力函数的人工势场法与方向随机逃脱方法相结合，再结合A*算法解决了无人巡航船上滤波器出现的获取不可达目标以及易陷入局部极小的问题。

在关于船舶路径跟踪控制的研究中，制导算法一直都是船舶运动控制的重要研究模块，视线制导方法(line of sight，LOS)是一种应用广泛的船舶制导算法，其主要应用在船舶直线路径跟踪中[8]。Healey等[9]首先利用LOS制导律对船舶进行控制，Fossen等[10,14-15]将自适应理论与LOS制导律结合，将漂角看作常量引入到LOS制导律中，有效地补偿了由于海洋环境引起的漂角，增强了控制器的鲁棒性。朱骋等[11]提出了一种基于Mamdani模型的自适应模糊LOS控制策略，该策略通过模糊控制修正LOS控制参数，改进了传统LOS算法各项参数固定，不能实时调整的特点。Qiu等[12]采用了轨迹线性化控制技术及有限时间干扰观测器，实现了期望的路径跟踪。沈智鹏等[13]针对建模不确定项以及内外扰动的欠驱动船舶路径跟踪控制问题，提出一种基于Lyapunov稳定性的神经元自适应迭代滑模控制方法，解决了复杂海洋环境干扰下船舶的精确路径跟踪问题。

在关于船舶路径规划与控制的研究中，Huynh等[16]提出了使用最优递推法对并联机器人进行路径规划，使用动态滤波法对机器人进行路径跟踪，保证被控对象运动距离最短，避免出现奇异点。Precup等[17]使用灰狼优化算法(grey wolf optimizer，GWO)对被控对象进行路径规划，使用比例积分模糊控制器进行路径跟踪，实验证明算法的有效性和性能显著提高。Ji等[18]将三角函数和障碍物指数函数叠加构建了三维虚拟危险势场，此方法适用于多种驾驶场合并具有良好的动态跟踪性和机动性。在船舶领域，Shen等[19]结合样条路径模板对船舶进行路径规划，利用非线性模型预测控制技术进行船舶路径跟踪，提高了算法的抗干扰性和运行效率。张力行[20]将A*算法与COLREGs相结合对欠驱动船舶进行路径规划，然后将LOS与神经网络控制器结合解决了欠驱动船舶路径跟踪的问题，这种方法在多种海洋环境的仿真实验中行之有效。

船舶路径规划结果最后必须通过船舶跟踪控制来实现。当前有关此方面研究较少，船舶路径规划及跟踪控制中还存在着算法完善性[21-22]、路径不够平滑[23]和路径跟踪信号为虚拟信号[24-25]等问题。基于以上研究分析，本文提出了一种基于改进人工势场法的船舶路径规划与跟踪控制方法，主要工作包括：

1)采用模拟退火算法优化传统人工势场法的斥力函数，既有效解决了传统人工势场法目标不可达和容易陷入局部极小值的缺陷，也解决了现有改进人工势场法只能优化传统人工势场法目标不可达或局部极小值问题。

2)在此基础上，对改进人工势场法规划出的路径进行曲线拟合，同时考虑船舶运动学及动力学特点，将拟合后的平滑路线作为参考输入信号进行路径跟踪，基于工业界广为使用的内外环控制思想，设计运动学跟踪引导率，实现了对参考信号的有效跟踪，同时解决了船舶的欠驱动问题。

3)使用神经网络对动力学子系统的不确定项进行逼近，实现了对内外扰动及未建模动态的补偿，有效跟踪了运动学控制子系统给出的期望速度，同时解决了传统人工势场法在进行路径规划时不能统筹考虑外界通航环境干扰的缺陷，实现了欠驱动船舶的高精度路径规划及跟踪控制。

1 改进人工势场法的船舶路径规划

1.1 人工势场法

船舶领域中，人工势场法(artificial potential field, APF)是将本船的航行环境抽象为势力场，将本船视为移动物，所有的障碍物(暗礁、岛屿、浮标、其他船舶等)视为具有最高势能的地方，目标点视为具有最低势能的地方，这样本船在障碍物产生的斥力以及目标点产生的引力共同作用下进行航行，逐渐地朝着势场中最低势能的地方也就是目标点航行。

设本船所在位置为(x,y)，X为本船位置向量，Xd为目标点位置向量，则本船在势场中受到目标点的引力为Fatt(X)，所受到的障碍物的斥力为Frep(X)，引力与斥力的合力为Ftotal(X)，其方向为本船应该改变的目标航向。随着本船的位置不断变化，合力Ftotal(X)的方向也随之变化，只要本船的航向随着合力的方向而改变，本船就可以在航行中避开障碍物到达目标点。

根据人工势场法原理可知，目标点对本船产生的引力Fatt(X)会随着本船与目标点间的距离的增加而单调递增，即本船距离目标点越远，目标点产生的引力越大；反之当本船距离目标点越近，目标点产生的引力越小，而当本船到达目标点时，此时目标点产生的引力为零。所以，本船的引力势场函数为：

(1)

式中：Ka为引力势场常量；(X-Xd)2为本船到目标点的相对距离：

(X-Xd)n=|(x-xd)n|+|(y-yd)n|

(2)

本船所受引力可以表示为引力势场函数的负梯度：

Fatt(X)=-grad(Uatt(X))=-Ka(X-Xd)=

-Ka[|(x-xd)n|+|(y-yd)n|]

(3)

式中引力Fatt(X)的方向为由本船指向目标点。

在势场中，为了避免本船与障碍物发生碰撞，障碍物会对本船产生一定的斥力Frep(X)，且斥力的大小会随着本船与障碍物间距离的变大而单调递减，即本船距离障碍物越近，障碍物产生的斥力越大；而当本船距离障碍物越远，障碍物所产生的斥力越小；而当这个距离无限大的时候即超出了障碍物的斥力影响范围，本船将不会再受到障碍物产生的斥力影响，即可以认定此时障碍物产生的斥力为0。所以在斥力势场部分需要分为2种情况计算。

假定以障碍物为中心的斥力势场影响范围为ρ0，本船与障碍物的距离为ρ，则当ρ≤ρ0时，本船与障碍物的距离与所受斥力成线性关系；当ρ>ρ0时，由于本船与障碍物距离ρ超出障碍物斥力场影响范围，所以不再受斥力场的影响，本船所受斥力将变为0。所以，本船的斥力势场函数为：

(4)

式中：kr为斥力势场常数；ρ(X,Xg)为本船到障碍物的欧几里得距离。

本船所受斥力可以表示为斥力势场函数的负梯度：

Frep(X)=-grad[Urep(X)]=

(5)

本船受到的合力为目标点产生的引力以及所有障碍物产生的斥力的矢量和：

(6)

式中n为本船所在位置受斥力影响的障碍物个数。

本船运动方向为所受的合力方向，在合力的控制下，本船在航行时会避开障碍物并向目标点不断靠近。当本船不断向目标点航行时，本船所受到目标点产生的引力将会成线性减小，当引力减小为0时，意味着本船到达目标点[26]。

1.2 模拟退火算法

模拟退火算法(simulated annealing algorithm, SA)来源于固体退火原理，主要受物理学中固体物质在融化状态下冷却至结晶状态的退火过程启发而提出的一种随机搜索算法。其退火过程主要包含加热升温、恒温过程和冷却过程。

模拟退火算法是以随机过程模仿固体物理冷却过程的一种算法，在这个过程中，固体内部的粒子即对应着模拟退火算法中的解，内能即是解的目标值，通过控制温度参数调整其他参数，使目标函数得到全局最小值也就是最优解。流程如下：

1)给定一个初始温度T0，随机给定初始状态X以及一个合适的退火速度θ，令T=T0，确定每个θ时的迭代次数，即Metropolis链长L；

2)令Xi=X+ΔX(ΔX为一个很小的均匀分布的随机扰动)。并计算新状态下势场强度与初始状态势场强度之差，即ΔU=U(Xi)-U(X)；

4)重复循环2)、3)直到系统达到平衡状态(判断逃离局部极小值后则终止)。

T(t)=AT(t-1)

式中：A为小于1但接近于1的数，具体参考0.85≤A≤1；t为时间变量。

选择销子的下表面为研究面，在摩擦实验前后，使用SEM S3700进行电镜扫描，观察销子下表面的微观结构的变化。使用EDX对销子下表面的化学组成进行测量，对比摩擦实验前后化学组成的变化。探究高温摩擦实验对样品钢渣表面和化学组成的影响。

1.3 改进人工势场法

人工势场法其不足之处在于当目标点靠近障碍物的时候,船舶将无法到达目标点,并存在局部稳定点,容易产生死锁现象,因而可能使船舶到达目标点之前就停留在局部最优点上[27]。

在众多有关人工势场法的研究中，目标点与障碍物距离相对较远，当船舶靠近目标点时，障碍物对船舶产生的斥力变得很小甚至可以忽略不计，船舶将只受到来自目标点的引力进而到达目标点。但在复杂的航行环境下，基本都会存在障碍物在目标点附近，这种情况下，船舶靠近目标点的同时也在向障碍物靠近，按照传统人工势场法中定义的引力函数和斥力函数，此时障碍物产生的斥力将远远大于船舶受到的引力，进而船舶将不会到达目标点。为解决目标不可达的问题，需要重新定义一个斥力函数，这个函数需要满足当船舶向目标点航行并靠近时，船舶受到的斥力尽可能地小至趋于零，那么目标点所在的位置将是整个势场的全局最小点，从而解决目标不可达的问题。新的势场函数中考虑了船舶与目标点之间的距离，在原有的斥力函数中添加一个系数项θ，改进以后的斥力函数为：

(7)

式中：Urep(X)为障碍物的斥力场；kr则表示斥力势场常数，是正数；ρ为船舶在空间位置中与障碍物间距离；ρ0为以障碍物为中心的斥力势场影响范围；X为船舶位置向量；Xd为目标点位置向量；ρ(X,Xg)表示船舶到障碍物的欧几里得距离；n为一个大于零的任意实数。其中，船舶与目标点间的相对距离|X-Xd|n=|(x-xd)n|+|(y-yd)|n，在原有的斥力函数中引入了船舶与目标点之间的相对距离，保障了目标点在整个势场中处于全局最小的位置。

船舶在航行过程中，在某一位置，船舶所受到的引力与斥力大小相等方向相反，处于受力平衡状态，达到稳定。从而使得该位置成为势场的全局最小点，导致船舶不能到达目标点。在复杂的航行环境中，可能存在多个局部极小值点。

针对势场中可能存在多个局部极小值点的问题，人工势场法中加入模拟退火算法，主要是利用模拟退火算法的突跳性对模型进行局部最优化。在程序中，给定一个初始温度以及一个初始状态，并给定合适的退火速度。随着本船的行进，初始温度也慢慢降低，在温度降低的过程中，模拟退火算法会对每一个状态下的势场强度进行计算，同时，会在每一个状态下给定一个随机扰动，并计算新状态下势场强度与初始状态势场强度之差，并对这个差值进行分析。按照Metropolis准则，若差值小于等于0，则程序会接受这个新状态；若差值大于0，则以一定的概率接受新状态，反之会保留原初始状态。如此往复，就可以避免本船在行进中产生局部极小值的情况，保证了程序的有效性以及可行性。

改进的人工势场法首先要改进原有的传统人工势场法斥力函数，在斥力函数中增加有关船舶与目标点之间的相对距离参数，将改进后的人工势场法与模拟退火算法相结合，得到最新的改进算法，改进后的算法流程图1所示。

图1 改进人工势场法流程

2 船舶路径跟踪控制

2.1 船舶的滑模路径跟踪控制

在研究船舶的路径规划及跟踪控制时，考虑船舶在6个自由度方向的运动会导致参数的增多，不仅会对仿真效果产生影响，而且还会增加运算量[28]。因此，本文只考虑船舶横荡、纵荡和艏摇。

(8)

(9)

为了实现x跟踪xd，y跟踪yd，首先要设计控制律U。设置理想的跟踪轨迹为(xd,yd)，则跟踪误差方程为：

(10)

式中：xe为船舶所在位置横坐标与跟踪轨迹横坐标的误差，则xe=x-xd；ye为船舶所在位置纵坐标与跟踪轨迹纵坐标的误差，则ye=y-yd。为了方便计算，可以取：

(11)

(12)

式中k1>0。

u2=yd-k2s2

(13)

式中k2>0。

(14)

设理想轨迹的船舶航向角为θd。如果θ=θd，则理想的轨迹控制律式(12)和式(13)可以实现，但是在实际模型式(9)中，初始阶段的θ和θd不可能完全一致，这样就会导致跟踪系统式(10)的不稳定。因此，需要将式(14)求得的船舶航向角当成理想值：

(15)

综上所述，得到实际的位置控制律为：

(16)

当θ与θd有误差时，会造成控制律无法精确实现，只有通过更快的收敛算法，使θ快速跟踪θd，才能解决这个问题。通过设计姿态控制律θ，实现θ跟踪θd。取θe=θ-θd，滑模函数为s3=θe，得到姿态控制律为：

ω=θd-k3s3-η3sgns3

(17)

式中k3、η3为函数系数，且k3>0，η3>0。

本文的运动学控制子系统基于内外环控制思想，位置子系统为外环系统，姿态子系统为内环系统，外环产生指令信号θd后，传递给内环系统，内环系统则通过滑模控制律实现对中间指令信号的追踪。为了实现稳定的内环滑模控制，本节采用的是工程上一般采用的方法，即内环收敛速度大于外环收敛速度的方法,通过θ快速跟踪θd，保证闭环系统的稳定性[24]。在本算法中通过调整内外环控制增益系数，保证内环收敛速度远大于外环收敛速度。具有内外双环的闭环系统如图2所示。

图2 具有双环的闭环系统结构

(18)

式中：R为微分器参数；U(t)为待微分的输入信号；x1为信号追踪；x2为信号一阶导数的估计；微分器初始值x1(0)=0，x2(0)=0。由于该微分器具有积分链式结构，本文对含有噪声的信号求导时，噪声只含在微分器的最后一层，通过积分作用信号一阶导数中的噪声能够被更充分地抑制。

2.2 基于神经网络船舶动力学控制

本节将2.1节的设计拓展到动力学控制，动力学控制器的目标是跟踪制导律给定的参考速度。船舶动力模型表示为[25]：

(19)

由于u=Ucosθ，则可以得到U和r的动态方程为：

(20)

动力学控制器的控制目的是让船舶跟踪期望前向速度与期望角速度:

(21)

式中δ1与δ2为较小的正数。

采用神经网络估计动力学模型不确定性与外部海洋环境的扰动，有界控制律给出控制力与力矩。式(20)可转化为一阶非线性控制系统:

(22)

muUsinθr)

(23)

(24)

(25)

式中c>0。则可以得到：

(26)

式中F(x)=f(x)+d(t)。

使用RBF神经网络逼近f(x)，网络算法为：

(27)

f=W*Th(x)+ε

(28)

(29)

(30)

定义Lyapunov函数为:

(31)

(32)

设计控制律为:

(33)

式中ηsgn(s)为鲁棒项，则：

(34)

取η>|ε|max，自适应律为:

(35)

3 实验仿真与分析

为了验证模拟退火算法改进的人工势场法进行船舶路径规划的有效性及船舶对期望航线的路径跟踪和控制器的有效性，本文采用Matlab/Simulink进行路径规划及跟踪控制仿真。

首先，构造具有障碍物的环境图，并对传统人工势场法和改进人工势场法进行分析对比，证明改进人工势场法对船舶路径规划的有效性。建立环境地图长和宽均为[-2,12]的正方形，在环境地图中随机建立n个黑色圆形障碍物，障碍物的圆心用一个n×2的向量表示，障碍物的半径为0.5。建立欧氏坐标系，设置船舶起始点的位置为(0，0)，目标点的位置为(10，10)。在对比实验中，传统人工势场法与改进人工势场法采用相同的参数。设置人工势场法的步长为0.1，引力增益系数为5，斥力增益系数为15，障碍影响距离为2。模拟退火算法中的参数设置为初始温度，终止温度T0=0，循环次数为100次。

图3为简单障碍物环境下，传统人工势场法与采用模拟退火算法改进的人工势场法路径规划的实验结果；图4为较复杂障碍物环境下，传统人工势场法与采用模拟退火算法改进的人工势场法路径规划的实验结果。

图3 简单障碍物下传统与改进人工势场法路径规划

图4 复杂障碍物下传统与改进人工势场法路径规划

通过图3～4结果可以看出，在简单障碍物条件下，传统人工势场法与改进人工势场法都可规划出一条由起点到终点的有效路径，但改进的人工势场法路径更平滑，且具有可行性；在较为复杂的障碍物条件下，传统人工势场法的路径规划出现了目标不可达的缺陷，而采用改进斥力函数并采用模拟退火算法改进的人工势场法克服了传统人工势场法的缺陷，对于较复杂的路径规划更加有效。

其次，对规划出的最优路径进行平滑处理，并作为输入信号进行路径跟踪。为了便于分析，以下实验采用图3(b)的简单障碍物下改进人工势场法进行实验。使用Matlab中ployfit函数对最优路径进行曲线拟合，拟合函数为：

y=a1xn+a2xn-1+a3xn-2+…+anx+an-1

(36)

式(36)为一个多项式，在拟合过程中，需要对不同的多项式次数进行拟合。拟合次数少，路径拟合效果比较差，拟合次数过多，会出现龙格现象，只有当误差最小时，才会出现较为理想的拟合效果。本文拟合函数为10次多项式，拟合效果如图5所示，图中蓝色圆圈为实际路径，红色曲线为拟合曲线。

图5 改进人工势场法路径曲线拟合

然后，对拟合出来的曲线进行路径跟踪。控制对象为式(8)，位置指令[xd,yd]为xd=t，yd为拟合多项式函数。参数设置为k1=k2=0.3，k3=3，η3=0.50，位置初始值为[0,0,0]，采用控制律式(18)和式(20)，针对姿态控制律式(20)的切换项，采用饱和函数代替切换函数,边界层厚度取0.1，微分器参数取xd=[Ud,rd]，仿真结果如图6～8所示。

图6 实际海况船舶路径跟踪航线

从图6～8中可以看出，船舶在跟踪路径时，位置和航向角与规划航线没有较大偏差，船舶在具有双环结构的闭环控制系统控制下，可以完美地跟踪路径，完成从起始点到目标点的跟踪任务。并且θd在区间(-π/2,π/2)内，满足式(17)的要求，具有良好的跟踪效果。

图7 位置和角度的跟踪曲线

图8 输入信号变化曲线

最后，因为实验中没有考虑船舶动力学控制问题，因此设计基于径向基神经网络的船舶动力学控制器，跟踪运动学制导律给出的期望速度，解决在真实的海洋环境航行时，船舶容易受到海洋环境风浪流的干扰，系统存在不确定性，对船舶航行造成影响的问题。设计控制指令为xd=[Ud,rd]，控制律采用式(33)，自适应律采用式(35)，取γ=50，η=0.5。根据网络输入x1和x2的实际范围来设计高斯基函数的参数，参数c和b的取值分别为[-2,-1,0,1,2]和3。网络权值中各个元素的初始值取0.1，仿真结果如图9和图10所示。

图9 动力学控制力矩

图10 动力学系统控制力

仿真结果分析可得，通过合理设计运动学内外双环的控制增益，在保证船舶航向收敛速度快于合速度收敛速度的前提下，运动学子系统能够快速跟踪拟合后的期望路径。动力学子系统能够快速跟踪运动学子系统给出的引导率，实现基于改进人工势场算法的船舶路径规划及跟踪控制。

4 结论

1)本文基于模拟退火算法对传统的人工势场法进行优化，解决了传统人工势场法存在的目标不可达以及易陷入局部最小值的问题。

2)将改进人工势场法的规划航线作为参考跟踪路径，通过多项式拟合，使其作为船舶运动学控制子系统的参考输入信号。

3)基于工业界广泛使用的内外双环控制方法，分别设计了船舶运动学控制律，即合速度引导率，转艏角速度引导率。

4)考虑船舶动力学子系统的特性，研究了船舶动力学控制问题。通过引入神经网络对船舶动力学子系统的未知不确定项进行逼近，设计了一种基于径向基神经网络算法的滑模控制器，实现了对内外扰动及未建模动态的补偿，同时解决了传统人工势场法在进行路径规划时不能统筹考虑外界通航环境干扰的缺陷。

5)通过仿真实验，验证了改进人工势场法路径规划的有效性和船舶对期望航线的路径跟踪的可行性，实现了对运动学控制子系统引导率的跟踪控制，进而实现了船舶的路径规划与跟踪，为船舶路径规划与跟踪控制的结合提供了较好的方法。

本文只考虑了单船的路径规划与跟踪控制，对于多船编队路径规划与跟踪控制方面还有待研究。