CN112070832B - 基于点云dca特征的非合作目标位姿跟踪方法及装置 - Google Patents

Abstract

本申请公开了一种基于点云DCA特征的非合作目标位姿跟踪方法及装置，其中，该方法包括：对三维目标点云模型和三维场景点云模型进行特征点检测；提取三维目标点云模型和三维场景点云模型中的点云局部特征描述子；根据FLANN算法对三维目标点云模型的点云局部特征描述子和三维场景点云模型中的点云局部特征描述子进行特征匹配；通过RANSAC算法去除特征匹配结果中的错误匹配，得到三维目标点云模型的初步位姿估计；将初步位姿估计作为迭代最近点算法的初始值，通过迭代最近点算法对位姿估计进行精调。该方法提出一种简洁高效的非合作目标位姿测量方法，可以更好的实现目标航天器相对位姿的测量。

Description

基于点云DCA特征的非合作目标位姿跟踪方法及装置

技术领域

本申请涉及位姿跟踪技术领域，特别涉及一种基于点云DCA特征的非合作目标位姿跟踪方法及装置。

背景技术

随着空间技术的发展，人类对航天器进行在轨维护、维修以及轨道垃圾清理等需求越来越迫切。作为服务对象的故障卫星、失效航天器及轨道垃圾等大多数都没有考虑过轨道服务，即没有安装合作标志器和应答机等辅助测量设备，也没有安装用于抓捕固定的装置。对其进行相对位姿测量是实现在轨服务的关键，可以为非合作目标的交会和维护等提供准确的相对姿态信息。然而，空间非合作目标相对位姿测量存在测量设备有限、目标尺度变化大、空间环境变化剧烈等不利因素，为相关的研究工作带来了极大挑战。

在相关技术中，基于双目立体视觉的相对测量方法计算精度不高，不能很好的适应光照和目标位置的变化，而基于稠密点云的相对位姿测量方法则需要事先知道目标的点云模型并需要较好的初始值。

发明内容

本申请旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本申请的一个目的在于提出一种基于点云DCA特征的非合作目标位姿跟踪方法，该方法提出一种简洁高效的非合作目标位姿测量方法，可以更好的实现目标航天器相对位姿的测量。

本申请的另一个目的在于提出一种基于点云DCA特征的非合作目标位姿跟踪装置。

为达到上述目的，本申请一方面实施例提出了一种基于点云DCA特征的非合作目标位姿跟踪方法，包括：

对三维目标点云模型和三维场景点云模型进行特征点检测；

提取所述三维目标点云模型和所述三维场景点云模型中的点云局部特征描述子；

根据FLANN算法对所述三维目标点云模型的点云局部特征描述子和所述三维场景点云模型中的点云局部特征描述子进行特征匹配；

通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计；

将所述初步位姿估计作为迭代最近点算法的初始值，通过所述迭代最近点算法对所述三维目标点云模型的位姿估计进行精调。

为达到上述目的，本申请另一方面实施例提出了一种基于点云DCA特征的非合作目标位姿跟踪装置，包括：

特征点检测模块，用于通过对三维目标点云模型和三维场景点云模型稀疏采样进行特征点检测；

特征描述模块，用于提取所述三维目标点云模型和所述三维场景点云模型中的点云局部特征描述子；

特征匹配模块，用于根据FLANN算法对所述三维目标点云模型的点云局部特征描述子和所述三维场景点云模型中的点云局部特征描述子进行特征匹配；

位姿粗估计模块，用于通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计；

位姿精调模块，用于将所述初步位姿估计作为迭代最近点算法的初始值，通过所述迭代最近点算法对所述三维目标点云模型的位姿估计进行精调。

本申请实施例的基于点云DCA特征的非合作目标位姿跟踪方法及装置，具有如下优势：

(1)利用点云的邻域点平均距离，曲率变化和邻域点法向夹角提出了一种简单、高效而且鲁棒的三维点云特征描述子DCA；

(2)针对在点云配准时ICP算法对初值依赖等问题，提出了基于点云DCA特征的非合作目标位姿跟踪算法。该方法首先计算点云的特征描述子(DCA)，然后基于该特征描述寻找点云的对应关系并求得初始配准结果，最后将该结果作为ICP的初始值，实现目标位姿的精确跟踪。

本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。

附图说明

本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪方法流程图；

图2为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪方法流程框图；

图3为根据本申请一个实施例的数据点与邻域点平均距离示意图；

图4为根据本申请一个实施例的数据点曲率示意图；

图5为根据本申请一个实施例的数据点与邻域点法向夹角示意图；

图6为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪装置结构示意图。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。

下面参照附图描述根据本申请实施例提出的基于点云DCA特征的非合作目标位姿跟踪方法及装置。

首先将参照附图描述根据本申请实施例提出的基于点云DCA特征的非合作目标位姿跟踪方法。

图1为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪方法流程图。

图2为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪方法流程框图。

结合图1和图2所示，该基于点云DCA特征的非合作目标位姿跟踪方法包括以下步骤：

步骤S1，对三维目标点云模型和三维场景点云模型进行特征点检测。

特征点检测是特征描述与特征匹配的前提，最常用的方法就是通过对点云的稀疏采样得到的。在本申请的实施例中采用对点云稀疏采样的方法实现特征点检测。

步骤S2，提取三维目标点云模型和三维场景点云模型中的点云局部特征描述子。

具体地，非合作目标位姿跟踪的主要目的是获得失效翻滚卫星的位置和姿态。本申请的实施例是基于三维点云的局部特征实现目标的姿态跟踪的。三维点云表面的凹凸程度、尖头、尖锐边等都可以表达点云数据的外观特征。其中，点云的法向量、曲率和密度等都不同程度的反应了点云的几何特征，本申请的实施例利用点云的密度、曲率和法向夹角作为点云的局部特征描述，实现非合作目标点云特征的识别。

(1)邻域点平均距离

点云的密度可以用邻域点距离进行表征，欧式距离ε通常被用来表示数据点的邻域。对于规则采样的表面，相同的ε具有相同数目的邻域点。而对于不规则采样的表面，相同的ε可能具有不同数目的邻域点。而作为数据点邻域的另一种常用表示方法k邻域来说，无论采样表面规则与否，在相同的k邻域内，都具有相同数目的邻域点。设G＝{g_i},i＝1,…,N为点云的数据点集，其中数据特征点g_i的三维坐标为(x_i,y_i,z_i)，N是点云数据集中点的个数。M(g_i)＝{g_ij}1≤j≤k表示与特征点g_i空间距离最近的k个点，即g_i的k邻域。

点云的特征可以用点与邻域点的平均距离来表示：当点与周围邻域点的平均距离较小时，点云分布比较密集，一般是点云的特征区域；反之，当点与周围邻域点的平均距离较大时，点云分布比较稀疏，一般是点云的平滑区域。如图3所示，数据点A与周围邻域点的平均距离较小点云密集，因此位于特征区域。数据点C与周围邻域点的平均距离较大点云稀疏，因此位于平滑区域。

邻域点平均距离如下所示：

为了保持三维点云的细节信息，在判断数据点是否位于特征区域时，将邻域点平均距离作为判别点云特征的一个参数。

(2)曲率

曲率反映了点云表面的凹凸程度是点云识别中重要的几何特征。对于仅由三维坐标表示且没有任何拓扑信息的曲面，只能近似估算其曲率。本申请实施例采用基于局部曲面拟合的方法计算点云数据点的曲率，

假设点云表面是光滑的，所有采样点的局部邻域可以用平面来拟合。对于数据点g_i，求得它的k个邻域点，计算一个最小二乘意义上的局部平面E：

式中n为平面法向量，d为到局部平面E坐标原点的距离。

对法向量的求解可以转化为对顶点相关矩阵的主元分析问题：

其

中是数据点g_i的邻域点集M(g_i)的重心，考虑下面特征值问题：

Ce_m＝λ_m·e_m,m∈{1,2,3} (4)

由于C是半正定的三阶对称矩阵，其中λ_m均为实数是对应点集

的主元，e_m相互正交为沿相应特征向量方向上的曲面变分。根据多元向量的分析，λ_m可以表示各个对应特征向量方向上局部曲面的变化度。因此，总的曲面变分为数据点g_i到它重心

的欧式距离的平方和，如下式所示：

假设λ₁≤λ₂≤λ₃，则椭圆平面

穿过

且到邻域点g_j距离的平方和最小。一个最小二乘拟合曲面由椭圆的两个主轴e₂和e₃张成。数据点g_i的法向量n_i可以近似用e₁表示，即n₁≈e₁。

数据点g_i在k邻域内的曲面变分如下所示：

其中λ₁表示曲面沿法向量方向的变化，λ₂、λ₃表示数据点g_i在切平面上的分布情况。曲面变分τ_i可以近似表示数据点g_i的曲率H_i即H_i≈τ_i。

如图4所示，黑色的点为三维点云的任意数据点，空心点为该数据点相应的邻域点，在该点处的切平面用直线表示。数据点g₁的邻域点与其切平面的距离较大，表明该点位于曲率较大的曲面上，因此该点位于特征区域；与之相反，数据点g₂的邻域点与其切平面间的距离较小，表明该点位于曲率较小的曲面上，因此该点位于点云的平滑区。

(3)法向夹角

法向夹角也是点云识别中常用的几何特征，它反映了点云表面弯曲或平坦的程度。设三维点云G中的任意点为g_i，它的邻域点为g_j，它们的法向量分别为

和

数据点g_i与邻域点g_j的法向夹角的余弦如下式所示：

式中

的取值范围为[0,π]。

为了得到数据点g_i与邻域点g_j法向夹角参数ω_a(g_i)，对g_i的所有邻域点法向夹角求和：

数据点与邻域点法向夹角如图5所示，邻域点个数k＝4。图中黑色点表示点云上的任意数据点，空心点表示该数据点相应的邻域点。数据点g₃位于弯曲度较大的曲面上，其与邻域点的法向夹角也较大，因此该点位于特征区域；与之相反，数据点g₈位于较平坦的曲面上，其余邻域点的法向夹角较小，因此该点位于点云的平滑区域。

数据点与邻域点法向夹角参数反映了数据点g_i的所有邻域点对其弯曲程度的影响。如果数据点g_i位于弯曲度较大的曲面上，则法向夹角参数ω_a(g_i)也较大，因此该点位于点云的特征区域；与之相反，如果数据点g_i位于弯曲度较小的曲面上，则法向夹角参数ω_a(g_i)也随之减小，因此该点很有可能位于点云较平坦的区域。因此，把数据点与邻域点法向夹角参数也作为判断点云特征的一个参数。

在本申请的实施例中，经过对多组实验数据分析可知，点云数据的密度和分布的均匀性决定了邻域点个数k的取值。当点云密度较大时，k值取值较小，一般为10-30；而当点云中包含噪声、遮挡等情况时，k值取值应该大些，一般为50-100。k值取值越大，点云特征描述精度越高，但计算时间越长。

可以理解的是，特征描述的目的是提取点云局部的特征信息使得点云中每个点可以区分开来。特征描述直接影响后续特征匹配的效果和效率，是基于点云局部特征位姿估计的一个关键环节。在整个流程中特征点检测、特征描述和特征匹配是为了建立两个点云的对应关系，它们占整个位姿估计流程时间的90％。本申请采用上节三维点云局部特征描述子对非合作目标的点云进行特征描述。该特征描述子为3维浮点型。

步骤S3，根据FLANN算法对三维目标点云模型的点云局部特征描述子和三维场景点云模型中的点云局部特征描述子进行特征匹配。

特征匹配直接影响位姿估计的结果。本申请中对目标点云特征描述的结果为3维浮点型数据，因此特征匹配问题转变为高维数据的对应点搜索问题。本申请采用FLANN(Fast Library for Approximate Nearest Neighbors)搜索欧式距离最近的点作为候选配对，并且只有最近点与次近点的比值小于一定阈值时(本申请实施例设为0.9)才认为是正确的匹配。FLANN是目前最完整的最近邻开源库，它实现了一系列快速查找高维度最近邻搜索的算法，比暴力搜索算法快几个数量级。

步骤S4，通过RANSAC算法去除特征匹配结果中的错误匹配，得到三维目标点云模型的初步位姿估计。

通过上述基于点云局部特征的匹配结果已经剔除了大部分的错误匹配，但仍然存在少量的错误配对，这些错误配对给后续的位姿估计带来了较大的影响。为了减小错误匹配对两个点云间初始位姿估计带来的影响，本申请的实施例利用RANSAC算法实现。

RANSAC算法首先假定数据是由“内点”和“噪点”组成，然后对数据进行少量的采样并不断的迭代计算模型的参数，来得到数据拟合最好的结果。对于本申请基于点云局部特征描述子匹配得到的N个配对结果，采用RANSAC算法实现错误配对剔除并获得配对点云间初始位姿的流程如下：

a)根据确定3维变换关系所需最小样本数的要求，在N个配对结果中随机选取3个，计算初始变换关系T_i0；

b)判断其他配对结果是否满足初始变换关系T_i0。其他配对结果中的一个点通过变换关系T_i0后得到的点，和其原配对点间的误差小于一定的阈值时，该配对被认为是内点，否则为噪点；

c)当内点数达到一定的数量时，变换关系T_i0就被认定为一个合理的位姿估计；

d)根据得到的所有内点配对重新计算变换关系T_i1，并统计该变换关系下的误差；

e)不断重复以上过程，每次都能得到一个变换关系：对于不合理的位姿估计，直接丢弃重新开始；对于合理的位姿估计，保留变换关系下误差最小的位姿估计作为最终的位姿估计结果输出。

就算数据中存在明显的野值，RANSAC也能得到鲁棒的估计结果，但是所需估计时间不确定。对于上述迭代过程，RANSAC需要迭代

次，当特征匹配结果N较大时，将需要较长的时间才能得到最优结果。出于效率的考虑，在实际应用中通常设置迭代次数的上限。这样得到的结果不一定是最优的，甚至有可能是错误的。在这种情况下，特征匹配的正确性就显得尤为重要。特征匹配的正确率越高，RANSAC收敛到最优结果的速度就越快，否则收敛的结果和速度都难以保证。本申请通过基于阈值的FLANN特征匹配策略，剔除了大部分错误匹配，提高了RANSAC的迭代效率和最终收敛结果的精度。

步骤S5，将初步位姿估计作为迭代最近点算法的初始值，通过迭代最近点算法对所述三维目标点云模型的位姿估计进行精调。

ICP(Iterative Closest Point)算法常被用来求解两组点云之间的变换关系，它具有精度高、适用性强的特点。但是ICP算法要求较好的初始值，并且两个点云之间变换不能太大，不能有太多遮挡等，否则算法收敛速度慢甚至无法收敛。本申请的实施例利用RANSAC算法得到的位姿估计作为ICP算法的初始值，进一步对位姿估计的结果进行精调。

基于三维点云的位姿估计具有精度高，不受光照和尺寸变化影响等特点。随着近年来低成本三维采集设备的出现，基于三维点云位姿估计的研究也得以迅速发展。基于点云的位姿估计本质上就是一个点云配准的过程，即给定一个模型点云G＝{g_i}和场景点云M＝{m_i}，求解两者之间的相对变换关系T＝(R，t)，使得两者之间共同的部分能够正确的匹配和搭接。点云配准通常是先根据点云的特征建立两个点云的对应关系，估计一个粗匹配，然后再通过迭代最近点(ICP)算法进行位姿精调。

根据本申请实施例提出的基于点云DCA特征的非合作目标位姿跟踪方法，通过利用点云的邻域点平均距离，曲率变化和邻域点法向夹角提出了一种简单、高效而且鲁棒的三维点云特征描述子DCA，基于该特征描述子寻找点云的对应关系并求得初始配准结果，最后将该结果作为ICP的初始值，实现目标位姿的精确跟踪。

其次参照附图描述根据本申请实施例提出的基于点云DCA特征的非合作目标位姿跟踪装置。

图6为根据本申请一个实施例的基于点云DCA特征的非合作目标位姿跟踪装置结构示意图。

如图6所示，该基于点云DCA特征的非合作目标位姿跟踪装置包括：特征点检测模块601、特征描述模块602、特征匹配模块603、位姿粗估计模块604和位姿精调模块605。

特征点检测模块601，用于通过对三维目标点云模型和三维场景点云模型稀疏采样进行特征点检测。

特征描述模块602，用于提取三维目标点云模型和三维场景点云模型中的点云局部特征描述子。

特征匹配模块603，用于根据FLANN算法对三维目标点云模型的点云局部特征描述子和三维场景点云模型中的点云局部特征描述子进行特征匹配。

位姿粗估计模块604，用于通过RANSAC算法去除特征匹配结果中的错误匹配，得到三维目标点云模型的初步位姿估计。

位姿精调模块605，用于将初步位姿估计作为迭代最近点算法的初始值，通过迭代最近点算法对所述三维目标点云模型的位姿估计进行精调。

进一步地，在本申请的一个实施例中，点云局部特征描述子包括：点云的邻域点平均距离、曲率变化和邻域点法向夹角；

点云的邻域点平均距离表示点云的疏密程度，点云的曲率变化表示点云表面的凹凸程度，点云的邻域点法向夹角表示点云表面弯曲或平坦的程度。

需要说明的是，前述对基于点云DCA特征的非合作目标位姿跟踪方法实施例的解释说明也适用于该实施例的装置，此处不再赘述。

根据本申请实施例提出的基于点云DCA特征的非合作目标位姿跟踪装置，通过利用点云的邻域点平均距离，曲率变化和邻域点法向夹角提出了一种简单、高效而且鲁棒的三维点云特征描述子DCA，基于该特征描述子寻找点云的对应关系并求得初始配准结果，最后将该结果作为ICP的初始值，实现目标位姿的精确跟踪。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，包括以下步骤：

对三维目标点云模型和三维场景点云模型进行特征点检测；

提取所述三维目标点云模型和所述三维场景点云模型中的点云局部特征描述子；

根据FLANN算法对所述三维目标点云模型的点云局部特征描述子和所述三维场景点云模型中的点云局部特征描述子进行特征匹配；

通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计；

将所述初步位姿估计作为迭代最近点算法的初始值，通过所述迭代最近点算法对所述三维目标点云模型的位姿估计进行精调，包括：

通过预设的三维目标点云模型点云G＝{g_i}和维场景点云模型的场景点云M＝{m_i}，通过点云配准求解两者点云之间的相对变换关系T＝(R，t)，使得两者点云之间共同的部分正确的匹配和搭接；其中，所述点云配准是根据点云的特征建立两个点云的对应关系估计粗匹配，再通过迭代最近点ICP算法进行位姿精调；

所述通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计，包括：

a)根据确定3维变换关系所需最小样本数的要求，在N个配对结果中随机选取3个，计算初始变换关系T_i0；

b)判断其他配对结果是否满足初始变换关系T_i0：其他配对结果中的一个点通过变换关系T_i0后得到的点，和其原配对点间的误差小于预设的阈值时，该配对被认为是内点，否则为噪点；

c)当内点数达到预设的数量时，变换关系T_i0被认定为正确的位姿估计；

d)根据得到的所有内点配对重新计算变换关系T_i1，并统计该变换关系下的误差；

e)重复a)-d)的过程，每次得到一个变换关系：对于非正确的位姿估计，直接丢弃重新开始；对于正确的位姿估计，保留变换关系下误差最小的位姿估计作为最终的位姿估计结果输出。

2.根据权利要求1所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，所述特征点检测包括对点云进行稀疏采样。

3.根据权利要求1所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，所述点云局部特征描述子包括：点云的邻域点平均距离、曲率变化和邻域点法向夹角；

所述点云的邻域点平均距离表示点云的疏密程度，所述点云的曲率变化表示点云表面的凹凸程度，所述点云的邻域点法向夹角表示点云表面弯曲或平坦的程度。

4.根据权利要求3所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，所述邻域点平均距离为：

其中，ω_nb(g_i)为特征点g_i的邻域平均距离，k为特征点g_i的邻域点个数，g_j为特征点g_i的邻域点，M(g_i)为特征点g_i的邻域点集。

5.根据权利要求3所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，对特征点g_i通过局部曲面拟合的方法得到局部平面，特征点g_i的曲率H_i通过曲面变分τ_i表示：

其中，λ₁表示曲面沿法向量方向的变化，λ₂、λ₃表示特征点g_i在切平面上的分布情况；其中，

其中，

中是特征点g_i的邻域点集M(g_i)的重心，g_ij为特征点g_i的第j个邻域点。

6.根据权利要求3所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，特征点g_i与邻域点g_j的法向量分别为

和

数据点g_i与邻域点g_j的法向夹角的余弦为：

其中，

的取值范围为[0,π]；

对特征点g_i的所有邻域点法向夹角求和，得到特征点g_i与邻域点g_j法向夹角参数ω_a(g_i)：

其中，M(g_i)为特征点g_i的邻域点集。

7.根据权利要求3-6任一一项所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，根据点云模型的特点调整邻域点个数的取值。

8.根据权利要求1所述的基于点云DCA特征的非合作目标位姿跟踪方法，其特征在于，所述根据FLANN算法对所述三维目标点云模型的点云局部特征描述子和所述三维场景点云模型中的点云局部特征描述子进行特征匹配，包括：采用FLANN算法搜索欧式距离最近的点作为候选配对，且在最近点与次近点的比值小于预设阈值时为正确匹配。

9.一种基于点云DCA特征的非合作目标位姿跟踪装置，特征在于，包括：

特征点检测模块，用于通过对三维目标点云模型和三维场景点云模型稀疏采样进行特征点检测；

特征描述模块，用于提取所述三维目标点云模型和所述三维场景点云模型中的点云局部特征描述子；

特征匹配模块，用于根据FLANN算法对所述三维目标点云模型的点云局部特征描述子和所述三维场景点云模型中的点云局部特征描述子进行特征匹配；

位姿粗估计模块，用于通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计；

位姿精调模块，用于将所述初步位姿估计作为迭代最近点算法的初始值，通过所述迭代最近点算法对所述三维目标点云模型的位姿估计进行精调，包括：

通过预设的三维目标点云模型点云G＝{g_i}和维场景点云模型的场景点云M＝{m_i}，通过点云配准求解两者点云之间的相对变换关系T＝(R，t)，使得两者点云之间共同的部分正确的匹配和搭接；其中，所述点云配准是根据点云的特征建立两个点云的对应关系估计粗匹配，再通过迭代最近点ICP算法进行位姿精调；

所述通过RANSAC算法去除所述特征匹配结果中的错误匹配，得到所述三维目标点云模型的初步位姿估计，的流程为：

a)根据确定3维变换关系所需最小样本数的要求，在N个配对结果中随机选取3个，计算初始变换关系T_i0；

b)判断其他配对结果是否满足初始变换关系T_i0：其他配对结果中的一个点通过变换关系T_i0后得到的点，和其原配对点间的误差小于预设的阈值时，该配对被认为是内点，否则为噪点；

c)当内点数达到预设的数量时，变换关系T_i0被认定为正确的位姿估计；

d)根据得到的所有内点配对重新计算变换关系T_i1，并统计该变换关系下的误差；

e)重复a)-d)的过程，每次得到一个变换关系：对于非正确的位姿估计，直接丢弃重新开始；对于正确的位姿估计，保留变换关系下误差最小的位姿估计作为最终的位姿估计结果输出。

10.根据权利要求9所述的基于点云DCA特征的非合作目标位姿跟踪装置，其特征在于，所述点云局部特征描述子包括：点云的邻域点平均距离、曲率变化和邻域点法向夹角；

所述点云的邻域点平均距离表示点云的疏密程度，所述点云的曲率变化表示点云表面的凹凸程度，所述点云的邻域点法向夹角表示点云表面弯曲或平坦的程度。

Images