[发明专利]一种三维姿态配准方法

权利要求书

1.一种三维姿态配准方法，其特征在于包括以下步骤：

(1)、根据待配准点云生成凸包点集，记为TB，提取凸包的最长边为边ab，凸包点集上另一个点作为顶点c，构成凸包点集上面积最大的三角形，再以凸包点集上又一个点d作为顶点，构成凸包点集上以该三角形为底体积最大的四面体abcd作为待配准四面体；

(2)、在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度同在指定的距离范围之内的四面体，作为候选模型四面体；

(3)、基于待配准四面体和候选模型四面体的四个顶点，解算待配准四面体和候选模型四面体之间的旋转矩阵R、平移矩阵T；

(4)、将旋转矩阵R、平移矩阵T作用于凸包点集TB，得到新的凸包点集TB'，在目标点云中，依次对新的待配准凸包点集TB'中的每个点寻找其所对应的最近点，得到TB'的最近点集；

(5)、计算凸包点集TB'与其最近点集之间的平均欧氏距离，判断平均欧氏距离是否小于预设阈值，如果小于，则根据计算的步骤(3)解算得到的旋转矩阵R，计算待配准点云的三轴姿态角和位置信息，结束姿态配准过程，否则，重新在目标点云中查找另一个四面体，更新候选模型四面体，执行步骤(3)～步骤(5)。

2.根据权利要求1所述的一种三维姿态配准方法，其特征在于步骤(5)所述预设阈值取值范围为：10^-5-10^-2。

3.根据权利要求1所述的一种三维姿态配准方法，其特征在于所述在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度之差在预设范围之内的四面体的具体过程为：

(2.1)、计算目标点云中两两点之间的距离，根据目标点云中两两点之间的距离进行散列，生成基于距离的哈希表，所述哈希表按照距离散列条件分成Num个桶，哈希表第一级对应桶编号N，第二级对应目标点云中点序号m_i，i∈[1,N_M]，N_M为目标点云的点数，第三级为目标点云中与目标点m_i满足距离散列条件的另一个目标点m_j，j∈[1,N_M]，且j≠i，所述距离散列条件为：

N(L(mi,mj))=[Num×(L(mi,mj)-Lmin)Lmax-Lmin]]]>

式中，L(m_i,m_j)为目标点对m_i和m_j间的距离，N(L(m_i,m_j))表示L(m_i,m_j)对应的桶编号，Num表示桶的总数，L_max、L_min分别为目标点云中两两目标点之间距离的最大值、最小值；

(2.2)、计算待配准四面体所有边的长度L_ab、L_ac、L_bc、L_ad、L_bd、L_cd，在哈希表中选取落入长度L_ab所在桶内的任意目标点对，作为候选模型四面体的两个端点m_a和m_b，此时，落入长度L_ab所在桶内的所有目标点对均为候选模型四面体的两个端点m_a和m_b的候选集合M_ab，进入步骤(2.3)；

(2.3)、在哈希表中，选取落入长度L_ac所在的桶内与m_a配对的目标点集，再选取落入长度L_bc所在的桶内与m_b配对的目标点集，两个点集中的公共点组成的集合即为目标点云中四面体的第三个顶点的候选集合M_c，从中选取任意一个点作为候选模型四面体的第三个顶点m_c；

(2.4)、在哈希表中，选取落入长度L_ad所在的桶内与m_a配对的目标点集，然后选取落入长度L_bd所在的桶内与m_b配对的目标点集，再选取落入长度L_cd所在的桶内与m_c配对的目标点集，这三个点集中的公共点组成的集合即为目标点云中四面体的第四个顶点m_d的候选集合M_d，从中选取任意一个点作为候选模型四面体的第四个顶点m_d，m_am_bm_cm_d构成候选模型四面体。