摘 要：面向机器人手眼协调抓取，提出一种针对家庭环境中常见物体的3维建模方法．利用RGB-D传感器能同时获取RGB图像与深度图像的特点，从RGB图像中提取特征点与特征描述子，利用特征描述子的匹配建立相邻帧数据间的对应关系，利用基于随机抽样一致性的三点算法实现帧间的相对位姿计算，并基于路径闭环用Levenberg-Marquardt算法最小化再投影误差以优化位姿计算结果．利用该方法，只需将待建模物体放置在平整桌面上，环绕物体采集10～20帧数据即可建立物体的密集3维点云模型．对20种适于服务机器人抓取的家庭常见物体建立了3维模型．实验结果表明，对于直径5 cm～7 cm的模型，其误差约为1 mm，能够满足机器人抓取时位姿计算的需要．
关键词：3维建模；特征点；特征描述子；自运动估计；位姿计算
A 3D Modeling Method for Robot’s Hand-Eye Coordinated Grasping
Abstract:For robot’s hand-eye coordinated grasping, a 3D modeling method for common objects in the household en-vironment is proposed. By simultaneously collecting RGB image and depth image from the RGB-D sensor, feature pointsand feature descriptors are extracted from the RGB image. The correspondences between adjacent frames are set up throughmatching of the feature descriptors. The RANSAC (RANdom SAmple Consensus) based three point algorithm is used to com-pute the relative pose between adjacent frames. Based on loop closure, the result is refined by minimizing the re-projectionerror with Levenberg-Marquardt algorithm. With this method, object’s dense 3D point cloud model can be obtained simplyby placing the object on a plane table, and collecting ten to twenty frames data around the object. 3D models are set upfor twenty household objects which are appropriate for the service robot to grasp. The experiment results show that theerror is about 1 mm for models with diameters between 5 cm and 7 cm, which fully satisfies the requirements for the posedetermination in robot grasping.
Keywords:3D modeling; feature point; feature descriptor; ego-motion estimation; pose determination
　　1 引言（Introduction）
　　物体3维建模在机器人学、医学、建筑测绘等领域有广泛的应用．传统的3维建模方法一般依赖于复杂的设备，例如结构光或专用的3维扫描仪，这种方法能够建立物体的高精度3维模型，但缺点是装置复杂、价格昂贵、不适合家庭环境使用．另一些3维建模方法则主要针对建筑物乃至城市的3维建模[1]，一般需要先采集海量数据，再对数据进行离线处理，对实时性要求不高．在机器人研究领域，3维建模主要应用在两个方面：一是对环境的感知与建模，以实现机器人的自主移动与导航；二是对物体的建模，以方便机器人对物体的抓取操作．常用的传感器包括激光、立体视觉等．随着微软Kinect的上市，基于RGB-D传感器的物体3维建模引起了研究者们广泛的关注．RGB-D传感器的优点在于它能同时获得场景的彩色信息与深度信息，数据量大，实时性好．利用RGB-D传感器，Henry等提出一种室内场景3维建模方法[2]，从RGB图像中提取特征点，结合深度数据，利用迭代最近点算法（ICP）实现传感器自运动估计，最后生成场景的密集3维点云模型．其处理速度为2帧／秒，位姿计算误差小于10cm．Izadi 等则只使用深度数据，利用经GPU（图形处理器）加速的ICP 算法实现对场景的实时3维重建，处理速度高达30帧／秒，但没有给出明确的精度数据[3]．Krainin等用机械手抓住要建模的物体在RGB-D传感器前运动，通过机械臂的运动学反馈获得各时刻物体在传感器坐标系下的坐标，最终建立物体的3维模型，对于直径6 cm～10 cm的模型平均误差约为2 mm[4]．在2011年国际机器人与自动化会议的视觉感知挑战赛中（http://www.icra2011.org/show.asp?id=40#3），为建立物体3维模型，将物体放置在可旋转平台上，利用棋盘格作为标志物来实现各帧数据间的对齐．但该方法需要使用者具备摄像机标定的相关知识，不适合家庭使用．家庭环境中面向机器人手眼协调抓取的物体3维建模具有以下4个特点：一是涉及到的物体体积范围不大，一般为易拉罐、茶杯等的大小，太大或太小的物体都不适合机器人抓取；二是建模设备与方法必须具有易用性，方便人们的日常操作；三是对精度与实时性有一定要求；四是模型表达以面向机器人手眼协调抓取为主，最后生成的模型并不需要具备视觉上的完整性．针对以上特点，本文提出一种基于RGB-D传感器的物体3维建模方法，只需将待建模物体放置在平整桌面上，手持传感器环绕物体拍摄若干帧数据（一般为10～20帧），即可获得物体的3维模型．与已有方法相比，该方法建立的模型具有很高的精度，且简单易用，适合普通操作者对家庭中物体的建模．
　　2 基于3维点集的位姿计算（Pose determi-nation based on 3D point set）
　　基于3维点集的位姿计算的基本算法是三点法[5]，其原理为对2个对应的3维点集fpig;fqig，i=1;2;3，若满足刚体变换关系qi=R¢ pi+t，其中R;t分别为两点集间的相对旋转与平移，则对R;t 的求解即为最小化以下目标函数：arg min3åi=1kqi¡(R¢ pi+t)k2(1)为了减少该函数中的未知数，先计算两点集的质心坐标p=13¢3åi=1pi 以及q=13¢3åi=1qi，通过将点集里各点减去其质心坐标p0i=pi¡p以及q0i=qi¡q，可将目标函数(1)变为arg min3åi=1kq0i¡R¢ p0ik2(2)为了求解该函数，构造矩阵H=3åi=1q0i¢ p0Ti ，对矩阵H进行奇异值分解H=U¢S¢VT，则R=V¢UT，t =q¡R¢ p.
　　3 基于特征点对应的自运动估计（Ego-motion estimation based on feature pointcorrespondences）
　　传感器的自运动估计是物体3维建模的核心问题，其本质是在不借助外部信息的前提下，通过传感器自身获取的数据，求取ti 时刻传感器坐标系相对于tj 时刻传感器坐标系的位姿变换．该位姿变换可以通过4£4的刚体变换矩阵T4£4=24R3£3t3£101£3135来描述，其中，R3£3 为两坐标系间的相对旋转，t3£1为两坐标系间的相对平移．
　　中的三点算法是在已知两点集里各点的对应关系的前提下计算刚体变换，但在实际应用中，确定两点集里点的对应关系是一个非常困难的问题．为了解决该问题，研究者们提出了多种算法，其中比较著名的一种是ICP 算法[6]．其基本思想是，将两点集里的最近点作为对应点，通过三点算法计算刚体变换．利用该变换，将两点集对齐到一起，再次寻找两点集里的最近点作为对应点，计算刚体变换．重复该迭代过程，直到达到终止条件，例如达到设定迭代次数，或者该次刚体变换矩阵与前次计算结果之差小于设定阈值．ICP算法虽然取得了广泛的应用，但仍存在明显的缺点．首先，因为每步迭代都要寻找两点集里的最近点，该算法计算量较大．另一方面，由于该算法将最近点作为对应点，迭代开始时要提供一个较好的初始值，否则会导致迭代不收敛或收敛到错误的结果．因此，如何寻找两点集里的对应点，是基于3维点集的自运动估计的重要问题．而针对点的匹配问题，计算机视觉领域的最新进展较好地解决了灰度图像中的对应点匹配问题．其思路为，通过分析灰度图像的灰度变化，提取特征点，通过统计特征点邻域的灰度直方图特性，为每一特征点建立一个具有一定长度的特征描述子．匹配阶段，度量各特征描述子间的距离，比如欧氏距离或L1 距离，将距离最小的特征点作为匹配特征点．为了提高匹配过程的效率，可采用基于KD树（K-dimensional tree）的近似最近邻算法来加速匹配过程．常用的特征点检测子和特征描述子包括SIFT（比例不变特征变换）[7]、SURF（加速鲁棒特征）[8]、Fern（随机蕨）[9]等．图1为基于SIFT特征的一对匹配图像，圆点显示了检测到的特征点，两点间的线连接了一对匹配特征点．第35卷第2期 杨扬，等：面向机器人手眼协调抓取的3维建模方法 153图1 基于SIFT的图像匹配Fig.1 Image matching based on SIFT将灰度图像的特征点与3维点集相结合，提出如下基于RGB-D传感器的自运动估计算法，其流程为：
　　1)对ti 时刻采集的数据Di（包含灰度图像grayImi 与深度图像depthImi）与tj 时刻采集的数据Dj，通过灰度图像与深度图像的注册，获得灰度图像中各点的增广坐标Paug= [u;v;x;y;z]，其中[u;v]为点在图像坐标系下的像素坐标，[x;y;z] 为点在传感器坐标系下的空间坐标．
　　2)从灰度图像grayImi 与grayImj 中提取特征点与特征描述子，通过特征点匹配获得匹配点集fPig与fQig，两点集里的点一一对应．在增广坐标的基础上进一步将各点的信息扩充为Paug= [u;v;x;y;z;Vdes]，其中Vdes为该点的特征描述子．
　　3)从点集fPig与fQig中随机提取3对对应点，应用上述三点算法计算R;t．4)利用上步计算的R;t，对点集里的所有点计算误差ei =kpi¡(R¢qi+t)k2，对于设定阈值z，若ei <z，则称该点为内点，统计内点数目．5)重复步骤(3)、(4)直到达到设定的迭代次数，保留拥有最多内点的R;t 作为最终计算结果．实际计算过程中，由于光线、视角等的变化，在第2)步匹配特征点时经常出现较多误匹配，虽然后面计算R;t 时的RANSAC过程能够在一定程度上消除误匹配的影响，但仍会存在计算结果错误的情况．为了减小特征点匹配时的错误，对于匹配特征点集fPig与fQig，进一步应用对极几何约束，即只保留满足pTi¢F¢qi=0的点，其中F为3£3的矩阵，称为基本矩阵（fundamental matrix）．
　　4 基于路径闭环的模型优化（Model refine-ment based on loop closure）
　　上节通过自运动估计可以获得tj 时刻传感器坐标系相对于ti 时刻传感器坐标系的变换矩阵iTj，对于物体3维建模，可将相邻帧间的变换矩阵相乘来获得各时刻数据相对于初始时刻数据的变换矩阵0Tk=0T1¢1T2¢ ¢ ¢k¡1Tk，从而将各时刻点云对齐到初始时刻传感器坐标系下，但该过程必然存在误差累积问题．为了解决该问题，引入基于路径闭环的模型优化．由于建模过程是环绕物体一周采集数据，末时刻采集的数据与初始时刻采集的数据间一般存在大量对应特征点，因此可以利用这些对应特征点，通过优化目标函数(3)，使点的再投影误差最小化．该目标函数可以用Levenberg-Marquardt算法求解．arg minnåi=1måj=1wi j°pi j¡P(Ci;Xj)°2(3)其中，Xj 表示空间点的3维坐标，Ci 表示ti 时刻摄像机的内外参数，P(Ci;Xj)则表示按照参数Ci 将点Xj 投影到图像坐标系上，因此，°pi j¡P(Ci;Xj)°2表示了该点的再投影误差．若该点在Ci 下可见，则wi j =1，否则wi j =0．
　　5 面向机器人手眼协调抓取的模型表达（Model representation for robot’s hand-eye coordinated grasping）
　　利用各时刻传感器相对于初始时刻传感器的变换矩阵，可以将各帧数据对齐到一起，从而获得场景的3维模型．为了进一步获得物体模型，需要对场景模型进行分割．由于在建模阶段将物体放置在平整桌面上，该分割可以通过两步完成．第一步，对深度数据取阈值，过滤掉距离传感器超过设定阈值的点．第二步，根据三点确定一个平面的原理，利用RANSAC算法拟合平面，过滤掉属于该平面的点．通过分割，可以获得物体的密集3维点云模型．但面向机器人手眼协调抓取的物体3维建模，主要是为机器人抓取时的物体识别与位姿计算服务，而当前物体识别领域的主流方法是基于特征点与特征描述子的方法，并不需要保存视觉上完整的密集3维点云模型，而只需要保存一系列特征点．如前所述，每一特征点Paug= [u;v;x;y;z;Vdes] 包含了其在图像坐标系下的像素坐标，在传感器坐标系下的空间3维坐标以及该点的特征描述子．这种稀疏的模型表达方法一方面减小了存储量，加快了索引速度，同时又能够满足后续物体识别与位姿计算的需要．但在下节实验中，为了直观表示，我们仍然保存了物体的完整模型．
　　6 实验（Experiment）
　　利用Kinect传感器对家庭中常见的物体，如易拉罐、包装盒、茶杯、各种塑料瓶等不同材质、不同154 机 器 人 2013年3月形状的物体建立3维模型，共建立了20种不同物体的模型，图2显示了其中的9种．表1列出了图2中各模型所包含的3维点数、特征点数以及建模过程中每帧数据的平均处理时间．其中物体编号对应图2中各物体，从上到下、从左至右逐一增加．图2 9种物体的3维模型Fig.2 3D models of nine objects表1 物体模型中包含的点数、特征点数、平均处理时间Tab.1 The point number, feature point number and averageprocessing time for each model物体编号 3维点数 特征点数 时间/ms1 169465 2837 851.82 96324 1522 908.03 120863 1153 853.34 60554 1159 788.45 113484 2093 826.16 92760 800 810.17 254966 2444 915.38 99931 1467 788.99 110455 1578 799.2进行该实验时所使用的电脑配置如下，CPU为Intel Core i5-2400，3.10 GHz，4GB内存，显卡为ATIRadeon HD5450，使用的操作系统是Windows7 32位操作系统．为了准确度量所建模型的精度，对其中的圆柱体模型作圆柱拟合，并将拟合出的半径与物体实际半径作比较，表2列出了6个圆柱体模型的精度．表2 圆柱体模型的精度Tab.2 The cylindrical object models’ precision物体名实际半径模型半径 误差/mm /mm /mm330ml听装可口可乐 32.50 33.56 1.06330 ml听装芬达 32.50 33.25 0.75330 ml听装王老吉 32.50 33.49 0.99240 ml听装杏仁露 27.00 26.45 0.55240 ml听装雀巢咖啡 27.00 27.70 0.70240 ml听装椰汁 27.00 27.27 0.277 结论（Conclusion）本文提出了一种面向服务机器人手眼协调抓取的物体3维建模方法，该方法不依赖于复杂的3维建模设备，只需使用者将待建模物体放置在平整桌面上，手持RGB-D传感器环绕物体拍摄若干帧图像，即可建立物体3维模型，因此非常方便在家庭环境下使用．对20种不同材质、不同形状的物体进行了建模实验，并对其中的圆柱体模型进行拟合，模型精度误差约1 mm，能够满足机器人抓取的定位需要．虽然本文只利用RGB-D传感器进行了实验，但该方法的本质是依赖于灰度图像与深度图像的注册，因此只需在数据采集阶段作相应的改动，即可应用于立体视觉传感器．由于RGB-D传感器的成像特点，该方法还无法建立透明或半透明物体的3维模型，这是下一步工作的方向．
　　参考文献（References）[1] Furukawa Y, Curless B, Seitz S M, et al. Towards Internet-scale multi-view stereo[C]//23rd IEEE Conference on Com-puter Vision and Pattern Recognition. Piscataway, NJ, USA:IEEE, 2010: 1434-1441.[2] Henry P, Krainin M, Herbst E, et al. RGB-D mapping: UsingKinect-style depth cameras for dense 3D modeling of indoorenvironments[J]. International Journal of Robotics Research,2012, 31(5): 647-663.[3] Izadi S, Kim D, Hilliges O, et al. KinectFusion: Real-time3D reconstruction and interaction using a moving depth cam-era[C]//24th Annual ACM Symposium on User Interface Soft-ware and Technology. New York, NY, USA: ACM, 2011: 559-568.第35卷第2期 杨扬，等：面向机器人手眼协调抓取的3维建模方法 155[4] Krainin M, Henry P, Ren X F, et al. Manipulator and objecttracking for in-hand 3D object modeling[J]. International Jour-nal of Robotics Research, 2011, 30(11): 1311-1327.[5] Arun K S, Huang T S, Blostein S D. Least-squares fitting oftwo 3-D point sets[J]. IEEE Transactions on Pattern Analysisand Machine Intelligence, 1987, 9(5): 698-700.[6] Besl P J, McKay N D. A method for registration of 3-Dshapes[J]. IEEE Transactions on Pattern Analysis and MachineIntelligence, 1992, 14(2): 239-256.[7] Lowe D G. Distinctive image features from scale-invariantkeypoints[J]. International Journal of Computer Vision, 2004,60(2): 91-110.[8] Bay H, Ess A, Tuytelaars T, et al. Speeded-up robust features(SURF)[J]. Computer Vision and Image Understanding, 2008,110(3): 346-359.[9] Ozuysal M, Calonder M, Lepetit V, et al. Fast keypoint recogni-tion using random ferns[J]. IEEE Transactions on Pattern Anal-ysis and Machine Intelligence, 2010, 32(3): 448-461.