一种三维姿态配准方法技术

本发明专利技术提供了一种三维姿态配准方法，属于点云配准领域。该方法先根据待配准点云生成凸包点集，再在凸包点集中确定待配准四面体，然后在目标点云中查找与其形状相似的四面体，基于此两个四面体的四个顶点解算待配准点云相对于模型点云的相对位置姿态(旋转、平移矩阵)将旋转平移矩阵作用于凸包点集从而得到新的凸包点集；在目标点云中，依次对新凸包点集中的每个点寻找其所对应的最近点，计算所有最近点对的平均欧氏距离，判断此平均欧氏距离是否满足阈值条件，以确定求解出的相对位置姿态是否满足精度需求。本发明专利技术采用了二分法查找类似四面体、相似候选配准四面体剔除和利用凸包点集代替整体点云进行误差计算等方式优化了整体运行效率。

A three-dimensional attitude registration method

The invention provides a three-dimensional attitude registration method, which belongs to the field of point cloud registration. Firstly, according to the formation of the convex hull of point cloud registration point set, and then concentrated to determine registration on the convex hull points and tetrahedron tetrahedron, similar in shape with the target point cloud search, the four vertices of the two tetrahedral solution for registration of point cloud with respect to the relative position and attitude model based on point cloud (rotation and translation matrix) the translation and rotation matrix acting on the convex hull in order to obtain new convex hull; the target point cloud, for each point of the new convex hull from find the nearest point of the corresponding calculation, all the nearest point on the average Euclidean distance, to determine whether the average Euclidean distance to meet the threshold conditions, to determine whether the relative position the solution can meet the accuracy requirement of attitude. The present invention adopts dichotomy to find tetrahedra, similar candidate registration, tetrahedron elimination and convex hull point set instead of integral point cloud for error calculation, etc., so as to optimize the overall operation efficiency.

【技术实现步骤摘要】
一种三维姿态配准方法
本专利技术涉及一种三维姿态配准方法，尤其是一种基于几何哈希法的姿态配准方法，属于三维点云配准领域。
技术介绍
对空间非合作目标类航天器位置姿态信息的获取是进行在轨接近、操控和服务的前提。因此，对空间非合作目标位置姿态的测量是一个重要的研究方向。一种有效的测量方法是，利用激光类主动测量敏感器获取空间非合作目标三维点云，然后再通过相关位置姿态解算方法即可求解目标位置姿态信息。位置姿态的求解一般是通过将敏感器测量所得的三维点云与另一模型点云进行配准来实现。随着空间技术的发展，空间任务对数据更新率的要求越来越高，对姿态求解方法的速度也提出了更高的要求。当敏感器获取的点云数据较多，或者获取的点云数据只包含目标的部分特征时，都会导致算法的整体效率降低。前者因为参与计算的点数多，后者则因为点云数据只包含目标的部分特征而导致配准过程中产生的候选匹配对象增多，匹配效率降低。
技术实现思路
本专利技术的技术解决问题是：克服现有技术的不足，提供一种三维姿态配准方法，该方法是一种采用待配准四面体和候选配准四面体求解旋转矩阵和平移矩阵；采用凸包点集代替整体点云进行误差计算实现位姿求解的方法，能够有效提高姿态求解方法的运算速度。本专利技术的技术解决方案是：一种三维姿态配准方法，该方法包括以下步骤：(1)、根据待配准点云生成凸包点集，记为TB，提取凸包的最长边为边ab，凸包点集上另一个点作为顶点c，构成凸包点集上面积最大的三角形，再以凸包点集上又一个点d作为顶点，构成凸包点集上以该三角形为底体积最大的四面体abcd作为待配准四面体；(2)、在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度同在指定的距离范围之内的四面体，作为候选模型四面体；(3)、基于待配准四面体和候选模型四面体的四个顶点，解算待配准四面体和候选模型四面体之间的旋转矩阵R、平移矩阵T；(4)、将旋转矩阵R、平移矩阵T作用于凸包点集TB，得到新的凸包点集TB'，在目标点云中，依次对新的待配准凸包点集TB'中的每个点寻找其所对应的最近点，得到TB'的最近点集；(5)、计算凸包点集TB'与其最近点集之间的平均欧氏距离，判断平均欧氏距离是否小于预设阈值，如果小于，则根据计算的步骤(3)解算得到的旋转矩阵R，计算待配准点云的三轴姿态角和位置信息，结束姿态配准过程，否则，重新在目标点云中查找另一个四面体，更新候选模型四面体，执行步骤(3)～步骤(5)。步骤(5)所述预设阈值取值范围为：10-5-10-2。所述在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度之差在预设范围之内的四面体的具体过程为：(2.1)、计算目标点云中两两点之间的距离，根据目标点云中两两点之间的距离进行散列，生成基于距离的哈希表，所述哈希表按照距离散列条件分成Num个桶，哈希表第一级对应桶编号N，第二级对应目标点云中点序号mi，i∈[1,NM]，NM为目标点云的点数，第三级为目标点云中与目标点mi满足距离散列条件的另一个目标点mj，j∈[1,NM]，且j≠i，所述距离散列条件为：式中，L(mi,mj)为目标点对mi和mj间的距离，N(L(mi,mj))表示L(mi,mj)对应的桶编号，Num表示桶的总数，Lmax、Lmin分别为目标点云中两两目标点之间距离的最大值、最小值；(2.2)、计算待配准四面体所有边的长度Lab、Lac、Lbc、Lad、Lbd、Lcd，在哈希表中选取落入长度Lab所在桶内的任意目标点对，作为候选模型四面体的两个端点ma和mb，此时，落入长度Lab所在桶内的所有目标点对均为候选模型四面体的两个端点ma和mb的候选集合Mab，进入步骤(2.3)；(2.3)、在哈希表中，选取落入长度Lac所在的桶内与ma配对的目标点集，再选取落入长度Lbc所在的桶内与mb配对的目标点集，两个点集中的公共点组成的集合即为目标点云中四面体的第三个顶点的候选集合Mc，从中选取任意一个点作为候选模型四面体的第三个顶点mc；(2.4)、在哈希表中，选取落入长度Lad所在的桶内与ma配对的目标点集，然后选取落入长度Lbd所在的桶内与mb配对的目标点集，再选取落入长度Lcd所在的桶内与mc配对的目标点集，这三个点集中的公共点组成的集合即为目标点云中四面体的第四个顶点md的候选集合Md，从中选取任意一个点作为候选模型四面体的第四个顶点md，mambmcmd构成候选模型四面体。在目标点云中查找另一个候选模型四面体时，采取先依次在候选集合Md中选取另一个目标点作为候选模型四面体的第四个顶点md，构成候选模型四面体，直到遍历完候选集合Md中所有的点，再依次在候选集合Mc中选取另一个目标点作为候选模型四面体的第三个顶点mc，根据mambmc再重新确定满足条件的第四个顶点md，构成候选模型四面体，直到遍历完候选集合Mc所有的点，最后在候选集合Mab中选取另一个目标点对作为候选模型四面体的第一个顶点ma和第二个顶点mb，根据ma和mb重新确定第三个顶点mc和相应的第四个顶点md。在目标点云中查找另一个候选模型四面体时，遍历候选集合Mc、候选集合Md和在候选集合Mab中选取目标点对时，采用二分法的遍历方式切换不同的目标点作为候选模型四面体的顶点。对步骤(5)更新候选模型四面体之后，插入如下步骤：将计算更新后的候选模型四面体与更新之前的候选模型四面体之间的相对三轴姿态角，若两相邻候选四面体之间的相对三轴姿态角小于一定阈值，则认为更新后的候选模型四面体与更新之前的候选模型四面体姿态相似，重新更新下一个候选模型四面体，再执行步骤(3)～步骤(5)，否则，直接执行步骤(3)～(5)。所述一定阈值的取值范围为[0，20°]。本专利技术与现有技术相比的优点在于：(1)、本专利技术采用待配准四面体和候选配准四面体求解旋转矩阵和平移矩阵，采用凸包点集代替整体点云进行误差计算实现位姿求解的方法，能够有效提高姿态求解方法的运算速度；(2)、采用了二分法的查找方式、剔除了相似候选配准对象和利用凸包点集代替整体点云进行误差计算等方式，因此能提高迭代方法的收敛速度。(3)、对相似度比较高的剔除，提高了迭代方法的收敛速度。附图说明图1为本专利技术方法的流程图；图2为哈希表存储结构；具体实施方式以下结合附图和具体实施例对本专利技术进行详细说明。如图1所示，本专利技术提供了一种三维点云姿态配准方法，该方法具体包括以下步骤：(1)、根据待配准点云生成凸包点集，记为TB，提取凸包的最长边为边ab，凸包点集上另一个点作为顶点c，构成凸包点集上面积最大的三角形，再以凸包点集上又一个点d作为顶点，构成凸包点集上以该三角形为底体积最大的四面体abcd作为待配准四面体；凸包是指包含点集中所有点的最小体积的四面体，记为FP，凸包点集即为构成凸包的点的集合。(2)、在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度同在指定的距离范围之内的四面体，作为候选模型四面体；所述在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度之差在预设范围之内的四面体的具体过程为：(2.1)、计算目标点云中两两点之间的距离，根据目标点云中两两点之间的距离进行散列，生成基于距离的哈希表，所述哈希表按照距离本文档来自技高网...

【技术保护点】
一种三维姿态配准方法，其特征在于包括以下步骤：(1)、根据待配准点云生成凸包点集，记为TB，提取凸包的最长边为边ab，凸包点集上另一个点作为顶点c，构成凸包点集上面积最大的三角形，再以凸包点集上又一个点d作为顶点，构成凸包点集上以该三角形为底体积最大的四面体abcd作为待配准四面体；(2)、在目标点云中查找与待配准四面体形状相同、各条边长度与待配准四面体对应各边长度同在指定的距离范围之内的四面体，作为候选模型四面体；(3)、基于待配准四面体和候选模型四面体的四个顶点，解算待配准四面体和候选模型四面体之间的旋转矩阵R、平移矩阵T；(4)、将旋转矩阵R、平移矩阵T作用于凸包点集TB，得到新的凸包点集TB'，在目标点云中，依次对新的待配准凸包点集TB'中的每个点寻找其所对应的最近点，得到TB'的最近点集；(5)、计算凸包点集TB'与其最近点集之间的平均欧氏距离，判断平均欧氏距离是否小于预设阈值，如果小于，则根据计算的步骤(3)解算得到的旋转矩阵R，计算待配准点云的三轴姿态角和位置信息，结束姿态配准过程，否则，重新在目标点云中查找另一个四面体，更新候选模型四面体，执行步骤(3)～步骤(5)。

【技术特征摘要】
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，第二级对应目标点云中点序号mi，i∈[1,NM]，NM为目标点云的点数，第三级为目标点云中与目标点mi满足距离散列条件的另一个目标点mj，j∈[1,NM]，且j≠i，所述距离散列条件为：式中，L(mi,mj)为目标点对mi和mj间的距离，N(L(mi,mj))表示L(mi,mj)对应的桶编号，Num表示桶的总数，Lmax、Lmin分别为目标点云中两两目标点之间距离的最大值、最小值；(2.2)、计算待配准四面体所有边的长度Lab、Lac、Lbc、Lad、Lbd、Lcd...

【专利技术属性】
技术研发人员：吴云，王立，朱飞虎，郭绍刚，刘达，吴奋陟，
申请(专利权)人：北京控制工程研究所，
类型：发明
国别省市：北京,11