引导语：本文为“3D视觉创新应用（三维重建）竞赛”作品集系列之一，该作品由来自深圳大学团队完成，团队成员：赵晨昊、张曼英、周制黔、郭宏希、陈炎彬。全文约2604个字，阅读时长约5分钟，旨在为更多开发者提供学习参考。
一、项目介绍
利用奥比中光科技集团股份有限公司提供的Zora P1开发板、 RGBD相机，以及深圳大学物理与光电工程学院智能光测研究院提供的川崎机器人（6轴）开发出了一款RGB-D相机结合机械臂对任意摆放的复杂加工零件的三维重建及无序抓取系统。
源码地址：
https://github.com/3DCVdeveloper/zhaochenhao
二、项目背景
1.市场分析
随着“中国制造 2025”的提出，中国正处在“制造大国”到“制造强国”转型的进程，这一过程将会促使制造业转向智能化与自动化。
尽管中国已成为全球工业机器人密度增速最快的国家，但与国外相比，我国机器人密度仍处于较低水平。考虑到未来中国对于工业机器人的大规模需求以及智能化的要求，辅助机器人智能化的视觉技术将具有庞大的市场潜力。
而在工业机器人技术发展的过程中，工业分拣系统为其中重要的一环。在以往，传统工业分拣技术利用人工方式，耗力耗时，不仅难以满足自动化长时间作业的需求，同时人力长时间进行重复性工作时，也会导致出错率大幅度提高，使生产受到影响。现如今，自动化技术快速发展，在各类工业化生产流水线上，机器人技术正在被大规模的推广使用。
2.存在的问题
预先编程的工业机器人被应用在传统的工业自动分拣系统中，进行工业分拣。尽管它可以使重复性动作得到复现，然而这种工业机器人不能根据实际情况来变换自身动作，其所使用的分拣系统必须严格将分拣对象的摆放位置进行设定，并将大量的零部件位置固定，这虽然使得机器人不需要具备自主辨识的能力，对机器视觉技术的要求较低，但也因此此系统无法实现分类工作，只能实现简单的分拣任务。然而，在大部分的智能制造生产线中，多品种的零部件大多摆放位置随意、数目不固定，需要分拣系统正确识别出零件的类型，并从堆叠的零件中准确抓取零件，因此需要愈加智能化的机器视觉技术。基于姿态估计的机器视觉工业分拣系统在应对堆叠零件无序摆放的挑战上，具备较强的灵活性与鲁棒性，因而在工业生产中，有必要大力推广带姿态的机器视觉分拣系统，在使产品质量得以保证的同时，提升工业的生产效率和泛用性。
三、系统组成
系统搭建分为硬件系统部分和软件系统部分。
其中，硬件系统主要包括以下两部分，如图1所示。
（1）控制系统：
川崎6轴机器人，气泵及气动夹具
（2）视觉系统：
Orbbec Astra Pro相机，Zora P1开发板，高性能计算机
图1.系统构成
软件系统主要包括：点云处理模块和相机与机械臂之间的通信模块，如图2所示。
图2(a).软件系统
图2(b). 软件设计流程图
四、关键技术及创新点
1.关键技术
①多帧及点云预处理，误差补偿
图3为工作距离（620mm）下白色背景板点云在Geomagic中拟合的平面，标准差1.43mm，正负最大距离为9mm，正负平均距离1.22mm。由数据可知在此工作距离下的平面标准差为1.43mm，比较大，且点云会出现分层现象，导致了手眼标定时会产生误差，点云分割会导致同一个面分割成两个部分，以及点云配准时的不准确等问题。这也是本项目中最大误差产生的来源。我们通过点云滤波（直通滤波，平滑滤波），取平均中心点和法线方向来平均掉误差，并且在控制系统中，让机械臂去在x，y，z方向补偿误差来进行抓取。
图3(a).上为白色背景板
图3(b). 拟合平面图
②手眼标定：（4点SVD分解法）
相机坐标系下获取点云并显示，随机获取四个特征点坐标（如棋盘格的角点），示教模式下控制机器人的末端到达实际特征点所在位置，获取基于机器人坐标系下的四个特征点坐标，两组点具有旋转平移关系，利用SVD分解法进行对两组点进行求解，获得相机与机器人之间固定位置的转换矩阵。
③点云分割：（点云聚类分割求取平面法线及中心点）
本项目中对于任意摆放的长方体，其点云特性为不同深度下具有平面特征，以及在拐角处的法线变化较大，所以在点云分割中采取了区域生长的聚类分割算法，利用了法线，曲率，欧式距离等信息来判断点云是否应该聚成一类。并且使用kd-tree加速检索，取得了比较好的分割效果。
④平面拟合及中心点求取：
在点云分割过程中，选取最多点簇作为分割出的木块平面进行拟合。由于点云存在分层现象，所以采用求取点云重心的方法确定中心点。对于平面拟合则采用最小二乘拟合的方法求解。至此我们就可以确定出在机械臂基体坐标系下木块表面中心点及法向量，便于后续机器人姿态计算。
⑤机器人姿态求解：（X,Y,Z,O,A,T)求解
利用获取的小木块归一化后的法向量与机器人Z轴单位向量（基于地面为（0，0，1））进行叉乘，来获取代表向量角度变换后的罗德里格斯向量，基于罗德里格斯向量获取罗德里格斯矩阵，此时的罗德里格斯矩阵代表六轴机器人绕各向轴旋转之后的最终矩阵（Z-Y-Z方向），逆向求解OAT即可。
2.创新点：
①采用一种快速4点手眼标定法，在没有强度信息下进行手眼标定。利用4点对SVD分解出RT矩阵。
②可以带姿态进行无序抓取。即对任意摆放的物体可以从平面的法线方向进行抓取。
五、实验过程
1.相机输出深度图，转为点云图，并进行多帧，点云滤波等处理（直通滤波，体素滤波等）。
2. 6轴机器人与计算机之间的通信（UDP协议）
3. 相机与6轴机器人计算手眼标定矩阵，这里使用了4对点SVD分解法进行求解。得到视觉系统与机器人本体系统之间的转换矩阵。
4. 制作木块模板，并求出木块本体坐标系下的抓取点和法线信息，保存至文件中。
5. 利用相机拍得目标（木块）点云，得到点云进行点云滤波，并进行下采样（实际中点云较少可以去掉）。
6. 点云分割（这里使用了聚类分割方法，由于木块是都是平面构成，平面内法线连续，距离连续，拐角处法线变化大，欧氏距离大，区域生长的聚类分割效果较好）。
图4.点云分割结果图
7. 点云配准（4PCS+ICP）由于点云精度较差导致匹配结果较差，且不鲁棒，一致性差，所以放弃此方案。
图5.点云配准结果图
8. 转为使用平面拟合及中心点的方案进行抓取。将点云分割出的最多点构成的面进行平面拟合，计算平面中心点和法向量
9. 利用法向量与机器人坐标系之间的关系，求解机器人的抓取姿态（OAT）
10. 输入坐标与姿态信息指令（ X，Y，Z，O，A，T）给机器人实现抓取。
11. 开发板与相机、机械臂进行通信（QT实现）
12. 开发板环境搭建（QT5，OPENCV320，PCL1.8）遇到flann库编译冲突并解决。
13. 开发板代码编译运行（由于开发板内存较小，无法正常运行）
