CN112486209A - 一种自主水下机器人三维路径跟踪方法、装置及存储介质 - Google Patents

Abstract

本发明公开了一种自主水下机器人三维路径跟踪方法，先构建全驱动自主水下机器人动力学模型；设计时间最优控制结合的二阶滑模控制器和引入动态面的自适应反步动态滑模控制器；将上述两者相结合，对非线性系统进行控制。本发明还提供了基于上述方法的自主水下机器人三维路径跟踪装置及存储介质。本发明解决了控制系统计算爆炸的问题，简化了系统的复杂程度，消除传统滑模存在的抖振现象，增强了控制系统的鲁棒性。

Description

一种自主水下机器人三维路径跟踪方法、装置及存储介质

技术领域

本发明涉及水下机器人路径跟踪领域，尤其涉及一种近似时间二阶滑模和动态面相融合的水下机器人路径跟踪方法、装置及存储介质。

背景技术

AUV尺寸小、速度低，受海洋环境影响明显，动力学模型复杂，因此对AUV控制系统的设计提出了严格的要求。最早由于传统的控制理论不能解决复杂的非线性系统的控制问题，直到计算机控制技术、人工智能技术和先进控制理论的长足发展，国内外的学者对于AUV的三维路径跟踪提出了多种智能算法，例如，自适应控制、滑模控制、模糊控制和神经网络控制等各种控制方法，但是由于单一的水下机器人控制方法，在实际应用中会暴露出很多缺点，影响系统的控制性能。如Levant等人设计一种新型的二阶滑模控制方法，T.Salgado和Imran等人成功的将此方法应用到AUV的三维路径跟踪控制上。还有通过将滑模控制和模糊控制算法相结合，不仅削弱传统滑模抖振并且提高了系统的控制精度，Hebertt等人提出反步滑模控制这一概念，该控制算法结合了反步方法和滑模控制的优点，进一步扩大了控制算法的应用范围。

现有技术公开了采用改进的非奇异终端滑模控制算法进行水下机器人三维路径跟踪的方法，在滑模控制中，设计分段切换函数，实现多个滑模动态，通过模糊算法对其进行优化，提高了其AUV在回收直线归位阶段的跟踪控制精度。但该方法的不足是在远离平衡点时系统的状态收敛速度较慢，同时与全局到达条件结合单位向量控制律，不可避免得存在抖振问题，此外，滑模超平面中指数项参数要求为奇数，也限制了参数的选取范围，降低了滑模面设计的灵活性。

此外，现有技术中依旧存在没有有效减弱抖振对系统的影响，使用的神经网络方法计算量过大导致系统的响应速度变慢，对未知的水下干扰项没有进一步的减弱影响，水下适应能力弱等问题。

发明内容

发明目的：本发明提出一种解决了系统不确定性问题的鲁棒性好、自适应性强的自主水下机器人三维路径跟踪方法。本发明还提供了基于上述方法的水下机器人三维路径跟踪装置及存储介质。

技术方案：本发明所述的自主水下机器人三维路径跟踪方法，包括步骤：

(1)构建全驱动自主水下机器人动力学模型；

(2)设计时间最优控制结合的二阶滑模控制器，提高状态反馈控制效率；同时，在根据反步法进行迭代控制时添加了非线性阻尼项，使系统对未知的环境干扰适应性变强，也使控制算法更加系统化、结构化；解决了在控制系统算法在设计过程中计算爆炸的问题，简化了系统的复杂程度。引入二阶动态滑模控制，消除传统滑模存在的抖振现象，增强了控制系统的鲁棒性。

(3)设计引入动态面的自适应反步动态滑模控制器，优化改进系统环境；

(4)将所述时间最优控制结合的二阶滑模控制器与所述引入动态面的自适应反步动态滑模控制器相结合，对非线性系统进行控制，减少计算量，提高系统反应度和精度。

本发明提供了一种自主水下机器人三维路径跟踪装置，包括存储器、处理器及在所述存储器上存储并可运行的自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现所述自主水下机器人三维路径跟踪方法的部分或全部步骤。

本发明还提供了一种计算机可读存储介质，其上存储有自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现所述自主水下机器人三维路径跟踪方法的部分或全部步骤。

有益效果：本方法基于反步滑模算法，采用时间最优控制相结合自适应控制法、反步法、动态面二阶滑模控制进行改进，使得系统在远离平衡点时，能够选取时间最优的参数，快速收敛到滑模面，提高了系统的响应速度并且减弱了滑模控制技术固有的抖振问题，解决了系统的不确定性。

附图说明

图1是本发明方法流程图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

本发明所述的自主水下机器人三维路径跟踪方法，包括：

(1)构建全驱动自主水下机器人动力学模型，具体包括步骤：建立固定坐标系、机器人载体坐标系，构建全驱动自主水下机器人动力学模型：

式中，η＝[x y z θ ψ]^T,x、y、z分别为水下机器人相对于惯性坐标系中的位置，θ、ψ为姿态角，其中θ为俯仰角，ψ为航向角；v＝[u v w q r]^T,为水下机器人的载体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z方向的线速度，q,r为y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为包含附加质量的惯量矩阵且满足对称性M＝M^T>0，

C(v)v为包含了质量矩阵和附加质量矩阵引起的科氏力与向心力且满足反对称性；D(v)为黏性水动力系数矩阵且D(v)>0；v为去除环境干扰时水下机器人的速度；g(η)是恢复力和力矩；τ是各自由度的力和力矩；ξ为环境干扰。

(2)设计时间最优控制结合的二阶滑模控制器，具体包括步骤：

对反步滑模控制器设计，首先假设被控对象为如下的n阶的非线性系统：

其中，x[x₁,x₂,…x_n]^T∈Rⁿ为系统的可测状态向量，u∈R为控制输入，y∈R为系统的输出，b(x,t)≠0,d(t)为外界未知干扰和建模误差等不确定性影响总和，且|d(t)|≤D+η＝D_η，D为不确定性影响总和d(t)值的绝对值的最大值，η为很小的正实数。假设d(t)是慢时变的，即

在保证所有状态量有界的条件下，基于反步法的思想来设计合适的滑模控制律，使系统的输出y＝x₁跟踪期望输出y_d是控制器设计的目的。

(21)定义跟踪误差为：

z₁＝y-y_d

则

定义虚拟控制量：

其中,k₁>0,k₁为常数；

定义z₂＝x₂-α₁

其中，α₁为虚拟控制量；

定义Lyapunon函数：

对定义的李亚普若夫函数求一阶导，再把定义的跟踪误差代入上一步得到：

依次类推到n-1步可得：

(22)考虑到滑模控制得强鲁棒性，将其引入到在反步法的最后一步，即第n步，设计如下的滑模面：

S＝c₁z₁+…+c_n-1z_n-1

选取常数c₁,c₂,…c_n-1,使得多项式P(p)＝p^(n-1)+c_n-1p^n-2+…+c₂p+c₁为Hurwitz稳定，P为Laplace算子；

定义李亚普若夫函数：

设计控制器为：

其中，h和S为正常数，sign为符号函数，b(x,t)≠0,D为不确定影响总和，

为虚拟控制量，x∈Rⁿ为系统的状态向量，t为时间且t≥0，f(x,t):R₊×Rⁿ为系统的非线性向量函数，状态数n表示系统的自由度；

(23)设计时间最优控制结合的二阶滑模控制器设计，提高状态反馈控制效率。

建立惯用的非线性系统模型：

式中，X∈Rⁿ,U∈R分别为系统的状态向量和控制量，ΔA,ΔB为参数摄动，D为干扰参数向量，F为外干扰项向量，A,B为系统参数；

选取合适的切换函数：

s＝C^TX＝x_n+c_n-1x_n-1+…+c₂x₂+c₁x₁

式中，C∈Rⁿ，采用极点配置或二次型最优法确定矢量C，确保系统进入到滑动模态后，具有理想的动态特性；

计算s的一阶和二阶导数：

X∈Rⁿ,U∈R分别为系统的状态向量和控制量，ΔA,ΔB为参数摄动，D为干扰参数向量，F为外干扰项向量，A,B为系统参数项；

令y₁＝s,

构造其方程组

满足性能指标

最短，其中t_f为时间t的积分上限，导出v(t)：

使y₁和y₂从任意初始值开始，以最短时间达到平衡点，v的符号只切换一次，M为趋近滑模的最大速度，sign为符号函数；

(24)以最短的时间来达到平衡点，路径有两段抛物线组成。在设计控制器时，假设非线性系统中的参数摄动和外部干扰为零时，计算虚拟控制率U：

其中，

是关于时间τ的函数，τ的积分上限为t，A,B为参数项，C∈Rⁿ，v为关于t的函数，X∈Rⁿ；

(25)将求得控制率代入s的二阶导数，令r_b＝(C^TΔB)(C^TB)^-1，ΔB为参数摄动，C^T为C的转秩且C∈Rⁿ，将C^TΔB记作Δb，将C^TB记作b，则r_b＝Δb/b；

计算二阶滑模达到率：

令G(t)＝G_XX_(t)+G_uU(t)+G_f(t),K＝1+r_b,

根据假设可得到‖G_X‖、‖G_U‖、‖G_f‖有界，|r_b|≤r<1,r为正实数，则有1-r<K<1+r，得到二阶滑模达到率

理想情况下，G(t)、K视为外界干扰和参数摄动。

(3)设计引入动态面的自适应反步动态滑模控制器，具体包括步骤：

(31)通过在线辨识的方式计算不确定性影响总和和

定义李亚普若夫函数：

其中，

为估计误差，

为d的估计值，γ为正常数；

(32)设计自适应控制器为：

其中，h和S为正常数,sign为符号函数，f(x,t)为系统函数，b(x,t)≠0,D为不确定影响总和，

为虚拟控制量；

(33)设计自适应律为：

(34)引入动态面控制算法，首先定义跟踪误差：

S₁＝x₁-x_1d

定义V的李雅普诺夫函数为

其一阶时间导数为：

将步骤(23)中定义的非线性系统的

等式代入上式，再利用Young不等式

ρ₁为正实数，ε为任意小实数，d₁(t,x)为外界未知干扰和建模误差不确定因素总和。选择稳定函数

使得S₁→0，得到：

k₁为常数，

为期望平均值，f₁(x₁)为系统函数，经过低通滤波器式，可得期望信号x_2d：

ε是一个任意小的正常数，它是期望跟踪精度的量度,最后得到动态面控制器和定义的跟踪误差：

s_i＝x_i-x_id,i＝1,…,n-1

s_n＝x_n-x_nd

s_n为定义的跟踪的误差，u_n为引入动态面的自适应反步动态滑模控制器。

采用引入动态面的设计融入到自适应反步滑模算法，解决计算膨胀的问题和降低虚拟光滑信号的要求。

(4)将所述时间最优控制结合的二阶滑模控制器与所述引入动态面的自适应反步动态滑模控制器相结合，对非线性系统进行控制，具体包括：

采用动态面控制算法，推导出第n-2步，计算出期望信号计算出期望信号x_n-1d；然后定义出滑模变量S_n-1＝σ＝x_n-1-x_n-1d，S_n-1为输出的相对度为2，后采用混合二阶滑模控制算法式为：

u₂＝-λ|σ|^ρsignσ

u＝u₁+u₂

其中，σ为滑模变量，sign为符号函数，u为混合二阶滑模控制器，β₁，λ，α为正常数；

根据所述混合二阶滑模控制算法对非线性系统进行控制。

本发明所述的自主水下机器人三维路径跟踪装置，包括存储器、处理器及在所述存储器上存储并可运行的自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现所述自主水下机器人三维路径跟踪方法的部分或全部步骤。

本发明所述的计算机可读存储介质，其上存储有自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现所述自主水下机器人三维路径跟踪方法的部分或全部步骤。

本发明采用了时间最优二阶滑模控制器采用与时间最优控制方法，来构造滑模面的二阶到达条件，对滑模的切换面起到了简化的作用，用约束条件保证系统的稳定性和鲁棒性，既提高了控制系统的响应速度，又尽可能降低了滑模固有的抖振。二阶滑模控制将实际控制信号的微分信号作为控制输入，设计为不连续信号，而相应的实际控制信号是连续的，从而有效削弱传统滑模所存在的抖振现象。自适应反步动态滑模控制器采用动态面控制方法，利用一阶积分滤波器来计算虚拟控制的导数，可消除微分项，使控制器和参数设计简单，从而避免了“计算爆炸”的问题，简化了系统的复杂程度。使用的自适应控制，得到我们所需的控制率，能够修正自己的特性以适应对象和扰动的特性变化，在控制系统中不断提取有关模型信息，使模型越来越准确。

Claims (8)

1.一种自主水下机器人三维路径跟踪方法，其特征在于，包括步骤：

(1)构建全驱动自主水下机器人动力学模型；

(2)计算时间最优控制的二阶滑模达到率；

(3)设计引入动态面的自适应反步动态滑模控制器；

(4)基于所述时间最优控制的二阶滑模达到率，对所述引入动态面的自适应反步动态滑模控制器进行改进，得到混合二阶滑模控制器，利用所述混合二阶滑模控制器对水下机器人进行路径跟踪。

2.根据权利要求1所述的自主水下机器人三维路径跟踪方法，其特征在于，步骤(1)包括：建立惯性坐标系、水下机器人载体坐标系，构建全驱动自主水下机器人动力学模型：

式中，η＝[x y z θ ψ]^T,x、y、z分别为水下机器人相对于惯性坐标系中的位置，θ、ψ为姿态角，其中θ为俯仰角，ψ为航向角；v＝[u v w q r]^T,为水下机器人的载体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z方向的线速度，q,r为y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为包含附加质量的惯量矩阵且满足对称性M＝M^T>0，

C(v)v为包含了质量矩阵和附加质量矩阵引起的科氏力与向心力且满足反对称性；D(v)为黏性水动力系数矩阵且D(v)>0；v为去除环境干扰时水下机器人的速度；g(η)是恢复力和力矩；τ是各自由度的力和力矩；ξ为环境干扰。

3.根据权利要求1所述的自主水下机器人三维路径跟踪方法，其特征在于，所述步骤(2)包括：

(21)定义跟踪误差为：

z₁＝y-y_d

其中，z₁为定义误差推导的初始值，y为实际路径，y_d为期望路径；

则定义误差一阶导为：

其中，

为定义误差一阶导，

和

都是定义误差函数中的一阶导；

和

和x₂定义相同，x₂为x₁的一阶导；

定义虚拟控制量：

其中,k₁>0,k₁为常数；

定义z₂＝x₂-α₁

其中，α₁为虚拟控制量；

定义Lyapunon函数：

对定义的李亚普若夫函数求一阶导，再把定义的跟踪误差代入上一步得到：

根据反步法依次类推到n-1步可得：

(22)设计如下的滑模面：

S＝c₁z₁+…+c_n-1z_n-1

选取常数c₁,c₂,…c_n-1,使得多项式P(p)＝p^(n-1)+c_n-1p^n-2+…+c₂p+c₁为Hurwitz稳定，P为Laplace算子；

定义李亚普若夫函数：

设计控制器为：

其中，h和S为正常数，sign为符号函数，b(x,t)≠0,D为不确定影响总和，

为虚拟控制量，x∈Rⁿ为系统的状态向量，t为时间且t≥0，f(x,t):R₊×Rⁿ为系统的非线性向量函数，状态数n表示系统的自由度；

(23)建立水下机器人的非线性运动控制系统：

式中，X∈Rⁿ,U∈R分别为系统的状态向量和控制量，ΔA,ΔB为参数摄动，D为干扰参数向量，F为外干扰项向量；A,B为系统参数；

构造切换函数：

s＝C^TX＝x_n+c_n-1x_n-1+…+c₂x₂+c₁x₁

式中，矢量C∈Rⁿ的选取确保系统进入到滑动模态后具有动态特性；

计算s的一阶和二阶导数：

X∈Rⁿ,U∈R分别为系统的状态向量和控制量，ΔA,ΔB为参数摄动，D为干扰参数向量，F为外干扰项向量，A,B为系统参数项；

令y₁＝s,

构造其方程组

满足性能指标

最短，其中t_f为时间t的积分上限，导出v(t)：

v₂＝M*sign(y₁),

使y₁和y₂从任意初始值开始，以最短时间达到平衡点，v的符号只切换一次，M为趋近滑模的最大速度，sign为符号函数；

(24)计算虚拟控制率U：

其中，

是关于时间τ的函数，τ的积分上限为t，A,B为参数项，C∈Rⁿ，v为关于t的函数，X∈Rⁿ；

(25)将求得虚拟控制率代入s的二阶导数，令r_b＝(C^TΔB)(C^TB)^-1＝Δb/b，ΔB为参数摄动，C^T为C的转秩且C∈Rⁿ，得到二阶滑模达到率：

令G(t)＝G_XX_(t)+G_uU(t)+G_f(t),K＝1+r_b,

根据假设可得到‖G_X‖、‖G_u‖、‖G_f‖有界，|r_b|≤r<1,r为正实数，则有1-r<K<1+r，得到二阶滑模达到率

G(t)、K视为外界干扰和参数摄动。

4.根据权利要求1所述的自主水下机器人三维路径跟踪方法，其特征在于：所述步骤(23)采用极点配置或二次型最优法确定矢量C。

5.根据权利要求1所述的自主水下机器人三维路径跟踪方法，其特征在于，所述步骤(3)包括：

(31)通过在线辨识的方式计算不确定性影响总和和

定义李亚普若夫函数：

其中，

为估计误差，

为d的估计值，γ为正常数；

(32)设计自适应控制器为：

其中，h和S为正常数,sign为符号函数，f(x,t)为系统函数，b(x,t)≠0,D为不确定影响总和，

为虚拟控制量；

(33)设计自适应律为：

(34)引入动态面控制算法，首先定义跟踪误差：

S₁＝x₁-x_1d

定义V的李雅普诺夫函数为

其一阶时间导数为：

将步骤(23)中定义的非线性系统的

等式代入上式，再利用Young不等式

ρ₁为正实数，ε为任意小实数，d₁(t,x)为外界未知干扰和建模误差不确定因素总和。选择稳定函数

使得S₁→0，得到：

k₁为常数，

为期望平均值，f₁(x₁)为系统函数，经过低通滤波器式，可得期望信号x_2d：

ε是一个任意小的正常数，它是期望跟踪精度的量度,最后得到动态面控制器和定义的跟踪误差：

s_i＝x_i-x_id,i＝1,…,n-1

s_n＝x_n-x_nd

s_n为定义的跟踪的误差，u_n为引入动态面的自适应反步动态滑模控制器。

6.根据权利要求1所述的自主水下机器人三维路径跟踪方法，其特征在于，所述步骤(4)包括：

采用动态面控制算法，推导出第n-2步，计算出期望信号计算出期望信号x_n-1d；然后定义出滑模变量S_n-1＝σ＝x_n-1-x_n-1d，S_n-1为输出的相对度为2，后采用混合二阶滑模控制算法式为：

u₂＝-λ|σ|^ρsignσ

u＝u₁+u₂

其中，σ为滑模变量，sign为符号函数，u为混合二阶滑模控制器，β₁，λ，α为正常数；

根据所述混合二阶滑模控制器对水下机器人进行路径跟踪。

7.一种自主水下机器人三维路径跟踪装置，其特征在于，该装置包括：存储器、处理器及在所述存储器上存储并可运行的自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现如权利要求1至6中任一项所述自主水下机器人三维路径跟踪方法的步骤。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有自主水下机器人三维路径跟踪的程序，所述自主水下机器人三维路径跟踪的程序被处理器执行时实现如权利要求1至6中任一项所述自主水下机器人三维路径跟踪方法的步骤。

Images