mpu9250姿态融合算法_基于投票方式的机器人装配姿态估计

作者：仲夏夜之星
来源：公众号 @3D视觉工坊
链接：基于投票方式的机器人装配姿态估计

论文题目：《Voting-Based Pose Estimation for Robotic Assembly Using a 3D Sensor》

这篇文章被发表在2012年的IEEE International Conference on Robotics and Automation上。

摘要：本文提出了一种基于投票的适用于三维传感器的位姿估计算法，能够快速代替机器人行业、计算机视觉和游戏应用程序中二维相应的算法。最近研究表明在投票框架中，一对有方向的三维点，即物体表面上的点有其对应的法线可以实现快速和鲁棒的位姿估计。尽管定向表面点对于有足够曲率变化的对象具有辨别能力，但对于许多工业和现实生活中大部分平面的对象来说，它们不够紧凑和辨别能力强。由于边缘在二维配准中起关键作用，深度不连续性在三维中起关键作用。在本文中，我们研究和发展了一组姿态估计算法，以更好地利用这一边界信息。除了有方向的表面点，我们使用了另外两个基本元素：有方向的边界点和边界线段。实验表明，这些经过精心选择的原语编码的信息更紧凑，从而为广泛的工业零件提供更高的精度，并使得计算速度更快，本文使用提出的算法和一个3D传感器来演示了一个实用的机器人捡货系统。

一、拣货系统概述

图1

如上图左边所示，展示了捡箱系统的设置。本系统使用一个附在六轴工业机器人手臂上的3D传感器来估计随机放置在箱子里的物体的姿态。3D传感器是基于结构光使用红外激光，并提供三维数据作为像素的深度图。3D传感器相对于机器人手臂进行校准，从而允许使用估计的姿态抓取和挑选对象。右图为算法流程图，本系统使用3D传感器扫描对象的箱子，给定目标物体的三维CAD模型，使用基于投票的算来扫描三维点云对目标物体进行检测和姿态估计，这提供了多个粗的位姿假设。该系统选择几个最优位姿假设，并使用ICP算法的变体对其进行单独的优化。该算法利用当前位姿估计对CAD模型进行渲染，并通过对渲染模型表面的采样生成三维点。然后为模型中的每个三维点计算扫描点云中最近的三维点，并利用三维点对应更新姿态估计。改进后的配准误差由对应场景与模型点之间的平均距离给出，当投票算法计算出的粗位姿不正确，或者由于被其他物体遮挡而丢失部分物体时，配准误差会很大。如果配准误差很小，并且机器人手臂能安全地到达估计的姿态，系统就会抓取目标。

二、投票位姿估计

A.点对特征

1）S2S — Surface-to-Surface

2）B2B — Boundary-to-Boundary

3）S2B — Surface-to-Boundary

4）L2L — Line-to-Line

图2

B.物体表示

C.投票机制

在计算点对特征并构造哈希表结构的基础上，通过计算场景对特征与一组对应的模型对特征之间的刚体变换，找到提出假设，为了提高搜索效率，本文采用了投票方案，利用局部坐标将投票空间缩减为2D空间。首先，在哈希表中搜索场景点对，其中是来自场景的基元集，并找到对应的模型点对, 然后将点对的参考点对准中间坐标系，如图3所示。为了完全对齐对，参考点和，应该通过围绕法线旋转物体来对齐。计算平面旋转角度后，局部坐标由参考模型点对与平面旋转角度定义,到的变换为:

图3

D．L2L点对的投票机制

图4

E．位姿聚类

本文采用的是欧式聚类，首先对原始的pose假设按投票数的降序排序，从最高的投票中创建了一个新的集群。如果下一个位姿假设靠近一个已有的簇，则将该假设添加到簇中，簇中心更新为簇内位姿假设的平均值。如果下一个假设不接近任何一个集群，它就会创建一个新的集群。在平移和旋转过程中，采用固定阈值进行近似测试。平移距离计算和均值化在三维欧几里得空间中进行，旋转距离计算和均值化使用四元数表示。聚类后，对聚类按总票数的递减顺序进行排序，总票数决定了估计姿态的置信度。