CN110744541A - 一种视觉引导的水下机械臂控制方法 - Google Patents

Abstract

一种视觉引导的水下机械臂控制方法，属于水下机械臂技术领域。本发明以水下机械臂的与运动学及动力学建模、水下机械臂标定技术的相关研究为基础，通过手眼标定方法，模仿生物视觉的手眼协调控制系统框架及视觉引导系统控制流程；通过单目视觉对目标物体进行定位、并采用二次成像对图像信息水下目标物进行信息估算，建立水下机械臂运动学和动力学模型，将终端滑模控制和自抗扰控制结合在一起，用分散控制思想，将复杂的机械臂系统进行解耦，介绍了一种六自由度水下机械臂关节控制器‑终端滑模自抗扰控制器。本发明为水下机械臂进行水下精确作业提供了解决方案，提升了水下机械臂自主作业能力，极大提高了水下作业的自动化水平。

Description

一种视觉引导的水下机械臂控制方法

技术领域

本发明属于水下机械臂技术领域，具体涉及一种视觉引导的水下机械臂控制方法。

背景技术

基于视觉引导的水下机械臂是水下机器人系统中的重要组成部分，在深海资源勘探和水下作业工程中发挥着不可或缺的作用，能够根据操作者的指令代替人进行水下极端环境作业，因此进行基于视觉引导的水下机械臂的研究具有重要的科学价值和实际工程意义。

为了水下机械臂能够承担多种水下任务，水下机械臂的控制规划问题是非常重要的研究内容，控制规划问题是视觉引导任务中最重要的环节之一，同时也是水下机械臂研究领域的一个重要分支。经过几十年来各国科研人员的不断努力，视觉引导的机械臂控制方法早已变成目前机械臂控制方向的研究热点与难点。目前主流的三种基于视觉引导的机械臂控制方法为：基于运动的视觉引导控制方法、基于学习的视觉引导控制方法、混合型视觉引导控制方法(2.5D视觉引导控制方法)。

第一种控制方法的思想是发挥学习训练方法的函数拟合能力，在宏观角度对机械臂手眼协调系统的非线性特性进行控制，基于以上两点思想建立图像信息闭环反馈控制律。但存在需要在控制之前进行大量的样本学习等缺点。第二种控制方法首先预设了参考运动的光流场，控制机械臂手眼系统运动到指定的位置，令相机图像的光流场与目标物体的光流场并行。该控制方法不需要大量的先验参数，但对控制帧率要求极高，对实验数据造成影响。第三种控制方法将相机的平动位移矩阵(V)与旋转矩阵(Ω)的闭环控制解耦，利用从目标物体中检测出的特征信息，重新模拟出三维空间坐标系下的位置与图像深度信息，平动位移采用2D空间坐标系上的特征点进行表征。但需要求解的单应矩阵的多次迭代十分繁琐，受图像噪声的影响更大等缺点。

本发明为水下机械臂进行水下精确作业提供了解决方案，提升了水下机械臂自主作业能力，极大提高了水下作业的自动化水平。

发明内容

本发明的目的在于提供一种视觉引导的水下机械臂控制方法，有效的增强控制系统的抗干扰能力，以应对复杂多变的海洋工作环境，实现基于视觉控制的水下机械臂控制。

本发明的目的是这样实现的：

一种视觉引导的水下机械臂控制方法，包括如下步骤：

步骤1：当水下机械臂处于初始位置，采用视觉引导方式控制水下机械臂运动，位于腕部的水下摄像机开始搜寻水下目标，确定水下目标物；

步骤2：确定水下目标物之后，通过视觉对水下目标物进行定位，确定本运动周期的水下机械臂末端位置点；

步骤3：建立水下机械臂的运动学模型，计算出本周期的机械臂各关节运动量，构造基于终端滑模的自抗扰控制器，驱动水下机械臂各关节运动；

步骤4：检测水下机械臂是否到达水下目标点，若没有到达目标点，重复步骤2和步骤3的运动过程，若到达则进行相应的水下机械抓取工作。

所述步骤1中视觉引导方式控制包括：

步骤1.1：通过水下机械臂手爪关节的相机获取水下目标物体P的特征信息，从已经构建的3D模型集合中随机抽取一个3D模型，采用外参数矩阵M验证方法选取与P的特征信息相匹配的3D模型；

步骤1.2：从3D模型中提取与P的特征匹配的全部几何特征，进而建构对目标位姿估算的特征集合；

步骤1.3：设P的图像特征的在计算机坐标系(o_pix-x_pixy_pix)下的坐标为{(u₀ v₀),(u₁ v₁)······(u_i v_i)}，(u₀v₀),(u₁ v₁)······(u_i v_i)，代表水下目标物体P的i个几何特征的坐标；设与P匹配的3D模型在世界坐标系(O_w-X_wY_wZ_w)下的三维空间坐标为{(x₀ y₀ z₀),(x₁ y₁ z₁)······(x_i y_i z_i)}；

步骤1.4：根据摄像机成像几何模型的建立原理，求解摄像机的外参数矩阵M。

所述步骤1.1中外参数矩阵M验证方法吧包括：

步骤1.1.1：相机处于B时水下目标物体的外参数矩阵为

处于A时水下目标物体的外参数矩阵为M，则其中^BR_A为相机分别处于A、B两个位置时的转换矩阵；

步骤1.1.2：将与

相匹配的3D模型提取出来并进行计算，得到水下目标物体在计算机坐标系(o_pix-x_pixy_pix)下的坐标；

步骤1.1.3：若通过相机采集目标物体所得到的外参数矩阵M与转换矩阵计算所得的外参数矩阵

对同一目标物体的估算结果一致，则认为3D模型匹配一致；若对同一目标物体的估算结果不一致，则认为3D模型匹配不合理；

步骤1.1.4：若3D模型匹配不合理，则需要重新构建3D模型集合或继续在当前集合中寻找新的3D模型进行水下目标物体匹配工作，重复步骤1.1.1、步骤1.1.2和步骤1.1.3的直至满足要求为止。

所述步骤2中通过视觉对水下目标物进行定位首先定义水下目标物体为一球体P，P的半径为r，P的坐标为(u₀,v₀)，计算球体P的位姿信息

其中相机从Z_C移动到ΔZ_C采集P的图像信息，获取m及n帧图像时相机的坐标分别为(X_m,Y_m,Z_m)及(X_n,Y_n,Z_n)，P的半径分别为r_m及r_n，令Δr'＝r_m-r_n表示从m帧到n帧图像球体P的半径变化；然后计算的P估算半径式中，ΔZ_C和Δr'的取值决定于相机所在位置，k表示比例常数；最后将此时球体P的估算半径r作为先验知识保存下来，利用现有的目标物体信息已知的算法实现对水下目标物的定位工作。

所述的步骤3中建立水下机械臂的运动学模型具体为：

六自由度水下机械臂的矩阵形式的动力学方程为：

其中，M_m(q)为惯性力矩阵；

为哥氏力及向心力阵；

为阻尼力矩阵；G_m(q)为恢复力矩阵；τ_m为驱动力矩阵；将水下机械臂的每一个关节模块定义为一个独立的子系统，并将局部变量

进行分离，得到水下机械臂的动力学模型为：

其中q_i,

G_i(q_i)、τ_i分别为向量q,

G(q)、τ的第i个分量，i＝1,2,......,n；τ_d为水下机械臂关节所受到外界的扰动；

为第i个关节的总耦合项，表达式为：

所述步骤3中构造基于终端滑模的自抗扰控制器为对于包含扩张状态观测器(ESO)的水下机械臂第i个关节的控制器

其中

为关节子系统的跟踪误差，z₃为W(t)的估计值，F(x_i,τ_i)＝f(x_i,τ_i)+b_iτ_i+b_iτ_d，

sat(s_i/Φ)为饱和函数

其中Φ为边界层厚度。

本发明有益效果在于：

(1)本发明采用视觉引导反馈引导，提高了机械臂操作精度；采用二次成像对水下目标物图像信息进行信息估算，提高了图像信息精度；

(2)本发明提高水下机械臂自主作业能力，增强系统作业的稳定性和可靠性；

(3)本发明采用分散控制思想，解决了水下机械臂的耦合问题，降低控制器控制设计难度，起到提高控制的效果；

(4)本发明采用终端滑模控制自抗扰控制器作为机械臂关节控制器，提高系统抵抗外干扰能力，提高了控制系统的鲁棒性；

(5)本发明采用饱和函数sat(s_i/Φ)取代符号函数sgn(s_i)降低抖振，使得控制输出更加平缓，从而起到减轻磨损，延长电机寿命。

附图说明

图1为本发明总体流程图。

图2为视觉引导运动控制机制的机械臂手眼协调示意图。

图3为视觉引导水下机械臂系统控制流程图。

图4为二次成像法示意图。

图5为单关节系统滑模自抗扰控制器原理图。

图6为六自由度水下机械臂系统结构示意图。

图7为施加正弦规律干扰-角度跟踪曲线图。

图8为施加随机噪声干扰-角度跟踪曲线图。

图9为施加正弦规律干扰-角度跟踪误差曲线图。

图10为施加随机噪声干扰-角度跟踪误差曲线图。

图11为施加正弦规律干扰-角速度跟踪曲线图。

图12为施加随机噪声干扰-角速度跟踪曲线图。

图13为施加正弦规律干扰-角速度跟踪误差曲线图。

图14为施加随机噪声干扰-角速度跟踪误差曲线图。

图15为静水中普通滑模控制和终端滑模控制对比结果表。

图16为海流环境下三种控制器控制效果对比表。

具体实施方式

下面结合发明内容，通过以下实施例阐述本发明的一种详细实施方案与效果。

本发明提供了一种视觉引导的水下机械臂控制方法。该方法能够实现水下机械臂控制器的稳定性，有效增强水下机械臂的抗干扰能力，有更好的控制效果，减少控制误差。为水下机械臂进行水下精确作业提供了解决方案，提升了水下机械臂自主作业能力，极大提高了水下作业的自动化水平。

本发明模仿生物视觉的手眼协调控制系统，利用视觉前馈控制引导水下机械臂的手爪关节抓取该物体、视觉反馈控制及水下机械臂的本体感知信息对已经规划的轨迹进行误差校准；再利用所获得的信息进行视觉引导控制水下机械臂，从而建立水下机械臂运动学模型和动力学模型，根据获取目标物的位置信息，解算出水下机械臂的各关节的期望位姿；用分散控制思想，将复杂的机械臂系统进行解耦，建立单关节的动力学模型，基于此动力学模型，介绍了一种机械臂独立关节的终端滑模自抗扰控制器。

人类通过眼睛控制手臂抓取物体的这一过程叫做基于生物的视觉引导控制，这种控制方式的思想是通过大脑处理眼睛观察到的信息，并结合人类自身的感知实现从二维平面视觉信息到三维空间运动的转换。本次发明所用的手眼协调控制系统框架便是参考该思想，实现基于视觉引导的水下机械臂控制。

结合图2所示，本发明利用视觉前馈控制引导水下机械臂的手爪关节抓取该物体、视觉反馈控制及水下机械臂的本体感知信息对已经规划的轨迹进行误差校准；如果这时水下目标物离开视野范围内或不满足视差条件的阈值，就重复上述过程直至满足视差条件的阈值，再利用所获得的信息进行视觉引导控制水下机械臂，完成快速接近并抓取目标物体的作业任务。

结合图3所示，假设水下目标物体为P，视觉引导系统控制思路如下：

(1)通过水下机械臂手爪关节的相机获取P的特征信息，从已经构建的3D模型集合中随机抽取一个3D模型，并假设P的特征信息与该3D模型匹配。

(2)从3D模型中提取与P的特征匹配的全部几何特征，进而建构可对目标位姿估算的特征集合。

(3)设P的图像特征的在计算机坐标系(o_pix-x_pixy_pix)下的坐标为{(u₀ v₀),(u₁v₁)······(u_i v_i)}，(u₀v₀),(u₁ v₁)······(u_i v_i)代表水下目标物体P的i个几何特征的坐标；设与P匹配的3D模型在世界坐标系(O_w-X_wY_wZ_w)下的三维空间坐标为{(x₀y₀ z₀),(x₁ y₁ z₁)······(x_i y_i z_i)}。

(4)根据摄像机成像几何模型的建立原理，求解摄像机的外参数矩阵M。

上述的视觉引导控制系统是建立在P的特征信息与随机选取的3D模型匹配的前提之下，然而这一前提并不一定能实现。因此，本发明采用外参数矩阵M验证方法，其步骤如下：

(1)设相机处于B时水下目标物体的外参数矩阵为

处于A时水下目标物体的外参数矩阵为M，则存在如下关系：

(^BR_A为相机分别处于A、B两个位置时的转换矩阵)；

(2)将与

相匹配的3D模型提取出来，并进行计算，即可得到水下目标物体在计算机坐标系(o_pix-x_pixy_pix)下的坐标；

(3)如果通过相机采集目标物体所得到的外参数矩阵M与转换矩阵计算所得的外参数矩阵

对同一目标物体的估算结果一致，那么则认为3D模型匹配一致；如果对同一目标物体的估算结果不一致，则认为3D模型匹配不合理；

(4)如果3D模型匹配不合理，则需要重新构建3D模型集合或者继续在当前集合中寻找新的3D模型进行水下目标物体匹配工作，直至满足步骤(1)、(2)、(3)的要求为止。

手爪关节上配备的相机获取水下目标物的图像信息，并且采用一种水下目标物目标尺寸信息恢复的方法以达到期望的深度信息精度，采用二次成像法对图像信息进行水下目标物的大小、长宽比例等信息估算，最后利用现有的目标物体信息已知的算法(摄像机标定方法)实现对水下目标物的定位工作。

结合图4所示，设水下目标物体为一球体P，P的半径为r，P的坐标为(u₀,v₀)，相机从Z_C移动到ΔZ_C采集P的图像信息，设获取m及n帧图像时相机的坐标分别为(X_m,Y_m,Z_m)及(X_n,Y_n,Z_n)，P的半径分别为r_m及r_n，令Δr'＝r_m-r_n表示从m帧到n帧图像球体P的半径变化，式(1)为球体P的位姿信息估算表示式。

通过式(1)和摄像机标定的知识，便可得到第n帧时P的坐标(X_n,Y_n,Z_n)，P估算半径r的表达式如下：

式(2)中，ΔZ_C和Δr'的取值决定于相机所在位置，k表示比例常数。

此时将球体P的估算半径r作为先验知识保存下来，最后利用现有的目标物体信息已知的算法(摄像机标定方法)实现对水下目标物的定位工作。如果仅利用m及n两帧图像对球体P的半径r进行估计误差较大，因此需多个角度采集水下目标物进行多帧分析，以减少估算误差。

用分散控制思想，将复杂的机械臂系统进行解耦，建立单关节的动力学模型，基于此动力学模型，介绍了一种机械臂独立关节的终端滑模自抗扰控制器；

六自由度水下机械臂的矩阵形式的动力学方程如式(3)所示。

式(3)中各个参数的含义如下：

M_m(q)∈R^6×6：Inertia matrix(惯性力矩阵)；

Coriolis and Centripetal matrix(哥氏力及向心力阵)；

Damping matrix(阻尼力矩阵)；

G_m(q)∈R^6×1：Restoring matrix(恢复力矩阵)；

τ_m：Driving matrix(驱动力矩阵)。

将水下机械臂的每一个关节模块定义为一个独立的子系统，并将局部变量进行分离。结合图5所示，由此可以得到水下机械臂的动力学模型如下所示：

其中，q_i,

G_i(q_i)、τ_i分别为向量q,

G(q)、τ的第i个分量(i＝1,2,......,n)，τ_d为水下机械臂关节所受到外界的扰动，

为第i个关节的总耦合项，

的具体表达式如下：

综上所述可，得六自由度水下机械臂系统结构示意图如图6所示。

设

则式(4)可以表示为如下状态空间的形式：

其中x_i，y_i分别代表S_i的输入与输出，表示子系统的耦合项，因此可以得到如下关系式：

令F(x_i,τ_i)＝f(x_i,τ_i)+b_iτ_i+b_iτ_d，

其中W(t)为水下机械臂单关节系统所受到的总扰动，则式(6)可以改写为：

关节子系统的跟踪误差可以定义如下：

e＝x_i1-y_i1 (10)

定义误差向量为

滑模设计函数为：

s＝C(E-P) (12)

式中，C＝[c 1]，

为了实现全局鲁棒性，取E(0)＝P(0)，即p(0)＝e(0)，

为了实现按指定时间T收敛，取t＝T时，p(t)＝0，

则可构造终端滑模函数p(t)的多项式如下：

其中，a_ij(i,j＝0,1,2)为系数，可通过解方程得到。

由式(11)可得：

由式(12)可得：

可得理想状态下水下机械臂第i个关节的控制器可以设计为：

式(16)中η_isgn(s_i)为终端滑模控制中包含的鲁棒项，η_i定义为一正常数，sgn(·)表示符号常数。然而式(16)所求出的水下机械臂单关节控制器没有考虑到扩张状态观测器(ESO)，实际上利用扩张状态观测器(ESO)观测系统的角度值、角加速度值以及总扰动，并且进行总扰动补偿。本发明所设计的扩张状态观测器(ESO)的具体数学表达为：

上式中，z₁，z₂，z₃分别对应q_i，

及W(t)的估计值，其中β₁，β₂，β₃均大于零为观测器的增益，起到误差校正的作用，其中

在本系统中，只利用z₃观测出单关节系统所受到的扰动值并进行补偿，因此结合公式(17)可得对于包含扩张状态观测器(ESO)的水下机械臂第i个关节的控制器为：

式(18)中，sat(s_i/Φ)为饱和函数(Φ为边界层厚度)。相比于符号函数sgn(s_i)，sat(s_i/Φ)函数具有使控制信号更加稳定、控制输出更加平缓的优点。sat(s_i/Φ)的表达式如式(19)所示。

本发明设计了基于终端滑模控制的自扰性控制器，有效的增强了控制系统的抗干扰能力提高了控制精度等优点，满足实际任务和环境对水下机械臂的控制要求。

本发明采用三种情况模仿水下的实际情况对所设计的终端滑模自抗扰控制器进行控制性能的测试，分别在静水环境下及海流环境下使用普通滑模制器和终端滑模控制器进行仿真对比，分析使用两种控制器对系统控制指标的不同影响。最后在海流环境下加入外部扰动，使用终端滑模控制器与本发明所采用的终端滑模自抗扰控制器进行水下机械臂单关节系统角度、角度误差、角速度、角速度误差等四项指标的不同仿真，指标验证本发明所提控制方法的快速跟踪性、稳定性及有效性。

在静水环境下两种控制方法对水下机械臂关节整体跟踪效果都较好，表现出了良好的动态特性，但是使用终端滑模控制可以更加迅速的跟踪到期望轨迹，调节时间快、超调小、无振荡，控制过程比较稳定平滑。对比结果如图15所示。

图16为海流环境下三种控制器控制效果对比表，从图16中可以看出滑模控制器再加入水动力干扰后，各个控制性能参数调节时间明显增长(由不到1s增加到7s左右)，终端滑模控制器的调节时间也有所增长，但各个控制性能参数仍能控制在1s以内；终端滑模控制器的角度跟踪误差及角速度跟踪误差参数也出现了较为明显抖震现象。由此说明，在海流环境中普通滑模控制效果没有加入之前优良，受干扰影响较大，跟踪时间过长且抖震严重。而终端滑模控制器在加入干扰后，在各个跟踪参数的调整时间有一定的延长，部分参数出现了抖震现象，但整体受干扰影响相对较小。

由于机械臂在水下实际作业时可能受到各种外界干扰，首先给水下机械臂施加一个规律的干扰力矩：

进行仿真，其次在规律干扰力矩的基础上再加入一些随机的噪声干扰进行仿真对比，存在外部干扰环境中终端滑模自抗扰控制仿真结果效果图如图7、图8、图9、图10、图11、图12、图13、图14所示。

对水下机械臂施加正弦规律干扰及随机噪声干扰后，水下机械臂的跟踪控制时间仍然能维持在0.5s之内，各个参数的调节时间与无干扰情况相比也没有明显增长，这说明终端滑模自抗扰控制器鲁棒性及稳定性十分优越；虽然在加入随机噪声干扰后，角度跟踪误差参数及角速度跟踪误差参出现了一定程度的抖振，但抖振幅度极小，不影响水下机械臂的跟踪控制。

Claims (6)

1.一种视觉引导的水下机械臂控制方法，其特征在于，包括：

步骤1：当水下机械臂处于初始位置，采用视觉引导方式控制水下机械臂运动，位于腕部的水下摄像机开始搜寻水下目标，确定水下目标物；

步骤2：确定水下目标物之后，通过视觉对水下目标物进行定位，确定本运动周期的水下机械臂末端位置点；

步骤3：建立水下机械臂的运动学模型，计算出本周期的机械臂各关节运动量，构造基于终端滑模的自抗扰控制器，驱动水下机械臂各关节运动；

步骤4：检测水下机械臂是否到达水下目标点，若没有到达目标点，重复步骤2和步骤3的运动过程，若到达则进行相应的水下机械抓取工作。

2.根据权利要求1所述的一种视觉引导的水下机械臂控制方法，其特征在于：所述的步骤1中视觉引导方式控制包括

步骤1.1：通过水下机械臂手爪关节的相机获取水下目标物体P的特征信息，从已经构建的3D模型集合中随机抽取一个3D模型，采用外参数矩阵M验证方法选取与P的特征信息相匹配的3D模型；

步骤1.2：从3D模型中提取与P的特征匹配的全部几何特征，进而建构对目标位姿估算的特征集合；

步骤1.3：设P的图像特征的在计算机坐标系(o_pix-x_pixy_pix)下的坐标为{(u₀ v₀),(u₁v₁)……(u_i v_i)}，(u₀ v₀),(u₁ v₁)……(u_i v_i)，代表水下目标物体P的i个几何特征的坐标；设与P匹配的3D模型在世界坐标系(O_w-X_wY_wZ_w)下的三维空间坐标为{(x₀ y₀ z₀),(x₁ y₁z₁)……(x_i y_i z_i)}；

步骤1.4：根据摄像机成像几何模型的建立原理，求解摄像机的外参数矩阵M。

3.根据权利要求2所述的一种视觉引导的水下机械臂控制方法，其特征在于：所述的步骤1.1中外参数矩阵M验证方法包括

步骤1.1.1：相机处于B时水下目标物体的外参数矩阵为

处于A时水下目标物体的外参数矩阵为M，则其中^BR_A为相机分别处于A、B两个位置时的转换矩阵；

步骤1.1.2：将与

相匹配的3D模型提取出来并进行计算，得到水下目标物体在计算机坐标系(o_pix-x_pixy_pix)下的坐标；

步骤1.1.3：若通过相机采集目标物体所得到的外参数矩阵M与转换矩阵计算所得的外参数矩阵

对同一目标物体的估算结果一致，则认为3D模型匹配一致；若对同一目标物体的估算结果不一致，则认为3D模型匹配不合理；

步骤1.1.4：若3D模型匹配不合理，则需要重新构建3D模型集合或继续在当前集合中寻找新的3D模型进行水下目标物体匹配工作，重复步骤1.1.1、步骤1.1.2和步骤1.1.3的直至满足要求为止。

4.根据权利要求1所述的一种视觉引导的水下机械臂控制方法，其特征在于：所述的步骤2中通过视觉对水下目标物进行定位首先定义水下目标物体为一球体P，P的半径为r，P的坐标为(u₀,v₀)，计算球体P的位姿信息

其中相机从Z_C移动到ΔZ_C采集P的图像信息，获取m及n帧图像时相机的坐标分别为(X_m,Y_m,Z_m)及(X_n,Y_n,Z_n)，P的半径分别为r_m及r_n，令Δr'＝r_m-r_n表示从m帧到n帧图像球体P的半径变化；然后计算的P估算半径式中，ΔZ_C和Δr'的取值决定于相机所在位置，k表示比例常数；最后将此时球体P的估算半径r作为先验知识保存下来，利用现有的目标物体信息已知的算法实现对水下目标物的定位工作。

5.根据权利要求1所述的一种视觉引导的水下机械臂控制方法，其特征在于：所述的步骤3中建立水下机械臂的运动学模型具体为：

六自由度水下机械臂的矩阵形式的动力学方程为：

其中，M_m(q)为惯性力矩阵；为哥氏力及向心力阵；

为阻尼力矩阵；G_m(q)为恢复力矩阵；τ_m为驱动力矩阵；将水下机械臂的每一个关节模块定义为一个独立的子系统，并将局部变量

进行分离，得到水下机械臂的动力学模型为：

其中q_i,

G_i(q_i)、τ_i分别为向量q,

G(q)、τ的第i个分量，i＝1,2,......,n；τ_d为水下机械臂关节所受到外界的扰动；

为第i个关节的总耦合项，表达式为：

6.根据权利要求1所述的一种视觉引导的水下机械臂控制方法，其特征在于：所述的步骤步骤3中构造基于终端滑模的自抗扰控制器为对于包含扩张状态观测器(ESO)的水下机械臂第i个关节的控制器

其中

为关节子系统的跟踪误差，z₃为W(t)的估计值，F(x_i,τ_i)＝f(x_i,τ_i)+b_iτ_i+b_iτ_d，

sat(s_i/Φ)为饱和函数

其中Φ为边界层厚度。

Images