一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法技术

本发明专利技术涉及基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法。包括如下步骤：步骤一：通过张正友相机标定法和ArUco Markers标定法标定机械臂底座坐标系和相机坐标系；步骤二：利用TSDF函数对获取的图像信息进行三维场景重建，以减少物体与物体之间的环境噪声点；步骤三：建立强化学习网络模型；步骤四：将预测的末端执行器抓取姿态反向投影至三维重建场景中，判断本次预测抓取质量；步骤五：通过机器人正逆运动学完成机械臂抓取移动；步骤六：进行强化学习模型训练，使得机械臂完成抓取动作；本发明专利技术克服现有技术的缺点，提出一种易实现、适用性高的，基于TSDF三维重建的机械臂六自由度视觉闭环抓取系统，此系统减少物体间遮挡、堆叠造成的环境噪声，降低了单一视觉传感器干扰导致的深度误差。此外，该系统在保证高精度的同时，可以实现快速实时的目标检测并完成抓取动作。成抓取动作。成抓取动作。

【技术实现步骤摘要】
一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法


[0001]本专利技术属于一种非结构环境下的机械臂六自由度抓取物体技术，具体机器人抓取控制领域。

技术介绍

[0002]目标抓取一直是机器人学重要的问题，但始终没有令人满意地相应解决方案。得益于机器臂的多功能操纵系统，使其能够在三维空间内灵活控制具有高自由度的末端执行器，实现对于物体的灵巧抓取以及对环境变化的动态响应能力。最近，随着深度学习和强化学习的快速发展和相应体系的构建，为机械臂智能抓取方式提供了多方面的可行思路。
[0003]尽管机械臂6自由度抓取控制具有较高的实用价值适用于更复杂的操作环境，但是目前多数数据驱动抓取算法仅在简单的桌面设置中进行自上而下的抓取(4Dof：x，y，z，yaw)，或利用物理分析来掌握合适的抓取姿态。然而由于受限制于三维移动抓取，算法对于应用场景的限制很大，机械臂末端执行器只能从垂直往下的方向接近物体，在某些情况下，它不能沿着这个方向抓取物体。例如，夹持器很难抓手很难抓住水平放置的盘子。此外这些基于物理分析的方法不仅需要计算大量的实验环境数据，并且需要计算估计精确的目标模型，从而产生的必然是大量的时间和计算成本，而对于非结构化目标物体的物理模型往往是不通用的，因此大多算法很难直接应用于新颖物体的抓取，系统的鲁棒性和泛化性较差。
[0004]由此，机械臂6自由度(6
‑
DOF)抓取想法被提出。虽然，Hongzhuo Liang提出的PointnetGPD采用采样
‑
评估的两步法，通过评估大量的样本以确定可靠的抓取姿态。但是，这种方法无疑是相当耗时的。(Hongzhuo Liang et al.“Pointnetgpd:Detecting grasp configurations from point sets”.In:2019International Conference on Robotics and Automation(ICRA).IEEE.2019,pp.3629
–
3635)。Florence等人从现有的抓取姿态中进行姿态转移。但是该算法对于数据集中未记录的目标对象和物体几何形状时，其成功率就会相对较为低下，故此不能推广到新的应用场景。(Peter Florence,Lucas Manuelli,and Russ Tedrake.“Dense Object Nets:Learning Dense Visual Object Descriptors By and For Robotic Manipulation”.In:Conference on Robot Learning(CoRL)(2018))。最近，从RGBD相机获取的部分视图中重建了点云场景，然后使用设计的Pointnet++网络模型提取物体特征后，直接完成6Dof抓取姿势回归并在重建的点云场景上进行抓取规划。(P.Ni,W.Zhang,X.Zhu,and Q.Cao,“Learning an end
‑
to
‑
end spatial grasp generation and refinement algorithm from simulation,”Machine Vision and Applications,vol.32,no.1,pp.1
–
12,2021.)。

技术实现思路

[0005]本专利技术克服现有技术的缺点，提出一种易实现、适用性高的机械臂六自由度抓取方法。本专利技术设计了特征提取网络和Policy Gradient强化学习框架，可以实现对于训练数据集中未记录物体的定位和六自由度计划抓取。
[0006]本专利技术以图像序列作为输入，首先通过张正友相机标定法和ArUco Markers标定法构建机械臂坐标系和相机坐标系。然后利用Intel Rrealsense D435深度相机获取抓取操作台的彩色和深度信息。通过Truncated Signed Distance Function(TSDF)对获取的图像信息进行三维场景重建，以减少物体与物体之间的环境噪声点。从上至下截取重建场景的二维俯视图像，并作为本文设计抓取神经网络模型的输入，模型对输入图像进行编码和特征提取，输入当前环境的预测抓取区域和机械臂抓取姿态。网络模型结构图见附图1。该模型采用Renet全卷积特征提取网络架构，学习从RGB
‑
D部分观察中提取非结构化特征。模型将彩色特征和深度特征进行通道连接，采用2个额外的Batch Normalization(BN)，Nonlinear activation functions(ReLU)，以及1
×
1Convolutional Layers交织。然后使用平均池化(Average Pooling)来聚合同一像素单元的特征嵌入，并进行线性上采样，使得卷积特征在不同位置和方向上共享，输出图片和输入分辨率一致，选取最大抓取概率热点图块的所在坐标，将其对应至实验前定义的半球状三维抓取姿态点(见附图2)，并将对应的6维姿态作为预测的6自由度抓取姿态。其次将模型输入的预测抓取姿态反向投影至重建的三维场景中，利用TSDF函数从三维场景中截取当前姿态下深度图像，并根据夹持器两端的深度信息判断本次抓取质量。最后，当渲染图像中目标坐标点深度小于等于夹持器两端指尖深度(15cm)时，将预测姿态输入机器人正运动学得到相应机械臂运动轨迹，完成机械臂本次抓取动作。如果本次抓取成功，则赋值给智能体奖励为1，反之赋值给智能体奖励为0。系统抓取流程图见附图3。
[0007]本专利技术为一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，具体步骤如下：
[0008]步骤1：张正友相机标定法和ArUco Markers标定法标定机械臂底座坐标系和相机坐标系：
[0009]首先将Intel D415深度相机垂直固定于机械臂末端，使得相机与末端执行器夹爪之间呈固定姿态变换，使得双目相机能够采集抓取操作台上物体的图像信息。然后，张正友相机标定法和ArUco Markers标定法标定相机内参和外参，构建机械臂底座为原点的三维坐标系。相机内参和外参标定公式如下所示：
[0010][0011][0012][0013]其中，x
w
,y
w
,z
w
为世界坐标系下三维坐标；x
c
,y
c
,z
c
为图像坐标系下坐标；u,v为像
素坐标系；z
m
为假设的相机坐标系中的一点；p为图像坐标系中的成像点；f为成像距离；d
x
,d
y
为像素点在图像坐标系下坐标；R,T为旋转矩阵和平移矩阵。公式(1)为世界坐标系到相机坐标系公式。公式(2)为相机坐标系到理想图像坐标系公式。公式(3)为相机坐标系到像素坐标系矩阵，即内参矩阵。
[0014]步骤2：利用TSDF函数对获取的图本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，其特征在于：包括如下步骤：步骤1：通过张正友相机标定法和ArUco Markers标定法标定机械臂底座坐标系和相机坐标系；步骤2：利用TSDF函数对获取的图像信息进行三维场景重建，以减少物体与物体之间的环境噪声点；步骤3：建立强化学习网络模型；步骤4：将预测的末端执行器抓取姿态反向投影至三维重建场景中，判断本次预测抓取质量；步骤5：通过机器人正逆运动学完成机械臂抓取移动；步骤6：进行强化学习模型训练，使得机械臂完成抓取动作。2.根据权利要求1所述的基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，其特征在于：所述步骤1具体步骤如下：首先将IntelD415深度相机垂直固定于机械臂末端，使得相机与末端执行器夹爪之间呈固定姿态变换，使得双目相机能够采集抓取操作台上物体的图像信息。然后，张正友相机标定法和ArUco Markers标定法标定相机内参和外参，构建机械臂底座为原点的三维坐标系。相机内参和外参标定公式如下所示：系。相机内参和外参标定公式如下所示：系。相机内参和外参标定公式如下所示：其中，x
w
,y
w
,z
w
为世界坐标系下三维坐标；x
c
,y
c
,z
c
为图像坐标系下坐标；u,v为像素坐标系；z
m
为假设的相机坐标系中的一点；p为图像坐标系中的成像点；f为成像距离；d
x
,d
y
为像素点在图像坐标系下坐标；R,T为旋转矩阵和平移矩阵。公式(1)为世界坐标系到相机坐标系公式。公式(2)为相机坐标系到理想图像坐标系公式。公式(3)为相机坐标系到像素坐标系矩阵，即内参矩阵。3.根据权利要求1所述的基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，其特征在于：所述步骤2具体步骤如下：2.1)：本发明在实际实验中采用单一传感器获取视觉信息，通过OpenCV获取双目相机对抓取操作台捕获的信息(彩色信息和深度信息)。2.2)：由于光线，物体之间相互遮挡等环境因素导致传感器获取的深度图像中存在较
大噪声点，从而影响网络模型对图像的抓取预测。首先本发明在每次抓取前对抓取操作台进行球型点采样，通过多次采样局部图像降低单一图像中深度误差。其次，利用TSDF函数将采样点姿态和采样图像信息融合成三维重建体素网格。对于单独一个体素而言，不仅包含直观的x,y,z坐标还用另外两个值(SDF和CAM)来表示体素到最表面的距离。其中SDF为符号距离函数用于深度图的融合，它用0表示表面，正值表示模型外部点，负值表示模型内部点，数值正比于点到最近表面的距离。表面容易提取为函数的零交叉点。如图2
‑
4所示，如果第i帧深度图像D(u,v)处深度值D(u,v)不为0，则比较D(u,v)与体素相机坐标V(x,y,z)中的z相比较，如果D(u,v)大于z，说明此体素距离相机更近，用于重建场景外表面；如果D(u,v)小于z，说明此体素距离相机更远，为重建场景内表面。即体素点位于表面之外(更靠近相机一侧)，则SDF值为正值，体素在表面之内，则SDF值为负值。2.3)：定义上述体素栅格所在的坐标系为世界坐标系，体素坐标可表示为2.3)：定义上述体素栅格所在的坐标系为世界坐标系，体素坐标可表示为相机的位姿为相机的内参矩阵为K。所以根据ICP配准得到的变换矩阵将世界坐标系下的体素栅格投影到相机坐标系，再根据相机内参矩阵K转换到图像坐标系I
ij
，其中i∈(0,1,...,n),j∈(0,1,...,m)。将体素栅格投影至图像坐标系后，首先需要计算每个体素初始化SDF值，如公式(6)所示式中表示第j个体素在世界坐标系下的位置信息，表示第i个相机位姿在世界坐标系下的位置信息，dep(I
ij
)表示第j个体素在第i个相机深度图像坐标系下的深度值。紧接着根据公式(19)截断每个体素的sdf值。式中tsdf
i
为体素截断距离值，trunc表示人为设定的截断距离，可理解为相机深度信息的误差值，如果误差越大，则该值设置大一些，否则可能造成深度相机获取的很多信息丢掉，本项目trunc设定为1。将每个体素的sdf值截断后，根据公式(20)重新计算每个体素的tsdf值：其中ω
j
(x,y,z)为当前帧全局体素栅格中体素的权重，ω
j
‑1(x,y,z)为上一帧体素栅格中体素的权重，tsdf
j
为当前帧全局数据立方体中体素到物体表面的距离，tsdf
j
‑1为上一帧全局数据立方体中体素到物体表面的距离，V
.z
表示体素在相机坐标系下的z轴坐标，D(u,v)
表示当前帧深度图像(u,v)处的深度值。公式(9)计算每个体素的权重值。w
j
＝w
j
‑1+w
j
ꢀꢀ
(9)从上至下渲染出抓取操作台的环境信息并以此作为网络模型的输入。4.根据权利要求1所述的基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，其特征在于：所述步骤3具体步骤如下：3.1)：设计机械臂抓取策略，定义强化学习的动作空间：通常将目标抓取分为两个抓取位置区域和三维旋转量，通过两个网络模型分别预测抓取区域和三维旋转量。而本发明设计使用一个二维卷积网络通过升高网络维度直接回归6
‑
...

【专利技术属性】
技术研发人员：欧林林，徐靖，禹鑫燚，周利波，魏岩，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：