基于分组阶梯式阈值判断的激光点云匹配方法与流程

本发明涉及机器人导航领域，尤其涉及一种基于分组阶梯式阈值判断的激光点云匹配方法。

背景技术：

目前，机器人定位技术广泛应用于园区巡检，仓储搬运等领域中，机器人自主定位导航技术的应用可以有效代替人完成部分作业，因此，机器人的定位导航技术式当前的研究热点。

在机器人导航过程中，通过激光雷达对周围环境进行扫描，实现对机器人自身的定位。其中，机器人定位技术的难点在于对周围障碍物的识别并成功匹配。例如，在激光雷达扫描过程中，激光雷达会在不同时间、不同位置对同一障碍物进行扫描，需要将这两组或两组以上的点云进行匹配，来成功匹配出环境中的障碍物。现有技术常使用迭代最近点(icp)算法对周围环境的障碍物进行扫描匹配，是一种点集对点集的匹配方法。迭代最近点法是将同一障碍物扫描的两组点云，通过旋转变换，使两组点云最大化地重叠，完成匹配。

在进行迭代最近点匹配过程中，需要对一组点云的每一个点进行最近邻点匹配，计算量大，且可能会陷入局部最优问题。在传统的icp算法中，在寻找对应点的时候，认为欧式距离最近的点就是对应点，这种假设有可能会产生一定数量的错误对应点。由于迭代最近点计算量大等问题存在，导致机器人障碍物匹配实时性低，定位效果差，不能较好地完成避障功能。

技术实现要素：

针对上述现有技术中的不足，本发明提供一种基于分组阶梯式阈值判断的激光点云匹配方法，可以在不降低定位精度的情况下，降低算法计算量。

为了实现上述目的，本发明提供一种基于分组阶梯式阈值判断的激光点云匹配方法，包括步骤：

s1：在当前时刻获取一激光雷达的前一帧的第一点云数据m和后一帧的第二点云数据n；

s2：将当前所述第一点云数据m和所述第二点云数据n分别分成固定数组的第一云数据子组和第二云数据子组；所述第一云数据子组和所述第二云数据子组分别包括多个点云；

s3：对当前所述第一点云数据m和所述第二点云数据n的每一所述点云进行迭代最近点匹配；

s4：判断所述第一云数据子组和所述第二云数据子组的匹配率是否大于第一预设阈值；如大于，所述第一云数据子组与所述第二云数据子组匹配成功，继续后续步骤，否则，所述第一云数据子组与所述第二云数据子组匹配失败，舍弃当前所述第一云数据子组与所述第二云数据子组；

s5：判断所述第一点云数据m和所述第二点云数据n的匹配成功的匹配组率是否大于第二预设阈值；如大于，匹配成功，结束步骤，否则继续后续步骤；

s6：获取下一时刻的所述激光雷达的前后两帧的两组点云数据，分别作为新的所述第一点云数据m和所述第二点云数据n，返回步骤s2。

优选地，所述s3步骤进一步包括步骤：

s31：利用公式(1)计算当前所述第一点云数据m和所述第二点云数据n的各所述点云的质心：

其中，μm表示第一点云数据m中第i组第j个点云的质心；μn表示第二点云数据n中第i组第j个点云的质心；d表示所述第一云数据子组和所述第二云数据子组包含的每个点集点的实际个数；mij表示第一点云数据m中第i组第j个点云；nij表示第二点云数据m中第i组第j个点云；i、j表示大于零的自然数；

s32：自各所述点云中去除所述质心，获得更新后的所述第一点云数据m’和更新后的所述第二点云数据n’；

s33：利用公式(2)和更新后的所述第一点云数据m’和更新后的所述第二点云数据n’计算获得第一变换矩阵u和第二变换矩阵v：

其中，w表示求解奇异值分解的矩阵；m′ij表示更新后的所述第一点云数据m’的第i组第j个点云的点集合；n′ij表示更新后的所述第二点云数据n’的第i组第j个点云的点集合；t表示转置；σ1表示分解矩阵w的奇异值；σ2表示分解矩阵w的奇异值；σ3表示分解矩阵w的奇异值；

s34：当rank(w)＝3求得所述第一变换矩阵u和所述第二变换矩阵v的唯一解；

s35：利用公式(3)计算获得一变换矩阵r和一平移矩阵t′；

s36：利用所述变换矩阵r和所述平移矩阵t′计算获得n″ij，n″ij表示更新后的所述第二点云数据n’的第i组第j个点云的理论值。

优选地，所述s4步骤进一步包括步骤：

s41：计算更新后的所述第一点云数据m’的第i组第j个点云m′ij与n″ij的距离；

s42：利用公式(4)判断当前所述第一云数据子组和所述第二点云数据n对应位置的点云是否匹配合格；

其中，p(i)表示匹配系数；d(i)表示m′ij与n″ij的距离；e表示预设距离阈值；当p(i)值为0时表示匹配失败；当p(i)值为1时表示当前对应位置的两点云匹配合格，记录点云匹配合格的数量；

s43：计算当前第一云数据子组和所述第二点云数据n的匹配率，所述匹配率等于当前第一云数据子组中点云匹配合格的数量除以当前第一云数据子组中点云总数；

s44：判断所述匹配率是否大于所述第一预设阈值；如大于，输出当前所述第一点云数据m和所述第二点云数据n并继续后续步骤，否则舍弃当前所述第一点云数据m和所述第二点云数据n。

优选地，所述匹配组率等于所述第一云数据子组与所述第二云数据子组匹配成功的组数除以所述固定数。

优选地，所述第一云数据子组与所述第二云数据子组匹配成功后还包括步骤：输出当前所述第一云数据子组与所述第二云数据子组，并记录所述第一云数据子组与所述第二云数据子组匹配成功的组数。

本发明由于采用了以上技术方案，使其具有以下有益效果：

本发明将第一点云数据m和第二点云数据n根据扫描的时间先后顺序从头至尾分别平均分为k组，在m和n之间对应的每一组的点云来进行最近邻匹配，是将每组的每一个点云进行匹配，且是将激光雷达点云进行分组匹配，可以在不降低定位精度的情况下，降低算法计算量，并在运用于机器人路径规划时，可有效改善机器人的避障功能。

附图说明

图1为本发明实施例的基于分组阶梯式阈值判断的激光点云匹配方法的流程图。

具体实施方式

下面根据附图1，给出本发明的较佳实施例，并予以详细描述，使能更好地理解本发明的功能、特点。

请参阅图1，本发明实施例的一种基于分组阶梯式阈值判断的激光点云匹配方法，包括步骤：

s1：在当前时刻获取一激光雷达的前一帧的第一点云数据m和后一帧的第二点云数据n。

s2：将当前第一点云数据m和第二点云数据n分别分成固定数组的第一云数据子组和第二云数据子组；第一云数据子组和第二云数据子组分别包括多个点云。

s3：对当前第一点云数据m和第二点云数据n的每一点云进行迭代最近点匹配。

其中，s3步骤进一步包括步骤：

s31：利用公式(1)计算当前第一点云数据m和第二点云数据n的各点云的质心：

其中，μm表示第一点云数据m中第i组第j个点云的质心；μn表示第二点云数据n中第i组第j个点云的质心；d表示第一云数据子组和第二云数据子组包含的每个点集点的实际个数；mij表示第一点云数据m中第i组第j个点云；nij表示第二点云数据m中第i组第j个点云；i、j表示大于零的自然数；

s32：自各点云中去除质心，获得更新后的第一点云数据m’和更新后的第二点云数据n’；

s33：利用公式(2)和更新后的第一点云数据m’和更新后的第二点云数据n’计算获得第一变换矩阵u和第二变换矩阵v：

其中，w表示求解奇异值分解的矩阵；m′ij表示更新后的第一点云数据m’的第i组第j个点云的点集合；n′ij表示更新后的第二点云数据n’的第i组第j个点云的点集合；t表示转置；σ1表示分解矩阵w的奇异值；σ2表示分解矩阵w的奇异值；σ3表示分解矩阵w的奇异值；

s34：当rank(w)＝3求得第一变换矩阵u和第二变换矩阵v的唯一解；

s35：利用公式(3)计算获得一变换矩阵r和一平移矩阵t′；

s36：利用变换矩阵r和平移矩阵t′计算获得n″ij，n″ij表示更新后的第二点云数据n’的第i组第j个点云的理论值。

s4：判断第一云数据子组和第二云数据子组的匹配率是否大于一第一预设阈值；如大于，第一云数据子组与第二云数据子组匹配成功，继续后续步骤，否则，第一云数据子组与第二云数据子组匹配失败，舍弃当前第一云数据子组与第二云数据子组。

其中，s4步骤进一步包括步骤：

s41：计算更新后的第一点云数据m’的第i组第j个点云m′ij与n″ij的距离；

s42：利用公式(4)判断当前第一云数据子组和第二点云数据n对应位置的点云是否匹配合格；

其中，p(i)表示匹配系数；d(i)表示m′ij与n″ij的距离；e表示预设距离阈值；当p(i)值为0时表示匹配失败；当p(i)值为1时表示当前对应位置的两点云匹配合格，记录点云匹配合格的数量；

s43：计算当前第一云数据子组和第二点云数据n的匹配率，匹配率等于当前第一云数据子组中点云匹配合格的数量除以当前第一云数据子组中点云总数；

s44：判断匹配率是否大于第一预设阈值；如大于，输出当前第一点云数据m和第二点云数据n并继续后续步骤，否则舍弃当前第一点云数据m和第二点云数据n。

优选地，匹配组率等于第一云数据子组与第二云数据子组匹配成功的组数除以固定数。

优选地，第一云数据子组与第二云数据子组匹配成功后还包括步骤：输出当前第一云数据子组与第二云数据子组，并记录第一云数据子组与第二云数据子组匹配成功的组数。

s5：判断第一点云数据m和第二点云数据n的匹配成功的匹配组率是否大于一第二预设阈值；如大于，匹配成功，结束步骤，否则继续后续步骤；

s6：获取下一时刻的激光雷达的前后两帧的两组点云数据，分别作为新的第一点云数据m和第二点云数据n，返回步骤s2。

本发明提供一种基于分组阶梯式阈值判断的激光点云匹配方法，利用激光雷达来获取周围环境的实时点云信息，可将实时点云信息与已知地图所匹配，以此来得到机器人在地图中的当前位置。在点云匹配过程中，本发明利用分组阶梯式阈值判断法来进行实时点云的匹配。输入激光雷达前后两帧点云数据，对点云数据进行分组，将点云数据进行组对组匹配。若该组点云匹配率达到要求，则判定该组点云匹配成功。若点云的匹配组率达到要求，则判定两点集点云匹配成功。

例如：

ⅰ、输入激光雷达前后两帧点云数据，对点云数据进行分组。

(1.1)首先将激光雷达的前后两帧扫描作为点云数据，分别作为m点集和n点集。

(1.2)将m点集和n点集分别分成f组，记为m1,m2,m3...mk和n1,n2,n3...nk，每个点集合点的个数为d。

ii、对每组点云进行icp匹配。

(2.1)将mij和nij点云进行icp匹配，(mij表示m点集中第i组的第j个点云)利用式(1)计算两个点云mij和nij的质心：

从两个点集中分别去掉对应的质心得到新的点集m′ij,n′ij。

(2.2)利用svd(singularvaluedecomposition)分解(奇异值分解)来求得变换矩阵,如下列式(2)来求得u，v。

若rank(w)＝3(矩阵的秩)，则求得解唯一，以此来利用式(3)来求得旋转变换矩阵r和平移矩阵t′.

(2.3)利用r和t求得n′ij，比较m′ij，n′ij的距离d。

其中，p(i)用来判断点匹配是否合格，e为经过试验得到的距离阈值，试验中为5mm。

(2.4)若点匹配未达到距离阈值，则对位置进行旋转平移后的点云重新进行匹配，重新进行(2.1)到(2.3)的过程，进行反复迭代。若点云的距离小于阈值e，则判定两点云匹配成功。

iii、对每组点云的匹配率和点云匹配组数进行阈值判断，是否达到要求。

(3.1)若mi，ni的匹配率达到阈值ζ(匹配率为该组成功点云数除以该组点云总数)，停止迭代，则表明这两组对应点云匹配成功。

若m，n的匹配组率(匹配成功组除以总的组数)达到阈值β，停止迭代，则表明m，n点集匹配成功。

以上结合附图实施例对本发明进行了详细说明，本领域中普通技术人员可根据上述说明对本发明做出种种变化例。因而，实施例中的某些细节不应构成对本发明的限定，本发明将以所附权利要求书界定的范围作为本发明的保护范围。