CN111580523A - 一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法 - Google Patents

Abstract

本发明属于无人艇路径跟踪控制技术领域，具体涉及一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法。本发明设计的ELOS导引算法采用侧滑角观测器对时变未知侧滑角进行估计和补偿，避免了传统导引算法在侧滑角未知条件下存在路径跟踪误差较大的问题，减少了无人艇上测量装置的数量，经济实用，提高了系统的冗余性。本发明设计的艏向及纵向速度动态面自抗扰控制器能够使无人艇跟踪期望路径时有较快的误差收敛速度和较高的跟踪精度；设计的跟踪微分器对艏向角和纵向速度的期望值有较快的跟踪速度；采用的线性扩张状态观测器能够很好地估计纵向风力和转艏风力矩；采用动态面控制避免了反步法存在的“微分爆炸”问题。

Description

一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法

技术领域

本发明属于无人艇路径跟踪控制技术领域，具体涉及一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法。

背景技术

近年来，水面无人艇是国内外智能化海洋装备研究领域的热点。对于在海洋中航行的无人艇，保证其在安全航行的前提下沿着期望航线航行是无人艇执行各项任务的前提条件，因此，研究无人艇路径跟踪控制十分重要。由于无人艇在时变海风扰动作用下航行时会出现时变侧滑角，当侧滑角无法给出或者给出的值不准确时，由于无人艇横荡方向上没有直接的控制力，因此侧滑角可能会引起很大的路径跟踪误差，为了保证路径跟踪性能，需要对未知的侧滑角进行估计和补偿，从而设计适用于侧滑角未知条件的路径跟踪导引算法，进一步设计高性能的路径跟踪自抗扰控制器。

目前，处理侧滑角使用较多的方法是在LOS导引中加入积分项的积分导引(ILOS)和将侧滑角看作未知常量参数进行估计的自适应导引(ALOS)，这两种方法都能有效补偿侧滑角，但是仍存在不足之处。其一，两种方法都只能处理恒定侧滑角，而无人艇在海上航行时，时变海风干扰会导致时变侧滑角，即使海风干扰恒定，无人艇跟踪曲线路径时侧滑角也是时变的；其二，两种方法都含有积分项，可能会因为相位滞后造成稳定裕度降低；此外，由于跟踪误差大，ALOS导引在过渡阶段可能会出现不理想的偏差或振荡。因此，通过引入侧滑角观测器对时变侧滑角进行估计，设计一种可用于侧滑角未知条件下的路径跟踪导引算法在理论研究和工程领域都具有十分重要的意义。

发明内容

本发明的目的在于提供解决现有的路径跟踪导引算法在时变海风干扰以及侧滑角无法给出或者给出的值不准确时路径跟踪误差较大、跟踪性能较差的问题，保证无人艇在时变海风扰动和侧滑角未知条件下能够较好的跟踪上期望路径的一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：无人艇在执行路径跟踪任务时，获取当前时刻无人艇的位姿及速度信息，根据设定的期望几何路径，以目标跟踪点为原点构造SF坐标系并计算SF坐标系下的无人艇位姿误差向量；

所述的当前时刻无人艇的位姿及速度信息包括无人艇在北东坐标系下位姿(x,y,ψ)、船体坐标系下无人艇的纵向速度、横向速度及艏向角速度(u,v,r)；

北东坐标系中目标跟踪点的坐标为P(θ)＝[x(θ),y(θ)]^T，以P(θ)为原点构造SF坐标系，其纵轴和横轴分别为x_SF轴和y_SF轴；SF坐标系下的无人艇位姿误差向量[x_e,y_e,ψ_e]^T为：

其中，ψ_w＝ψ+β表示无人艇的航迹角，β＝arctan(v/u)；ψ_θ＝arctan2(y′(θ),x′(θ))表示北东坐标系向SF坐标系转换时的旋转角度；

步骤2：根据ELOS导引算法计算时变未知侧滑角的估计值

和无人艇的期望艏向角ψ_d；

其中，

表示无人艇的合速度大小；k为ELOS导引参数；Δ为目标跟踪点P(θ)处沿设定路径切向的前视距离参数；

步骤3：通过艏向线性扩张状态观测器观测当前时刻艏向海风扰动和无人艇的艏向角，得到艏向海风扰动观测值x₁₁和艏向角观测值z₁₁；通过纵向线性扩张状态观测器观测当前时刻纵向海风扰动和无人艇的纵向速度，得到纵向海风扰动观测值x₂₁和纵向速度观测值z₂₁；

步骤4：将计算得到的无人艇期望艏向角ψ_d输入到跟踪微分器TD中，得到输出结果

将设定的无人艇期望速度u_d输入到跟踪微分器TD中，得到输出结果

步骤5：计算虚拟控制率

并将虚拟控制率

输入到一阶滤波器中，得到一阶滤波器的输出结果α₁₂；

S₁₁＝x₁₁-v₁₁

其中，k₁₁为滤波器设计参数；v₁₂为v₁₁的导数，

为v₁₂的导数，由

通过积分获取v₁₂，由v₁₂通过积分获取v₁₁；

步骤6：计算转艏力矩控制律，并输入到无人艇艏向控制器中，实现对无人艇艏向的运动控制；计算纵向推力控制律，并输入到无人艇纵向速度控制器中，实现对无人艇纵向速度的控制；

所述的转艏力矩控制律为：

所述的纵向推力控制律为：

u₂₀＝-k₂₁S₂₁+v₂₂

其中，m₁₁,m₂₂,m₃₃为无人艇的惯性质量参数，d₁₁,d₁₂,d₁₃,d₂₂,d₂₃,d₃₂,d₃₃为无人艇的水动力阻尼参数；k₁₂为艏向控制器设计参数；k₂₁为纵向速度控制器设计参数；

为v₂₂的导数，由

通过积分获取v₂₂；

步骤7：判断无人艇是否收敛到期望的位置，若未收敛到期望位置则返回步骤1计算相应偏差，进行下一时刻的运动控制。

本发明的有益效果在于：

本发明设计的ELOS导引算法采用侧滑角观测器对时变未知侧滑角进行估计和补偿，并给出目标跟踪点的路径参数更新率和期望艏向角，避免了传统导引算法在侧滑角未知条件下存在路径跟踪误差较大的问题，由于设计的侧滑角观测器可以在侧滑角未知条件下得到侧滑角的估计值，因此减少了无人艇上测量装置的数量，经济实用，提高了系统的冗余性。

本发明设计的艏向及纵向速度动态面自抗扰控制器能够使无人艇跟踪期望路径时有较快的误差收敛速度和较高的跟踪精度，其中，设计的跟踪微分器安排的过渡过程对艏向角和纵向速度的期望值有较快的跟踪速度；采用的线性扩张状态观测器能够很好地估计纵向风力和转艏风力矩，并且将观测器带宽作为唯一参量，简化了参数整定工作，避免了传统非线性扩张状态观测器结构复杂、理论分析困难、待调节参数较多且参数整定难度较大的问题；采用动态面控制避免了反步法存在的“微分爆炸”问题。

附图说明

图1为基于侧滑角补偿的无人艇路径跟踪自抗扰控制原理图。

图2为ELOS导引原理图。

图3为ELOS导引坐标图。

图4为USV路径跟踪艏向动态面自抗扰控制器结构图。

图5为USV路径跟踪纵向速度动态面自抗扰控制器结构图。

图6为设定曲线路径和USV跟踪航迹曲线图。

图7为侧滑角估计曲线图。

图8为SF坐标系下纵向和横向位置误差曲线图。

图9为艏向角和纵向速度跟踪误差曲线图。

图10为艏向角、转艏角速度和纵向速度观测误差曲线图。

图11为纵向风力和转艏风力矩观测误差曲线图。

图12为USV控制输入曲线图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明设计了一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法，包括以下步骤：

步骤1：无人艇在执行路径跟踪任务时，获取当前时刻无人艇的位姿及速度信息，根据设定的期望几何路径，以目标跟踪点为原点构造SF坐标系并计算SF坐标系下的无人艇位姿误差向量；

所述的当前时刻无人艇的位姿及速度信息包括无人艇在北东坐标系下位姿(x,y,ψ)、船体坐标系下无人艇的纵向速度、横向速度及艏向角速度(u,v,r)；

北东坐标系中目标跟踪点的坐标为P(θ)＝[x(θ),y(θ)]^T，以P(θ)为原点构造SF坐标系，其纵轴和横轴分别为x_SF轴和y_SF轴；SF坐标系下的无人艇位姿误差向量[x_e,y_e,ψ_e]^T为：

其中，ψ_w＝ψ+β表示无人艇的航迹角，β＝arctan(v/u)；ψ_θ＝arctan2(y′(θ),x′(θ))表示北东坐标系向SF坐标系转换时的旋转角度；

步骤2：根据ELOS导引算法计算时变未知侧滑角的估计值

和无人艇的期望艏向角ψ_d；

其中，

表示无人艇的合速度大小；k为ELOS导引参数；Δ为目标跟踪点P(θ)处沿设定路径切向的前视距离参数；

步骤3：通过艏向线性扩张状态观测器观测当前时刻艏向海风扰动和无人艇的艏向角，得到艏向海风扰动观测值x₁₁和艏向角观测值z₁₁；通过纵向线性扩张状态观测器观测当前时刻纵向海风扰动和无人艇的纵向速度，得到纵向海风扰动观测值x₂₁和纵向速度观测值z₂₁；

步骤4：将计算得到的无人艇期望艏向角ψ_d输入到跟踪微分器TD中，得到输出结果

将设定的无人艇期望速度u_d输入到跟踪微分器TD中，得到输出结果

步骤5：计算虚拟控制率

并将虚拟控制率

输入到一阶滤波器中，得到一阶滤波器的输出结果α₁₂；

S₁₁＝x₁₁-v₁₁

其中，k₁₁为滤波器设计参数；v₁₂为v₁₁的导数，

为v₁₂的导数，由

通过积分获取v₁₂，由v₁₂通过积分获取v₁₁；

步骤6：计算转艏力矩控制律，并输入到无人艇艏向控制器中，实现对无人艇艏向的运动控制；计算纵向推力控制律，并输入到无人艇纵向速度控制器中，实现对无人艇纵向速度的控制；

所述的转艏力矩控制律为：

所述的纵向推力控制律为：

u₂₀＝-k₂₁S₂₁+v₂₂

其中，m₁₁,m₂₂,m₃₃为无人艇的惯性质量参数，d₁₁,d₁₂,d₁₃,d₂₂,d₂₃,d₃₂,d₃₃为无人艇的水动力阻尼参数；k₁₂为艏向控制器设计参数；k₂₁为纵向速度控制器设计参数；

为v₂₂的导数，由

通过积分获取v₂₂；

步骤7：判断无人艇是否收敛到期望的位置，若未收敛到期望位置则返回步骤1计算相应偏差，进行下一时刻的运动控制。

本发明的整体设计思路及理论推导验证过程如下：

首先，设计侧滑角观测器对时变未知侧滑角进行估计，在Serret-Frenet坐标框架下建立了路径跟踪位姿误差模型，将北东坐标系下的位姿误差转化到SF坐标系下，设计了ELOS导引算法，能够在侧滑角未知条件下计算出目标跟踪点的路径参数更新率和期望艏向角，然后，将喷水推进无人艇的艏向和纵向速度控制模型写成便于采用扩张状态观测器的形式，按照动态面自抗扰控制的结构设计控制器部分，采用跟踪微分器对期望艏向角和设定的期望纵向速度分别安排过渡过程，采用线性扩张状态观测器实时估计纵向风力和转艏风力矩，采用动态面控制和自抗扰控制相结合的办法，以动态面控制代替自抗扰控制理论中的非线性状态误差反馈控制律部分，分别设计艏向控制器和纵向速度控制器，最后，将得到的海风干扰观测值在控制器中进行扰动补偿，以消除海风扰动产生的影响。本发明设计的ELOS导引算法和动态面自抗扰控制器能够使无人艇在时变海风扰动和侧滑角未知条件下跟踪正弦曲线路径时有良好的跟踪性能，跟踪误差的收敛速度比较快，路径跟踪精度比较高。

本发明通过设计ELOS导引算法和动态面自抗扰控制器来实现无人艇在时变海风干扰和侧滑角未知条件下跟踪期望路径。首先，对于侧滑角无法给出或者给出的值不准确的情况，设计侧滑角观测器对时变未知侧滑角进行估计，在Serret-Frenet坐标框架下建立路径跟踪位姿误差模型，设计了ELOS导引算法，用于计算目标跟踪点的路径参数更新率和期望艏向角；其次，将喷水推进无人艇的艏向和纵向速度数学模型写成便于采用扩张状态观测器的形式，对转换后的模型按照自抗扰控制的结构分别设计了跟踪微分器、线性扩张状态观测器，并采用动态面自抗扰控制算法分别设计了艏向控制器和纵向速度控制器；最后，将得到的海风干扰观测值在控制器中进行扰动补偿，从而使无人艇在时变海风扰动和侧滑角未知条件下跟踪正弦曲线路径时有较快的跟踪收敛速度和较高的路径跟踪精度。

本发明主要包括利用ELOS导引算法对时变未知侧滑角进行在线估计，从而在侧滑角未知条件下计算出目标跟踪点的路径参数更新率和期望艏向角，采用跟踪微分器对期望艏向角和设定的期望纵向速度安排过渡过程，采用线性扩张状态观测器实时估计纵向风力和转艏风力矩，采用动态面自抗扰控制算法分别设计艏向控制器和纵向速度控制器，将得到的海风干扰观测值加入到控制器中进行扰动补偿，以消除海风扰动的影响。

设计侧滑角观测器对时变未知侧滑角进行估计，在Serret-Frenet坐标框架下建立了路径跟踪位姿误差模型，将北东坐标系下的位姿误差转化到SF坐标系下，设计ELOS导引算法，在侧滑角未知条件下计算出目标跟踪点的路径参数更新率和期望艏向角；

设计的ELOS导引算法具体如下：

其中，g＝Ucos(ψ-ψ_θ)β，

为设计的观测器对g的估计值，p为观测器辅助状态，k＞0为待设计的观测器增益，

为时变侧滑角的估计值，

为路径参数更新率，ψ_d为期望的艏向。

将喷水推进无人艇的艏向和纵向速度数学模型写成便于采用扩张状态观测器的形式，使用跟踪微分器对ELOS导引算法计算出的期望艏向角以及设定的期望纵向速度安排过渡过程，引入线性扩张状态观测器实时估计纵向风力和转艏风力矩。为便于采用扩张状态观测器，对无人艇艏向数学模型进行如下转换：

式中，u₁₀＝b₁₀u₁+f₁(·)+w₁，

u₁＝τ_r为系统的控制输入。

设计跟踪微分器对期望的艏向安排过渡过程，使期望艏向平稳连续变化。为尽可能消除无人艇在实际做路径跟踪任务时受到的扰动影响，采用线性扩张观测器(LESO)将系统总扰动扩张成新的一维系统状态，并对其进行观测，即采用如下线性扩张状态观测器：

式中，z₁₁为艏向角ψ的观测值；z₁₂为转艏角速度r的观测值，z₁₃为扰动w₁扩张x₁₁后的观测值，e₁₂为艏向角观测误差。β₁，β₂，β₃为待设计观测器增益，为简单起见，选取观测器增益为：β₁₁＝3ω₀₁,

ω₀₁为观测器带宽。

同理，为便于扩张状态观测器的采用，对无人艇速度数学模型进行相应的变换：

式中，u₂₀＝b₂₀u₂+f₂(·)+w₂，

u₂＝τ_u为系统控制输入。

同理，设计跟踪微分器对设定的期望速度安排过渡过程，采用线性扩张观测器将扰动w₂扩张成新状态x₂₁，并对其进行相应的观测，具体采用形式如下：

式中，z₂₁为纵向速度u的观测值，z₂₂为扰动w₂扩张x₂₁后的观测值，e₂₂为纵向速度观测误差。β₂₁和β₂₂为待设计观测器增益参数，为简单起见，选取观测增益为：β₂₁＝2ω₀₂，

ω₀₂为观测器带宽。

根据喷水推进无人艇的实际特性，可对其纵向和艏向进行控制，采用动态面自抗扰控制算法设计相应的控制器，即设计相应的转艏力矩控制器τ_r和纵向推力控制器τ_u。

采用动态面自抗扰控制算法设计的控制律具体表达式如下：

其中，S₁₂为动态误差面，k₁₂＞0为待设计参数，

其中，

为虚拟控制律，α₁₂为滤波器输出，τ₁₂＞0为待调节时间常数。

将扩张状态观测器对海风扰动x₁₁的观测值z₁₃补偿到u₁₀中，可得转艏力矩控制律：

τ_r＝u₁＝(u₁₀-z₁₃-f₁(·))/b₁₀

同理，可得纵向推力控制律：

τ_u＝u₂＝(u₂₀-z₂₂-f₂(·))/b₂₀

其中，控制律

S₂₁为动态误差面，k₂₁＞0为待设计参数，v₂₂由跟踪微分器安排期望纵向速度u_d过渡过程获得

本发明提出了一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法，目的在于解决现有的路径跟踪导引算法在时变海风干扰以及侧滑角无法给出或者给出的值不准确时路径跟踪误差较大、跟踪性能较差的问题，通过设计侧滑角观测器对时变未知侧滑角进行估计和补偿，设计了ELOS导引算法，从而减小侧滑角对路径跟踪性能的影响，并在ELOS导引的基础上采用动态面自抗扰控制算法分别设计了艏向和纵向速度控制器，保证无人艇在时变海风扰动和侧滑角未知条件下能够较好的跟踪上期望路径。

本发明所提出的基于侧滑角补偿的无人艇路径跟踪自抗扰控制算法如图1所示，其中包括设计侧滑角观测器得到时变未知侧滑角的估计值、设计跟踪微分器对导引得到的期望艏向角和设定的期望纵向速度安排过渡过程、设计线性扩张状态观测器实时估计纵向风力和转艏风力矩、采用动态面自抗扰控制算法分别设计艏向和纵向速度控制器并将得到的海风干扰观测值在控制器中加以补偿。具体步骤如下：

步骤一、考虑无人艇在执行路径跟踪任务时，会受到海风扰动的影响，建立水平面喷水推进无人艇欠驱动数学模型；设计ELOS导引算法对海风干扰产生的时变侧滑角进行估计并给出目标跟踪点的路径参数更新率和期望艏向角；

通过欧拉旋转变换以及动量和动量矩定理可得，建立的水平面无人艇三自由度运动数学模型如下：

其中,(x,y,ψ)分别表示无人艇在北东坐标系下位姿；(u,v,r)分别表示船体坐标系下无人艇的速度及角速度；m₁₁,m₂₂,m₃₃表示无人艇的惯性质量参数，d₁₁,d₁₂,d₁₃,d₂₂,d₂₃,d₃₂,d₃₃表示无人艇的水动力阻尼参数，τ_u,τ_r分别表示无人艇纵向推力和转艏力矩；X_wind、Y_wind和N_wind分别表示外界海风扰动在纵荡、横荡方向上的作用力和艏摇方向上的作用力矩。

ELOS导引算法原理图如图2所示，其具体坐标选定原理如图3所示，其中，P为设定的期望几何路径，将其上的点P(θ)看作当前的目标跟踪点，北东坐标系中目标跟踪点的坐标为P(θ)＝[x(θ),y(θ)]^T，其沿着期望几何路径运动的速度为

以p(θ)为原点构造SF坐标系，其纵轴和横轴分别为x_SF轴和y_SF轴；

由微分同胚变换可知：

其中，ψ_w＝ψ+β表示无人艇的航迹角，β＝arctan(v/u)；x_θ、y_θ和ψ_θ分别表示x(θ)、y(θ)和ψ(θ)的简写，ψ(θ)＝arctan2(y'(θ),x'(θ))表示北东坐标系向SF坐标系转换时的旋转角度；[x_e,y_e,ψ_e]^T表示SF坐标系下的位姿误差向量；[x-x_θ,y-y_θ,ψ_w-ψ_θ]^T表示北东坐标系下的位姿误差向量；

考虑到实际情况中侧滑角β相对较小，因此，可近似认为cosβ≈1，sinβ≈β，对上式中x_e和y_e求导可得：

其中，

表示无人艇的合速度大小，θ表示期望路径P的路径参数，g＝Ucos(ψ-ψ_θ)β；

设计如下观测器对g进行估计：

其中，

为g的估计值，p为观测器辅助状态，k＞0为待设计的观测器增益，p(t)的初值p(t₀)最好选为g(t₀)ky_e(t₀)，g(t₀)和y_e(t₀)分别为待观测量g(t)和y_e(t)的初值，但由于g(t)是通过观测器估计得到，所以g(t₀)是未知的，因此通常设置初始值p(t₀)＝0；

则时变未知侧滑角β的估计值

为：

当所设计的观测器对g的观测误差收敛时，时变未知侧滑角β的估计误差也是收敛的，下面简要分析观测器观测误差的收敛性：

假设存在正常数g₀，使得g满足|(d^rg/dt^r)|≤g₀(r＝0,1)，定义g的观测误差为

可以求得：

选取如下李雅普诺夫函数：

V₁对时间求导可得：

利用如下完全平方不等式：

其中，ξ为正常数，满足k-ξ＞0；

可以得到：

其中，a＝2(k-ξ)＞0，

则有：

由V₁的定义可知，观测器对g的估计误差

指数收敛于以原点为中心、半径为

的球域；选取合适的参数值k和ξ，能使估计误差

任意小；

步骤二、将喷水推进无人艇的艏向数学模型写成便于采用扩张状态观测器的形式，使用跟踪微分器对导引得到的期望艏向角安排过渡过程，采用线性扩张状态观测器实时估计转艏风力矩，对无人艇艏向数学模型进行如下转换：

式中，u₁₀＝b₁₀u₁+f₁(·)+w₁，

u₁＝τ_r为系统的控制输入。

步骤三、采用动态面自抗扰控制算法设计艏向控制器，并将海风扰动x₁₁的观测值z₁₃补偿到控制器中，以消除海风扰动的影响

定义如下误差动态面S₁₁和S₁₂：

S₁₁＝x₁₁-v₁₁，S₁₂＝x₁₂-α₁₂

其中，v₁₁，v₁₂为期望艏向角ψ_d安排的过渡过程及其导数，可由跟踪微分器获得，α₁₂为滤波器输出，滤波表达式为：

τ₁₂>0为待调节时间常数，

为虚拟控制律。

设计相应的转艏控制律τ_r使误差动态面S₁₁和S₁₂收敛，并在所得的控制律中加以动态补偿，以消除海风扰动的影响。

步骤四、将喷水推进无人艇的纵向速度控制模型写成便于采用扩张状态观测器的形式，使用跟踪微分器对设定的期望纵向速度安排过渡过程，线性扩张状态观测器实时估计纵向风力，对无人艇速度数学模型进行相应的变换：

式中，u₂₀＝b₂₀u₂+f₂(·)+w₂，

u₂＝τ_u为系统控制输入。

步骤五、采用动态面自抗扰控制算法设计纵向速度控制器，并将海风扰动x₂₁的观测值z₂₂补偿到控制器中，以消除海风扰动的影响。

定义如下误差动态面S₂₁：

S₂₁＝x₂₁-v₂₁

其中，v₂₁为跟踪微分器对设定的期望纵向速度u_d安排的过渡过程。

设计相应的纵向控制律τ_r使误差动态面S₂₁收敛，并在所得的控制律中加以动态补偿，以消除海风扰动的影响。

本发明具有以下有益效果：

1、设计的ELOS导引算法采用侧滑角观测器对时变未知侧滑角进行估计和补偿，并给出目标跟踪点的路径参数更新率和期望艏向角，避免了传统导引算法在侧滑角未知条件下存在路径跟踪误差较大的问题，由于设计的侧滑角观测器可以在侧滑角未知条件下得到侧滑角的估计值，因此减少了无人艇上测量装置的数量，经济实用，提高了系统的冗余性。

2、设计的艏向及纵向速度动态面自抗扰控制器能够使无人艇跟踪期望路径时有较快的误差收敛速度和较高的跟踪精度，其中，设计的跟踪微分器安排的过渡过程对艏向角和纵向速度的期望值有较快的跟踪速度；采用的线性扩张状态观测器能够很好地估计纵向风力和转艏风力矩，并且将观测器带宽作为唯一参量，简化了参数整定工作，避免了传统非线性扩张状态观测器结构复杂、理论分析困难、待调节参数较多且参数整定难度较大的问题；采用动态面控制避免了反步法存在的“微分爆炸”问题。

基于侧滑角补偿的无人艇路径跟踪自抗扰控制算法结构图如图1所示，具体步骤如下：

步骤一、考虑海风扰动对无人艇运动产生的影响，在建立喷水推进无人艇三自由度数学模型的基础上，采用侧滑角观测器对侧滑角进行估计，并用ELOS导引算法给出目标跟踪点的路径参数更新率和期望艏向角；

SF坐标系下的纵、横向位置误差收敛是无人艇收敛到期望路径的必要条件，设计如下路径参数更新率使纵向位置误差收敛：

其中，k_x＞0为待设计参数；x′_θ和y′_θ分别为x(θ)和y(θ)对路径参数θ的导数；

假设设计的艏向控制器可保证无人艇的艏向角很好地跟踪期望艏向角，即认为ψ＝ψ_d，设计如下期望艏向：

其中，Δ＞0为目标跟踪点P(θ)处沿设定路径切向的前视距离参数；

综合考虑横向位置误差y_e和前视距离参数Δ，对传统定常前视距离参数进行改进，设计如下时变动态前视距离参数使路径跟踪具有更好的动态适应性：

其中，γ＞0为待设计参数；

综上所述，设计如下的ELOS导引算法能够实现控制目标

步骤二、为便于扩张观测器的采用，需对无人艇艏向数学模型进行相应的转化，具体转换过程如下：

式中，u₁₀＝b₁₀u₁+f₁(·)+w₁，

u₁＝τ_r为系统的控制输入。

在此基础上，使用跟踪微分器对导引得到的期望艏向角安排过渡过程，对变换后的艏向数学模型进行线性扩张观测器的设计来实时估计转艏风力矩；

根据ELOS导引算法给出的USV路径跟踪期望艏向，设计出相应的无人艇路径跟踪艏向动态面自抗扰结构框图，具体框图如图4所示，采用跟踪微分器TD对期望艏向ψ_d安排过渡过程，微分跟踪器的设计具体如下：

其中，v₁₁表示对期望艏向ψ_d安排的过渡过程，v₁₂为v₁₁的导数，fhan(x₁,x₂,r,h₀)为最速综合函数，具体表达式如下：

其中，r表示速度因子，r增大，安排的过渡过程跟踪设定信号速度加快，选取合适的h₀值可以对噪声进行滤波，sign(·)为符号函数。

对转换后的无人艇艏向数学模型采用如下线性扩张状态观测器：

式中，z₁₁为艏向角ψ的观测值；z₁₂为转艏角速度r的观测值，z₁₃为扰动w₁扩张为x₁₁后的观测值，e₁₂为艏向角观测误差。β₁，β₂，β₃为待设计观测器增益，为简单起见，选取观测器增益为：β₁₁＝3ω₀₁,

ω₀₁为观测器带宽。

步骤三、如图4所示，采用动态面自抗扰控制算法设计艏向控制器，并将扰动观测值z₁₃在控制器中加以补偿。定义如下误差动态面S₁₁和S₁₂，设计转艏力矩控制律τ_r，使误差动态面S₁₁和S₁₂收敛；

S₁₁＝x₁₁-v₁₁，S₁₂＝x₁₂-α₁₂

其中，v₁₁，v₁₂为期望艏向角ψ_d安排的过渡过程及其导数，可由跟踪微分器获得，α₁₂为滤波器输出。

对误差动态面S₁₁和S₁₂分别求导可得：

设计虚拟控制律

其中，k₁₁＞0为待设计参数，φ(y_e,S₁₁)的表达式为：

动态面自抗扰控制算法使用一阶滤波器对虚拟控制律

进行滤波，解决反步法“微分爆炸”问题，滤波表达式为：

则有

其中，α₁₂为滤波器输出，τ₁₂＞0为待调节时间常数；

设计控制律

其中，k₁₂＞0为待设计参数。

将扰动x₁₃的观测值z₁₃补偿到控制律u₁₀中，得到转艏力矩控制律为：

τ_r＝u₁＝(u₁₀-z₁₃-f₁(·))/b₁₀

步骤四、为便于扩张观测器的采用，需对无人艇速度数学模型进行相应的变化，经变换后的数学模型如下：

式中，u₂₀＝b₂₀u₂+f₂(·)+w₂，

u₂＝τ_u为系统控制输入。

并采用跟踪微分器对设定的期望速度安排过渡过程，对变换后的速度数学模型采用线性扩张观测器来实时估计纵向风力；

采用跟踪微分器TD对设定的期望速度u_d安排过渡过程，微分跟踪器TD设计的具体表达式如下：

式中，v₂₁和v₂₂分别表示对设定的期望速度u_d安排的过渡过程及其导数，fhan(e₂₁,v₂₁,r₂,h₀₂)为最速控制综合函数，其计算方法与步骤二中最速综合函数相同。

采用线性扩张观测器将扰动w₂扩张为系统状态x₂₁并对其进行观测，具体采用线性扩张观测器如下：

式中，z₂₁为纵向速度u的观测值，z₂₂为x₂₁的观测值，e₂₂为纵向速度观测误差。β₂₁和β₂₂为待设计观测器增益参数，为简单起见，选取观测增益为：β₂₁＝2ω₀₂，

ω₀₂为观测器带宽。

步骤五、USV路径跟踪纵向速度动态面自抗扰控制器结构框图如图5所示，对速度实施的控制方案与艏向控制类似，对闭环反馈得到的速度偏差，采用动态面自抗扰控制算法设计纵向速度控制器，并将扰动观测值z₂₂在控制器中加以补偿。定义如下动态误差面S₂₁，设计纵向推力控制律τ_u，使误差动态面S₂₁收敛；

S₂₁＝x₂₁-v₂₁

式中，v₂₁为设定的期望纵向速度u_d安排的过渡过程。

对误差动态面S₂₁求导可得：

其中v₂₂为v₂₁的导数。

设计控制律

其中，k₂₁＞0为待设计参数。

将x₂₁的观测值z₂₂补偿到控制律u₂₀中，得到纵向推力控制律为：

τ_u＝u₂＝(u₂₀-z₂₂-f₂(·))/b₂₀

本发明仿真参数设置：初始位姿为x＝25，y＝0，ψ＝3π/5；初始速度为u＝0，v＝0，r＝0；初始期望位姿为x_d＝0，y_d＝0，ψ_d＝0；期望几何路径为P(θ)＝[Asin(θ/λ),θ]^T；海风干扰参数平均风速

绝对风向角为ψ_w＝3π/5；ELOS导引参数为k＝0.15，k_x＝18，γ＝0.1；跟踪微分器TD参数为r₁＝0.12，h₀₁＝0.1，r₂＝0.003，h₀₂＝0.05；LESO线性扩张观测器参数为ω₀₁＝4，ω₀₂＝8；控制器参数为：k₁₁＝2，τ₁₂＝0.04，k₁₂＝0.1，k₂₁＝5；期望速度为u_d＝9。

仿真结果图6、图7、图8、图9、图10、图11和如图12所示，分别为设定曲线路径和USV跟踪航迹曲线图、侧滑角估计曲线图、SF坐标系下纵向和横向位置误差曲线图、艏向角和纵向速度跟踪误差曲线图、艏向角、转艏角速度和纵向速度观测误差曲线图、纵向风力和转艏风力矩观测误差曲线图、USV控制输入曲线图。由仿真图可以看出，本发明中设计的基于侧滑角补偿的无人艇路径跟踪自抗扰控制算法可以很好的解决由外界海风引起的侧滑角影响路径跟踪精度的问题。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于侧滑角补偿的无人艇路径跟踪自抗扰控制方法，其特征在于，包括以下步骤：

步骤1：无人艇在执行路径跟踪任务时，获取当前时刻无人艇的位姿及速度信息，根据设定的期望几何路径，以目标跟踪点为原点构造SF坐标系并计算SF坐标系下的无人艇位姿误差向量；

所述的当前时刻无人艇的位姿及速度信息包括无人艇在北东坐标系下位姿(x,y,ψ)、船体坐标系下无人艇的纵向速度、横向速度及艏向角速度(u,v,r)；

北东坐标系中目标跟踪点的坐标为P(θ)＝[x(θ),y(θ)]^T，以P(θ)为原点构造SF坐标系，其纵轴和横轴分别为x_SF轴和y_SF轴；SF坐标系下的无人艇位姿误差向量[x_e,y_e,ψ_e]^T为：

其中，ψ_w＝ψ+β表示无人艇的航迹角，β＝arctan(v/u)；ψ_θ＝arctan2(y′(θ),x′(θ))表示北东坐标系向SF坐标系转换时的旋转角度；

步骤2：根据ELOS导引算法计算时变未知侧滑角的估计值

和无人艇的期望艏向角ψ_d；

其中，

表示无人艇的合速度大小；k为ELOS导引参数；Δ为目标跟踪点P(θ)处沿设定路径切向的前视距离参数；

步骤3：通过艏向线性扩张状态观测器观测当前时刻艏向海风扰动和无人艇的艏向角，得到艏向海风扰动观测值x₁₁和艏向角观测值z₁₁；通过纵向线性扩张状态观测器观测当前时刻纵向海风扰动和无人艇的纵向速度，得到纵向海风扰动观测值x₂₁和纵向速度观测值z₂₁；

步骤4：将计算得到的无人艇期望艏向角ψ_d输入到跟踪微分器TD中，得到输出结果

将设定的无人艇期望速度u_d输入到跟踪微分器TD中，得到输出结果

步骤5：计算虚拟控制率

并将虚拟控制率

输入到一阶滤波器中，得到一阶滤波器的输出结果α₁₂；

S₁₁＝x₁₁-v₁₁

其中，k₁₁为滤波器设计参数；v₁₂为v₁₁的导数，

为v₁₂的导数，由

通过积分获取v₁₂，由v₁₂通过积分获取v₁₁；

步骤6：计算转艏力矩控制律，并输入到无人艇艏向控制器中，实现对无人艇艏向的运动控制；计算纵向推力控制律，并输入到无人艇纵向速度控制器中，实现对无人艇纵向速度的控制；

所述的转艏力矩控制律为：

所述的纵向推力控制律为：

u₂₀＝-k₂₁S₂₁+v₂₂

其中，m₁₁,m₂₂,m₃₃为无人艇的惯性质量参数，d₁₁,d₁₂,d₁₃,d₂₂,d₂₃,d₃₂,d₃₃为无人艇的水动力阻尼参数；k₁₂为艏向控制器设计参数；k₂₁为纵向速度控制器设计参数；

为v₂₂的导数，由

通过积分获取v₂₂；

步骤7：判断无人艇是否收敛到期望的位置，若未收敛到期望位置则返回步骤1计算相应偏差，进行下一时刻的运动控制。

Images