基于Kinova机械臂RGBD相机的目标抓取

黄 伟, 郭瑞科, 刘大勇, 康 柯, 周忠达, 胡海东

1. 中国空间技术研究院杭州中心,杭州 310024

2. 北京控制工程研究所,北京 100094

0 引 言

对物体感知和操作是目前机械臂领域研究的热点.随着科学技术的发展,执行重复性劳动已经由人工转向机械臂,然而机械臂通常不具备目标识别和定位的能力,因此,需要为机械臂配置一双“眼睛”实现更强大的功能.

在轨服务如气液加注、故障修复等空间应用场景[1], 通过装有手眼相机的机械臂对故障航天器进行识别、定位,再进行修复,可以有效延长在轨航天器的使用寿命, 降低航天器运行成本, 对航天事业发展具有重要意义.

机器视觉可以实现物体的检测和定位,充当机械臂的“眼睛”.相机作为机器人的眼睛进行图像的采集、目标捕获和定位等功能,在工业应用中往往安装于传送带正上方,相机至传送带的垂直距离固定不变,只进行二维平面的定位[2-3].黄梦涛等[4]在快递行业中采用在物体表面贴二维码进行定位,通过ar_track_alvar功能包获取二维码相对于机械臂工具坐标系的位姿.但贴二维码定位方法存在以下不足:物体表面需平整,以便于粘贴二维码,并需要预先输入二维码的大小等参数;不适用于表面弯曲或者体积较小的物体等.毛成林等[5]提出一种结合TCP(tool central point)标定过程同步标定深度相机手眼关系的新方法,减少了TCP和手眼关系独立标定累积误差影响.此外,杜惠斌等[6]提出一种利用低成本3D 打印球作为标定件对机械臂与RGBD 相机进行手眼标定的方法,这些方法集中介绍深度相机的标定问题,没有提及深度相机与彩色相机配准问题.与上述方法不同,本文的创新之处是在xyz3个方向定位,涉及姿态信息且阐述了深度相机与彩色相机配准,使得距离方向检测更加准确.本文采用HSV方法[7]进行图像分割,获得彩色图像中目标位置,通过彩色图与深度图的配准关系,结合深度相机的深度信息以及机械臂与相机的变换关系,完成目标物体三维定位和抓取过程.

1 系统介绍

本文采用Kinova机械臂进行目标抓取任务.Kinova官网提供开源的机器人操作系统(robot operating system,ROS)程序功能包,包括机械臂控制的ROS_KORTEX功能包和相机采集的ROS_KORTEX_VISION功能包,方便用户二次开发和使用.ROS 操作方便、功能强大,非常适用于机器人多节点多任务的复杂场景.自 2010 年诞生以来,ROS 就受到了学术界和工业界的欢迎,如今已经广泛应用于机械臂、移动底盘和无人机等许多种类的机器人上[8].ROS以节点为基本程序单元,可以很方便地实现机器人的运动、路径规划,与视觉、控制等模块之间的通讯也有相应的功能包,极大扩展了应用范围.

目标检测和定位常见的工具有OpenCV、Halcon和Matlab图像处理包等,而Halcon、Matlab等由于收费等原因,实际应用范围相对小.本文采用OpenCV实现.

综上所述,本实验平台采用Ubuntu 18+ROS Melodic 开发,基于ROS_KORTEX功能包实现Kinova机械臂运动控制和目标抓取,配合ROS的MoveIt功能包可在Rviz中实现可视化.视觉定位方面,使用OpenCV,采用HSV方法,进行图像分割处理,在图像坐标系下定位,结合深度相机完成目标检测定位.该方法对物体表面形状和体积无特别要求,可实现更通用的目标抓取.

2 相机标定、配准与三维定位

2.1 相机内参标定及手眼标定

相机标定是通过物理空间点及其对应的像素点,获得相机的位置信息和内部参数的过程,完成相机标定后系统才能根据目标的图像像素位置计算得到其物理空间坐标,从而为机械臂的抓取指定物理目标位置.相机标定包含相机内参和外参2个部分:相机内参表示从相机坐标系到图像坐标系的转换关系,由相机加工、装配过程或物理属性决定;相机外参表示世界坐标系到相机坐标系的转换关系.通常将机械臂与相机的标定称为手眼标定,分为眼在手上和眼在手外.

Kinova机械臂的相机安装于末端执行器上方,官方提供的ROS驱动程序中已经内置了相机内参和外参.相机坐标系和末端执行器工具坐标系之间的变换矩阵,是一个固定值,由安装位置决定,而相机坐标系相对于世界坐标系(基座)的关系可以通过ROS的TF(transform)树直接读取,因此不需要进行额外的手眼标定步骤.

2.2 彩色相机和深度相机(RGBD)的配准

本节首先介绍ROS图像消息与OpenCV图像格式的转换方法,其次介绍彩色相机和深度相机图像之间的配准方法,实现彩色相机目标像素的三维定位.最后,基于HSV颜色空间进行目标检测、图像分割以及空间定位方法.

(1)图像获取和格式转换.

Kinova机械臂内置的RGB相机和深度相机获取图像信息后, ROS系统的ROS_KORTEX_VISION功能包将按30 fps(帧每秒)的频率发布实时的图像信息.对于RGB相机,ROS通过/camera/color/image_raw 话题发布“bgr8”格式的Image类型消息.对于深度相机,ROS会通过/camera/depth/image_raw话题发布“16UC1”格式的Image类型消息,其中每个像素的深度数据以毫米为单位.

上述Image类型消息OpenCV不能直接读取和识别,需要在程序中订阅相应的图像信息通过CvBridge实现不同图像格式间的转换,并将其转换为OpenCV可以处理的Mat矩阵格式.

(2)深度图像和彩色图像的配准.

彩色相机和深度相机分别获取到彩色、深度图像后,需要对2幅图像进行配准,实际就是让深度图和彩色图重合在一起,即将深度图像的图像坐标系转换到彩色图像的图像坐标系下,从而能够确定RGB图像内特定位置像素的深度信息(即获取每个像素的RGBD信息).首先,找出深度相机图像坐标系和彩色相机图像坐标系之间的转换关系.

对于深度相机D,从深度相机坐标系到图像坐标系的变换可以表示为

(1)

其中,XD、YD和ZD是相机坐标系下的三维坐标,uD、vD是图像坐标系下的坐标,fx、fy、u0和v0是相机内参.整理为4×4齐次矩阵

(2)

(3)

深度相机与彩色相机的坐标变换矩阵M通过以下方式获取.世界坐标系下的点P变换到深度相机坐标系和RGB相机坐标系下,得到

PD=RDP+TD

(4)

PR=RRP+TR

(5)

将式(4)中的P用PD、TD和RD表示,代入式(5),得到

(6)

对比式(4)和(5),可得

(7)

T=TR-RTD

(8)

R和T即是矩阵M的旋转和平移部分.

这样世界坐标系下的点在深度相机和彩色相机坐标系下有如下关系:

(9)

由式(2)、(3)和(4)可以得到2相机像素之间的转换关系

(10)

其中,ZD是深度相机至目标物的距离,ZR是彩色相机至目标物的距离.由于深度相机和彩色相机至目标物的距离几乎相等,即ZD≈ZR.式(10)可简化为

(11)

其中,

(12)

将公式展开,可得深度相机图像坐标系转换至彩色相机图像坐标系的解析解

(13)

至此,深度图像坐标系和彩色图像坐标系的关系已经确定.

2.3 基于彩色相机和深度相机(RGBD)的三维定位

通过下面2个步骤计算RGB图像坐标系下特定像素的空间位置:

根据转换关系,遍历彩色图像的每一个像素,找到其对应的深度图像坐标并记录数据,便可得到配准后的深度图,如图1(a)所示.

图1 配准后的Depth/RGB图Fig.1 Depth/RGB map after match

结合像素位置、深度信息及彩色相机的内外参矩阵,以及ROS发布的TF树,便能够得到彩色图像目标像素相对于世界坐标系的位置.如下图1(b)所示,图中的红点表示目标像素位置,左上角显示了当前像素对应的世界坐标.

3 抓取实验

3.1 目标的检测与定位

视觉定位方法通常有以下几种:模板匹配方法(如基于点云迭代最近点算法)、机器视觉与深度学习相结合方法和基于图像特征方法.每种方法都有相应的应用场合和优缺点.

本文采用红色和蓝色正方体作为被抓取物体,边长约为5 cm.视觉定位算法属于图像特征方法.和传统的图像特征提取方法不同,不仅得到二维图像的坐标信息,还利用深度相机计算得到物体相对于机械臂基坐标系的三维坐标.采用基于目标物体的颜色进行图像分割,并根据形态学方法分析获得特定颜色目标在图像坐标系中的位姿信息,结合2.3节的三维定位方法,可以实时输出目标相对于世界坐标系的三维位姿.目标分割是在HSV色彩空间(如图2[8])内执行的,根据颜色的直观特性创建的一种颜色空间,也称六角锥体模型(hexcone model),其颜色参数分别是色调(hue)、饱和度(saturation)和明度(value).

图2 HSV色彩空间示意图Fig.2 HSV color space

红色、蓝色和绿色物体,其对应的HSV色彩范围为:

红色:H(0-8)∪(160-180)、S(80-255)和V(50-220)

蓝色:H(90-124)、S(43-255)和V(26-255)

绿色:H(35-77)、S(43-255)和V(26-255).

目标检测的图像处理过程如图3所示,图3(a)是原图,图3(b)进行HSV分割,图3(c)进行形态学开运算和最小外接矩形获取,图3(d)计算目标深度信息.具体步骤如下:

图3 图像处理过程Fig.3 Image processing process

1)筛选像素.基于配准后的深度图,筛选与相机距离小于1m的像素,从而消除距离相机较远部分背景对目标检测的干扰.之后,将经过初步筛选的图像转换为HSV色彩空间,并进一步筛选HSV在特定范围的所有像素点,得到包含所有蓝色像素的掩膜(如图3(b)).

2)获取外接矩形.通过形态学开运算,先以10×10像素大小的图像算子进行腐蚀,再以10×10像素大小的图像算子进行膨胀,去除噪点,之后获取图像中体积最大的外轮廓,并计算其最小外接矩形(如图3(c)).

3)获取三维坐标.取外接矩形中心位置像素并计算其相对于世界坐标系的三维坐标,将该值作为抓取目标的空间坐标.此外,根据外接矩形与图像坐标系x轴的夹角,可以确定目标在执行器z轴方向的姿态(如图3(d)).

3.2 基于位置控制的目标抓取

3.1节通过颜色空间分析图像获取目标像素坐标,结合深度相机获取三维位置,以便使用机械臂进行目标抓取.具体的抓取流程,如图4所示.

图4 抓取流程图Fig.4 Capture flowchart

首先,机械臂移动末端执行器至垂直向下正视桌面的初始姿态,从而能够观察到放置于桌面的目标物体.

接着,系统将利用基于颜色空间HSV功能包检测相机画面中的正方体.当画面中存在红色或蓝色正方体时,系统会将正方体的中心位置和相机坐标系间的转换关系以TF消息的形式发布.机械臂控制程序通过订阅TF话题,得到正方体在世界坐标系下的三维位置与姿态.

获取到目标位置信息后,功能包程序将询问操作者是否执行抓取任务.

确认执行抓取任务后,程序会根据正方体位置计算结果引导末端执行器移动至正方体正上方,同时调整其z轴方向的姿态,使得执行器的抓手与目标正方体的棱边平行.结合目标位置与位姿,程序调用ROS_KORTEX API的位置控制接口,生成抓取路径并执行目标的抓取,最终将正方体放置于指定的位置.完成全部的抓取和放置动作后,程序将控制机械臂回到初始姿态,并重新检测是否存在抓取目标信息.

正方体实物抓取如图5所示.图5(b)为Rviz中显示的可视化虚拟机械臂,与实际机械臂会保持同步运动.图5(c)为视觉检测窗口,系统将通过HSV颜色分割检测蓝色和红色正方体的位置与姿态,正方体的轮廓与正方体中心点的三维位置在图中通过绿色标注显示.实物抓取实验过程中,在相机视野内随机摆放正方体,连续试验50次,均抓取成功,成功率100%,说明基于颜色空间的方法在分割红色和蓝色正方体上具有很高的可靠性;此外,从被检测到的正方体外接矩形框(图5(c)绿色框所示)与实际正方体边线的重合度,可以看出检测过程的准确度较高.

图5 目标抓取过程Fig.5 Target capture process

4 结 论

本文针对在轨服务与维护应用场景负载轻、近距离感知和精准定位等特点.采用Kinova机械臂及自带的RGBD相机进行感知和抓取实验.介绍了机械臂系统和相机标定的问题,详细阐述彩色相机RGB图像与深度相机深度图的匹配问题以及三维空间定位问题,通过彩色图像和深度图像的配准提高定位准确性.在实验环节介绍了颜色空间的图像处理和分割,得到目标的二维坐标点和旋转角度,结合深度图获得三维坐标信息.通过机械臂的控制模块成功实现基于位置控制的目标抓取,为在轨服务提供了理论验证.