CN113848958A - 基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，包括：建立基于四元数的水下机器人运动数学模型；通过水下机器人运动数学模型获取水下机器人的运动状态信息及参考轨迹，建立轨迹跟踪误差动力学方程；结合双曲正切函数设计非线性快速终端滑模变量；考虑到未知的外界扰动和时变的惯性参数，根据轨迹跟踪误差动力学方程和非线性快速终端滑模变量设计自适应容错控制器。该方法解决了全驱动水下机器人的轨迹跟踪控制问题，考虑到未知的洋流扰动、时变的惯性参数以及执行机构故障的影响，实现了水下机器人在有限时间内跟踪上期望轨迹。

Description

基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟 踪控制方法

技术领域

本发明涉及水下机器人控制技术领域，特别涉及一种基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法。

背景技术

水下机器人由于其可控性好、续航能力长、信息处理方式智能，取代了人工潜水，为人类进行海洋资源的开发和利用提供了强有力的工具。随着多功能水下机器人的发展，由于其在资源勘探、环境监测、海洋研究等各种工程任务中有着广泛应用，因此对水下机器人的进一步的开发和利用无疑有着极高的现实意义。

值得注意的是，考虑到水下机器人的总体结构存在高度耦合，模型的高度非线，理想的轨迹跟踪控制器设计仍然面临着许多挑战。首先是由于洋流扰动以及参数摄动引起的系统不确定性问题，主要包括未知的干扰力矩和时变的惯性参数；随后，欧拉角虽然作为一种标准的姿态描述方法，但水下机器人在进行大角度机动时会不可避免的出现奇异性问题；最后，由于现代水下机器人的复杂性不断增加，会不可避免地出现导致控制性能下降甚至整个控制系统瘫痪的各种故障。在所有可能的故障类型中，其中最常见的类型即为执行机构故障，包括部分失效故障及随机漂移故障等。因此，目前迫切地需要一种针对全驱动水下机器人的轨迹跟踪控制算法，以保证所开发的控制器不仅具有全局姿态描述能力，而且在面对外界扰动、参数摄动及执行机构故障条件下具有更强的鲁棒性和可靠性。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的目的在于提出一种基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，该方法实现了水下机器人在有限时间内跟踪上期望轨迹。

为达到上述目的，本发明实施例提出了基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，该方法实现了水下机器人在有限时间内跟踪上期望轨迹，包括以下步骤：步骤S1，建立基于四元数的水下机器人运动数学模型；步骤S2，通过所述水下机器人运动数学模型获取水下机器人的运动状态信息及参考轨迹，建立轨迹跟踪误差动力学方程；步骤S3，结合双曲正切函数设计非线性快速终端滑模变量；步骤S4，考虑到未知的外界扰动和时变的惯性参数，根据所述轨迹跟踪误差动力学方程和所述非线性快速终端滑模变量设计自适应容错控制器。

本发明实施例的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，构造了一个具有无奇异性和抗退绕能力的非线性滑模曲面，一方面利用双曲正切函数可以有效地解决奇异性问题，另一方面，通过将标量四元数的初始值加到滑动模态曲面上，好地解决展开问题，从而该新颖的结构滑模具有良好的性能；同时，本发明实施例在基于四元数的水下机器人姿态描述中，基于自适应控制对其执行器故障进行实时估计和补偿，与大多数相关技术中使用单位四元数对水下机器人的旋转运动进行全局描述的方法不同，如果执行器出现故障，该方案就会失去效果，与此类相关技术相比，所开发的控制器不仅具有全局姿态描述能力，而且具有容错能力。

另外，根据本发明上述实施例的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述水下机器人运动数学模型为：

其中，η₁为水下机器人在大地坐标系下的位置坐标，R(Q)为旋转矩阵，υ为水下机器人的沿机体坐标轴的线速度，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，q₀为单位四元数的标量部分，I^3×1是单位向量，ω为水下机器人的沿机体坐标轴的角速度，

为包含附加质量矩阵的正定惯性矩阵，θ＝[υ^T,ω^T]^T为水下机器人的速度量，

为在四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

进一步地，在本发明的一个实施例中，所述轨迹跟踪误差动力学方程为：

其中，η_1e为位置误差，R(Q)为旋转矩阵，

为在相对坐标系下的期望姿态，

为期望的线速度，

为期望的角速度，υ_e为线速度误差，q_ωe为单位四元数向量部分的误差，q_0e为单位四元数标称部分的误差，I^3×3为单元矩阵，ω_e为角速度误差，

为包含附加质量矩阵的正定惯性矩阵，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

进一步地，在本发明的一个实施例中，所述非线性快速终端滑模变量S为：

其中，υ_e为线速度误差，ω_e为角速度误差，α>1的常数，β和λ为大于零的常数，η_1e为位置误差，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，tanh(η_1e)为包含位置误差在内的双曲正切项，tanh(q_ωe)为包含单位四元数向量部分在内的双曲正切项。

进一步地，在本发明的一个实施例中，所述步骤S4具体为：

对集总模型的不确定性f_d进行处理，处理后为：

||f_d||≤ξ₁+ξ₂||θ||+ξ₃||θ||²＝ξψ

其中，ξ为ξ＝[ξ₁ ξ₂ ξ₃]，ψ为ψ＝[1 ||θ|| ||θ||²]^T，ξ_i,(i＝1,2,3)为未知但有界的正常数，且||ξ||≤μ，μ>0总是成立的；

定义

则所述自适应容错控制器被设计为：

τ_θ＝τ₁+τ_aux,τ₁＝τ_nom+τ_c

其中，τ_θ为基础控制器和辅助控制器的集合，τ₁为基础控制器，τ_aux为辅助控制器，τ_nom为基础控制器中用来抵消模型不确定性的集合，τ_c为基础控制器中用来抵消额外干扰的集合，ζ为辅助控制器中的自适应参数，||τ₁||为基础控制器的欧几里得范数，

为所设计的自适应律中的双曲正切项，ε₃为可调参数，S为非线性快速终端滑模变量，

为包含附加质量矩阵的正定惯性矩阵，μ为基础控制器中的自适应参数，R(Q_e)为包含单位四元数误差的旋转矩阵，

为期望的线速度，

为期望的角速度，α>1的常数，β和λ为大于零的常数，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，

为包含位置误差在内的双曲正切项求导后产生的项，

为包含单位四元数向量部分在内的双曲正切项求导后产生的项，η_1e为位置误差，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

ε₁为消除控制器抖振的参数项，||ψ||为包含线速度和角速度在内的欧几里得范数，其中，ψ＝[1 ||θ|| ||θ||²]^T，||S||为非线性快速终端滑模变量的欧几里得范数，sig^γ(S)为包含非线性快速终端滑模变量在内的符号函数，λ_max表示最大特征值，k_i(i＝1,2,3,4)，δ_i(i＝1,2,3)，γ，ε₃均为大于零的设计参数，且参数γ满足0＜γ＜1。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一个实施例的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法的流程图；

图2是本发明一个仿真实施例的水下机器人位置跟踪误差曲线；

图3是本发明一个仿真实施例的水下机器人姿态跟踪误差曲线，其中，(a)为为在初始四元数Q₀＝[0.9233,0.3613,-0.1033,-0.0797]^Τ条件下的姿态跟踪误差曲线，(b)为初始四元数Q₀＝[-0.73,0.2,0.253,0.6026]^Τ条件下的姿态跟踪误差曲线；

图4是本发明一个仿真实施例的水下机器人姿态跟踪误差曲线其中，(a)为驱动力，(b)为驱动力矩。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参照附图描述根据本发明实施例提出的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法。

图1是本发明一个实施例的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法的流程图。

如图1所示，该基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，包括以下步骤：

在步骤S1中，建立基于四元数的水下机器人运动数学模型。

其中，水下机器人运动数学模型为：

其中，η₁为水下机器人在大地坐标系下的位置坐标，R(Q)为旋转矩阵，υ为水下机器人的沿机体坐标轴的线速度，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，q₀为单位四元数的标量部分，I^3×1是单位向量，ω为水下机器人的沿机体坐标轴的角速度，

为包含附加质量矩阵的正定惯性矩阵，θ＝[υ^T,ω^T]^T为水下机器人的速度量，

为在单位四元数建模条件下的集合项，

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

具体而已，步骤S1的具体构建过程为：

式中，水下机器人在大地坐标系下的位置量为

其中η₁和η₂分别代表位置坐标和姿态角度；机体坐标系下，水下机器人的速度量为θ＝[υ^T,ω^T]^T，其中，υ为水下机器人的沿机体坐标轴的线速度，ω为水下机器人的沿机体坐标轴的角速度，方向满足右手螺旋定理，J(η)为关于欧拉角的变换矩阵，

为包含附加质量矩阵的正定惯性矩阵，

为科氏力矩阵，D(θ)为水动力阻尼矩阵；

为包括重力和浮力在内的恢复力，

为考虑执行机构故障时应用于水下机器人的实际控制向量：

τ＝Eτ_θ

式中，E＝diag{e₁,e₂,K,e₆}为有效系数矩阵且满足0＜φ≤e_i≤1，φ>0为能够维持执行机构正常运行的最低限度。

为控制输入信号。

欧拉角通常被用来定义水下机器人的姿态，但当俯仰角接近±π/2时，会产生潜在的奇异性问题。为了避免这种现象，采用基于四元数的姿态描述方法，使水下机器人能够进行复杂的三维轨迹跟踪运动。

接下来，在欧拉轴

和旋转角度χ∈[0,2π]的基础上，单位四元数

和旋转矩阵R(Q)∈SO(3)可定义为：

式中，q_ω＝[q₁,q₂,q₃]^T和q₀分别为单位四元数的向量部分和标量部分，I^3×3表示单位矩阵，另外，需要引入运算符号×，对于任意一个向量a＝[a₁,a₂,a₃]^T，a^×表示为：

利用单位四元数

的性质，则基于四元数的水下机器人运动数学模型可以被表述为：

式中，I^3×1为单位向量，f_d为集总模型的不确定性。

在步骤S2中，通过所述水下机器人运动数学模型获取水下机器人的运动状态信息及参考轨迹，建立轨迹跟踪误差动力学方程。

具体地，定义

为大地坐标系下的期望位置，

为在相对坐标系下的期望姿态，则运动学跟踪误差可表示为:

式中，

为误差四元数，Q_e描述了在机体坐标系下相对于参考坐标系的水下机器人的相对姿态，运算符o表示四元数之间的乘法。

定义

和

分别为期望的的线速度和角速度，则动力学跟踪误差可表示为:

式中，υ_e和ω_e是线速度误差和角速度误差，方向余弦矩阵R(Q_e)∈SO(3)可定义为:

为了使水下机器人达到预期的跟踪控制性能，则相应的误差动力学表达式可以被表示为：

其中，η_1e为位置误差，R(Q)为旋转矩阵，

为在相对坐标系下的期望姿态，

为期望的线速度，

为期望的角速度，υ_e为线速度误差，q_ωe为单位四元数向量部分的误差，q_0e为单位四元数标称部分的误差，I^3×3为单元矩阵，ω_e为角速度误差，

为包含附加质量矩阵的正定惯性矩阵，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

为方便后续的控制器设计，针对本发明实施例中的水下机器人轨迹跟踪控制系统，给出如下假设：

假设一：水下机器人的期望轨迹及其对时间的一阶导数是连续且有界的。

假设二：获取得到的水下机器人位置和姿态信息可用于控制器的设计。

假设三：作用于水下机器人的集总模型干扰ξ是有界的，即满足||ξ||≤μ，其中μ为大于零的未知常数。

在步骤S3中，结合双曲正切函数设计非线性快速终端滑模变量。

其中，非线性快速终端滑模变量S被设计为：

其中，υ_e为线速度误差，ω_e为角速度误差，α>1的常数，β和λ为大于零的常数，η_1e为位置误差，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，tanh(η_1e)为包含位置误差在内的双曲正切项，tanh(q_ωe)为包含单位四元数向量部分在内的双曲正切项。

为方便后续的设计和推导，本发明实施例进行了如下定义：

||g||表示向量的欧几里得范数；对于任意一个向量ξ＝[ξ₁,...,ξ_n]^T，sig(ξ)^α表示为sig(ξ)^α＝[sign(ξ₁)|ξ₁|^α,...,sign(ξ_n)|ξ_n|^α]^T，tanh(ξ)表示为tanh(ξ)＝[tanh(ξ₁),...,tanh(ξ_n)]^Ttanh²(ξ)＝[tanh²(ξ₁),...,tanh²(ξ_n)]^T。

在步骤S4中，考虑到未知的外界扰动和时变的惯性参数，根据所述轨迹跟踪误差动力学方程和所述非线性快速终端滑模变量设计自适应容错控制器。

具体地，首先，集总模型的不确定性f_d可以做如下处理：

||f_d||≤ξ₁+ξ₂||θ||+ξ₃||θ||²＝ξψ

式中，ξ的定义为ξ＝[ξ₁ ξ₂ ξ₃]，ψ的定义为ψ＝[1 ||θ|| ||θ||²]^T，ξ_i,(i＝1,2,3)为未知但有界的正常数。此外，||ξ||≤μ，μ>0总是成立的。

定义

则自适应容错控制器被设计为：

τ_θ＝τ₁+τ_aux,τ₁＝τ_nom+τ_c

式中，τ_θ为基础控制器和辅助控制器的集合，τ₁为基础控制器，τ_aux为辅助控制器，τ_nom为基础控制器中用来抵消模型不确定性的集合，τ_c为基础控制器中用来抵消额外干扰的集合，ζ为辅助控制器中的自适应参数，||τ₁||为基础控制器的欧几里得范数，

为所设计的自适应律中的双曲正切项，ε₃为可调参数，S为非线性快速终端滑模变量，

为包含附加质量矩阵的正定惯性矩阵，μ为基础控制器中的自适应参数，R(Q_e)为包含单位四元数误差的旋转矩阵，

为期望的线速度，

为期望的角速度，α>1的常数，β和λ为大于零的常数，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，I_η1e为包含位置误差在内的双曲正切项求导后产生的项，I_qωe为包含单位四元数向量部分在内的双曲正切项求导后产生的项，η_1e为位置误差，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

ε₁为消除控制器抖振的参数项，||ψ||为包含线速度和角速度在内的欧几里得范数，其中，ψ＝[1 ||θ|| ||θ||²]^T，||S||为非线性快速终端滑模变量的欧几里得范数，sig^γ(S)为包含非线性快速终端滑模变量在内的符号函数，λ_max表示最大特征值，k_i(i＝1,2,3,4)，δ_i(i＝1,2,3)，γ，ε₃均为大于零的设计参数，且参数γ满足0＜γ＜1。为了削弱抖振的影响，采用饱和函数代替符号函数sig^γ(S)。

其中，饱和函数sat(S)的表达式为

且参数设定为δ＝0.01。

下面通过一个仿真实例对本发明实施例构建的控制器的性能进行展示和验证。

给定系统状态的初始值选为：η(0)＝[0.5,0.5,0.5]^Τ，Q₀＝[-0.73,0.2,0.253,0.6026]^Τ，υ＝[0.1,0.1,0.1]^Τ，ω＝[0.1,0.1,0.1]^Τ。

其中，为了充分说明本例中的水下机器人具有抗退绕的功能，进行了一组对照仿真，改变了四元数初始值，选定为Q₀＝[0.9233,0.3613,-0.1033,-0.0797]^Τ。

外界干扰f_d被设定为：

f_d1＝0.02sin(0.02t),f_d2＝0.03sin(0.15t)

f_d3＝0.02sin(0.04t)+0.03cos(0.02t)

f_d4＝0.015cos(0.02t),f_d6＝0.001sin(0.01t)

f_d5＝0.015sin(0.03t)+0.05cos(0.03t)

水下机器人的参考轨迹设定为：

η_d(0)＝[0.1,-0.2,0.3]^Τ

Q_d＝[0,0,0,1]^Τ

υ_d＝[0.5,0.5,0.3]^Τ

ω_d＝10^-2[4sin(t/30),4sin(t/40),4sin(t/50)]^Τ

执行机构的容错系数为：

本例中将控制器的各参数取值为：

α＝1.5,β＝0.05,λ＝0.1

k₁＝5,k₂＝1,k₃＝0.5,γ＝0.1,δ₁＝1,δ₂＝5

k₄＝0.01,ε₃＝0.1,δ₃＝0.05

如图2和3所示，容错控制方案的轨迹跟踪误差在20秒时发生变化，但很快就趋于稳定，说明尽管执行器存在故障，水下机器人仍然可以很好地跟踪轨迹。此外，图3的仿真结果说明即使四元数的初始值不同，q₀也不会越过零点边界到达相反的平衡点，从而防止了退绕现象的发生。如图4所示，控制力矩使水下机器人保持在可控范围内向期望的轨迹移动。由此可知本发明实施例提出的执行机构故障的水下机器人有限时间轨迹追踪制算法具备抗退绕的性能，可使水下机器人准确跟踪参考轨迹。

综上，本发明实施例提出的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，具有以下有益效果：

(1)构造了一个具有无奇异性和抗退绕能力的非线性滑模曲面，一方面利用双曲正切函数可以有效地解决奇异性问题，另一方面，通过将标量四元数的初始值加到滑动模态曲面上，好地解决展开问题，从而该新颖的结构滑模具有良好的性能；

(2)本发明实施例在基于四元数的水下机器人姿态描述中，基于自适应控制对其执行器故障进行实时估计和补偿，与大多数相关技术中使用单位四元数对水下机器人的旋转运动进行全局描述的方法不同，如果执行器出现故障，该方案就会失去效果，与此类相关技术相比，本发明实施例所开发的控制器不仅具有全局姿态描述能力，而且具有容错能力。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (5)

1.一种基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤S1，建立基于四元数的水下机器人运动数学模型；

步骤S2，通过所述水下机器人运动数学模型获取水下机器人的运动状态信息及参考轨迹，建立轨迹跟踪误差动力学方程；

步骤S3，结合双曲正切函数设计非线性快速终端滑模变量；

步骤S4，考虑到未知的外界扰动和时变的惯性参数，根据所述轨迹跟踪误差动力学方程和所述非线性快速终端滑模变量设计自适应容错控制器。

2.根据权利要求1所述的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，其特征在于，所述水下机器人运动数学模型为：

其中，η₁为水下机器人在大地坐标系下的位置坐标，R(Q)为旋转矩阵，υ为水下机器人的沿机体坐标轴的线速度，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，q₀为单位四元数的标量部分，I^3×1是单位向量，ω为水下机器人的沿机体坐标轴的角速度，

为包含附加质量矩阵的正定惯性矩阵，θ＝[υ^T,ω^T]^T为水下机器人的速度量，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

3.根据权利要求1所述的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，其特征在于，所述轨迹跟踪误差动力学方程为：

其中，η_1e为位置误差，R(Q)为旋转矩阵，

为在相对坐标系下的期望姿态，

为期望的线速度，

为期望的角速度，υ_e为线速度误差，q_ωe为单位四元数向量部分的误差，q_0e为单位四元数标称部分的误差，I^3×3为单元矩阵，ω_e为角速度误差，

为包含附加质量矩阵的正定惯性矩阵，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

为考虑执行机构故障时应用于水下机器人的实际控制向量，f_d为集总模型的不确定性。

4.根据权利要求1所述的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，其特征在于，所述非线性快速终端滑模变量S为：

其中，υ_e为线速度误差，ω_e为角速度误差，α>1的常数，β和λ为大于零的常数，η_1e为位置误差，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，tanh(η_1e)为包含位置误差在内的双曲正切项，tanh(q_ωe)为包含单位四元数向量部分在内的双曲正切项。

5.根据权利要求1所述的基于四元数的全驱动抗退绕水下机器人有限时间容错轨迹跟踪控制方法，其特征在于，所述步骤S4具体为：

对集总模型的不确定性f_d进行处理，处理后为：

||f_d||≤ξ₁+ξ₂||θ||+ξ₃||θ||²＝ξψ

其中，ξ为ξ＝[ξ₁ξ₂ξ₃]，ψ为ψ＝[1 ||θ|| ||θ||²]^T，ξ_i,(i＝1,2,3)为未知但有界的正常数，且||ξ||≤μ，μ>0总是成立的；

定义

则所述自适应容错控制器被设计为：

τ_θ＝τ₁+τ_aux,τ₁＝τ_nom+τ_c

其中，τ_θ为基础控制器和辅助控制器的集合，τ₁为基础控制器，τ_aux为辅助控制器，τ_nom为基础控制器中用来抵消模型不确定性的集合，τ_c为基础控制器中用来抵消额外干扰的集合，ζ为辅助控制器中的自适应参数，||τ₁||为基础控制器的欧几里得范数，

为所设计的自适应律中的双曲正切项，ε₃为可调参数，S为非线性快速终端滑模变量，

为包含附加质量矩阵的正定惯性矩阵，μ为基础控制器中的自适应参数，R(Q_e)为包含单位四元数误差的旋转矩阵，

为期望的线速度，

为期望的角速度，α>1的常数，β和λ为大于零的常数，q_ωe为四元数向量部分的误差，q_0e为四元数标称部分的误差，q_ω＝[q₁,q₂,q₃]^T为单位四元数的向量部分，

为包含位置误差在内的双曲正切项求导后产生的项，

为包含单位四元数向量部分在内的双曲正切项求导后产生的项，η_1e为位置误差，

为在单位四元数建模条件下的集合项，包括科氏力矩阵

水动力阻尼矩阵D(θ)以及包含重力和浮力在内的恢复力

ε₁为消除控制器抖振的参数项，||ψ||为包含线速度和角速度在内的欧几里得范数，其中，ψ＝[1 ||θ|| ||θ||²]^T，||S||为非线性快速终端滑模变量的欧几里得范数，sig^γ(S)为包含非线性快速终端滑模变量在内的符号函数，λ_max表示最大特征值，k_i(i＝1,2,3,4)，δ_i(i＝1,2,3)，γ，ε₃均为大于零的设计参数，且参数γ满足0＜γ＜1。

Images