CN110335282A - 一种基于栅格的轮廓线段特征提取算法 - Google Patents

Abstract

本发明涉及移动机器人的技术领域，更具体地，涉及一种基于栅格的轮廓线段特征提取算法，包括数据点的聚类、每个聚类内数据点的排序、每个聚类内数据点的分段、每个数据段内数据点进行线段拟合，前三项为本发明提出的搜寻线段支持区的方法，结合该方法再在支持区内用最小二乘法拟合线段。本发明算法在寻找线段支持区时，不需要较大的内存和较长的计算时间，在拟合线段时，拟合的方法简单；本发明能够降低对旋转激光扫描仪的性能要求，降低噪点对于提取效果的影响；且本发明不用设置阈值等参数，降低对旋转激光扫描仪的性能要求，降低噪点对提取效果的影响，提取线段的分辨率高、鲁棒性强、效率高、实时性强。

Description

一种基于栅格的轮廓线段特征提取算法

技术领域

本发明涉及移动机器人的技术领域，更具体地，涉及一种基于栅格的轮廓线段特征提取算法。

背景技术

同时定位和地图构建(SLAM：simultaneous localization and mapping)问题一直是移动机器人研究的重要方向，而栅格地图是SLAM问题中地图构建的常用方法。与边缘、角、区域、脊等其他环境特征相比，线段是数学上简单的中层描述符，利用它不但可以描述许多环境物体，而且提取复杂度较低。所以对栅格地图进行线段特征提取，是SLAM问题中很好的研究方向。

目前，线段类特征提取方法主要有PDBS(Point-Distance-Base Segmentation)、SEF(Successive Edge Following)、LT(Line Tracking)、IEPF(Iterative End PointFit)、SM(Split-and-Merge)等，然而这些提取方法都是对传感器的采样速度、数据有效性、连续型具有较高的要求，容易受噪点影响，且依赖于合适的阈值，环境的复杂性和多变性导致阈值选取困难。其中：PDBS算法与SEF算法依赖于激光扫描数据的连续性，若出现数据丢失或噪点，则容易误判，且由于激光数据对近处障碍物和远处障碍物具有不同的探测精度，因此算法中阈值dth的设定往往顾此失彼；LT算法容易将下一线段的数据点划入上一线段，造成线段端点处的误判；IEPF算法容易受到到噪点的影响，造成线段分割过多，且由于工作环境的复杂性和多变性，阈值的选取存在一定困难；SM算法对噪声和阈值敏感，容易得到错误的结果。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于栅格的轮廓线段特征提取算法，计算过程简单，可降低对旋转激光扫描仪的性能要求，同时降低噪点对提取效果的影响。

为解决上述技术问题，本发明采用的技术方案是：

提供一种基于栅格的轮廓线段特征提取算法，包括以下步骤：

S10.构建包括有若干障碍物栅格的栅格地图，提取障碍物栅格的中心点坐标组成数据点集M＝{P₁,P₂,....,P_N}；

S20.基于欧式距离的聚类方法，通过数据点之间的欧氏距离度量将点集M划分为若干个第一聚类点集M_i＝{P₁,P₂,...,P_K}；

S30.基于路径搜索和分支点判断对每个第一聚类点集M_i的数据点进行排序，得到排序后的第二聚类点集M_ii，M_ii内的数据点满足拓扑结构的先后顺序；

S40.将每个第二聚类点集M_ii内的数据点划分为若干个数据段S_j＝{P₁,P₂,...,P_J}，每个数据段内的数据点可拟合成一条线段；

S50.对每个数据段内的数据点采用最小二乘法进行线段拟合。

本发明的基于栅格的轮廓线段特征提取算法，通过聚集程度为障碍物栅格分区域，再计算位置关系为区域内障碍物栅格分组，最后对组内障碍物栅格进行线段拟合；本发明的算法在寻找线段支持区时，不需要较大的内存和较长的计算时间，在拟合线段时，拟合的方法简单；且本发明能够降低对旋转激光扫描仪的性能要求，降低噪点对于提取效果的影响。

优选地，步骤S10的实现方法是：逐行逐列扫描栅格地图，若扫描到的栅格为障碍物，则将所述栅格的中心点坐标作为数据点加入数据点集M。

优选地，步骤S20中，对M_i内每个数据点P_i在M_i内寻找至少一个相邻的数据点P_j满足0＜|P_j-P_i|≤1,P_i,P_j∈M_i。

优选地，步骤S20包括以下步骤：

S21.建立一个第一聚类点集M_i；

S22.从数据点集M中取出第一个数据点P₁，作为第一聚类点集M_i的第一个数据点，同时将P₁从数据点集M中删除；

S23.从第一聚类点集M_i中取出一个数据点P_j；

S24.逐点扫描数据点集M中的数据点P_i，若0＜|P_j-P_i|≤1，则将P_i加入第一聚类点集M_i中，并将P_i从数据点集M中删除；

S25.重复步骤S23～S24，直至遍历完第一聚类点集M_i中所有的数据点；

S26.重复步骤S21～S25，直至遍历完数据点集M中所有的数据点。

聚类算法的目的是将一组未知类标的数据样本进行划分，希望找到数据集中隐藏的潜在结构，并按照某种相似性度量，使得具有相似性质的数据归于同一类，不相似性质的数据尽可能分开，以便于对数据进行分析处理。

优选地，步骤S30包括以下步骤：

S31.建立一个第二聚类点集M_ii，选取第一聚类点集M_i＝{P₁,P₂,...,P_K}的最后一点P_K存入第二聚类点集M_ii，并将P_K从M_i中删除，标记M_ii的第一点为未遍历点；

S32.从第二聚类点集M_ii中取出第一个未遍历点P_ii，并将P_ii标记为已遍历点；

S33.按顺序搜索P_ii周围相邻的栅格P_i：

若P_i∈M_i，且P_i被判断为分支点，则将P_i存入分支点缓存点集B_ii，并将P_i从M_i删除；跳入下一步；

若P_i∈M_i，且P_i不是分支点，则将P_i加入M_ii，标记为未遍历点，同时将P_i从M_i删除，跳入下一步；

S34.重复S33，直至搜索完P_ii周围相邻的所有栅格；

S35.重复S32-S34，直至第二聚类点集M_ii中没有未遍历点；

S36.从B_ii中取出一点P_j添加到M_ii中，标记为未遍历点，并将P_j从B_ii删除；

S37.重复步骤S32～S36，直至分支点缓存点集B_ii中没有数据点。

优选地，步骤S33所述的分支点判定方法为：

若M_ii中存在至少一个数据点P_jj满足|P_i-P_jj|≥1且则判定P_i为分支点；

若M_ii中存在至少两个数据点P_jj满足则判定P_i为分支点。

聚类内的数据点之间不存在拓扑结构上的先后顺序，不利于后续的分析处理，本发明引入路径搜索和分支点判断，对M_i＝{P₁,P₂,....,P_K}的数据点进行排序，从而使得聚类内的数据点具有拓扑结构上的先后顺序，有利于后续算法分析。其中，分支点是指从该点出发，路径搜索可能会扩充到两条互不相交的路径；在路径扩充时，若遇到分支点，则将分支点缓存起来，将当前路径扩充完成后，再从分支点出发继续进行扩充；这样处理的目的是使得存入M_ii的数据点中，位于同一线段上的数据点在M_ii内总是按顺序排列。

优选地，步骤S40中包括以下步骤：

S41.取聚类M_ii中的首尾两点P₁,P_K作为端点，另A₀＝P₁,A₁＝P_K，数据段计数为0；

S42.以A₀,A₁为端点，作线段L，数据段计数n＝n+1；

S43.计算M_i内位于A₀,A₁间的所有数据点对L的垂直距离d，假设在数据点P_i处取得最大值d_max：若d_max＞dth，则令A₁＝P_i，跳到2，dth为设定的阈值；若d_max≤dth，则将M_i内A₀,A₁及它们之间的数据点存入数据段S_n，并将以上数据点从M_ii删除，令A₀＝P₁,A₁＝P_K，跳到2；

S44.重复步骤S42～S43，直到遍历完M_ii中所有数据点。

聚类内的数据点可能包含多个线段特征，分段的目的是将M_ii的数据点划分为若干个数据段S_j，使得S_j内的数据点满足：以首尾两点P₁,P_J作直线L，S_j内其它数据点P₂,...,P_J-1到该直线L的距离均小于设定的阈值。

优选地，步骤S50按以下步骤进行：

S51.对于点集{P₁(x₁,y₁),P₂(x₂,y₂),...,P_N(x_N,y_N)}，按下式计算a、b、c的值：

c＝-aμ_x-bμ_y

式中，μ_x是x_i的均值，μ_y是y_i的均值，μ_xx与是x_i的方差，μ_yy是y_i的方差，μ_xy是x_i和y_i的协方差；

S52.将分段点集S_j＝{P₁,P₂,...,P_J}代入步骤S51中公式，可得到拟合的直线：

a_jx+b_jy+c_j＝0

S53.将点集S_j的首尾两点P₁(x₁,y₁),P_J(x_J,y_J)投影到直线a_jx+b_jy+c_j＝0，得到直线端点E₀(x_e0,y_e0),E₁(x_e1,y_e1)：

与现有技术相比，本发明的有益效果是：

(1)本发明通过聚类、排序、分段的方法搜寻栅格地图中的线段支持区，不需要较大的内存和较长的计算时间，且编程实现简单代码量较少，可以节省存储空间，比较适用于存储空间紧张且计算能力较弱的设备；且本发明算法的搜寻效果好，符合人眼的观察标准；

(2)本发明计算简单，不用设置阈值等参数，降低对旋转激光扫描仪的性能要求，同时降低噪点对提取效果的影响，提取线段的分辨率高、鲁棒性强。

附图说明

图1为本发明的基于栅格的轮廓线段特征提取算法的流程图。

图2为实施例一基于栅格的轮廓线段特征提取算法的相邻栅格搜索顺序的示意图。

图3为实施例一基于栅格的轮廓线段特征提取算法的分支点判定的示意图。

图4为实施例一基于栅格的轮廓线段特征提取算法的分段的示意图。

图5为实施例二中单矩形空间的实景图及栅格地图。

图6为实施例二中多矩形空间的实景图及栅格地图。

具体实施方式

下面结合具体实施方式对本发明作进一步的说明。其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

实施例一

如图1至图4所示为本发明的基于栅格的轮廓线段特征提取算法的实施例，包括以下步骤：

S10.构建包括有若干障碍物栅格的栅格地图，提取障碍物栅格的中心点坐标组成数据点集M＝{P₁,P₂,....,P_N}；

S20.基于欧式距离的聚类方法，通过数据点之间的欧氏距离度量将点集M划分为若干个第一聚类点集M_i＝{P₁,P₂,...,P_K}；

S30.基于路径搜索和分支点判断对每个第一聚类点集M_i的数据点进行排序，得到排序后的第二聚类点集M_ii，M_ii内的数据点满足拓扑结构的先后顺序；

S40.将每个第二聚类点集M_ii内的数据点划分为若干个数据段S_j＝{P₁,P₂,...,P_J}，每个数据段内的数据点可拟合成一条线段；

S50.对每个数据段内的数据点采用最小二乘法进行线段拟合。

步骤S10中，其通过以下方法实现：逐行逐列扫描栅格地图，若扫描到的栅格为障碍物，则将所述栅格的中心点坐标作为数据点加入数据点集M。

步骤S20中，对M_i内每个数据点P_i寻找至少一个相邻的数据点P_j满足0＜|P_j-P_i|≤1,P_i,P_j∈M_i；步骤S20具体按以下步骤实施：

S21.建立一个第一聚类点集M_i；

S22.从数据点集M中取出第一个数据点P₁，作为第一聚类点集M_i的第一个数据点，同时将P₁从数据点集M中删除；

S23.从第一聚类点集M_i中取出一个数据点P_j；

S24.逐点扫描数据点集M中的数据点P_i，若0＜|P_j-P_i|≤1，则将P_i加入第一聚类点集M_i中，并将P_i从数据点集M中删除；

S25.重复步骤S23～S24，直至遍历完第一聚类点集M_i中所有的数据点；

S26.重复步骤S21～S25，直至遍历完数据点集M中所有的数据点。

聚类算法的目的是将一组未知类标的数据样本进行划分，希望找到数据集中隐藏的潜在结构，并按照某种相似性度量，使得具有相似性质的数据归于同一类，不相似性质的数据尽可能分开，以便于对数据进行分析处理。

步骤S30按以下步骤进行：

S31.建立一个第二聚类点集M_ii，选取聚类M_i＝{P₁,P₂,...,P_K}的最后一点P_K存入第二聚类点集M_ii，并将P_K从M_i中删除，标记M_ii的第一点为未遍历点；

S32.从第二聚类点集M_ii中取出第一个未遍历点P_ii，并将P_ii标记为已遍历点；

S33.按顺序搜索P_ii周围相邻的栅格P_i：

若P_i∈M_i，且P_i被判断为分支点，则将P_i存入分支点缓存点集B_ii，并将P_i从M_i删除；跳入下一步；

若P_i∈M_i，且P_i不是分支点，则将P_i加入M_ii，标记为未遍历点，同时将P_i从M_i删除，跳入下一步；

S34.重复S33，直至搜索完P_ii周围相邻的所有栅格；

S35.重复S32-S34，直至第二聚类点集M_ii中没有未遍历点；

S36.从B_ii中取出一点P_j添加到M_ii中，标记为未遍历点，并将P_j从B_ii删除；

S37.重复步骤S32～S36，直至分支点缓存点集B_ii中没有数据点。

步骤S33中，本实施例以P_ii周围有8个相邻的栅格为例进行说明，但本发明并不局限于此。按顺序搜索P_ii周围相邻的8个栅格，其搜索顺序如图2所示，从编号1的栅格顺次搜索至编号为8的栅格。

步骤S33中，如图3所示，从M_ii中取出一点P_ii作为出发点，则分支点的判定方法为：

若M_ii中存在至少一个数据点P_jj满足|P_i-P_jj|≥1且则判定P_i为分支点；

若M_ii中存在至少两个数据点P_jj满足则判定P_i为分支点；

步骤S40按以下步骤进行，表示为图4：

S41.取聚类M_ii中的首尾两点P₁,P_K作为端点，另A₀＝P₁,A₁＝P_K，数据段计数为0；

S42.以A₀,A₁为端点，作线段L，数据段计数n＝n+1；

S43.计算M_ii内位于A₀,A₁间的所有数据点对L的垂直距离d，假设在数据点P_i处取得最大值d_max：若d_max＞dth，则令A₁＝P_i，跳到2，dth为设定的阈值；若d_max≤dth，则将M_i内A₀,A₁及它们之间的数据点存入数据段S_n，并将以上数据点从M_i删除，令A₀＝P₁,A₁＝P_K，跳到2；

S44.重复步骤S42～S43，直到遍历完M_i中所有数据点。

步骤S50按以下步骤进行：

S51.对于点集{P₁(x₁,y₁),P₂(x₂,y₂),...,P_N(x_N,y_N)}，按下式计算a、b、c的值：

c＝-aμ_x-bμ_y

式中，μ_x是x_i的均值，μ_y是y_i的均值，μ_xx与是x_i的方差，μ_yy是y_i的方差，μ_xy是x_i和y_i的协方差；

S52.将分段点集S_j＝{P₁,P₂,...,P_J}代入步骤S51中公式，可得到拟合的直线：

a_jx+b_jy+c_j＝0

S53.将点集S_j的首尾两点P₁(x₁,y₁),P_J(x_J,y_J)投影到直线a_jx+b_jy+c_j＝0，得到直线端点E₀(x_e0,y_e0),E₁(x_e1,y_e1)：

为了从点集{P₁(x₁,y₁),P₂(x₂,y₂),...,P_N(x_N,y_N)}中拟合出一条Hessian范式表示的直线ax+by+c＝0，需要将下式的误差最小化：

实施例二

如图5、图6所示为本发明的基于栅格的轮廓线段特征提取算法的实施例，旨在验证本发明算法提取线段的可靠性和有效性。模拟搭建不同的实验环境，采用本发明算法进行测试。如图5、图6所示分别为单矩形空间、多矩形空间的实景图及栅格地图，左侧是测试环境实景图，右侧是移动机器人生成的栅格地图及线段特征；栅格地图中，白色表示栅格状态为未知，蓝色表示栅格状态为障碍物，绿色表示栅格状态为已清扫，黄色表示栅格状态为空白。测试结果表明，在障碍物分布复杂、密集且不规则的环境下，算法仍能稳健地搜寻到合适的线段拟合区域，完成线段提取，提取出的线段与观察结果相符。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (8)

1.一种基于栅格的轮廓线段特征提取算法，其特征在于，包括以下步骤：

S10.构建包括有若干障碍物栅格的栅格地图，提取障碍物栅格的中心点坐标组成数据点集M＝{P₁,P₂,....,P_N}；

S20.基于欧式距离的聚类方法，通过数据点之间的欧氏距离度量将点集M划分为若干个第一聚类点集M_i＝{P₁,P₂,...,P_K}；

S30.基于路径搜索和分支点判断对每个第一聚类点集M_i的数据点进行排序，得到排序后的第二聚类点集M_ii，M_ii内的数据点满足拓扑结构的先后顺序；

S40.将每个第二聚类点集M_ii内的数据点划分为若干个数据段S_j＝{P₁,P₂,...,P_J}，每个数据段内的数据点可拟合成一条线段；

S50.对每个数据段内的数据点采用最小二乘法进行线段拟合。

2.根据权利要求1所述的基于栅格地轮廓线段特征提取算法，其特征在于，步骤S10的实现方法是：逐行逐列扫描栅格地图，若扫描到的栅格为障碍物，则将所述栅格的中心点坐标作为数据点加入数据点集M。

3.根据权利要求1所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S20中，对M_i内每个数据点P_i在M_i内寻找至少一个相邻的数据点P_j满足0＜|P_j-P_i|≤1,P_i,P_j∈M_i。

4.根据权利要求3所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S20包括以下步骤：

S21.建立一个第一聚类点集M_i；

S22.从数据点集M中取出第一个数据点P₁，作为第一聚类点集M_i的第一个数据点，同时将P₁从数据点集M中删除；

S23.从第一聚类点集M_i中取出一个数据点P_j；

S24.逐点扫描数据点集M中的数据点P_i，若0＜|P_j-P_i|≤1，则将P_i加入第一聚类点集M_i中，并将P_i从数据点集M中删除；

S25.重复步骤S23～S24，直至遍历完第一聚类点集M_i中所有的数据点；

S26.重复步骤S21～S25，直至遍历完数据点集M中所有的数据点。

5.根据权利要求1所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S30包括以下步骤：

S31.建立一个第二聚类点集M_ii，选取第一聚类点集M_i＝{P₁,P₂,...,P_K}的最后一点P_K存入第二聚类点集M_ii，并将P_K从M_i中删除，标记M_ii的第一点为未遍历点；

S32.从第二聚类点集M_ii中取出第一个未遍历点P_ii，并将P_ii标记为已遍历点；

S33.按顺序搜索P_ii周围相邻的栅格P_i：

若P_i∈M_i，且P_i被判断为分支点，则将P_i存入分支点缓存点集B_ii，并将P_i从M_i删除；跳入下一步；

若P_i∈M_i，且P_i不是分支点，则将P_i加入M_ii，标记为未遍历点，同时将P_i从M_i删除，跳入下一步；

S34.重复S33，直至搜索完P_ii周围相邻的所有栅格；

S35.重复S32-S34，直至第二聚类点集M_ii中没有未遍历点；

S36.从B_ii中取出一点P_j添加到M_ii中，标记为未遍历点，并将P_j从B_ii删除；

S37.重复步骤S32～S36，直至分支点缓存点集B_ii中没有数据点。

6.根据权利要求5所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S33所述的分支点判定方法为：

若M_ii中存在至少一个数据点P_jj满足|P_i-P_jj|≥1且则判定P_i为分支点；

若M_ii中存在至少两个数据点P_jj满足则判定P_i为分支点。

7.根据权利要求1所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S40中包括以下步骤：

S41.取第二聚类点集M_ii中的首尾两点P₁,P_K作为端点，令A₀＝P₁,A₁＝P_K，数据段计数为0；

S42.以A₀,A₁为端点，作线段L，数据段计数n＝n+1；

S43.计算M_ii内位于A₀,A₁间的所有数据点对L的垂直距离d，假设在数据点P_i处取得最大值d_max：若d_max＞dth，则令A₁＝P_i，跳到2，dth为设定的阈值；若d_max≤dth，则将M_i内A₀,A₁及它们之间的数据点存入数据段S_n，并将以上数据点从M_i删除，令A₀＝P₁,A₁＝P_K，跳到2；

S44.重复步骤S42～S43，直到遍历完M_i中所有数据点。

8.根据权利要求1至7任一项所述的基于栅格的轮廓线段特征提取算法，其特征在于，步骤S50按以下步骤进行：

S51.对于点集{P₁(x₁,y₁),P₂(x₂,y₂),...,P_N(x_N,y_N)}，按下式计算a、b、c的值：

c＝-aμ_x-bμ_y

式中，μ_x是x_i的均值，μ_y是y_i的均值，μ_xx与是x_i的方差，μ_yy是y_i的方差，μ_xy是x_i和y_i的协方差；

S52.将分段点集S_j＝{P₁,P₂,...,P_J}代入步骤S51中公式，可得到拟合的直线：

a_jx+b_jy+c_j＝0

S53.将点集S_j的首尾两点P₁(x₁,y₁),P_J(x_J,y_J)投影到直线a_jx+b_jy+c_j＝0，得到直线端点E₀(x_e0,y_e0),E₁(x_e1,y_e1)：