CN111687822B - 一种双机器人导引轨迹多空间自适应插补方法 - Google Patents

Abstract

本发明提出一种双机器人导引轨迹多空间自适应插补方法，以机器人末端的运动学约束为基础，并根据机器人笛卡尔空间和关节空间的运动限制条件对导引轨迹进行双空间插补规划。本发明的方法能够提高双机器人系统在复杂环境中的适应能力、降低协调轨迹规划算法的复杂性，保证双机器人系统导引控制算法的安全性；同时根据安全限制条件分别对笛卡尔空间和关节空间进行自适应插补，可以有效排除因传感器力信号突变或者操作者导引力太大而导致的机器人安全运行隐患。

Description

一种双机器人导引轨迹多空间自适应插补方法

技术领域

本发明属于机器人运动学技术领域，尤其是一种双机器人导引轨迹多空间自适应插补方法。

背景技术

双机器人协调搬运弱刚性构件是装配工作的基础，多机器人协调控制系统的核心目标是实现被夹持对象的期望运动。因此，相较于控制从机器人跟随主机器人运动来间接实现对象运动的方式，直接将操作对象作为控制目标将更加简单直观，而且更容易实现被夹持对象的复杂运动轨迹。

由力传感器导引控制的原理可知，当导引传感器的力信号突变或过大时，对象的导引轨迹也会按照相同的趋势进行变化。然而操作对象与机器人末端是一个相互耦合的运动学系统，倘若对象仍按照不合理的导引轨迹运行，机器人末端以及机器人关节可能会发生诸多超出安全限制的危险运动。

因此，以机器人末端的运动学约束为基础，根据机器人的运动限制条件对导引轨迹进行插补规划至关重要。

发明内容

本发明所解决的技术问题在于提供一种双机器人导引轨迹多空间自适应插补方法，以机器人末端的运动学约束为基础，并根据机器人笛卡尔空间和关节空间的运动限制条件对导引轨迹进行双空间插补规划，可以有效排除因传感器力信号突变或者操作者导引力太大而导致的机器人安全运行隐患。

实现本发明目的的技术解决方案为：

一种双机器人导引轨迹多空间自适应插补方法，包括以下步骤：

步骤1：根据操作对象与机器人末端的运动学约束关系，计算对象导引轨迹对应的各机器人末端轨迹，包括机器人的位置约束、速度约束和加速度约束；

步骤2：监测机器人的末端速度和加速度、关节速度和加速度，若机器人的运动参数均满足安全限制条件，则直接下发执行，否则，根据超限情况对导引轨迹进行自适应插补后下发执行；

步骤3：对导引轨迹进行笛卡尔空间的自适应插补：

步骤3-1：将操作对象的位置增量作为初始离散导引轨迹Δx_initial(n)，对象的速度和加速度为：

Δt为系统控制周期，各机器人末端的速度和加速度为[v_e1 w_e1]^T、[v_e2 w_e2]^T、

设定两台机器人在笛卡尔空间中的最大限制速度为v_{max_e1}、v_{max_e2}，最大限制角速度为w_{max_e1}、w_{max_e2}，最大限制线加速度为

最大限制角加速度为

步骤3-2：根据操作对象的角速度过大和/或角加速度过大，对其旋转导引轨迹进行插补；

步骤3-3：在操作对象的旋转导引轨迹插补后，重新计算机器人末端的速度v_ei、加速度w_ei以及对象导引轨迹离散点Δx_initial(n)，再根据操作对象的移动速度过大和/或移动加速度过大，对其移动导引轨迹进行插补；

步骤4：根据各关节的运动约束条件，对导引轨迹进行关节空间的自适应插补：

步骤4-1：设定各机器人在关节空间中的关节最大限制速度为

最大限制加速度为

机器人末端速度与关节速度之间通过机器人雅克比矩阵J(q_i)表示,两机器人的关节速度

表示为：

步骤4-2：根据机器人关节速度过大和/或关节加速度过大，对机器人的关节导引轨迹进行插补。

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤1中的位置约束具体为：

根据操作对象位置计算机器人的末端位姿矩阵：

^R1T_c＝^R1T_e1·^e1T_c1·^c1T_c＝^R1T_R2·^R2T_e2·^e2T_c2·^c2T_c

^R1T_c为操作对象在世界坐标系{R1}的齐次变化矩阵，^R1T_e1、^R2T_e2为双机器人在各自坐标系下的位姿齐次矩阵，^c1T_c、^c2T_c、^e1T_c1、^e2T_c2是由对象抓取方式确定的常数矩阵，^R1T_R2为双机器人基坐标系标定矩阵。

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤1中的速度约束具体为：

当双机器人末端与操作对象采用刚性抓取方式时，操作对象与双机器人末端的速度关系为：

v_c、w_c表示对象运动的线速度与角速度，v_ei、w_ei表示第i个机器人末端的线速度与角速度，r_ei＝[r_eix r_eiy r_eiz]^T表示第i个机器人末端在对象坐标系中的位置矢量；

则，机器人末端的速度约束方程为：

其中，

E₃表示3阶单位矩阵，O₃表示3阶零矩阵。

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤1中的加速度约束具体为：

操作对象与机器人末端的加速度约束关系为：

则，机器人末端的加速度约束方程为：

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤3-2具体包括以下步骤：

1)当操作对象角速度过大而导致机器人末端角速度超限时，计算各机器人末端角速度超限比例R_{velo_ei}，选取最大超限比例R_{velo_END}进行降速插补，得到：

R_{velo_ei}＝ABS(w_ei)/w_{max_ei},i∈[1,2]

R_{velo_END}＝max(R_{velo_e1},R_{velo_e2}),R_{velo_END}＞1

R_{velo_END}为导引轨迹插补的调节参数，以机器人末端角速度约束条件来插补操作对象离散导引轨迹的旋转角度为：

Δθ_{vcal_END}(n)＝Δθ_initial(n)/R_{velo_END}

2)当操作对象角加速度过大而导致机器人末端角加速度超限时，计算各机器人末端角加速度超限比例R_{accel_ei}，选取最大超限比例值R_{accel_END}进行插补，得到：

R_{accel_END}＝max(R_{accel_e1},R_{accel_e2}),R_{accel_END}＞1

R_{accel_END}为引导轨迹插补的调节参数，以机器人末端角加速度约束条件来插补操作对象导引轨迹的旋转角度为：

3)当操作对象同时存在角速度过大且角加速度过大而导致机器人超限时，则选取两者中的最小值作为插补操作对象导引轨迹的旋转角度；

即，Δθ_{cal_END}作为本周期的离散轨迹点输出值表示为：

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤3-3具体包括以下步骤：

1)当对象移动速度过大而导致机器人末端速度超限时，计算各机器人末端的速度超限比例R′_{velo_ei}，选取最大超限比例R′_{velo_END}进行降速插补，得到：

R′_{velo_ei}＝ABS(v_ei)/v_{max_ei} i∈[1,2]

R′_{velo_END}＝max(R′_{velo_e1},R′_{velo_e2}),R′_{velo_END}>1

R′_{velo_END}为导引轨迹插补的调节参数，以机器人末端速度约束条件来插补对象导引轨迹的离散轨迹点(包括位置与姿态)为：

Δx_{vcal_END}(n)＝Δx_initial(n)/R′_{velo_END}

2)当对象移动加速度过大而导致机器人末端加速度超限时，计算各机器人末端的加速度超限比例R′_{accel_ei}，选取最大超限比例R′_{accel_END}进行插补，得到：

R′_{accel_END}＝max(R′_{accel_e1},R′_{accel_e2}),R′_{accel_END}>1

R′_{accel_END}为导引轨迹插补的调节参数，以机器人末端加速度约束条件来插补对象导引轨迹的离散轨迹点(包括位置与姿态)为：

3)当操作对象同时存在移动速度过大和移动加速度过大而导致机器人超限时，则选取两者中的最小值作为插补操作对象导引轨迹的离散轨迹点；

即，Δx_{cal_END}为本周期的离散轨迹点输出量表示为：

进一步的，本发明的双机器人导引轨迹多空间自适应插补方法，步骤4-2具体包括以下步骤：

1)当机器人关节速度过大而导致超限时，计算机器人各关节的关节角速度超限比例，选取最大超限比例R_{velo_Ji}进行插补降速，得到：

R_{velo_JOINT}＝max(R_{velo_J1},R_{velo_J2})

R_{velo_JOINT}为导引轨迹插补的调节参数，以单位时间的关节角速度超限比例对笛卡尔空间插补后的离散导引轨迹点Δx_{vcal_JOINT}进行插补，得到：

Δx_{vcal_JOINT}(n)＝Δx_{cal_END}(n)/R_{velo_JOINT}

2)当机器人关节加速度过大而导致超限时，计算机器人各关节的关节角加速度超限比例，选择最大超限比例R_{accel_Ji}进行插补，得到：

R_{accel_JOINT}＝max(R_{accel_J1},R_{accel_J2})

R_{accel_JOINT}为导引轨迹插补的调节参数，以机器人关节角加速度超限比例对笛卡尔空间插补后的离散导引轨迹点Δx_{cal_END}进行插补，得到：

3)当机器人的关节同时存在关节速度过大和关节加速度过大而导致超限时，则选取两者中的较小值作为插补；

即，Δx作为本周期的离散轨迹点输出量表示为：

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明的双机器人导引轨迹多空间自适应插补方法能够提高双机器人系统在复杂环境中的适应能力、降低协调轨迹规划算法的复杂性，保证双机器人系统导引控制算法的安全性。

2、本发明的双机器人导引轨迹多空间自适应插补方法根据安全限制条件分别对笛卡尔空间和关节空间进行自适应插补，可以有效排除因传感器力信号突变或者操作者导引力太大而导致的机器人安全运行隐患。

附图说明

图1是操作对象与机器人末端运动约束示意图；

图2是导引轨迹插补示意图。

图3是基于操作对象的双机器人运动控制仿真系统。

图4是操作对象在笛卡尔空间的平移/旋转运动示意图。

图5是操作对象变比例旋转仿真图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

一种双机器人导引轨迹多空间自适应插补方法，包括以下步骤：

步骤1：根据操作对象与机器人末端的运动学约束关系，计算对象导引轨迹对应的各机器人末端轨迹。

在双机器人系统导引控制过程中，操作对象与机器人末端之间存在确定的运动学约束关系。根据该约束关系，可以求解出对象导引轨迹对应的机器人末端轨迹。因此，对象与机器人末端的运动学约束分析是导引轨迹插补规划的重要前提，下面将从位置约束、速度约束、加速度约束3个方面进行分析。

1)位置约束

由对象位置解算机器人末端位姿矩阵是双机器人协调运动控制的基础，因此对象与双机器人末端的位置约束即为系统闭环运动学的约束：

^R1T_c＝^R1T_e1·^e1T_c1·^c1T_c＝^R1T_R2·^R2T_e2·^e2T_c2·^c2T_c

式中，^c1T_c、^c2T_c、^e1T_c1、^e2T_c2是由对象抓取方式确定的常数矩阵，^R1T_R2为双机器人基坐标系标定矩阵。

2)速度约束

当双机器人末端与对象采用刚性抓取方式时，如图1所示，对象与双机器人末端的广义速度关系可直接表示为：

式中：v_c、w_c表示对象运动的线速度与角速度；v_ei、w_ei表示机器人i末端的线速度与角速度；r_ei＝[r_eix r_eiy r_eiz]^T表示第i个机器人末端在对象坐标系中的位置矢量；

根据上式，当获得对象的运动速度时可直接解耦计算出各机器人末端的速度关系。

式中，

其中E₃表示3阶单位矩阵，O₃表示3阶零矩阵；

3)加速度约束

对速度约束方程进行微分求导，可得对象与机器人末端的加速度约束关系为：

将加速度约束方程用矩阵形式表示即为：

步骤2：监测机器人的末端速度和加速度、关节速度和加速度，若机器人的运动参数均满足安全限制条件，则直接下发执行，否则，根据超限情况对导引轨迹进行自适应插补后下发执行。

步骤3：对导引轨迹进行笛卡尔空间的自适应插补。

在面向对象的运动导引控制过程中，为保证机器人在笛卡尔空间的运动满足用户设定的安全值，实时对机器人末端速度和加速度信息进行监控。首先将对象的位置增量作为初始离散导引轨迹Δx_initial(n)。然后根据对象与机器人之间的运动学约束关系解算机器人末端速度和加速度信息，如果机器人末端运动信息满足安全限制条件则直接下发运行，否则将根据超限情况对导引轨迹进行自适应插补运算。

由单机器人运动学和对象与机器人之间的运动学约束关系可知，对象的导引轨与机器人末端/机器人关节轨迹是一种非线性时变关系。以非线性问题分段线性化的思想分析，机器人单位控制周期内导引轨迹与机器人末端轨迹可近似呈现为线性关系，机器人末端轨迹与关节轨迹也可近似为线性关系。因此根据机器人末端和机器人关节的超限比例线性插补初始的导引轨迹时，只会改变导引轨迹的插补密度，并不会改变原有的轨迹方向，如图2所示。在图2中，由于初始导引轨迹的位置增量Δx(3)过大，双空间中产生了运动超限情况。根据双空间的超限比例R线性插补Δx(3)后，导引轨迹同时满足了双空间的运动限制。

在控制过程中，单位时间的位置增量变化就是对象的速度，Δt为系统控制周期，对象速度和加速度表示为：

由对象与机器人末端的速度约束方程可知各机器人末端的速度和加速度[v_e1w_e1]^T、[v_e2 w_e2]^T、

同时，设定两台机器人在笛卡尔空间中的最大限制速度为v_{max_e1}、v_{max_e2}；最大限制角速度为w_{max_e1}、w_{max_e2}；最大限制线加速度为

最大限制角加速度为

操作对象在笛卡尔空间的运动分为移动和旋转。操作对象的移动只对机器人末端引入移动线速度，而对象的旋转不仅引入机器人末端的角速度，还会引入机器人末端线速度。为避免插补过程中机器人末端线速度的混淆，先对象旋转运动进行判断和插补，然后再对移动运动进行判别和插补。

(1)操作对象旋转导引轨迹插补。

旋转导引轨迹的超限分为角速度过大和角加速度过大两种超限类型，分别对这两种超限方式进行插补，若两种类型同时发生，则综合选取最安全的插补值作为控制输出量。

1)当对象旋转速度过大导致机器人末端角速度超限时：

首先计算各机器人末端角速度超限比例R_{velo_ei}确定各机器人的超限程度，在导引轨迹插补的同时需要保证双机轨迹的协同，因此各机器人都按照最大超限比例R_{velo_END}进行降速插补。

R_{velo_ei}＝ABS(w_ei)/w_{max_ei},i∈[1,2]

R_{velo_END}＝max(R_{velo_e1},R_{velo_e2}),R_{velo_END}＞1

将R_{velo_END}作为导引轨迹插补的调节参数，以机器人末端角速度约束条件插补对象离散导引轨迹的旋转角度，表示为：

Δθ_{vcal_END}(n)＝Δθ_initial(n)/R_{velo_END}

2)当对象旋转加速度过大导致机器人末端角加速度超限时：

首先计算各机器人末端的角加速度超限比例R_{accel_ei}，同理为保证多机轨迹的正确，选取加速度超限比例的最大值R_{accel_END}来插补导引轨迹。

R_{accel_END}＝max(R_{accel_e1},R_{accel_e2}),R_{accel_END}＞1

将R_{accel_END}作为轨迹插补调节参数，以机器人末端角加速度约束条件来重新插补对象导引轨迹的旋转角度为：

3)如果同时超限时则选取最小值，保证插补轨迹的安全性，最终以Δθ_{cal_END}作为本周期的离散轨迹点输出值。

(2)操作对象移动导引轨迹插补。

在完成对象旋转导引轨迹插补后，重新计算刷新机器人末端的速度v_ei，w_ei，以及对象导引轨迹离散点Δx_initial(n)。然后继续进行下列关于对象移动时的约束判断。同理，移动导引轨迹也分为了速度超限和加速度超限两种超限方式。

1)当对象移动速度太大导致机器人末端速度超限时：

首先计算各机器人末端的速度超限比例R′_{velo_ei}，同理，两台机器人按照速度最大超限比例R′_{velo_END}进行降速插补。

R′_{velo_ei}＝ABS(v_ei)/v_{max_ei} i∈[1,2]

R′_{velo_END}＝max(R′_{velo_e1},R′_{velo_e2}),R′_{velo_END}>1

将R′_{velo_END}作为轨迹插补调节参数，以机器人末端速度约束条件来再次插补对象导引轨迹的离散轨迹点(包括位置与姿态)为：

Δx_{vcal_END}(n)＝Δx_initial(n)/R′_{velo_END}

2)当对象移动加速度太大而导致机器人末端加速度超限时：

首先计算各机器人末端的加速度超限比例R′_{accel_ei}，并选取加速度超限比例的最大值R′_{accel_END}来插补导引轨迹。

R′_{accel_END}＝max(R′_{accel_e1},R′_{accel_e2}),R′_{accel_END}>1

将R′_{accel_END}作为轨迹插补调节参数，以机器人末端加速度约束条件来重新插补对象导引轨迹的离散轨迹点(包括位置与姿态)为：

3)当机器人同时超限时则选取最小值，保证插补轨迹的安全性，最终以Δx_{cal_END}作为本周期的离散轨迹点输出量。

步骤4：根据各关节的运动约束条件，对导引轨迹进行关节空间的自适应插补：

机器人关节空间与笛卡尔空间的运动学关系是非线性耦合的，所以机器人末端满足笛卡尔空间运动约束条件时，机器人的关节运动仍可能超出关节轴的安全限制。本文在笛卡尔空间完成导引轨迹插补后，根据各关节的运动约束再次对导引轨迹进行关节空间的判断插补，进一步保证导引轨迹的平滑和安全问题。同时，设定各机器人关节的最大限制速度为

最大限制加速度为

根据机器人运动学理论知识可知，机器人末端速度与关节速度之间可以通过机器人雅克比矩阵J(q_i)进行表示,两机器人的关节速度

可以表示为：

同样，采用非线性问题分段线性化的思想，以短时间的关节超限比例来调节机器人末端笛卡尔空间轨迹只会增加导引轨迹的插补密度从而降低运行速度，而不会发生原轨迹的偏离。而机器人关节空间的超限则分为了关节角速度超限和关节角加速度超限两种超限情况。

1)当机器人关节速度太大超过限制时：

首先计算机器人各关节的关节角速度超限比例，为保证单机器人运动学的正确性，选择超限最严重的关节作为该机器人的关节角速度超限比例R_{velo_Ji}。在导引轨迹插补的同时需保证双机器人的协调关系，因此两机器人按照最大的关节速度超限比例R_{velo_JOINT}对导引轨迹进行插补降速。

R_{velo_JOINT}＝max(R_{velo_J1},R_{velo_J2})

将R_{velo_JOINT}作为轨迹插补调节参数，采用非线性问题分段线性化的思想，以单位时间的关节速度超限比例再次对笛卡尔空间插补后的离散导引轨迹点Δx_{vcal_JOINT}进行插补，即：

Δx_{vcal_JOINT}(n)＝Δx_{cal_END}(n)/R_{velo_JOINT}

2)当机器人关节加速度太大超过限制时：

首先计算机器人各关节的关节角加速度超限比例，同理，选择超限最严重的关节作为该机器人的关节角加速度超限比例R_{accel_Ji}，同时两机器人也按最大加速度超限比例R_{accel_JOINT}进行插补。

R_{accel_JOINT}＝max(R_{accel_J1},R_{accel_J2})

将R_{accel_JOINT}作为轨迹插补调节参数，同样以非线性问题分段线性化思想以机器人关节角加速度超限比例对笛卡尔空间插补后的离散导引轨迹点Δx_{cal_END}进行插补，即：

3)当机器人的关节两种同时超限时则选取最小值，保证插补轨迹的安全性，最终以Δx作为本周期的离散轨迹点输出量，从而实现笛卡尔空间和关节空间的多空间轨迹自适应插补规划。

针对本发明的方法进行算法仿真验证：

为验证双机器人运动控制算法的正确性，在Matlab-Simulink仿真环境中建立仿真模型，如图3所示，仿真系统主要包括双机器人正运动学模块、双机器人闭环运动学控制算法模块、位置/姿态调整模块、双机器人逆运动学及显示模块等4部分。其中，正运动学模块设置双机器人的初始位姿角度并计算末端位姿矩阵；闭环运动学控制算法模块即为双机器人闭环运动学模型，模块A表示双机器人的基坐标系标定矩阵，而模块B则表示对象坐标系位置调节比例K；位置/姿态调整模块为系统输入空间6自由度的运动指令；运动学逆解及显示模块则绘制双机器人末端运行轨迹及各关节的驱动角度。

为保证系统仿真与实际控制情况尽可能吻合，将Simulink设置为固定步长仿真，单步仿真时间为0.002s。在位置/姿态调节模块中分别给定对象在X、Y、Z三个方向的位移和旋转输入，模拟操作者通过传感器所施加的引导轨迹，双机器人末端轨迹在统一的世界坐标系中的仿真结果如图4所示。由(a)X方向平移、(b)Y方向平移、(c)Z方向平移和(d)X方向旋转、(e)Y方向旋转、(f)Z方向旋转分别看出双机器人基于闭环运动学模型顺利完成了对象在空间中的移动和旋转操作。

通过调节对象旋转中心位置的比例K使对象绕世界坐标系中的Z轴旋转，机器人末端轨迹的仿真结果如图5所示，可见K从0～1之间不同的比例都反应了各末端所旋转的半径比。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进应视为本发明的保护范围。

Claims (7)

1.一种双机器人导引轨迹多空间自适应插补方法，其特征在于，包括以下步骤：

步骤1：根据操作对象与机器人末端的运动学约束关系，计算对象导引轨迹对应的各机器人末端轨迹，包括机器人的位置约束、速度约束和加速度约束；

步骤2：监测机器人的末端速度和加速度、关节速度和加速度，若机器人的运动参数均满足安全限制条件，则直接下发执行，否则，根据超限情况对导引轨迹进行自适应插补后下发执行；

步骤3：对导引轨迹进行笛卡尔空间的自适应插补：

步骤3-1：将操作对象的位置增量作为初始离散导引轨迹Δx_initial(n)，对象的速度和加速度为：

Δt为系统控制周期，各机器人末端的速度和加速度为[v_e1 w_e1]^T、[v_e2 w_e2]^T、

设定两台机器人在笛卡尔空间中的最大限制速度为v_{max_e1}、v_{max_e2}，最大限制角速度为w_{max_e1}、w_{max_e2}，最大限制线加速度为

最大限制角加速度为

步骤3-2：根据操作对象的角速度过大和/或角加速度过大，对其旋转导引轨迹进行插补；

步骤3-3：在操作对象的旋转导引轨迹插补后，重新计算机器人末端的速度v_ei、加速度w_ei以及对象导引轨迹离散点Δx_initial(n)，再根据操作对象的移动速度过大和/或移动加速度过大，对其移动导引轨迹进行插补；

步骤4：根据各关节的运动约束条件，对导引轨迹进行关节空间的自适应插补：

步骤4-1：设定各机器人在关节空间中的关节最大限制速度为

最大限制加速度为

机器人末端速度与关节速度之间通过机器人雅克比矩阵J(q_i)表示,两机器人的关节速度

表示为：

步骤4-2：根据机器人关节速度过大和/或关节加速度过大，对机器人的关节导引轨迹进行插补。

2.根据权利要求1所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤1中的位置约束具体为：

根据操作对象位置计算机器人的末端位姿矩阵：

^R1T_c＝^R1T_e1·^e1T_c1·^c1T_c＝^R1T_R2·^R2T_e2·^e2T_c2·^c2T_c

^R1T_c为操作对象在世界坐标系{R1}的齐次变化矩阵，^R1T_e1、^R2T_e2为双机器人在各自坐标系下的位姿齐次矩阵，^c1T_c、^c2T_c、^e1T_c1、^e2T_c2是由对象抓取方式确定的常数矩阵，^R1T_R2为双机器人基坐标系标定矩阵。

3.根据权利要求1所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤1中的速度约束具体为：

当双机器人末端与操作对象采用刚性抓取方式时，操作对象与双机器人末端的速度关系为：

v_c、w_c表示对象运动的线速度与角速度，v_ei、w_ei表示第i个机器人末端的线速度与角速度，r_ei＝[r_eix r_eiy r_eiz]^T表示第i个机器人末端在对象坐标系中的位置矢量；

则，机器人末端的速度约束方程为：

其中，

E₃表示3阶单位矩阵，O₃表示3阶零矩阵。

4.根据权利要求3所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤1中的加速度约束具体为：

操作对象与机器人末端的加速度约束关系为：

则，机器人末端的加速度约束方程为：

5.根据权利要求1所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤3-2具体包括以下步骤：

1)当操作对象角速度过大而导致机器人末端角速度超限时，计算各机器人末端角速度超限比例R_{velo_ei}，选取最大超限比例R_{velo_END}进行降速插补，得到：

R_{velo_ei}＝ABS(w_ei)/w_{max_ei},i∈[1,2]

R_{velo_END}＝max(R_{velo_e1},R_{velo_e2}),R_{velo_END}＞1

R_{velo_END}为导引轨迹插补的调节参数，以机器人末端角速度约束条件来插补操作对象离散导引轨迹的旋转角度为：

Δθ_{vcal_END}(n)＝Δθ_initial(n)/R_{velo_END}

2)当操作对象角加速度过大而导致机器人末端角加速度超限时，计算各机器人末端角加速度超限比例R_{accel_ei}，选取最大超限比例值R_{accel_END}进行插补，得到：

R_{accel_END}＝max(R_{accel_e1},R_{accel_e2}),R_{accel_END}＞1

R_{accel_END}为引导轨迹插补的调节参数，以机器人末端角加速度约束条件来插补操作对象导引轨迹的旋转角度为：

3)当操作对象同时存在角速度过大且角加速度过大而导致机器人超限时，则选取两者中的最小值作为插补操作对象导引轨迹的旋转角度；

即，Δθ_{cal_END}作为本周期的离散轨迹点输出值表示为：

6.根据权利要求1所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤3-3具体包括以下步骤：

1)当对象移动速度过大而导致机器人末端速度超限时，计算各机器人末端的速度超限比例R′_{velo_ei}，选取最大超限比例R′_{velo_END}进行降速插补，得到：

R′_{velo_ei}＝ABS(v_ei)/v_{max_ei} i∈[1,2]

R′_{velo_END}＝max(R′_{velo_e1},R′_{velo_e2}),R′_{velo_END}>1

R′_{velo_END}为导引轨迹插补的调节参数，以机器人末端速度约束条件来插补对象导引轨迹的离散轨迹点为：

Δx_{vcal_END}(n)＝Δx_initial(n)/R′_{velo_END}

所述离散轨迹点包括位置与姿态；

2)当对象移动加速度过大而导致机器人末端加速度超限时，计算各机器人末端的加速度超限比例R′_{accel_ei}，选取最大超限比例R′_{accel_END}进行插补，得到：

R′_{accel_END}＝max(R′_{accel_e1},R′_{accel_e2}),R′_{accel_END}>1

R′_{accel_END}为导引轨迹插补的调节参数，以机器人末端加速度约束条件来插补对象导引轨迹的离散轨迹点为：

所述离散轨迹点包括位置与姿态；

3)当操作对象同时存在移动速度过大和移动加速度过大而导致机器人超限时，则选取两者中的最小值作为插补操作对象导引轨迹的离散轨迹点；

即，Δx_{cal_END}为本周期的离散轨迹点输出量表示为：

7.根据权利要求6所述的双机器人导引轨迹多空间自适应插补方法，其特征在于，步骤4-2具体包括以下步骤：

1)当机器人关节速度过大而导致超限时，计算机器人各关节的关节角速度超限比例，选取最大超限比例R_{velo_Ji}进行插补降速，得到：

R_{velo_JOINT}＝max(R_{velo_J1},R_{velo_J2})

R_{velo_JOINT}为导引轨迹插补的调节参数，以单位时间的关节角速度超限比例对笛卡尔空间插补后的离散导引轨迹点Δx_{vcal_JOINT}进行插补，得到：

Δx_{vcal_JOINT}(n)＝Δx_{cal_END}(n)/R_{velo_JOINT}

2)当机器人关节加速度过大而导致超限时，计算机器人各关节的关节角加速度超限比例，选择最大超限比例R_{accel_Ji}进行插补，得到：

R_{accel_JOINT}＝max(R_{accel_J1},R_{accel_J2})

R_{accel_JOINT}为导引轨迹插补的调节参数，以机器人关节角加速度超限比例对笛卡尔空间插补后的离散导引轨迹点Δx_{cal_END}进行插补，得到：

3)当机器人的关节同时存在关节速度过大和关节加速度过大而导致超限时，则选取两者中的较小值作为插补；

即，Δx作为本周期的离散轨迹点输出量表示为：

Images