CN110648361A 一种三维目标物体的实时位姿估计方法及定位抓取系统 （深圳市华汉伟业科技有限公司）

道留仙大道众创产业园B53栋401-406室一种三维目标物体的实时位姿估计方法及一种三维目标物体的实时位姿估计方法及的实时位姿估计方法应用于三维目标物体的定出的位姿信息控制运动机构准确地抓取目标物2采用体素栅格对所述图形信息进行降采样处理，以降低所述图形信息中数据点的数根据所述第二点云数据形成所述目标物体的局部采用体素栅格对所述模型信息进行降采样处理，以降低所述模型信息中数据点的数根据所述第一点云数据形成所述目标物体的全局所述全局特征描述子中的每个特征描述子、所述局部特根据所述全局特征描述子中的各个特征描述子分别建立一基准坐标系，以所述第一特征集Pm中任意第一点对内首个3由此计算得到特征点p1在所述基准坐标系中的平移向表示特征点p1的转置(pzt/pyt)；根据所述局部特征描述子中的每个特征描述子分别形的特征点p,1作为所述基准坐标系的基准点，计算该特征点p,1在所述基准坐标系中的平移标系中获取该第二点对对应的位姿变换矩阵，且表示为根据所述位姿变换矩阵获得该第二点对的旋转平移关系，且将所述位姿变换矩阵加入预设的队列4根据取得最大的评估匹配质量时所对应的第一点对和第二所述模板建立模块用于根据所述目标物体对应的三维模型的模型信息建立三维模型所述识别与定位模块和所述模板建立模块连接，用所述优化模块用于根据所述目标物体的位姿信息规划所述运动机构的运动路线和抓5所述控制模块用于控制所述运动机构按照规划的运动路线和抓取位置对所述目标物67[0007](2)在直接利用深度信息进行零件识别中，基于特征匹配的方法对点云特征的构述全局特征描述子建立所述三维模型数据库；所述全局特征描述子中的每个特征描述子、所述局部特征描述子中的每个特征描述子均为四维的特征向量，用公式表示为F(p1,p2)=8由此计算得到特征点p1在所述基准坐标系中的平移[0013]T＝-Rp1＝-R(px,py,移向量T和旋转矩阵R对第一点对((p1,n1),(p2,n2))内的另一个特征点p2进行基准坐标变换矩阵，且表示为根据所述位姿变换矩阵获得该第二点对对应的旋表9别进行比较，若位姿数据之间的平移距离小于预设的第一值且平移角度小于预设的第二述子时，采用体素栅格和特征阈值的降采样处理方式对目标物体的图形信息进行了处理，的第一点对按照相同或相似特征值划分入不同的哈希表中，且为每一个哈希表设置键值，点对对应的评估匹配质量和旋转平移关系，且利用评估匹配质量来判断位姿估计的准确在实时位姿估计方法中还利用位姿聚类或ICP配准对位姿信息进行修正与优化处理，这种[0029]图4为利用三维模型数据库且通过三维的位姿估计来得到目标物体的位姿信息的根据说明书中的描述以及本领域的一般技术知识即可完了本次执行对哈希表的相似性查找而不是插入，此查找还允许我们计算场景对的位姿变果最高的m个Pcur都投票给了某一旋转角度则[0058]本领域的技术人员可以理解，对于第二点云数据S，例如得知了其中两个特征点[0063]关于三维模型数据库的建立过程可以参考下文的步骤S510-S560，关于三维的位iiminmaxmin[0076]这里对相同索引值的坐标点完成聚类计算，具体可以采用均值聚类或者高斯聚[0082]从而获得距离权重u-c"或者αi＝1/di，所有权重和为坐标输出为[0085]步骤S530，采用特征阈值对降采样处理后的模型信息中的各个即M为匹配的各个模板特征点所构成的集合。另一个方[0094]本实施例中为了便于说明，将根据第一点云数据M得到的所有特征描述子的集合[0095]需要说明的是，在得到目标物体的全局特征描述子之后，可以通过下面的步骤[0096]步骤S550，根据全局特征描述子中的各个特征描述子分别形基准坐标系的X轴方向一致，计算X轴方向夹角θ=arccos(nx)和等效轴角单位方向向量由此计算得到特征点p1在该基准坐标系中的平移向点p2进行基准坐标变换，得到变换后的坐标pt＝(pxt,pyt,pzt)以及对应的旋转角度ψm＝-集中具有相同或相似特征值的第一点对划分为一组且存入同一个哈希表内；(2)为每一个[0110]本领域的技术人员可以理解，在通过步骤S510-S570建立完成三维模型数据库之[0111]步骤S310，根据局部特征描述子中的各个特征描述子分别形2s+q2+q3+q4。′zTzp’[0130](3)根据该第二点对内两个特征点在场景坐标系的坐标得到这两个特征点对应的特征描述子对三维模型数据库中的各个哈希表进行特征匹配以获取所有相匹配的第一点坐标和对应的旋转角度，从而根据该第二点对内两个特征点在场景坐标系中的坐标(以及分别对应的法向量)得到这两个特征点对应的特征描述子，进而与已经建立的三维模型数对应的旋转平移关系，若不相同则从第二特征集Ps中选取另一个第二点对作为参考数据，量超过了预设的阈值，则确定目标物体的位姿，得到目标物体的位姿信息(即实现步骤以再次进行迭代计算，重新计算得到新的旋转平移关系和新的评估匹配质量(即实现步骤-arctan(p9z/p9y)。新计算得到新的旋转平移关系和新的评估匹配质量，然后根据图5再次对评估匹配质量进[0145]步骤S410，通过位姿聚类或ICP配准对位姿信息进行处理，以修正与优化位姿信[0146]其中，通过位姿聚类对位姿信息进行处理可以描述为：(1)根据上文提及的队列点的点群三维数据整合到一个统一的坐标系的过程，迭代最近点法(iterativeclosest模型的位姿给出了当前粗略的位姿，然后采用ICP算法将识别出的模型点云和场景点云进时位姿估计方法主要包括两个实施部分，一部分是建立三维模型数据库所代表的离线阶信息进行点云预处理(包括体素栅格降采样处理和特征阈值筛选处理)，得到第一点云数物体的三维模型数据库，还能够利用该三维模型数据库对目标物体进行实时地位姿估计，(1)根据图形信息计算得到目标物体的局部特征描述子时而实现对模型模板下各个模板特征点的描述；(3)由于利用点云预处理功能对图形信息或处理速度得以加快，利于提高实时位姿估计的算法效率；(4)由于在建立三维模型数据库[0158]控制器13与传感器11和处理器1连接，控制器13用于控制传感器13采集目标物体[0159]在一具体实施例中，见图12，处理器12包括模板建立模块121、识别与定位模块[0160]其中，模板建立模块121用于根据目标物体对应的三维模型的模型信息建立三维信息进行点云预处理(包括体素栅格降采样处理和特征阈值筛选处理)，得到第一点云数三维的图形信息，对图形信息进行点云预处理(包括体素栅格降采样处理和特征阈值筛选一方面控制模块132还输出控制指令来控制传