基于PSO-NTSMC的UUV有限时间路径跟踪控制

于浩淼， 刘承蔚， 郭晨， 贾兆岩

(1.大连海事大学 船舶电气工程学院， 辽宁 大连 116026；2.中国兵器工业集团 航空弹药研究院有限公司， 黑龙江 哈尔滨 150001)

0 引言

水下无人航行器(UUV)路径跟踪是近年来的热点问题。路径跟踪控制不受时间约束，只要求欠驱动UUV收敛至期望路径，而对于何时到达何地没有严格要求。通过引入“虚拟向导”跟踪制导策略，设计虚拟向导切向速度，并且建立了欠驱动UUV与期望路径参考点之间的误差方程。由于其本身的欠驱动性和非完整性约束，欠驱动UUV路径跟踪问题的研究一直是当前的热点与难点。Lapierre等针对模型不确定性约束下的欠驱动UUV水平面路径跟踪控制问题，采用反步法和李雅普诺夫直接法设计一种鲁棒非线性控制器，提高了模型不确定性约束下系统的鲁棒性，实现了欠驱动UUV的水平面路径跟踪控制。Moe等研究未知洋流情况下的UUV二维路径跟踪运动，提出了制导律和自适应反馈线性化方法与滑模控制方法相结合组成的闭环系统，完成了路径跟踪控制。苗建明等提出一种基于改进反步法的三维空间曲线路径跟踪控制器，解决了运动学控制器存在非因果现象的问题和动力学控制器过于复杂的问题。Habib等针对由海浪和海流扰动引起的时变干扰约束下的欠驱动UUV路径跟踪控制，提出一种改进的自抗扰控制器。张伟等针对UUV垂直面路径跟踪提出一种基于非线性连续模型预测控制算法的控制器，实现了欠驱动UUV路径跟踪控制。Guo等针对参数摄动约束下欠驱动UUV的三维直线路径跟踪控制问题，提出了一种固定时间收敛的非奇异快速终端滑模三维直线路径跟踪控制方法。

粒子群优化(PSO)算法是一种典型的智能优化算法,该算法模拟鸟群觅食行为的原理，利用群体智能建立模型。PSO算法具有优化性能好、操作简单、容易实现的优点，因而受到了国内外学者的广泛关注。胡文华等设计一种针对四轴飞行器轨迹跟踪的串级自抗扰控制器，并且采用改进PSO算法进行控制器参数自整定，使系统性能得到优化。张天佳等提出一种基于并行结构的多种群PSO算法，在基本PSO算法的基础上，增加了能加速和减缓粒子群移动的两种更新公式，既改善了PSO算法种群多样性差的缺陷，又能保证算法的计算精度。Fakoor等针对带有时变载荷的4自由度卫星，设计了一种滑动模态和线性二次调节器(LQR)相结合的控制器，减少了阻尼干扰、提高了精度，并在其基础上提出了一种寻找变量最优值和归一化积分平方误差的PSO算法，有效地优化了控制器参数，提高了系统性能。

在UUV路径跟踪控制领域，目前研究成果相对成熟。文献[6-7]提出了具有创新性的动力学控制器，但在运动学制导律的设计上有所欠缺；文献[5]对传统反步法进行改进，但没有解决参数摄动对控制器的影响；文献[8]提出有限时间控制策略，但控制器参数精度上有所欠缺。因此，本文对上述问题进行分析改进。

本文研究的是欠驱动UUV在水平面直线上的路径跟踪问题。从本质上来说，直线轨迹与曲线轨迹没有区别，只是执行任务的需求不同。本文针对参数摄动和恒定未知海流对于系统的影响，提出了一种基于PSO的有限时间路径跟踪方法。首先，基于改进的视线法(LOS)制导方法构造欠驱动UUV的误差方程，将位置跟踪问题转化为艏向角误差的镇定问题；与此同时，将速度跟踪问题转化为纵向速度误差和转艏角速度误差的跟踪问题。其次，通过引入非奇异终端滑模面，设计路径跟踪控制器，用以镇定纵向速度误差、转艏角速度误差和艏向角误差，并且克服了参数摄动和恒定未知海流干扰对系统产生的负面影响。接着，应用PSO算法，将控制器参数调节转化为目标函数极小值的搜索问题，克服了手动调参不准确的影响，得到更优秀的控制效果。最后，进行了系统的数值仿真，数值仿真的结果表明，该控制方法可以在有限时间内有效地执行水平面欠驱动UUV路径跟踪任务。

1 欠驱动UUV运动模型

定义固定坐标系和随体坐标系两个坐标系：固定坐标系是一个惯性坐标系，其轴指向地理位置的正北方向，轴指向地理位置的正东方向，轴指向地心方向；随体坐标系的轴指向UUV的艏部，轴指向UUV的右舷，轴指向UUV的底部。

本文考虑的模型是欠驱动UUV在水平面上的运动问题，即只考虑进退(沿轴平移)、横移(沿轴平移)和回转(绕轴旋转)3个自由度。欠驱动UUV模型在横移方向缺少控制输入，则在此自由度上会受到加速度约束——2阶非完整性约束。

因此，UUV运动学模型被描述为

(1)

式中：、分别表示随体坐标系下UUV进退方向和横移方向的线速度；表示UUV的艏向角；表示UUV的转艏角速度。

UUV动力学模型被描述为

(2)

式中：

(3)

2 欠驱动UUV路径跟踪制导律设计

UUV在开阔海域时，可以看作沿两个航迹点之间做直线运动。定义一系列离散航迹点{,,…,,…,}，那么UUV就依次沿着相邻目标点做直线运动。这些离散航迹点的位置坐标通常可以表示为[,]。相邻航迹点(,+1)之间的直线段表示为，而航迹点为由航迹线-1到航迹线的转向点。在直线段内，UUV的期望航迹可以被描述为

(4)

(5)

UUV水平面直线航迹跟踪示意图如图1所示。在图1中，每个航迹点(除最后一个外)均引入一个局域Serret-Frenet坐标系{}，且坐标系{}的轴与重合。将惯性坐标系的原点平移到航迹点上，然后进行旋转，使惯性坐标系与局域Serret-Frenet坐标系{}重合，其中旋转角定义为

(6)

当UUV运动时，任一时刻随体坐标系{}的原点在期望航迹上的投影在图1中记为点，UUV在期望轨迹上的目标点在图1中记为点。当航行器不断运动时，局域坐标系{}始终以投影点为原点，沿着轨迹不断平移。根据几何关系，随体坐标系{}的原点到直线航迹的距离为

(7)

(8)

定义航行器的期望艏向角为

=+Δ

(9)

图1 UUV水平面路径跟踪示意图Fig.1 UUV horizontal path tracking diagram

由(8)式可知，当Δ→0°时，位置误差趋近于0。那么UUV的水平面位置跟踪问题转化为艏向角误差的问题，速度跟踪问题转化为纵向速度误差(进退方向)和艏向角速度误差(回转方向)的问题。

3 欠驱动UUV路径跟踪控制器设计

为了简化设计，将UUV水平面运动控制分解为纵向速度控制和转艏控制两个子系统。引入非奇异终端滑模面，达到路径跟踪的效果，设计出全局有限时间收敛的运动学控制器。

3.1 纵向速度控制器设计

在实际工程中，横向速度不可测量，且纵向速度远大于横向速度，可以忽略横向速度对控制器的影响。引入纵向速度误差，则纵向速度控制问题就转变为了的镇定问题。引入如(10)式非奇异终端滑模来镇定:

()=++()

(10)

(11)

(12)

(13)

选取如下李雅普诺夫函数：

(14)

对求1阶导数，得

(15)

由有限时间定理可知，存在一个时间1，当时间>1时滑模面收敛至0 rad/s，即=0 rad/s。

由=0 rad/s可得

(16)

(16)式等号两边对时间积分，可得

(17)

解上述不等式，可得

(18)

因此，存在时间常数=，当时间>=1+时，纵向速度误差收敛至0 m/s。

3.2 转艏控制器设计

由第2节可知，UUV的水平面位置跟踪问题已经转化为艏向角误差的问题，定义=-。那么首先构造李雅普诺夫函数：

(19)

(20)

由上文可知，跟踪艏向角问题已经转化为镇定艏向角速度误差的问题，为完成控制目标，引入非奇异终端滑模：

()=++()

(21)

(22)

与31节同理，转艏控制律为

(23)

式中：

(24)

选取如下李雅普诺夫函数：

(25)

对求1阶导数，得

(26)

由有限时间定理可知，存在一个时间常数2，当时间>2时滑模面收敛至0 rad/s，即=0 rad/s。

4 欠驱动UUV路径跟踪控制器参数优化

4.1 PSO算法

本文采用PSO算法提升控制器的性能，利用PSO算法迭代寻优的特性，找到定义适应度函数的更优解。

本文应用的是标准PSO算法，粒子群每次迭代更新自身速度和位置的公式为

(+1)=·()+()[()-()]+
()[()-()]

(27)

(+1)=()+(+1)

(28)

式中：=1,2,…,，为粒子个数；=1,2,…,，为最大迭代次数；和为[0,1]范围内的均匀随机数；和为学习因子；为惯性权重；为粒子的位置；为粒子的速度；()表示第个粒子在第次迭代时的参数；()表示所有粒子在第次迭代时的最优参数。

4.2 控制律参数优化方法

如32节所述，欠驱动UUV水平面路径跟踪问题实际上已经转化为纵向速度误差、转艏角速度误差和艏向角误差的镇定问题。适应度函数选取IAT指标，其定义为

(29)

本文中选取待优化的参数有、、、、，故选取五维PSO算法，各参数的取值范围见表1。设置粒子数为40，迭代次数为40。惯性权重的变化影响着PSO算法对最小值的搜索性能，当较大时全局收敛能力较强，局部收敛性较弱；当较小时局部收敛性较强，全局收敛性较弱。设置惯性权重初值为=1，更新公式为(+1)=099()。 随着搜索的进行不断减小，这样可以保证在算法开始时，各粒子能够以较大的速度步长在全局范围内探测到较好的区域；而搜索后期值较小，粒子能够在极值点附近做精细的探索，从而使算法大概率向全局最优解位置收敛。

随着迭代次数的增加，适应度值不断减小，最后得到最小值点，该点值即为最优解。最优解对应的参数值即为最优参数值。

表1 参数搜索范围

5 稳定性分析

构造李雅普诺夫函数

(30)

对(28)式求导，可得

(31)

为了分析欠驱动UUV水平面路径跟踪控制系统稳定性，选取李雅普诺夫函数：

=+++

(32)

对进行求导，可得

(33)

6 系统仿真及结果

为了验证非奇异终端滑模控制器的控制效果以及PSO算法的优化效果，在MATLAB软件中进行仿真实验。选取粒子种群规模=40，粒子最大迭代次数=40，学习因子=15、=20；该型欠驱动UUV的相关运动参数详见文献[18]。

航行器水平面期望路径的航迹点分布为(100 m,100 m)→(100 m,400 m)→(300 m,400 m)→(300 m,600 m)→(500 m,600 m)→(500 m,100 m)→(700 m,100 m)→(1000 m,600 m)。

航行器的初始状态设置为(0)=200 m、(0)=50 m、(0)=90°、(0)=0 m/s、(0)=0 m/s、(0)=0 rad/s；航行器控制器的参数设置为=7、=5、=5、=5、=3、=3、=6、=6、=6、=6。在控制系统的动力学模型中，加入10的参数摄动和恒定未知海流，来验证控制系统的鲁棒性。恒定未知海流被加入到航行器的动力学模型中，海流初始流速和流向分别为05 m/s和30°。

在PSO算法优化过程中，随着迭代次数的增加，全局极值不断减小，最后维持不变。算法找到的适应度值最优点=75636 1，对应的参数值=0575 4、=0972 8、=16167 3、=1318 0、=0100 2。图2为目标函数的适应度值变化曲线。

图2 适应度值曲线Fig.2 Fitness value curve

为了验证PSO算法的优越性，将基于粒子群的非奇异终端滑模控制(PSO-NTSMC)算法与非奇异终端滑模控制(NTSMC)算法、传统积分滑模控制(ISMC)算法进行对比。NTSMC算法的参数为手动调参最优值，ISMC算法中定义的滑模面与其控制律如下：

在数值仿真中，航行器的期望纵向速度被设置为2 kn(大概为1 m/s)，仿真结果如图3所示。从图3中可以看出NTSMC算法和PSO-NTSMC算法很好地完成了纵向速度跟踪任务，使纵向速度无超调、快速地追踪期望值，并且PSO-NTSMC算法的纵向速度与NTSMC算法相比更接近期望值。ISMC算法也完成了速度跟踪任务，但误差较大。

图3 纵向速度响应曲线Fig.3 Longitudinal velocity response curve

图4为路径跟踪结果，从中可以看出，控制器很好地完成了位置跟踪任务，并且在左侧放大图中可以看出，PSO-NTSMC算法在(127.6 m，400.0 m)处完成转弯，NTSMC和ISMC算法分别在(161.4 m，400.0 m)和(175.8 m，400.0 m)处完成转弯。因此PSO-NTSMC算法的效果要优于NTSMC和ISMC算法。

图4 水平面路径跟踪效果图Fig.4 Map of Horizontal path-following effect

图5给出了路径跟踪误差曲线，航行器误差在控制器的作用下收敛到0 m。由于靠近航迹点时需要转弯，误差迅速增大，但在短时间内重新收敛为0 m。在局部放大图中可以看出ISMC、NTSMC和PSO-NTSMC算法分别在(595.9 m，0 m)、(598 m，0 m)和(598.8 m，0 m)处误差增大，并于(688.7 m，-0.02 m)、(672.3 m，-0.02 m)、(658.2 m，-0.02 m)处误差基本减小为0 m，故PSO-NTSMC控制效果更佳。图6给出了艏向角及艏向角误差响应曲线。由图6可知，PSO-NTSMC算法在转弯处期望漂角更小，故期望艏向角更平稳；PSO-NTSMC算法的艏向角误差更小，在横坐标为452 s的转弯处为0.15°；NTSMC和ISMC算法分别为-3.4°和-20.1°。 图7给出了航行器跟踪控制器的输出。由图7可见：尽管相比NTSMC算法，PSO-NTSMC算法进退方向的力有些许抖动，但在开始阶段NTSMC算法的力为320 N远远大于PSO-NTSMC算法的160 N。 而ISMC算法开始阶段的力较小，但是抖动过大。故多方面考虑，PSO-NTSMC算法所需要的能源少、抖动小，故控制策略更佳。

图5 路径跟踪误差曲线Fig.5 Path-following error curve

图6 艏向角和艏向角误差响应曲线Fig.6 Response curve of yaw angle and yaw angle errors

图7 控制器输出Fig.7 Controller output

7 结论

本文针对欠驱动UUV水平面直线路径跟踪问题，结合非奇异终端滑模控制和PSO算法提出控制器，克服了恒定未知海流、参数摄动以及控制器参数不精确的缺点。利用LOS制导法思想和Serret-Frenet坐标系结合，得到跟踪误差方程，进而将路径跟踪问题转化为镇定纵向速度误差、艏向角速度误差和艏向角误差的问题，设计非奇异终端滑模控制器达到有限时间内完成路径跟踪的目的和效果。引入PSO算法，搜索更优的控制器参数，提高路径跟踪的精度。最后，仿真结果表明该控制器可以克服参数摄动和恒定未知海流干扰的影响，精确地执行路径跟踪任务。