大规模实时3D重建与料箱抓取系统的物体姿态估计

### 大规模实时3D重建与料箱抓取系统的物体姿态估计 #### 1. 引言 机械臂抓取是机器人自动化领域的重要研究课题，在分拣、装配、装载、焊接和喷涂等方面应用广泛。然而，目标物体空间位置的不确定性、获取点云的低分辨率、重建速度慢、场景复杂以及遮挡等问题，增加了机械臂抓取目标物体的难度，对现有的机械臂分拣技术构成了严峻挑战。 随着TOF、结构光和激光雷达等3D视觉技术的快速发展，3D点云数据更易获取，3D视觉系统的应用领域也越来越广泛。物体识别是机器人和机器视觉中的经典问题，准确确定杂乱和遮挡场景中物体的位置和姿态，吸引了众多研究者的关注。常见的物体识别方法有： - **RANSAC方法**：通过随机采样确定目标模型的内点和外点，可用于识别预定义的几何形状。 - **LINEMOD特征**：融合RGBD和法线信息，能在遮挡和杂乱场景中进行位姿计算，解决无纹理目标的识别问题。 - **3DSC算法**：在关键点处创建3D球形网格，通过计算网格内投票的加权值生成特征向量。 - **FPFH特征描述**：利用相邻点之间的空间特征直方图作为局部表面描述符，通过加权计算和分解数组加速计算过程。 - **SHOT描述符**：结合直方图和识别标记，将邻域划分为块，并在每个块中生成局部直方图。 - **VFH方法**：引入额外的视点方差，将视点方向与表面形状融合，扩展了FPFH方法的思想和特征。 - **CVFH方法**：优化了VFH方法，应用区域生长算法获取稳定的目标区域。 - **ICP匹配**：基于点的位置进行匹配的刚性匹配算法，具有高精度的匹配性能。 - **PPF方法**：结合局部和全局特征的优点构建点对特征，是一种基于模型的点云特征描述符，通过特征堆叠和组合增强点云特征描述能力，并应用霍夫投票方案。 #### 2. 实时结构光重建 ##### 2.1 GPU与并行处理管道 随着结构光投影图像数量的增加，用于重建处理的像素数量也随之增多。本文需要捕获22张图像来重建场景，使用传统CPU串行计算进行大规模点云重建在时间和效率上存在局限性，无法满足工业实时性要求。因此，本文研究了基于CUDA编程的GPU并行加速方法，以实现密集点云处理算法的加速。 与CPU串行计算相比，GPU的并行加速可以显著减少重建算法的运行时间，提高算法在工业应用中的效率和可行性。基于GPU并行化的算法整体流程如下： ```mermaid graph LR classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px A([开始]):::startend --> B(分配内存):::process B --> C(复制数据到GPU):::process C --> D{GPU计算}:::process D --> E(解码核心函数):::process D --> F(坐标计算核心函数):::process D --> G(去噪核心函数):::process E --> H(结果返回CPU):::process F --> H G --> H H --> I(释放内存):::process I --> J([结束]):::startend ``` 所有3D重建算法都在GPU上运行，包括解码核心函数、坐标计算核心函数和去噪核心函数。主函数在CPU平台上运行，包括分配内存、复制数据和释放内存。该3D重建算法和程序具有良好的并行优化性能。 为了确定离群点，本文对3D点云进行空间网格化，根据点所在网格及其周围网格中的点云数量来判断。为了加速GPU上的哈希搜索过程，本文对哈希数组设计进行了并行化优化。除了原始哈希数组外，还创建了四个辅助数组：无重复哈希数组、数据索引数组、起始哈希索引数组和结束哈希索引数组。 ##### 2.2 3D重建算法性能测试 为了验证GPU加速的3D重建算法的效率，本文对不同图像大小的ROI区域进行了点云重建实验，重建的点云数量近似于ROI区域的像素数量。输入图像为11位编码模式，每个位模式包含正负图像，共22张图像。同时，在CPU平台上运行相同的算法进行对比，实验对比结果如下表所示： | ROI | 平台 | 内存复制时间(ms) | 解码时间(ms) | 坐标计算时间(ms) | 总时间(ms) | 加速比 | | ---- | ---- | ---- | ---- | ---- | ---- | ---- | | 600×400 | CPU | \ | 44 | 8.7 | 52.7 | 1.67 | | 600×400 | GPU | 29.5 | 2 | 0.1 | 31.6 | - | | 1200×800 | CPU | \ | 78 | 26 | 104 | 3.16 | | 1200×800 | GPU | 30 | 2.4 | 0.5 | 32.9 | - | | 1920×1200 | CPU | \ | 191 | 134 | 325 | 9.26 | | 1920×1200 | GPU | 29 | 5.2 | 0.9 | 35.1 | - | 实验结果表明，随着重建的ROI区域增大，GPU平台上的算法处理时间变化不大，而CPU平台上的算法处理时间显著增加。当选择较大的图像ROI区域进行3D重建时，处理的像素点越多，GPU平台的性能优势越明显，算法加速比越大。本文的算法在重建大规模点云时效率有显著提升，重建约200万个点云时算法时间为35.1ms，而传统CPU并行算法需要325ms，GPU重建的加速比达到约九倍。 此外，本文使用标准物体测试了结构光系统的重建精度，实验结果表明，所设计和研究的结构光传感器和重建算法的Z轴误差小于0.5mm，能够满足工业3D重建的要求。 #### 3. 基于PPF的物体姿态估计 基于场景的实时3D重建，需要从场景中准确识别和定位待抓取的目标物体。本文提出的目标识别算法过程可分为离线训练和在线匹配两个阶段，具体流程如下： ```mermaid graph LR classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px A([开始]):::startend --> B(离线训练):::process B --> C(加载模型3D CAD数据):::process C --> D(预处理):::process D --> E(PPF特征提取):::process E --> F(创建带加权投票的模型哈希查找表):::process A --> G(在线匹配):::process G --> H(扫描场景点云):::process H --> I(预处理):::process I --> J(PPF特征提取):::process J --> K(哈希表查找):::process K --> L(姿态假设计算):::process L --> M(姿态聚类与平均):::process M --> N(后处理):::process N --> O(ICP姿态细化):::process O --> P([结束]):::startend ``` ##### 3.1 加权霍夫投票方案 在原始PPF算法的离线训练阶段，会提取模型的所有点对特征并存储在哈希表中，创建全局模型点对特征。但这种离线全局模型特征计算会包含场景中未出现的冗余点对特征。为解决这一问题，本文基于不同视角下的模型可见性进行离线训练。物体点云的可见性受观察视角和物体自身形状的影响，本文通过设置虚拟相机从不同视角观察点云来计算点云可见性。 在原始PPF方法中，所有模型特征点被赋予相同的投票值，这并不合理，因为一些模型点包含更多的特征信息，具有更强的判别能力。包含更多信息的点对应该被赋予更多的权重。对于由平面组成的工业物体，原始PPF特征不适合匹配，因为平面物体的自由度不足，平面内的点对具有相同的PPF值，会导致生成的姿态结果出现错误。 为了准确计算模型点对特征，需要降低表面特征的平坦投票权重。定义两点的法线向量$n_1$和$n_2$，基于法线向量一致性的点对特征投票权重$w$定义如下： $w = 1 - \lambda|n_1 \cdot n_2|$ ##### 3.2 后处理 通过姿态聚类和排序方法，可以过滤出最可能的识别结果，姿态获得的投票数可作为姿态结果可信度的评分条件。然而，姿态聚类的投票数可能无法准确代表模型物体与场景的匹配程度，一些错误的姿态可能获得高投票，而正确的姿态获得较低投票。同时，如果对所有姿态进行ICP精细匹配，会导致效率损失。 通常，位置结果的匹配程度通过位置变换后模型点云与现场点云的距离近似度来衡量，但这种基于距离的方法存在局限性，因为点云的匹配不仅包括空间位置的匹配，还包括点云法线向量的对齐。对于随机堆叠的场景，由于遮挡的存在，物体的点云不完整，这会降低距离评分的准确性。 本文提出了一种基于距离和法线向量匹配精度的评估方法。给定场景点云$S$和经过位置变换后的模型点云$M$，对于模型点云$M$中的每个点$m_i$，在场景点云$S$中搜索最近点$s_i$。$m_i$和$s_j$之间的距离$d_i$可表示为： $d_i = \min\{\|m_i - s_j\|, m_i \in M, s_j \in S\}$ 对于一组对应的最近邻点对$c_i = (m_i, s_j)$，$m_i$和$s_j$的法线向量相似度$\eta_i$定义为： $\eta_i = 1 - |n_{m_i} \cdot n_{s_j}|$ 其中，$n_{m_i}$是$m_i$的法线向量，$n_{s_j}$是$s_j$的法线向量。当法线向量$n_{m_i}$和$n_{s_j}$方向相同时，$\eta_i$将达到最小值。 综上所述，对于给定的姿态结果$P \in SE(3)$，其匹配精度得分$f_a$定义为： $f_a = \frac{1}{n} \sum_{i=1}^{n} \lambda d_i + \eta_i d_i$ 其中，$n$是模型点云的数量，$\lambda$是加权因子。姿态结果的精度误差得分$f_a$越小，表明目标识别的精度越高。这种误差评分方法对于有噪声和遮挡的点云数据具有较高的准确性和鲁棒性。在对姿态进行精度误差评分后，可以选择$f_a$值较小的姿态作为匹配结果，进行后续的ICP匹配。 对于散乱堆叠物体的分拣任务，环境中会存在多个物体实例，需要区分不同物体实例的姿态并去除重复姿态。同一待识别物体可能会产生重复姿态，为了避免对所有重复姿态结果进行精细对齐造成资源和时间的浪费，对于位置结果$p_1$中的每个点$m_{1i}$，在位置结果$p_2$中找到最近点$m_{2i}$，并计算它们之间的距离。$p_1$和$p_2$之间的距离定义为： $dis = \frac{1}{n} \sum_{i=1}^{n} \|m_{1i} - m_{2i}\|$ 其中，$n$是模型点云的数量。 #### 4. 实验结果与分析 ##### 4.1 数据集物体识别结果 在数据集上进行物体识别实验，为验证本文方法的有效性，将原始PPF方法与本文的识别方法在识别率和算法时间方面进行对比，结果如下表所示： | 方法 | 识别率 | 算法时间(s) | | ---- | ---- | ---- | | 原始PPF | 98% | 5.67 | | 本文方法 | 98% | 3.62 | | 方法 | 识别率 | 算法时间(s) | | ---- | ---- | ---- | | 原始PPF | 76.1% | 1.51 | | 本文方法 | 82.6% | 0.67 | 从实验结果可以看出，本文方法在识别率和算法时间上都有一定的优势。 ##### 4.2 实际场景重建与识别结果 基于视觉的机械臂抓取系统由结构光传感器、机器人和抓取执行模块组成，系统分为结构光视觉模块、点云处理模块和抓取控制模块。 为验证本文机器人识别和分拣系统的有效性，对不同物体和场景进行分拣实验，实验场景和结果如下表所示： | 物体类型 | 实验次数 | 识别失败次数 | 抓取失败次数 | 识别成功率 | | ---- | ---- | ---- | ---- | ---- | | 三通管 | 63 | 0 | 3 | 100% | | 四通管 | 60 | 0 | 0 | 100% | | 斜三通管 | 62 | 1 | 2 | 98.34% | | 总体成功率 | - | - | - | 99.45% | 实验结果表明，本文提出的实时3D重建和物体姿态估计方法在实际应用中取得了较好的效果，能够满足工业场景的需求。 ### 大规模实时3D重建与料箱抓取系统的物体姿态估计 #### 5. 方法优势总结 - **高效的3D重建**：通过基于CUDA编程的GPU并行加速方法，显著减少了大规模点云重建的时间。与传统CPU串行计算相比，在处理较大图像ROI区域时，GPU平台的性能优势明显，加速比可达约九倍。例如，重建约200万个点云时，GPU算法时间为35.1ms，而CPU需要325ms。 - **精准的物体姿态估计**： - **加权霍夫投票方案**：考虑了模型点云的可见性和点对特征的信息含量，对不同点对赋予不同的投票权重，避免了平面物体匹配时的错误，提高了姿态估计的准确性。 - **后处理优化**：提出基于距离和法线向量匹配精度的评估方法，有效解决了遮挡和噪声环境下点云匹配的问题，同时通过去除重复姿态，提高了算法的效率。 #### 6. 应用场景拓展 本文所提出的实时3D重建和物体姿态估计方法，在多个工业领域具有广泛的应用前景： - **物流分拣**：在物流仓库中，机械臂可以利用该方法快速准确地识别和抓取不同形状和位置的货物，提高分拣效率。 - **智能制造**：在生产线中，能够精确识别零部件的位置和姿态，实现自动化装配，提高生产质量和效率。 - **仓储管理**：可以对仓库中的货物进行实时3D建模和姿态估计，便于库存管理和货物调度。 #### 7. 技术挑战与展望 尽管本文的方法取得了较好的实验结果，但在实际应用中仍面临一些挑战： - **复杂场景适应性**：在极端复杂的场景中，如光照变化剧烈、物体表面材质特殊等情况下，点云数据的质量可能会受到影响，从而降低识别和姿态估计的准确性。 - **实时性要求**：随着工业生产节奏的加快，对系统的实时性要求越来越高，需要进一步优化算法，提高处理速度。 未来的研究可以从以下几个方面展开： - **多传感器融合**：结合多种传感器（如RGB相机、激光雷达等）的数据，提高点云数据的质量和丰富度，增强系统对复杂场景的适应性。 - **深度学习融合**：引入深度学习算法，对3D点云数据进行更深入的特征提取和分析，提高物体识别和姿态估计的准确性和鲁棒性。 #### 8. 总结 本文提出了一种用于料箱抓取系统的实时3D重建和物体姿态估计方法。通过基于GPU的并行加速实现了高效的3D点云重建，同时对PPF算法进行优化，提出加权霍夫投票方案和后处理方法，提高了物体姿态估计的准确性和效率。实验结果表明，该方法在数据集和实际场景中都取得了较好的效果，具有较高的识别率和较低的算法时间。未来，随着技术的不断发展和完善，该方法有望在更多工业领域得到广泛应用。 以下是本文方法的关键步骤总结表格： | 步骤 | 具体内容 | | ---- | ---- | | 3D重建 | 利用GPU并行加速，通过解码、坐标计算和去噪核心函数处理点云数据，对3D点云进行空间网格化并优化哈希数组设计确定离群点。 | | 物体姿态估计 - 离线训练 | 加载模型3D CAD数据，进行预处理、PPF特征提取，创建带加权投票的模型哈希查找表。 | | 物体姿态估计 - 在线匹配 | 扫描场景点云，预处理、PPF特征提取，进行哈希表查找、姿态假设计算、姿态聚类与平均、后处理和ICP姿态细化。 | | 后处理 | 提出基于距离和法线向量匹配精度的评估方法，去除重复姿态。 | 下面是整个系统的简化流程图： ```mermaid graph LR classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px A([开始]):::startend --> B(3D重建):::process B --> C(物体姿态估计):::process C --> D(离线训练):::process C --> E(在线匹配):::process D --> F(创建模型哈希表):::process E --> G(姿态计算与优化):::process G --> H(后处理):::process H --> I(机械臂抓取):::process I --> J([结束]):::startend ``` 通过以上的总结和分析，我们可以看到该方法在工业应用中的潜力和价值，同时也明确了未来的研究方向和挑战。希望本文的研究能够为相关领域的发展提供有益的参考。