针对路径跟踪的导引律问题，国内外进行了大量的研究。文献[1]应用视线法(LOS)导引律来镇定横向跟踪误差，然后应用反馈线性化方法设计了动力学控制器，实现了欠驱动水面船舶直线路径跟踪控制，但没有考虑模型的不确定性。为了改善路径跟踪的效果，文献[2]设计了一种前视距离时变的LOS导引律。文献[3]针对无人水面艇的路径跟踪控制，应用积分视线法(ILOS)导引律，并采用反馈线性化方法设计了航速和航向的动力学控制器。另外，文献[4]针对欠驱动水面船舶的路径跟踪控制设计了一种向量场(VF)导引律。针对AUV动力学模型的非线性、耦合性和不确定性，动力学控制的设计也得到了广泛的研究。目前，常用的方法有PID控制[5]、反馈线性化控制[6]、自适应控制[7]、鲁棒控制[8]、滑模控制(SMC)[9]、模糊控制[10]和神经网络控制[11]等。但是以上文献都没有考虑实际控制输入的约束问题。文献[12]考虑了输入的约束，应用模型预测控制(MPC)对LOS导引律进行优化，改善了船舶对直线路径的跟踪质量。文献[13]考虑了舵角的幅值和速度约束，应用MPC设计控制器实现了AUV的纵倾和深度控制。但是文献[12-13]都是基于名义模型，没有考虑模型的不确定性。基于路径点的直线跟踪控制虽然路径规划简单便于应用，但是路径在路径点处是不光滑的。在AUV对路径进行跟踪过程中，当路径在路径点附近进行切换时，跟踪误差会存在突变，从而使舵角很容易产生饱和。舵角频繁出现饱和不仅会降低系统的稳定性，而且还会增加AUV的阻力和能耗。文献[14]针对船舶的路径跟踪控制，考虑了舵角的饱和并应用反步法设计了控制器，但并没有从根本上改善舵角的饱和现象。另外，在路径跟踪过程中，经常需要避障，设计避障导引律[15]和对路径进行重新规划[16]是目前常用的避障方法，但是这2种方法都会增加控制器的复杂度。
本文针对欠驱动AUV三维直线路径跟踪和避障控制，基于级联控制理论设计了运动学和动力学控制器。在运动学控制器中考虑了AUV角速度存在的约束，应用MPC设计了导引律，负责产生期望的角速度信号。为了实现对期望速度信号的跟踪，在动力学控制中考虑了推进器转速和舵角的饱和，应用SMC设计了动力学控制器，可以有效地克服模型的不确定性，保证系统的鲁棒性。
1 欠驱动AUV运动模型 本文研究的欠驱动AUV在尾部配置一个推进器用来实现对纵向速度的控制、水平舵和方向舵分别实现对纵倾和艏摇的控制，在横向和垂向没有驱动力。图 1为欠驱动AUV三维直线路径跟踪示意图，{B}:O-xyz和{I}:E-ξηζ分别为随体坐标系和固定坐标系；?和p分别为横摇角和横摇角速度；Rk为接受圆半径。因为存在恢复力，AUV的横摇很小，另外路径跟踪不需要对横摇进行控制，忽略横摇后，AUV的运动学模型可以简化为如下五自由度的形式：
 |
| 图 1 欠驱动AUV三维直线路径跟踪示意图 Fig. 1 Schematic diagram of underactuated AUV 3D straight path following |
| 图选项 |
 | (1) |
式中：η=[ξ η ζ θ ψ]T表示AUV在固定坐标系{I}中的位置和姿态，(ξ, η, ζ)为AUV在{I}中的坐标，θ和ψ分别为纵倾角和艏摇角; v=[u v w q r]T表示AUV在随体坐标系{B}中的速度分量, u、v、w、q、r分别为纵向、横向、垂向速度和纵倾、艏摇角速度。J(η)为{B}到{I}的旋转变换矩阵，表达式为
 |
AUV的动力学模型可以简化为
 | (2) |
式中：
2 三维直线路径跟踪误差模型 为了便于建立路径跟踪误差模型，以当前直线路径的起点pk为原点定义坐标系{F}:P-xFyFzF。{F}坐标系的xF轴与直线路径重合并指向下一个路径点pk+1，yF轴指向右侧，zF轴指向下方。定义{F}坐标系相对于{I}坐标系的姿态为AF=[θF ψF]T，其计算公式为
 | (3) |
式中：Δξ=ξk+1-ξk，Δη=ηk+1-ηk，Δζ=ζk+1-ζk；(ξk ηk ζk)，(ξk+1ηk+1ζk+1)分别为当前直线路径起点pk和终点pk+1在{I}坐标系中的坐标。定义AUV相对于{I}坐标系的姿态为AB=[θ ψ]T，则AUV相对于{F}坐标系的姿态误差计算如下：
 | (4) |
式中：下标e表示误差。
对式(4)求导可得
 | (5) |
定义AUV在{I}坐标系中的坐标PO=[ξo，ηo，ζo]T，P在{I}坐标系中的坐标PP=[ξk，ηk，ζk]T，则AUV相对于{F}坐标系的位置误差为
 | (6) |
对式(6)求导可得
 | (7) |
3 三维直线路径跟踪控制器的设计 控制器分为运动学和动力学控制器两部分，首先在运动学控制器中应用MPC设计了导引律，负责产生期望的纵倾角速度qd和艏摇角速度rd。然后，应用SMC设计了动力学控制器，负责产生实际的控制信号，即推进器的转速np、水平舵角δs和方向舵角δr。
3.1 运动学控制器 因为动力学控制器对期望速度信号的响应有一定的延时，为了减小延时对控制效果的影响，添加如下低通滤波器：
 | (8) |
式中：T1>0、T2>0为可调时间常数。
因为本文研究的欠驱动AUV在横向和垂向没有驱动力，所以v和w是非常小的，为了简化控制器设计，可对其进行忽略。另外，纵向误差xe不需要进行控制。根据式(5)、式(7)和式(8)，三维直线跟踪可简化为以下系统的镇定控制问题：
 | (9) |
式中：ky=cos θesin ψe/ψe, kz=sin θe/θe，为了避免奇点，当ψe≤π/12时, ky设置为ky=cos θe，当|θe|≤π/12时, kz设置为kz=1；ye=ye-yed，ze=ze-zed，yed和zed是为了实现避障添加的惩罚项，当没有障碍物时它们被设置为零，当需要避障时，设置yed或zed为一个正值；可以实现从障碍物的右侧或下方避障，反之亦然。系统(9)可表示为
 | (10) |
式中：x=[ye ze θe ψe q r]T和u=[qd rd]T分别为状态量和控制量。系统(10)可以近似离散化为
 | (11) |
式中：
 |
其中：T为采样周期。
根据预测模型(11)，未来的状态变量可以通过式(12)进行预测：
 | (12) |
式中：Np和Nc分别为预测时域和控制时域，满足Nc≤Np。根据以上状态变量, 可得输出量为
 | (13) |
输出量可以重新整理为如下矩阵形式：
 | (14) |
式中：
 |
 |
考虑控制量的幅值存在如下约束：
 | (15) |
通过用Uk, k参数化，可以将约束(15)整理为如下线性不等式的形式：
 | (16) |
式中：Mc=INc?I2；Nmax=1Nc?umax; Nmin=-1Nc?umin, 1NcT=[1 1 1…1]1×Nc, 符号?代表克罗内克积(Kronecker Product)。为了求取最优的控制增量Uk, k，定义当前采样时刻的目标函数为
 | (17) |
式中：R=INc?R，R=diag(R11, R22); Q=INp?Q，Q=diag(Q11, Q22, Q33, Q44, Q55, Q66)为权重矩阵。
因为对目标函数的求解并不一定能绝对保证MPC的稳定性，所以接下来需要设计稳定约束条件。定义上一采样时刻的目标函数为
 | (18) |
式中：
 |
取上一采样时刻最优解Uk-1, k-1的后Nc-1个控制增量和当前时刻最优解Uk, k的最后一个控制增量组成一个当前时刻的可行解，该可行解及在其控制作用下的预测输出分别为
 |
定义ΔJk, k=Jk, k-Jk-1, k-1，则
 |
所以只要满足约束条件(19)则可以保证ΔJk, k≤0，从而保证MPC的稳定：
 | (19) |
为了便于计算最优解，约束(19)中的变量通过用Uk, k参数化，可以整理为如下形式：
将式(14)代入目标函数(17)可得
 | (20) |
式中：E=ΘTQΘ+R; F=ΘTQΨxk, k。
因为式(20)等号右边第1项为常量，所以MPC每一步的优化问题等价为求解如下约束问题：
 | (21) |
通过对式(21)求解可以得到预测时域内的控制量Uk, k，将Uk, k的第1个值uk, k代入式(22)，即可得到当前采样时刻纵倾和艏摇角速度的最优期望值为
 | (22) |
3.2 动力学控制器 因为本文研究的欠驱动AUV在横向和垂向没有驱动力，所以其横向速度v和垂向速度w是非常小的，为了简化动力学控制器的设计，可以将动力学方程进一步简化为
 | (23) |
式中：
 |
因为推进器的推力和舵力是具有饱和限制的，定义输入饱和量为
 | (24) |
为了对输入饱和进行补偿，设计如下辅助系统：
 | (25) |
式中：
 |
定义误差变量：
 | (26) |
式中：ud>0为期望的航速。
定义滑模函数：
 | (27) |
因为AUV的期望航速ud被设置为常值，所以系统可以近似为自治的，可以认为du、dq、dr有界且慢时变，其导数可以忽略。定义误差变量：
 | (28) |
式中：
定义Lyapunov函数：
 | (29) |
对V求导可得
 |
设计控制律和自适应律为
 | (30) |
可得：
控制律(30)中kj>0(j=1, 2…, 8)，鲁棒项的系数εi能够根据滑模函数自动进行调整，可以减小鲁棒项的抖振。控制信号np、δs、δr可以通过如下推进器和舵的水动力模型计算：
 | (31) |
式中：ρ、KT、DP、kL分别为流体密度、螺旋桨推力系数、螺旋桨直径、舵的升力系数。
4 仿真结果与分析 本文采用REMUS-100作为仿真模型，模型参数可以参考文献[17-18]。仿真采用了2种控制方法进行了对比。
1) 方法1
采用本文所设计的控制器，控制器的主要设计参数为：T=0.1, T1=0.5, T2=0.5, Q11=2, Q22=2, Q33=2, Q44=2, Q55=1, Q66=1, R11=2, R22=2, Np=10, Nc=3, k1=0.1, k2=0.1, k3=0.1, k4=0.1, k5=0.1, k6=0.1, k7=0.2, k8=30, c11=1, c12=1, c13=1, c21=1, c22=1, c23=1, c31=1, c32=1, c33=1, umax=[0.15 0.2]T, umin=[-0.15-0.2]T。期望直线路径基于表 1所列路径点生成。AUV的初始位姿和速度均为零。期望航速ud=1 m/s，路径切换的接受圆半径Rk =10 m。在仿真中设置了一个障碍物，其半径为5 m，在固定坐标系中的坐标为：PA=[ξA, ηA, ζA]T=[85, 0, 21]T m。仿真中选择从右侧避障，惩罚项按式(32)进行设置：
 | (32) |
表 1 路径点 Table 1 Waypoints
| 序号 | ξ | η | ζ |
| 1 | 10 | 0 | 0 |
| 2 | 10 | 25 | 3 |
| 3 | -40 | 25 | 9 |
| 4 | -40 | -25 | 15 |
| 5 | 10 | -25 | 21 |
| 6 | 60 | -25 | 21 |
| 7 | 110 | 25 | 21 |
| 8 | 135 | 25 | 21 |
| 9 | 135 | -25 | 21 |
| 10 | 85 | -25 | 21 |
表选项
式中：
2) 方法2
运动学控制器采用了LOS导引律进行路径跟踪和避障[15]，动力学控制器采用PID控制器。为了验证控制器的鲁棒性，仿真时段0~100 s水动力参数增加20%，100~250 s采用名义水动力参数，250~400 s水动力参数减小20%。
图 2为三维直线路径跟踪和避障示意图及其在水平面、垂直面的投影图。可以看到, 2种控制方法都能实现对直线路径的跟踪和避障。但是通过对比也可以看到，在每次进行路径切换时，方法1的跟踪效果更好一些，能够更快地收敛到期望路径上，另外方法1能够更快更准确地收敛到期望的横向误差从而更好地完成避障。图 3为路径跟踪误差曲线。可以看到, 每一段直线路径上的位置误差和姿态误差都能收敛到零，同时可以看到方法1大约在250 s时将yed设置为了7 m，从而启动了避障，当避障完成后yed又被设置为零，最终使AUV重新收敛到期望路径上。图 4为实际的控制输入曲线。可以看到, 控制输入存在有规律的抖振，其主要原因是直线路径在路径点处不光滑，路径切换时跟踪误差存在跳变。仿真中根据实际情况将舵角的最大绝对值设置为25°，可以看到方法2中的舵角在路径切换时出现了饱和现象，但方法1没有出现饱和现象，整体上方法1的舵角更加平稳，这样更利于系统的稳定性和节能。图 5为AUV的速度曲线。可以看到，2种控制方法都能使AUV的纵向速度收敛到期望值，但方法1的控制效果更稳定一些, 同时可以看到横向和垂向速度很小而且是收敛的。方法1的纵倾角速度和艏摇角速度都在约束范围之内，从而有效地改善了舵角饱和现象。图 6为方法1的鲁棒项系数和滑模函数曲线。可以看到，所有的滑模函数在路径切换时都出现了一定的抖动，但均是收敛的且趋于零，鲁棒项系数能够自动进行调整从而有效地减小鲁棒项引起的抖振。
 |
| 图 2 路径跟踪和避障仿真结果 Fig. 2 Simulation results of path following and obstacle avoidance |
| 图选项 |
 |
| 图 3 路径跟踪误差曲线 Fig. 3 Path following error curves |
| 图选项 |
 |
| 图 4 控制输入曲线 Fig. 4 Control input curves |
| 图选项 |
 |
| 图 5 速度曲线 Fig. 5 Speed curves |
| 图选项 |
 |
| 图 6 鲁棒项系数和滑模函数曲线 Fig. 6 Robust term coefficients and sliding mode function curves |
| 图选项 |
5 结论 1) 本文基于级联控制理论分别应用MPC和SMC设计了运动学和动力学控制器，从而实现了欠驱动AUV三维直线路径跟踪和避障控制。
2) 在运动学控制器中设计了线性时变MPC导引律，并设计了MPC的稳定约束条件。因为运动学方程不受动力学模型不确定性的影响，所以运动学控制器采用MPC不仅能保证预测的效果，而且可以简化控制器的设计。
3) 运动学控制器考虑了期望角速度的约束，可以有效地改善舵角的饱和现象，从而更利于系统的稳定性和节能。
4) 动力学控制器采用SMC可以克服动力学模型的不确定性，保证系统的鲁棒性。
接下来将针对存在海浪、海流等环境干扰下的欠驱动AUV三维路径跟踪进行进一步的研究。
参考文献
| [1] | LEKKAS A M, FOSSEN T I.Minimization of cross-track and along-track errors for path tracking of marine underactuated vehicles[C]//2014 European Control Conference(ECC).Piscataway: IEEE Press, 2014: 3004-3010. |
| [2] | LEKKAS A M, FOSSEN T I. A time-varying lookahead distance guidance law for path following[J]. IFAC Proceedings Volumes, 2012, 45(27): 398-403. DOI:10.3182/20120919-3-IT-2046.00068 |
| [3] | 陈霄, 刘忠, 张建强, 等. 基于改进积分视线导引策略的欠驱动无人水面艇路径跟踪[J]. 北京航空航天大学学报, 2018, 44(3): 489-499. CHEN X, LIU Z, ZHANG J Q, et al. Path following of underactuated USV based on modified integral line-of-sight guidance strategies[J]. Journal of Beijing University of Aeronautics and Astronautics, 2018, 44(3): 489-499. (in Chinese) |
| [4] | XU H, SOARES C G. Vector field path following for surface marine vessel and parameter identification based on LS-SVM[J]. Ocean Engineering, 2016, 113: 151-161. DOI:10.1016/j.oceaneng.2015.12.037 |
| [5] | FOSSEN T I, PETTERSEN K Y, GALEAZZI R. Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 820-827. DOI:10.1109/TCST.2014.2338354 |
| [6] | CAHARIJA W, PETTERSEN K Y, S?RENSEN A J, et al. Relative velocity control and integral line of sight for path following of autonomous surface vessels:Merging intuition with theory[J]. Proceedings of the Institution of Mechanical Engineers, Part M:Journal of Engineering for the Maritime Environment, 2014, 228(2): 180-191. DOI:10.1177/1475090213512293 |
| [7] | FOSSEN T I, LEKKAS A M. Direct and indirect adaptive integral line-of-sight path-following controllers for marine craft exposed to ocean currents[J]. International Journal of Adaptive Control and Signal Processing, 2017, 31(4): 445-463. DOI:10.1002/acs.2550 |
| [8] | 李娟, 边信黔, 熊华胜, 等. AUV的精确航迹跟踪系统的鲁棒控制[J]. 哈尔滨工业大学学报, 2013, 45(1): 112-117. LI J, BIAN X Q, XIONG H S, et al. Robust control research for AUV trajectory control system[J]. Journal of Harbin Institute of Technology, 2013, 45(1): 112-117. (in Chinese) |
| [9] | YAN Z P, YU H M, ZHANG W, et al. Globally finite-time stable tracking control of underactuated UUVs[J]. Ocean Engineering, 2015, 107: 132-146. DOI:10.1016/j.oceaneng.2015.07.039 |
| [10] | 潘永平, 黄道平, 孙宗海. 欠驱动船舶航迹Backstepping自适应模糊控制[J]. 控制理论与应用, 2011, 28(7): 907-914. PAN Y P, HUANG D P, SUN Z H. Backstepping adaptive fuzzy control for track-keeping of underactuated surface vessels[J]. Control Theory & Applications, 2011, 28(7): 907-914. (in Chinese) |
| [11] | LIU C, ZOU Z J, YIN J C. Trajectory tracking of underactuated surface vessels based on neural network and hierarchical sliding mode[J]. Journal of Marine Science and Technology, 2015, 20: 322-330. DOI:10.1007/s00773-014-0285-y |
| [12] | OH S R, SUN J. Path following of underactuated marine surface vessels using line-of-sight based model predictive control[J]. Ocean Engineering, 2010, 37(2-3): 289-295. DOI:10.1016/j.oceaneng.2009.10.004 |
| [13] | YAO X L, YANG G Y, PENG Y. Nonlinear reduced-order observer-based predictive control for diving of an autonomous underwater vehicle[J]. Discrete Dynamics in Nature and Society, 2017, 4394571. |
| [14] | ZHENG Z W, FEROSKHAN M. Path following of a surface vessel with prescribed performance in the presence of input saturation and external disturbances[J]. IEEE/ASME Transactions on Mechatronics, 2017, 22(6): 2564-2575. DOI:10.1109/TMECH.2017.2756110 |
| [15] | MOE S, PETTERSEN K Y.Set-based line-of-sight (LOS) path following with collision avoidance for underactuated unmanned surface vessel[C]//IEEE Mediterranean Conference on Control & Automation.Piscataway: IEEE Press, 2016: 402-409. |
| [16] | SHIM T, ADIREDDY G, YUAN H L. Autonomous vehicle collision avoidance system using path planning and model-predictive-control-based active front steering and wheel torque control[J]. Proceedings of the Institution of Mechanical Engineers, Part D:Journal of Automobile Engineering, 2012, 226(D6): 767-778. |
| [17] | PRESTERO T.Verification of a six-degree of freedom simulation model for the REMUS autonomous underwater vehicle[D].Cambridge: Massachusetts Institute of Technology, 2001: 12-112. |
| [18] | 段斐.微小型水下机器人运动仿真研究[D].哈尔滨: 哈尔滨工程大学, 2012: 11-80. DUAN F.Research on motion simulation for mini autonomous underwater vehicle[D].Harbin: Harbin Engineering University, 2012: 11-80(in Chinese). |
