CN117301064A - 一种基于固定时间收敛的双臂机器人安全协同控制方法 - Google Patents

Abstract

本发明公开了一种基于固定时间收敛的双臂机器人安全协同控制方法，属于双臂机器人控制领域。首先建立双臂机器人及其负载运动学模型和动力学模型，然后根据动力学模型基于固定时间收敛的滑模控制算法设计位置控制器，得到机械臂各关节控制力矩；再通过动力学模型和负载受力分析，计算双臂机器人左右臂末端夹持方向上的内力；设计内力控制器将内力误差转换为控制输出力，并将其转换为各关节力控制力矩，实现双臂机器人安全控制；最后将力和位置控制力矩叠加输入到动力学模型中，使双臂机器人实现位姿和夹持力的同步控制。其既可以保证双臂机器人对期望位姿的快速跟踪，又可以实现对双臂机器人夹持力的同步控制，最终完成双臂协同搬运任务。

Description

一种基于固定时间收敛的双臂机器人安全协同控制方法

技术领域

本发明涉及双臂机器人控制领域，具体是一种基于固定时间收敛的双臂机器人安全协同控制方法。

背景技术

随着科技的不断发展，机器人技术是目前科学研究与技术创新的热点，与传统的单机械臂相比，双臂机器人具有灵巧度高、负载能力大、精度易于保证等优点，目前在工业、航天与家庭服务等领域已广泛应用于协同搬运、焊接与装配等场景，同时随着电子技术、计算机技术的不断进步及生产制造成本的不算降低，双臂机器人在越来越多的领域展现出了应用潜力。

由于需要双机械臂及作业对象协同工作，双臂机器人控制系统比单台机械臂更加复杂：双臂的协调作业不仅要考虑双臂末端的运动学协调，同时也要考虑到双臂与目标物之间的动力学协调；运动学协调是指双臂末端应同步运动并始终满足位姿约束关系，使得机械臂末端保持恒定的间距和相对姿态以完成搬运目标物体的任务；动力学协调是指双臂末端按照期望轨迹移动的过程中必须保持合适的接触力以满足搬运要求。在算法控制方面，往往通过鲁棒控制、自适应控制和PID控制等方法来确保机械臂的轨迹跟踪精度。然而，此类方法的误差收敛速度和收敛精度难以得到保障，同时也无法保证协同工作的安全性。

因此在双臂机器人控制过程中，一方面如何提升跟踪误差的收敛速度，使双臂机器人实现对期望位姿快速跟踪；另一方面怎样优化双臂机器人夹持力同步控制，提升其稳定性和精准度，均是目前双臂机器人算法急需迫切解决的问题。

有鉴于此，特提出本发明。

发明内容

本发明的目的：针对上述现有技术存在的问题和不足，本发明旨在提出一种基于固定时间收敛的双臂机器人安全协同控制方法。考虑双臂机器人对负载物体搬运场景，进行平动和转动控制负载姿态。通过位姿与内力控制器的混合控制策略，既可以保证双臂机器人对期望位姿的快速跟踪，又可以实现对双臂机器人夹持力的同步控制，最终完成双臂协同搬运任务。

本发明的技术方案：为了实现上述发明目的，提供一种基于固定时间收敛的双臂机器人安全协同控制方法，双臂机器人包括左右两个三关节机械臂，机械臂端部设有能够夹持的负载的末端执行器，同时考虑双臂机器人的左右臂对夹持的负载进行平动和转动控制实现夹持搬运操作；

具体步骤如下：

步骤1，基于几何方法建立双臂机器人协同搬运负载时的运动学模型，并运用拉格朗日方程建立双臂机器人的动力学模型；

步骤2，根据双臂机器人协同搬运负载时的动力学模型，基于固定时间收敛的滑模控制算法设计关节位置控制器，得到左右臂各关节控制力矩以实现两机械臂高精度的轨迹跟踪控制；

步骤3，通过双臂机器人协同搬运负载时的动力学模型和机械臂的负载受力分析，计算出双臂机器人左右臂末端执行器的夹持内力；

步骤4，设计内力控制器对双臂机器人左右臂末端执行器的夹持内力进行实时控制以避免对搬运负载产生破坏，将夹持内力误差转换为左右两机械臂控制输出力，将控制输出力转换为双臂机器人各关节力控制力矩，避免左右机械臂协同作业失效，实现双臂机器人夹持力的安全控制；

步骤5，采用力位混合控制，将内力控制器和关节位置控制器的控制力矩叠加输入到双臂机器人动力学模型中，实现双臂机器人位置与姿态和夹持力的同步控制。

进一步，双臂机器人协同搬运负载时的运动学模型建立方法为：

根据下式确定双臂机器人左右机械臂末端执行器与搬运的负载的质心关系为：

式中，(x_c,y_c)与分别为负载的质心位置坐标及负载的姿态，姿态通过角度描述；记/>d为负载长度的1/2，(x_a,y_a)与/>分别为左臂末端位置及姿态，对式(1)求导得到机器人左臂末端速度为：

式中，J_o,a为左臂的速度雅可比矩阵，字符上的原点为变量点，表示字符代表变量关于时间导数的阶数；同理，双臂机器人右臂末端速度为：

式中，(x_b,y_b)与为右臂末端位置及姿态，J_o,b为右臂末端速度雅可比矩阵，进而完成运动学模型建立。

进一步，建立双臂机器人的动力学模型的方法如下：

基于拉格朗日方程，得到负载的动力学方程如下所示：

式中，M_c∈R^3×3代表机器人搬运的负载的质量惯性矩阵，C_c∈R^3×3代表科氏力/离心力矩阵，代表双臂机器人的速度雅可比矩阵，F＝[F_a ^T,F_b ^T]^T∈R^6×1代表被搬运物体对双臂机器人末端作用力,F_ε代表负载动力学建模不确定项，/>与/>分别代表负载速度与加速度；

在双臂机器人左右臂夹持负载的情况下，双臂机器人的动力学方程为：

式中，M＝diag([M_a,M_b])∈R^6×6为双臂机器人惯性矩阵，M＝diag(·)代表以元素·为对角线元素的对角阵，τ_w＝[τ_w1 ^T,τ_w2 ^T]^T∈R^6×1表示机器人关节控制力矩，代表左右机械臂各个关节角加速度，N＝[N₁ ^T,N₂ ^T]^T∈R^6×1为科氏力/离心力项，J＝diag[J_a,J_b]∈R^6×6代表左右机械臂末端和其各个关节速度的雅可比矩阵，τ_d＝[τ_d1 ^T,τ_d2 ^T]^T∈R^6×1表示双臂机器人未建模项及外部扰动；

结合联立式(2)、式(3)、式(5)得到双臂机器人-负载的动力学方程：

式中，M_x＝M_c+J_o ^TJ^-TMJ^-1J_o为双臂机器人-负载的惯性矩阵，d_x＝J_o ^TJ^-Tτ_d+F_ε为双臂机器人-负载的总扰动，为双臂机器人-负载的科氏力/离心力项。

进一步，基于双臂机器人-负载动力学方程，利用滑模控制算法设计固定时间收敛的快速终端滑模控制，得到双臂机器人协同搬运的关节位置控制器方程：双臂机器人协同搬运的位置控制器方程如下式(11)、式(12)、式(16)所示：

设计固定时间收敛滑模面，将跟踪误差e₁为：

e₁＝X_c-X_cd (7)

式中，X_cd为负载的期望位置与姿态；

为实现跟踪误差e₁具有固定时间收敛特性，设计如下形式滑模面函数：

式中，e为自然常数，其中a₁为大于0小于1的常数，b₁＞0、c₁＞0且b₁与c₁均为偶数，对角阵K(e₁)＝diag([k₁(e₁₁),k₂(e₁₂),k₃(e₁₃)])，式中系数 为自定义可调整的固定收敛时间，其中为大于1奇数，/>为大于1小于2的奇数，其中p₁至p₄为奇数，满足p₁＞p₂＞0与0＜p₃＜p₄，分别记/>与e₂为速度跟踪误差，具体如下：

对式(8)进行求导，得到：

式中，与K'(e₁)分别为/>与K(e₁)关于时间的导数；

为实现滑模面固定时间收敛，S_σ满足其中k₃、k₄均为正常数，0＜σ₃＜1，σ₄＞1，设计内力控制器的控制力矩为τ_w＝τ_eq+τ_es+τ_di，τ_eq、τ_es与τ_di分别为理想控制律、鲁棒控制项和外部扰动估计项；

理想控制律为：

式中，为中间函数，具体为：

鲁棒控制项为：

式中， 为自定义的固定收敛时间；根据式(6)可得：

式中，f_d＝-M_x ^-1·d_x，定义辅助变量V为：

式中，λ₁＞0、λ₂＞0、α₀＞1；为对公式(13)中的扰动f_d进行估计，以提高双臂机器人搬运负载的位置轨迹跟踪精度，设计扰动观测器形式如下：

式中，为扰动估计值，λ₃与λ'为正常数，/>为/>上界的估计值，其自适应更新率为/>δ₀＜0.01为正常数；

综上所得，外部扰动估计项为：

利用滑模面函数实现机械臂轨迹跟踪误差固定时间收敛，利用设计的控制力矩τ_w保证滑模面固定时间收敛，最终实现双臂机器人在搬运负载时的轨迹跟踪精度具有有限时间收敛特性。

进一步，将式(2)和式(3)代入式(8)得双臂机器人双臂末端夹持力F为：

将双臂末端作用力分解夹持的负载所需的内力和运动诱导力，如下所示：

F＝F_I+F_m (18)

式中，F_I与表示双臂机器人对夹持的负载所需的内力，F_m表示双臂机器人对夹持的负载所需的运动诱导力；

根据式(18)得到机器人双臂末端运动诱导力公式为：

即可得夹持内力表达式如下：

式中，I_n∈R^3×3代表单位矩阵；

根据内力表达式(20)与被控对象模型(6)，得到需要控制的夹持内力组成部分如下所示：

根据式(21)可以得到左右臂末端夹持内力为：

式中，F_Ia和F_Ib分别为左右臂末端的夹持内力。

进一步，利用双臂机器人左右臂末端的实际夹持内力F_Ia和F_Ib，将内力误差定义为：

ΔF_a＝F_Ia-F_Iad (24)

ΔF_b＝F_Ib-F_Ibd (25)

式中，F_Iad和F_Ibd为左右臂末端的期望夹持内力，根据内力误差设计如下所示力控制器为：

式中，k_p和k_I均为正增益对角阵，τ_a和τ_b分别表示左右臂末端夹持的负载的控制力矩，记为机器人左右双臂的力控制器所得控制力矩。

7.根据权利要求6所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤5中，将步骤2和步骤4所得到的双臂机器人搬运负载使左右双臂所需要的位姿控制器与内力控制器的控制力矩进行叠加为τ＝τ_li+τ_w，进而完成了双臂机器人位姿控制器与内力控制器设计，使双臂机器人既能实现对负载期望位姿的跟踪，又能实现对负载夹持力的同步控制。

与现有技术相比，本发明所提供的一种基于固定时间收敛的双臂机器人安全协同控制方法，其有益效果包括：

(1)本发明在轨迹跟踪方面，采用固定时间收敛的快速终端滑模控制器；一方面借助滑模控制的固定时间收敛特性，使得其误差收敛速度加快；另一方面该方法加入新提出的固定时间收敛特性的扰动观测器，在扰动上界未知的情况下可以快速估计扰动值，从而提高了双臂机器人在搬运负载时的轨迹跟踪精度。

(2)本发明在双臂协同搬运负载方面，采用力位混合控制策略进行控制，保证了协同控制的安全性。相较于目前主从臂搬运负载的方法，本发明在双臂协同运动时进行夹持力的控制，解决了夹持力过大或过小产生的夹持失效问题。并且考虑在负载进行搬运过程中的平动和转动运动控制问题，对物体的位置与姿态实现了精确控制。

(3)本发明在确保滑模控制时间收敛算法方面进行了创新：在设计位姿控制器时，提出了一种新型基于固定时间收敛的快速终端滑模面，通过鲁棒控制项设计，保证其固定时间收敛特性并提升其收敛速度和精度，设计一种新型扰动观测器，对系统外部不确定因素导致的扰动进行精准估计并补偿输入。以上方法使得在确保系统稳定性的同时，系统误差的收敛速度和精度显著提升。

附图说明

图1为本发明基于固定时间收敛的双臂机器人安全协同控制方法的流程示意图；

图2为本发明使用的双臂机器人及其负载模型示意图；

图3为本发明使用的双臂机器人机械臂运动学模型示意图；

图4为本发明基于固定时间收敛的双臂机器人安全协同控制方法的框图；

图5为本发明实施例中位置轨迹跟踪仿真示意图；

图6为本发明实施例中机械臂夹持力的仿真示意图；

图7为本发明实施例中双臂机器人沿X方向外部扰动跟踪示意图；

图8为本发明实施例中双臂机器人沿Y方向外部扰动跟踪示意图；

图9为为本发明实施例中双臂机器人沿转动方向外部扰动跟踪示意图。

具体实施方式

下面结合图1对本发明的实施方式进行更加详细解释说明。

如图1、图2和图4所示，本实施例的一种基于固定时间收敛的双臂机器人安全协同控制方法，主要包括一下步骤：

步骤1，建立双臂机器人协同搬运负载时的运动学模型，并运用拉格朗日方程建立其动力学模型。所述的双臂机器人包括左右两个机械臂，机械臂端部设有能够夹持物体的末端执行器，本算法考虑对夹持物体进行平动和转动控制实现夹持搬运操作；

步骤2，根据动力学模型，基于固定时间收敛的滑模控制算法设计位置控制器，得到左右两机械臂的各关节控制力矩；

步骤3，通过动力学模型和负载受力分析，计算双臂机器人左右臂末端夹持内力；

步骤4，设计内力控制器对夹持力进行实时控制以避免对物体产生破坏，将内力误差转换为控制输出力，并将其转换为各关节力控制力矩，实现双臂机器人安全控制；

步骤5，采用力位混合控制，将力和位置控制力矩叠加输入到动力学模型中，实现双臂机器人位置与姿态和夹持力的同步控制。

下面描述本发明的实施例，实施例的实例在附图中示出，参考实施例是示例性的，旨在解释本发明，而不能理解为对本发明的限制。

本发明所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤1中，机械臂负载未质量均匀且质心在几何中心处的长方体，其质量为m_c，长度为2d，高度为h，质心坐标为(x_c,y_c)，旋转角度为记/>根据机械臂与负载的几何关系可得到左机械臂末端位姿为：

本发明所设计双臂机器人工作场景考虑平动和转动方向，分析考虑负载沿X轴、Y轴的平移速度以及绕Z轴的角速度。对式(1)求导，并将其改写为矩阵形式为：

式中，J_o,a为左臂的速度雅可比矩阵。同理右机械臂末端与负载速度之间的关系如下所示：

式中，和/>表示右臂末端平动时在X轴Y轴上的速度，/>表示其转动时的旋转角速度，可得到：

本发明所设计双臂机器人工作场景同时考虑平动和转动方向，同时为简化算法设计过程，将以平面双臂机器人为例进行说明，本发明同样适用于具有空间运动形式的双臂机器人。

对双臂机器人抓取的未知负载对象进行动力学建模如下所示：

式中，代表负载的惯性矩阵，且C_c∈R^3×3,J_c代表转动惯量，m_c代表未知负载质量，代表双臂机器人的速度雅可比矩阵，F＝[F_a ^T,F_b ^T]^T∈R^6×1代表双臂机器人末端执行器的力，F_ε代表未知负载动力学建模不确定项，/>与/>分别代表负载速度与加速度。

在双臂机器人左右臂握持负载的情况下，可以得到双臂机器人的动力学方程为：

式中，M＝diag[M_a,M_b]∈R^6×6为双臂机器人质量惯性矩阵，M＝diag(·)代表以元素·为对角线元素的对角阵，τ_w＝[τ_w1 ^T,τ_w2 ^T]^T∈R^6×1表示机器人关节控制力矩，代表各个关节角加速度，N＝[N₁ ^T,N₂ ^T]^T∈R^6×1为机器人双臂的科氏力与离心力项，J＝diag[J_a,J_b]∈R^6×6代表双臂末端执行器和双臂机器人各个关节速度的雅可比矩阵，τ_d＝[τ_d1 ^T,τ_d2 ^T]^T∈R6×1表示双臂机器人未建模项及外部扰动。

联立式(5)与式(6)，经过变换可以得到：

根据图2所描述的双臂机器人及其负载的模型图解和图3描述的双臂机器人机械臂运动学模型，可推导出左机械臂末端速度与负载的速度关系如下所示：

式中，设定逆时针方向角度为正，左臂θ₁₁、θ₁₂和θ₁₃分别代表三个关节相对水平面其角度的绝对值，s₁、s₁₂、s₁₂₃分别代表sin(θ₁₁)、sin(θ₁₁+θ₁₂)、sin(θ₁₁+θ₁₂+θ₁₃)，c₁、c₁₂、c₁₂₃分别代表cos(θ₁₁)、cos(θ₁₁+θ₁₂)、cos(θ₁₁+θ₁₂+θ₁₃)。将式(8)与(2)联立可以得到：

同理，右机械臂末端相对于各个关节的速度表达式如下所示：

式中，设定逆时针方向角度为正，θ₂₁、θ₂₂和θ₂₃分别代表右臂三个关节相对水平面的角度，S₁、S₁₂、S₁₂₃分别代表sin(θ₂₁)、sin(θ₂₁+θ₂₂)、sin(θ₂₁+θ₂₂+θ₂₃)，C₁、C₁₂、C₁₂₃分别代表cos(θ₂₁)、cos(θ₂₁+θ₂₂)、cos(θ₂₁+θ₂₂+θ₂₃)。

将式(10)与式(3)联立可以得到

结合式(9)与式(11)，可以得到双臂机器人负载及各关节角度之间的速度关系为：

对上式求导，可得各关节角加速度为：

将式(12)和(13)带入到式(7)，合并同类项并整理可得双臂机器人-负载的动力学方程如下所示：

式中，M_x＝M_c+J_o ^TJ^-TMJ^-1J_o为惯性矩阵，为科氏力/离心力项，d_x＝J_o ^TJ^-Tτ_d+F_ε为模型不确定项以及外部环境扰动，即总扰动。

该实施例所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤2中，在双臂机器人-负载动力学方程的基础上，设计固定时间收敛的快速终端滑模控制器，得到双臂机器人协同搬运的位置控制器。

如图4所示，设计固定时间收敛滑模面，将跟踪误差定义为：

e₁＝X_c-X_cd (15)

式中，X_cd为负载的期望位置与姿态。为实现跟踪误差e₁具有固定时间收敛特性，将滑模面函数设计为以下形式：

式中，其中a₁为大于0小于1的常数，b₁＞0、c₁＞0且b₁与c₁均为偶数，K(e₁)＝diag([k₁(e₁₁),k₂(e₁₂),k₃(e₁₃)])，系数 e为自然常数， 为自定义可调整的固定收敛时间，为大于1奇数，/>为大于1小于2的奇数，其中p₁＞p₂＞0，0＜p₃＜p₄且为奇数。

将跟踪误差式(15)求导，可得速度跟踪误差为：

将式(16)进行求导，可得到：

式中，与K'(e₁)分别为/>与K(e₁)关于时间的导数。

为保证固定时间收敛，使S_σ满足其中k₃、k₄均为正常数，0＜σ₃＜1，σ₄＞1，设计控制器为τ_w＝τ_eq+τ_es+τ_di，τ_eq、τ_es与τ_di分别为理想控制律、鲁棒控制项和外部扰动估计项。

理想控制律为：

式中，定义为

鲁棒控制项为：

式中， 为自定义可调整的固定收敛时间。

根据式(14)可得：

式中，f_d＝-M_x ^-1·d_x为总扰动。

式中，λ₁＞0、λ₂＞0、α₀＞1。为对扰动进行估计，以提高轨迹跟踪精度，设计扰动观测器形式如下：

式中，为对误差进行的扰动估计值，λ₃＞0、λ'＞0，/>为对/>上界的估计值，其自适应更新率为/>δ₀＜0.01为正常数。

综上所得，扰动估计项为：

该实施例所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤3中，将动力学模型中所得的式(13)代入式(6)，可得双臂末端作用力为：

可将双臂末端作用力分解夹持物体所需的内力和运动诱导力，如下所示：

F＝F_I+F_m (26)

根据式(18)可得运动诱导力公式为：

将式(27)代入到式(26)中可得夹持力表达式为，如图6所示：

式中，I_n∈R^3×3代表单位矩阵。

将夹持力表达式与被控对象模型相结合，可以得到其内力组成部分如下所示：

根据式(29)并运动学模型结合分析，可以得到其夹持力为：

该实施例所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤4中，通过步骤3所得到的夹持方向上的内力F_Ia和F_Ib，定义跟踪力误差为：

ΔF_a＝F_Ia-F_Iad (32)

ΔF_b＝F_Ib-F_Ibd (33)

式中，F_Iad和F_Ibd为左右臂末端的期望夹持内力。将内力误差转换为力控制器中的控制力矩，如下式所示：

式中，k_p和k_I均为正增益对角阵，τ_a和τ_b并表示左右臂末端夹持物体的控制力矩，记为双臂机器人的力控制器所得控制力矩。

该实施例所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤5中，将步骤2和步骤4所得到的双臂机器人搬运所需要的位置控制力矩和力控制力矩，将位置控制力矩和力控制力矩进行叠加为τ＝τ_li+τ_w。将其输入到动力学模型中，使双臂机器人既能实现对负载期望位姿的跟踪，又能实现对负载夹持力的同步控制。

空间中负载质量为m_c＝0.05kg，d＝0.05m，双臂机器人与负载物体接触如图2所示。测试实验：对基于固定时间收敛的双臂机器人安全协同控制方法进行仿真实验。

图2是双臂机器人及其负载模型图。图3是双臂机器人机械臂运动学模型图。图4是基于有限时间双环滑模的双臂机器人协同搬运力位混合控制算法框图，通过构建基于双环滑模位置控制器和力控制器实现力位混合控制。图5是发明算法的位置轨迹跟踪控制仿真结果。图6是发明算法的力控制仿真结果，仿真时间为1s。图7-9是本发明涉及的双臂机器人对平动和转动三个方向扰动进行精准估计。

尽管上面已经示出和描述了本发明的实施例，可理解为上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求及等同限定。

Claims (7)

1.一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，双臂机器人包括左右两个三关节机械臂，机械臂端部设有能够夹持的负载的末端执行器，同时考虑双臂机器人的左右臂对夹持的负载进行平动和转动控制实现夹持搬运操作；

具体步骤如下：

步骤1，基于几何方法建立双臂机器人协同搬运负载时的运动学模型，并运用拉格朗日方程建立双臂机器人的动力学模型；

步骤2，根据双臂机器人协同搬运负载时的动力学模型，基于固定时间收敛的滑模控制算法设计关节位置控制器，得到左右臂各关节控制力矩以实现两机械臂高精度的轨迹跟踪控制；

步骤3，通过双臂机器人协同搬运负载时的动力学模型和机械臂的负载受力分析，计算出双臂机器人左右臂末端执行器的夹持内力；

步骤4，设计内力控制器对双臂机器人左右臂末端执行器的夹持内力进行实时控制以避免对搬运负载产生破坏，将夹持内力误差转换为左右两机械臂控制输出力，将控制输出力转换为双臂机器人各关节力控制力矩，避免左右机械臂协同作业失效，实现双臂机器人夹持力的安全控制；

步骤5，采用力位混合控制，将内力控制器和关节位置控制器的控制力矩叠加输入到双臂机器人动力学模型中，实现双臂机器人位置与姿态和夹持力的同步控制。

2.根据权利要求1所述的一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，双臂机器人协同搬运负载时的运动学模型建立方法为：

根据下式确定双臂机器人左右机械臂末端执行器与搬运的负载的质心关系为：

式中，(x_c,y_c)与分别为负载的质心位置坐标及负载的姿态，姿态通过角度描述；记d为负载长度的1/2，(x_a,y_a)与/>分别为左臂末端位置及姿态，对式(1)求导得到机器人左臂末端速度为：

式中，J_o,a为左臂的速度雅可比矩阵，字符上的原点为变量点，表示字符代表变量关于时间导数的阶数；同理，双臂机器人右臂末端速度为：

式中，(x_b,y_b)与为右臂末端位置及姿态，J_o,b为右臂末端速度雅可比矩阵，进而完成运动学模型建立。

3.根据权利要求2所述的一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，建立双臂机器人的动力学模型的方法如下：

基于拉格朗日方程，得到负载的动力学方程如下所示：

式中，M_c∈R^3×3代表机器人搬运的负载的质量惯性矩阵，C_c∈R^3×3代表科氏力/离心力矩阵，代表双臂机器人的速度雅可比矩阵，F＝[F_a ^T,F_b ^T]^T∈R^6×1代表被搬运物体对双臂机器人末端作用力,F_ε代表负载动力学建模不确定项，/>与/>分别代表负载速度与加速度；

在双臂机器人左右臂夹持负载的情况下，双臂机器人的动力学方程为：

式中，M＝diag([M_a,M_b])∈R^6×6为双臂机器人惯性矩阵，M＝diag(·)代表以元素·为对角线元素的对角阵，τ_w＝[τ_w1 ^T,τ_w2 ^T]^T∈R^6×1表示机器人关节控制力矩，代表左右机械臂各个关节角加速度，N＝[N₁ ^T,N₂ ^T]^T∈R^6×1为科氏力/离心力项，J＝diag[J_a,J_b]∈R^6×6代表左右机械臂末端和其各个关节速度的雅可比矩阵，τ_d＝[τ_d1 ^T,τ_d2 ^T]^T∈R^6×1表示双臂机器人未建模项及外部扰动；

结合联立式(2)、式(3)、式(5)得到双臂机器人-负载的动力学方程：

式中，M_x＝M_c+J_o ^TJ^-TMJ^-1J_o为双臂机器人-负载的惯性矩阵，d_x＝J_o ^TJ^-Tτ_d+F_ε为双臂机器人-负载的总扰动，为双臂机器人-负载的科氏力/离心力项。

4.根据权利要求3所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，基于双臂机器人-负载动力学方程，利用滑模控制算法设计固定时间收敛的快速终端滑模控制，得到双臂机器人协同搬运的关节位置控制器方程：双臂机器人协同搬运的位置控制器方程如下式(11)、式(12)、式(16)所示：

设计固定时间收敛滑模面，将跟踪误差e₁为：

e₁＝X_c-X_cd (7)

式中，X_cd为负载的期望位置与姿态；

为实现跟踪误差e₁具有固定时间收敛特性，设计如下形式滑模面函数：

式中，e为自然常数，其中a₁为大于0小于1的常数，b₁＞0、c₁＞0且b₁与c₁均为偶数，对角阵K(e₁)＝diag([k₁(e₁₁),k₂(e₁₂),k₃(e₁₃)])，式中系数 为自定义可调整的固定收敛时间，其中为大于1奇数，/>为大于1小于2的奇数，其中p₁至p₄为奇数，满足p₁＞p₂＞0与0＜p₃＜p₄，分别记/>与e₂为速度跟踪误差，具体如下：

对式(8)进行求导，得到：

式中，与K'(e₁)分别为/>与K(e₁)关于时间的导数；

为实现滑模面固定时间收敛，S_σ满足其中k₃、k₄均为正常数，0＜σ₃＜1，σ₄＞1，设计内力控制器的控制力矩为τ_w＝τ_eq+τ_es+τ_di，τ_eq、τ_es与τ_di分别为理想控制律、鲁棒控制项和外部扰动估计项；

理想控制律为：

式中，为中间函数，具体为：

鲁棒控制项为：

式中， 为自定义的固定收敛时间；根据式(6)可得：

式中，f_d＝-M_x ^-1·d_x，定义辅助变量V为：

式中，λ₁＞0、λ₂＞0、α₀＞1；为对公式(13)中的扰动f_d进行估计，以提高双臂机器人搬运负载的位置轨迹跟踪精度，设计扰动观测器形式如下：

式中，为扰动估计值，λ₃与λ'为正常数，/>为/>上界的估计值，其自适应更新率为δ₀＜0.01为正常数；

综上所得，外部扰动估计项为：

利用滑模面函数实现机械臂轨迹跟踪误差固定时间收敛，利用设计的控制力矩τ_w保证滑模面固定时间收敛，最终实现双臂机器人在搬运负载时的轨迹跟踪精度具有有限时间收敛特性。

5.根据权利要求4所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，将式(2)和式(3)代入式(8)得双臂机器人双臂末端夹持力F为：

将双臂末端作用力分解夹持的负载所需的内力和运动诱导力，如下所示：

F＝F_I+F_m (18)

式中，F_I与表示双臂机器人对夹持的负载所需的内力，F_m表示双臂机器人对夹持的负载所需的运动诱导力；

根据式(18)得到机器人双臂末端运动诱导力公式为：

即可得夹持内力表达式如下：

式中，I_n∈R^3×3代表单位矩阵；

根据内力表达式(20)与被控对象模型(6)，得到需要控制的夹持内力组成部分如下所示：

根据式(21)可以得到左右臂末端夹持内力为：

式中，F_Ia和F_Ib分别为左右臂末端的夹持内力。

6.根据权利要求5所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于，利用双臂机器人左右臂末端的实际夹持内力F_Ia和F_Ib，将内力误差定义为：

ΔF_a＝F_Ia-F_Iad (24)

ΔF_b＝F_Ib-F_Ibd (25)

式中，F_Iad和F_Ibd为左右臂末端的期望夹持内力，根据内力误差设计如下所示力控制器为：

式中，k_p和k_I均为正增益对角阵，τ_a和τ_b分别表示左右臂末端夹持的负载的控制力矩，记为机器人左右双臂的力控制器所得控制力矩。

7.根据权利要求6所述一种基于固定时间收敛的双臂机器人安全协同控制方法，其特征在于步骤5中，将步骤2和步骤4所得到的双臂机器人搬运负载使左右双臂所需要的位姿控制器与内力控制器的控制力矩进行叠加为τ＝τ_li+τ_w，进而完成了双臂机器人位姿控制器与内力控制器设计，使双臂机器人既能实现对负载期望位姿的跟踪，又能实现对负载夹持力的同步控制。