CN115625711A - 一种考虑末端力的双臂机器人协同控制方法 - Google Patents

Abstract

本发明属于机器人控制技术领域，设计了一种考虑末端力的双臂机器人协同控制方法；过干扰观测器对双臂系统未建模部分和未知的干扰等进行观测并对改进滑模控制器输出进行补偿从而减小控制误差；力外环采用自适应阻抗控制器根据末端接触力误差情况对环境参数进行自适应进而获得最优的机械臂末端接触力和位置之间的关系，避免了繁琐的阻抗系统参数调节的过程，另外根据物体运动的实际情况随时调整机械臂的期望力，得以达到更精确的机械臂力控制和位置控制的效果；实现了双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制。

Description

一种考虑末端力的双臂机器人协同控制方法

技术领域

本发明属于机器人控制技术领域，尤其涉及一种考虑末端力的双臂机器人协同控制方法。

背景技术

随着社会科技的发展，机械臂在工业中得到了广泛的应用。近年来，由于生产任务越来越复杂和单机械臂自身的局限性，很多任务无法通过单机械臂完成，例如：协调搬运、协同焊接和救援行动等。双臂机器人相比于单机械臂具有更大的负载能力、更高的灵巧度等优势，可以完成更多复杂的工作，逐渐进入生产、生活领域。

然而，由于多自由度的双臂机器人系统是高度耦合的非线性系统，其动力学建模比传统的单臂机器人要复杂，模型的不确定性增加。不确定因素会对机械臂的性能产生不利影响，针对双臂机器人的非线性和不确定性，并且在工作中不可避免力接触和力协调等问题，很多研究被展开。目前，如何实现双臂机器人位置和力的双重控制是目前国内外研究人员关注的主要研究方向。

发明内容

针对现有技术的不足，本发明提供一种考虑末端力的双臂机器人协同控制方法，不仅仅实现末端轨迹的精确控制，还考虑了机械臂末端力的跟踪效果，实现双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制。

一种考虑末端力的双臂机器人协同控制方法，具体为以下步骤：

步骤1：双臂机器人协同搬运物体属于紧协调操作任务，存在严格的运动约束关系，假设两个机械手都刚性地附在负载上，使抓握点与被抓物之间无滑移现象；当双臂机器人协同操作同一个物体时，机器人和物体形成一个闭链系统，定义：{O}为物体的中心位置，{O_R1}、{O_R1}分别为机器人R₁和R₂的基座坐标系，{O₂}、{O₁}分别为机器人R₁和R₂的工具坐标系；根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：

其中

为坐标系{O_R1}下机器人R₂末端执行器的位姿，

为坐标系{O_R1}下机器人R₁末端执行器的位姿；¹T₂为工具坐标系{O₂}到{O₁}的位姿变换矩阵，为：¹T₂＝Rot(z，π)×Trans(d₁，0，0)；

式1为机器人R₁和R₂之间的运动约束，同时联系着R₁和R₂的运动学，若当已知机器人R₁的构型或者末端执行器的位置，则通过运动约束求解机器人R₂的正向运动学与逆向运动学，无需再对机器人R₂进行运动学建模求解；

步骤2：依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析结果如下：

其中m为被搬运物体的质量；

和

分别为被双臂机器人搬运物体在x，y和z方向上的加速度；F₁和F₂分别是两个机械臂末端对物体的操作力；F_s1y，F_s2y，F_s1z和F_s2z分别是机械臂与物体的接触摩擦力在y轴和z轴方向的分力；μ是摩擦系数；

为保持物体平衡应该满足以下条件：

F_s1y ²+F_s1z ²＜(μF₁)²

F_s2y ²+F_s2z ²＜(μF₂)² (3)

双机械臂末端施加给被搬运物体的力由下式得到：

步骤3：运用拉格朗日方法，得到单机械臂的动力学方程为：

其中M_i(q_i)是第i个机械臂的对称正定惯性矩阵；

是科里奥利矩阵和离心矩阵；G_i(q_i)是引力矢量；J_i(q_i)是雅可比矩阵；F_i是物体施加在手臂上的力矢量；w是外部环境对于机械臂关节的扰动力矩；τ_i是机械臂关节的控制输入力矩；

根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程为：

其中，q＝[q₁ ^T，q₂ ^T]^T∈R^6×1为双机械臂的各关节角度矩阵；M(q)＝diag[M₁(q₁)，M₂(q₂)]∈R^6×6双机械臂的正定质量惯性矩阵；

为双机械臂的科里奥利力与离心力矩阵；G(q)＝[G₁ ^T(q₁)，G₂ ^T(q₂)]^T∈R^6×1为双机械臂的重力矩阵；τ＝[τ₁ ^T，τ₂ ^T]^T∈R^6×1为双机械臂的各关节控制输入力矩矩阵；F＝[F₁，F_s1y，F_s1z，F₂，F_s2y，F_s2z]^T为外环境力矩阵。J(q)＝diag[J₁(q₁)，J₂(q₂)]∈R^6×6为双机械臂的雅可比矩阵；w为干扰力矩矩阵；

双机械臂的正运动学模型表示为：

其中，

为机械臂末端的速度，

为机械臂关节角速度；

双机械臂的逆运动学模型表示为：

其中，

为机械臂末端的加速度，

为机械臂关节角加速度，

为J^-1(q)的导数；

双机械臂各关节力矩与双机械臂末端执行器的输出力之间的关系为：

τ＝J^T(q)F_x (9)

其中，F_x为双机械臂末端执行器的输出力；

将式8和9代入6式中得到双机械臂在笛卡尔坐标系下的动力学方程：

其中：

M_x(q)＝J^-T(q)M(q)J^-1(q)；

G_x(q)＝J^-T(q)G(q)；F_x＝J^-T(q)τ

步骤4：针对步骤3中提出的动力学模型方程式10，将外部干扰和未建模部分归类为干扰项，并用干扰观测器进行观测并与双曲正切滑模控制器相结合对双臂机器人末端进行位置控制；基于干扰观测器的改进滑模控制器为：

其中：

为干扰向量f_d的观测值；z为辅助变量，定义为：

为干扰观测器增益矩阵，向量

为干扰观测器辅助向量；

和

分别为实际值M_x(q)，

和G_x(q)的估计值；K＝diag{k₁，…k₆}，k_i＞0，i＝1，…6为滑模控制器的增益矩阵，ε＞0，η＞0，S为误差函数定义为：

Λ＝diag(λ₁，λ₂…λ₆)，λ_i＞0；e为机械臂末端的位置的跟踪误差e＝X_d-X；

步骤5：把物体的实际运动位置X_m与期望运动位置

进行比较，然后将误差进行运动/力的转换对开始期望力进行力补偿得到末端期望接触力F_d，通过自适应控制器估计环境的刚度和位置得到阻抗控制的初始期望位置X_c；实现过程如下：

步骤5.1：对物体的实际运动状态与期望运动状态进行比较，然后将误差进行运动/力的转换作为开始期望力的一个力补偿f_m为：

其中，M_d，B_d和K_d分别表示质量、阻尼、刚度矩阵；X_m，

和

表示物体运动的实际的位置、速度、加速度；

和

表示物体的期望的位置、速度、加速度；

步骤5.2：双机械臂末端与物体的期望接触力由下式表示：

其中：f＝[f_m，0₃]^T，0₃表示3×3的零矩阵；

步骤5.3：为了进行简化，下面仅考虑一个方向，通过自适应控制器估计环境的刚度和位置得到阻抗控制器的初始期望位置为：

其中，

表示环境位置x_e的估计值，

表示环境刚度k_e的估计值；采用间接自适应算法来对环境位置估计值

和环境刚度估计值

进行计算得到：

其中，x为机械臂末端位置，ξ₁和ξ₂为正常数，

为实际接触力F_c的估计值；

步骤6：将双臂机器人末端的实际接触力与期望接触力的误差通过阻抗控制修正后的期望位置为：

其中，F_c表示机器人末端与物体实际的接触力；F_d代表机器人末端与物体的期望接触力；M_m＝[M_m1，M_m2]^T，B_m＝[B_m1，B_m2]^T和K_m＝[K_m1，K_m2]分别表示被操作物体的质量惯性、阻尼、刚度系数矩阵；X，

和

表示机械臂末端的实际的期望位置、速度、加速度；X_c，

和

表示机械臂末端初始的期望的位置、速度、加速度；

步骤7：将步骤6得到的期望位置作为由步骤4得到的位置控制器的期望输入位置，与实际位置比较做差得到位置误差并通过控制器输出机械臂的输出力，控制双臂机器人完成对被操作物体的搬运任务；控制过程步骤具体为：

步骤7.1：根据步骤5得到的初始期望位置和期望接触力作为控制外环的期望输入；

步骤7.2：根据步骤6中由接触力误差修正后的位置作为步骤4控制内环控制器的期望输入，以此实现对双臂机器人末端接触力的精确控制；

步骤7.3：将位置误差通过步骤4提出的控制器得到机械臂的输出力/力矩，以此实现对双臂机器人末端位置的精确控制，以完成双臂机器人对物体进行搬运。

本发明有益技术效果：

为实现双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制问题，本申请提出了一种基于干扰观测器的改进滑膜力协同鲁棒自适应阻抗控制方法。为了提高控制精度，减小位置和力跟踪的稳态误差，位置内环采用基于干扰观测器的改进滑模控制器，通过干扰观测器对双臂系统未建模部分和未知的干扰等进行观测并对改进滑模控制器输出进行补偿从而减小控制误差。力外环采用自适应阻抗控制器根据末端接触力误差情况对环境参数进行自适应进而获得最优的机械臂末端接触力和位置之间的关系，避免了繁琐的阻抗系统参数调节的过程，另外根据物体运动的实际情况随时调整机械臂的期望力，得以达到更精确的机械臂力控制和位置控制的效果；实现了双臂机器人协同搬运物体的操作任务中末端力和位置的双重控制。

附图说明

图1本发明双臂机器人坐标示意图。

图2本发明被操作物体受力分析示意图。

图3本发明基于干扰观测器的改进滑模双臂协同自适应阻抗控制结构示意图。

图4本发明双臂系统阻抗模型结构示意图。

图5本发明双臂系统的Matlab建模及工作空间示意图。

图6本发明有无阻抗控制里跟踪对比示意图。

图7本发明基于不同切换函数的滑模控制器输出力矩比较示意图。

图8本发明被操作物体运动轨迹跟踪示意图。

图9本发明物体在空间中各个方向上的位置误差示意图。

图10本发明加干扰时物体在x轴方向的误差示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明；

一种考虑末端力的双臂机器人协同控制方法，具体为以下步骤：

步骤1：双臂机器人协同搬运物体属于紧协调操作任务，存在严格的运动约束关系，如图1所示双臂机器人系统由2个机械臂R₁和R₂组成，假设两个机械手都刚性地附在负载上，使抓握点与被抓物之间无滑移现象；当双臂机器人协同操作同一个物体时，机器人和物体形成一个闭链系统，定义：{O}为物体的中心位置，{O_R1}、{O_R1}分别为机器人R₁和R₂的基座坐标系，{O₂}、{O₁}分别为机器人R₁和R₂的工具坐标系；根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：

其中

为坐标系{O_R1}下机器人R₂末端执行器的位姿，

为坐标系{O_R1}下机器人R₁末端执行器的位姿；¹T₂为工具坐标系{O₂}到{O₁}的位姿变换矩阵，为：¹T₂＝Rot(z，π)×Trans(d₁，0，0)；

式1为机器人R₁和R₂之间的运动约束，同时联系着R₁和R₂的运动学，若当已知机器人R₁的构型或者末端执行器的位置，则通过运动约束求解机器人R₂的正向运动学与逆向运动学，无需再对机器人R₂进行运动学建模求解；

步骤2：双臂末端夹持和搬运目标物体过程中物体的受力状况可以由图2表示。其中，m为被操作物体的质量；F₁和F₂分别是两个机械臂末端对物体的操作力；F_s1和F_s2是机械臂与物体的接触摩擦力，用于牢固地支撑负载；F_s1y，F_s2y，F_s1z和F_s2z分别是摩擦力在y轴和z轴方向的分力；μ是摩擦系数。依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析结果如下：

其中m为被搬运物体的质量；

和

分别为被双臂机器人搬运物体在x，y和z方向上的加速度；F₁和F₂分别是两个机械臂末端对物体的操作力；F_s1y，F_s2y，F_s1z和F_s2z分别是机械臂与物体的接触摩擦力在y轴和z轴方向的分力；μ是摩擦系数；

为保持物体平衡应该满足以下条件：

F_s1y ²+F_s1z ²＜(μF₁)²

F_s2y ²+F_s2z ²＜(μF₂)² (3)

双机械臂末端施加给被搬运物体的力由下式得到：

步骤3：运用拉格朗日方法，得到单机械臂的动力学方程为：

其中Mi(qi)是第i个机械臂的对称正定惯性矩阵；

是科里奥利矩阵和离心矩阵；G_i(q_i)是引力矢量；J_i(q_i)是雅可比矩阵；F_i是物体施加在手臂上的力矢量；w是外部环境对于机械臂关节的扰动力矩；τ_i是机械臂关节的控制输入力矩；

根据单机械臂的动力学方程可以推广得到双机械臂在关节空间中的动力学方程为：

其中，q＝[q₁ ^T，q₂ ^T]^T∈R^6×1为双机械臂的各关节角度矩阵；M(q)＝diag[M₁(q₁)，M₂(q₂)]∈R^6×6双机械臂的正定质量惯性矩阵；

为双机械臂的科里奥利力与离心力矩阵；G(q)＝[G₁ ^T(q₁)，G₂ ^T(q₂)]^T∈R^6×1为双机械臂的重力矩阵；τ＝[τ₁ ^T，τ₂ ^T]^T∈R^6×1为双机械臂的各关节控制输入力矩矩阵；F＝[F₁，F_s1y，F_s1z，F₂，F_s2y，F_s2z]^T为外环境力矩阵。J(q)＝diag[J₁(q₁)，J₂(q₂)]∈R^6×6为双机械臂的雅可比矩阵；w为干扰力矩矩阵；

双机械臂的正运动学模型表示为：

其中，

为机械臂末端的速度，

为机械臂关节角速度；

双机械臂的逆运动学模型表示为：

其中，

为机械臂末端的加速度，

为机械臂关节角加速度，

为J^-1(q)的导数；

双机械臂各关节力矩与双机械臂末端执行器的输出力之间的关系为：

τ＝J^T(q)F_x (9)

其中，F_x为双机械臂末端执行器的输出力；

将式8和9代入6式中得到双机械臂在笛卡尔坐标系下的动力学方程：

其中：

M_x(q)＝J^-T(q)M(q)J^-1(q)；

G_x(q)＝J^-T(q)G(q)；F_x＝J^-T(q)τ

步骤4：针对步骤3中提出的动力学模型方程式10，将外部干扰和未建模部分归类为干扰项，并用干扰观测器进行观测并与双曲正切滑模控制器相结合对双臂机器人末端进行位置控制；基于干扰观测器的改进滑模控制器为：

其中：

为干扰向量f_d的观测值；z为辅助变量，定义为：

为干扰观测器增益矩阵，向量

为干扰观测器辅助向量；

和

分别为实际值Mx(q)，

和Gx(q)的估计值；K＝diag{k₁，…k₆}，ki＞0，i＝1，…6为滑模控制器的增益矩阵，ε＞0，η＞0，S为误差函数定义为：

Λ＝diag(λ₁，λ₂…λ₆)，λ_i＞0；e为机械臂末端的位置的跟踪误差e＝X_d-X；

步骤5：把物体的实际运动位置X_m与期望运动位置

进行比较，然后将误差进行运动/力的转换对开始期望力进行力补偿得到末端期望接触力F_d，通过自适应控制器估计环境的刚度和位置得到阻抗控制的初始期望位置X_c；实现过程如下：

步骤5.1：把物体的实际运动位置X_m与期望运动位置

进行比较，然后将误差进行运动/力的转换作为开始期望力的一个力补偿fm为：

其中，M_d，B_d和K_d分别表示质量、阻尼、刚度矩阵；X_m，

和

表示物体运动的实际的位置、速度、加速度；

和

表示物体的期望的位置、速度、加速度；

步骤5.2：双机械臂末端与物体的期望接触力由下式表示：

其中：f＝[f_m，0₃]^T，0₃表示3×3的零矩阵；

步骤5.3：为了进行简化，下面仅考虑一个方向，通过自适应控制器估计环境的刚度和位置得到阻抗控制器的初始期望位置为：

其中，

表示环境位置x_e的估计值，

表示环境刚度ke的估计值；采用间接自适应算法来对环境位置估计值

和环境刚度估计值

进行计算得到：

其中，x为机械臂末端位置，ξ₁和ξ₂为正常数，

为实际接触力F_c的估计值；

步骤6：阻抗控制的目的是将原有的物体动力学修正为期望的动力学。通过阻抗控制的理论，可以将机械臂末端与物体的接触表现的像是两个质量-弹簧-阻尼系统。双臂机器人的阻抗控制结构示意图如图4所示，其中F_R1和F_R2表示机械臂R₁和R₂末端与被操作物体的接触力矩阵，M_m1、M_m2、B_m1、B_m2、K_m1和K_m2为机械臂R₁和R₂末端与被操作物体之间的阻抗参数矩阵。将双臂机器人末端的实际接触力与期望接触力的误差通过阻抗控制修正后的期望位置为：

其中，F_c表示机器人末端与物体实际的接触力；F_d代表机器人末端与物体的期望接触力；M_m＝[M_m1，M_m2]^T，B_m＝[B_m1，B_m2]^T和K_m＝[K_m1，K_m2]分别表示被操作物体的质量惯性、阻尼、刚度系数矩阵；

和

表示机械臂末端的实际的期望位置、速度、加速度；

和

表示机械臂末端初始的期望的位置、速度、加速度；

步骤7：将步骤6得到的期望位置作为由步骤4得到的位置控制器的期望输入位置，与实际位置比较做差得到位置误差并通过控制器输出机械臂的输出力，控制双臂机器人完成对被操作物体的搬运任务；整体的基于干扰观测器的改进滑模双臂协同自适应阻抗控制结构如图3所示；控制过程步骤具体为：

步骤7.1：根据步骤5得到的初始期望位置和期望接触力作为控制外环的期望输入；

步骤7.2：根据步骤6中由接触力误差修正后的位置作为步骤4控制内环控制器的期望输入，以此实现对双臂机器人末端接触力的精确控制；

步骤7.3：将位置误差通过步骤4提出的控制器得到机械臂的输出力/力矩，以此实现对双臂机器人末端位置的精确控制，以完成双臂机器人对物体进行搬运。

测试实验：对考虑末端力的双臂机器人协同控制方法进行仿真实验。

设空间中有一目标物体，其长度为l＝1m，质量为1kg，在世界坐标系(0.2，1，0)处。将两台平放的三自由度机械臂基座安装在(0.5，0，0)和(-0.5，0，0)处；如图6所示。假设两台机械臂在初始状态下，它们的末端间距为1m，恰好与目标物体两端相接触，如图5(a)所示。图5(b)为双机械臂系统的工作空间，期望轨迹在双机械臂系统的工作空间内选取，选为：

仿真时间为5s。

为验本文提出的基于阻抗控制的滑模控制在机械臂末端力跟踪的有效性，本文针对有无阻抗控制对右臂末端各方向的力进行跟踪对比，对比结果如图6所示，图6(a)为无阻抗控制时的正切滑模控制力跟踪结果图，图6(b)为有阻抗控制时的正切滑模控制力跟踪结果图，可见本申请提出的使用阻抗控制的双曲正切滑模控制器可以很好的实现能够实现对期望接触力的跟踪。图7为本发明基于不同切换函数的滑模控制器输出力矩比较图，从图中可以看出，与基于符号函数(sign)滑模控制器相比，本文采用的双曲正切函数(tanh)滑模控制器可以使输出力矩更为平滑，进而消除抖振现象。为验证最终提出系统的有效性，将提出的加干扰观测器和力补偿的基于阻抗控制的双正切函数滑模控制器(Im+SM+f+ob)与加力补偿的基于阻抗控制的双曲正切函数滑模控制(Im+SM+f)和基于阻抗控制的双曲正切函数滑模控制器(Im+SM)进行仿真对比分析。仿真结果如图8和图9所示。由图8可知，这三种控制器都可以实现机械臂末端操作物体实现对期望轨迹的跟踪，能够完成目标物体的协调搬运任务。由图9轨迹跟踪误差对比可知，这三种控制器在x轴和y轴方向上的跟踪精度可以达0.001m以内，在z轴方向上的跟踪精度可以达到0.005m以内。其次为验证系统的鲁棒性，在t＝2s时在机械臂末端的x方向加入10N的外部干扰。各种算法下物体在x轴方向的误差如图10所示，图中分别比较了无外部干扰时，加入外扰时各种控制算法控制下的物体在x轴运动方向的误差曲线图。从图10中可以明确的看出本文提出的控制方法(Im+SM+f+ob)在面对外部干扰时波动后可以重新到达之前的控制精度，具有很好的鲁棒性。

综上，针对双臂机器人协同搬运物体的任务，本申请提出的控制方法可以实现双臂机器人末端搬运物体完成期望的轨迹和对末端接触力的良好跟踪。

Claims (8)

1.一种考虑末端力的双臂机器人协同控制方法，其特征在于，具体为以下步骤：

步骤1：双臂机器人协同搬运物体属于紧协调操作任务，存在严格的运动约束关系，假设两个机械手都刚性地附在负载上，使抓握点与被抓物之间无滑移现象；当双臂机器人协同操作同一个物体时，机器人和物体形成一个闭链系统，定义：{O}为物体的中心位置，{O_R1}、{O_R1}分别为机器人R₁和R₂的基座坐标系，{O₂}、{O₁}分别为机器人R₁和R₂的工具坐标系；根据双臂机器人工作时的情况确定双臂机器人的运动约束关系；

步骤2：依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析；

步骤3：运用拉格朗日方法，得到单机械臂的动力学方程；根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程；

步骤4：针对步骤3中提出的动力学模型，将外部干扰和未建模部分归类为干扰项，并用干扰观测器进行观测并与双曲正切滑模控制器相结合对双臂机器人末端进行位置控制；

步骤5：把物体的实际运动位置X_m与期望运动位置

进行比较，然后将误差进行运动/力的转换对开始期望力进行力补偿得到末端期望接触力F_d，通过自适应控制器估计环境的刚度和位置得到阻抗控制的初始期望位置X_c；

步骤6：将双臂机器人末端的实际接触力与期望接触力的误差通过阻抗控制修正后得到期望位置；

步骤7：将步骤6得到的期望位置作为由步骤4得到的位置控制器的期望输入位置，与实际位置比较做差得到位置误差并通过控制器输出机械臂的输出力，控制双臂机器人完成对被操作物体的搬运任务。

2.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤1所述根据双臂机器人工作时的情况确定双臂机器人的运动约束关系为：

其中

为坐标系{O_R1}下机器人R₂末端执行器的位姿，

为坐标系{O_R1}下机器人R₁末端执行器的位姿；¹T₂为工具坐标系{O₂}到{O₁}的位姿变换矩阵，为：¹T₂＝Rot(z，π)×Trans(d₁，0，0)；

式1为机器人R₁和R₂之间的运动约束，同时联系着R₁和R₂的运动学，若当已知机器人R₁的构型或者末端执行器的位置，则通过运动约束求解机器人R₂的正向运动学与逆向运动学，无需再对机器人R₂进行运动学建模求解。

3.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤2依据牛顿第二定律对双臂机器人搬运的物体的受力情况进行受力分析结果如下：

其中m为被搬运物体的质量；

和

分别为被双臂机器人搬运物体在x，y和z方向上的加速度；F₁和F₂分别是两个机械臂末端对物体的操作力；F_s1y，F_s2y，F_s1z和F_s2z分别是机械臂与物体的接触摩擦力在y轴和z轴方向的分力；μ是摩擦系数；

为保持物体平衡应该满足以下条件：

F_s1y ²+F_s1z ²＜(_μF₁)²

F_s2y ²+F_s2z ²＜(μF₂)² (3)

双机械臂末端施加给被搬运物体的力由下式得到：

。

4.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤3运用拉格朗日方法，得到单机械臂的动力学方程为：

其中M_i(q_i)是第i个机械臂的对称正定惯性矩阵；

是科里奥利矩阵和离心矩阵；G_i(q_i)是引力矢量；J_i(q_i)是雅可比矩阵；F_i是物体施加在手臂上的力矢量；w是外部环境对于机械臂关节的扰动力矩；_τi是机械臂关节的控制输入力矩；

根据单机械臂的动力学方程推广得到双机械臂在关节空间中的动力学方程为：

其中，q＝[q₁ ^T，q₂ ^T]^T∈R^6×1为双机械臂的各关节角度矩阵；M(q)＝diag[M₁(q₁)，M₂(q₂)]∈R^6×6双机械臂的正定质量惯性矩阵；

为双机械臂的科里奥利力与离心力矩阵；G(q)＝[G₁ ^T(_q1)，G₂ ^T(_q2)]^T∈R^6×1为双机械臂的重力矩阵；τ＝[τ₁ ^T，τ₂ ^T]^T∈R^6×1为双机械臂的各关节控制输入力矩矩阵；F＝[F₁，F_s1y，F_s1z，F₂，F_s2y，F_s2z]^T为外环境力矩阵；J(q)＝diag[J₁(q₁)，J₂(q₂)]∈R^6×6为双机械臂的雅可比矩阵；w为干扰力矩矩阵；

双机械臂的正运动学模型表示为：

其中，

为机械臂末端的速度，

为机械臂关节角速度；

双机械臂的逆运动学模型表示为：

其中，

为机械臂末端的加速度，

为机械臂关节角加速度，j^-1(q)为J^-1(q)的导数；

双机械臂各关节力矩与双机械臂末端执行器的输出力之间的关系为：

τ＝J^T(q)F_x (9)

其中，F_x为双机械臂末端执行器的输出力；

将式8和9代入6式中得到双机械臂在笛卡尔坐标系下的动力学方程：

其中：

M_x(q)＝J^-T(q)M(q)J^-1(q)；

G_x(q)＝J^-T(q)G(q)；F_x＝J^-T(q)τ。

5.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤4基于干扰观测器的改进滑模控制器为：

其中：

为干扰向量f_d的观测值；z为辅助变量，定义为：

为干扰观测器增益矩阵，向量

为干扰观测器辅助向量；

和

分别为实际值M_x(q)，

和G_x(q)的估计值；K＝diag{k₁，…k₆}，k_i＞0，i＝1，…6为滑模控制器的增益矩阵，ε＞0，η＞0，S为误差函数，定义为：

Λ＝diag(λ₁，λ₂…λ₆)，λ_i＞0；e为机械臂末端的位置的跟踪误差e＝X_d-X。

6.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤5具体为：

步骤5.1：对物体的实际运动状态与期望运动状态进行比较，然后将误差进行运动/力的转换作为开始期望力的一个力补偿f_m为：

其中，M_d，B_d和K_d分别表示质量、阻尼、刚度矩阵；X_m，

和

表示物体运动的实际的位置、速度、加速度；

和

表示物体的期望的位置、速度、加速度；

步骤5.2：双机械臂末端与物体的期望接触力由下式表示：

其中：f＝[f_m，0₃]^T，0₃表示3×3的零矩阵；

步骤5.3：为了进行简化，下面仅考虑一个方向，通过自适应控制器估计环境的刚度和位置得到阻抗控制器的初始期望位置为：

其中，

表示环境位置x_e的估计值，

表示环境刚度k_e的估计值；采用间接自适应算法来对环境位置估计值

和环境刚度估计值

进行计算得到：

其中，x为机械臂末端位置，ξ₁和ξ₂为正常数，

为实际接触力F_c的估计值。

7.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤6所述通过阻抗控制修正后得到期望位置具体为：

其中，F_c表示机器人末端与物体实际的接触力；F_d代表机器人末端与物体的期望接触力；M_m＝[M_m1，M_m2]^T，B_m＝[B_m1，B_m2]^T和K_m＝[K_m1，K_m2]分别表示被操作物体的质量惯性、阻尼、刚度系数矩阵；X，

和

表示机械臂末端的实际的期望位置、速度、加速度；X_c，

和

表示机械臂末端初始的期望的位置、速度、加速度。

8.根据权利要求1所述的一种考虑末端力的双臂机器人协同控制方法，其特征在于，步骤7具体为：

步骤7.1：根据步骤5得到的初始期望位置和期望接触力作为控制外环的期望输入；

步骤7.2：根据步骤6中由接触力误差修正后的位置作为步骤4控制内环控制器的期望输入，以此实现对双臂机器人末端接触力的精确控制；

步骤7.3：将位置误差通过步骤4提出的控制器得到机械臂的输出力/力矩，以此实现对双臂机器人末端位置的精确控制，以完成双臂机器人对物体进行搬运。

Images