CN112091976A - 一种水下机械臂任务空间控制方法 - Google Patents

Abstract

本发明提供一种水下机械臂任务空间控制方法，包括如下步骤：步骤一：建立水下机械臂任务空间动力学模型；步骤二：引入全局快速终端滑模面使跟踪误差快速收敛；步骤三：迭代学习控制方式增强轨迹跟踪收敛能力和鲁棒性；步骤四：引入初始修正银子克服初始值误差问题；步骤五：自适应控制补偿外界干扰和模型不确定性的影响。本发明充分考虑到水下作业环境的水动力学未知项和海流干扰，使机械臂控制系统具有较好的鲁棒性和适应性；本发明实现任务空间轨迹跟踪误差的有限时间内的快速收敛，消除控制中的滑膜抖振现象；本发明通用性好，广泛用于深海作业型机器人系统中；本发明为水下机械臂控制方案提供有效的借鉴方案，具有重要的理论价值和工程意义。

Description

一种水下机械臂任务空间控制方法

技术领域

本发明涉及一种任务空间控制方法，尤其涉及一种水下机械臂任务空间控制方法。

背景技术

水下机械臂在靠近定点作业目标的过程中，存在模型不确定性及海流干扰的情况下，水下机械臂关节空间轨迹跟踪控制问题，以及水下机械臂的末端的任务空间轨迹跟踪问题，为了避免碰撞，影响机器人系统的稳定性，不仅需要对基座的姿态进行控制，也需要使用关节控制方法对水下机械臂的关节角度进行控制。目前针对水下机械臂作业环境复杂的情况，水下机械臂的控制一般是多种控制方法结合使用达到减小外界干扰的影响，保证机械臂的稳定和精度。

国内外学者对此进行了许多深入的研究，Sarfraz等(2017)提出一种基于ISMC的自适应鲁棒水下机器人鲁棒控制算法，可以应用于两种不同类型的水下机器人，该方法具有通用性，而且提出的方案确保了整个状态空间的鲁棒稳定以及颤振抑制，然而将其他方法与SMC结合使用具有明显的缺陷，与神经网络相结合的缺点是难以选择合适的网络，并且收敛到平衡点的速度缓慢。同样，与模糊逻辑控制结合的主要缺点是难以获得模糊规则和隶属函数。Chen and Wang(2015)设计了一种鲁棒自适应观测器的前馈模糊轨迹补偿器，用于水下多连杆机械臂，采用前馈模糊补偿器解决抖振问题，消除了水动力的影响，实现了滑模控制算法通过强抗干扰性实现机械手的高精度控制，并且在两自由度机械臂上进行仿真验证，验证了该控制器的轨迹跟踪性能良好。但是，该方案并未考虑实际试验中每个关节的摩擦阻力，导致实验与仿真之间存在较大差异。Pandian和Sakagami(2008)提出采用神经模糊PD控制的方法，用模糊增益整定方法适应具有不确定性和海流干扰的水下环境，采用神经网络进行动态逼近水下机械臂的模型，解决了模型不确定的问题，并添加了前馈补偿PD控制器，实现了减少扰动，但是也同样存在收敛到平衡点速度缓慢的问题。水动力对水下机械臂动力学模型的精度的影响的主要因素，如何消除水下扰动，解决抖振问题成为水下机械臂控制中的主要问题。

发明内容

本发明的目的是针对目前现有水下机械臂存在模型不确定性及海流干扰的情况而提供一种水下机械臂任务空间控制方法。

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

一种水下机械臂任务空间控制方法，包括如下步骤：

步骤一：建立水下机械臂任务空间动力学模型；

步骤二：引入全局快速终端滑模面使跟踪误差快速收敛；

步骤三：迭代学习控制方式增强轨迹跟踪收敛能力和鲁棒性；

步骤四：引入初始修正银子克服初始值误差问题；

步骤五：自适应控制补偿外界干扰和模型不确定性的影响。

本发明还包括这样一些特征：

所述步骤一具体为：

对于水下机械臂的运动学，在其各个关节上建立相应的坐标系，再由坐标系之间的变换关系求解得到末端的位置和姿态信息，分为正运动学和逆运动学：

(1)正运动学：如附图2为水下机械臂的实物模型，由图可见，其主要由腰部和腕部的回转关节、大臂和小臂的摆动关节共同组成；对其各个关节进行分析，根据D-H参数法，建立如附图3所示的坐标系；可得水下机械臂各个关节的其次变换矩阵T_i(i＝1～4)：

其中，s₁，s₂，s₃和s₄分别表示sinq₁，sinq₂，sinq₃和sinq₄，c₁，c₂，c₃和c₄分别表示cosq₁，cosq₂，cosq₃和cosq₄；

将这个四个矩阵顺次相乘，可以推出水下机械臂末端执行器在基座标系下的位姿为：

其中，

c₂₃＝cos(q₂+q₃)，s₂₃＝sin(q₂+q₃)；

(2)逆运动学：逆推变换法进行逆运动学的求解，首先在水下机；械臂的运动学方程的左侧顺次乘以位姿矩阵的逆；第一步，推算水下机械臂系统的连杆变换矩阵对应的逆T₁ ^-1，T₂ ^-1，T₃ ^-1和T₄ ^-1：

第二步，求解q₁，用T₁ ^-1左乘得

由上式可得：

将上式两边展开：

有上式可得：c₁p_y-s₁p_x＝0

容易得到：q₁＝arctan2(p_y,p_x)

同理可以求解q₂、q₃、q₄，得到水下机械臂运动学逆解；

水下机械臂的动力学模型的推导是实现水下机械臂控制的基础，可以描述水下机械臂的力与力矩信息和运动状态信息的关系；采用Lagrange方程法对四自由度水下机械臂进行动力学分析；

Lagrange函数可以被定义为系统的动能E_k和势能E_p的差来表示：

L＝E_k-E_p

根据Lagrange函数L可以得到Lagrange方程：

其中，q_i和

分别为广义坐标和广义速度，F_i为第i个广义坐标上的广义力或广义力矩，n为连杆数；

采用Lagrange方程法对四自由度水下机械臂的动力学分析如下；由于水下机械臂的第四个关节为回转关节，对机械臂的动力学建模并不会产生影响，并且，本发明不考虑机械臂系统末端执行器的姿态，只考虑末端执行器所处的位置，因此四自由度的水下机械臂可以简化为腰部回转，大臂摆动和小臂摆动的三连杆结构；简化后的水下机械臂的连杆的质心位于连杆的中心，第一关节的质量为m₁，第二关节质量为m₂，第三关节质量为m₃，长度分别为a₂、a₃和d₄，简化的水下机械臂模型如附图4所示；

当水下机械臂在流体环境中运动时，流体会对水下机械臂连杆的表面产生一定的阻力，水下机械臂的连杆按照阻力的方向可以分解为在其法线方向上流体阻力和切线方向上流体阻力，因为机械臂的连杆是一个规则的圆柱体，所以，其切线方向流体阻力很小，常常忽略不计；由于水流存在粘性，所以，在机械臂的连杆在做加速运动时，连杆周围的水流也会发生相对的运动从而水下机械臂的连杆产生一个反方向上的作用力，水下机械臂连杆所受的反作用力也叫做附加质量力；因此，水下机械臂在进行海洋作业时，必须对其进行水动力分析；本发明基于微元法和Morison公式对水下机械臂的单个连杆进行水动力学分析如附图5所示，考虑海洋流体环境的影响；

综合以上分析，水下机械臂的动力学模型的矩阵形式可以被表示为：

其中，

表示水下机械臂各个关节的关节变量，

表示水下机械臂的惯性矩阵，

是水下机械臂的哥式力和向心力项，

是水下阻尼矩阵，

表示水下机械臂的重力项，

表示水下机械臂的各个关节控制输入；

通过任务空间和关节空间的转换矩阵h(q)将四自由度水下机械臂的末端执行器在任务空间的轨迹表示成如下形式：

x_E＝h(q)

x_E是末端执行器在任务空间中的轨迹；水下机械臂的任务空间轨迹x_E和关节空间轨迹q的导数关系如下：

进一步整理，可以得到水下机械臂的任务空间动力学方程：

对上式，可得在[0,t_f]上重复运行的水下机械臂的任务空间动力学方程：

目的是在给定一个期望的轨迹x_d(t)，寻找控制力矩F_mx ^j(t)，实现系统输出关于任务空间期望轨迹的理想跟踪。；

所述步骤二具体为：得到任务空间的动力学模型后，引入全局快速终端滑膜面：

通过全局快速滑模面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象；

所述步骤四具体为：选择初始修正因子为：

定义修正函数：

其中，ε为一个充分小的正数；

最终通过Lyapunov定理证明任务空间控制器的稳定性，并且得到控制力矩和自适应律：

来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。

与现有技术相比，本发明的有益效果是：

(1)本发明设计一种用于实现水下机械臂定点作业的机械臂控制系统，该系统能够在存在海流干扰和模型存在不确定性情况下，水下机械臂末端执行器的位置和速度仍可以较高的精度在有限时间内快速趋近期望状态；

(2)本发明充分考虑到水下作业环境的水动力学未知项和海流干扰，使机械臂控制系统具有较好的鲁棒性和适应性；

(3)本发明实现任务空间轨迹跟踪误差的有限时间内的快速收敛，消除控制中的滑膜抖振现象；

(4)本发明通用性好，可广泛用于深海作业型机器人系统中；

(5)本发明为水下机械臂控制方案提供有效的借鉴方案，具有重要的理论价值和工程意义。

附图说明

图1是水下机械臂任务空间控制方法控制过程流程图；

图2是水下机械臂的Solidworks的实物模型示意图；

图3是四关节机械臂坐标系示意图；

图4是简化的水下机械臂模型示意图；

图5是水下机械臂连杆微元法表示示意图；

图6是水下机械臂任务空间的控制框图；

图7是工况1中水下机械臂的末端执行器迭代完成的位置跟踪；

图8是工况1中水下机械臂的末端执行器迭代完成的速度跟踪；

图9是工况1中水下机械臂的末端执行器迭代后的位置跟踪误差；

图10是工况1中水下机械臂关节控制输入；

图11是工况1中水下机械臂的末端执行器迭代后的任务空间；

图12是工况1中水下机械臂的末端执行器迭代后的任务空间轨迹跟踪误差；

图13是工况2中水下机械臂的末端执行器迭代完成的位置跟踪；

图14是工况2中水下机械臂的末端执行器迭代完成的速度跟踪；

图15是工况2中水下机械臂的末端执行器迭代后的位置跟踪误差；

图16是工况2中水下机械臂关节控制输入；

图17是工况2中水下机械臂的末端执行器迭代后的任务空间轨迹；

图18是工况2中水下机械臂的末端执行器任务空间轨迹跟踪误差。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明目的在于提供一种水下机械臂任务空间控制方法，能够在存在模型不确定性以及海流干扰的情况下，实现定点作业，并且满足精度和稳定性要求。其控制过程如附图1，通过转换矩阵得到水下机械臂任务空间的动力学模型后，引入全局终端快速滑膜面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，然后以迭代学习控制的方式增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，再引入初始状态的修正因子来克服迭代学习控制中存在的初始值误差问题，最终通过自适应控制来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。本发明提出的控制系统在终端滑膜控制的基础上加自适应控制，终端滑膜控制能使其在有限时间收敛，保证了快速性并且具有很好的鲁棒性，再加上自适应控制，增强了水下的抗扰能力，快速性和鲁棒性得到充分保证。

一种水下机械臂任务空间控制系统，设计了一种自适应迭代滑模控制方法来实现水下机械臂任务空间轨迹的跟踪控制，消除了滑模抖振的现象，增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，通过自适应控制来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。

水下机械臂是四关节机械臂，其主要由腰部和腕部的回转关节、大臂和小臂的摆动关节共同组成。对其各个关节进行分析，根据DH参数法进行运动学建模，可得水下机械臂各个关节的齐次变换矩阵T_i(i＝1～4)：

其中，s₁，s₂，s₃和s₄分别表示sinq₁，sinq₂，sinq₃和sinq₄，c₁，c₂，c₃和c₄分别表示cosq₁，cosq₂，cosq₃和cosq₄。将这个四个矩阵顺次相乘，可以推出水下机械臂末端执行器在基座标系下的位姿为：

其中，

c₂₃＝cos(q₂+q₃)，s₂₃＝sin(q₂+q₃)。

而对于逆运动学，提出逆推变换法进行逆运动学的求解，首先在水下机械臂的运动学方程的左侧顺次乘以位姿矩阵的逆，来依次推算得到四个关节的运动学逆解。

采用Lagrange方程法对四自由度水下机械臂的动力学分析如下。

根据Lagrange函数L可以得到Lagrange方程：

其中，q_i和

分别为广义坐标和广义速度，F_i为第i个广义坐标上的广义力或广义力矩，n为连杆数。

由于水下机械臂的第四个关节为回转关节，对机械臂的动力学建模并不会产生影响，并且，不考虑机械臂系统末端执行器的姿态，只考虑末端执行器所处的位置，因此四自由度的水下机械臂可以简化为腰部回转，大臂摆动和小臂摆动的三连杆结构。

当水下机械臂在流体环境中运动时，流体会对水下机械臂连杆的表面产生一定的阻力，水下机械臂的连杆按照阻力的方向分解为在其法线方向上流体阻力和切线方向上流体阻力，因为机械臂的连杆是一个规则的圆柱体，所以，其切线方向流体阻力很小，常常忽略不计，由于水流存在粘性，所以，在机械臂的连杆在做加速运动时，连杆周围的水流也会发生相对的运动从而水下机械臂的连杆产生一个反方向上的作用力，水下机械臂连杆所受的反作用力也叫做附加质量力。基于微元法和Morison公式对水下机械臂的单个连杆进行水动力学分析，考虑海洋流体环境的影响，综合分析得到水下机械臂的动力学模型的矩阵形式：

并且通过任务空间和关节空间的转换矩阵得到在[0,t_f]上重复运行的水下机械臂的任务空间动力学方程：

其中，j为迭代次数，t∈[0,t_f]属于运行时间。

任务空间的控制方案直接对末端执行器与水下定点目标的误差进行闭环控制，对水下机械臂的结构误差、模型的不确定性和外扰具有更强的适应性，很适用于考虑水下机械臂和环境之间控制交互的作业任务。

采用一种自适应迭代滑模控制方法来实现水下机械臂任务空间轨迹的跟踪控制，通过全局快速滑模面：

实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象。以迭代学习控制的方式的增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，针对迭代学习控制中存在的初始值误差问题，通过引入初始状态的修正因子：

来克服初始重置误差带来的影响，与此同时，通过自适应控制：

来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。

本发明涉及水下作业型机械臂任务空间的控制技术，该系统能够在存在模型不确定性以及海流干扰的情况下，实现水下机械臂关节轨迹跟踪的有限时间稳定，以及末端执行器在任务空间的高精度的轨迹跟踪，可以在有限时间内快速趋近期望值，满足水下定点作业过程中的海底取样等定点作业需求。本发明为水下机械臂适应未知海洋条件下的作业提供技术保障，极大了提高了水下机械臂的环境适应性和作业稳定性，有力地推进水下作业技术的发展。

本发明针对目前现有水下机械臂存在模型不确定性及海流干扰的情况，提供一种基于自适应迭代滑模的水下机械臂任务空间控制，主要目标实现水下机械臂关节轨迹跟踪的有限时间稳定，以及末端执行器在任务空间的高精度的轨迹跟踪，可以在有限时间内快速趋近期望值，满足水下定点作业过程中的海底取样等定点作业需求。

本发明采用的具体技术方案是：

水下机械臂的任务空间控制主要针对水下定点作业过程中其末端执行器的闭环控制任务，要求其末端可以在设定时间内快速到达期望位置。基于其水下作业的任务需求，设计了一种自适应迭代滑模控制方法来实现水下机械臂任务空间轨迹的跟踪控制，通过全局快速滑模面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象。以迭代学习控制的方式的增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，针对迭代学习控制中存在的初始值误差问题，通过引入初始状态的修正因子来克服初始重置误差带来的影响，与此同时，通过自适应控制来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。

本发明针对水下作业环境存在模型不确定性以及海流干扰的情况下，提出一种基于自适应迭代滑模的水下机械臂任务空间控制方法，满足机械臂水下定点作业过程中的海底取样等定点作业需求。

本发明采用的技术方案是：

实施1：本发明主要分为两部分，一部分为机械臂的数学建模部分，运动学模型研究各个关节的变量和末端执行器位姿的几何关系，动力学模型描述水下机械臂的力与力矩信息和运动状态信息的关系，为水下机械臂后续的控制任务提供良好的理论依据；一部分为水下机械臂控制系统的设计，针对定点作业处的末端执行器闭环控制问题，提出了基于任务空间的自适应迭代滑模控制方法，实现水下机械臂的关节控制和末端执行器的位置控制。

实施2：对于水下机械臂的运动学，在其各个关节上建立相应的坐标系，再由坐标系之间的变换关系求解得到末端的位置和姿态信息，分为正运动学和逆运动学。

(1)正运动学：如附图2为水下机械臂的实物模型，由图可见，其主要由腰部和腕部的回转关节、大臂和小臂的摆动关节共同组成。对其各个关节进行分析，根据D-H参数法，建立如附图3所示的坐标系。可得水下机械臂各个关节的其次变换矩阵T_i(i＝1～4)：

其中，s₁，s₂，s₃和s₄分别表示sinq₁，sinq₂，sinq₃和sinq₄，c₁，c₂，c₃和c₄分别表示cosq₁，cosq₂，cosq₃和cosq₄。

将这个四个矩阵顺次相乘，可以推出水下机械臂末端执行器在基座标系下的位姿为：

其中，

c₂₃＝cos(q₂+q₃)，s₂₃＝sin(q₂+q₃)；

(2)逆运动学：逆推变换法进行逆运动学的求解，首先在水下机；械臂的运动学方程的左侧顺次乘以位姿矩阵的逆。第一步，推算水下机械臂系统的连杆变换矩阵对应的逆T₁ ^-1，T₂ ^-1，T₃ ^-1和T₄ ^-1：

第二步，求解q₁，用T₁ ^-1左乘得

由上式可得：

将上式两边展开：

有上式可得：c₁p_y-s₁p_x＝0

容易得到：q₁＝arctan2(p_y,p_x)

同理可以求解q₂、q₃、q₄，得到水下机械臂运动学逆解。

实施3：水下机械臂的动力学模型的推导是实现水下机械臂控制的基础，可以描述水下机械臂的力与力矩信息和运动状态信息的关系。采用Lagrange方程法对四自由度水下机械臂进行动力学分析。

Lagrange函数可以被定义为系统的动能E_k和势能E_p的差来表示：

L＝E_k-E_p

根据Lagrange函数L可以得到Lagrange方程：

其中，q_i和

分别为广义坐标和广义速度，F_i为第i个广义坐标上的广义力或广义力矩，n为连杆数。

采用Lagrange方程法对四自由度水下机械臂的动力学分析如下。由于水下机械臂的第四个关节为回转关节，对机械臂的动力学建模并不会产生影响，并且，本发明不考虑机械臂系统末端执行器的姿态，只考虑末端执行器所处的位置，因此四自由度的水下机械臂可以简化为腰部回转，大臂摆动和小臂摆动的三连杆结构。简化后的水下机械臂的连杆的质心位于连杆的中心，第一关节的质量为m₁，第二关节质量为m₂，第三关节质量为m₃，长度分别为a₂、a₃和d₄，简化的水下机械臂模型如附图4所示。

当水下机械臂在流体环境中运动时，流体会对水下机械臂连杆的表面产生一定的阻力，水下机械臂的连杆按照阻力的方向可以分解为在其法线方向上流体阻力和切线方向上流体阻力，因为机械臂的连杆是一个规则的圆柱体，所以，其切线方向流体阻力很小，常常忽略不计。由于水流存在粘性，所以，在机械臂的连杆在做加速运动时，连杆周围的水流也会发生相对的运动从而水下机械臂的连杆产生一个反方向上的作用力，水下机械臂连杆所受的反作用力也叫做附加质量力。因此，水下机械臂在进行海洋作业时，必须对其进行水动力分析。本发明基于微元法和Morison公式对水下机械臂的单个连杆进行水动力学分析如附图5所示，考虑海洋流体环境的影响。

综合以上分析，水下机械臂的动力学模型的矩阵形式可以被表示为：

其中，

表示水下机械臂各个关节的关节变量，

表示水下机械臂的惯性矩阵，

是水下机械臂的哥式力和向心力项，

是水下阻尼矩阵，

表示水下机械臂的重力项，

表示水下机械臂的各个关节控制输入。

实施4：通过任务空间和关节空间的转换矩阵h(q)将四自由度水下机械臂的末端执行器在任务空间的轨迹表示成如下形式：

x_E＝h(q)

x_E是末端执行器在任务空间中的轨迹。水下机械臂的任务空间轨迹x_E和关节空间轨迹q的导数关系如下：

进一步整理，可以得到水下机械臂的任务空间动力学方程：

对上式，可得在[0,t_f]上重复运行的水下机械臂的任务空间动力学方程：

目的是在给定一个期望的轨迹x_d(t)，寻找控制力矩F_mx ^j(t)，实现系统输出关于任务空间期望轨迹的理想跟踪。

得到任务空间的动力学模型后，如附图6所示，引入全局快速终端滑膜面：

通过全局快速滑模面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象。以迭代学习控制的方式的增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，针对迭代学习控制中存在的初始值误差问题，通过引入初始状态的修正因子来克服初始重置误差带来的影响：

选择初始修正因子为：

定义修正函数：

其中，ε为一个充分小的正数。

最终通过Lyapunov定理证明任务空间控制器的稳定性，并且得到控制力矩和自适应律：

实施5：对本发明提出的任务空间控制系统进行仿真实验分析，当水下机械臂的腰部回转关节运动时，会对基座在橫摇方向上产生干扰力矩，当干扰力矩较大时，基座很可能或发生侧翻事故，因此本发明保持腰部回转关节一直处于静止状态，使水下机械臂的大臂和小臂连杆只在基座的垂直面上运动，将水下机械臂的动力学模型简化为二连杆机构。

首先令

定义

则dx_E＝J_mdq，其中

其为水下机械臂的任务空间的Jacobi矩阵。由上式可得，Jacobi矩阵及其Jacobi矩阵导数得具体形式：

进而得到简化后的位置动力学模型：

其中，

水下机械臂的末端执行器在任务空间的期望轨迹：

将其作为自适应迭代滑模控制的水下机械臂的空间控制器的控制输入，自适应律为：

并在两种工况下验证算法的有效性和鲁棒性，工况1为水下机械臂的水动力已知且不存在海流干扰，工况2为水下机械臂的水动力学未知且工作环境存在海流干扰。

由图7-10所示的迭代完成后位置跟踪误差效果可知，水下机械臂可以在有限时间内跟踪上期望的任务空间轨迹，且控制力矩的抖振较轻。

由图11-12可知，水下机械臂末端可以较快收敛于期望的任务空间轨迹，且无建模不确定性和外加干扰的情况下，仿真结束后的任务空间轨迹的跟踪误差为3.6×10^-4m，取得理想的控制效果。

由图13-17可知，大臂和小臂关节的控制输入为抵抗系统的不确定性有所增大，纵向和横向的位置跟踪误差稍微增大，任务空间轨迹的跟踪效果也受到了建模不确定性和外界干扰的影响，跟踪效果有所下降，但是，由图18可知，水下机械臂的任务空间轨迹跟踪误差大约为3mm，该控制器在水动力学未知和海流干扰的情况下，仍以较高精度跟踪在有限时间内上期望的任务空间轨迹，在水下作业环境中，具有较好的鲁棒性和适应性。

综上所述：本发明提供一种水下机械臂任务空间控制方法，能够在存在模型不确定性以及海流干扰的情况下，实现定点作业，并且满足精度和稳定性要求。水下机械臂的任务空间控制主要针对水下定点作业过程中其末端执行器的闭环控制任务，要求其末端可以在设定时间内快速到达期望位置。基于其水下作业的任务需求，设计了一种自适应迭代滑模控制方法来实现水下机械臂任务空间轨迹的跟踪控制，通过全局快速滑模面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象。以迭代学习控制的方式的增强水下机械臂末端轨迹跟踪的收敛能力和鲁棒性，针对迭代学习控制中存在的初始值误差问题，通过引入初始状态的修正因子来克服初始重置误差带来的影响，与此同时，通过自适应控制来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。本发明为水下机械臂适应未知海洋条件下的作业提供技术保障，极大了提高了水下机械臂的环境适应性和作业稳定性，有力地推进水下作业技术的发展。

Claims (4)

1.一种水下机械臂任务空间控制方法，其特征是，包括如下步骤：

步骤一：建立水下机械臂任务空间动力学模型；

步骤二：引入全局快速终端滑模面使跟踪误差快速收敛；

步骤三：迭代学习控制方式增强轨迹跟踪收敛能力和鲁棒性；

步骤四：引入初始修正银子克服初始值误差问题；

步骤五：自适应控制补偿外界干扰和模型不确定性的影响。

2.根据权利要求1所述的水下机械臂任务空间控制方法，其特征是，所述步骤一具体为：

对于水下机械臂的运动学，在其各个关节上建立相应的坐标系，再由坐标系之间的变换关系求解得到末端的位置和姿态信息，分为正运动学和逆运动学：

(1)正运动学：如附图2为水下机械臂的实物模型，由图可见，其主要由腰部和腕部的回转关节、大臂和小臂的摆动关节共同组成；对其各个关节进行分析，根据D-H参数法，建立如附图3所示的坐标系；可得水下机械臂各个关节的其次变换矩阵T_i(i＝1～4)：

其中，s₁，s₂，s₃和s₄分别表示sin q₁，sin q₂，sin q₃和sin q₄，c₁，c₂，c₃和c₄分别表示cosq₁，cos q₂，cos q₃和cos q₄；

将这个四个矩阵顺次相乘，可以推出水下机械臂末端执行器在基座标系下的位姿为：

其中，

c₂₃＝cos(q₂+q₃)，s₂₃＝sin(q₂+q₃)；

(2)逆运动学：逆推变换法进行逆运动学的求解，首先在水下机；械臂的运动学方程的左侧顺次乘以位姿矩阵的逆；第一步，推算水下机械臂系统的连杆变换矩阵对应的逆T₁ ^-1，T₂ ^-1，T₃ ^-1和T₄ ^-1：

第二步，求解q₁，用T₁ ^-1左乘得

由上式可得：

将上式两边展开：

有上式可得：c₁p_y-s₁p_x＝0

容易得到：q₁＝arctan2(p_y,p_x)

同理可以求解q₂、q₃、q₄，得到水下机械臂运动学逆解；

水下机械臂的动力学模型的推导是实现水下机械臂控制的基础，可以描述水下机械臂的力与力矩信息和运动状态信息的关系；采用Lagrange方程法对四自由度水下机械臂进行动力学分析；

Lagrange函数可以被定义为系统的动能E_k和势能E_p的差来表示：

L＝E_k-E_p

根据Lagrange函数L可以得到Lagrange方程：

其中，q_i和

分别为广义坐标和广义速度，F_i为第i个广义坐标上的广义力或广义力矩，n为连杆数；

采用Lagrange方程法对四自由度水下机械臂的动力学分析如下；由于水下机械臂的第四个关节为回转关节，对机械臂的动力学建模并不会产生影响，并且，本发明不考虑机械臂系统末端执行器的姿态，只考虑末端执行器所处的位置，因此四自由度的水下机械臂可以简化为腰部回转，大臂摆动和小臂摆动的三连杆结构；简化后的水下机械臂的连杆的质心位于连杆的中心，第一关节的质量为m₁，第二关节质量为m₂，第三关节质量为m₃，长度分别为a₂、a₃和d₄，简化的水下机械臂模型如附图4所示；

当水下机械臂在流体环境中运动时，流体会对水下机械臂连杆的表面产生一定的阻力，水下机械臂的连杆按照阻力的方向可以分解为在其法线方向上流体阻力和切线方向上流体阻力，因为机械臂的连杆是一个规则的圆柱体，所以，其切线方向流体阻力很小，常常忽略不计；由于水流存在粘性，所以，在机械臂的连杆在做加速运动时，连杆周围的水流也会发生相对的运动从而水下机械臂的连杆产生一个反方向上的作用力，水下机械臂连杆所受的反作用力也叫做附加质量力；因此，水下机械臂在进行海洋作业时，必须对其进行水动力分析；本发明基于微元法和Morison公式对水下机械臂的单个连杆进行水动力学分析如附图5所示，考虑海洋流体环境的影响；

综合以上分析，水下机械臂的动力学模型的矩阵形式可以被表示为：

其中，

表示水下机械臂各个关节的关节变量，

表示水下机械臂的惯性矩阵，

是水下机械臂的哥式力和向心力项，

是水下阻尼矩阵，

表示水下机械臂的重力项，

表示水下机械臂的各个关节控制输入；

通过任务空间和关节空间的转换矩阵h(q)将四自由度水下机械臂的末端执行器在任务空间的轨迹表示成如下形式：

x_E＝h(q)

x_E是末端执行器在任务空间中的轨迹；水下机械臂的任务空间轨迹x_E和关节空间轨迹q的导数关系如下：

进一步整理，可以得到水下机械臂的任务空间动力学方程：

对上式，可得在[0,t_f]上重复运行的水下机械臂的任务空间动力学方程：

目的是在给定一个期望的轨迹x_d(t)，寻找控制力矩F_mx ^j(t)，实现系统输出关于任务空间期望轨迹的理想跟踪。

3.根据权利要求1所述的水下机械臂任务空间控制方法，其特征是，所述步骤二具体为：得到任务空间的动力学模型后，引入全局快速终端滑膜面：

通过全局快速滑模面实现任务空间轨迹跟踪误差的有限时间内的快速收敛，且控制律中不含切换项，从而消除了滑模抖振的现象。

4.根据权利要求1所述的水下机械臂任务空间控制方法，其特征是，所述步骤四具体为：选择初始修正因子为：

定义修正函数：

其中，ε为一个充分小的正数；

最终通过Lyapunov定理证明任务空间控制器的稳定性，并且得到控制力矩和自适应律：

来补偿外界干扰和模型不确定性所带来的影响，增强水下机械臂在复杂水下环境下的适应能力和抗扰能力。

Images