基于ROS的双机械臂协同感知抓取系统设计与实现

Design and Realization of ROS-Based Dual Robotic Arm Cooperative Sensing and Grasping System

摘要: 本文旨在设计和实现基于ROS的双机械臂协同感知抓取系统，探讨双臂协同控制策略和视觉感知技术在机械臂任务中的应用。通过机械臂运动学分析和双臂协同系统设计，结合视觉点云技术实现双臂协同抓取，利用ROS平台构建双机械臂系统实验平台，验证协同控制策略的有效性。主要研究内容包括机械臂运动学正解和逆运动学模型分析、视觉点云识别在双臂协同任务中的应用、双臂协同控制策略的实现以及协同抓取实验结果与分析。通过本研究，成功设计并实现了基于ROS的双机械臂协同感知抓取系统，验证了双臂协同控制策略的有效性。

Abstract: This article aims to design and realize a dual robotic arm cooperative perception grasping system based on ROS, and explore the application of dual-arm cooperative control strategy and visual perception technology in robotic arm tasks. By analyzing the kinematics of the robotic arm and designing the dual-arm cooperative system, combining the visual point cloud technology to realize the dual-arm cooperative grasping, and constructing an experimental platform of the dual-me- chanical-arm system by using the ROS platform to verify the effectiveness of the cooperative control strategy. The main research content includes the analysis of positive and inverse kinematic models of robotic arm kinematics, the application of visual point cloud recognition in the dual-arm cooperative task, the realization of dual-arm cooperative control strategy, and cooperative grasping experimental results and analysis. Through this study, a ROS-based dual robotic arm cooperative perception grasping system is successfully designed and realized, and the effectiveness of the dual-arm cooperative control strategy is verified.

1. 引言

随着机器人技术的不断发展和普及，双机械臂协同感知抓取系统作为一种先进的自动化解决方案，正逐渐成为工业生产和服务领域的热门研究方向[1]。该系统通过结合双臂协同控制、视觉感知和抓取规划技术，实现机器人在复杂环境中高效感知目标物体并完成抓取任务的能力。在这一背景下，基于机器人操作系统(ROS)的双机械臂协同感知抓取系统设计与实现变得尤为重要。ROS作为一个灵活、强大的机器人软件平台[2]，为机器人系统的开发和集成提供了便利。通过充分利用ROS的功能和工具，设计和实现双机械臂协同感知抓取系统，将为工业生产、物流领域等提供更高效、智能的解决方案。

本文旨在探讨基于ROS的双机械臂协同感知抓取系统的设计与实现。首先，将介绍双臂协同控制策略，包括运动规划、运动学控制等关键技术。其次，将重点讨论视觉感知在双臂协同任务中的应用，包括点云识别、目标定位等方面的技术。最后，将详细描述基于ROS的双机械臂系统的设计与实现过程，包括系统架构、模块设计、算法实现等内容。

通过本文的研究，旨在为机器人领域的研究人员和工程师提供有关双机械臂协同感知抓取系统设计与实现的深入理解和实践经验。相信，基于ROS的双机械臂协同感知抓取系统将为未来智能制造和服务机器人领域的发展带来新的机遇和挑战，推动机器人技术迈向更加智能、灵活和高效的方向。

2. 机械臂运动学分析

2.1. 机械臂运动学正解

DH参数控制系统是一种用四个参数表示二个主要关节或连杆机构间位移方向关联的机器臂模型和坐标系选择系统[3]。因为控制原点定位与X轴线的相对方向，而人为降低了二个自由度，所以只需要四个参数就可以定义为一种有六自由度的极坐标系。DH的四个参数及描述如表1所示。

用DH计算法则为机器人进行建模，如图1所为单机械臂DH示意图。DH参数描述机械臂各关节之间的几何关系，通过这些参数建立坐标变换矩阵来描述机械臂的位置和姿态。对于四个参的DH机械臂模型，对其坐标系进行描述，定义各个关节的坐标系，使用右手坐标系，其中$z_{i-1}$ 轴指向关节i，$x_{i-1}$ 轴垂直$z_{i-1}$ 轴，构成右手坐标系。给各个关节确定一个本地的基础坐标系，对各个关节来说只需要确定一下Z轴和X轴。给定Z轴，如果关节是公转地，Z轴就是按右手规则自转的方位。绕Z轴的公转角为关节的自变量；确定关节为滑关节，则Z轴是沿垂直运行的方向。沿Z轴的连杆机构长度，d为关节变量。对于X轴，当二个滑膜关节并不相互平行或交叉时，则Z轴都是等高，且都有一个相距较短的公垂线，它们都正交于任何二个等高。在公垂线方位上，确定了本地基准坐标系的X轴。

Table 1. Four parameters and description of DH

表1. DH的四个参数及描述表

参数名称	参数表示	描述
连杆直径$l_i$	link length	沿着$x_{i-1}$ 轴的位移，即二个主要关节的轴间的公共法线方向间距。
连杆扭转${\alpha }_i$	link twist	围绕$x_{i-1}$ 轴的旋转角度，即指一个关节的主轴相对于另一关节的主轴， 绕它的公共法线所转动的夹角。
连杆偏移$d_i$	link offset	沿着$z_{i-1}$ 轴的位移，即上一个关节与下一个关节的公用法线方向， 以及其与上一关节的公用法线方向与这个滑膜关节轴线的长度。
关节转动${\theta }_i$	joint angle	围绕$z_{i-1}$ 轴的旋转角度，即上一个关节与下一个关节的共同法线向， 以及其与上一关节的共同法线向绕这个关节轴的转动。

Figure 1. Schematic diagram of single robot arm DH

图1. 单机械臂DH示意图

根据DH参数描述和示意图，建立各个关节之间的坐标变换矩阵，根据DH参数，建立相邻关节之间的坐标变换矩阵$A_i$ ，表示为公式(1)。

$A_i=Rot\left(z_{i-1},{\theta }_i\right)\cdot Tran\left(z_{i-1},d_i\right)\cdot Tran\left(x_{i-1},l_i\right)\cdot Rot\left(x_{i-1},{\alpha }_i\right)$ (1)

其中，$Rot$ 表示绕轴旋转，$Tran$ 表示沿轴平移。

计算末端执行器的坐标，通过将各个关节间的坐标变换矩阵相乘，得到末端执行器相对于基坐标系的坐标变换矩$T_{end}$ ，为公式(2)所示。

$T_{end}=\left(\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i& 0& l_{i-1}\\ \sin {\theta }_i\cos {\theta }_{i-1}& \cos {\theta }_i\cos {\alpha }_{i-1}& -\sin {\alpha }_{i-1}& -\sin {\alpha }_{i-1}{d}_i\\ \sin {\theta }_i\sin {\alpha }_{i-1}& \cos {\theta }_i\sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& \cos {\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right)$ (2)

2.2. 机械臂逆运动学模型

通过机械臂运动学逆解确定机臂末端姿态，计算机臂的关节值，先对机械臂模型进行简化，机械臂运动学逆解简化图如图2所示。

Figure 2. Schematic diagram of the inverse kinematics solution of a single robotic arm

图2. 单机械臂运动学逆解示意图

图2中，通过把机器臂的模型简化，去掉底座云台和执行器部分只留下机器臂的主体，为三关节、无滑动结构。由图2知，机械臂的末端端点P的方向位置$\left(x,y\right)$ ，由三部分所构成$\left(x_1+x_2+x_3,y_1+y_2+y_3\right)$ 。${\theta }_1$ 、${\theta }_2$ 、${\theta }_3$ 都是所需解的操控舵机装置的角度，$\alpha$ 是手爪和水平面的相对角度，由图分析可得爪子的俯视角$\alpha ={\theta }_1+{\theta }_2+{\theta }_3$ ，由此给出公式(3)。

$\{\begin{array}{l}x=l_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)+l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\\ y=l_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)+l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}$ (3)

其中，$x,y$ 为由机器视觉获得末端应到位置的坐标值，而$l_1$ 、$l_2$ 、$l_3$ 均为机械臂的机械结构固有特性，采用无滑动即为固定值，通过三元一次方程带入求解获得角度值，控制关节舵机转动。

3. 双臂协同系统设计

3.1. 使用视觉点云实现双臂协同抓取

视觉识别部分使用TFLite框架进行目标识别和定位实现，TFLite (TensorFlow Lite)是TensorFlow针对移动设备和嵌入式设备优化的轻量级解决方案[4]，用于进行机器学习模型的部署和推理。在TFLite框架中，通常不存在明确的服务器(server)和客户端(client)的模式，因为TFLite主要用于在本地设备上运行模型，而不涉及网络通信。但是TFLite提供了轻量级的解决方案，使得在资源受限的移动装卸机器人平台上能够高效地运行机器学习模型，将TFLite模型集成到服务器–客户端架构中，服务器端负责模型训练、模型更新和模型的分发，而客户端负责加载模型并执行推理任务，客户端可以通过网络请求从服务器端获取更新的模型或推理的结果。TFLite框架如图3所示。

Figure 3. TFLite framework diagram

图3. TFLite框架示意图

使用TFLite框架结合Python编程进行视觉识别，首先，加载TFLite模型，其次，读取并预处理输入的图像，其中图像为装卸机器人装卸的物品，实验时使用方形盒子，然后，将预处理后的输入数据设置到模型的输入张量中，执行推理任务，即确定装卸的物品类型及位置，最后解析出结果，选择概率最高的类别作为预测结果输出。

TFLite需要先进行初始化模型加载，所使用的模型是由官方提供的。模型初始化加载成功之后将当前摄像头捕获的图像数据进行编码，最后传入到模型求解器，获得当前图像的标签，其中概率值最大的标签即为识别结果。目标识别算法识别到需要被抓取的物体之后，识别成功的物体像素坐标以位置+四元数的形式发布。将需要抓取的物体位姿赋值给机械臂末端，以实现其姿态控制，通过运动学逆解求得各个关节角度值，调用MOVEIT中move_group传递给驱动节点执行即可实现手眼协同抓取的功能，仿真场景如下图4所示。

Figure 4. Robot arm simulation scene

图4. 机械臂仿真场景

在图4中，双机械臂协同感知抓取系统设计使用视觉点云技术，通过获取环境中物体的三维点云数据，实现双机械臂之间的协同工作，提高抓取的准确性和效率[5]。把服务器Sever端定义为机器人A，把Client端定义为机器人B，机械臂视觉协同算法如表2所示。

Table 2. Description of robot vision collaboration algorithm

表2. 机械臂视觉协同算法描述表

步骤	实现功能	描述
step1	点云数据获取	机器人A通过深度相机获取环境中物体的三维点云数据， 并将其转换为ROS中的点云消息格式；机器人B接收机器人A传输的点云数据。
step2	点云处理	机器人A对获取的点云数据进行滤波、分割、配准等操作， 提取目标物联的点云信息；机器人B接收处理后的点云信息。
step3	目标检测	机器人B利用机器学习算法对接收到的点云数据进行目标检测和识别， 确定需要抓取的目标；机器人A接收机器人B的目标信息。
step4	路径规划	机器人A根据接收到的目标物体的位置和形状信息，规划双机械臂的运动路径， 确保双臂协同抓取的顺利进行。
step5	抓取执行	机器人A根据路径规划结果，控制双机械臂执行抓取动作； 机器人B协同执行抓取动作，完成双臂协同抓取任务。

3.2. 双机械臂系统实验平台描述

ROS系统中人工智能定义形式URDF (Unified Robot Description Format)功能中包括有一组URDF的标准C++解释器，URDF文档通过XML格式定义智能机器人模式。在URDF文件描述中，机器人有连杆和铰链关节组成。ROS系统在加载解析URDF模型的同时，也会通过铰链状态发布器JSP (Joint State Publisher)将机器人各部分的TF (Transform)坐标变换关系一并发出。在双机械臂系统实现平台中，通过Gazebo仿真平台进行数据仿真模拟测试，使用TF坐标变换来描述机器人各部分组成的位姿关系。

双机械臂的关节位姿关系通过TF坐标变换来描述。在ROS中，每个机器人关节都有一个对应坐标系，通过TF来描述这些坐标系之间的相对关系。对于机械臂的控制，有两种控制实现，一种是直接采用机械臂运动学正解，设置机械臂各个关节角度值实现控制，另一种是采用机械臂运动学逆解，设定好机械臂末端的位姿来进行操控。由于系统中在机械臂进行抓取之前已经通过视觉获取目标物体的位置坐标，即采用机械臂逆解进行目标物体的抓取。

4. 双臂协同控制策略实现

4.1. 机械臂IK和FK执行精度检测

在双机械臂协同感知抓取系统中，机械臂的逆运动学(IK)和正运动学(FK)的执行精度是影响末端抓取准确度的主要指标，执行精度检测是通过ROS中的JSP节点来实现。JSP节点可以用于发布机器人各个关节的状态信息，包括关节位置、速度等。在执行精度检测时，通过JSP节点发布目标关节状态，然后利用逆运动学求解器计算出对应的末端执行器的位姿，再通过正运动学求解器将末端执行器的位姿转换回关节角度，最终比较计算得到的关节角度与目标关节状态之间的误差。以$q=\left[\begin{array}{cccc}0& \pi /4& \pi /6& \pi /3\end{array}\right]$ 为所要分析的位姿来对机械臂末端位姿进行误差分析。

假设第$\left\{i\right\}$ 个关节存在$\Delta {\theta }_i=0.001\text{rad}$ 的误差。让第一关节在其空间运动，其余关节保持不变，可以根据$\Delta p=\sqrt{\Delta x^2+\Delta y^2+\Delta z^2}$ 求得末端位置误差。然后，让第二关节、第三关节和第四关节依次运动，绘制各关节运动时的误差分布曲线，如图5所示。

Figure 5. Joint angle error distribution curve

图5. 关节角误差分布曲线图

根据图5(a)和图5(d)中的观察结果，可以得出在机械臂模型下，当关节角误差保持一定时，移动关节的运动对误差的影响保持不变，转动关节的运动同样不会影响误差的数值，误差不会随着这些关节的运动而改变。除去图5中的首末两关节外，在运动关节之前的关节角误差随着关节运动而变化，而在运动关节之后的关节角误差所产生的误差则不会随着关节运动而改变。然而，通过各个连杆的传递，一个较小的关节误差在传递到末端后将会导致一个较大的误差值。此外，随着传递的关节数据增多，产生的误差也相对增大。因此，逆运动学和正运动学的精度检测可以帮助及时发现机械臂运动控制中的误差和偏差，从而为系统的精确控制和协同工作提供保障。

4.2. 协同抓取实验结果及分析

在本实验中，通过基于ROS的双机械臂协同感知抓取系统进行了一系列实验，评估了系统在相机点云识别物体、MoveIt逆解执行结果和抓取精度等方面的性能表现。

1) 相机点云识别物体

首先对系统进行了相机点云识别物体的实验，仿真环境及相机视角如图6所示，评估系统对工作环境中物体的识别和定位能力。实验中，在Gazebo仿真环境下放置了不同形状和大小的物体，并通过Kinetic深度相机获取其三维点云数据。系统通过ROS中的点云处理库对点云数据进行预处理和分割[6]，然后采用目标检测算法对物体进行识别，并获取其位置和姿态信息。

通过点云数据处理，计算出方形物体中心点的位置坐标如图7所示。

Figure 6. Simulation environment and camera perspective

图6. 仿真环境与相机视角

Figure 7. The position coordinates of the center point of the square object

图7. 方形物体中心点的位置坐标

实验结果显示，相机点云识别系统能够准确地识别各种形状和大小的物体，并实时更新其位置和姿态信息。即使在复杂的环境中，系统也能够有效地区分目标物体和背景，并提供可靠的识别结果，为后续的抓取操作奠定了良好的基础。

2) MoveIt逆解执行结果

对系统进行了MoveIt逆解执行结果的实验，评估系统在运动规划和控制方面的性能表现。实验中，利用MoveIt对双机械臂进行逆解运动规划，以实现对目标物体的精确抓取。系统首先将双机械臂的运动空间建模为运动规划场景，并设置了运动约束和目标姿态(图8)。然后，利用MoveIt中的运动规划算法，对目标物体的抓取路径进行规划，并生成关节角度的逆解。

Figure 8. MoveIt identifies and plans control, planning time 0.049 s

图8. MoveIt识别并规划控制，规划时间0.049 s

实验结果表明，的MoveIt逆解执行器能够快速而准确地规划机械臂的运动路径，并实现对目标物体的精确抓取。无论是在简单的抓取场景还是在复杂的环境中，系统均能够稳定地执行抓取动作，验证了系统在运动规划和控制方面的可靠性和高效性。

3) 抓取精度

在实验中，为评估系统的抓取性能，定义了一组抓取指标，如抓取成功率、抓取姿态稳定性和抓取速度等。通过对具备不同颜色方块的实验对象进行一系列实验，机械臂定位至目标物体上方，利用视觉系统获取位置信息并执行抓取动作。在此之前，机械臂通过精确的运动学计算确定抓取点，并在抓取时进行实时调整以确保准确性。实验结果显示部分坐标信息，以五次数据为例，所测得的部分坐标信息如表3所示。

Table 3. Location coordinate information

表3. 位置坐标信息

编号	X/mm	Y/mm	Z/mm
1	305.1	980.2	400.0
2	302.4	980.7	400.0
3	302.6	1000.1	400.0
4	303.7	990.1	400.0
5	300.7	990.3	400.0

根据实验数据，目标物体固定坐标为(30 cm, 100 cm)，每次坐标波动范围在0.6 cm以内，不影响机械臂抓取操作。考虑到色块尺寸和夹爪间距，即使存在几毫米偏差，夹爪仍可有效抓取。综合数据分析表明，设计方案具备可行性。双机械臂协同感知抓取系统展现出高抓取精度和稳定性，在不同物体上均实现准确、稳定抓取，具有强适应性和实用性[7]。

5. 结束语

本研究致力于设计并实现基于ROS的双机械臂协同感知抓取系统，通过视觉点云识别技术和先进的运动规划算法，实现双机械臂在复杂环境中的高效协同操作。主要研究结论如下：

1) 基于ROS的系统架构优势

采用ROS作为系统架构基础，简化开发流程，提高可扩展性和可维护性，通过ROS的消息传递机制，实现了视觉传感器、点云处理、运动规划和机械臂控制之间的高效通信和协同工作。

2) 视觉点云识别技术的有效应用

利用RGB-D相机和PCL库成功实现对目标物体的精确识别和定位，为双机械臂的抓取和协同操作提供可靠感知基础。

3) 高效的双机械臂协同路径规划

通过MoveIt!框架实现双机械臂的协同路径规划和运动控制，提高任务执行的安全性和效率。

4) 任务分解与同步控制策略

设计的任务分解与同步控制策略使双机械臂高效协作，提高任务执行的可靠性和效率。

5) 系统的鲁棒性和实用性

经过大量实验测试，系统在不同工作环境和任务条件下表现出较高的稳定性和鲁棒性系统表现出较高的稳定性和鲁棒性，在多样化的物体抓取任务中表现优异，具有广泛应用前景。

尽管本研究取得了一定的成果，但仍存在一些局限性。首先，在处理动态环境中的目标物体时，系统的响应速度和实时性还有待提高。其次，物体识别的准确性在某些复杂场景中仍需进一步优化。

基金项目

河南省教育厅2023年大学生创新创业训练计划项目(202312746032)郑州科技学院2023年大学生创新创业训练计划项目(DC202332)。