双机器人不同约束下的协调运动轨迹规划方法的研究＊

苏赫朋 苗鸿宾② 李梦虔 申光鹏 余 浪

（①中北大学机械工程学院，山西 太原 030051；②山西省深孔加工工程技术研究中心，山西 太原 030199）

在工业自动化程度不断提高的今天，在复杂的生产环境下，单机器人难以满足现实的需求，如大型搬运、孔轴部件的装配、复杂表面的焊接等。与单机器人相比，双机器人不仅要考虑工作空间和外部约束，还要考虑二者的运动协调关系，它们在进行配合作业时就建立了约束关系。

Saridis G N[1]提出了面向复杂环境的高层决策模式，这种模式以智能与精度为核心，将系统划分为3 个层次，最低控制是低智能、高精度的，最高控制是高智能、低精度的。Kasagami F 等[2]提出了双机协同焊接的控制方法，两个机器人分别夹持工件和进行焊接，对夹持工件的机器人示教，焊接机器人根据运动学约束自动寻找并跟踪焊接位置。该方法避免了多次示教引起误差，缩短了准备时间。李洋等[3]构建了基于RRT 的双机协作平台，并对其进行了轨迹规划。王雷[4]通过集中控制系统，使两台机器人在时间和空间上保持一致，并提出了两种解决方案：点对点规划和路径规划。孟石等[5]在Ke-Scope 平台上，对两个机器人的叠加和耦合运动模式进行了分析，并对两个机器人的协同工作进行了程序设计和仿真。

本文以JLRB8-600 型机器人为研究对象，以协调搬运和协调写字为例，分别研究紧约束和松约束运动，分析其运动学关系及空间约束关系，使用Matlab 进行实验验证。

1 机器人运动学建模

欧德吉JLRB8-600 型六自由度串联机器人具有结构精巧、占地少以及重量轻的特点，其主要技术指标为：臂展长不大于1 000 mm，臂重42 kg，最大荷重8 kg，重复定位精度0.05 mm。机器人及其运动范围如图1 所示。

图1 机器人及其运动范围

机器人的运动学模型需满足有完备性、参数极少化和连续性的条件[6]。所以，D-H 参数法被用于运动学模型的建立，只需要 θi（关节转角）、di（连杆偏移）、ai-1（连杆长度）、 αi-1（连杆扭转）4 个运动参量就能描述机械臂关节，其D-H 参数表见表1。

表1 D-H 参数表

2 双机器人紧约束下的协调运动轨迹规划

两个机械臂末端执行器在工作过程中位姿保持不变的情况称为紧约束。两个开链机器人通过紧约束形成了一个闭环运动链[7]。由底座、关节、末端、末端执行器和工件组成。在紧约束中，两个机器人不同坐标系之间的关系如图2 所示，对机器人进行编号i，定义世界坐标系为主机器人i1的基坐标系，{Ri}为机器人i的基坐标系，{Ei}为机器人i的末端坐标系， {Ti}为机器人i的工具坐标系， {C}为工件的质心坐标系，为末端坐标系到基坐标系的变换矩阵，为末端坐标系到工具坐标系的变换矩阵，为工具坐标系到工件质心坐标系的变换矩阵。

图2 紧约束中各坐标系之间的关系

联合搬运的紧约束运动形成了闭环运动链：

分别右乘对应矩阵得到：

机器人i1的基坐标系到末端坐标系的变换矩阵可由等号右边求得，逆运动学分析可获得各关节的角位移。已知和，通过示教得出工件在机器人i1基坐标下的运动轨迹为

式（3）中都是常数矩阵，结合式（1）有：

左乘对应矩阵得：

机器人i1与i2的是随时间变化的函数矩阵，式（5）中可根据解得，这样就可以实现机器人i1与i2在时间上的同步。紧约束运动学原理如图3 所示。

图3 紧约束运动学原理

3 双机器人松约束下的协调运动轨迹规划

松约束运动中机器人末端的相对位姿因任务的不同而发生变化[8]。设世界坐标系与机器人i1基坐标系重合，即{R}={R1} ， {U}代表工件坐标系，将其定义在被i2末端执行器夹持固定的工件上，是工具坐标系到工件坐标系的变换矩阵。松约束中各坐标系关系如图4 所示。

图4 松约束中各坐标系之间的关系

双机器人松约束运动中，仍有一条闭环运动链，这条运动链从世界坐标系，即主机器人的基本坐标系出发，再将两个机器人的身体分割成两个部分，最后汇合在一起，形成一个闭环[9]。因此，可以推导出主从机之间的关系：

式（6）和式（7）是从基础坐标到工具坐标的主、从机器人的运动链，可以得到以下关系：

4 仿真实验分析

4.1 紧约束下的Matlab 双机器人轨迹验证

假定工件C 的尺寸为Cx×Cy×Cz（单位为mm），质心与工件坐标系 {C}原点重合，位于中心位置，Z轴沿机器人i1基坐标系X轴方向，X轴竖直向上，Y轴由右手法则确定，如图5 所示。

图5 工件尺寸和质心坐标系位置

图6 显示了在两个机械手的紧约束中不同部分的坐标系信息。将任务目标设置为：机器人在两边搬运工件C，定义起点A，终点B，总长L，i1在左、i2在右，二者沿世界坐标系轴x相对立，搬运过程中，二者相对速度为零。得到工件C与世界坐标系的变换矩阵为

图6 紧约束下各坐标系相对位置

在紧约束下，机器人i1与i2夹持工具相对于机器人末端的变换矩阵为

在搬运过程中，工件相对机器人i1与i2工具坐标系变换矩阵为

设置从机器人基坐标系相对于世界坐标系的位置为

根据式（2）可以得出i1末端相对于基坐标系的变换矩阵：

由于两机器人末端的位姿不发生变化，由式（3）得：

根据（5）可得机器人i2末端到基坐标系的变换矩阵：

由式（14）和式（16），可得机器人i1与i2末端相对基坐标系的变换矩阵，再通过逆运动学求出各关节的角位移，从而实现机械臂的动作。

在 Matlab 的基础上，将上述双机器人协调搬运参数设定为：A=[400,250,200]、B=[400,-250,200]，工件C轮廓尺寸100 mm×100 mm×200 mm，工件质心与末端坐标系原点保持水平，有f2=f3=100 mm，起点为A，终点为B，i1与i2的工具有效长度为f1=f4=50 mm，设置f=1 000 mm。上面的数值被替换到式（14）～式（16）中仿真。

Matlab 因其良好的运算性能而被广泛应用[10]。利用 Matlab 中的Robotic Toolbox 对两个机器人在动画层面上的动作进行了模拟和分析。该工具箱是澳洲学者Peter Corke 发明的，用来研究和模拟关节型和移动型的机器人，里面包含了一些关于机器人的基础算法的函数，有一些比较成熟的代码，可以很直观地显示出编写的程序。

图7 所示为用 Matlab 软件实现的两台机械臂协同操作的模拟原理图。

图7 双机器人紧约束运动仿真

红线表示工件C 的轨迹，基座在世界坐标系沿轴线X相对放置，由此可以看出，两台机器人的末端X方向是相同的，轴线Y与轴线Z方向相反。

图8 所示为主机器人的轨迹，此时动作已经结束，末端执行器的轨迹为直线，运动过程中末端位姿不发生变化，移动距离为500 mm 。图9 所示为主机器人各关节的角位移。

图8 末端运动轨迹

图9 主机器人各关节角位移变化

对于机器人i2，基坐标原点位于[1000,0,0]，X轴与世界坐标系X轴方向相反，运动轨迹总长500 mm，i2的末端轨迹如图10 所示，i2的各关节的角位移如图11 所示。

图10 末端运动轨迹

图11 从机器人各关节角位移变化

仿真结果与预定参数一致，各关节运动相对平缓，无明显拐点，机器人的动作平稳顺畅，验证了该方法的正确性和有效性。

4.2 松约束下的Matlab 双机器人轨迹验证

在协同写字过程中，使机器人i1夹持书写板，机器人i2夹持书写笔；写字板变速移动，写字笔书写“123”字体，并随着写字板移动，如图12 所示；定义工件坐标系 {U}的原点在写字板中心，i2工具坐标系{T2}原点在写字笔的顶端，图13 和图14 分别为各坐标系相对位置关系，仍按图6 设置i1与i2的基坐标系。

图12 书写内容

图13 主机器人末端、工具及工件坐标系之间的关系

图14 从机器人末端、工具及工件坐标系之间的关系

i1的末端、工具和工件坐标系如图13 所示。

i2的末端、工具和工件坐标系如图14 所示。

设i1基坐标系与世界坐标系重合，在主基坐标系中，起点为A[Ax,Ay,Az]，终点为B[Bx,By,Bz]，总长l1。i1的运动路径确定后，即可推导出i2的运动路径及相关参数。

写字的过程中，规定从左到右，从上到下，一笔写完一个字，写完后抬笔移到下一个位置。每个字的起始位置为左上角，写字笔的理想路径如图15 所示。设抬笔高度为d，计算得写字总长度l2为

图15 写字笔写字顺序

以式（8）和式（9）为基础，得到i1与i2末端相对基坐标的关系等式。从图4 中可以看出，在松约束下中，i1工具坐标系、基坐标系分别与工件坐标系之间的变换矩阵：

∆ai(i=x,y,z)是写字板移动过程中与写字笔所需的距离，该数值有正负之分。

i1末端与工具坐标系的关系矩阵：

由式（8）、式（18）、式（19）和式（20）可得i1末端相对基坐标系的变换矩阵，逆运动学求出各关节角位移，使机器人到达相应位置：

i2末端与写字笔工具坐标系之间的关系：

写字笔工具坐标系与写字板工件坐标系的关系为

∆l为写字笔在轴上的移动距离，[pi,qi,wi]是工件坐标系相对工具坐标在某一时刻的位置，与书写内容和顺序有关。由图15 可以看出，完成这一任务共需18 笔，而“3”中只有1 次重复。因此可以得出笔的书写过程是由18 个点来控制的结论，也就是说i=2,3,4,···,i≥18，i=1时是起始位置，是i2工具坐标系中书写开始点。

假设世界坐标系与i1基坐标系重合，那么i2与世界坐标系的关系就可以确定：

据式（8）、式（19）、式（22）、式（23）、式（24），i2末端相对基坐标系的变换矩阵为

假设各参数为

在Matlab Robotic Toolbox 中建立机器人模型进行仿真，结果如图16 所示。

图16 松约束运动仿真结果

图16 所示为两个机器人的初始位置，i1在左侧，基坐标系与世界坐标系一致，i2在右侧，基坐标系在世界坐标系中[1 000,0,0]。写字板的轨迹如图17 所示。

图17 主机器人的运动轨迹

写字板作直线运动，从[500,500,600] 到[500,-500,600]，运动过程中，i1各关节的角位移变化如图18 所示。

图18 主机器人各关节角位移变化

从图中可以看到，主机器人写字板是直线运动，各关节角的变化较小。i2工具坐标系原点在笔尖，仿真结果如图19 所示。当i1运动时，写字笔会写下任务要求的文字，运动轨迹如图20 所示。粗线是书写轨迹，细线是起笔、落笔、移动的轨迹，写字笔沿着世界坐标系y轴正向移动。“2”和“3”中缺少的“—”，可由写字板沿着y轴正方向移动写出对。写出的标准字迹如图21 所示，能够看到，书写过程非常顺畅，每一次抬笔落笔都能精确地移动到相应位置，i2的各个关节的角位移如图22 所示。

图19 从机器人的运动轨迹

图20 世界坐标系下笔的轨迹

图21 写字板上的笔记

图22 从机器人各关节角位移变化

由图18 可知，i1各关节角位移光滑平坦，这是由于i1的运动只有y轴上的平移，不需要关节角发生快速改变。由图22 可知，i2各关节角的变化比较复杂，这是因为为了书写所需的笔迹，机器人需要在空间中频繁移动，但曲线上整体没有突出拐点，关节的变化相对平稳。两机器人的运动都是可以实现的，在运动过程中每个关节的变形都是平缓的。通过以上实验，证明了该方法的正确性及有效性。

5 结语

双机器人之间的协同运动，最重要的就是它们的轨迹是否符合任务要求。本文的研究内容分为紧约束搬运任务和松约束书写任务，对运动轨迹规划的理论方法进行了分析设计，用 Matlab 测试了两种方法的正确性及稳定性。实验证明，两台机械臂均能很好地完成提前设定的任务，并且各关节角位移变化平缓、没有突兀的拐点，这对于研究两台机械臂的协同控制有一定的理论价值。