(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210551360.5 (22)申请日 2022.05.18 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 欧林林　徐靖　禹鑫燚　周利波　 魏岩　 (74)专利代理 机构 杭州天正专利事务所有限公 司 33201 专利代理师 舒良 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于TSDF三维重建的机械臂六自由度 视觉闭环抓取方法 (57)摘要 本发明涉及基于TSDF三维重建的机械臂六 自由度视觉闭环抓取方法。 包括如下步骤： 步骤 一： 通过张正友相机标定法和ArUco  Markers标 定法标定机械臂底座坐标系和相机坐标系； 步骤 二： 利用TSDF函数对获取的 图像信息进行三维场 景重建， 以减少物体与物体之间的环境噪声点； 步骤三： 建立强化学习网络模型； 步骤四： 将预测 的末端执行器抓取姿态反向投影至三维重建场 景中， 判断本次预测抓取质量； 步骤五： 通过机器 人正逆运动学完成机械臂抓取移动； 步骤六： 进 行强化学习模型训练， 使得机械臂完成抓取动 作； 本发明克服现有技术的缺点， 提出一种易实 现、 适用性高的， 基于TSDF三维重建的机械臂六 自由度视觉闭环抓取系统， 此系统减少物体间遮 挡、 堆叠造成的环境噪声， 降低了单一视觉传感 器干扰导致的深度误差。 此外， 该系统在保证高 精度的同时， 可以实现快速实时的目标检测并完成抓取动作。 权利要求书5页 说明书10页 附图4页 CN 114851201 A 2022.08.05 CN 114851201 A 1.一种基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法， 其特征在于： 包括如 下步骤： 步骤1： 通过张正友相机标定法和ArUco  Markers标定法标定机械臂底座坐标系和相机 坐标系； 步骤2： 利用TSDF函数对获取的图像信息进行三维场景重建， 以减少物体与物体之间的 环境噪声点； 步骤3： 建立强化学习网络模型； 步骤4： 将预测的末端执行器抓取姿态反向投影至三维重建场景中， 判断本次预测抓取 质量； 步骤5： 通过机器人正逆运动学完成机 械臂抓取移动； 步骤6： 进行强化学习模型训练， 使得机 械臂完成抓取动作。 2.根据权利要求1所述的基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法， 其 特征在于： 所述 步骤1具体步骤如下： 首先将IntelD415深度相机垂直固定于机械臂末端， 使得相机与末端执行器夹爪之间 呈固定姿态变换， 使得双目相机能够采集抓取操作台上物体的图像信息。 然后， 张正友相机 标定法和ArUco  Markers标定法标定相机内参和外参， 构建机械臂底座为原点的三维坐标 系。 相机内参和外参标定公式如下 所示： 其中， xw,yw,zw为世界坐标系下三维坐标； xc,yc,zc为图像坐标系下坐标； u,v为像素坐 标系； zm为假设的相机坐标系中的一点； p为图像坐标系中的成像点； f为成像距离； dx,dy为 像素点在图像坐标系下坐标； R,T为旋转矩阵和平移矩阵。 公式(1)为世界坐标系到相机坐 标系公式。 公式(2)为相机坐标系到理想图像坐标系公式。 公式(3)为相机坐标系到像素坐 标系矩阵， 即内参矩阵。 3.根据权利要求1所述的基于TSDF三维重建的机械臂六自由度视觉闭环抓取方法， 其 特征在于： 所述 步骤2具体步骤如下： 2.1)： 本发明在实际实验中采用单一传感器获取视觉信息， 通过OpenCV获取双目相机 对抓取操作台捕获的信息(彩色信息和深度信息)。 2.2)： 由于光线， 物体之间相互遮挡等环境因素导致传感器获取的深度图像中存在较权　利　要　求　书 1/5 页 2 CN 114851201 A 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值为正值， 体素在表面之内， 则S DF值为负值。 2.3)： 定义上述体素栅格所在的坐标系为世界坐标系， 体素坐标可表示为 相机的位姿为 相机的内参矩阵为K。 所以根据ICP配准得到的变换矩阵将世界坐标系下的体素栅格 投影到相机坐标系， 再根据相机内参矩阵K转换到图像坐标系Iij， 其中i∈(0,1,. ..,n),j∈(0,1,. ..,m)。 将体素栅格投影至图像坐标系后， 首先需要计算每个体素初始化SDF值， 如公式(6)所 示 式中 表示第j个体素在世界坐标系下的位置信息， 表示第i个相机位姿在世界坐标 系下的位置信息， dep(Iij)表示第j个 体素在第i个相机深度图像坐标系下的深度值。 紧接着根据公式(19)截断每 个体素的sdf值。 式中tsdfi为体素截断距 离值， trunc表示人为设定的截断距离， 可理解为相机深度信息 的误差值， 如果误差越大， 则该值设置大一些， 否则可能造成深度相机获取的很多信息丢 掉， 本项目t runc设定为1。 将每个体素的sdf值截断后， 根据公式(20)重新计算每 个体素的tsdf值： 其中ωj(x,y,z)为 当前帧全局体素栅格中体 素的权重， ωj‑1(x,y,z)为上一帧体 素栅格 中体素的权重， tsdfj为当前帧全局数据立方体中体素到物体表面的距离， tsdfj‑1为上一帧 全局数据立 方体中体素到物体表面的距离， V.z表示体素在相机坐标系下的z轴坐标， D(u,v)权　利　要　求　书 2/5 页 3 CN 114851201 A 3