[发明专利]全局一致渐进路径跟踪引导控制方法

权利要求书

1.一种全局一致渐进路径跟踪引导控制方法，其特征在于，包括：

读入当前船舶姿态、当前航路点和下一个航路点；

判断是否达到下一个路航点；

若是，则调用引导系统模块算法，更新航路点坐标和目标路径，计算实时期望航向角；

若否，则调用引导系统模块算法，保持当前目标路径，计算实时期望航向角；

调用控制系统模块算法，计算船舶相应的转艏力矩和纵向力；

所述计算实时期望航向角包括步骤：

(1)在引导系统中，对轨迹追踪横向误差进行积分并自适应改变积分增益；

(2)利用(1)实时在线计算船舶期望航向角度，同时对船舶进行侧滑角补偿：

假设船舶的操纵人员通过人机界面或者其他输入设备确定一系列航路点{p₁(x₁，y₁)......p_i(x_i，y_i)......p_n(x_n，y_n)，2≤i≤n}，要求船舶以某种角度航行通过上述航路点或者以尽可能接近上述航路点时再转向，同时，两个航路点之间要求船舶以直线的方式航行；

首先，定义追踪误差σ：若船舶当前位置是p(x，y)，那么有其中，s(t)是纵向追踪误差，e(t)＝-(x-x_i)sinψ+(y-y_i)cosψ是横向追踪误差，无论船舶偏离目标航线，还是恰好航行在目标航线上，都要求利用视野线法进行引导控制；在传统的视野线法(line-of-sight，简称LOS)是：其中e是横向追踪误差，Δ是根据船舶长度而设定的预设值；此时，要求船舶的航向能够与目标航线相一致，为此，在常值扰动下对船舶的期望艏向角度进行补偿：ψ_d＝ψ_k+ψ_LOS-β，β＝χ-ψ＝atan2(w，u)，其中ψ_k是目标航线的角度，u是船舶纵向速度，w是船舶横荡速度，当在缺少测量横荡速度w的条件下，利用改进的视野线法-自适应变积分增益视野线法进行侧滑角补偿，即该方法表达式：根据前者可以看出积分增益是根据纵向追踪误差自适应调节增益值的，则有期望航向角是：χ_d＝ψ_k+ψ_mILOS，其中P_k：第k个航路点，ψ：艏向角，K_p＝1/Δ＞0，p：p＞0是一个设计参数，χ：船艏合成速度w与正北向的交角，y_e：偏差e的积分项。

2.根据权利要求1所述的全局一致渐进路径跟踪引导控制方法，其特征在于，所述计算船舶相应的转艏力矩和纵向力包括步骤：

在将期望艏向角输入控制系统前，要求信号ψ_d通过三个串联的低通滤波器以得到平滑可微的期望艏向角ψ_d和高阶微分与设船舶当前姿态为η＝[x，y，ψ]^T∈R²×[-π，π]，欠驱动船舶的三个自由度模型为：

船舶的船体固定坐标系中速度向量是v＝[u，w，r]^T∈R³；N(v)：阻尼系数

进行坐标转换：其中是映射向量，是后面设计到的镇定函数； 由于和h^TRv＝h^Tv，对z₀求得时间导数： ；

设计第一个控制李雅普诺夫函数(CLF)：对V₁求时间导数：将镇定函数α₃进行关于z₀的线性化，即可得：其中k₀＞0； 所以，

为了镇定第一个控制李雅普诺夫函数和自适应估计外部扰动，设计第二个控制李雅普诺夫函数：其中估计误差是对扰动b的估计并且Γ＝Γ^T∈R^3×3＞0是设计正定矩阵；

由和船舶三自由度数学模型，可得到V₂的时间导数是：

将v＝z+α和代入上式可以得到：

通过选择使得该方程右边第三项为0；

选择控制输入：

或者

选择τ₍₁₎时，

选择τ₍₂₎时，

其中，K＝diag(k₁，k₂，k₃)＞0是另一个设计正定矩阵；

至此，由于V₂正定和控制输入τ₍₁₎或τ₍₂₎，负定，所以平衡点是全局一直渐进稳定；

为了实现船舶能够按照期望的速度航向，假设镇定函数α₁＝u_d，u_d＞0是所期望的船舶纵向前进速度，由欠驱动船舶的执行器物理机构，可以知道转艏力矩和横向力具有比例关系，为了使得船舶的控制输入能够适应此物理关系，利用τ₍₁₎＝[τ₁，τ₂，τ₃]中τ₂＝στ₃的关系可以得到：

那么通过对动态变量积分求解可以得到镇定函数项α₂，此时镇定函数向量α＝[α₁，α₂，α₃]的三个参数已经确定；

对横向追踪误差进行求时间导数：

该表达式可以变换表达形式：利用三角函数的变换关系，得到：

设：纵向速度与横向速度的合成速度

其中f_e(e，t)＝Usin(ψ_d-ψ_k+β)，是上文所提到的控制系统；

显然，对于无强制系统通过设计CLF：V_e＝(1/2)e²可以得出是全局一致渐进有界的，而驱动项g_e(e，ζ)显然是以上文的控制系统ζ-子系统为输入的；根据控制领域已有定理，可以知道完整系统(e，ζ)-系统是全局一致渐进稳定的；至此，在假设可以测量纵向速度u的前提下，对侧滑角β进行了补偿，并得到了引导系统和控制系统串联结合而成的全局一致渐进追踪算法，在此基础之上，如果不能直接补偿侧滑角度β，通过改进传统的LOS引导算法，得到自适应变积分增益的积分型LOS引导算法：其中应该特别指出的是积分项y_e的增益是关于e的自然指数函数e^-ρ|e|，ρ＞0和K_p＝1/Δ＞0；自然指数函数e^-ρ|e|占居十分重要的地位，其随着e的绝对值变大而呈指数减小，这意味着在船舶距离目标航线较远时能够使得引导系统算法中的积分项变为次要地位而比例项却占有重要作用以引导船舶在自身性能容许下尽快航向目标航线，从而避免了积分所引起的船舶执行器饱和效应；当船舶已经保持在目标航线时，即e＝0，积分增益消失变为1但保留着我们所需要的积分运算，以此实现了船舶在目标航线航行时仍然具有“侧滑角补偿”相类似的效果；

为了说明该自适应变积分增益算法的稳定性，我们设计第三个CLF：

因为那么将控制李雅普诺夫函数V_mILOS进行时间微分，得：所以可以得出(e)--系统在e＝0处是全局一致渐进和全局局部指数稳定的平衡点，

M：惯性矩阵

m₂₂：惯性矩阵M中的元素

m₂₃：惯性矩阵M中的元素

m₃₂：惯性矩阵M中的元素

m₃₃：惯性矩阵M中的元素

N(v)是与速度相关的阻尼系数阵。