CN110992427B - 一种形变物体的三维位姿估计方法及定位抓取系统 - Google Patents

Abstract

一种形变物体的三维位姿估计方法及定位抓取系统，其中位姿估计方法包括：获取形变物体在任意形变状态下三维的场景数据，根据场景数据组建得到形变物体的场景点对集；利用预先建立的三维形变模板数据库获得形变物体的模板点对集，根据场景点对集和模板点对集构建图模型；优化计算获得图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对形变物体进行三维位姿的估计，得到形变物体的位姿信息。技术方案可以将形变物体的三维位姿估计问题转化为图模型的优化计算问题，只要优化计算得到输出矩阵的最大特征值对应的特征向量就可以确定场景内的形变物体转换到模板坐标系内的相对位姿，从而方便得到形变物体的位姿信息。

Description

一种形变物体的三维位姿估计方法及定位抓取系统

技术领域

本发明涉及机器视觉技术领域，具体涉及一种形变物体的三维位姿估计方法及定位抓取系统。

背景技术

在当今的制造业中，装配过程所用的时间、所花费的资金都比较多，为了提高生产效率，降低人力成本，人们开始探索利用机器人实现自动化装配。零件识别与抓取位置规划作为自动化装配过程中不可缺少的重要环节，对装配的质量有至关重要的影响，基于视觉的零件位姿判定与抓取位置规划可以明显提高产品装配的自动化程度、灵活度，减少耗时并降低成本，从而提高生产制造效率。机器人自动化装配涉及两项关键技术：零件识别与自动抓取。面对复杂的作业环境以及产品的多样化需求，从场景中识别需要装配的零件，并完成抓取送达安装工位，如何提升准确性和保证高效性是需要克服的技术难题。在存在噪声、遮挡等复杂场景中能够准确获取目标物体的三维位姿，对于很多工业应用和机器人抓取系统来说，至关重要。目前，很多三维位姿获取方法假设物体是刚性，在外部压力的作用下，物体不发生形变，这种情况下可以基于CAD模板或者三维扫描数据建立模型匹配，对于可形变物体，如何快速并且稳定的地获取物体的姿态，仍是需要解决的问题。对象识别是很多计算机视觉应用的一部分，与处理平面的基于2D图像的计算机视觉相比，计算机视觉处理三维信息，并且对于需要检查或操控的非平面对象和表面尤其重要。

基于计算机视觉的零件识别算法按照处理数据类型的不同，可以分为两类：基于二维图像的识别、基于三维点云信息的识别。物体的三维点云是指其表面轮廓上的采样点集，具有空间坐标信息；对于机械零件而言，其标准CAD模型往往是已知的，则通过匹配目标点云与标准模型点云，即可识别零件，得到两者的旋转平移关系。点云匹配有两类常用方法：基于特征匹配的算法和迭代最近点(ICP)的算法。

目前，匹配方法主要是基于特征和描述子这两类特征，通过使用表面特征或描述子而找到场景中的点与对象上的点之间的对应关系，从而获得相对于模板的位姿关系。表面特征是使用低维度表示物体表面的点与其他点之间的特征信息，通常基于对象表面上所有点来计算表面特征的描述子并且将其存储于数据库中。为了识别场景中的对象，对于场景中的点计算表面特征的描述子，并且使用预先计算出的数据库搜索对应的对象点，一旦找出足够的对应关系，就可以恢复对象的姿态。基于特征的位姿计算方法，主要根据物体的基元信息(如线段、圆等)获取物体表面的表达。很多方法使用几何图元来检测场景中的对象。几何图元是一种简单的几何对象，例如平面、圆柱体或者球体，与自由形式的对象相比，几何图元由于其固有特性而更易于在场景中检测出，有很多现有方法将检测对象分割为几何图元。于是在场景中搜索这些几何图元，并且通过标识场景中的与对象中的图元相似的图元而识别对象的位置和姿态。

然而，通过表面特征的描述子进行对应关系搜索的方法中还存在一些缺点。首先，局部描述子无法区分对象上的各相似表面部分(例如较大的平面小块)，这样的相似部分产生相同或者相似的局部描述子，并且进而导致场景与对象之间的不正确的对应关系，使得不相似的表面特征包括于描述子的初始化中；其次局部描述子的计算过程较慢且无法在实时系统中实现，还需要若干秒的处理时间，影响实时性地位姿识别要求；此外，对于杂波，局部描述子不能正确表示物体中感兴趣的场景部分。

三维匹配目的是获取模型间的稀疏或者全部点的对应关系，对于刚体变换，诸如旋转、平移等方法已经在很多成熟的算法得以应用，然而对于非刚体形变间的对应关系还需要进行进一步地研究。在非刚体三维匹配中，主要的任务是寻找两个模型上的顶点之间的对应关系，这就要求模型具有度量距离的保有性，即在发生诸如刚体下的平移、旋转、非刚体下的弯曲、顶点缺失等情形时具有距离关系的不变性或近似不变性。但是一般情况下，由于数字模型的近似特性，很难保证完全的保有性，即使发生了刚体形变仍可能存在这种问题。

发明内容

有鉴于此，本申请解决的技术问题是如何对形变物体进行三维位姿估计，为解决上述技术问题，本申请提供一种形变物体的三维位姿估计方法及定位抓取系统。

根据第一方面，一种实施例中提供一种形变物体的三维位姿估计方法，包括：获取形变物体在任意形变状态下三维的场景数据，根据所述场景数据组建得到所述形变物体的场景点对集，所述场景点对集包括多个场景点对，所述场景点对由所述场景数据中的任意两个场景点形成；利用预先建立的三维形变模板数据库获得所述形变物体的模板点对集，所述模板点对集包括多个模板点对，每个所述模板点对由所述形变物体在参考状态下的模板数据中的任意两个模板点形成，或者由所述形变物体在形变状态下的形变数据中的任意两个模板点形成；根据所述场景点对集和所述模板点对集构建图模型，所述图模型包括相匹配的所述场景点对和所述模板点对形成的节点以及节点之间的连接边；优化计算获得所述图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息。

所述根据所述场景数据组建得到所述形变物体的场景点对集，包括：根据预设的采样间隔对所述场景数据形成的点云数据进行离散化采样，形成场景点云；所述场景点云包括所述形变物体在当前场景下多个场景点的坐标以及对应的法向量，所述场景点为所述形变物体在任意形变状态下三维表面的数据；将所述场景点云中的各个场景点进行组合，使得任意两个场景点的坐标和各自对应的法向量形成所述场景点对，利用形成的各个所述场景点对组建所述形变物体的场景点对集，且所述场景点对集用公式表示为

P_s＝{((p′₁,n′₁),(p′₂,n′₂)),(p′₁,n′₁)∈S,(p′₂,n′₂)∈S}＝S²；

其中，((p′₁,n′₁),(p′₂,n′₂))＝(s₁,s₂)表示所述场景点对，p′₁、n′₁分别为场景点s₁的坐标和对应的法向量，p′₂、n′₂分别为场景点s₂的坐标和对应的法向量，S为所述场景数据。

所述三维形变模板数据库的建立过程包括：获取所述形变物体对应的三维模型在参考状态下的模板数据，和在各个形变状态下的形变数据；采用体素栅格对所述模板数据进行降采样处理，并且采用特征阈值对降采样处理后所述模板数据中的各个模板点进行筛选，得到模板点云，根据所述模板点云形成所述形变物体在参考状态下的全局特征描述子；所述模板点云包括所述形变物体在参考状态下多个模板点的坐标以及对应的法向量，所述全局特征描述子中的每个特征描述子与所述模板数据中的任意两个模板点形成的模板点对相对应且为四维的特征向量；根据预设的参考点将所述形变数据转换到与所述模板数据相同的坐标系内，在同一坐标系内确定所述形变物体的各个形变状态相对于参考状态的形变变化范围，得到各个形变状态下的形变数据在所述形变变化范围内的模板点，形成所述形变物体在形变状态下的局部特征描述子，所述局部特征描述子中的每个特征描述子与所述形变变化范围内任意两个模板点形成的模板点对相对应且为四维的特征向量；利用所述模板数据中的任意两个模板点形成的模板点对，和所述形变变化范围内任意两个模板点形成的模板点对组建所述形变物体的模板点对集，且所述模板点对集用公式表示为

P_m＝{((p₁,n₁),(p₂,n₂)),(p₁,n₁)∈M,(p₂,n₂)∈M}＝M²；

其中，((p₁,n₁),(p₂,n₂))＝(m₁,m₂)表示所述模板点对，p₁、n₁分别为模板点m₁的坐标和对应的法向量，p₂、n₂分别为场景点m₂的坐标和对应的法向量，M为所述模板数据和所述形变数据；利用所述全局特征描述子和所述局部特征描述子建立数据库，且将所述模板点对集中的各个所述模板点对加入至建立的数据库之中，构建得到所述三维形变模板数据库。

所述利用所述全局特征描述子和所述局部特征描述子建立数据库，且将所述模板点对集合中的各个所述模板点对加入至建立的数据库之中，构建得到所述三维形变模板数据库，包括：建立一数据库且内部生成有哈希表，根据所述全局特征描述子中的每个特征描述子获得相对应的模板点对的特征值，将具有相同或相似特征值的点对划分为一个数据组且存入所述哈希表；根据所述局部特征描述子中每个特征描述子相对应的模板点对的特征值，将该模板点对加入所述哈希表中的对应数据组内，且与对应数据组内已存入点对的特征值相一致；为所述哈希表中的每个数据组设置一个键值，建立该键值和该数据组内所存储的点对之间的映射关系，在已建立的数据库的基础上利用所述哈希表构建得到所述三维形变模板数据库。

所述根据所述场景点对集和所述模板点对集构建图模型，包括：对所述场景点对集中每个所述场景点对内的两个场景点与所述模板点对集中每个所述模板点对内的两个模板点进行对应匹配，利用相匹配的场景点和模板点形成节点且添加至预先设立的稀疏图中；在任意所述场景点对内的两个场景点均在所述稀疏图中形成有节点时，则对所形成的节点进行匹配，利用相匹配的节点在所述稀疏图中生成节点之间的连接边；利用所述稀疏图中的各个节点和所形成节点之间的连接边构建图模型。

所述对所述场景点对集中每个所述场景点对内的两个场景点与所述模板点对集中每个所述模板点对内的两个模板点进行对应匹配，利用相匹配的场景点和模板点形成节点且添加至预先设立的稀疏图中，包括：建立一中间局部坐标系，以所述场景点对集P_s中任意所述场景点对内的一个场景点s作为所述中间局部坐标系的基准点，计算该场景点s在所述中间局部坐标系中的平移向量T和旋转矩阵R；根据所述平移向量T和旋转矩阵R对所述模板点对集P_m中任意所述模板点对内的一个模板点m进行基准坐标变换，使得场景点和模板点之间形成的对应关系；利用变换关系T(s,m,α)＝T(s)^-1R_x(α)T(m)得到变换后的坐标以及对应的旋转角度α，根据所旋转角度α映射为索引坐标的索引值对对应的局部参数(m,α)进行投票计数，得到第一匹配得分，在所述第一匹配得分超过预设的第一阈值时，利用相匹配的场景点和模板点形成节点v，且v＝(m,α,s)，将所形成的节点添加至预先设立的稀疏图中。

所述在任意所述场景点对内的两个场景点均在所述稀疏图中形成有节点时，则对所形成的节点进行匹配，利用相匹配的节点在所述稀疏图中生成节点之间的连接边，包括：若所述场景点对内的两个场景点s₁、s₂分别在所述稀疏图中形成有节点v₁＝(m₁,α₁,s₁)，v₂＝(m₂,α₂,s₂)，则计算对应的旋转角度α₁、α₂之间的差值；根据所述差值映射为索引坐标的索引值对节点v₂进行投票计数，得到第二匹配得分，在所述第二匹配得分超过预设的第二阈值时，利用相匹配的节点v₂在所述稀疏图中生成节点v₁、v₂之间的连接边将利用所述稀疏图中的各个节点和所形成节点之间的连接边构建的图模型用公式表示为G＝(V,E)，其中，V为各个节点的集合，E为节点之间的连接边的集合。

所述优化计算获得所述图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息，包括：根据所述图模型建立关于节点的连接矩阵，以将节点的匹配过程转化为求解矩阵的最大值的优化过程，表示为

其中，N₁为模板点的特征向量个数，N₂为场景点的特征向量个数，||X||_F表示Frobenius范数；i、j均为集合V中节点v的序号，X为节点v之间存在的关系状态，若存在对应关系则X_v＝1，否则X_v＝0；H＝w_i,j且w_i,j为第i个节点和第j个节点之间连接状态；argmax为最大函数值的求参函数，score为分类评分函数，maxscore为最大评分计算函数；初始化矩阵H，利用随机数初始化矩阵向量X，迭代计算X^k+1＝(HX^k)/(|HX^k|)，其中上标k为迭代次数，直至迭代收敛或停止时，输出矩阵H的最大特征值对应的特征向量，由此获得所述图模型中节点的匹配数量最多的连接边；根据优化计算得到的连接边上的节点确定所述场景点对集表示的场景特征在所述模板点对集表示的模板特征中的位置，得到所述形变物体的位姿信息。

根据第二方面，一种实施例中提供一种形变物体的定位抓取系统，包括：传感器，用于采集形变物体的图像，以形成所述形变物体在当前形变状态下三维的场景数据；处理器，与所述传感器连接，用于通过上述第一方面中所述的三维位姿估计方法得到所述形变物体的位姿信息；控制器，与所述传感器和所述处理器连接，用于控制所述传感器采集所述形变物体的图像，以及根据所述处理器输出的位姿信息控制输出运动指令；运动机构，与所述控制器连接，用于根据所述运动指令移动抓取所述形变物体。

所述处理器包括模板建立模板、识别与定位模块；所述模板建立模板用于根据所述形变物体对应的三维模型在参考状态下的模板数据和在各个形变状态下的形变数据建立三维形变模板数据库；所述识别与定位模块和所述模板建立模板连接，用于利用所述三维形变模板数据库以及所述形变物体在任意形变状态下三维的场景数据，对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息。

根据第二方面，一种实施例中提供一种计算机可读存储介质，其特征在于，包括程序，所述程序能够被处理器执行以实现上述第一方面中所述的三维位姿估计方法。

本申请的有益效果是：

依据上述实施例的一种形变物体的三维位姿估计方法及定位抓取系统，其中位姿估计方法包括：获取形变物体在任意形变状态下三维的场景数据，根据场景数据组建得到形变物体的场景点对集；利用预先建立的三维形变模板数据库获得形变物体的模板点对集，根据场景点对集和模板点对集构建图模型，该图模型包括相匹配的场景点对和模板点对形成的节点以及节点之间的连接边；优化计算获得图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对形变物体进行三维位姿的估计，得到形变物体的位姿信息。第一方面，由于形变物体的模板点对集中的每个模板点对由形变物体在参考状态下的模板数据中的任意两个模板点形成，或者由形变物体在形变状态下的形变数据中的任意两个模板点形成，使得模板点对集可以同时表示有形变物体在参考状态和形变状态下的模板特征，为形变物体在任意形变状态下的三维位姿匹配提供了方便；第二方面，由于根据场景点对集和模板点对集构建了图模型，使得图模型包括相匹配的场景点对和模板点对形成的节点以及节点之间的连接边，这样就可以在面对场景特征和模板特征的匹配问题时，将点的刚性对应关系转换为图模型中节点和连接边的柔性对应关系，弥补了刚性对应的不足，为形变物体的三维位姿估计提供可行性；第三方面，由于根据图模型中节点的匹配数量最多的连接边所对应的节点对形变物体进行三维位姿的估计，使得非刚性物体(形变物体)的三维位姿估计问题转化为图模型的优化计算问题，只要优化计算得到输出矩阵的最大特征值对应的特征向量就可以确定场景点对集表示的场景特征在模板点对集表示的模板特征中的位置，从而得到形变物体的位姿信息；第四方面，本申请技术方案根据刚性方法获取物体的初始的最优对应关系，然后将初始的对应关系转化为图模型中的节点，若两个节点的两个点在某个位姿下完全对应则建立两个节点之间的连接边，方便找到图模型中节点的匹配数量最多的连接边，利用依据最多的边连接的姿态作为形变物体的位姿；第五方面，由于利用全局特征描述子和局部特征描述子建立数据库，将模板点对集中的各个模板点对加入至建立的数据库之中，从而得到三维形变模板数据库，如此便于借助数据库内部的哈希表对各个点对进行区别存储和统一表示，如此可以有效地对数据进行分类管理且提高对哈希表的查询效率；第六方面，由于本申请既对三维形变模板数据库的数据存储形态做了改变，也提供了根据图模型进行三维位姿估计的新方法，如此能够提高位姿估计方法的鲁棒性和有效性，为三维非刚性物体的三维位姿检测提供了技术支持；第七方面，本申请将请求保护的三维位姿估计方法应用于形变物体的定位抓取系统之中，能够使得控制器根据处理器输出的位姿信息控制运动机构准确地抓取形变物体，在保证抓取效率的同时还能有效地提高抓取准确度，增强定位抓取系统在应用过程中的实用性能。

附图说明

图1为本申请中三维形变模板数据库的建立方法的流程图；

图2为将模板点对集中的各模板点对加入数据库且利用哈希表构建三维形变模板数据库的流程图；

图3为利用体素栅格进行降采样处理的原理示意图；

图4为利用两个模板点计算特征描述子的原理示意图；

图5为本申请中形变物体的三维位姿估计方法的流程图；

图6为根据场景点对集和模板点对集构建图模型的流程图；

图7为获得图模型中节点的匹配数量最多的连接边以及进行三维位姿的估计得到位姿信息的流程图；

图8为基准坐标变换的原理示意图；

图9为构建图模型的原理示意图；

图10为对形变物体进行特征匹配的示意图；

图11为本申请中形变物体的定位抓取系统的结构示意图；

图12为处理器和控制器的结构示意图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。

本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本申请所说“连接”、“联接”，如无特别说明，均包括直接和间接连接(联接)。

本申请技术方案的发明构思是：将非刚性物体(即形变物体)的三维位姿估计转化为基于图模型的优化计算问题，主要分为两个阶段来完成。在离线阶段中完成三维形变模板数据库的构建过程，在在线阶段中完成场景与模板位姿的估计过程。离线阶段中，用户需要提供形变物体的模板数据和形变数据，比如形变物体的原始形状和几何形变形状，而不需要颜色、纹理或表面反射信息，如此可以适合于广泛的对象类型，由此建立得到的三维形变模板数据库可以稍后用于匹配对象。在线阶段中，用户仅需提供形变物体在某一个形变状态下的场景数据，在所建立的三维形变模板数据库中对场景点进行搜索，返回数据库中具有最多匹配的姿态。在基于图模型的优化计算时，首先根据刚性方法获取形变物体的初始最优对应关系，然后将初始最优对应关系转化为图模型中的节点，如果两个节点对应场景点和模板点在某个位姿下完全对应，则建立两个节点的连接关系，得到两个节点的连接边，从而可以在后续处理过程中通过优化图模型来得到节点匹配数量最多的连接边以及对应的姿态，进而将确定的姿态作为形变物体的位姿。

实施例一、

请参考图1，本申请公开一种三维形变模板数据库的建立方法，这里的三维形变模板数据库用于形变物体的三维位姿估计，该建立方法包括步骤S110-S160，下面分别说明。

步骤S110，获取形变物体对应的三维模型在参考状态下的模板数据，和在各个形变状态下的形变数据。

需要说明的是，形变物体的参考状态可以是物体在原始情形下表面无形变的状态，而形变状态是参考状态之外的状态。对于形变物体的三维模型，可以通过多种方式来获取该三维模型的模板信息或者形变数据，例如直接导入形变物体的建模模型(如CAD模型、ProE模型)，从而得到三维模型的模板信息，对建模模型作出外形改变时就可以得到三维模型的形变数据；也可以通过3D扫描仪来扫描得到三维模型的模板数据或形变数据。由于三维模型往往是整体结构的数字形式表示，所以这里的三维模型的模板数据是形变物体表面的全部的外观形状数据，三维模型的形变数据是形变物体表面的部分或全部的外观形状数据。

步骤S120，采用体素栅格对模板数据进行降采样处理，并且采用特征阈值对降采样处理后模板数据中的各个模板点进行筛选，得到模板点云；这里的模板点云包括形变物体在参考状态下多个模板点的坐标以及对应的法向量。

体素栅格的降采样处理是点云滤波中的常用方法，往往作为点云预处理中的一个重要措施。由于模板数据中具有大量的数据点，包括离散点和稠密稀疏点，为了缩短计算时间，故采用体素栅格降采样处理减少点云中点的数量。见图3，体素栅格降采样处理生成一个单位尺寸的3D体素栅格(可把体素栅格想象为微小的空间三维立方体的集合)，将数据对应储存到每个体素(即三维立方体)中，然后在每个体素内，用体素中所有点的重心来近似显示体素中其他点，这样该体素就内所有点就用一个重心点最终表示，从而模板数据被这些点云中的中心点所替换。原始的模板数据被体素栅格降采样处理之后，虽然点的数量大规模减少，但数据所表示的点云形状却没有发生改变，从而保证在位姿估计时丝毫不降低位姿估计的精度。

那么，可以认为滤波后的数据就是模板点云，其中包括形变物体在参考状态下多个模板点的坐标以及对应的法向量，任意一个模板点的坐标和对应的法向量可以记作其中/>

需要说明的是，模板点云实际上就是形变物体在参考状态下各个模板点的坐标以及对应法向量的集合。这里可以定义模板点云为M，假设每个坐标点的单位法向量位于单位球上，则即M为匹配的各个模板点所构成的集合。另一个方面，可以认为M由一系列的K构成，/>(表示一系列3D坐标点及其法向量)，则M又可表示为/>

步骤S130，根据模板点云形成形变物体在参考状态下的全局特征描述子；该全局特征描述子中的每个特征描述子与模板数据中的任意两个模板点形成的模板点对相对应且为四维的特征向量。

本领域的技术人员可以理解，对于模板点云M，例如图4，得知其中两点p₁、p₂及分别对应的法向量n₁、n₂，那么在直角坐标系中可以定义这两点的特征描述子为

F(p₁,p₂)＝(||d||,∠(n₁,d),∠(n₂,d),det(n₁,n₂,d)/||d||)

其中，d表示p₁到p₂的向量，||d||表示两点p₁和p₂之间的距离；∠(a,b)∈[0,π]表示两个向量之间的夹角(其中a、b表示计算过程的参数)，例如∠(n₁,d)∈[0,π]表示n₁和d之间的夹角，∠(n₂,d)∈[0,π]表示n₂和d之间的夹角；det(n₁,n₂,d)表示向量的行列式的值，相当于表征三个向量形成的六面体的体积。这里的p₁、p₂均为模板坐标系中的模板点。

本实施例中为了便于说明，将根据模板点云M得到的所有特征描述子的集合称之为全局特征描述子，因为这些所有的特征描述子代表了形变物体在参考状态下表面的全部无形变外观形状的特征。

在本实施例中，为便于形成特征描述子，优选地采用直角坐标系中表示的特征描述子，具体的形成过程可以参考图4。然而，在其他情况下还可以在球坐标系或者柱坐标系中定义两点的特征描述子，在球坐标系中表示为

F(p₁,p₂)＝[2r sin(α/2),(π-α)/2,(π+α)/2,α]

其中，α表示球投影面上p₁和p₂对应的法向量的夹角。

在柱坐标系中表示为

F(p₁,p₂)＝[|h|,∠(n₁,h),π-∠(n₁,h),α]

其中，n₁＝(1,0,0)^T，h＝(r(1-cosα),r sinα,l)^T，α表示柱投影面上p₁和p₂对应的法向量的夹角。

步骤S140，根据预设的参考点将形变数据转换到与模板数据相同的坐标系内，在同一坐标系内确定形变物体的各个形变状态相对于参考状态的形变变化范围，得到各个形变状态下的形变数据在形变变化范围内的模板点，形成形变物体在形变状态下的局部特征描述子；这里的局部特征描述子中的每个特征描述子与形变变化范围内任意两个模板点形成的模板点对相对应且为四维的特征向量。

需要说明的是，可以将形变数据转换到与模板数据相同的模板坐标系内，从而在模板坐标系内确定形变物体的各个形变状态相对于参考状态的形变变化范围，这里的形变变化范围可以是模板点在形变前后距离的变化范围，这里确定形变变化范围的目的是减少三维形变模板数据库中的数据量，以及减少后续的计算量和匹配时间。例如，任意模板点在参考状态下的形变距离为0mm，第一次形变状态下的形变距离为2mm，第二次形变状态下的形变距离为4mm，那么可以确定2～4mm为形变变化范围，从而利用形变距离在形变变化范围内对应的各个模板点形成局部特征描述子。

例如，对于形变状态1、形变状态2、…、形变状态n分别在形变变化范围内的模板点构成的模板M₁、M₂……M_n，将参考状态下模板点m_i对应的第k个模板中模板点(或者认为是形变点)记为那么就可以得到任意形变状态下的模板点对(m_i,m_j)∈M²，进而可以得到形成局部特征描述子所需的点对集合，表示为

从而根据D(m_i,m_j)计算得到局部特征描述子中每个特征描述子的公式表现形式，具体可以参考步骤S130中的特征描述子F(p₁,p₂)，这里不再进行赘述。

本实施例中为了便于说明，将根据形变变化范围内的模板点得到的所有特征描述子的集合称之为局部特征描述子，因为这些所有的特征描述子代表了形变物体在形变状态下表面的部分形变外观形状的特征。

步骤S150，利用模板数据中的任意两个模板点形成的模板点对，和形变变化范围内任意两个模板点形成的模板点对组建形变物体的模板点对集，且模板点对集用公式表示为

P_m＝{((p₁,n₁),(p₂,n₂)),(p₁,n₁)∈M,(p₂,n₂)∈M}＝M²；

其中，((p₁,n₁),(p₂,n₂))＝(m₁,m₂)表示模板点对，p₁、n₁分别为模板点m₁的坐标和对应的法向量，p₂、n₂分别为场景点m₂的坐标和对应的法向量，M为模板数据和形变数据的集合。

步骤S160，利用全局特征描述子和局部特征描述子建立数据库，且将模板点对集中的各个模板点对加入至建立的数据库之中，构建得到三维形变模板数据库。

在一个具体实施例中，参见图2，步骤S160可以包括步骤S161-S163，分别说明如下。

步骤S161，建立一数据库且内部生成有哈希表，根据全局特征描述子中的每个特征描述子获得相对应的模板点对的特征值，将具有相同或相似特征值的点对划分为一个数据组且存入哈希表。

步骤S162，根据局部特征描述子中每个特征描述子相对应的模板点对的特征值，将该模板点对加入哈希表中的对应数据组内，且与对应数据组内已存入点对的特征值相一致。

步骤S163，为哈希表中的每个数据组设置一个键值，建立该键值和该数据组内所存储的点对之间的映射关系，在已建立的数据库的基础上利用哈希表构建得到三维形变模板数据库。

在本实施例中，哈希表主要由两部分构成(key,data)，其中key为键值，data为数据，也就是把相同或者相似特征值的数据点设置为相同的键值，那么就可以依据键值快速地在哈希表中找到具有同一类特征的模板点。此外，可以采用MurHash函数来建立特征描述子F到键值的映射关系，这里不再进行详细说明。

实施例二、

请参考图5，在实施例一中公开的三维形变模板数据库的基础上，本申请公开一种形变物体的三维位姿估计方法，其包括步骤S210-S240，下面分别说明。

步骤S210，获取形变物体在任意形变状态下三维的场景数据，根据场景数据组建得到形变物体的场景点对集；这里的场景点对集包括多个场景点对，场景点对由场景数据中的任意两个场景点形成。

在一个具体实施例中，在得到形变物体在任意形变状态下三维的场景数据之后，通过以下具体的步骤组建形变物体的场景点对集：

(1)根据预设的采样间隔对场景数据形成的点云数据进行离散化采样，形成场景点云；这里的场景点云包括形变物体在当前场景下多个场景点的坐标以及对应的法向量，场景点为形变物体在任意形变状态下三维表面的数据。

通常，离散化采样包括间隔采样、数值量化、数据编码的处理，由于各个处理方式均属于现有技术，所以这里不再对离散化采样进行详细说明。

需要说明的是，场景点云实际上就是形变物体所在场景下各个场景点的坐标以及对应法向量的集合。对于3D场景的位姿定位情形，可以定义场景点云为S，假设每个坐标点的单位法向量位于单位球上，则即S为匹配的各个场景点所构成的集合。另一个方面，可以认为S由一系列的K构成，/>(表示一系列3D坐标点及其法向量)，则S又可表示为S＝{s_i,i＝1,…,|S|},/>此外，对于场景点云中的每个场景点，其具有坐标以及对应的法向量，那么场景点可以记作/>

(2)将场景点云中的各个场景点进行组合，使得任意两个场景点的坐标和各自对应的法向量形成场景点对，利用形成的各个场景点对组建形变物体的场景点对集，且场景点对集用公式表示为

P_s＝{((p′₁,n′₁),(p′₂,n′₂)),(p′₁,n′₁)∈S,(p′₂,n′₂)∈S}＝S²；

其中，((p′₁,n′₁),(p′₂,n′₂))＝(s₁,s₂)表示场景点对，p′₁、n′₁分别为场景点s₁的坐标和对应的法向量，p′₂、n′₂分别为场景点s₂的坐标和对应的法向量，S为场景数据。

在本实施例中，由场景点或者模板点所形成的点对在公式构造上是一样的，为了以示区分且便于说明，为场景点加注了上标“′”。

步骤S220，利用预先建立的三维形变模板数据库获得形变物体的模板点对集；这里的模板点对集包括多个模板点对，每个模板点对由形变物体在参考状态下的模板数据中的任意两个模板点形成，或者由形变物体在形变状态下的形变数据中的任意两个模板点形成。

根据实施例一中的步骤S150和步骤S160可知，三维形变模板数据库中按照特征值存储有模板点对集P_m中的各个模板点对，那么可以方便地从三维形变模板数据库中获取任意一个模板点对，从而获得形变物体的模板点对集。

步骤S230，根据场景点对集和模板点对集构建图模型，这里的图模型包括相匹配的场景点对和模板点对形成的节点以及节点之间的连接边。

需要说明的是，图模型表示的是场景点和模板点之间的对应关系，图模型是由顶点集合(Vertex)及顶点间的关系集合(Edge)组成的一种数据结构G＝(V,E)，其中，V＝{x|x∈数据对象}是顶点的有穷非空集合(即顶点集，不可以为空)，E＝{(x,y)|x,y∈V}是顶点之间的关系的有穷集合(即边集，可以为空)。在本实施例中，可以利用相匹配的场景点对中的场景点和模板点对中的模板点来形成节点，从而使用节点根据同样的数据结构来构建图模型，那么节点v∈V，连接边e＝(v₁,v₂)∈E。

需要说明的是，图模型包括相匹配的场景点对和模板点对形成的节点以及节点之间的连接边，这样就可以在面对场景特征和模板特征的匹配问题时，将点的刚性对应关系转换为图模型中节点和连接边的柔性对应关系，弥补了刚性对应的不足，为形变物体的三维位姿估计提供可行性。

步骤S240，优化计算获得图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对形变物体进行三维位姿的估计，得到形变物体的位姿信息。

需要说明的是，利用图模型中节点的匹配数量最多的连接边所对应的节点对形变物体进行三维位姿的估计时，使得形变物体的三维位姿估计问题转化为图模型的优化计算问题，只要优化计算得到输出矩阵的最大特征值对应的特征向量就可以确定场景点对集表示的场景特征在模板点对集表示的模板特征中的位置，从而得到形变物体的位姿信息。

在本实施中，参见图6，上述的步骤S230涉及图模型的构建过程，具体可以包括步骤S231-S233，分别说明如下。

步骤S231，对场景点对集中每个场景点对内的两个场景点与模板点对集中每个模板点对内的两个模板点进行对应匹配，利用相匹配的场景点和模板点形成节点且添加至预先设立的稀疏图中。

在一个具体实施例中，该步骤S231可以具体包括以下过程：

(1)建立一中间局部坐标系，参见图8，以场景点对集P_s中任意场景点对内的一个场景点s作为中间局部坐标系的基准点，计算该场景点s在中间局部坐标系中的平移向量T和旋转矩阵R。例如，将场景点s转换至该中间局部坐标系的原点，将对应的法向量n₁′的方向转换至与该中间局部坐标系的x轴方向一致，计算x轴方向夹角θ＝arccos(n_x)和等效轴角单位方向向量由此计算得到场景点s在该中间局部坐标系中的平移向量T和旋转矩阵R，分别表示为

T＝-Rp₁＝-R(p_x,p_y,p_z)^T；

其中，cθ表示cosθ，sθ表示sinθ，(p_x,p_y,p_z)^T表示场景点s的转置运算。

(2)根据平移向量T和旋转矩阵R对模板点对集P_m中任意模板点对内的一个模板点m进行基准坐标变换，使得相同特征在同一坐标系中的位姿通过旋转和平移变换可以重合，由此得到T_s→ls_i＝R(α)T_m→lm_i，T_m→lm_i＝R(α)T_m→ls_i，下标i表示序号，从而得到场景点和模板点之间形成的对应关系。

需要说明的是，参见图8，对于场景点s_r，T_s→l是将s_r平移到原点并且将s_r的法线旋转到x轴上(指向正方向)的刚性3D变换；对于模型点m_r，T_m→l是将m_r平移到原点并且将m_r的法线旋转到x轴上(指向正方向)的刚性3D变换；T^—1表示逆变换的意思。对于旋转角度α，R_x(α)是以角度α绕着x轴旋转的刚性3D变换。

(3)利用变换关系T(s,m,α)＝T(s)^-1R_x(α)T(m)得到变换后的坐标以及对应的旋转角度α，根据所旋转角度α映射为索引坐标的索引值对对应的局部参数(m,α)进行投票计数，得到第一匹配得分，在第一匹配得分超过预设的第一阈值时，利用相匹配的场景点和模板点形成节点v，且v＝(m,α,s)，将所形成的节点添加至预先设立的稀疏图中。这里的投票计数可以简单表述为：若一个索引坐标的索引值与其它的相同则对该索引值的索引计数+1，若不相同则对新的索引值的索引计数+1，那么就可以根据投票计数结果得到关于(m,α)的第一匹配得分。

利用相匹配的场景点和模板点形成节点v，且添加入稀疏图的过程可以参见图9，若针对场景点s₁和模板点m₁得到局部参数(m₁,α₁)，其第一匹配得分大于第一阈值(如5)则认为场景点s₁和模板点m₁相匹配，就可以将形成的节点v₁＝(m₁,α₁,s₁)添加到稀疏图中。同理，若针对场景点s₂和模板点m₂得到局部参数(m₂,α₂)，其第一匹配得分大于第一阈值则认为场景点s₂模板点m₂匹配，就可以将形成的节点v₂＝(m₂,α₂,s₂)添加到稀疏图中。

需要说明的是，投票匹配策略的结果是一系列3D位姿，同时包含关联着投票数的目标类型，为了提高位姿估计结果的精度，所有的3D位姿都聚合在一起以便在一个聚类中的所有位姿都彼此相似。那么，一个位姿聚类的分数是在一个聚类中所有位姿投票数的平均值，当投票策略匹配上该位姿聚类时，这些投票数则会增加，最后辨识出具有最大投票数的位姿聚类，对聚类中的位姿求平均值，从而得到最终位姿结果。位姿聚类的方法提高了采用基于投票匹配策略得到的3D位姿估计的精度，对于聚类后结果也可以采用求加权平均的方法，权重可以采用相似度作为度量。

步骤S232，在任意场景点对内的两个场景点均在稀疏图中形成有节点时，则对所形成的节点进行匹配，利用相匹配的节点在稀疏图中生成节点之间的连接边。

例如图9，若场景点对内的两个场景点s₁、s₂分别在稀疏图中形成有节点v₁＝(m₁,α₁,s₁)，v₂＝(m₂,α₂,s₂)，则计算对应的旋转角度α₁、α₂之间的差值；根据该差值映射为索引坐标的索引值对节点v₂进行投票计数，得到第二匹配得分，在第二匹配得分超过预设的第二阈值(比如3)时，利用相匹配的节点v₂在稀疏图中生成节点v₁、v₂之间的连接边

在本实施例中，将利用稀疏图中的各个节点和所形成节点之间的连接边构建的图模型用公式表示为G＝(V,E)，其中，V为各个节点的集合，E为节点之间的连接边的集合。

需要说明的是，为方便完成三维位姿的估计，可以设定旋转角度α的离散化步长为δ_α，那么V可以参数化为节点的总个数为

步骤S233，利用稀疏图中的各个节点和所形成节点之间的连接边构建图模型。

在本实施中，参见图7，上述的步骤S240涉及根据图模型对形变物体进行三维位姿的估计且得到形变物体的位姿信息的过程，可以具体包括步骤S241-S243，分别说明如下。

步骤S241，根据图模型建立关于节点的连接矩阵，以将节点的匹配过程转化为求解矩阵的最大值的优化过程，表示为

其中，N₁为模板点的特征向量个数，N₂为场景点的特征向量个数，||X_F表示Frobenius范数；i、j均为集合V中节点v的序号，X为节点v之间存在的关系状态，若存在对应关系则X_v＝1，否则X_v＝0；H＝w_i,j且为矩阵，w_i,j为第i个节点和第j个节点之间连接状态，可以用1、0分别表示连接和非连接状态下矩阵的数值；argmax为最大函数值的求参函数，score为分类评分函数，max score为最大评分计算函数；e为集合E内节点之间的关系表示，如e＝(v_i,v_j)。

步骤S242，初始化矩阵H，利用随机数初始化矩阵向量X，迭代计算X^k+1＝(HX^k)/(|HX^k|)，其中上标k为迭代次数，直至迭代收敛或停止时，输出矩阵H的最大特征值对应的特征向量，由此获得图模型中节点的匹配数量最多的连接边。

步骤S243，根据优化计算得到的连接边上的节点确定场景点对集表示的场景特征在模板点对集表示的模板特征中的位置，得到形变物体的位姿信息。

在一具体实施例中，获得图模型中节点的匹配数量最多的连接边，就可以确定该连接边上的节点(比如v₁和v₂)，从而得到场景点对(s₁,s₂)和模板点对(m₁,m₂)，以及对应的旋转角度α₁和α₂，如此便可以认定场景点对(s₁,s₂)和模板点对之间形成特征相互对应，进而确定场景点对集表示的场景特征在模板点对集表示的模板特征中的位置，以确定形变物体在中间局部坐标系的位姿；根据形变物体中间局部坐标系的位姿，得到形变物体在模板坐标系中的位姿信息，这里的模板坐标系为描述模板点时所使用的坐标系。

例如图10中示意的对形变物体进行特征匹配的过程，根据图模型可以优化计算得到场景点和模板点之间的准确匹配关系，能够适应形变物体的非刚性变化要求。这种通过节点之间的连接边的关系所建立出来的形变物体的一致性变化关系，很好地描述了形变物体的变化对应关系。

本领域的技术人员可以理解，本实施例中公开的一种形变物体的三维位姿估计方法主要包括两个实施部分，一部分是建立三维形变模板数据库所代表的离线阶段，另一部分是对形变物体进行三维位姿估计所代表的在线阶段。

离线阶段的过程可以描述为：获取形变物体对应的三维模型在参考状态下的模板数据，和在各个形变状态下的形变数据；采用体素栅格对模板数据进行降采样处理，并且采用特征阈值对降采样处理后模板数据中的各个模板点进行筛选，得到模板点云，根据模板点云形成形变物体在参考状态下的全局特征描述子；根据预设的参考点将形变数据转换到与模板数据相同的坐标系内，在同一坐标系内确定形变物体的各个形变状态相对于参考状态的形变变化范围，得到各个形变状态下的形变数据在形变变化范围内的模板点，形成形变物体在形变状态下的局部特征描述子；利用模板数据中的任意两个模板点形成的模板点对，和形变变化范围内任意两个模板点形成的模板点对组建形变物体的模板点对集；利用全局特征描述子和局部特征描述子建立数据库，且将模板点对集中的各个模板点对加入至建立的数据库之中，构建得到三维形变模板数据库。关于离线阶段的具体描述可以参考实施例一中的步骤S110-S160，这里不再进行赘述。

在线阶段的过程可以描述为：获取形变物体在任意形变状态下三维的场景数据，根据场景数据组建得到形变物体的场景点对集；利用预先建立的三维形变模板数据库获得形变物体的模板点对集；根据场景点对集和模板点对集构建图模型；优化计算获得图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对形变物体进行三维位姿的估计，得到形变物体的位姿信息。关于在线阶段的具体描述可以参考实施例二中的步骤S210-S240，这里不再进行赘述。

本领域的技术人员可以理解，通过上面的离线阶段和在线阶段不仅可以建立针对形变物体的三维形变模板数据库，还能够利用该三维形变模板数据库对形变物体进行三维位姿的估计，从而输出高准确度的位姿信息。总体来说，本申请中请求保护的三维位姿识别方法具有一些优势：(1)形变物体的模板点对集中的每个模板点对由形变物体在参考状态下的模板数据中的任意两个模板点形成，或者由形变物体在形变状态下的形变数据中的任意两个模板点形成，使得模板点对集可以同时表示有形变物体在参考状态和形变状态下的模板特征，为形变物体在任意形变状态下的三维位姿匹配提供了方便；(2)根据场景点对集和模板点对集构建了图模型，使得图模型包括相匹配的场景点对和模板点对形成的节点以及节点之间的连接边，这样就可以在面对场景特征和模板特征的匹配问题时，将点的刚性对应关系转换为图模型中节点和连接边的柔性对应关系，弥补了刚性对应的不足，为形变物体的三维位姿估计提供可行性；(3)根据图模型中节点的匹配数量最多的连接边所对应的节点对形变物体进行三维位姿的估计，使得形变物体的三维位姿估计问题转化为图模型的优化计算问题，只要优化计算得到输出矩阵的最大特征值对应的特征向量就可以确定场景点对集表示的场景特征在模板点对集表示的模板特征中的位置，从而得到形变物体的位姿信息；(4)根据刚性方法获取物体的初始的最优对应关系，然后将初始的对应关系转化为图模型中的节点，若两个节点的两个点在某个位姿下完全对应则建立两个节点之间的连接边，方便找到图模型中节点的匹配数量最多的连接边，利用依据最多的边连接的姿态作为形变物体的位姿；(5)利用全局特征描述子和局部特征描述子建立数据库，将模板点对集中的各个模板点对加入至建立的数据库之中，从而得到三维形变模板数据库，如此便于借助数据库内部的哈希表对各个点对进行区别存储和统一表示，如此可以有效地对数据进行分类管理且提高对哈希表的查询效率；(6)本申请既对三维形变模板数据库的数据存储形态做了改变，也提供了根据图模型进行三维位姿估计的新方法，如此能够提高位姿估计方法的鲁棒性和有效性，为三维非刚性物体的三维位姿检测提供了技术支持。

实施例三、

请参考图11，本申请公开一种形变物体的定位抓取系统，其包括传感器31、处理器32、控制器33和运动机构34。

传感器31用于采集形变物体的图像，以形成形变物体在当前形变状态下三维的场景数据。这里的传感器31可以是一些具有图像采集功能的视觉传感器，如摄像设备、激光扫描设备。这里的形变物体可以是工业流水线上的产品、物件箱内的机械零件、操作台上的工具等，其不必是刚性非形变物件，可以是具有轻微形变特性的物件，不做具体限定。

处理器32与传感器31连接，用于通过实施例二中公开的三维位姿估计方法得到形变物体的位姿信息；

控制器33与传感器31和处理器32连接，用于控制传感器31采集形变物体的图像，比如可设置传感器31的图像采集时间、采集间隔、采集位置等参数。此外，控制器33可以根据处理器32输出的位姿信息控制输出运动指令。

运动机构34与控制器33连接，用于根据运动指令移动抓取形变物体。

在一个具体实施例中，参见图12，处理器32包括模板建立模板321、识别与定位模块322。

其中，模板建立模板321用于根据形变物体对应的三维模型在参考状态下的模板数据和在各个形变状态下的形变数据建立三维形变模板数据库。具体包括：获取形变物体对应的三维模型在参考状态下的模板数据，和在各个形变状态下的形变数据；采用体素栅格对模板数据进行降采样处理，并且采用特征阈值对降采样处理后模板数据中的各个模板点进行筛选，得到模板点云，根据模板点云形成形变物体在参考状态下的全局特征描述子；根据预设的参考点将形变数据转换到与模板数据相同的坐标系内，在同一坐标系内确定形变物体的各个形变状态相对于参考状态的形变变化范围，得到各个形变状态下的形变数据在形变变化范围内的模板点，形成形变物体在形变状态下的局部特征描述子；利用模板数据中的任意两个模板点形成的模板点对，和形变变化范围内任意两个模板点形成的模板点对组建形变物体的模板点对集；利用全局特征描述子和局部特征描述子建立数据库，且将模板点对集中的各个模板点对加入至建立的数据库之中，构建得到三维形变模板数据库。

识别与定位模块322和模板建立模板321连接，用于利用三维形变模板数据库以及形变物体在任意形变状态下三维的场景数据，对形变物体进行三维位姿的估计，得到形变物体的位姿信息。具体包括：获取形变物体在任意形变状态下三维的场景数据，根据场景数据组建得到形变物体的场景点对集；利用预先建立的三维形变模板数据库获得形变物体的模板点对集；根据场景点对集和模板点对集构建图模型；优化计算获得图模型中节点的匹配数量最多的连接边，根据该连接边对应的节点对形变物体进行三维位姿的估计，得到形变物体的位姿信息。

在一具体实施例中，参见图12，控制器33包括优化模块331和控制模块332。其中，优化模块331和处理器32中的识别与定位模块322连接，用于根据形变物体相对于传感器31的位姿信息规划运动机构34的运动路线和抓取位置；控制模块332与优化模块331连接，用于输出控制指令，一方面控制运动机构14按照规划的运动路线和抓取位置对形变物体进行抓取，另一方面控制模块332还输出控制指令来控制传感器31采集形变物体的图像。

本领域的技术人员可以理解，本实施例中公开的定位抓取系统能够使得控制器根据处理器输出的位姿信息控制运动机构准确地抓取形变物体，在保证抓取效率的同时还能有效地提高抓取准确度，增强定位抓取系统在应用过程中的实用性能。

本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (11)

1.一种形变物体的三维位姿估计方法，其特征在于，包括：

获取形变物体在任意形变状态下三维的场景数据，根据所述场景数据组建得到所述形变物体的场景点对集，所述场景点对集包括多个场景点对，所述场景点对由所述场景数据中的任意两个场景点形成；

利用预先建立的三维形变模板数据库获得所述形变物体的模板点对集，所述模板点对集包括多个模板点对，每个所述模板点对由所述形变物体在参考状态下的模板数据中的任意两个模板点形成，或者由所述形变物体在形变状态下的形变数据中的任意两个模板点形成；

根据所述场景点对集和所述模板点对集构建图模型，所述图模型包括相匹配的所述场景点对和所述模板点对形成的节点以及节点之间的连接边；

优化计算获得所述图模型中节点的匹配数量最多的连接边以使得所述形变物体的三维位姿估计问题转化为所述图模型的优化计算问题，根据该连接边对应的节点对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息。

2.如权利要求1所述的三维位姿估计方法，其特征在于，所述根据所述场景数据组建得到所述形变物体的场景点对集，包括：

根据预设的采样间隔对所述场景数据形成的点云数据进行离散化采样，形成场景点云；所述场景点云包括所述形变物体在当前场景下多个场景点的坐标以及对应的法向量，所述场景点为所述形变物体在任意形变状态下三维表面的数据；

将所述场景点云中的各个场景点进行组合，使得任意两个场景点的坐标和各自对应的法向量形成所述场景点对，利用形成的各个所述场景点对组建所述形变物体的场景点对集，且所述场景点对集用公式表示为

P_s＝{((p′₁,n′₁),(p′₂,n′₂)),(p′₁,n′₁)∈S,(p′₂,n′₂)∈S}＝S²；

其中，((p′₁,n′₁),(p′₂,n′₂))＝(s₁,s₂)表示所述场景点对，p′₁、n′₁分别为场景点s₁的坐标和对应的法向量，p′₂、n′₂分别为场景点s₂的坐标和对应的法向量，S为所述场景数据。

3.如权利要求2所述的三维位姿估计方法，其特征在于，所述三维形变模板数据库的建立过程包括：

获取所述形变物体对应的三维模型在参考状态下的模板数据，和在各个形变状态下的形变数据；

采用体素栅格对所述模板数据进行降采样处理，并且采用特征阈值对降采样处理后所述模板数据中的各个模板点进行筛选，得到模板点云，根据所述模板点云形成所述形变物体在参考状态下的全局特征描述子；所述模板点云包括所述形变物体在参考状态下多个模板点的坐标以及对应的法向量，所述全局特征描述子中的每个特征描述子与所述模板数据中的任意两个模板点形成的模板点对相对应且为四维的特征向量；

根据预设的参考点将所述形变数据转换到与所述模板数据相同的坐标系内，在同一坐标系内确定所述形变物体的各个形变状态相对于参考状态的形变变化范围，得到各个形变状态下的形变数据在所述形变变化范围内的模板点，形成所述形变物体在形变状态下的局部特征描述子，所述局部特征描述子中的每个特征描述子与所述形变变化范围内任意两个模板点形成的模板点对相对应且为四维的特征向量；

利用所述模板数据中的任意两个模板点形成的模板点对，和所述形变变化范围内任意两个模板点形成的模板点对组建所述形变物体的模板点对集，且所述模板点对集用公式表示为

P_m＝{((p₁,n₁),(p₂,n₂)),(p₁,n₁)∈M,(p₂,n₂)∈M}＝M²；

其中，((p₁,n₁),(p₂,n₂))＝(m₁,m₂)表示所述模板点对，p₁、n₁分别为模板点m₁的坐标和对应的法向量，p₂、n₂分别为场景点m₂的坐标和对应的法向量，M为所述模板数据和所述形变数据；

利用所述全局特征描述子和所述局部特征描述子建立数据库，且将所述模板点对集中的各个所述模板点对加入至建立的数据库之中，构建得到所述三维形变模板数据库。

4.如权利要求3所述的三维位姿估计方法，其特征在于，所述利用所述全局特征描述子和所述局部特征描述子建立数据库，且将所述模板点对集合中的各个所述模板点对加入至建立的数据库之中，构建得到所述三维形变模板数据库，包括：

建立一数据库且内部生成有哈希表，根据所述全局特征描述子中的每个特征描述子获得相对应的模板点对的特征值，将具有相同或相似特征值的点对划分为一个数据组且存入所述哈希表；

根据所述局部特征描述子中每个特征描述子相对应的模板点对的特征值，将该模板点对加入所述哈希表中的对应数据组内，且与对应数据组内已存入点对的特征值相一致；

为所述哈希表中的每个数据组设置一个键值，建立该键值和该数据组内所存储的点对之间的映射关系，在已建立的数据库的基础上利用所述哈希表构建得到所述三维形变模板数据库。

5.如权利要求4所述的三维位姿估计方法，其特征在于，所述根据所述场景点对集和所述模板点对集构建图模型，包括：

对所述场景点对集中每个所述场景点对内的两个场景点与所述模板点对集中每个所述模板点对内的两个模板点进行对应匹配，利用相匹配的场景点和模板点形成节点且添加至预先设立的稀疏图中；

在任意所述场景点对内的两个场景点均在所述稀疏图中形成有节点时，则对所形成的节点进行匹配，利用相匹配的节点在所述稀疏图中生成节点之间的连接边；

利用所述稀疏图中的各个节点和所形成节点之间的连接边构建图模型。

6.如权利要求5所述的三维位姿估计方法，其特征在于，所述对所述场景点对集中每个所述场景点对内的两个场景点与所述模板点对集中每个所述模板点对内的两个模板点进行对应匹配，利用相匹配的场景点和模板点形成节点且添加至预先设立的稀疏图中，包括：

建立一中间局部坐标系，以所述场景点对集P_s中任意所述场景点对内的一个场景点s作为所述中间局部坐标系的基准点，计算该场景点s在所述中间局部坐标系中的平移向量T和旋转矩阵R；根据所述平移向量T和旋转矩阵R对所述模板点对集P_m中任意所述模板点对内的一个模板点m进行基准坐标变换，使得场景点和模板点之间形成的对应关系；利用变换关系T(s,m,α)＝T(s)^-1R_x(α)T(m)得到变换后的坐标以及对应的旋转角度α，根据所旋转角度α映射为索引坐标的索引值对对应的局部参数(m,α)进行投票计数，得到第一匹配得分，在所述第一匹配得分超过预设的第一阈值时，利用相匹配的场景点和模板点形成节点v，且v＝(m,α,s)，将所形成的节点添加至预先设立的稀疏图中。

7.如权利要求6所述的三维位姿估计方法，其特征在于，所述在任意所述场景点对内的两个场景点均在所述稀疏图中形成有节点时，则对所形成的节点进行匹配，利用相匹配的节点在所述稀疏图中生成节点之间的连接边，包括：

若所述场景点对内的两个场景点s₁、s₂分别在所述稀疏图中形成有节点v₁＝(m₁,α₁,s₁)，v₂＝(m₂,α₂,s₂)，则计算对应的旋转角度α₁、α₂之间的差值；根据所述差值映射为索引坐标的索引值对节点v₂进行投票计数，得到第二匹配得分，在所述第二匹配得分超过预设的第二阈值时，利用相匹配的节点v₂在所述稀疏图中生成节点v₁、v₂之间的连接边

将利用所述稀疏图中的各个节点和所形成节点之间的连接边构建的图模型用公式表示为G＝(V,E)，其中，V为各个节点的集合，E为节点之间的连接边的集合。

8.如权利要求7所述的三维位姿估计方法，其特征在于，所述优化计算获得所述图模型中节点的匹配数量最多的连接边以使得所述形变物体的三维位姿估计问题转化为所述图模型的优化计算问题，根据该连接边对应的节点对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息，包括：

根据所述图模型建立关于节点的连接矩阵，以将节点的匹配过程转化为求解矩阵的最大值的优化过程，表示为

其中，N₁为模板点的特征向量个数，N₂为场景点的特征向量个数，||X||_F表示Frobenius范数；i、j均为集合V中节点v的序号，X为节点v之间存在的关系状态，若存在对应关系则X_v＝1，否则X_v＝0；H＝w_i,j且w_i,j为第i个节点和第j个节点之间连接状态；argmax为最大函数值的求参函数，score为分类评分函数，maxscore为最大评分计算函数；

初始化矩阵H，利用随机数初始化矩阵向量X，迭代计算X^k+1＝(HX^k)/(|HX^k|)，其中上标k为迭代次数，直至迭代收敛或停止时，输出矩阵H的最大特征值对应的特征向量，由此获得所述图模型中节点的匹配数量最多的连接边；

根据优化计算得到的连接边上的节点确定所述场景点对集表示的场景特征在所述模板点对集表示的模板特征中的位置，得到所述形变物体的位姿信息。

9.一种形变物体的定位抓取系统，其特征在于，包括：

传感器，用于采集形变物体的图像，以形成所述形变物体在当前形变状态下三维的场景数据；

处理器，与所述传感器连接，用于通过权利要求1-8中任一项所述的三维位姿估计方法得到所述形变物体的位姿信息；

控制器，与所述传感器和所述处理器连接，用于控制所述传感器采集所述形变物体的图像，以及根据所述处理器输出的位姿信息控制输出运动指令；

运动机构，与所述控制器连接，用于根据所述运动指令移动抓取所述形变物体。

10.如权利要求9所述的定位抓取系统，其特征在于，所述处理器包括模板建立模板、识别与定位模块；

所述模板建立模板用于根据所述形变物体对应的三维模型在参考状态下的模板数据和在各个形变状态下的形变数据建立三维形变模板数据库；

所述识别与定位模块和所述模板建立模板连接，用于利用所述三维形变模板数据库以及所述形变物体在任意形变状态下三维的场景数据，对所述形变物体进行三维位姿的估计，得到所述形变物体的位姿信息。

11.一种计算机可读存储介质，其特征在于，包括程序，所述程序能够被处理器执行以实现如权利要求1-8中任一项所述的三维位姿估计方法。