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轴。

根据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 知，机械臂的末端端点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$均为机械臂的机械结构固有特性，采用无滑动即为固定值，通过三元一次方程带入求解获得角度值，控制关节舵机转动。

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

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

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

在 图4 中，双机械臂协同感知抓取系统设计使用视觉点云技术，通过获取环境中物体的三维点云数据，实现双机械臂之间的协同工作，提高抓取的准确性和效率 [5] 。把服务器Sever端定义为机器人A，把Client端定义为机器人B，机械臂视觉协同算法如 表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来描述这些坐标系之间的相对关系。对于机械臂的控制，有两种控制实现，一种是直接采用机械臂运动学正解，设置机械臂各个关节角度值实现控制，另一种是采用机械臂运动学逆解，设定好机械臂末端的位姿来进行操控。由于系统中在机械臂进行抓取之前已经通过视觉获取目标物体的位置坐标，即采用机械臂逆解进行目标物体的抓取。

在双机械臂协同感知抓取系统中，机械臂的逆运动学(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 所示。

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

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

1) 相机点云识别物体

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

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

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

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

2) MoveIt逆解执行结果

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

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

3) 抓取精度

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

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