基于改进的投饵船模型预测轨迹跟踪控制

张丽珍,顾素郡,吴 迪

（上海海洋大学工程学院，上海 201306）

0 引言

轨迹跟踪控制是自主巡航投饵船运动控制的一项重要研究内容，它是指投饵船从起始位置沿着给定的参考轨迹航行到目的地[1,2]。投饵船具有时滞的特点，并且投饵船的运动学模型会随着航行状态的不同而变化，海上环境因素的干扰也会导致跟踪发生偏离。另外，投饵船在轨迹跟踪的过程中，系统独立控制变量的个数比其本身的自由度个数少。以上这些特点使得投饵船轨迹跟踪控制成为一个非线性、欠驱动、受多种约束限制的控制问题[3]。对于投饵船这样存在多种约束限制的系统，约束会破坏系统获取理想的控制性能，导致跟踪发生不稳定的现象。而对执行机构采用较大的控制动作能够控制投饵船保持较高程度的跟随性以及稳定性。但是，投饵船执行动作不能过大是投饵船系统对控制增量的时域约束。所以投饵船系统对控制量的时域约束与对控制性能的要求产生矛盾，这让投饵船轨迹跟踪问题更加困难[4]。

模型预测控制（Model Predictive Control,MPC）是一种基于状态空间方程的反馈控制，该控制算法利于解决投饵船这种非线性动力学问题。系统通过对每一当前时刻的优化，来充分利用允许的控制动作，从而提高控制的性能，达到最优控制。同时，在预测系统未来的输出中，能够及时纠正因时变约束引起的跟踪不稳定现象[5,6]。是处理时域约束系统控制的最优方法，具有较强的鲁棒性，对解决受约束自主巡航投饵船轨迹跟踪控制问题具有独特的优势。

文献[7]在模型预测控制器中考虑了环境干扰因素，实现了无人船轨迹和航向的稳定跟踪。但是没有进行实时控制，处理约束条件的能力低，使得无人船跟踪收敛效果比较差。文献[8]采用广义预测控制的算法，根据所建立的大型船舶模型提出了约束条件，设计了约束轨迹跟踪控制器，解决了直线轨迹跟踪不稳定的问题，但是没有考虑曲线以及其它航迹的跟踪问题。文献[9]针对无人艇非线性系统，构造了稳定性控制律，实现了较精准的轨迹跟踪。但是，该控制算法是建立在船舶存在横摇运动的基础上，要求船舶在参考轨迹的邻域内航行，这就导致了船舶不能实现全面完整的轨迹跟踪。

本文在充分考虑投饵船非线性欠驱动约束的特性下，分别研究了投饵船受时域约束的直线和两种不同曲线的轨迹跟踪性能。采用模型预测控制的方法处理时变控制输入约束，并将线性时变模型预测系统输出结合系统状态量、控制量以及控制增量的时域约束，把轨迹跟踪控制转化为二次规划目标函数的求解。通过仿真证实了该控制器是提高直线，双移线和蛇形轨迹跟踪性能的最优方案。

1 投饵船系统模型建立

1.1 运动学模型

投饵船三自由度运动学方程[10]为：

式中，位置状态向量E=[x,y,ψ]分别是投饵船所在位置的纵坐标，横坐标和航向角；速度状态向量v=[u,v,r]分别表示投饵船沿纵坐标方向的运动速度，沿横坐标方向的运动速度以及航向角速度。

1.2 动力学模型

投饵船的动力学方程为：

式中,mi（i=1,2,3）为投饵船包含附加质量在内的惯性质量；di（i=1,2,3）为投饵船三个位置状态下的水动力系数；τ=[τ1τ2τ3]=[F1+F20 (F1-F2)·L]，F1和F2分别是左右推进器的推力,L表示推进器到投饵船中线的距离。

2 问题制定

把上式(1)和式(2)转换为非线性状态空间方程的形式：

设定参考轨迹上的每一个点都能够满足上述运动学和动力学方程。用d代表参考量，则非线性状态空间方程为：

将式(3)和式(4)相减，则系统输出量和期望量的误差量为：

为了让投饵船系统良好地跟踪参考轨迹，期望投饵船的控制输入接近参考轨迹的虚拟输入，即满足。

3 线性时变模型预测集成控制器设计

模型预测控制的投饵船轨迹跟踪控制器的原理图如图1。在整个控制的过程中，设定一条期望的参考轨迹，并建立与控制器的接口。投饵船系统通过求解改进的目标函数以及对控制量，控制增量以及输出量的时域约束优化，得到控制时域内一系列控制输出，系统将其最优输出值设定为投饵船控制系统当前时刻的实际控制量。控制器则根据当前时刻投饵船系统的获取值和参考轨迹，预测未来一段时间的系统输出。在下一个时刻，系统重复滚动优化。加上时域约束，确保了结果是在投饵船动力学模型执行范围内。同时，二次规划最优问题的算法往往需要反复迭代的结果，加上时域约束，能够减少控制系统对状态空间方程的计算时间。

图1 控制器原理图

3.1 线性误差模型

将投饵船误差模型进行线性离散化处理[11]，则系统在每个κ采样时刻的线性时变模型预测方程为：

基于增量形式的状态空间预测模型，用控制增量ΔU（κ）代替控制量U（κ）作为输入量，同时约束控制增量和控制量，从而避免投饵船受外界干扰而稳定地跟踪目标轨迹[12,13]。新的增广状态空间方程为：

式中，各矩阵定义如下：

3.2 未来状态预测

3.3 约束优化

式中：INC为NC行列向量，⊗为克罗内克积，U（t-1）为上一时刻投饵船实际的控制量。

由式(15)和式(16)，控制量约束可转化为如下形式：

在每一控制时域周期内计算式(13)，得到最优解为ΔU*C（κ）。把最优解的第一列作为系统实际控制增量，并把该增量和上一时刻的控制量之和作用于当前时刻：

依据模型预测控制的滚动优化策略，下一时刻依据上一时刻的状态信息重新预测下一时刻的输出，反复求解，得出系统控制过程中的最优控制输入。

4 仿真分析

为了验证本文所设计控制器的跟踪性，在MATLAB/Simulink环境下对投饵船进行轨迹跟踪的仿真。对比三种不同轨迹仿真工况下，投饵船轨迹跟踪的实时稳定性以及跟随性。不同仿真工况结果如下：

表1 投饵船模型参数

投饵船初始位置和方向设置为:x（0）=0，y（0）=0，ψ=0，初始速度和角速度设置为：u=2m/s，v=0/s，r=0m/s。

模型预测控制器仿真参数以及时域约束条件设置为：Ts=0.02s，预测时域NY=15，控制时域NC=1，误差权矩阵Q=[600 0；0 800]，权值矩阵R=10。-20°≤ψ≤40°，-15°≤δ≤30°。

工况一：直线轨迹跟踪

从图2的仿真结果可以看出，本文设计的控制器基本实现了对直线轨迹较高精度的跟踪。由图（a）到图（g）可知，投饵船的初始位置不在期望直线轨迹的初始位置上，但在5s内，投饵船能够快速地跟踪上期望的直线轨迹，并且在随后的运动中保持稳定跟踪。直线轨迹跟踪的最大误差值为投饵船初始位置与期望位置的距离，为3m。随着投饵船不断地向期望轨迹靠近，误差值逐渐减小。在5s后，横向位置跟踪误差基本收敛为零。航向角跟踪在最初的5s内，为了调整到期望轨迹的航向，变化较大，误差范围在40°以内。5s后，航向跟踪上的期望的轨迹航向，基本实现零偏差。前5s内，航向角速度变化范围大，最大为40°/s，但5s后基本能保持平缓稳定。在投饵船进行直线跟踪过程中，前5s内，因为调整航向，速度出现波动，但5s后，速度能够稳定控制在2m/s内。在15s的航行中，转角始终控制在约束范围内。

图2 直线轨迹跟踪仿真结果

工况二：双移线轨迹跟踪

从图3的仿真结果可以看出，本文设计的控制器对双移线有良好的跟踪性能。由图3(h)到图3(n)可知，在整个控制时域内，投饵船基本实现了横向位置良好的跟踪效果，跟踪误差在0.05m范围内波动。可能由于动力学模型与控制器之间存在延迟模块，用于计算系统实际航向角和采用的双移线参考轨迹线性化方法所确定的航向角之间的偏差，造成了控制在时间上的滞后，导致航向控制在拐点处产生了6°左右的偏差值，但控制器能够及时调整偏差，最终仍能保持向期望的双移线航向前进。航向角速度基本能够平稳的跟踪上期望值，航行中有较小的波动，但波动范围都在可控范围内，并且航向角速度最终实现了收敛。在整个双移线航行中，前2s，因为需要转向，速度有微小的跳变，但随后，纵向速度基本控制在2m/s。完成双移线跟踪需要25s，转角始终控制在6°范围内。

图3 双移线轨迹跟踪仿真结果

工况三：蛇形轨迹跟踪

从图4的仿真结果可以看出，本文设计的控制器提高了蛇形运动轨迹的跟踪性能。由图4(o)到图4(u)可知，投饵船能够稳定地跟踪期望的蛇形轨迹，在整个蛇形轨迹航行中，横向位置误差缩小在0.05m范围内。因为蛇形轨迹的曲率在实时变化，航向控制的执行存在时间滞后，导致在整个控制时域内，航向始终存在误差，最大误差达5°左右，但在控制器可控范围之内，不会使投饵船出现严重偏离。航向角速度在每一时刻下，对蛇形轨迹的跟踪都能够保持平滑稳定，基本没有跳变现象，并且最终能够有效收敛。在蛇形轨迹跟踪过程中，速度在最初航行时出现较小的波动，大约2s后，基本稳定在2m/s。完成蛇形轨迹的跟踪需要50s，航行中的转角仍然控制在6°范围内。

图4 蛇形轨迹跟踪仿真结果

5 结语

本文研究了带有时域约束的自主巡航投饵船轨迹跟踪问题，采用模型预测轨迹跟踪控制器实现了投饵船在移动时域下对直线，双移线和蛇形参考轨迹的跟踪控制。在控制器的设计过程中，考虑了控制输入受时变的约束影响，让其能够有效的处理系统时域约束以及非线性动力学问题。仿真结果表明，采用模型预测控制算法可以有效地解决带时域约束的投饵船轨迹跟踪控制问题，达到良好的跟随性和稳定性。但仍然存在不足，对轨迹曲率实时变化下的跟踪仍有一定的偏差，需在后续工作中进一步展开控制研究。