CN112936277A - 一种水下机器人-机械手系统固定时间轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种水下机器人‑机械手系统固定时间轨迹跟踪方法，属于机器人控制领域，该方法包括以下步骤：设计固定时间扰动观测器，采用固定时间扰动观测器在固定时间内实现对水下机器人‑机械手系统的未建模动态与外界扰动的进行观测，得到未建模动态与外界扰动对水下机器人‑机械手系统的影响力矩；设计固定时间积分滑模控制器，固定时间积分滑模控制器根据影响力矩产生一个补偿力矩，抵消未建模动态与外部扰动对水下机器人‑机械手系统的影响，最终固定时间积分滑模控制器实现水下机器人‑机械手系统的运行轨迹的精确跟踪；该方法所设计的固定时间积分滑模控制器，可在固定时间内跟踪期望轨迹，实现了系统的固定时间稳定。

Description

一种水下机器人-机械手系统固定时间轨迹跟踪方法

技术领域

本发明涉及机器人控制领域,尤其涉及一种水下机器人-机械手系统固定时间轨迹跟踪方法。

背景技术

滑模变结构控制对系统未建模部分不敏感，具有响应速度快、鲁棒性强等特点因此广泛应用于非线性系统的控制，因此更加适用于复杂工作环境下的水下机器人-机械手系统(Underwater Vehicle-Manipulator Systems，UVMS)。Wang等人提出基于时延估计的非奇异终端滑模控制方法，结合时延估计无需动力学模型的特性和非奇异终端滑模控制的强鲁棒性，使算法具有良好的控制性能和抗干扰能力，实现复杂干扰下UVMS的轨迹跟踪控制；汤奇荣等人提出一种基于指数趋近律的滑模变结构控制方法，仿真实验证明响应速度较、控制误差小，能够有效地实现UVMS的运动轨迹控制；Huang等人提出来一种基于非线性扰动观测器的反步滑模控制方法，实现对外界扰动的准确观测，提高了移动机械臂末端的轨迹跟踪控制精度。此外，Wang等人针对水面无人艇编队控制问题，设计了一种积分滑模控制策略，通过Lyapunov稳定性理论证明了所设计编队控制方法的整体稳定性，仿真实验证明所设计控制方案能够有效提高水面无人艇编队系统的精确性与鲁棒性,但积分滑模技术在UVMS轨迹跟踪控制领域鲜有应用，具有一定潜在研究价值。

现阶段，国内外学者应用在UVMS中的控制方法有PID控制方法、神经网络控制方法、自适应控制方法与滑模控制方法。张铭钧教授课题组提出了一种双闭环PID运动控制器，分别基于速度PI和位置PD减小了UVMS速度响应时间与位置超调量；Xu等为估算离散形式下的UVMS未知动态，采用了神经网络补偿器，并设计了一种基于模糊PD的神经网络控制算法，该算法对载荷变化与外界扰动的影响具有强鲁棒性；Lu等通过低通滤波器在UVMS动力学模型中得到一个近似的辅助模型，从而减小模型参数受扰动产生的大幅度振荡，证明载体的稳定是执行器轨迹准确跟踪的必要前提，设计了一种频率受限的自适应控制方法。PID控制方法相对成熟，并在实际工程中广泛应用，其控制参数需要适应模型参数的变化；神经网络控制并不依赖精确的数学模型，学习能力和适应能力强，但神经网络控制需要通过大量的样本训练来提高控制精度；自适应控制适用于模型参数不确定性强、参数变化缓慢和自由度较少的场合，但控制稳定性较差。但UVMS模型有很大不确定性，且自由度较高、参数变化快，因此PID控制方法、神经网络控制、自适应控制方法不完全适用于UVMS的控制。

发明内容

根据现有技术存在的问题，本发明公开了一种水下机器人-机械手系统固定时间轨迹跟踪方法，包括以下步骤：

S1：设计固定时间扰动观测器，采用固定时间扰动观测器在固定时间内实现对水下机器人-机械手系统的未建模动态与外界扰动的进行观测，得到未建模动态与外界扰动对水下机器人-机械手系统的影响力矩；

S2：设计固定时间积分滑模控制器，固定时间积分滑模控制器根据影响力矩产生一个补偿力矩，抵消未建模动态与外部扰动对水下机器人-机械手系统的影响，最终固定时间积分滑模控制器实现水下机器人-机械手系统的运行轨迹的精确跟踪。

进一步地，所述固定时间扰动观测器的表达式如下：

其中κ₁，κ₂，κ₃＞0，为扰动观测器控制系数，且κ₃＞L_d，γ₁＞0，0＜γ₂＜1，为扰动观测器增益系数，z₁为速度误差ω_e的观测值，观测误差e₁＝z₁-ω_e，z₂为外界扰动δ_d的观测值，观测误差e₂＝z₂-δ_d。

进一步地，所述固定时间积分滑模控制器的表达式如下：

其中λ₁,λ₂,λ₃＞0，为固定时间积分滑模控制器控制系数；k₁,k₂为滑模面控制系数；β₁,β₂为滑模面增益系数；p与q为奇数，满足0＜p＜q。

由于采用了上述技术方案，本发明提供的一种水下机器人-机械手系统固定时间轨迹跟踪方法，设计了一种固定时间扰动观测器，能在固定时间内实现对水下机器人-机械手系统未建模动态与外界扰动进行观测，且收敛时间上界不受扰动初值影响；本发明所设计的固定时间积分滑模控制器，可在固定时间内跟踪期望轨迹，同时控制输入光滑，降低了控制系统的抖振，并且实现了系统的固定时间稳定。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为惯性坐标系与附体坐标系图；

图2为UVMS运动轨迹跟踪图；

图3为UVMS位姿状态图；

图4为UVMS速度状态图；

图5为UVMS位姿误差图；

图6为UVMS速度误差图；

图7为FTDO观测值图；

图8为FTDO观测误差图；

图9为UVMS控制输入图。

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：

本发明设计了一种水下机器人-机械手系统固定时间轨迹跟踪控制方法，该方法也可称为基于固定时间扰动观测器的积分滑模控制策略(Integral Sliding ModeControl Based on Fixed-time Disturbance Obeserver，FTDO-ISMC)，该方法包括以下步骤：

S1：设计固定时间扰动观测器(FTDO)，采用固定时间扰动观测器在固定时间内实现对水下机器人-机械手系统的未建模动态与外界扰动的进行观测，得到未建模动态与外界扰动对水下机器人-机械手系统的影响力矩；

S2：设计固定时间积分滑模控制器(ISMC)，固定时间积分滑模控制器根据影响力矩产生一个补偿力矩，抵消未建模动态与外部扰动对水下机器人-机械手系统的影响，最终固定时间积分滑模控制器实现水下机器人-机械手系统的运行轨迹的精确跟踪。

其中固定时间积分滑模控制器的工作过程包括以下两步：

S2-1：固定时间积分滑模控制器在固定时间内到达滑模面；

S2-2：到达滑模面，跟踪误差系统在有限时间内收敛至零，实现对UVMS的精准轨迹跟踪。

在水下机器人-机械手系统这一类非线性系统中，有以下定义：

定义1：考虑以下非线性系统：

其中，x＝[x₁,...,x_n]^T为n维系统的状态向量，f(x(t))为原点邻域上的非线性连续函数，如果系统(1)渐进稳定并具有负齐次度，则该系统是有限时间稳定的。

定义2：如果系统(1)为有限时间稳定，并且收敛时间T有界，则对于任意x∈Rⁿ，存在T_max＞0，使T≤T_max，则此刻系统是固定时间稳定的；

引理1：考虑一标量函数：

其中，k₁,k₂,k₃＞0，k₃≥|k₄|，0＜p＜q，且p，q均为奇数。此时，式(2)为固定时间稳定，且收敛时间T不受初值的影响并满足下式：

引理2：在跟踪误差系统这一类二阶系统中，有以下引理：

能够保证

的有限时间稳定。

其中，l₁,l₂＞0，0＜m₁＜1，m₂＝2m₁/(1+m₁)，

其中i＝1,2。

为描述水下机器人-机械手系统(UVMS)运动状态，图1为惯性坐标系与附体坐标系图；其中，惯性坐标系(E-XYZ)以地球上一定点E为坐标原点，X轴、Y轴、Z轴分别指向正北、正东与地心；附体坐标系(o-xyz)以UVMS重心为坐标原点，x轴、y轴、z轴分别指向水下机器人正前方、右舷与底部；

将UVMS未建模动态与外界扰动由一集总扰动δ_d代表；

假设1：假设集总扰动δ_d有上界，既满足||δ_d||≤L_d(L_d＜∞)；

包含四自由度水下机器人与二自由度水下机械手的动力学模型：

其中，η＝[x_m,y_m,z_m,ψ,θ₁,θ₂]^T为UVMS在E-XYZ下的位姿向量，[x_m,y_m,z_m]^T为机械手末端的位置向量，[ψ,θ₁,θ₂]^T分别代表航向角与关节角向量；υ＝[u,v,w,r,q₁,q₂]^T为UVMS在o-xyz下的速度向量，[u,v,w,r]^T代表水下机器人的速度向量，[q₁,q₂]^T代表机械手关节角的速度向量；τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T分别表示水下机器人纵向、横向、垂向、航向角与机械手关节上的广义控制输入，τ_δ＝MJ(η)^-1δ_d，代表抵消δ_d影响的补偿输入，J(η)表示UVMS在惯性坐标系与附体坐标系转换矩阵，描述为：

其中J₁₄至J₂₆由式(7)表示，s(·)，c(·)分别表示sin(·)，cos(·)：

M表示UVMS质量矩阵(包含附加质量)，且满足M＝M^T＞0，描述为：

其中，m＝m_v+m₁+m₂，m为水下机器人-机械手系统总质量，m_v、m₁、m₂分别为水下机器人本体与机械手关节的质量，

分别为六个自由度上的附加质量，I_z、I₁、I₂代表转动惯量，满足下式：

C(υ)表示科氏向心力矩阵，描述为：

D(υ)表示阻尼矩阵:

D(υ)＝-diag(X_u+X_u|u||u|,Y_v+Y_v|v||v|,Z_w+Z_w|w||w|,N_r+N_r|r||r|,K_q1+K_q1|q1||q1|,K_q2+K_q2|q2|q2|) (11)

其中：X_u、Y_v、Z_w、N_r、K_q1、K_q2分别为横向、纵向、垂向、航向角以及机械手关节角的一阶阻尼系数，X_u|u|、Y_v|v|、Z_w|w|、N_r|r|、K_q1|q1|、K_q2|q2|分别为横向、纵向、垂向、航向角以及机械手关节角的二阶阻尼系数。

根据式(5)，可以水下机器人-机械手系统额(UVMS)得到期望轨迹下的数学模型：

式(12)中，η_d＝[x_d,y_d,z_d,ψ_d,θ_1d,θ_2d]^T表示UVMS在惯性坐标系下的期望位姿向量，υ_d＝[u_d,v_d,w_d,r_d,q_1d,q_2d]^T表示期望速度和角速度，τ_d＝[τ_d1,τ_d2,τ_d3,τ_d4,τ_d5,τ_d6]^T表示期望轨迹下对应的控制输入。

构建位姿与速度的跟踪误差系统：

式(13)中η_e＝η-η_d代表位姿误差，ω_e＝ω-ω_d代表速度误差，U_e＝U-U_d-J(η_d)M^-1τ_d。

η与ω分别表示UVMS在E-XYZ下位姿与速度的实际值，满足：

式(14)中

η_d与ω_d分别表示UVMS在E-XYZ下位姿与速度的期望值，满足：

式(15)中

ω_d＝J(η_d)υ_d。

为削弱δ_d产生的影响，设计一种固定时间扰动观测器；

设计的固定时间扰动观测器的表达式如下：

其中κ₁，κ₂，κ₃＞0，且κ₃＞L_d。γ₁＞0，0＜γ₂＜1。z₁为速度误差ω_e的观测值，观测误差e₁＝z₁-ω_e。z₂为外界扰动δ_d的观测值，观测误差e₂＝z₂-δ_d。

根据UVMS跟踪误差系统式(13)，设计关于η_e，ω_e的积分滑模面(ISM)：

式(18)中s＝[s₁,s₂,s₃,s₄,s₅,s₆]^T，满足k₁,k₂＞0，0＜β₁＜1，β₂＝2β₁/(1+β₁)；

结合式(17)与式(18)，得到积分滑模面的导数：

设计固定时间积分滑模控制器的表达式如下：

其中λ₁,λ₂,λ₃＞0。p与q为奇数，满足0＜p＜q。

进一步地，对扰动观测器稳定性进行分析如下：

定理1：所设计的FTDO能够在固定时间内实现对δ_d未建模动态与外界扰动的观测。

证明：将式(16)整理后得到下式：

进一步整理得到：

由引理1可知，式(22)中

固定时间收敛于零，此时观测误差e₂也将收敛至零，使z₂＝δ_d，实现固定时间内对δ_d的观测，整理后的收敛时间上界与初值无关，满足：

进一步地，固定时间积分滑模控制器稳定性分析如下：

定理2：所设计的ISMC能够在固定时间内到达滑模面，到达滑模面后当η_e与ω_e将在有限时间内收敛至零，实现对UVMS期望轨迹的精确跟踪。

对于上述定理的证明分为两个步骤，分别为积分滑模控制中到达阶段和滑动阶段的证明：

①到达阶段

证明误差系统能够在固定时间镇定到滑模面。

选取Lyapunov函数：

对上式求导同时将式(19)带入，整理得到：

当水下机器人-机械手系统的扰动被观测后，满足z₂-δ_d＝0，可进一步将式(25)整理为：

由引理1可知，所设计的ISMC能使UVMS在固定时间内到达滑模面，其收敛时间上界为：

其中

且

②滑动阶段

证明到达滑模面后，跟踪误差在有限时间内收敛至零。

当η_e，ω_e镇定到滑模面s后：

进一步改写为：

式中，j＝1,2,3,4,5,6。

根据引理2，系统(28)有限时间稳定，η_e和ω_e可快速收敛到零。

综上，在最大收敛时间T_2,max内，滑模面s能够到达。滑模面到达后s＝0，

跟踪误差η_e和ω_e将在有限时间内收敛至零，假设滑动阶段到达稳定的时间上界为一很小的正值T_ε，从而得到控制系统达到稳定的时间上界为：

T_3,max＝T_2,max+T_ε (30)

进而所设计的控制策略能在固定时间内保证η≡η_d，ω≡ω_d，实现对期望轨迹跟踪的跟踪。

为了验证本发明提出的水下机器人-机械手系统的FTDO-ISMC轨迹跟踪控制策略的有效性与优越性，本发明与反步滑模控制进行仿真实验对比研究。

表1 D-H参数定义表，如下：

参数	数值	参数	数值
				k<sub>1</sub>	3.6	p	5
k<sub>2</sub>	2.2	q	9
				β<sub>1</sub>	0.5	κ<sub>1</sub>	1.4
β<sub>2</sub>	0.67	κ<sub>2</sub>	1.4
				λ<sub>1</sub>	9.2	κ<sub>3</sub>	30
λ<sub>2</sub>	0.3	γ<sub>1</sub>	1.3
				λ<sub>3</sub>	0.1	γ<sub>2</sub>	0.7

仿真实验中，选取UVMS初始位姿η(0)＝[5,2,0,-π/2,π/6,-π/6]^T，初始速度向量υ(0)＝[1,1,-1,0.5,0.5,0.15]^T。期望初始位姿η_d(0)＝[4,1,-π/2,0,0,0]^T，期望初始速度向量v_d(0)＝[1,0,0,0,0,0]^T。期望轨迹下的控制输入为式(31)，集总扰动项为式(32)。

τ_d＝[150,12cos²(0.1πt),-60,-8sin²(0.1πt),0.25-0.25/(1+e^t-30),0.25/(1+e^{t -30})]^T (31)

δ_d＝[10cos(0.1πt-π/3)，10cos(0.2πt-π/4)，10cos(0.3πt-π/6)，10cos(0.1πt-π/4)，10cos(0.2πt-π/3)，10cos(0.2πt-π/3)]^T (32)

结合式(23)与式(27)，上述所取参数得到观测器收敛时间上界为T_1,max＝3.74s，控制器收敛时间上界T_3,max＝5.69s+T_ε。

仿真结果如图2至图9所示。其中，图2为UVMS运动轨迹跟踪图，可见在同样的未建模动态及外部扰动下，所设计的控制策略(FTDO-ISMC)可使UVMS精确的跟踪期望轨迹，反步滑模控制策略(BSMC)无法实现精确跟踪。

图3为UVMS位姿状态图；图4为UVMS速度状态图；图3与图4表示UVMS位姿与速度随时间的变化状态，可见FTDO-ISMC各维度上的变量均在一定时间内完成了期望任务。

图5为UVMS位姿误差图；图6为UVMS速度误差图；图5与图6显示，UVMS的位姿与速度误差均在3s左右收敛至零，收敛时间均小于5.69s+T_ε(T_3,max)，与理论计算结果一致。

图7为FTDO观测值图，图中FTDO观测值曲线显示，本专利设计的固定时间扰动观测器能够实现对未建模动态与外部有界扰动的精准观测，且能及时补偿未建模动态与扰动对系统产生的影响。

图8为FTDO观测误差图，显示FTDO观测误差曲线，可见观测误差在固定时间内收敛至零，收敛时间小于3.74s(T_1,max)，与理论计算结果一致。

图9为UVMS控制输入图,显示UVMS控制输入曲线，可见与BSMC相比，FTDO-ISMC的控制输入更加光滑，且验证了FTDO-ISMC控制器能够在固定时间内趋于稳定。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (3)

1.一种水下机器人-机械手系统固定时间轨迹跟踪方法，其特征在于：包括以下步骤：

S1：设计固定时间扰动观测器，采用固定时间扰动观测器在固定时间内实现对水下机器人-机械手系统的未建模动态与外界扰动的进行观测，得到未建模动态与外界扰动对水下机器人-机械手系统的影响力矩；

S2：设计固定时间积分滑模控制器，固定时间积分滑模控制器根据影响力矩产生一个补偿力矩，抵消未建模动态与外部扰动对水下机器人-机械手系统的影响，最终固定时间积分滑模控制器实现水下机器人-机械手系统的运行轨迹的精确跟踪。

2.根据权利要求1所述的一种水下机器人-机械手系统固定时间轨迹跟踪方法，其特征在于，所述固定时间扰动观测器的表达式如下：

其中κ₁，κ₂，κ₃＞0，为扰动观测器控制系数，且κ₃＞L_d，γ₁＞0，0＜γ₂＜1，为扰动观测器增益系数，z₁为速度误差ω_e的观测值，观测误差e₁＝z₁-ω_e，z₂为外界扰动δ_d的观测值，观测误差e₂＝z₂-δ_d。

3.根据权利要求1所述的一种水下机器人-机械手系统固定时间轨迹跟踪方法，其特征在于，所述固定时间积分滑模控制器的表达式如下：

其中λ₁,λ₂,λ₃＞0，为固定时间积分滑模控制器控制系数；k₁,k₂为滑模面控制系数；β₁,β₂为滑模面增益系数；p与q为奇数，满足0＜p＜q。

Images