CN108803632A - 基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法 - Google Patents

Abstract

基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，本发明涉及水面无人艇全状态约束轨迹跟踪控制方法。本发明为了解决现有的针对水面无人艇轨迹跟踪控制的控制方法存在未对状态约束和饱和性的问题进行处理，导致控制误差大的问题。本发明包括：一、建立水面无人艇的动力学模型；二：根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统；三：根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律的饱和函数；四：建立水面无人艇的闭环系统；五：采用自适应法处理外界干扰，得到自适应估计误差；六：根据步骤五得到的自适应估计误差，实现对水面无人艇的全状态约束轨迹跟踪控制。本发明用于轨迹跟踪控制领域。

Description

基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制 方法

技术领域

本发明涉及轨迹跟踪控制领域，具体涉及水面无人艇的控制方法。

背景技术

目前人们对海洋开发日益重视，由此水面无人船、遥控无人潜水器、自主式水下潜器等海洋勘测、开发工具不断得到发展。其中水面无人艇作为一种多用途、低成本的无人载具，无论是在军用还是民用领域都有着广阔的应用前景，因而对水面无人艇进行控制方面的研究具有重大意义。

如今随着对水面无人艇的日益重视，已经对其有了多种运动控制方法，如轨迹跟踪控制、路径跟踪控制、镇定控制、编队控制(廖煜雷.无人艇的非线性运动控制方法研究[D].哈尔滨工程大学,2012)等等。但由于在实际情况的复杂性，因此在控制中需要考虑多方面的因素，如动力学模型的不确定性、外界干扰、状态约束、饱和性等等，而目前的轨迹跟踪控制方法中虽然采用了各种技术方法进行提高精度、稳定性、鲁棒性等，但有些对实际情况的考虑不太全面，导致其在实际应用中可能存在一定误差。为此，本发明为水面无人艇提出了一种基于Anti-windup补偿器的全状态约束轨迹跟踪控制方法，能够较全面地处理实际情况。

发明内容

本发明为了解决现有的针对水面无人艇轨迹跟踪控制的控制方法存在未对状态约束和饱和性的问题进行处理，导致控制误差大的问题，而提出基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法。

基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法包括以下步骤：

步骤一、建立水面无人艇的动力学模型；

步骤二：根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统；

步骤三：根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律τ的饱和函数；

步骤四：根据步骤三建立的水面无人艇控制律τ的饱和函数，建立水面无人艇的闭环系统；

步骤五：采用自适应法处理外界干扰H，得到自适应估计误差

步骤六：根据步骤五得到的自适应估计误差确定水面无人艇的控制律τ和自适应律并引入水面无人艇的闭环系统中，实现对水面无人艇的全状态约束轨迹跟踪控制。

本发明的有益效果为：

为了解决水面无人艇的轨迹跟踪控制问题，并结合实际情况的去考虑其中的各个影响因素，本发明算法采用了一种基于Anti-windup补偿器的全状态约束轨迹跟踪控制方法。若是实际的轨迹能满足预先期望的轨迹，以及实际的速度能够满足预先期望的速度，即两者的误差变量能够在约束界限之中，且控制输入能够贴合实际情况，则实现了目标水面无人艇的轨迹跟踪控制。

若是想要实现对水面无人艇的轨迹跟踪控制，除了本发明算法之外，还有基于非对称模型的欠驱动水面无人艇轨迹跟踪控制、基于模型预测控制的轨迹跟踪控制方案。以下将它们与本发明算法进行比较：

基于非对称模型的欠驱动水面无人艇轨迹跟踪控制，此方法针对欠驱动水面无人艇的轨迹跟踪控制，由于其欠驱动性和非完整性，提出了一种基于非对称模型的欠驱动高速水面无人艇轨迹跟踪控制方法(董早鹏.基于非对称模型的欠驱动USV运动控制技术研究[D].哈尔滨工程大学,2016)，并在其中考虑了海流干扰的情况。此方法还解决了角速度持续激励问题，并且通用性更强。但是与本发明算法相比，这基于非对称模型的方法完全没有考虑状态约束、执行机构的饱和性这些问题，因而导致其在实际问题中可能出现一定偏差。

基于模型预测控制的轨迹跟踪控制，此方法以最优化问题和最优控制方法为基础，设计了一种模型预测控制(牟鹏程.水面无人船轨迹跟踪控制方法研究.哈尔滨工程大学,2013)，从而解决了水面无人艇的轨迹跟踪问题。在大多数的约束或者干扰的初始条件下，并分别考虑了线性阻力模型与非线性阻力模型，其都能够实现开环的最优性能。但其同样存在着缺点，一个为控制器要消耗大量的计算时间，另一个则是考虑的情况不够全面，没有对模型的不确定性、执行机构的饱和性等问题进行处理。

本发明仿真结果如图所示，从图1和图2中可以看出x₁和x₂在初始状态时会与期望轨迹存在一定偏差，但其很快就可以追踪到期望轨迹且精确度较高；从图3和图4中可看出z₁,z₂虽然在初始状态离0较远，但很快就能收敛到0，并且能够保证误差变量全程都在约束界限之内。而其相应的控制输出则如图5所示，可见其实际控制输出基本保持在-500～500的范围内。

附图说明

图1为x₁与x₁₁轨迹比较图；

图2为x₁与x₁₂轨迹比较图；

图3为x₁与x₁₃轨迹比较图；

图4为x₂与x₂₁轨迹比较图；

图5为x₂与x₂₂轨迹比较图；

图6为x₂与x₂₃轨迹比较图；

图7为追踪误差z₁₁轨迹图；

图8为追踪误差z₁₂轨迹图；

图9为追踪误差z₁₃轨迹图；

图10为追踪误差z₂₁轨迹图；

图11为追踪误差z₂₂轨迹图；

图12为追踪误差z₂₃轨迹图；

图13为控制输入sat(τ₁)图；

图14为控制输入sat(τ₂)图；

图15为控制输入sat(τ₃)图。

具体实施方式

具体实施方式一：基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法包括以下步骤：

在说明本实施方式之前，首先对相关参数进行定义：

η_x,η_y,η_ψ——目标水面无人艇相对于地面坐标系的位置分量和航向；

v_x,ν_y,ν_ψ——目标水面无人艇的纵荡、横荡和艏摇速度；

M——对称正定惯性矩阵；

C(ν)——向心力和科氏力矩阵；

D(ν)——阻尼矩阵；

g(η)——由重力、海流和浮力引起的恢复力；

H——未知干扰；

J(η)——从随体坐标系到地面坐标系的非奇异转换矩阵；

τ——期望控制输入；

sat(τ)——实际控制输入；

Δτ——实际控制输入和期望控制输入之间的差值；

x_d——位置的期望轨迹；

x_2d——速度的期望轨迹；

z₁——关于位置的误差变量；

z₂——关于速度的误差变量；

α——虚拟控制函数；

V₁,V₂——Lyapunov函数；

b₁——关于z₁的约束界限；

b₂——关于z₂的约束界限；

b_a,b_c,A₀,A₁,B₀——正向量；

k₁,k₂——控制增益；

θ——Δτ的范数上界；

h_m——未知干扰的上界；

——自适应的估计误差；

——自适应的估计值；

κ——自适应的真实值；

ε,γ₀,γ₁——大于零的常数；

C₁,C₂——对角线元素均为正常数的对角阵；

ζ₁,ζ₂——饱和补偿辅助系统的输出。

步骤一、建立水面无人艇的动力学模型；

步骤二：根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统；

步骤三：根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律τ的饱和函数；

步骤四：根据步骤三建立的水面无人艇控制律τ的饱和函数，建立水面无人艇的闭环系统；

步骤五：采用自适应法处理外界干扰H，得到自适应估计误差

步骤六：根据步骤五得到的自适应估计误差确定水面无人艇的控制律τ和自适应律并引入水面无人艇的闭环系统中，实现对水面无人艇的全状态约束轨迹跟踪控制。

理论基础

(1)水面无人艇的动力学模型

单点系泊系统的的运动和状态变量的定义和测量由地面坐标系和随体坐标系决定。随体坐标系o-xy的坐标原点o位于水面无人艇的重心处，x轴沿中纵轴线从船尾指向船首，y轴指向左舷；地面坐标系O-XY的坐标原点O位于系泊线和系泊终端的连接处，X、Y轴与随体坐标系的x、y轴在同一平面内。

3自由度并且是多输入多输出的水面无人艇动力学模型如下：

其中包括大地坐标系下的位置(η_x,η_y)和航向角(η_ψ)。包括纵荡的速度v_x、横荡的速度v_y、艏摇的速度v_ψ。为控制输入。是一个对称正定的惯性矩阵，代表向心力和科氏力矩阵，为阻尼矩阵，g(η)代表由重力、海流和浮力引起的恢复力，H为未知干扰。J(η)为从随体坐标系到地面坐标系的非奇异转换矩阵，其定义如下：

(2)饱和性闭环系统的建立

令x₁＝η,x₂＝ν，则水面无人艇的动力学模型式(1)可以表示如下：

由于实际应用中，执行机构能够提供的控制力和控制力矩通常是有限的，所以在控制器设计时有必要考虑输入饱和对控制性能的影响。饱和函数sat(τ)一般可以表达为如下形式：

sat(τ)＝[sat(τ₁),sat(τ₂),sat(τ₃)]^T (13)

其中，sat(τ_i)＝sgn(τ_i)min{τ_imax,|τ_i|},i＝1,2,3，τ_imax为饱和函数幅值。

很明显，期望控制输入τ有可能比执行机构实际上能提供的控制输入sat(τ)大。所以，在期望控制输入和实际控制输入之间会存在一个差值Δτ，表示为：

Δτ＝sat(τ)-τ (14)

假设1：对控制输入饱和形式式(13)和式(14)，存在一个非负实数θ，满足以下条件：

||Δτ||≤θ (15)

注1：假设1的成立有其合理性。因为当控制输入饱和情况出现时，期望控制输入和实际控制输入之间的差值Δτ若是无穷大的，则系统将不可控。

为了消除输入饱和对系统的影响，设计如下的饱和补偿辅助系统：

其中C₁、C₂为对角线元素均为正常数的对角阵，ζ₁＝[ζ₁₁,ζ₁₂,ζ₁₃]^T，ζ₂＝[ζ₂₁,ζ₂₂,ζ₂₃]^T是辅助系统的输出。则可以重新定义误差变量z₁和z₂为：

可得闭环系统为：

假设2：针对式(16)所描述的考虑输入饱和的实际系统，应存在一个合理的实际控制输入sat(τ)以满足跟踪控制目标。

(3)自适应方法的处理

与之前处理干扰上界未知情况的方法相同，防止引入符号函数造成不必要的抖振，选用自适应方法对未知干扰上界d_m的平方κ进行估计。

取κ＝h_m ²，并且本发明定义自适应估计误差为且存在

(4)障碍Lyapunov函数的建立

对于z₁，选取障碍Lyapunov函数为:

其中b₁＝b_a-A₀＝[b₁₁,b₁₂,b₁₃]^T为z₁的约束界限，可以看出，V₁正定且在|z_1i|≤b_1i区域内连续。

对V₁关于时间求导，可得：

设计虚拟控制函数α为：

假设3：对于任意正向量b_a，存在正向量A₁＝[A₁₁,A₁₂,A₁₃]^T，A₂＝[A₂₁,A₂₂,A₂₃]^T，S₀＝[S₀₁,S₀₂,S₀₃]^T,满足A₁≤S₀≤b_a，也就是期望轨迹x_d(t)以及其关于时间的导数均满足-A₁≤x_d(t)≤A₁，

假设4：矩阵J(x₁)已知，并存在一个边界。根据假设3，可进一步假设存在正向量B₀＝[B₀₁,B₀₂,B₀₃]^T以及A₀＝[A₀₁,A₀₂,A₀₃]^T，满足A₀≤B₀≤b_c，也就是α(t)满足-A₀≤α(t)≤A₀。

假设4表明了α是连续有界的。

假设5：未知干扰H有界，即时，存在常数使得||H||≤h_m。

将式(18)和式(21)代入式(20)中，可得：

其中

引入自适应方法，取Lyapunov函数为：

其中γ₀为大于零的常数。

对上式求导，可得：

(5)控制律和自适应律的确立

根据伪逆条件，可得：

设计控制律和自适应律形式如下：

其中，ε,γ₁均为大于零的常数，k₂是一个控制增益。

将控制律式(26)和自适应律式(27)代入式(24)内，并考虑式(16)可得：

因为Young’s不等式，可得：

将式(29)代入式(28)中，可得：

其中

引理1(Zhao Z,He W,Ge S S.Adaptive Neural Network Control of a FullyActuated Marine Surface Vessel With Multiple Output Constraints[J].IEEETransactions on Control Systems Technology,2014,22(4):1536-1543)：对任意由正常数组成的向量若|x|＜k_b则不等式成立。

由引理1，式(30)可进一步化为：

其中

引理2：对于任意正常数b_i,i＝1,2,...,n，假设 为开集。考虑如下系统：

其中η:＝[w,z]∈N为系统状态，在t上分段连续，且关于z_i满足局部Lipschitz条件，同时在上关于t一致连续。假设存在两个正定函数和在各自的区域内连续且可导，则有：

当|z_i|→b_i时，V_i(z_i)→∞

γ₁(||w||)≤U(w)≤γ₂(||w||)

其中γ₁,γ₂为K_∞函数。令z_i的初值z_i(0)在集合z_i(0)∈(-b_i,b_i)内。若不等式：

成立，那么z_i(t)在上始终在集合z_i∈(-b_i,b_i)内。

引理3(Ren B,Ge S S,Tee K P,et al.Adaptive Neural Control for OutputFeedback Nonlinear Systems Using a Barrier Lyapunov Function[J].IEEETransactions on Neural Networks,2010,21(8):1339)：对于有界初始条件，若存在一个连续正定的Lyapunov函数V(x)，且实数为K_∞函数，c₁,c₂为正常数，满足：

(1)π₁(||x||)≤V(x)≤π₂(||x||)

(2)

则系统x(t)一致有界。

根据引理2，如果初值条件满足z_1i(0)∈Ω_z1，z_2i(0)∈Ω_z2，可以得到有|z_1i|＜b_1i，|z_2i|＜b_2i。同时根据引理3，可知均有界。设||ζ_1i||≤δ_1i，||ζ_2i||≤δ_2i。则由x₁＝z₁+ζ₁+x_d，x₂＝z₂+ζ₂+α可以得到

将式(31)两端积分，得：

由V₂定义，有

对式(33)两边求指数次幂，得：

同理，对z₂，有所以可以通过选取适当的控制器参数，使得z₁,z₂最终收敛到0的任意小的邻域内。

在这种情况下，显然可以得到在实现了轨迹跟踪误差的全状态约束的同时，实际位置轨迹x₁能够满足期望轨迹x_d，实际速度轨迹x₂能够满足期望轨迹x_2d，这也意味着实现了对水面无人艇的轨迹跟踪控制。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立水面无人艇的动力学模型的具体过程为：

随体坐标系o-xy的坐标原点o位于水面无人艇的重心处，x轴沿中纵轴线从船尾指向船首，y轴指向左舷；地面坐标系O-XY的坐标原点O位于系泊线和系泊终端的连接处，X、Y轴与随体坐标系的x、y轴在同一平面内；

3自由度、多输入多输出的水面无人艇的动力学模型如下：

其中空心表示实数域；(η_x,η_y)表示地面坐标系下的位置，η_ψ表示地面坐标系下的航向角；为η的一阶导数；为无人舰的速度向量，其中，v_x为纵荡的速度，v_y为横荡的速度，v_ψ为艏摇的速度；为期望控制输入；是一个对称正定的惯性矩阵，代表向心力和科氏力矩阵，为阻尼矩阵，g(η)代表由重力、海流和浮力引起的恢复力，H为未知外界干扰；J(η)为从随体坐标系到地面坐标系的非奇异转换矩阵，其定义如下：

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统的具体过程为：

本发明采用抗饱和补偿器对饱和函数进行处理，设计饱和补偿辅助系统：

其中M为对称正定惯性矩阵，C₁,C₂为对角线元素均为正常数的对角阵，ζ₁,ζ₂为饱和补偿辅助系统的输出，J为从随体坐标系到地面坐标系的非奇异转换矩阵，Δτ为期望控制输入τ与实际控制输入sat(τ)之间存在的差值；

对位置的误差变量z₁和速度的误差变量z₂进行重新定义：

其中x₁＝(x₁₁，x₁₂，x₁₃)表示水面无人艇的位置，(x₁₁，x₁₂)表示地面坐标系下的位置，x₁₃表示地面坐标系下的航向角，x₂表示水面无人艇速度，x_d为位置的期望轨迹，α为虚拟控制函数。并将误差变量z₁和z₂的新形式代入进动力学模型中进行计算。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三中根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律τ的饱和函数的具体过程为：

由于水面无人艇上的执行机构在实际情况中，其能提供的控制力和控制力矩是有限的，为此选择引入饱和函数这一个概念，从而可以在对控制器设计时考虑输入饱和对控制性能的影响。饱和函数sat(τ)一般可以表达为如下形式：

sat(τ)＝[sat(τ₁),sat(τ₂),sat(τ₃)]^T

其中，sat(τ_i)＝sgn(τ_i)min{τ_imax,|τ_i|},i＝1,2,3，sgn(τ_i)为关于τ_i的符号函数，τ_imax为饱和函数幅值，Δτ表示为：

Δτ＝sat(τ)-τ

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤四中根据步骤三建立的水面无人艇控制律τ的饱和函数，建立水面无人艇的闭环系统的具体过程为：

对称正定的惯性矩阵M，向心力和科氏力矩阵C(ν)，以及阻尼矩阵D(ν)如下：

其中v_x,ν_y,ν_ψ分别为目标水面无人艇的纵荡、横荡和艏摇速度，m为目标水面无人艇质量，X_du为纵向力关于x轴方向运动的加速度系数，Y_dv为横向力关于y轴方向运动的加速度系数，x_g为重心纵向位置，Y_dr为横向力关于z轴方向转动的加速度系数，N_dr为偏航力矩关于z轴方向转动的加速度系数，X_u为纵向力关于x轴方向运动的速度系数，X_uu为纵向力关于x轴方向运动的二阶速度系数，X_uuu为在x轴方向的关于x轴方向运动的三阶速度系数，Y_v为横向力关于y轴方向运动的速度系数，Y_vv为横向力关于y轴方向运动的二阶速度系数，Y_r为横向力关于z轴方向转动的速度系数，Y_rr为横向力关于z轴方向转动的二阶速度系数，Y_rv为横向力关于z轴方向转动和y轴方向运动的耦合系数，Y_vr为横向力关于y轴方向运动和z轴方向转动的耦合系数，N_v为偏航力矩关于y轴方向运动的速度系数，N_vv为偏航力矩关于y轴方向运动的二阶速度系数，N_r为偏航力矩关于z轴方向转动的速度系数，N_rr为偏航力矩关于z轴方向转动的二阶速度系数，N_rv为偏航力矩关于z轴方向转动和y轴方向运动的耦合系数，N_vr为偏航力矩关于y轴方向运动和z轴方向转动的耦合系数，x_g为目标水面无人艇重心的纵向位置，I_z为目标水面无人艇关于z轴的惯性矩；根据目标水面无人艇的自身性质和水动力参数，确定上述的M、C(ν)、D(ν)；

确定从随体坐标系到地面坐标系的从随体坐标系到地面坐标系的非奇异转换矩阵J(η)的形式：

其中η_ψ为目标水面无人艇相对于地面坐标系的航向角；

并根据相应情况建立恢复力g(η)和未知干扰H；根据目标船舶的重量、重心、型线、排水体积等等性质，可以通过相关实验测得恢复力；根据目标船舶和外界环境(海浪、风等)，可以大致确定干扰的数量级；

由实际需求，建立期望轨迹x_d＝[x_d1(t),x_d2(t),x_d3(t)]^T，其中x_d1(t)表示目标在地面坐标系下关于x方向的期望轨迹，x_d2(t)表示目标在地面坐标系下关于y方向的期望轨迹，x_d3(t)表示目标在地面坐标系下关于航向角的期望轨迹；

3自由度并且是多输入多输出的水面无人艇动力学模型如下：

其中目表示标水面无人艇相对于地面坐标系的位置，包括大地坐标系下的位置(η_x,η_y)和航向角(η_ψ)；为目标水面无人艇的速度，包括纵荡的速度v_x、横荡的速度v_y、艏摇的速度v_ψ，表示实数域；

令x₁＝η,x₂＝ν，建立水面无人艇的动力学模型：

其中H为未知外界干扰，是一个对称正定的惯性矩阵，代表向心力和科氏力矩阵，为阻尼矩阵，g(x₁)代表由重力、海流和浮力引起的恢复力，J(x₁)为从随体坐标系到地面坐标系的非奇异转换矩阵，其定义如下：

根据式(1)、式(2)、式(3)和式(4)，建立起所需的闭环系统：

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤五中采用自适应法处理外界干扰，得到自适应估计误差的具体过程为：

对于外界的随机干扰，本方法采用自适应法对其进行处理。假定未知干扰H有界，即时，存在常数使得||H||≤h_m；采用自适应方法对未知干扰上界h_m进行估计，取κ＝h_m ²，以及定义和 为自适应的估计误差，为自适应的估计值，κ为自适应的真实值；

将自适应估计误差引入在第二次障碍李雅普诺夫函数V₂中进行求解，并通过设计合适的控制律和自适应律进行处理。

现有研究中大多采用障碍李雅普诺夫函数来处理状态约束的问题，而它其实是基于势函数思想建立的一种状态约束控制方法，它可以通过保证在闭环系统中障碍李雅普诺夫函数的有界性来确保状态不会超过约束条件。它可以设计成对称形式或非对称的形式，其表达式主要为：其中b_i为约束界限，z_i为存在约束的状态向量。

在本发明中为了进行状态约束，因此采用障碍李雅普诺夫函数的形式进行设计，其具体形式为：

其中V₁为第一次李雅普诺夫函数，V₂为第二次李雅普诺夫函数，γ₀为自适应参数(大于0的常数)；b_1i为约束界限b₁的第i元素，b_2i约束界限b₂的第i元素，z_1i为误差变量z₁的第i元素，z_2i为误差变量z₂的第i元素。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述步骤六中确定水面无人艇的控制律τ和自适应律的具体过程为：

对于z₁,z₂，分别确定z₁的约束边界b₁，z₂的约束边界b₂：

b₁＝b_a-A₀＝[b₁₁,b₁₂,b₁₃]^T (8)

b₂＝b_c-A₁＝[b₂₁,b₂₂,b₂₃]^T (9)

其中b_a,b_c,A₀,A₁为正向量；b₁₁为约束界限b₁的第1元素，b₁₂为约束界限b₁的第2元素，b₁₃为约束界限b₁的第3元素，b₂₁约束界限b₂的第1元素，b₂₂约束界限b₂的第2元素，b₂₃约束界限b₂的第3元素；

综合考虑，确定控制增益k₁＝diag[k₁₁,k₁₂,k₁₃],k₂＝diag[k₂₁,k₂₂,k₂₃]；

合理的对自适应参数ε,γ₀,γ₁进行设定；

根据实际情况，确定饱和函数sat(τ)＝[sat(τ₁),sat(τ₂),sat(τ₃)]^T；

最终确立控制律τ和自适应律

其中J_i为从随体坐标系到地面坐标系的非奇异转换矩阵的第i个元素，ε为自适应参数，γ₀为自适应参数，γ₁为自适应参数。

其它步骤及参数与具体实施方式一至六之一相同。

实施例一：

本发明采用的是Cybership II的模型船，这是一个由挪威科技大学的海洋控制实验室建造的测量船的1:70的复制品(K.P.Tee,S.S.Ge.Control of fully actuated oceansurface vessels using a class of feedforward approximators.IEEE Transactionson Control Systems Technology,2006,14(4))。

选择的期望轨迹如下：

x_1d(t)＝[x_1xd(t),x_1yd(t),x_1ψd(t)]^T

对称正定的惯性矩阵M，向心力和科氏力矩阵C(ν)，以及阻尼矩阵D(ν)如下：

本发明中相应的水动力参数如下：m＝23.8，I_z＝1.76，x_g＝0.046，X_u＝-0.7225，X_uu＝-1.3274，X_uuu＝-5.8664，Y_v＝-0.8612，Y_vv＝-36.2823，Y_r＝0.1079，N_v＝0.1052，N_vv＝5.0437，X_du＝-2.0，Y_dv＝-10.0，Y_dr＝-0，N_dv＝0，N_dr＝-1.0，Y_rv＝2，Y_vr＝1，Y_rr＝3，N_rv＝5，N_r＝4，N_vr＝0.5，N_rr＝0.8。

其相应的状态初始值为x₁(0)＝[0.01,1,-0.012]^T,x₂(0)＝[0.08,0.08,-0.1]^T；控制参数K₁＝diag[10,10,5],K₂＝diag[0.4,40,0.001]；约束界限b₁＝[4.6,0.57,1.6]^T,b₂＝[5.8,4.5,8]^T；自适应参数γ₀＝0.1,γ₁＝0.01,ε＝0.5；饱和补偿辅助系数C₁＝diag[20,20,20]，C₂＝diag[20,20,20]；饱和限制τ_1max＝300、τ_2max＝350、τ_3max＝350。

其仿真结果如图所示，从图1和图2中可以看出x₁和x₂在初始状态时会与期望轨迹存在一定偏差，但其很快就可以追踪到期望轨迹且精确度较高；从图3和图4中可看出z₁,z₂虽然在初始状态离0较远，但很快就能收敛到0，并且能够保证误差变量全程都在约束界限之内。而其相应的控制输出则如图5所示，可见其实际控制输出基本保持在-500～500的范围内。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (7)

1.基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法包括以下步骤：

步骤一、建立水面无人艇的动力学模型；

步骤二：根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统；

步骤三：根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律τ的饱和函数；

步骤四：根据步骤三建立的水面无人艇控制律τ的饱和函数，建立水面无人艇的闭环系统；

步骤五：采用自适应法处理外界干扰H，得到自适应估计误差

步骤六：根据步骤五得到的自适应估计误差确定水面无人艇的控制律τ和自适应律实现对水面无人艇的全状态约束轨迹跟踪控制。

2.根据权利要求1所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤一中建立水面无人艇的动力学模型的具体过程为：

随体坐标系o-xy的坐标原点o位于水面无人艇的重心处，x轴沿中纵轴线从船尾指向船首，y轴指向左舷；地面坐标系O-XY的坐标原点O位于系泊线和系泊终端的连接处，X、Y轴与随体坐标系的x、y轴在同一平面内；

3自由度、多输入多输出的水面无人艇的动力学模型如下：

其中空心表示实数域；(η_x,η_y)表示地面坐标系下的位置，η_ψ表示地面坐标系下的航向角；为η的一阶导数；为无人舰的速度向量，其中，v_x为纵荡的速度，v_y为横荡的速度，v_ψ为艏摇的速度；为期望控制输入；是一个对称正定的惯性矩阵，代表向心力和科氏力矩阵，为阻尼矩阵，g(η)代表由重力、海流和浮力引起的恢复力，H为外界干扰；J(η)为从随体坐标系到地面坐标系的非奇异转换矩阵。

3.根据权利要求2所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤二中根据步骤一建立的水面无人艇的动力学模型，设计饱和补偿辅助系统的具体过程为：

采用抗饱和补偿器对饱和函数进行处理，设计饱和补偿辅助系统：

其中M为对称正定惯性矩阵，C₁,C₂为对角线元素均为正常数的对角阵，ζ₁,ζ₂为饱和补偿辅助系统的输出，J为从随体坐标系到地面坐标系的非奇异转换矩阵，Δτ为期望控制输入τ与实际控制输入sat(τ)之间存在的差值；

对位置的误差变量z₁和速度的误差变量z₂进行重新定义：

其中x₁表示水面无人艇的位置，x₂表示水面无人艇速度，x_d为位置的期望轨迹，α为虚拟控制函数。

4.根据权利要求3所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤三中根据步骤二设计的饱和补偿辅助系统，建立水面无人艇控制律τ的饱和函数的具体过程为：

饱和函数sat(τ)表达为如下形式：

sat(τ)＝[sat(τ₁),sat(τ₂),sat(τ₃)]^T

其中，sat(τ_i)＝sgn(τ_i)min{τ_imax,|τ_i|},i＝1,2,3，sgn(τ_i)为关于τ_i的符号函数，τ_imax为饱和函数幅值，Δτ表示为：

Δτ＝sat(τ)-τ。

5.根据权利要求4所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤四中根据步骤三建立的水面无人艇控制律τ的饱和函数，建立水面无人艇的闭环系统的具体过程为：

对称正定的惯性矩阵M，向心力和科氏力矩阵C(ν)，以及阻尼矩阵D(ν)如下：

其中v_x,ν_y,ν_ψ分别为目标水面无人艇的纵荡、横荡和艏摇速度，m为目标水面无人艇质量，X_du为纵向力关于x轴方向运动的加速度系数，Y_dv为横向力关于y轴方向运动的加速度系数，x_g为重心纵向位置，Y_dr为横向力关于z轴方向转动的加速度系数，N_dr为偏航力矩关于z轴方向转动的加速度系数，X_u为纵向力关于x轴方向运动的速度系数，X_uu为纵向力关于x轴方向运动的二阶速度系数，X_uuu为在x轴方向的关于x轴方向运动的三阶速度系数，Y_v为横向力关于y轴方向运动的速度系数，Y_vv为横向力关于y轴方向运动的二阶速度系数，Y_r为横向力关于z轴方向转动的速度系数，Y_rr为横向力关于z轴方向转动的二阶速度系数，Y_rv为横向力关于z轴方向转动和y轴方向运动的耦合系数，Y_vr为横向力关于y轴方向运动和z轴方向转动的耦合系数，N_v为偏航力矩关于y轴方向运动的速度系数，N_vv为偏航力矩关于y轴方向运动的二阶速度系数，N_r为偏航力矩关于z轴方向转动的速度系数，N_rr为偏航力矩关于z轴方向转动的二阶速度系数，N_rv为偏航力矩关于z轴方向转动和y轴方向运动的耦合系数，N_vr为偏航力矩关于y轴方向运动和z轴方向转动的耦合系数，x_g为目标水面无人艇重心的纵向位置，I_z为目标水面无人艇关于z轴的惯性矩；

确定从随体坐标系到地面坐标系的从随体坐标系到地面坐标系的非奇异转换矩阵J(η)的形式：

其中η_ψ为目标水面无人艇相对于地面坐标系的航向角；

建立期望轨迹x_d＝[x_d1(t),x_d2(t),x_d3(t)]^T，其中x_d1(t)表示目标在地面坐标系下关于x方向的期望轨迹，x_d2(t)表示目标在地面坐标系下关于y方向的期望轨迹，x_d3(t)表示目标在地面坐标系下关于航向角的期望轨迹；

3自由度并且是多输入多输出的水面无人艇动力学模型如下：

令x₁＝η,x₂＝ν，建立水面无人艇的动力学模型：

其中代表向心力和科氏力矩阵，为阻尼矩阵，g(x₁)代表由重力、海流和浮力引起的恢复力，J(x₁)为从随体坐标系到地面坐标系的非奇异转换矩阵，其定义如下：

根据式(1)、式(2)、式(3)和式(4)，建立闭环系统：

6.根据权利要求5所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤五中采用自适应法处理外界干扰，得到自适应估计误差的具体过程为：

假定干扰H有界，即时，存在常数使得||H||≤h_m；采用自适应方法对干扰上界h_m进行估计，取κ＝h_m ²，以及定义和 为自适应的估计误差，为自适应的估计值，κ为自适应的真实值。

7.根据权利要求6所述基于饱和补偿技术的水面无人艇全状态约束轨迹跟踪控制方法，其特征在于：所述步骤六中确定水面无人艇的控制律τ和自适应律的具体过程为：

对于z₁,z₂，分别确定z₁的约束边界b₁，z₂的约束边界b₂：

b₁＝b_a-A₀＝[b₁₁,b₁₂,b₁₃]^T (6)

b₂＝b_c-A₁＝[b₂₁,b₂₂,b₂₃]^T (7)

其中b_a,b_c,A₀,A₁为正向量；b₁₁为约束界限b₁的第1元素，b₁₂为约束界限b₁的第2元素，b₁₃为约束界限b₁的第3元素，b₂₁约束界限b₂的第1元素，b₂₂约束界限b₂的第2元素，b₂₃约束界限b₂的第3元素；

确定控制增益k₁＝diag[k₁₁,k₁₂,k₁₃],k₂＝diag[k₂₁,k₂₂,k₂₃]；

对自适应参数ε,γ₀,γ₁进行设定；

根据饱和函数sat(τ)＝[sat(τ₁),sat(τ₂),sat(τ₃)]^T；

最终确立控制律τ和自适应律

其中J_i为从随体坐标系到地面坐标系的非奇异转换矩阵的第i个元素，ε为自适应参数，γ₀为自适应参数，γ₁为自适应参数。