1.由于对装配精度的要求很高，机器人轴孔装配一直是一个具有挑战性的任务。以往的工作往往通过限制末端执行器的自由度，或限制目标与初始姿态位置之间的距离来简化问题，这阻止了它们在实际制造中部署。本文提出了一种从粗到细的视觉伺服(CFVS)打孔方法，实现了基于三维视觉反馈的六自由度末端执行器运动控制。CFVS可以处理任意倾斜角度和大的初始对准误差，通过快速姿态估计前细化。此外，通过引入置信度映射来忽略目标的不相关轮廓，CFVS对噪声具有鲁棒性，可以处理训练数据以外的各种目标。大量实验表明，CFVS优于最先进的方法，在3-DoF、4-DoF和6-DoF井内钉的平均成功率分别为100%、91%和82%。

2.方法

CFVS是一种从粗到精的6自由度轴孔方法，由两个部分组成：具有目标不可知关键点网络(OAKN)的开环控制和具有偏移预测网络(OPN)的视觉伺服，如图所示。输入的三维点云是由眼控相机在世界坐标下捕获的深度图像重建而成的。采用了广泛应用于三维机器人操作的特征提取器pointnet++作为骨干网，将三维点编码为高维特征进行进一步处理。

（1）目标不可知的关键点网络(OAKN)

OAKN的设计目的是在进一步精细化之前，将末端执行器引导到孔附近的目标位置。提出利用点云特征来预测开环控制所需的位置和方向信息，点云特征比二维图像包含更丰富的几何信息。具体描述如下：

1）开环控制：开环控制的位姿信息可以用三个关键点来表示。其中，k1表示孔洞的中心位置，沿x轴和z轴的方向分别由和计算。根据这些关键点可以计算出旋转矩阵（见Algorithm1），并通过逆运动学将末端执行器引导到目标位姿。难点在于如何基于深度相机捕获的非结构化三维点获得高质量的关键点。

2）关键点预测：设为输入点云，其中为三维坐标中的一个点，N为点的个数。骨干网络将X编码为特征，并将其发送给多层感知机以预测关键点偏移量。因此，通过计算，可得到N个候选关键点。获得开环控制最终目标K的一种直观方法是对所有K求平均值。然而实验表明，这种朴素的平均池化策略会导致次优性能。因此，将候选关键点与可学习的置信度图结合起来，该置信度图可以自适应地重新加权的重要性。

3）置信图：在确定了多个候选孔中心后，下一步是确定最终的关键点，以便将末端执行器导向孔的中心。然而，由于在推断时孔的中心位置是未知的，因此不能根据距离直接为每个候选关键点分配置信度分数。因此，引入了一个可学习的置信度映射，对每个候选关键点重新加权，得到最终的关键点。置信图也由MLP预测，该MLP接收z作为输入和输出。因此，通过对每个候选关键点重新加权，可以得到最终关键点K。通过这种注意机制，可以消除不相关的视觉特征(如物体轮廓和嘈杂的背景)所带来的负面影响。结果表明，该模型对形状变化具有较强的鲁棒性，可以应用于多个不同的目标对象（如图所示）。

4）损失函数：通过最小化损失训练，其中是预测关键点偏移量的损失，表示置信图的损失。具体来说，被表示为预测和真实关键点偏移量之间的加权L1损失，其中表示候选关键点Ki的重要性，是真实的3D关键点偏移量。由于Ki的重要性不具有真值，所以是由一个三维高斯函数生成的：，其中是孔的中心位置，σ控制置信图的范围，靠近中心位置的点被赋予更高的置信度。这种加权机制使得OAKN忽略了物体和背景的轮廓，更加关注孔中心附近的信息。另一方面，被表示为均方根误差，这迫使预测的置信图近似于以孔为中心的高斯分布：。

（2）基于偏移量预测网络的视觉伺服

在第二阶段，采用视觉伺服进行进一步的细化。OAKN预测靠近孔的末端执行器的位姿，但对于插入来说，该位姿不准确。因此，通过带有OPN的视觉伺服来改进姿态，OPN通过视觉反馈迭代地估计平移和旋转偏移量，直到达到足够小的偏移量或重复到特定的时间。

1）平移和旋转偏移量：通过估计表示当前姿态和理想目标姿态之间的姿态差异的偏移量来细化末端执行器的姿态，以实现精确插入。首先将输入点云裁剪为，其中为末端执行器附近的点个数。通过裁剪点云，更关注轴和孔，而忽略其他冗余信息。给定裁剪的点云，OPN提取全局特征，并将其输入到MLP中，以预测三维坐标下的平移偏移量，以及欧拉角表示下的旋转偏移量。

2）视觉伺服：使用视觉伺服，一种闭环控制方法，通过迭代估计平移偏移量∆t和旋转偏移量∆r与OPN来优化末端执行器的插入姿态。首先记录末端执行器的原始位姿。将旋转偏移量∆r转换为旋转矩阵的表示。然后，将末端执行器移动到下一个姿态，其中。反复执行Algorithm2概述的精细方法，直到预测偏移量小于误差容限，或者OPN重复到特定时间。最后，执行插入命令来完成任务。

3）损失函数：通过最小化损失来训练OPN，其中和是平移和旋转偏移的损失。对于平移偏移量，我们使用均方根误差和余弦距离来优化平移误差，其中为平移偏移量真值。使用均方根误差来学习平移偏移量的大小和余弦距离来学习运动方向。对于旋转偏移量，采用均方根误差来最小化旋转误差，其中为旋转偏移量真值。

（3）数据增强

在Coppeliasim上收集粗数据集Dcoarse和细数据集Dfine。粗糙数据集在OAKN上训练，精细数据集在OPN上训练。对于粗糙和精细数据集，孔对象在工作空间内随机平移和旋转。

1）粗数据集：用M次迭代收集粗数据集。对于每次迭代，保持末端执行器处于初始构型并记录粗数据。

2）细数据集：用L次迭代反向收集精细数据集。对于每次迭代，初始设置末端执行器的姿态为孔的真值姿态。随机确定-∆t和-∆θr，它们是平移和旋转偏移量真值的负值。然后，根据-∆t和-∆r移动末端执行器，并将∆t和∆r记录为精细数据。

3）增强：训练数据集通过随机缩放、随机旋转以及这两种方法的混合来增强，如图所示。只沿着对象框架的x轴和y轴增加每个对象。为了防止孔的变形，在孔中心附近的点被省略。这种增强增加了训练集的丰富性并提高了性能。

4.实验

进行了一系列的实验来测量CFVS的性能。主要回答 (1)与其他基线相比，提出的方法在初始对准误差较小和较大的3-DoF，4-DoF和6-DoF的轴孔装配中的效果；(2)提出的方法能否推广到形状变化较大的看不见的物体上；(3)在初始对准误差大小的情况下，整体框架的速度有多快。

（1）实验设置

实验用UR5机器人在Coppeliasim上进行。通过仿真器自动求解的运动学逆解，将末端执行器控制到所需的三维坐标位姿。在模拟环境中，RGB-D相机是眼握在手，末端执行器是一个圆形或方形的挂钩，严格地固定在机械臂上。也就是说，轴和机械臂之间没有相对运动。在测试时，在从深度图像转换的点云上添加高斯噪声N (0 mm, 1 mm2)。圆轴的半径和高度分别为2.3厘米和10厘米，圆孔的半径和深度分别为2.5厘米和4.5厘米。方轴是一个4.6厘米×4.6厘米×10厘米的长方体，而方孔是一个5厘米×5厘米×4.5厘米的矩形腔。轴与孔之间的间隙设置为4mm。将轴与孔之间的初始距离设置为15 cm作为初始对准误差的小值，将30 cm作为初始对准误差的大值。

（2）任务

在实验中，设计了3-DoF、4-DoF和6-DoF任务。分别以15 cm和30 cm的初始对准误差对每个对象进行测试。所有这三个任务都有4毫米的间隙，任务的目标是将一个钉子插入一个洞。对于物体的形状，取四个长方体进行训练，然后使用两个相似的未见过的变化较小的长方体和两个变化较大的未见过的形状进行测试，由于末端执行器的限制，将垂直轴和插入方向之间的倾斜角设置为[0,50]度。如果倾斜角度太大，孔回到机械臂上，则末端执行器无法达到这样的姿态。也就是说，逆运动学没有解。

（3）评价指标

用成功率来衡量绩效。所有钻孔对象深度为45mm。在末端执行器执行插入后，如果挂钩接触到孔的底部则认为成功，否则为失败。提出的方法与三个不同的基线进行了比较。对每个看不见的对象测试250次，每个任务总共测试250×4 = 1000次。对于视觉伺服方法，还进行了效率实验。

（4）基线

ICP-这种方法是一种传统的点云配准方法，不是一种学习策略。在训练集中使用7mm×13mm×13mm长方体的三维模型来估计目标姿态。初始变换矩阵被设置为从末端执行器位置到工作空间中心点的变换。

ICP w/ kpts-与ICP类似，ICP w/ kpts还被赋予一个粗略的对象姿态。利用神经网络对初始变换矩阵进行姿态预测。

3DRHD-这是一种利用RGB-D相机估计孔位置和插入方向的开环方法。采用了覆盖整个场景的点云来代替物体表面。

KOVIS-用默认设置训练KOVIS，一个2D视觉伺服系统。KOVIS最初是测试为4DoF任务与圆轴孔。在设置的4-DoF和6-DoF任务中，用方轴孔测试KOVIS。

（5）结果

1）成功率

对于3-DoF任务，如果没有准确的初始变换信息，ICP很难完成插入任务。3DRHD和ICP w/ kpts的性能较低，因为它们是开环方法，没有进一步改进。KOVIS和提出的初始对准误差为15厘米。然而，由于纯视觉伺服，KOVIS降低了30厘米的初始对准误差的性能。对于4-DoF任务，所有基线的成功率都大大降低。由于目标物体与给定的3D模型相似，因此ICP / kpts仍然可以以较小的形状变化插入孔中。然而，它们的形状变化很大。KOVIS用方形钉和孔失败了。在他们的实验中，他们只测试了圆钉和孔，这并不能证明他们学会了z轴旋转。对于6-DoF任务，提出的方法表现最好，表明CFVS通过理解钉孔和孔的三维关系对倾斜孔具有鲁棒性。此外，实验表明，CFVS可以对目标不可知，解决了形状变化大的问题。

2）准确率

在视觉伺服方法方面，比较了CFVS与KOVIS在初始对准误差为15 cm和30 cm时的效率。为公平起见，将视觉伺服时间记录在3-DoF钉入孔中，观察到当初始对准误差为15 cm和30 cm时，CFVS的任务完成时间约为7秒，而当初始对准误差为30 cm时，KOVIS的任务完成时间更长。这是因为KOVIS需要一步一步地前进。相反，CFVS首先实现近似姿态，然后再对姿态进行细化以完成任务。

5.结论

本文提出了CFVS，一种基于粗到精的三维点视觉伺服框架，它是第一个实现6自由度倾斜孔的钉孔装配。CFVS解决了传统视觉伺服方法初始误差大的问题，在逐步细化之前进行快速粗略的姿态估计。这种方法大大缩小了勘探范围。此外，由于只关注孔洞周围的信息，CFVS可以很好地推广到看不见的物体，并且对目标形状的变化具有鲁棒性。

Lu B S, Chen T I, Lee H Y, et al. CFVS: Coarse-to-Fine Visual Servoing for 6-DoF Object-Agnostic Peg-In-Hole Assembly[C]//2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023: 12402-12408