位置不确定下移动机器人自主抓取方法

王迷迷，钱 堃，朱 林，郑 英

(1.东南大学成贤学院，江苏南京 210088；2.东南大学自动化学院，江苏南京 210096)

0 引言

往返取物递送是移动作业机器人的基本功能之一。移动机器人的运动底盘和多自由度机械臂机构提供了冗余自由度，使得移动机器人手臂能够有效扩展工作空间[1-4]，并使其快速高效地达到预定位姿、实现物品抓取和递送。近年来许多研究人员利用深度视觉(RGB-D)传感器为机器人抓取提供物体三维信息。文献[5-6]提出了利用深度点云数据自动非监督的3D表面重建方法。文献[7]提出了基于颜色信息和深度信息的物体分割算法。然而受到物体所放置支撑平面和其他物体遮挡的影响，通过RGB-D传感器仅能获取单一视角点云，缺少物体完整精确的三维点云，难以精确计算手爪的空间抓取位姿。

针对二指手爪和待抓物形状特点，文献[8]基于场景颜色和深度信息进行抓取矩形计算以确定手爪的位姿和开合度。为了获取抓取姿态几何特征信息，可以采用深度视觉传感器，但需要解决如何选取适合的特征参数来表征降维手爪位姿的问题。此外，为减少视觉相机视野遮挡[9]，可采用“眼在体外”的配置形式来获取单视角物体颜色与深度点云。相比常规固定基座机械臂，移动平台本身具有较低的运动精度及自定位精度，导致重复入位定点取物时机器人“手-眼-脚”三者位姿关系不确定，因此不能简单地沿用固定基座机械臂手眼标定方法[10-11]。

为此，本文提出一种克服移动机器人自身位置不确定性的物体抓取新方法。论文主要贡献是通过机器人本体激光传感器数据与机器人体外定点位置非精确深度视觉传感器数据结合处理标定移动机器人的手眼关系，补偿机器人多次重返作业点带来的位置偏差。针对在平整支撑面上的规则物体抓取需求，考虑机器人多自由度臂和二指平动爪的结构特征，采取顶抓(top grasp)策略[12]，以位置与形状信息五元组描述被抓物体的特征，经过被抓物识别与定位、抓取位姿确定等系列处理过程，基于兼顾障碍物环境条件的目标偏向改进RRT*-Connect算法进行机械臂运动规划，实现了对规则形状物体的Look-then-move式连续自动抓取。

1 往返取物机器人取物作业模型与工作流程

往返取物递送移动作业机器人通常的工作场景是：定点取物作业后，携物递送至远端，再返回取物作业工位进行下一轮抓取作业。本文建立的移动机器人定点抓取作业系统如图1所示，由抓取作业平台、搭载五自由度机械臂和激光传感器的移动机器人、RGB-D视觉传感器组成。取物作业平台位置固定；为避免机械臂遮挡视觉传感器在机器人体外定点安装；机器人可自由运动，机器人车载控制器无线收集、计算处理视觉传感器的数据。

移动机器人实现重复往返取物，需要完成以下动作：机器人进行首次抓取作业前，需要先对系统标定，建立RGB-D视觉传感器与机器人之间的关系；对待抓物体的识别与定位、根据被抓物体位置确定手抓位姿、机械臂运动规划等。同时，由于多次重复往返动作和机器人定位不精确，抓取时机器人位置存在不确定性，相对标定时存在位姿偏差(如图1所示)，对抓取结果有较大影响，需要对位姿偏差补偿。基于以上分析，给出机器人抓取作业工作流程如图2所示。

本文对物体抓取的关键问题进行了研究，包括移动机器人与视觉传感器关系标定、对往返定点位置不确定性误差补偿，基于RGB-D视觉传感器对抓取物体的识别与定位，抓取位姿的确定、机械臂运动规划等关键模块的实现方法。

2 关键过程与方法

2.1 系统手眼关系标定

抓取作业系统由RGB-D视觉传感器与机器人组成完整的识别与抓取系统的前提是对视觉摄像机与机器人机械臂组成的手眼关系进行标定。标定的基本过程如下：用Harris-3D角点检测算子在7×9棋盘格中提取不少于4个棋盘格角点，如图3所示。RGB-D传感器采用经典针孔相机模型，在此基础上采用EPNP[13]方法标定视觉传感器外参，计算世界坐标系和视觉传感器坐标系之间的转换关系R,T。通过视觉传感器确定检测到的角点在视觉传感器坐标系{S}下的具体坐标值{pSi，i=1,2,…,N}。驱动机器人手抓指尖触碰角点，采用运动学正解方法求得角点在机器人基座坐标系{B}下的坐标。

当机器人手抓指尖触碰到选定角点时，由手臂各关节的转动角计算获得选定角点在机器人基座坐标系{B}下的坐标值{pBi,i=1,2,…,N}。基于选定角点在机器人基座坐标系和视觉传感器坐标系下的坐标，采用SVD分解法求解得到机器人基座坐标系到视觉传感器坐标系的旋转矩阵RBS和平移矩阵TBS，即[RBS,TBS]为手眼标定结果，如算法1所示。

算法1 SVD分解计算转换矩阵步骤

输入：角点在两个坐标系下的坐标值：pSi、pBi

输出：两个坐标系之间的转换矩阵

a)计算在两个坐标系中的质心

b)计算qSi=pSi-pS，qBi=pBi-pB

d)SVD分解A=U∑VT

e)计算得到旋转矩阵RBS和平移向量TBS：

RBS=VUT,TBS=pS-RBS·pB

2.2 机器人抓取时位姿偏差补偿

在机器人取物作业系统中，由于抓取作业平台在世界坐标系中的位置固定，机器人在抓取作业时相对标定时的位姿偏差，可以由两者相对抓取作业平台的位置偏差确定。机器人观察到的作业平台支撑体(台面与桌腿)形状信息，以机器人本体激光传感器坐标系下的激光测距数据表示。

设机器人在进行手眼标定时机器人基座处于Xcali处，观测到抓取作业台面和桌腿形状的激光数据为Lcali,转换为点云Pcali；设抓取时机器人停在Xgras处，观测到抓取作业台面和桌腿形状的激光数据为Lgras,转换为点云Pgras。激光传感器与机器人基座一体，位置关系固定，系统标定与抓取时激光传感器的位姿偏差即是机器人基座位姿偏差，采用基于激光数据的迭代最近点(iterative closest points，ICP)算法估计Xcali和Xgras之间的偏差，原理是将手眼标定时的激光点云Pcali作为参考点云，将抓取时的激光点云Pgras作为源点云，对两者进行基于最小二乘法的最优匹配，每次迭代将Pgras通过旋转平移匹配Pcali，使二者点集尽可能重叠，每次迭代优化旋转与平移动作，通过多次迭代，达到最优配准，得出使两幅点云最优匹配的旋转平移矩阵[Rdevi,Tdevi]，满足式(1)关系。

Pcali=Rdevi·Pgras+Tdevi

(1)

迭代得到的旋转平移矩阵[Rdevi,Tdevi]表征了机器人抓取作业时与标定时基座坐标系的偏差。桌面与桌腿形状信息激光点云匹配如图4所示。

根据抓取与标定时的位姿偏差[Rdevi,Tdevi]，对手眼关系[RBS,TBS]进行补偿，得到实际抓取时的手眼关系，即式(2)。

[R′BS,T′BS]=[RBS,TBS]·[Rdevi,Tdevi]

(2)

2.3 被抓物体的识别与定位

对被抓物体的识别与定位，基于外定点布置的视觉传感器采集抓取作业台面上物体的深度图像信息，由机器人控制器分析计算完成。首先利用体素网格法(Voxel Grid)对原始的视觉传感器数据进行采样和滤波，减少后续处理计算量。采用随机采样一致性算法(RANSAC)来提取支撑物体放置的平面、获取其平面高度并将其滤除，使用欧式聚类方法实现被抓物体点云的聚类，并将其从原始点云图像中分割出来，得到一系列索引号标记的待抓取物体，记为OS={pSi|i=1,2,…,n}。聚类分割结果如图5所示。

得到待抓物体在视觉传感器坐标系下的点云OS后，根据机器人位姿偏差补偿修正后的手眼关系，将其转换到机器人基座坐标系下，计算方法如式(3)所示，实现了被抓物在机器人基座坐标系中的定位。

(3)

2.4 抓取位姿的确定

要实现机器人对物体的精确抓取，需要根据待抓物体的信息确定抓取时应采取的手抓位姿策略。抓取场景如图6所示。二指手爪抓取物体主要关注待抓取物体在机器人基座坐标下的的位置与形状，其中位置信息需要体现物体在抓取作业面的坐标及相对手抓二指移动方向的角度，以便手抓确定抓取位置与角度；形状信息需要体现物体的高度与被抓时的宽度，以便确定手抓二指开合度及夹取点。故被抓物体信息可以由五元组〈x0,y0,h,θ,w〉确定。五元组数据由待抓物体在机器人基座坐标系下点云投影计算得出，其中(x0,y0)是待抓物点云投影的质心坐标数据；h是待抓物点云高度数据，θ是待抓物点云分布主轴与机器人基座坐标系x轴夹角，w是待抓物体点云分布在与主轴垂直方向上的宽度。

将视觉传感器坐标系下的三维点云投影到物体支撑平面，即抓取作业面，并变换到当前实际抓取时的机器人基座坐标系下。记Psha为待抓物点云投影在机器人基座坐标系下的坐标psha={pshai|pshai=(xi,yi,zi),i=1,2,…；n}，其中zi为已获知的抓取平面高度，计算投影后点云Psha的质心。使用经典主成分分析法(PCA)计算得出投影后物体点云主轴，计算出其与基座坐标系x轴的夹角θ。

采用图6所示的顶部抓取(top grasp)策略，使用(x,y,z,σ,ζ,τ)来描述机器人手爪的抓取姿态，其中(x,y,z)表示手爪坐标系原点在机器人基座坐标系下的位置，(σ，ζ，τ)表示手爪坐标系在机器人基座坐标系下的欧拉变换角度。根据图6所示的抓取模型示意图，在机器人基座坐标系下，初始时手抓的姿态描述为式(4)。

(4)

式中a、o、n分别为手抓坐标系的x、y、z方向的数值。

将机器人手爪从初始状态绕手抓x轴旋转θ-π/2角度，使手爪二指相向平动连线(即手爪坐标系的y轴)转动到与待抓物点云的主轴垂直方向，到达可抓取位置，此时获取确定手爪位姿。

则手抓绕自身x轴旋转后的姿态描述为式(5)。

(5)

式中：s=sinθ；c=cosθ。

机器人的手抓抓取位置(x,y,z)用式(6)确定。

(x,y,z)=(x0,y0,z0+h+ε)

(6)

式中：(x0,y0)为被抓物体点云投影到机器人作业平面后的质心；z0为机器人作业平面的高度；h为物体本身的高度；ε为预留在被抓物体上方的偏移量，防止手爪碰倒物体及弥补物体定位误差。

综上，手爪抓取位姿由式(7)姿态变换矩阵Rgras唯一确定：

(7)

2.5 机械臂运动规划

对被抓物定位并确定手抓抓取位姿后，通过运动规划驱动机械臂末端达到目标位置完成抓取。机器人采用五自由度机械臂，规划算法需要适用于高维度机械臂运动规划。基于应用场景综合考虑，采用兼顾障碍物环境条件的目标偏向改进RRT*-Connect算法(描述见算法2)。当体外摄像机观察到无障碍物时，机械臂手抓直接快速向目标点运动。当观察到障碍物时，采用基于目标偏向思想改进的RRT*-Connect算法进行机械臂运动规划，该算法具有探索完备性[14]，分别从起始节点和目标节点开始生长两棵树相向探索，采用启发式采样，通过贪婪搜索连接两棵树，收敛速度快，搜索时间短。为了克服RRT*-Connect算法搜索过程采样点的随机性较大和采样区域太分散造成搜索效率低的问题，采用目标偏向思想，用均匀概率分布随机产生概率值P，如果P值大于设定的门限P0,则随机树探索点qrand设定为目标点qgoal，否则设定为在自由空间中的随机采样点，兼顾探索的方向性与避障性能。

设表征机械臂末端的可行空间的构型为Cfree，设以qnear点为中心以足够小尺度ε为半径的球形空间组成的构型为Q(qnear,ε)，当两棵随机树末端节点在构型Q(qnear,ε)空间内相遇，表示寻找到了从起始点到目标点的路径，基于改进的RRT*-Connect的机械臂运动规划双随机树连接过程如图7所示。

算法2 兼顾障碍物环境条件的目标偏向改进RRT*-Connect算法

输入：机械臂末端起始位置qinit，目标位置qgoal

输出：qinit与qgoal的连接路径Path(qinit,qgoal)

(A1)初始化起点树T1与终点树T2

T1_q0←qinit,T1_qnear←qinit；T2_q0←qgoal,T2_qnear←qgoal

(A2)获取障碍物环境信息及目标偏向概率数值

obstacle←camera()；

P←rand(0,1)；

while(FindPath≠ture)

{

(B1)if(obstacle=ture or P>P0)，

T1_qrand←qgoal

else

T1_qrand←randsample()

(B2)ifT1_qrand∉Cfree，go to (B1)

else ifT1_qrand∉Q(T2_qnear,ε)，

thenT1_qnear←T1_qrand,

else ifT1_qrand∈Q(T2_qnear,ε)

then

FindPath←ture；

return Path(qinit,qgoal)；

(C1)if(obstacle=ture or P>P0),

T2_qrand←qinit

else

T2_qrand←randsample()

(C2)：ifT2_qrand∉Cfree,go to (C1)

else ifT2_qrand∉Q(T1_qnear,ε)，

thenT2_qnear←T2_qrand

else ifT2_qrnad∈Q(T1_qnear,ε)，

then

FindPath←ture；

return Path(qinit,qgoal)；

3 实验结果

实验平台采用搭载PhantomX Pincher五轴轻质机械臂的Turtlebot移动机器人。机器人本体自带激光雷达(型号URG-04LX-UG01)；机器人控制器采用Intel Core I3处理器与4 GB内存的计算机；机器人体外定点位置的RGB-D视觉传感器采用Kinect视觉摄像机，分辨率640×480，视觉传感器模块通过无线网络通信方式将物体深度图像发给机器人控制器识别被抓物体并对其定位。

实验验证主要目的是评估基于本文方法对往返定点取物移动机器人位姿不确定条件下进行物体抓取的实际效果，重点验证对本文提出的通过机器人定点位姿偏差补偿克服机器人重复往返后位置不确定性方法，对提升抓取成功率的作用。

实验一测试验证机器人物体抓取系统手眼标定结果的误差。系统标定后，将Kinect传感器下视觉观察到的特征点转换为机器人基座坐标系下，与机器人手抓末端触摸选取特征点的坐标进行对比，得到手眼分别对物体定位的误差。以长方体木块为例，选取物体表面若干特征点来比较定位误差。

验证场景如图8所示，测试数据见表1。可以看出，物体在x和y方向定位误差小于0.5 cm，而z方向定位误差小于1 cm，z方向定位误差通过在抓取时预留一定z方向高度来弥补。标定测试结果的精度能满足本实验系统机器人物体抓取的要求。

表1 关键点手眼定位数据 mm

实验二测试验证对机器人定点位姿偏差的修正作用。机器人经过数次重复往返后定点停在抓取作业工位时，自身位姿相对初始手眼标定时的位姿已有明显偏差，实验测试发现若继续沿用标定时的手眼关系，机器人几乎无法抓取目标物体。

此时，将机器人激光传感器在标定[图9(a)]与抓取[图9(b)]时观测到的激光数据导入位姿补偿模块，计算出机器人位姿偏差补偿Xdevi。

Xdevi=[Rdevi,Tdevi]

据此修正手眼关系后，实验测试可顺利成功抓取物体，达到了机器人定点位姿修正的作用。

实验三测试验证带位姿修正的机器人物体抓取成功率。采用本文方法，对机器人抓取作业时的位姿偏差进行补偿，对被抓物体识别与定位，调整手抓位姿，采用算法2进行机械臂运动规划，对正方体、长方体、圆柱体等3种形状小尺寸(小于10 cm)物体进行抓取测试，如图10所示，结果表明对3类形状的物体均能达到良好的抓取效果。进一步对正方体、长方体、圆柱体3种典型物体，分别在采用本文方法位姿修正与不采用位姿修正条件下，连续往返取物对比测试，分析抓取效果，评估总体抓取成功率。

经过连续20次重复抓取实验，结果数据如表2所示。可以看出，不采用位姿修正抓取成功率小于25%，采用本文方法进行位姿修正后，抓取成功率高于70%。表明本文提出的带位姿修正的抓取方法能够显著提高重复往返取物机器人定点自主抓取作业成功率。

表2 典型物体抓取测试数据

4 结束语

本文提出了一种移动机器人手臂抓取物体方法，基于机器人本体激光传感器观测环境信息补偿移动机器人重复进入作业工位的位姿偏差，修正机器人体外定点位置的视觉传感器与机器人手臂关系的标定结果，恢复出被抓物体在机器人基座坐标系中的实际位姿；经过抓取位姿确定、机械臂运动规划等过程，达到了一次系统标定、重复移动抓取的效果。明显提升了移动机器人自主物体抓取的成功率。