CN113985898B - 一种欠驱动海洋航行器的非线性路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种欠驱动海洋航行器的非线性路径跟踪控制方法，该方法包括：构建欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型；通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器；采用非线性跟踪微分器对基于UDE的控制器进行简化，得到简化后的控制器；采用UKF联合估计算法对系统状态和不确定性进行联合估计。通过使用本发明，实现对期望路径的精确跟踪控制。本发明作为一种欠驱动海洋航行器的非线性路径跟踪控制，可广泛应用于路径跟踪控制领域。

Description

一种欠驱动海洋航行器的非线性路径跟踪控制方法

技术领域

本发明涉及路径跟踪控制领域，尤其涉及一种欠驱动海洋航行器的非线性路径跟踪控制方法。

背景技术

水面或水下自主航行器具有机动性好，控制灵活，投资低等特点，在海上石油天然气作业，海洋测绘、检测等各个海洋经济领域发挥着重要作用。为了更好地完成上述任务，建立有效和稳健的控制系统来实现水面或水下自主航行器的精确运动控制至关重要。运动控制中对路径跟踪控制有较高的要求，即控制航行器跟踪一条与时间无关且满足航向要求和性能约束的期望轨迹。目前，大部分的海洋航行器受到重量、可靠性、复杂性及效率等多方面的因素影响，其控制输入少于系统自由度，因而属于欠驱动系统，加上其本身具有高度的非线性、耦合性以及加速度不可积的非完整约束等特性，使得欠驱动海洋航行器的路径跟踪控制存在较大的挑战。尤其在欠驱动海洋航行器还存在未建模动态、模型参数不确定性及风浪流等多不确定性干扰，传统的控制方法往往不能保证其路径跟踪控制的精度，有时甚至无法满足实际作业的需要。针对多不确定性干扰的情况，有学者提出基于观测器的控制(Disturbance-observer-basedcontrol，DOBC)方法进行处理，目前主流的有观测器有干扰观测器(Disturbanceobserver，DOB)，扩展状态观测器(Extendedstateobserver，ESO)。但是，DOBC对模型的精确度要求较高，且存在求逆运算，在工程中不易实现，而基于ESO的控制策略，虽然减小了对模型的依赖，但是对干扰的类型进行了限制，只能解决导数有界的干扰。同时，控制系统中还可能存在随机干扰、测量噪声等，影响系统的性能。

发明内容

为了解决上述技术问题，本发明的目的是提供一种欠驱动海洋航行器的非线性路径跟踪控制方法，实现对期望路径的精确跟踪控制。

本发明所采用的技术方案是：一种欠驱动海洋航行器的非线性路径跟踪控制方法，包括以下步骤：

构建欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型；

通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器；

采用非线性跟踪微分器对基于UDE的控制器进行简化，得到简化后的控制器；

采用UKF联合估计技术对系统状态和不确定性进行联合估计。

进一步，所述欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型表示如下：

上式中，(x_e,y_e)表示航行器质心在Serret-Frenet坐标系下的坐标，ψ_e表示航行器的航向轴与额定路径在虚拟点处的切线之间的夹角，v_t表示航行器的合成速度，表示虚拟向导速度，c_c表示期望路径的曲率。u、v和r分别表示航行器质心在惯性坐标系下的纵向角速度、横向角速度和航向角速度，τ_u和τ_r表示外部控制输入力和外部控制输入力矩，m表示航行器质量，X_u、/>X_u|u|、Y_v、/>Y_v|v|、N_r、/>和N_r|r|表示名义水动力参数，/>表示航行器在前进自由度上的附加质量，/>表示航行器在横漂自由度上的附加质量，表示航行器在艏摇自由度上的附加质量，d_u、d_v和d_r表示动力学模型集总不确定性，包括外部干扰，模型参数不确定性和未建模动态等，d_ψ表示运动学模型不确定性，z₁、z₂、z₃和z₄表示系统输出变量。

进一步，所述通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器这一步骤，其具体包括：

采用传统视线制导律计算期望视线角；

根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法构建航向角虚拟控制律；

根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法，设计位置子系统，确定虚拟向导速度控制律；

根据包含多不确定性的动力学模型，结合李雅普诺夫直接法和back-stepping法，确定外部输入力控制律和外部输入力矩控制律；

通过UDE对欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型中的不确定性进行估计，并结合航向角虚拟控制律、外部输入力控制律和外部输入力矩控制律，得到基于UDE的控制器。

进一步，所述航向角虚拟控制律的表达式如下：

上式中，ψ_los表示期望视线角。

进一步，所述虚拟向导速度控制律的表达式如下：

上式中，k₂＞0表示设计的控制增益。

进一步，所述外部输入力控制律和外部输入力矩控制律的表达式如下：

进一步，所述基于UDE的控制器的表达式如下：

进一步，所述简化后的控制器的表达式如下：

上式中，r_c和为虚拟参考指令r_d通过非线性跟踪微分器后得到的跟踪信号和微分信号。

进一步，所述采用UKF联合估计技术对系统状态和不确定性进行联合估计这一步骤，其具体包括：

考虑随机干扰、测量噪声的影响，通过增广UKF联合估计算法对控制系统的状态和横向通道不确定性进行联合估计。

本发明方法的有益效果是：本发明将运动学误差模型中的未知参数当作运动学不确定性，将动力学模型中的外部环境扰动、模型参数不确定性以及未建模动态等当作动力学集总不确定性，采用基于UDE的控制器估计并消除多不确定性的影响；通过非线性跟踪微分器对控制器进行简化，降低计算的复杂性，提高该控制系统的实用性；本发明不依赖于控制对象精确的数学模型，将没有控制输入的横向通道存在的不确定性dv当作未知参数，并将控制系统可能存在的随机干扰、测量噪声等问题考虑在内，采用UKF联合估计技术对状态和dv进行估计；采用UDE技术和基于UKF的联合估计技术主动消除模型不确定性，即对未知模型主动建模的方式保证UKF的收敛，改变了传统强跟踪算法的被动行为。

附图说明

图1是本发明一种欠驱动海洋航行器的非线性路径跟踪控制方法的步骤流程图；

图2是本发明一种欠驱动海洋航行器的非线性路径跟踪控制方法的路径跟踪控制框图；

图3是本发明具体实施例期望路径及欠驱动海洋航行器的实际路径的示意图；

图4是本发明具体实施例欠驱动海洋航行器路径跟踪误差及速度曲线示意图；

图5是本发明具体实施例控制输入及虚拟向导运动控制曲线示意图；

图6是横向通道不确定性及其估计值。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

参照图1和图2，本发明提供了一种欠驱动海洋航行器的非线性路径跟踪控制方法，该方法包括以下步骤：

S1、构建欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型；

欠驱动海洋航行器在Serret-Frenet曲线坐标系路径跟踪误差模型可以表示为：

上述模型中包含未知参数根据公式/>及动力学模型，/>取决于航行器纵向速度u、横向速度v、航向角速度r及模型参数和不确定性d_u、d_v，因此我们无法直接计算得到/>的值。在本发明中，/>被看作一个不确定性进行处理，即/>进而误差模型的第三个方程转化为：

包含多不确定性的欠驱动海洋航行器的动力学模型可以表示为：

因此，欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型表示如下：

上式中，(x_e,y_e)表示航行器质心在Serret-Frenet坐标系下的坐标，ψ_e表示航行器的航向轴与额定路径在虚拟点处的切线之间的夹角，v_t表示航行器的合成速度，表示虚拟向导速度，c_c表示期望路径的曲率。u、v和r分别表示航行器质心在惯性坐标系下的纵向角速度、横向角速度和航向角速度，τ_u和τ_r表示外部控制输入力和外部控制输入力矩，m表示航行器质量，X_u、/>X_u|u|、Y_v、/>Y_v|v|、N_r、/>和N_r|r|表示名义水动力参数，/>表示航行器在前进自由度上的附加质量，/>表示航行器在横漂自由度上的附加质量，表示航行器在艏摇自由度上的附加质量，d_u、d_v和d_r表示动力学模型集总不确定性，表示动力学模型集总不确定性，包括外部干扰、模型参数不确定性和未建模动态等，d_ψ表示运动学模型不确定性，z₁、z₂、z₃和z₄表示系统输出变量。

S2、通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器；

具体地，S21、采用传统视线角制导律计算期望视线角：

上式中，ψ_los表示视线角，Δ表示视线距离。

S22、姿态子系统设计，根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法构建航向角虚拟控制律；

李雅普诺夫方程表示如下：

将公式(6)对时间求导，并代入公式(2)，得到：

为了使为负，选取航向角虚拟控制律如下：

其中，k₁＞0表示控制增益。

S23、位置子系统设计，根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法，确定控制律

考虑如下李雅普诺夫方程：

将公式(9)对时间求导，并代入公式(1)，得到：

为了使为负，由于在姿态子系统控制律设计中，ψ_e→ψ_los已得到保证，所以y_ev_tsinψ_e≤0，选取虚拟向导速度控制律为：

其中，k₂＞0为控制增益。

S24、动力学控制器设计，根据包含多不确定性的动力学模型，结合李雅普诺夫直接法和back-stepping法，确定外部输入力控制律和外部输入力矩控制律。

定义虚拟航向角速度期望值为：

考虑如下的李雅普诺夫函数：

将公式(13)对时间求导，可得：

其中，为了使/>为负，设计输入力控制律τ_u及输入力矩控制律τ_r分别为:

S25、通过UDE对欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型中的不确定性进行估计，并结合航向角虚拟控制律、外部输入力控制律和外部输入力矩控制律，得到基于UDE的控制器；

具体地，上面设计的控制器由于包含不确定性d_u、d_ψ和d_r，因此还不能直接使用，下面采用UDE技术来对不确定性d_u、d_ψ和d_r进行估计。

根据模型(4)，不确定性d_u、d_ψ和d_r可以重写为：

因此，d_u、d_ψ和d_r可以通过系统已知的信息和控制信号得到，然而其不能直接用于推导控制律。g_f(t)为理想低通滤波器G_f(s)的脉冲响应函数，并且其带宽可以涵盖不确定性d_u、d_ψ和d_r的频率范围，则d_u、d_ψ和d_r可以通过下式来进行估计：

上式中，和/>为不确定d_u、d_ψ和d_r的估计值，‘*’表示卷积符号。

将式(17)代入式(15)，并经过整理，可以得到基于UDE的控制器为：

其中，‘L^-1’为反拉普拉斯符号，选取T＝1/ω_f，1(t)为阶跃信号。控制器(18)进一步简化为：

S3、采用非线性跟踪微分器对基于UDE的控制器进行简化，得到简化后的控制器；

具体地，步骤S2中设计的动力学控制器τ_r中包括表达式结合虚拟指令表达式(12)，可知动力学控制器非常复杂，且包含不确定性d_ψ，本发明采用非线性跟踪微分器对运动学虚拟控制指令r_d进行数值求导，非线性跟踪微分器可以表示为：

上式中，h为采样周期，r为加速因子，简化后的控制器为：

上式中，r_c和为虚拟参考指令r_d通过非线性跟踪微分器后得到的跟踪信号和微分信号

S4、采用UKF联合估计技术对系统状态和不确定性进行联合估计，以抑制控制系统中可能存在的随机干扰、测量噪声等产生的影响。

具体地，将上述设计的基于UDE的控制器(21)代入系统数学模型(4)，可得：

定义UDE估计误差：考虑到式(17)，式(22)可以重写为：

可以看出，式(23)还包含不确定性d_v和UDE估计误差等，无法使用标准的UKF直接进行滤波。为解决此问题，本发明将d_v看作一个未知参数，并采用基于UKF的状态和参数联合估计算法同时估计状态和不确定性d_v。

为了弥补过程噪声统计特性复杂、滤波初始值误差和过程噪声不确定性的影响，保证滤波器的稳定，在仿真中，适当采用设置值比较大的噪声方差阵可以保证滤波的稳定性，增广UKF联合估计算法描述如下：

令增广状态向量为：

x＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇]^T＝[x_e y_e ψ_e u v r d_v]^T (24)

假设不确定性d_v有如下的变化规律：

对式(28)进行离散化，并考虑过程噪声和测量噪声的影响，可得：

其中，f(x_k)＝[f₁(x_k) f₂(x_k) f₃(x_k) f₄(x_k) f₅(x_k) f₆(x_k) f₇(x_k)]^T；

h(x_k)＝[h₁(x_k) h₂(x_k) h₃(x_k) h₄(x_k)]^T。

f(x_k)的分量如下：

f₄(x_k)＝x_4,k+Δt{-k₅(x_4,k-1)}；

其中，和/>为最速离散跟踪微分器k时刻的输出。

当不知道不确定性d_v的变化规律时，可以将d_v视为不相关的随机变量，其递归表达式可以表示为：f₇(x_k)＝x_7,k-1。

h(x_k)的分量如下：h₁(x_k)＝x_1,k；h₂(x_k)＝x_2,k；h₃(x_k)＝x_3,k；h₄(x_k)＝x_4,k。

w_k～N(0,Q)，v_k～N(0,R)为互不相关的零均值白噪声。

应用UKF算法对系统(21)进行状态和不确定性d_v的联合估计，即可从多不确定性和噪声干扰中估计出增广状态向量[x_e y_e ψ_e u v r d_v]^T。

为了验证上述提出的控制器的有效性，在MATLAB/Simulink环境下建立欠驱动海洋航行器的水平面内的路径跟踪控制系统的仿真模型，并对欠驱动海洋航行器在多不确定性和随机噪声及测量噪声情况下的路径跟踪控制性能进行验证。

航行器模型参数见表1，初始值设置为：

x(0)＝30m,y(0)＝-20m,u(0)＝0.1m/s,v(0)＝0m/s,r(0)＝0rad/s,ψ(0)＝π/2rad及s(0)＝0m；

期望速度为：u_d＝1m/s；

期望路径方程见式(27)，路径参数见表2.

控制器增益选取为k₁＝1,k₂＝0.1,k₃＝100,k₄＝1,k₅＝1,T＝0.05。施加外界环境扰动分别为d_u＝0.3sin(0.1(t-40)),d_v＝0.1sin(0.1(t-40)),d_r＝0.5sin(0.1(t-40))；采样间隔为0.01s；系统过程噪声协方差为Q＝diag{100,100,1,1,1,1,1}×10^-6；系统测量噪声协方差为R＝diag{1,1,0.01,0.01}。

表1航行器水动力参数

表2期望路径参数

路径参数	i＝0	i＝1	i＝2	i＝3	i＝4
						ai	0	0.87	-0.02	10-5	1.5×10-6
bi	0	0.5	-5×10-4	10-5	10-7

仿真结果如图3-6所示。从图3可以看出，基于UDE+UKF取得了较好的效果。图4为海洋航行器路径跟踪误差及速度曲线，可以看出在UDE+UKF控制器的作用下，路径跟踪误差渐进收敛到零，纵向速度快速收敛到设定的目标速度。图5为控制输入及虚拟向导运动控制曲线，可以看出虚拟向导运动控制律能够自动调节虚拟向导的速度，确保海洋航行器能够跟踪期望路径。图6表明本发明设计的状态和参数联合估计算法能较好的对横向通道不确定性dv进行估计。

通过分析可知，基于UDE+UKF的控制器能够实现欠驱动海洋航行器的路径跟踪，且性能受系统噪声和外界干扰的影响较小，具有较强的鲁棒性。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (5)

1.一种欠驱动海洋航行器的非线性路径跟踪控制方法，其特征在于，包括以下步骤：

构建欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型；

通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器；

采用非线性跟踪微分器对基于UDE的控制器进行简化，得到简化后的控制器；

采用UKF联合估计技术对系统状态和不确定性进行联合估计；

所述欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型表示如下：

上式中，(x_e,y_e)表示航行器质心在Serret-Frenet坐标系下的坐标，ψ_e表示航行器的航向轴与额定路径在虚拟点处的切线之间的夹角，v_t表示航行器的合成速度，表示虚拟向导速度，c_c表示期望路径的曲率，u、v和r分别表示航行器质心在惯性坐标系下的纵向角速度、横向角速度和航向角速度，τ_u和τ_r表示外部控制输入力和外部控制输入力矩，m表示航行器质量，X_u、/>X_u|u|、Y_v、/>Y_v|v|、N_r、/>和N_r|r|表示名义水动力参数，/>表示航行器在前进自由度上的附加质量，/>表示航行器在横漂自由度上的附加质量，表示航行器在艏摇自由度上的附加质量，d_u、d_v和d_r表示动力学模型集总不确定性，d_ψ表示运动学模型不确定性，z₁、z₂、z₃和z₄表示系统输出变量；

所述通过UDE对系统存在的多不确定性进行估计，并构建基于传统视线制导律的制导系统和基于UDE的控制器这一步骤，其具体包括：

采用传统视线制导律计算期望视线角；

根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法构建航向角虚拟控制律；

根据欠驱动系统的路径跟踪误差模型，结合李雅普诺夫直接法，设计位置子系统，确定虚拟向导速度控制律；

根据包含多不确定性的动力学模型，结合李雅普诺夫直接法和back-stepping法，确定外部输入力控制律和外部输入力矩控制律；

通过UDE对欠驱动系统的路径跟踪误差模型和包含多不确定性的动力学模型中的不确定性进行估计，并结合航向角虚拟控制律、外部输入力控制律和外部输入力矩控制律，得到基于UDE的控制器；

所述航向角虚拟控制律的表达式如下：

上式中，k₁＞0表示控制增益；

所述外部输入力控制律和外部输入力矩控制律的表达式如下：

其中，k₃,k₄,k₅＞0表示设计的对应控制增益，ψ_los表示期望视线角。

2.根据权利要求1所述一种欠驱动海洋航行器的非线性路径跟踪控制方法，其特征在于，所述虚拟向导速度控制律的表达式如下：

上式中，k₂＞0表示设计的控制增益。

3.根据权利要求2所述一种欠驱动海洋航行器的非线性路径跟踪控制方法，其特征在于，所述基于UDE的控制器的表达式如下：

4.根据权利要求3所述一种欠驱动海洋航行器的非线性路径跟踪控制方法，其特征在于，所述简化后的控制器的表达式如下：

上式中，r_c和为虚拟参考指令r_d通过非线性跟踪微分器后得到的跟踪信号和微分信号。

5.根据权利要求4所述一种欠驱动海洋航行器的非线性路径跟踪控制方法，其特征在于，所述采用UKF联合估计技术对系统状态和不确定性进行联合估计这一步骤，其具体包括：

考虑随机干扰、测量噪声的影响，通过增广UKF联合估计算法对控制系统的状态和横向通道不确定性进行联合估计。