CN108897217B - 一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法，能够在满足欠驱动系统输入和状态约束的基础上根据给定的性能指标得到保证系统稳定的优化跟踪性能，包括以下步骤：1)将轨迹跟踪误差模型投影到体坐标系下，利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律并结合系统约束求取终端不变集；2)获取当前时刻系统状态，构建有限时域包含终端约束的MPC约束优化问题；3)求解MPC约束优化问题，获取当前时刻预测的最优控制序列，将第一个最优控制作用于系统；4)新采样时刻获取新的系统状态并更新MPC约束优化问题，滚动迭代直到跟踪控制过程结束。该方法不但考虑了系统的实际约束，也能够根据给定的指标以较短的预测时域获得优化的跟踪性能。

Description

一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法

技术领域

本发明属于水面船运动控制技术领域，具体为一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法。

背景技术

海洋航行器作为海洋资源勘探、开发和海洋军事应用的主要运载平台，以其自主化、智能化、多功能化、适应性强等特点在民用和军用方面都发挥了极其重要的作用，受到了各个发达国家的关注和重视。民用方面的主要应用是海洋资源的勘探、海底地貌地形的勘测、水下设备的检测和深海打捞服务等。军用方面的主要应用是预先了解水下战场环境(战区的侦察和探测)、破坏敌方设备和设施、水下危险的排除、水下中继通信等。

作为各种应用的基础，海洋航行器的运动控制得到了广泛的研究。对于全驱动系统来说，由于系统控制变量的个数等于系统被控制自由度的个数，因此有着更好的灵活性。然而考虑到能量消耗、经济成本、重量等问题，水面船又常被设计成欠驱动的系统。当执行机构发生故障时，全驱动航行器也将成为欠驱动系统。水面船通常由两个执行机构驱动，是较为典型的欠驱动航行器，是海上运输、水上救援、海洋科考的主要平台，因此针对欠驱动水面船的研究具有十分重要的工程意义。

相比于全驱动水面船，欠驱动水面船由于系统控制量的个数少于被控制自由度的个数，属于高度非线性耦合系统，其运动控制更为困难。因此对于欠驱动水面船的跟踪问题较多研究针对的是位置跟踪问题，对于欠驱动水面船的轨迹跟踪问题，主要思路是进行局部线性化和模型的解耦。在欠驱动轨迹跟踪问题中：K.Y.Pettersen等通过坐标变换实现了对参考轨迹的跟踪，但变换后使得跟踪误差的物理意义不是那么明确；K.D.Do等从子系统的角度分析了闭环系统的稳定性，实现了对参考轨迹的跟踪，E.Lefeber等利用级联方法实现了对参考轨迹的跟踪，但是无法获取整个系统的Lyapunov函数且级联方法限制了控制器对不确定参数的自适应性以及无法保证全局指数稳定；Ashrafiuon等利用滑模方法实现了跟踪控制且对抖振进行了削弱，但是也在一定程度上降低了跟踪性能和鲁棒性；李雅普诺夫函数在非线性控制系统设计中具有重要的作用，但是用于系统控制设计的李雅普诺夫函数较难构造，反演法在一定程度上解决了这一问题，Z.P.Jiang等利用反步法实现了对参考轨迹的跟踪，但是控制器的结构一般较为复杂。可以发现，这些控制方法大都没有考虑执行机构和系统状态的实际约束，然而实际应用中执行机构的能力都是有限的，系统状态也常是受限的；另一方面，这些控制方法在控制的过程中无法满足一定的性能指标(能量消耗最少，优先跟踪某一系统状态)。因此，我们迫切地需要一种控制方法能够在设计控制律时考虑到这些因素。

模型预测控制(Model Predictive Control,MPC)方法就可以在满足约束的情况下达到最优控制性能。值得注意的是，虽然B.J.Guerreiro、F.Kühne、H.Zheng和C.G.Liu等研究了利用模型预测控制方法解决欠驱动水面船轨迹跟踪控制相关问题。但是，这些模型预测控制器都未考虑终端约束、难以确保实际应用过程中系统的稳定性。

发明内容

要解决的技术问题

本发明的目的是提供一种在满足约束的情况下能够获取稳定最优控制效果的模型预测控制方法，以解决欠驱动水面船轨迹跟踪控制中实际约束和给定性能无法满足以及无终端约束模型预测控制难以保证稳定性的问题。

技术方案

一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法，其特征在于：第一部分是利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律；第二部分是将第一部分求得的控制律作为有终端约束MPC的终端控制律，进而选取合适的终端不变集并结合系统模型、系统约束和优化指标构建MPC约束优化问题；步骤如下：

步骤1：终端控制律的设计及相应参数的选取：

步骤1.1：假设水面船有着良好的对称性且低速航行，忽略动力学模型中惯性矩阵的非对角项和阻尼矩阵的非对角项及非线性项，考虑参考轨迹是由一个虚拟水面船模型产生，则实际水面船运动模型

和虚拟水面船运动模型

可分别表示为：

其中，x＝[x,y,ψ,u,v,r]'为实际水面船的系统状态，x_d＝[x_d,y_d,ψ_d,u_d,v_d,r_d]'为虚拟水面船的系统状态，[x,y,ψ]'为水面船在地面坐标系下的位姿，[u,v,r]'为水面船在载体坐标系下的线速度和角速度，u为实际的控制输入，u_d为参考的控制输入；

步骤1.2：将误差模型

投影到体坐标系下得到

即：

其中，x_e＝[x_e,y_e,ψ_e,u_e,v_e,r_e]'为地面坐标系下的跟踪误差在载体坐标系下的投影；

步骤1.3：设置水面船的模型参数m₁₁、m₂₂、m₃₃、d₁₁、d₂₂、d₃₃；

步骤1.4：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和角速度r_d，利用虚拟水面船模型生成参考轨迹，设定实际水面船的初始状态x(0)；

步骤1.5：选取Lyapunov函数

利用反步法设计可得：在选取合适参数k_i,i＝0,…8的情况下，当实际的控制输入为u＝[X,N]'时，能够使得跟踪误差渐近收敛，X、N的具体形式分别为：

其中，

为虚拟角速度误差，

为虚拟角速度，

为虚拟前向速度误差，

为虚拟前向速度；

步骤1.6：由步骤1.4可知，选取合适的参数是设计控制律使得跟踪误差渐近收敛的关键部分，定义k₀＝m₂₂/(k₃d₂₂-m₂₂)、k₄＝m₂₂/(k₃m₁₁)、k₆＝k₄-d₁₁/m₁₁+C₁、

k₃＝m₂₂/(d₂₂λ)、c＝r_d(1-k₇)ψ_{e_max}m₁₁/d₂₂、

ψ_{e_max}＝maxψ_e，C₁为正常数；

步骤1.7：确定k₇：{k₇|0<1-k₈-k₇ψ_{e_max}}、确定λ₁₂：{λ₁₂|0<λ₁₂<1}、确定ε₃₄₇：{ε₃₄₇|0<ε₃₄₇}、确定λ₅₆：{λ₅₆|0<λ₅₆<1}、确定k₂：{k₂|0<k₂}；

步骤1.8：确定λ的范围，使得k₈为实数，λ：

步骤1.9：确定k₈的范围，使得满足k₅为实数的条件：

步骤1.10：确定k₅：

步骤1.11：构建优化问题

求解使得k₁最小时的ε₁、ε₂、ε₃、ε₄、ε₅、ε₆、ε₇，即：

s.t.ε₁+ε₂＝ε₁₂,ε₅+ε₆＝ε₅₆,ε₃+ε₄+ε₇＝ε₃₄₇,

ε₁>0,ε₂>0,ε₃>0,ε₄>0,ε₅>0,ε₆>0,ε₇>0,

其中，C₂为正常数；

步骤1.12：将已确定的参数k_i,i＝0,…8带入步骤1.4所描述的控制律X、N中；将此控制律X、N作用于误差系统，验证该时变状态反馈控制律是否能够使得跟踪误差渐近收敛，是否能够作为MPC的终端控制律；

步骤2：基于MPC的轨迹跟踪控制方法的设计：

步骤2.1：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和角速度r_d，利用虚拟水面船模型生成参考轨迹，设定实际水面船的初始状态为x(0)；

步骤2.2：设置MPC迭代的时间T/次数N_t、预测时域T_p、采样间隔δ。考虑到实际系统的采样机制，控制输入定义为

系统方程的状态值由四阶-五阶Runge-Kutta算法求解，其中

表示不大于t的最大采样时刻；

步骤2.3：设置当前t_k时刻MPC约束优化问题的目标函数为：

其中，T_p为预测时域，x_e(t；t_k)为系统在t_k时刻对t时刻误差状态的预测，u_e(t；t_k)为系统在t_k时刻对t时刻误差输入的预测，Q为误差状态的加权矩阵，R为输入误差的加权矩阵，P为终端误差状态的加权矩阵，其中，

P_e、Q_e的具体形式分别为：

步骤2.4：设置MPC优化问题的状态约束x_e∈X、输入约束u_e∈U以及终端约束x_e(t_k+T_p)∈Ω(t_k+T_p)；

步骤2.5：根据模型预测系统未来的动态，求解t_k时刻的MPC约束优化问题

得到t_k时刻预测的最优控制序列，t_k时刻优化问题的具体形式为：

其中，

步骤2.6：将第一个最优控制作用于误差系统，直到新的采样时刻t_k+1到来时，将新的采样状态作为初始状态构建新时刻t_k+1的MPC约束优化问题，滚动迭代直到跟踪控制过程结束。

有益效果

本发明提出的一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法，利用Lyapunov直接法和反步法，设计的使得跟踪误差渐近收敛的时变状态反馈控制律，在设计过程中跟踪误差不需要进行坐标变换、设计的Lyapunov函数为整个系统的Lyapunov函数，有着更加明确的物理意义；设计的MPC跟踪控制律将实际输入范围和状态范围作为硬约束，更加符合实际应用；设计的MPC跟踪控制律考虑到了给定的优化指标，可以实现对指定状态的优先跟踪；设计的MPC跟踪控制律考虑了终端约束，能够在满足约束的情况下以较短的预测时域得到使得误差系统稳定(输入状态稳定,Input-to-State-Stability,ISS)的最优控制律，解决欠驱动水面船轨迹跟踪控制中实际约束和给定性能无法满足以及无终端MPC难以保证稳定性的问题。

附图说明

图1：Cybership Ⅱ实验船

图2：参考轨迹

图3：反步法作用下的跟踪轨迹

图4：反步法作用下的跟踪误差

图5：反步法作用下跟踪误差的范数

图6：反步法作用下的控制输入

图7：MPC方法作用下的跟踪轨迹

图8：MPC方法作用下的跟踪误差

图9：MPC方法作用下跟踪误差的范数

图10：MPC方法作用下的控制输入

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明提出的一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法的原理是将跟踪误差模型投影到体坐标系下，利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律并结合系统约束获取合适的终端不变集；采样当前时刻系统状态，构建有限时域带终端约束的MPC约束优化问题；求解约束优化问题，得到当前时刻预测的最优控制序列，将第一个最优控制作用于系统；新采样时刻获取新的系统状态并更新MPC约束优化问题，滚动迭代直到跟踪控制过程结束。

实施方式主要分为两部分，第一部分是利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律；第二部分是将第一部分求得的控制律作为有终端约束MPC的终端控制律，进而选取合适的终端不变集并结合系统模型、系统约束和优化指标构建MPC约束优化问题，具体步骤为：

步骤1：终端控制律的设计及相应参数的选取，

步骤1.1：假设水面船有着良好的对称性且低速航行，忽略动力学模型中惯性矩阵的非对角项和阻尼矩阵的非对角项及非线性项，考虑参考轨迹是由虚拟水面船模型产生，则实际水面船运动模型

和虚拟水面船运动模型

可分别表示为：

其中，x＝[x,y,ψ,u,v,r]'为实际水面船的系统状态，x_d＝[x_d,y_d,ψ_d,u_d,v_d,r_d]'为虚拟水面船的系统状态，[x,y,ψ]'为水面船在地面坐标系下的位姿，[u,v,r]'为水面船在载体坐标系下的线速度及角速度，u为实际的控制输入，u_d为参考的控制输入。

步骤1.2：将误差模型

投影到体坐标系下得到

即：

其中，x_e＝[x_e,y_e,ψ_e,u_e,v_e,r_e]'为地面坐标系下的跟踪误差在载体坐标系下的投影。

步骤1.3：设置水面船的模型参数m₁₁、m₂₂、m₃₃、d₁₁、d₂₂、d₃₃，参照图1，本实施例中使用的模型为CybershipⅡ实验船，该船的驱动机构为尾部的两个推进器，具体参数为m₁₁＝25.8、m₂₂＝33.8、m₃₃＝2.8、d₁₁＝0.72、d₂₂＝0.89，d₃₃＝1.9，最大推力为2N，最大旋转力矩为1.5N·m。

步骤1.4：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和角速度r_d，利用虚拟水面船模型实现参考轨迹的生成。本实施例中所使用模型的最大前向速度为0.2m/s、最大角速度为0.5236rad/s、最大侧向速度为0.1m/s，本实施例考虑跟踪一圆周轨迹，选取参考角速度r_d＝0.05rad/s，参考前向速度u_d＝0.05m/s，参考轨迹的初始状态为x_d(0)＝[0,0,0,0.05,0,0.05]'，参考轨迹参照图2；设定实际水面船的初始状态为x(0)＝[-0.0478,-0.0086,0.0001,0.0553,0.0002,0.0426]'。

步骤1.5：选取Lyapunov函数

利用反步法设计可得：在选取合适参数(k_i,i＝0,…8)的情况下，当实际的控制输入为u＝[X,N]'时，能够使得跟踪误差渐近收敛，X、N的具体形式分别为：

其中，

为虚拟角速度误差，

为虚拟角速度，

为虚拟前向速度误差，

为虚拟前向速度。

步骤1.6：由步骤1.4可知，选取合适的参数是设计控制律使得跟踪误差渐近收敛的关键部分，定义k₀＝m₂₂/(k₃d₂₂-m₂₂)、k₄＝m₂₂/(k₃m₁₁)、k₆＝k₄-d₁₁/m₁₁+C₁、k₃＝m₂₂/(d₂₂λ)、c＝r_d(1-k₇)ψ_{e_max}m₁₁/d₂₂、

ψ_{e_max}＝maxψ_e。

步骤1.7：确定k₇：{k₇|0<1-k₈-k₇ψ_{e_max}}、确定λ₁₂：{λ₁₂|0<λ₁₂<1}、确定ε₃₄₇：{ε₃₄₇|0<ε₃₄₇}、确定λ₅₆：{λ₅₆|0<λ₅₆<1}、确定k₂：{k₂|0<k₂}。本实施例中，k₇＝0.8、λ₁₂＝0.2、ε₃₄₇＝0.5、λ₅₆＝0.5、k₂＝1、ψ_{e_max}＝5°，由此可确定c＝1.3482、ε₁₂＝0.0069、ε₅₆＝20.7134。

步骤1.8：确定λ的范围，使得k₈为实数，λ：

本实施例中λ＝0.9167、正常数C₁＝1，由此可确定k₃＝41.4268、k₀＝11.0105、k₄＝0.0316、k₆＝1.0037。

步骤1.9：确定k₈的范围，使得满足k₅为实数的条件：

本实施例中k₈＝0.1077。

步骤1.10：确定k₅：

本实施例中k₅＝2.2026。

步骤1.11：构建优化问题

求解使得k₁最小时的ε₁、ε₂、ε₃、ε₄、ε₅、ε₆、ε₇，即：

s.t.ε₁+ε₂＝ε₁₂,ε₅+ε₆＝ε₅₆,ε₃+ε₄+ε₇＝ε₃₄₇,

ε₁>0,ε₂>0,ε₃>0,ε₄>0,ε₅>0,ε₆>0,ε₇>0,

其中，C₂为正常数，本实施例中C₂＝10,由此可确定k₁＝85.0826。

步骤1.12：将已确定的参数(k_i,i＝0,…8)带入控制律X、N中并作用于误差系统，在该时变状态反馈控制律作用下的跟踪轨迹参照图3，从中可以看出该控制律有着较好的跟踪效果；跟踪误差参照图4，从中可看出误差系统的状态呈现收敛趋势；跟踪误差的范数参照图5，从中可看出跟踪误差的范数呈现渐近收敛趋势，与设计要求一致；控制输入的范围参照图6，从中可看出实际系统的控制输入在约束范围内。因此，在满足约束的情况下该控制律可使得跟踪误差渐近收敛，能够作为有终端条件MPC的终端控制律。

在所述步骤1中，本发明将轨迹跟踪误差投影到体坐标系下，并利用Lyapunov直接法和反步法设计了使得跟踪误差渐近收敛的控制律，并将此控制律作为有终端MPC的终端控制律。

步骤2：基于MPC的轨迹跟踪控制方法的实施，

步骤2.1：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和参考角速度r_d，利用虚拟水面船模型实现参考轨迹的生成。本实施例中所使用模型的最大前向速度为0.2m/s、最大角速度为0.5236rad/s、最大侧向速度为0.1m/s，本实施例考虑跟踪一圆周轨迹，选取参考角速度r_d＝0.05rad/s，参考前向速度u_d＝0.05m/s，参考轨迹的初始状态为x_d(0)＝[0,0,0,0.05,0,0.05]'，参考轨迹参照图2；设定实际水面船的初始状态为x(0)＝[-0.2，-0.2,0,0,0,0]'。

步骤2.2：设置MPC迭代的时间T/次数N_t、预测时域T_p、采样间隔δ，考虑到实际系统的采样机制，控制输入定义为

系统方程的状态值由四阶-五阶Runge-Kutta算法求解，其中

表示不大于t的最大采样时刻。本实施例中N_t＝274、T_p＝10s、δ＝0.5s。

步骤2.3：设置当前t_k时刻MPC优化问题的目标函数为：

其中，x_e(t；t_k)为t_k时刻对t时刻误差状态的预测，u_e(t；t_k)为t_k时刻对t时刻误差输入的预测，Q为状态误差加权矩阵，R为输入误差加权矩阵，P为终端状态误差加权矩阵，

本实施例中R＝diag(0.001,0.001)，Q_s＝diag(0.01,0.005,0.06,0.001,0.005,0.005)，P_e、Q_e的具体数值分别为：

步骤2.4：设置MPC优化问题的状态约束x_e∈X、输入约束u_e∈U以及终端约束x_e(t_k+T_p)∈Ω(t_k+T_p)。本实施例中所使用模型的最大推力为2N,最大旋转力矩为1.5N·m,由此可确定输入约束，输入约束和终端约束分别为：

步骤2.5：根据模型预测系统未来的动态，求解t_k时刻的MPC约束优化问题

得到t_k时刻预测的最优控制序列，t_k时刻优化问题的具体形式为：

其中，

步骤2.6：将第一个最优控制作用于误差系统，直到新的采样时刻t_k+1到来时，将新的采样状态作为初始状态构建新时刻t_k+1的MPC优化问题，滚动迭代直到跟踪控制过程结束。在模型预测控制的作用下，欠驱动水面船运动轨迹参照图7，从中可看出该控制律有着良好的跟踪性能；跟踪误差参照图8，从中可看出误差状态呈现较快地收敛趋势；跟踪误差的范数参照图9，从中可看出跟踪误差的范数呈现渐近收敛趋势，与设计要求一致；系统控制输入参照图10，从中可看出控制输入皆满足约束。从实施例中可以看出，所设计的基于模型预测控制的欠驱动水面船轨迹跟踪控制方法在满足系统实际约束的情况下以较短的预测时域求解到了保证系统ISS稳定的最优控制律，取得了较好的跟踪效果。

在所述步骤2中，结合系统约束和步骤1设计的终端控制律获取了合适的终端不变集，在MPC跟踪控制律的设计中考虑了实际系统的输入约束和状态约束，在MPC跟踪控制律的设计中考虑了指定的优化指标，并且通过构建有限时域带终端约束的MPC约束优化问题得到了使得欠驱动无人船误差系统稳定的最优控制律。

Claims (1)

1.一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法，其特征在于：第一部分是利用Lyapunov直接法和反步法设计跟踪控制律，从而使得跟踪误差渐进收敛；第二部分是将第一部分求得的控制律作为有终端约束MPC的终端控制律，进而选取合适的终端不变集并结合系统模型、系统约束和优化指标构建MPC约束优化问题；步骤如下：

步骤1：终端控制律的设计及相应参数的选取：

步骤1.1：假设水面船有着良好的对称性且低速航行，忽略动力学模型中惯性矩阵的非对角项和阻尼矩阵的非对角项及非线性项，考虑参考轨迹是由一个虚拟水面船模型产生，则实际水面船运动模型

和虚拟水面船运动模型

可分别表示为：

其中，x＝[x,y,ψ,u,v,r]'为实际水面船的系统状态，x_d＝[x_d,y_d,ψ_d,u_d,v_d,r_d]'为虚拟水面船的系统状态，[x,y,ψ]'为水面船在地面坐标系下的位姿，[u,v,r]'为水面船在载体坐标系下的线速度和角速度，u为实际的控制输入，u_d为参考的控制输入；

步骤1.2：将误差模型

投影到体坐标系下得到

即：

其中，x_e＝[x_e,y_e,ψ_e,u_e,v_e,r_e]'为地面坐标系下的跟踪误差在载体坐标系下的投影；

步骤1.3：设置水面船的模型参数m₁₁、m₂₂、m₃₃、d₁₁、d₂₂、d₃₃；

步骤1.4：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和角速度r_d，利用虚拟水面船模型生成参考轨迹，设定实际水面船的初始状态x(0)；

步骤1.5：选取Lyapunov函数

利用反步法设计可得：在选取合适参数k_i,i＝0,…8的情况下，当实际的控制输入为u＝[X,N]'时，能够使得跟踪误差渐近收敛，X、N的具体形式分别为：

其中，

为虚拟角速度误差，

为虚拟角速度，

为虚拟前向速度误差，

为虚拟前向速度；

步骤1.6：由步骤1.4可知，选取合适的参数是设计控制律使得跟踪误差渐近收敛的关键部分，定义k₀＝m₂₂/(k₃d₂₂-m₂₂)、k₄＝m₂₂/(k₃m₁₁)、k₆＝k₄-d₁₁/m₁₁+C₁、

k₃＝m₂₂/(d₂₂λ)、c＝r_d(1-k₇)ψ_{e_max}m₁₁/d₂₂、

ψ_{e_max}＝maxψ_e，C₁为正常数；

步骤1.7：确定k₇：{k₇|0＜1-k₈-k₇ψ_{e_max}}、确定λ₁₂：{λ₁₂|0＜λ₁₂＜1}、确定ε₃₄₇：{ε₃₄₇|0＜ε₃₄₇}、确定λ₅₆：{λ₅₆|0＜λ₅₆＜1}、确定k₂：{k₂|0＜k₂}；

步骤1.8：确定λ的范围，使得k₈为实数，λ：

步骤1.9：确定k₈的范围，使得满足k₅为实数的条件：

步骤1.10：确定k₅：

步骤1.11：构建优化问题

求解使得k₁最小时的ε₁、ε₂、ε₃、ε₄、ε₅、ε₆、ε₇，即：

s.t.ε₁+ε₂＝ε₁₂,ε₅+ε₆＝ε₅₆,ε₃+ε₄+ε₇＝ε₃₄₇,

ε₁＞0,ε₂＞0,ε₃＞0,ε₄＞0,ε₅＞0,ε₆＞0,ε₇＞0,

其中，C₂为正常数；

步骤1.12：将已确定的参数k_i,i＝0,…8带入步骤1.4所描述的控制律X、N中；将此控制律X、N作用于误差系统，验证该时变状态反馈控制律是否能够使得跟踪误差渐近收敛，是否能够作为MPC的终端控制律；

步骤2：基于MPC的轨迹跟踪控制方法的设计：

步骤2.1：设置参考轨迹，设定参考轨迹的初始状态x_d(0)和角速度r_d，利用虚拟水面船模型生成参考轨迹，设定实际水面船的初始状态为x(0)；

步骤2.2：设置MPC迭代的时间T/次数N_t、预测时域T_p、采样间隔δ；考虑到实际系统的采样机制，控制输入定义为

系统方程的状态值由四阶-五阶Runge-Kutta算法求解，其中

表示不大于t的最大采样时刻；

步骤2.3：设置当前t_k时刻MPC约束优化问题的目标函数为：

其中，T_p为预测时域，x_e(t；t_k)为系统在t_k时刻对t时刻误差状态的预测，u_e(t；t_k)为系统在t_k时刻对t时刻误差输入的预测，Q为误差状态的加权矩阵，R为输入误差的加权矩阵，P为终端误差状态的加权矩阵，其中，

P_e、Q_e的具体形式分别为：

步骤2.4：设置MPC优化问题的状态约束

输入约束

以及终端约束x_e(t_k+T_p)∈Ω(t_k+T_p)；

步骤2.5：根据模型预测系统未来的动态，求解t_k时刻的MPC约束优化问题

得到t_k时刻预测的最优控制序列，t_k时刻优化问题的具体形式为：

其中，

步骤2.6：将第一个最优控制作用于误差系统，直到新的采样时刻t_k+1到来时，将新的采样状态作为初始状态构建新时刻t_k+1的MPC约束优化问题，滚动迭代直到跟踪控制过程结束。

Images