[发明专利]一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法

权利要求书

1.一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法，其特征在于：包括如下步骤：

步骤1：通过张正友相机标定法和ArUco Markers标定法标定机械臂底座坐标系和相机坐标系；

步骤2：利用TSDF函数对获取的图像信息进行三维场景重建，以减少物体与物体之间的环境噪声点；具体步骤如下：

2.1)：采用单一传感器获取视觉信息，通过OpenCV获取双目相机对抓取操作台捕获的信息，所捕获的信息包括彩色信息和深度信息；

2.2)：由于光线、物体之间相互遮挡的环境因素导致传感器获取的深度图像中存在较大噪声点，从而影响网络模型对图像的抓取预测；首先，在每次抓取前对抓取操作台进行球型点采样，通过多次采样局部图像降低单一图像中深度误差；其次，利用TSDF函数将采样点姿态和采样图像信息融合成三维重建体素网格；对于单独一个体素而言，不仅包含直观的x,y,z坐标，还用SDF和CAM两个值来表示体素到最表面的距离，其中SDF为符号距离函数用于深度图的融合，它用0表示表面，正值表示模型外部点，负值表示模型内部点，数值正比于点到最近表面的距离；表面容易提取为函数的零交叉点；如果第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)：定义上述体素栅格所在的坐标系为世界坐标系，体素坐标表示为相机的位姿为相机的内参矩阵为K；

所以根据ICP配准得到的变换矩阵将世界坐标系下的体素栅格投影到相机坐标系，再根据相机内参矩阵K转换到图像坐标系I_ij，其中i∈(0，1，...，n)，j∈(0，1，...，m)；

将体素栅格投影至图像坐标系后，首先需要计算每个体素初始化SDF值，如公式(6)所示

式中表示第j个体素在世界坐标系下的位置信息，表示第i个相机位姿在世界坐标系下的位置信息，dep(I_ij)表示第j个体素在第i个相机深度图像坐标系下的深度值；

根据公式(7)截断每个体素的sdf值；

式中tsdf_i为体素截断距离值，trunc表示人为设定的截断距离，可理解为相机深度信息的误差值，如果误差越大，则trunc设置大一些，否则可能造成深度相机获取的很多信息丢掉，将trunc设定为1；

将每个体素的sdf值截断后，根据公式(8)重新计算每个体素的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)

从上至下渲染出抓取操作台的环境信息并以此作为网络模型的输入；

步骤3：建立强化学习网络模型；

步骤4：将预测的末端执行器抓取姿态反向投影至三维重建场景中，判断本次预测抓取质量；具体步骤如下：

将预测的末端执行器抓取姿态反向投影至三维重建场景中，判断本次预测抓取质量：将网络输出的6自由度抓取姿态反向投影至抓取前建立的三维体素场景中，通过视角锥体投影截取预测姿态下深度图像；通过比较渲染图像中夹持器两端深度信息，推理平行夹爪与物体间的距离位置，从而判断预测抓取姿态的抓取质量，形成机械臂6自由度抓取系统闭环反馈；

步骤5：通过机器人正逆运动学完成机械臂抓取移动；具体步骤如下：

当渲染出的深度图像中夹持器两端深度小于等于夹持器指尖深度，即夹持器预测姿态位于物体两侧，判断本次预测姿态可以抓取；然后，控制机械臂根据预测姿态执行抓取计划：通过机器人逆运动学求解出当前状态下机械臂的6个关节角度数；然后将所述步骤3中强化学习网络模型输出预测的机械臂末端6维抓取姿态输入机器人正运动学，求得机械臂从当前姿态变换至预测姿态点的末端执行器移动轨迹；当机械臂末端执行器运动至预测姿态后，机器人发出关闭夹持器信号，尝试进行抓取动作；夹持器关闭后，末端执行器垂直上移15cm，从双目相机获取末端执行器上移后深度图像，通过计算夹持器两端的深度判断实际抓取是否成功；当抓取成功时，强化学习模型奖励赋值为1；抓取失败时，强化学习网络奖励赋值为0；

步骤6：进行强化学习模型训练，使得机械臂完成抓取动作。