一种用于激光测距机的扫描数据匹配方法及装置

摘要

本发明提出一种用于激光测距机的扫描数据匹配方法及装置，其中该方法包括：采用激光测距机进行扫描，将得到的扫描点形成多条轮廓线；提取多条轮廓线的特征，形成轮廓特征向量集合；对相邻两次扫描数据处理后的轮廓特征向量集合进行匹配，得到相邻时刻的轮廓特征匹配结果，用于后续运动估计计算。本发明可以有效地提高轮廓特征提取的速度和匹配的精确度。

说明书

技术领域

本发明涉及机器人自主导航领域，具体为环境信息的处理与自身运动的估计，特别涉及 一种用于激光测距机的扫描数据匹配方法及装置。

背景技术

无论是军事国防领域还是抗灾救灾等民用领域都对机器人的自主导航性能有广泛 而迫切的需求。机器人利用自身传感器在室内环境下进行地图重构并估计自身位姿，是 机器人自主导航的重要基础。其中，激光测距机具有测量准确，测量角度大等诸多优点， 是机器人配备的重要传感器之一。激光测距机被广泛用来进行环境信息感知和自身位置 估计，利用相邻的两次扫描估计自身位姿变化时其中重要基础的算法。现有的ICP（最 近点迭代）算法进行点与点的迭代特征匹配，算法效率较低，且受到噪声点的影响较大。

发明内容

本发明旨在至少在一定程度上解决上述技术问题之一或至少提供一种有用的商业选择。 为此，本发明的一个目的在于提出一种匹配效率高、精确度好的用于激光测距机的扫描数据 匹配方法。本发明的另一个目的在于提出一种匹配效率高、精确度好的用于激光测距机的扫 描数据匹配装置。

根据本发明实施例的用于激光测距机的扫描数据匹配方法，包括：S1.采用激光测距机 进行扫描，将得到的扫描点形成多条轮廓线；S2.提取多条所述轮廓线的特征，形成轮廓特 征向量集合；S3.对相邻两次扫描数据处理后的所述轮廓特征向量集合进行匹配，得到相邻 时刻的轮廓特征匹配结果，用于后续运动估计计算。

在本发明一个实施例中，所述步骤S1进一步包括：S11.采用激光测距机进行扫描，得 到扫描点集合；S12.根据扫描点之间的欧氏距离进行聚类，同时将孤立的扫描点视为噪声点 删除，建立扫描点之间的连接关系，将离散的扫描点连接成轮廓线；S13.将各条轮廓线标记 出X个转折点，划分成X+1段的折线段,其中X等于扫描点数目最少的轮廓线所包括扫描点 数目减2。

在本发明一个实施例中，所述轮廓特征向量记为（N,D,θ₁,θ₂,……θ_X）,其中，N表示所述 轮廓线包括的扫描点数目，D表示所述轮廓线的长度值，θ₁至θ_X表示所述轮廓线中相邻折 线段的夹角余弦值。

在本发明一个实施例中，所述步骤S3进一步包括：S31.提供时刻k的轮廓特征向量集 ｛d_k,1,d_k,2,……d_k,m｝和时刻k+1的轮廓特征向量集｛d_k+1,1,d_k+1,2,……d_k+1,n｝；S32.选取时刻 k中未匹配过的轮廓特征向量d_k,I，其中1≤I≤m，对于时刻k+1中的各个轮廓特征向量d_k+1,i， 其中1≤i≤n，计算|d_k-d_k+1,i|，选取使|d_k,I-d_k+1,i|取值最小的d_k+1,i，作为时刻k的d_k在时刻 k+1中的轮廓特征向量的最优匹配；S33.重复步骤S32，直至时刻k中没有未匹配过的轮 廓特征向量，则循环结束。

在本发明一个实施例中，所述激光测距机为二维激光测距机。

根据本发明实施例的用于激光测距机的扫描数据匹配装置，包括：激光测距机，用于进 行扫描，获得扫描点集合；轮廓线拟合模块，用于根据扫描点集合，形成多条轮廓线；轮廓 特征向量提取模块，用于提取多条所述轮廓线的特征，形成轮廓特征向量集合；匹配模块， 用于对相邻两次扫描数据处理后的所述轮廓特征向量集合进行匹配，得到相邻时刻的轮廓特 征匹配结果，用于后续运动估计计算。

在本发明一个实施例中，所述轮廓线拟合模块进一步包括：聚类连接子模块，用于根据 扫描点之间的欧氏距离进行聚类，同时将孤立的扫描点视为噪声点删除，建立扫描点之间的 连接关系，将离散的扫描点连接成轮廓线；折线段划分子模块，用于将各条轮廓线标记出X 个转折点，划分成X+1段的折线段,其中X等于扫描点数目最少的轮廓线所包括扫描点数目 减2。

在本发明一个实施例中，所述轮廓特征向量记为（N,D,θ₁,θ₂,……θ_X）,其中，N表示所述 轮廓线包括的扫描点数目，D表示所述轮廓线的长度值，θ₁至θ_X表示所述轮廓线中相邻折 线段的夹角余弦值。

在本发明一个实施例中，所述匹配模块中，匹配算法为：根据时刻k的轮廓特征向量集 ｛d_k,1,d_k,2,……d_k,m｝和时刻k+1的轮廓特征向量集｛d_k+1,1,d_k+1,2,……d_k+1,n｝，选取时刻k 中未匹配过的轮廓特征向量d_k,I，其中1≤I≤m，对于时刻k+1中的各个轮廓特征向量d_k+1,i， 其中1≤i≤n，计算|d_k-d_k+1,i|，选取使|d_k,I-d_k+1,i|取值最小的d_k+1,i，作为时刻k的d_k在时刻 k+1中的轮廓特征向量的最优匹配。

在本发明一个实施例中，所述激光测距机为二维激光测距机。

本发明用于激光测距机的扫描数据匹配方法及装置有如下优点：（1）利用轮廓的特征来 进行迭代特征匹配，减少了匹配的计算量，同时由于轮廓作为一个整体，其特征比较明显， 较点与点的匹配，可以提高匹配的准确度和匹配速度；（2）把每一个轮廓用一个维数相同的 轮廓特征向量表示，便于迭代计算向量差的模的大小，便于寻找最优匹配；（3）使用适当数 量的折线段对轮廓进行拟合，既可以判断轮廓的走势特征，同时也可以忽略掉一些细小的干 扰以及细小的细节特征。综上，本发明可以有效地提高轮廓特征提取的速度和匹配的精确度。

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

附图说明

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

图1为本发明实施例的用于激光测距机的扫描数据匹配方法的流程图；

图2为本发明实施例的扫描点集合根据欧式距离聚类拟合轮廓线的详细流程图；

图3为本发明实施例的用于激光测距机的扫描数据匹配装置的示意图；

图4为本发明具体实施例的激光测距机在k时刻的扫描点、轮廓以及折线拟合示意图；

图5为本发明具体实施例的激光测距机在k+1时刻的扫描点、轮廓以及折线拟合示意图；

图6为本发明具体实施例的相邻时刻（k时刻和k+1时刻）的特征匹配结果示意图。

具体实施方式

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

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚 度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺 时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便 于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特 定的方位构造和操作，因此不能理解为对本发明的限制。

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

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术 语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机 械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元 件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明 中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以 包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们 之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一 特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特 征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅 仅表示第一特征水平高度小于第二特征。

本发明的目的旨在进一步解决轮廓的特征提取与匹配，具体来说就是提出一种用于 激光测距机扫描数据的特征提取与匹配方法。

为达到上述目的，本发明首先根据激光测距机扫描获得的数据提取轮廓线。得轮廓 线之后，进行折线进行拟合，然后提取出对应的多维特征向量（维数大于等于3，但对 于每一个轮廓特征向量的维数都相同），其中第一维表示该轮廓的数据点个数，第二维 表示该轮廓的长度（即把每两个相连接的点的距离求和），后面几维表示用折线拟合时 每一组相连接的直线的角度的余弦值。在获得了前后两个时刻激光测距机扫描地图的轮 廓特征向量后，开始进行匹配。把每个轮廓特征向量当成一个特征点，然后利用迭代方 法，求取前后两个时刻地图的特征匹配，从而可用于进一步的激光测距机运动估计计算。

如图1所示，根据本发明实施例的用于激光测距机的扫描数据匹配方法，包括以下步骤：

S1.采用激光测距机进行扫描，将得到的扫描点形成多条轮廓线。具体地：

首先，采用激光测距机进行扫描，得到扫描点集合。

在本发明的一个实施例中，该激光测距机为二维激光测距机，即扫描一次采样得到的是 某一时刻在某一高度下的平面中的多个数据点的信息。二维激光测距机可以扫描最大270度 平面方向的障碍物距离，并且可以自主调整扫描角度范围。同时，由于激光的稳定传播、不 易受到干扰等优点，所以激光测距机还具有测量误差小，精确度高的优点，可以很好地满足 本发明中的精度要求。最主要的是，二维激光测距机返回的数据可以通过图像显示出来，显 示出周围环境障碍物的轮廓信息，具有很好地可读性。

其次，根据扫描点之间的欧氏距离进行聚类，同时将孤立的扫描点视为噪声点删除，建 立扫描点之间的连接关系，将离散的扫描点连接成轮廓线。

该方法的具体流程可以参考图2。如图2所示，在进行提取之前，需要构建一个优先 队列joins，该优先队列中每一个元素表示将要连接的两个点的序号，该元素有三个特征 组成：父节点、子节点以及父子节点之间的距离。其中，父节点表示将要连接的两点中 的左面一个点的坐标，子节点表示将要连接的两点中的右面一个点的坐标，距离表示为 这两个点之间的欧氏距离。同时，该优先队列是按照的距离的大小进行排列的。提取轮 廓的第一步，要考察数据中的每一个点与其临近的点的距离，如果满足一定的距离要求， 就把它放入优先队列中，否则不予考虑。当全部数据点经过第一轮检验过后，进行提取 轮廓的循环内。如果优先队列已经为空，即表示已经检验过所有的点与点之间的连接关 系，则表明提取轮廓结束；否则，我们要进行下列步骤。首先，从优先队列中提取出最 好的一个元素（按照距离排序，距离越近表示越好），如果该父节点已经连接有其他子 节点，则表示该父节点作为左节点已经与其他距离更近的点（作为右节点）进行连接， 所以去掉该元素信息，不连接父子节点；如果该子节点已经连有其他父节点，则把该组 点信息放回优先队列，不连接父子节点；如果连接这两个节点需要付出的代价（如两个 点的距离）太大，则丢掉该组点信息，不连接。然后再判断优先队列是否为空，如果已 经为空，循环结束，得到轮廓信息；否则重复上面的三个判断过程。完成上述处理之后， 每一个点会与距离最近的一个点进行连接，作为该节点的后续节点，而该节点也有机会 成为其他节点的后续节点，最后形成了一个链状的轮廓，完成了轮廓的提取。

再次，将各条轮廓线标记出X个转折点，划分成X+1段的折线段,其中X等于扫描点数 目最少的轮廓线所包括扫描点数目减2。

由于在前一步提取轮廓时，少数散点之间的连线不能称之为轮廓线，只能被作为噪声点 删除,这意味着能成功定义的各条轮廓线中的扫描点的数目是有下限的，换言之，各条轮廓 线都至少包括特定数目的扫描点。将包含扫描点数目最少的轮廓线所包括扫描点数目减2， 记为X，然后对各条轮廓线标记出X个转折点，各条轮廓线被划分成X+1段的折线段。

例如，某时刻扫描后获得三条轮廓线line1、line2和line3，它们包含的扫描点数目分别 为16个、13个、4个，则扫描点数目最少的轮廓线line3所包括扫描点数目为4，于是X=4-2=2。 然后将line1、line2和line3中分别标记两个转折点、划分成三段折线段。标记转折点的时候， 一般是平均分布地标记。例如line1中选取第六个点、第十一个点为转折点；line2中选取第 五个点、第九个点为转折点；lin3仅有四个扫描点，恰好自成为三段折线段。但无论怎么划 分，至少保证每段折线段上有两个扫描点，可以表示该折线段的两端。

S2.提取多条轮廓线的特征，形成轮廓特征向量集合。

其中，轮廓特征向量记为（N,D,θ₁,θ₂,……θ_X）,N表示所述轮廓线包括的扫描点数目，D 表示所述轮廓线的长度值，θ₁至θ_X表示所述轮廓线中相邻折线段的夹角余弦值。其中，首 先将该轮廓线S包含的扫描点数目作为该轮廓特征向量的第一维；然后利用求和公式求取该 轮廓线的长度值D作为轮廓特征向量的第二维，最后利用折线段在固定坐标系（比如世界 坐标系）中的位置关系，分别求取相邻段的夹角的余弦值，作为轮廓特征向量的后面几维参 数。需要说明的是，前面已经介绍各条轮廓线都被标记出X个转折点，划分成X+1段折线 段，故夹角数目θ也为X。即使用扫描点个数、轮廓长度以及折线拟合获得的角度信息，来 获取特定维数轮廓特征向量。这样提取获得的轮廓特征向量的维数是固定的，即对于不同的 轮廓，进行折线拟合时，其使用的折线段的数目是固定的，形成的角度的余弦值的数目也是 固定的。而其折线段的数目可以根据获得轮廓时规定的每个轮廓包含的最小的数据点数目来 确定。

S3.对相邻两次扫描数据处理后的所述轮廓特征向量集合进行匹配，得到相邻时刻的轮 廓特征匹配结果，用于后续运动估计计算。换言之，以所述获得的轮廓特征向量作为特征“点”， 使用迭代算法，寻找相邻时刻两幅“地图”中轮廓特征向量的最优匹配。具体地：

首先，提供时刻k的轮廓特征向量集｛d_k,1,d_k,2,……d_k,m｝和时刻k+1的轮廓特征向量 集｛d_k+1,1,d_k+1,2,……d_k+1,n｝；

其次，选取时刻k中未匹配过的轮廓特征向量d_k,I，其中1≤I≤m，对于时刻k+1中的 各个轮廓特征向量d_k+1,i，其中1≤i≤n，计算|d_k,I-d_k+1,i|，选取使|d_k,I-d_k+1,i|取值最小的d_k+1,i， 作为时刻k的d_k在时刻k+1中的轮廓特征向量的最优匹配；

最后，重复前面的匹配步骤，直至时刻k中没有未匹配过的轮廓特征向量，则循环结束。

优选地，由于在实际情况中,θ表示的是折线段拟合后的轮廓的走势，更能够表现出该轮 廓的特征（但同一个轮廓特征向量中，各个夹角θ之间是平等的，故可以看成权重相同）， 故可以在上面的计算|d_k,I-d_k+1,i|，替换成下面的附带权值系数的计算方法。

设定μ_N,μ_D,μ_θ分别表示轮廓点数的权值、轮廓长度的权值、相邻两个折线段之间的 夹角余弦的权值。其中三者的系数不限定，只要令μ_θ最大即可（最大程度看夹角余弦权 重大小而定）。记d_k,I=（N_k,I,D_k,I,θ_k,I,1,θ_k,I,2,θ_k,I,3,……），以及d_k+1,i= （N_k+1,I,D_k+1,I,θ_k+1,i,1,θ_k+1,i,2,θ_k+1,i,3,……），则有：

${\left|\right|d_{k,I}-d_{k+1,i}\left|\right|}_{{\mu }_N,{\mu }_D,{\mu }_{\theta }}^2={\mu }_N{(N_{k,I}-N_{k+1,i})}^2+{\mu }_D{(D_{k,I}-D_{k+1,i})}^2+{\Sigma }_{x=1}^{x=X}{\mu }_{\theta }{({\theta }_{k,I,x}-{\theta }_{k+1,i,x})}^2,$

其中，x表示转折点的序号（也表示余弦值的序号）,1≤x≤X。

至此，可以得到的轮廓特征向量的最优匹配结果，通过分析在世界坐标系中，可以 考察相邻时刻的图像轮廓的位置变换，然后估计出激光测距机的从时刻k到时刻k+1的 运动估计。由于运动追踪过程不在该发明的算法中，故只作为提及，不在此展开说明。

本发明另一方面还提出一种用于激光测距机的扫描数据匹配装置，如图3所示，包括以 下部分：激光测距机100，轮廓线拟合模块200，轮廓特征向量提取模块300和匹配模块。 具体地：

激光测距机100用于进行扫描，获得扫描点集合。在本发明的一个实施例中，该激光测 距机为二维激光测距机，即扫描一次采样得到的是某一时刻在某一高度下的平面中的多个数 据点的信息。

轮廓线拟合模块200，用于根据扫描点集合，形成多条轮廓线。其中，该轮廓拟合模块 200进一步包括：聚类连接子模块210，该聚类连接子模块210根据扫描点之间的欧氏距离 进行聚类，同时将孤立的扫描点视为噪声点删除，建立扫描点之间的连接关系，将离散的扫 描点连接成轮廓线；折线段划分子模块220，该折线段划分子模块220将各条轮廓线标记出 X个转折点，划分成X+1段的折线段,其中X等于扫描点数目最少的轮廓线所包括扫描点数 目减2。

轮廓特征向量提取模块300，用于提取多条轮廓线的特征，形成轮廓特征向量集合。其 中，每条轮廓线对应一个轮廓特征向量。多条轮廓线的多条轮廓特征向量形成轮廓特征向量 集合。其中，轮廓特征向量记为（N,D,θ₁,θ₂,……θ_X）,其中，N表示轮廓线包括的扫描点数 目，D表示轮廓线的长度值，θ₁至θ_X表示轮廓线中相邻折线段的夹角余弦值。

匹配模块400，用于对相邻两次扫描数据处理后的轮廓特征向量集合进行匹配，得到相 邻时刻的轮廓特征匹配结果，用于后续运动估计计算。匹配模块400中，匹配算法为：根据 时刻k的轮廓特征向量集｛d_k,1,d_k,2,……d_k,m｝和时刻k+1的轮廓特征向量集｛d_k+1,1, d_k+1,2,……d_k+1,n｝，选取时刻k中未匹配过的轮廓特征向量d_k,I，其中1≤I≤m，对于时刻k+1 中的各个轮廓特征向量d_k+1,i，其中1≤i≤n，计算|d_k-d_k+1,i|，选取使|d_k,I-d_k+1,i|取值最小的 d_k+1,i，作为时刻k的d_k在时刻k+1中的轮廓特征向量的最优匹配。优选地，由于在实际 情况中,θ表示的是折线段拟合后的轮廓的走势，更能够表现出该轮廓的特征，故可以在计 算|d_k,I-d_k+1,i|过程中对不同维度的物理量附带权值系数进行向量距离的计算。

综上所述，根据本发明的用于激光测距机的扫描数据匹配方法及装置有如下优点：（1） 利用轮廓的特征来进行迭代特征匹配，减少了匹配的计算量，同时由于轮廓作为一个整体， 其特征比较明显，较点与点的匹配，可以提高匹配的准确度和匹配速度；（2）把每一个轮廓 用一个维数相同的轮廓特征向量表示，便于迭代计算向量差的模的大小，便于寻找最优匹配； （3）使用适当数量的折线段对轮廓进行拟合，既可以判断轮廓的走势特征，同时也可以忽 略掉一些细小的干扰以及细小的细节特征。综上，本发明可以有效地提高轮廓特征提取的速 度和匹配的精确度。

为使本领域技术人员更好地理解本发明，现结合图4-图6列举一详细实施例做介绍。

某实施例中，采用hokuyo激光测距机来获得二维扫描数据点的信息。

图4为该激光测距机在k时刻的扫描点、轮廓以及折线拟合示意图。如图所示，两条轮 廓线A和B均被划分成了六段折线，其中轮廓线A和轮廓线B的轮廓特征向量分别为 d_kA=(17,d_k,A,θ_k,A,1,θ_k,A,2,θ_k,A,3,θ_k,A,4,θ_k,A,5)和d_k,B=(18,d_k,B,θ_k,B,1,θ_k,B,2,θ_k,B,3,θ_k,B,4,θ_k,B,5)，为7 维轮廓特征向量，其中第一维表示该轮廓所含数据点数目，第二维表示该轮廓的长度，后面 五个θ值表示相邻两个折线的夹角的余弦值，下标k表示k时刻，下标A表示轮廓线A， 下标B表示轮廓线B；

图5为该激光测距机在k+1时刻的扫描点、轮廓以及折线拟合示意图。如图所示，两条 轮廓线A’和B’均被划分成了六段折线，其中轮廓线A’和轮廓线B’的轮廓特征向量分别为 d_k+1,A’=(17,d_k+1,A’,θ_k+1,A’,1,θ_k+1,A’,2,θ_k+1,A’,3,θ_k+1,A’,4,θ_k+1,A’,5)和 d_k+1,B’=(18,d_k+1,B’,θ_k+1,B’,1,θ_k+1,B’,2,θ_k+1,B’,3,θ_k+1,B’,4,θ_k,B’,5)，为7维轮廓特征向量，其中第一维 表示该轮廓所含数据点数目，第二维表示该轮廓的长度，后面五个θ值表示相邻两个折线的 夹角的余弦值，下标k+1表示k+1时刻，下标A’表示轮廓线A’，下标B’表示轮廓线B’。

图6为图4和图5中的轮廓线的特征匹配结果的示意图，图中可以看出,经过计算，可以 得到最优匹配为d_k+1,A’与d_k,A匹配，d_k+1,B’与d_k,B匹配。

需要说明的是，流程图中或在此以其他方式描述的任何过程或方法描述可以被理解 为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的 模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按 所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执 行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、 或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含 于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的 是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或 多个实施例或示例中以合适的方式结合。

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