基于倒角距离形状匹配的AGV初始定位方法及定位系统与流程

本申请属于机器人导航与定位技术领域，具体涉及一种基于倒角距离形状匹配度的agv初始定位方法及定位系统。

背景技术：

随着工业自动化程度的提高，各种先进技术和设备的应用愈发普及，越来越多的工厂和仓储车间由劳动密集型转向技术密集型，实现了智能化生产和运输，由传统模式向智能化、现代化升级。特别是制造强国战略的实施，使得agv(automatedguidedvehicle，自动导引车)在自动化生产和智能物流中的使用呈现爆发式增长，越来越多agv的应用于汽车制造、电力、烟草、医疗等行业。

导航定位技术作为agv系统中最为核心的技术，是agv性能的决定性因素。目前商用产品中最为常见的技术主要包括：电磁感应导航、光学导航、激光导航、惯性导航以及自然轮廓导航。自然轮廓导航是当前最为先进的技术，该技术利用二维lidar对现场环境进行测量，自动生成二维环境栅格地图，然后通过二维lidar扫描数据与环境栅格地图进行匹配，从而实现自动导航功能。与电磁导航、光学导航、激光导航、惯性导航等方法相比，基于自然轮廓导航的方法无需布设其他辅助设施，不受周围环境磁场、污染、遮挡等影响，无需因环境改变而进行硬件设备的变动，具有成本低、灵活性高、易于扩展、鲁棒性强等优点。特别是slam(simultaneouslocalizationandmapping，同步定位与地图构建)技术在移动机器人导航定位领域得到了广泛而深入的研究，使得自然轮廓导航已逐步成为agv导航定位的主流技术方案。

定位问题可以描述为在线估计连续时空中的位置，对agv而言就是获取其在全局坐标系下的位置。通常，定位问题包括初始定位和跟踪定位。对于跟踪定位，agv已知其初始位置，根据其上一时刻位置并考虑运动误差来估计当前位置，这一类问题得到了广泛研究，如基于卡尔曼滤波、粒子滤波的跟踪定位。然而初始位置并不总是已知值，当初始位置未知时跟踪定位将变得非常困难，还可能导致定位收敛时间过长或失败。因此，在跟踪定位之前需要先进行初始定位，在初始定位过程中，根据已知环境栅格地图和当前单帧lidar数据来估计agv当前的大致位置。

在视觉slam中，通常利用从影像中所获取的特征来确定进行初始定位，即词袋(bagofwords)方法。然而，对于2dlidar数据，由于其距离数据缺乏纹理信息，因此无法根据距离数据构建具有显著性的特征描述，也就无法确定agv的初始位置。

技术实现要素：

为至少在一定程度上克服相关技术中存在的问题，本申请提供了一种基于倒角距离形状匹配度的agv初始定位方法及定位系统。

根据本申请实施例的第一方面，本申请提供了一种基于倒角距离形状匹配度的agv初始定位方法，其包括以下步骤：

根据已建成的环境栅格地图生成距离地图；

对2dlidar点云进行预处理；

将预处理后的2dlidar点云与生成的距离地图进行匹配搜索，得到avg的初始位置和方向角。

上述基于倒角距离形状匹配的agv初始定位方法中，所述根据已建成的环境栅格地图生成距离地图的过程为：

生成与原环境栅格地图相同尺寸的距离地图，将所有障碍物对应的栅格的值置为0，将所有自由栅格的值置为∞；

对环境栅格地图中的当前扫描到的栅格的8邻域栅格进行前向扫描；

对环境栅格地图中的当前扫描到的栅格的8邻域栅格进行后向扫描；

采用高斯核对距离地图中的各个栅格值进行处理，得到高斯距离图；

对高斯距离图进行降采样处理，得到低分辨率的距离地图。

进一步地，所述对环境栅格地图中的当前扫描到的栅格的8邻域栅格进行前向扫描时，

按照下式计算距离值d0,d1,d2,d3,d4，

式中，p0表示当前扫描到的栅格，p0表示当前扫描到的栅格p0到与其最近障碍物栅格的距离；

p1,p2,p3,p4表示当前扫描到的栅格p0的8邻域栅格中左侧和上方的栅格；

p1表示当前扫描时8邻域栅格中栅格p1与其最近障碍物栅格的距离，dist(p0,p1)表示栅格p1与p0之间的欧式距离；

p2表示当前扫描时8邻域栅格中栅格p2与其最近障碍物栅格的距离，dist(p0,p2)表示栅格p2与p0之间的欧式距离；

p3表示当前扫描时8邻域栅格中栅格p3与其最近障碍物栅格的距离，dist(p0,p3)表示栅格p3与p0之间的欧式距离；

p4表示当前扫描时8邻域栅格中栅格p4与其最近障碍物栅格的距离，dist(p0,p4)表示栅格p4与p0之间的欧式距离；

通过比较得到d0～d4中的最小值，并用该最小值对栅格p0到与其最近障碍物栅格的距离p0进行更新。

更进一步地，所述对环境栅格地图中的当前扫描到的栅格的8邻域栅格进行后向扫描时，

按照下式计算距离值d0,d5,d6,d7,d8，

式中，p5,p6,p7,p8表示当前扫描到的栅格p0的8邻域栅格中右侧和下方的栅格；

p5表示当前扫描时8邻域栅格中栅格p5与其最近障碍物栅格的距离，dist(p0,p5)表示栅格p5与p0之间的欧式距离；

p6表示当前扫描时8邻域栅格中栅格p6与其最近障碍物栅格的距离，dist(p0,p6)表示栅格p6与p0之间的欧式距离；

p7表示当前扫描时8邻域栅格中栅格p7与其最近障碍物栅格的距离，dist(p0,p7)表示栅格p7与p0之间的欧式距离；

p8表示当前扫描时8邻域栅格中栅格p8与其最近障碍物栅格的距离，dist(p0,p8)表示栅格p8与p0之间的欧式距离。

通过比较得到d0与d5～d8中的最小值，并用该最小值对栅格p0到与其最近障碍物栅格的距离p0进行更新。

进一步地，所述高斯距离图为：

式中，p为栅格p的值。

上述基于倒角距离形状匹配的agv初始定位方法中，所述对2dlidar点云进行预处理的过程为：

对各扫描点是否属于同一连续障碍物进行判断；

通过对表示同一障碍物的连续点的数量与预设点数进行比较，滤除点云中的孤立点。

进一步地，所述对各扫描点是否属于同一连续障碍物进行判断的过程为：

从第一个扫描点开始，依次计算后一个点与前一个点之间的欧式距离；

如果两点之间的欧式距离大于距离阈值，则判定该点与前一个点不属于同一连续障碍物；

如果两点之间的欧式距离小于或等于距离阈值，则判定该点与前一个点属于同一连续障碍物。

更进一步地，所述距离阈值dth为：

式中，ρn-1表示lidar到点pn-1的距离，点pn与pn-1是时序上相邻的两点，表示扫描角分辨率，λ表示入射角，σ表示测距噪声。

进一步地，所述滤除点云中的孤立点的过程为：

将表示同一障碍物的连续点的数量与预设点数进行比较；

如果表示同一障碍物的连续点的数量小于预设点数，则将表示同一障碍物的连续点判定为孤立点，并将其从点云中滤除。

上述基于倒角距离形状匹配的agv初始定位方法中，所述将预处理后的2dlidar点云与生成的距离地图进行匹配搜索的过程为：

对预处理后的lidar点云进行刚体变换；

在低分辨率的距离地图上进行初始匹配搜索，得到agv的最大可能的初始位置和方向角；

在原始分辨率的距离地图上进行二次匹配搜索，得到agv最终的初始位置和方向角。

进一步地，所述在低分辨率的距离地图上进行初始匹配搜索的过程为：

确定一个平移和旋转搜索范围，在该搜索范围内按照预定的初始搜索间隔δt0和δθ0，根据下式对扫描点进行刚体变换：

式中，[xi,yi]表示点pi的原始坐标，[xti,yti]表示变换后点pi的坐标，[tx,ty,θ]表示刚体变换的平移和旋转参数；

确定变换后的点在距离地图上的对应栅格[ri,ci]：

式中，δ表示距离地图的分辨率，[]表示取整；

将低分辨率的距离地图上所有对应栅格的高斯距离值g[ri,ci]进行累加，得到匹配度：

对于每一组刚体变换的平移和旋转参数[tx,ty,θ]都对应得到一个匹配度，将最大匹配度对应的[tx,ty,θ]作为agv的最大可能的初始位置和方向角。

更进一步地，所述在原始分辨率的距离地图上进行二次匹配搜索的过程为：

在原始分辨率的距离地图上agv的最大可能的初始位置和方向角对应的平移和旋转搜索范围内，按照预定的二次搜索间隔δt和δθ，在原始分辨率的距离地图上进行匹配搜索，得到agv最终的初始位置和方向角。

根据本申请实施例的第二方面，本申请还提供了一种基于倒角距离形状匹配的agv初始定位系统，其包括：

距离地图生成模块，用于根据已建成的环境栅格地图生成距离地图；

点云数据预处理模块，用于对2dlidar点云进行预处理；

匹配搜索模块，用于将预处理后的2dlidar点云与生成的距离地图进行匹配搜索，得到avg的初始位置和方向角。

根据本申请的上述具体实施方式可知，至少具有以下有益效果：本申请采用lidar当前获取的轮廓与环境栅格地图之间的倒角距离作为匹配衡量测度，通过寻找最大匹配度来确定agv的初始位置和方位角，无需进行人工选点，具有很高的自动化程度；而且能够显著提高计算效率。

应了解的是，上述一般描述及以下具体实施方式仅为示例性及阐释性的，其并不能限制本申请所欲主张的范围。

附图说明

下面的所附附图是本申请的说明书的一部分，其示出了本申请的实施例，所附附图与说明书的描述一起用来说明本申请的原理。

图1为本申请具体实施方式提供的一种基于倒角距离形状匹配的agv初始定位方法的流程图。

图2为本申请具体实施方式提供的典型的环境栅格地图的示意图。

图3为本申请具体实施方式提供的一种基于倒角距离形状匹配的agv初始定位方法中8邻域栅格的状态示意图，其中，图(a)表示前向扫描时8邻域栅格的状态示意图，图(b)表示后向扫描时8邻域栅格的状态示意图。

图4为本申请具体实施方式提供的一种基于倒角距离形状匹配的agv初始定位方法中确定距离阈值的原理图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚明白，下面将以附图及详细叙述清楚说明本申请所揭示内容的精神，任何所属技术领域技术人员在了解本申请内容的实施例后，当可由本申请内容所教示的技术，加以改变及修饰，其并不脱离本申请内容的精神与范围。

本申请的示意性实施例及其说明用于解释本申请，但并不作为对本申请的限定。另外，在附图及实施方式中所使用相同或类似标号的元件/构件是用来代表相同或类似部分。

关于本文中所使用的“第一”、“第二”、…等，并非特别指称次序或顺位的意思，也非用以限定本申请，其仅为了区别以相同技术用语描述的元件或操作。

关于本文中所使用的方向用语，例如：上、下、左、右、前或后等，仅是参考附图的方向。因此，使用的方向用语是用来说明并非用来限制本创作。

关于本文中所使用的“包含”、“包括”、“具有”、“含有”等等，均为开放性的用语，即意指包含但不限于。

关于本文中所使用的“及/或”，包括所述事物的任一或全部组合。

关于本文中的“多个”包括“两个”及“两个以上”；关于本文中的“多组”包括“两组”及“两组以上”。

关于本文中所使用的用语“大致”、“约”等，用以修饰任何可以细微变化的数量或误差，但这些微变化或误差并不会改变其本质。一般而言，此类用语所修饰的细微变化或误差的范围在部分实施例中可为20％，在部分实施例中可为10％，在部分实施例中可为5％或是其他数值。本领域技术人员应当了解，前述提及的数值可依实际需求而调整，并不以此为限。

某些用以描述本申请的用词将于下或在此说明书的别处讨论，以提供本领域技术人员在有关本申请的描述上额外的引导。

虽然2dlidar数据的距离数据缺乏纹理信息，无法构建具有显著性的特征描述，但是从2dlidar数据中可以得到反射物的边缘轮廓信息，该边缘轮廓信息能够反映当前所处环境的形状，其实际为已知环境栅格地图的局部。因此，可以通过寻找当前获取的轮廓与环境栅格地图的匹配来实现agv的初始定位，即基于形状的匹配。

如图1所示，本申请基于倒角距离形状匹配的agv初始定位方法包括以下步骤：

s1、根据图2所示已建成的环境栅格地图生成距离地图。

距离地图表示环境栅格地图中每个栅格到其最近障碍物栅格的欧式距离。通过距离变换，离散的二值特征便转换为连续的距离测度，能够大大丰富距离地图的信息。

为了提高计算效率，可以采用两步扫描的倒角距离变换来近似欧式距离，得到距离地图。

具体地，距离地图可以通过以下方式得到：

s11、生成与原环境栅格地图相同尺寸的距离地图，将所有障碍物对应的栅格的值置为0，也就是说，与障碍物对应的栅格距离最近的障碍物栅格是其本身，即障碍物对应的栅格；并将所有自由栅格的值置为∞。自由栅格也就是非障碍物对应的栅格。

s12、前向扫描；如图3(a)所示，按照从上到下、从左到右的顺序，依次扫描环境栅格地图中的当前扫描到的栅格的8邻域栅格中的每一个栅格点。

如图3所示，假设p0是当前扫描到的栅格，如果p0是障碍物栅格，则跳过；如果p0为自由栅格，则在前向扫描中只考虑其8邻域中位于其上方和左侧的p1,p2,p3,p4四个栅格，按照下式计算5个距离值d0,d1,d2,d3,d4：

式(1)中，p1表示当前扫描时8邻域栅格中栅格p1与其最近障碍物栅格的距离，dist(p0,p1)表示栅格p1与p0之间的欧式距离；

p2表示当前扫描时8邻域栅格中栅格p2与其最近障碍物栅格的距离，dist(p0,p2)表示栅格p2与p0之间的欧式距离；

p3表示当前扫描时8邻域栅格中栅格p3与其最近障碍物栅格的距离，dist(p0,p3)表示栅格p3与p0之间的欧式距离；

p4表示当前扫描时8邻域栅格中栅格p4与其最近障碍物栅格的距离，dist(p0,p4)表示栅格p4与p0之间的欧式距离。

通过比较得到d0～d4中的最小值，并用该最小值对栅格p0到与其最近障碍物栅格的距离p0进行更新，即，

p0＝min(d0,d1,d2,d3,d4)(2)

s13、后向扫描；如图3(b)所示，按照从下到上、从右到左的顺序，依次扫描环境栅格地图中的每一个栅格点。

如图4所示，假设p0是当前扫描到的栅格，如果p0是障碍物栅格，则跳过；如果p0为自由栅格，则在后向扫描中只考虑其8邻域中位于其下方和右侧的p5,p6,p7,p8四个栅格，按照下式计算5个距离值d0,d5,d6,d7,d8：

式(3)中，p5表示当前扫描时8邻域栅格中栅格p5与其最近障碍物栅格的距离，dist(p0,p5)表示栅格p5与p0之间的欧式距离；

p6表示当前扫描时8邻域栅格中栅格p6与其最近障碍物栅格的距离，dist(p0,p6)表示栅格p6与p0之间的欧式距离；

p7表示当前扫描时8邻域栅格中栅格p7与其最近障碍物栅格的距离，dist(p0,p7)表示栅格p7与p0之间的欧式距离；

p8表示当前扫描时8邻域栅格中栅格p8与其最近障碍物栅格的距离，dist(p0,p8)表示栅格p8与p0之间的欧式距离。

通过比较得到d0与d5～d8中的最小值，并用该最小值对栅格p0到与其最近障碍物栅格的距离p0进行更新。

p0＝min(d0,d5,d6,d7,d8)(4)

s14、采用高斯核对距离地图中的各个栅格值进行处理，得到高斯距离图；

当前距离地图中各栅格点对应的值与其到最近障碍物的距离近似于线性关系，为了使距离障碍物近的栅格具有更强的响应值，即权重，采用高斯核对距离地图中各个栅格值进行处理，可以得到高斯距离图g0：

式(5)中，p为栅格p的值，即原始距离值。

s15、对高斯距离图进行降采样处理，得到低分辨率的距离地图。

可以理解的是，通过对高斯距离图g0进行降采样处理，得到低分辨率的距离地图，能够显著提高处理效率。

按照降采样系数，确定低分辨率的距离地图中每个栅格在原始分辨率的距离地图的对应栅格，然后将对应栅格的值进行累加计算，得到低分辨率的距离地图gl。

需要说明的是，步骤s12的前向扫描和步骤s13的后向扫描的顺序是可以互换的，也就是说，可以先进行后向扫描，再进行前向扫描，进而得到距离地图中的各个栅格值。

s2、对2dlidar点云进行预处理。

lidar点云描述了障碍物的轮廓信息，位于同一障碍物表面的点具有空间上的连续性。然而，由于lidar获取数据的方式不可避免的会产生一些孤立的、断裂的扫描点，其不具有语义信息。因此，需要对这些孤立的、断裂的扫描点进行滤除。

对2dlidar点云进行预处理的具体过程为：

s21、对各扫描点是否属于同一连续障碍物进行判断；

具体地，从第一个扫描点开始，依次计算后一个点与前一个点之间的欧式距离；

如果两点之间的欧式距离大于距离阈值，则判定该点与前一个点不属于同一连续障碍物；

如果两点之间的欧式距离小于或等于距离阈值，则判定该点与前一个点属于同一连续障碍物。

对于距离阈值，可以通过以下计算进行设定。

如图4所示，假设pn与pn-1是时序上相邻的两点，ρn-1表示lidar到点pn-1的距离，ρn表示lidar到点pn的距离，则距离阈值dth可按照下式计算得到：

式(6)中，表示扫描角分辨率，λ表示入射角，σ表示测距噪声。

s22、滤除点云中的孤立点；

将表示同一障碍物的连续点的数量与预设点数进行比较。

如果表示同一障碍物的连续点的数量小于预设点数，则将表示同一障碍物的连续点判定为孤立点，不具有语义信息，并将其从点云中滤除。

s3、将预处理后的2dlidar点云与生成的距离地图进行匹配搜索，得到avg的初始位置和方向角。

匹配搜索通过寻找lidar点云与障碍物之间的最小倒角距离来确定agv的初始位置和方位角，即寻找扫描点云与障碍物轮廓之间的最佳匹配。

具体地，匹配搜索可以通过以下方式得到：

s31、点云刚体变换：

假设经过预处理后的lidar点云共有n个离散点，对n个离散点进行刚体变换，即：

式(7)中，[xi,yi]表示点pi的原始坐标，[xti,yti]表示变换后点pi的坐标，[tx,ty,θ]表示刚体变换的平移和旋转参数。

s32、在低分辨率的距离地图上进行初始匹配搜索，得到agv的最大可能的初始位置和方向角；

在低分辨率的距离地图上进行匹配搜索，能够显著提高匹配搜索效率。

进行初始匹配搜索的过程为：

首先，确定一个平移和旋转搜索范围，在该搜索范围内按照预定的初始搜索间隔δt0和δθ0，根据式(7)对扫描点进行刚体变换。

其次，确定变换后的点在距离地图上的对应栅格[ri,ci]：

式(8)中，δ表示距离地图的分辨率，[]表示取整。

再次，将低分辨率的距离地图上所有对应栅格的高斯距离值g[ri,ci]进行累加，得到匹配度：

最后，对于每一组刚体变换的平移和旋转参数[tx,ty,θ]，均可以得到一个匹配度，将最大匹配度对应的[tx,ty,θ]作为agv的最大可能的初始位置和方向角。

s33、在原始分辨率的距离地图上进行二次匹配搜索，得到agv最终的初始位置和方向角，其具体过程为：

在原始分辨率的距离地图上agv的最大可能的初始位置和方向角对应的平移和旋转搜索范围内，按照预定的二次搜索间隔δt和δθ，采用与步骤s32相同的方式，在原始分辨率的距离地图上进行匹配搜索，得到agv最终的初始位置和方向角。

其中，二次搜索间隔δt小于初始搜索间隔δt0，二次搜索间隔δθ小于初始搜索间隔δθ0。初始搜索间隔δt0和δθ0根据低分辨率的距离地图的分辨率进行设定。二次搜索间隔δt和δθ根据原始分辨率的距离地图的分辨率进行设定。

本申请基于倒角距离形状匹配的agv初始定位方法针对2dlidarslam中的初始定位问题，提出了一种基于形状的匹配方法，用于确定agv在已知环境栅格地图中的初始定位。该方法采用lidar当前获取的轮廓与环境栅格地图之间的倒角距离作为匹配衡量测度，通过寻找最大匹配度来确定agv的初始位置和方位角，无需进行人工选点，具有很高的自动化程度。

在以上基于倒角距离形状匹配的agv初始定位方法的基础上，本申请还提供了一种基于倒角距离形状匹配的agv初始定位系统，其包括：

距离地图生成模块，用于根据已建成的环境栅格地图生成距离地图；

点云数据预处理模块，用于对2dlidar点云进行预处理；

匹配搜索模块，用于将预处理后的2dlidar点云与生成的距离地图进行匹配搜索，得到avg的初始位置和方向角。

具体地，距离地图生成模块包括设置模块、前向扫描模块、后向扫描模块、栅格值处理模块和降采样模块。

其中，设置模块，用于将环境栅格地图中所有障碍物对应的栅格的值设置为0，将自由栅格的值设置为∞。

前向扫描模块，用于按照从上到下、从左到右的顺序，依次扫描环境栅格地图中的当前扫描到的栅格的8邻域栅格中的每一个栅格点，并用最小距离值对当前扫描到的栅格到与其最近的障碍物栅格的距离进行更新；

后向扫描模块，用于按照从下到上、从右到左的顺序，依次扫描环境栅格地图中的当前扫描到的栅格的8邻域栅格中的每一个栅格点，并用最小距离值对当前扫描到的栅格到与其最近的障碍物栅格的距离进行更新

其中，上述最小距离值为当前扫描到的栅格到与其最近障碍物栅格的距离以及当前扫描时8邻域栅格中栅格与其最近障碍物栅格的距离和该栅格与当前扫描到的栅格之间的欧式距离的和值中的最小值。

栅格值处理模块，采用高斯核对距离地图中的各个栅格值进行处理，得到高斯距离图。

降采样模块，用于对高斯距离图进行降采样处理，得到低分辨率的距离地图。

具体地，点云数据预处理模块包括扫描点判断模块和孤立点滤除模块。

其中，扫描点判断模块，用于对各扫描点是否属于同一连续障碍物进行判断。

孤立点滤除模块，用于将表示同一障碍物的连续点的数量小于预设点数的孤立点从点云中滤除。

具体地，匹配搜索模块包括刚体变换模块、初始匹配搜索模块和二次匹配搜索模块。

其中，刚体变换模块，用于对经过预处理后的lidar点云中的点进行刚体变换；

初始匹配搜索模块，用于在低分辨率的距离地图上进行初始匹配搜索，得到agv的最大可能的初始位置和方向角；

二次匹配搜索模块，用于在原始分辨率的距离地图上进行二次匹配搜索，得到agv最终的初始位置和方向角。

需要说明的是，上述实施例提供的基于倒角距离形状匹配的agv初始定位系统仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述处理分配由不同的程序模块完成，即将agv初始定位系统的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分处理。另外，上述实施例提供的agv初始定位系统与agv初始定位方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

本申请基于倒角距离形状匹配的agv初始定位系统采用lidar当前获取的轮廓与环境栅格地图之间的倒角距离作为匹配衡量测度，通过寻找最大匹配度来确定agv的初始位置和方位角，无需进行人工选点，具有很高的自动化程度。

在示例性实施例中，本申请实施例还提供了一种计算机存储介质，是计算机可读存储介质，例如，包括计算机程序的存储器，上述计算机程序可由处理器执行，以完成上述agv初始定位方法中的所述步骤。

计算机可读存储介质可以是磁性随机存取存储器、只读存储器、可编程只读存储器、可擦除可编程只读存储器、电可擦除可编程只读存储器、快闪存储器、磁表面存储器、光盘、或只读光盘等存储器。

以上所述仅为本申请示意性的具体实施方式，在不脱离本申请的构思和原则的前提下，任何本领域的技术人员所做出的等同变化与修改，均应属于本申请保护的范围。