对激光雷达导引机器人路标定位的研究

赵华东，程苏全，翟晓彤

（郑州大学机械工程学院，河南 郑州 450001）

1 引言

移动机器人的定位问题是指移动机器人通过传感器信息估算自身位置和姿态的过程。文献[1]对移动机器人定位所涉及的设备和基本方法做了阐述。作为移动机器人导航最基本环节，定位机器人在工作环境中相对于全局坐标的位姿[2]。相关领域的研究人员提出过多种定位方法，如航位推算法、惯性导航法、路标定位法等。航位推算法存在标定位置不便、抗干扰性差等缺陷[3]。惯性导航法较航位推算法的定位精度高，但是较航位推算法成本高[4]，也存在难于标定的问题。以上两种方法均存在累积误差，无法适用于精确定位的要求[5]。路标定位法是利用在移动机器人上安装的传感器扫描识别出放置在行驶路径周边环境中的一些具有明显特征的人工路标[6]，再将识别出的路标与全局地图中已知位置的路标进行匹配，之后再利用已知坐标位置反求移动机器人当前的位置，从而实现精确定位[7]。

激光雷达是激光技术与雷达技术相结合的产物[8]。激光测距主要有连续波测距和脉冲测距[9]两种方法。在工业自动化和物流自动化的快速推动作用下，激光导引式移动机器人在柔性制造及智能仓储领域得到广泛应用[10]。

通过理论分析，提出了一种基于激光雷达传感器和人工路标导引的机器人的定位方法，实现了精确定位移动机器人的目的，为激光雷达定位系统的大规模开发提供了理论基础。

2 地图构建

根据测量目标维数的不同，激光可以分为一维、二维和三维。一维激光通常称为激光测距仪，只在单一方向上测距。二维、三维激光雷达都是在激光测距仪的基础上，借助扫描系统（旋转装置）改变探测方向获得二维、三维的点集。所采用的激光雷达为基于脉冲测距的二维激光雷达。

基于反射板定位的技术原理是在机器人行驶路径的周围安装位置精确的激光反射板，通过激光雷达发射激光束，同时采集由反射板反射的激光束，来确定其当前的位置和航向。总体说来，定位过程主要有三步，第一步是从扫描到的点中识别出可能是反射板的点簇（可称之为“准反射板”）[11]，第二步是计算出所有任意两块准反射间的距离并循环比较地图中已知的任意两块反射板的距离来匹配反射板，第三步是通过已经匹配的反射板的确切位置反向求得当前激光雷达所处的位置，即移动机器人的位置。

激光雷达获得的数据量大，每旋转一圈会获得361个测量点，我们把这361个测量数据称为一帧。每帧数据的点集为：Data=｛dk，θk｝，k=1，2，…361，dk为第 k 个反射点距激光雷达中心的直线距离，θk为该反射点激光雷达所建立的极坐标系的方位角。

在二维环境中，移动机器人的坐标通常用G（x，y）表示，为了便于研究，这里的坐标采用的是激光雷达中心的坐标，因为激光雷达相对移动机器人本体来说是相对固定的，后期可以通过非常简单的运算转化成移动机器人的实际坐标。定位的目的是算出每一时刻移动机器人的坐标（x，y）。

全局地图指的是由一些有标志性位置的点所组成二维点阵图形。在实际应用现场，激光反射板就是这些标志性位置点。建立全局地图需要首先设定全局坐标的原点、X轴、Y轴，坐标系采用平面直角坐标系。然后，实地测出各个激光反射板在全局坐标系下的具体位置。最后，把测得的点绘制在这张图上，就构成了移动机器人行走的全局地图。对全局地图中的其它位置，一律视为无特征区，理想化成空旷的场地。由此设定全局地图模型为Mglobal=｛Pi｝，Pi=（xi，yi），i∈N，（xi，yi）表示第 i个反射板的坐标。

激光雷达会根据其扫描特性，建立一个由若干反射板组成的局部地图。这个地图是极坐标系，激光雷达的位置为原点。局部地图模型为 Mlocal=｛Qj｝，Qj=（dj，θj），j∈N，（dj，θj） 表示第 j个反射板在激光雷达极坐标系中的位置。一般地，j

反射板的匹配过程即在Mglobal中寻找Mlocal的对应。

3 反射板检测

在机器人行走过程中，激光雷达扫描环境得到了一组自身建立的极坐标下的点数据，需要从这些数据中提取出反射板。该过程分为两个步骤：数据分割和特征提取。

3.1 数据分割

数据分割即点的聚类。由于激光雷达获得的数据量大，每帧数据包括361个测量点，而其中部分测量点为不感兴趣的点（不是反射板）或者是受环境影响而产生的噪点。这些点对下一步建立局部地图不利，因此需要进行预处理。数据分割目的是将一帧原始测量点的数据分成若干个点簇。先假设一个定义：

对于相邻两点，如果激光雷达原点与这两个点的距离差小于设定阈值时，那么这两点可视为同属一个物体。

首先求出相邻两点距激光雷达的几何距离差，如果该差值在某一阈值内，不妨设该阈值为f，则可视为这两个相邻的点属于同一个组。其判断规则如下：

那么，如何确定阈值f呢?

根据激光特性，相邻点与激光雷达中心连线的夹角为φ。即激光雷达的角度分辨率。在实际计算时，一般会出现倾斜，不妨把该倾斜的角度设为α。为了减少所分割部分对激光雷达到物体距离的依赖性，我们引入参数h，应在较长距离的dk（也有可能是dk+1）上设定一个所能允许的最大偏差h，不妨取垂点，如图1所示。

图1 聚类方法的原理图Fig.1 Schematic of Clustering Method

设相邻两点的距激光雷达的较小值为dmin，则：

如上所述，不难得出：β=φ/2，h=tanβ·dmin·sinφ。

为了调节激光雷达的纵向误差，可以通过经验值设定一个常数C1。故阈值f=C1+h。

综上所述，得出聚类方法判定方法为：

其中，k=1，2，…，361。

通过该方法可以将原始的一帧激光雷达数据点（361个）分为若干个组，为进一步处理打下基础。激光雷达数据分割流程，如图2所示。

图2 激光雷达数据分割流程图Fig.2 Flow Chart of Clustering Method

对于一帧激光雷达数据，在聚类前的数据表示为Data=｛dk，θk｝，k=1，2，…361，聚类后则为：

式中：j=1，2，…，361；signj—第 j个点簇；（djb，θjb）—该点簇起始点的极坐标；（dje，θje）—该点簇结束点的极坐标；（dj，θj）—该点簇的中心极坐标；Lj—该点簇起始点和结束点之间的直线距离。

3.2 特征提取

从上面数据分割过程中，得到了j个如式（2）表示的物体。实际上，反射板便在这些物体中。反射板的形状有平面和圆柱形状。考虑到要尽可能保证反射板在各个方向都能有良好的反射性能，选择圆柱状的反射板。

这里研究时时反射板为圆柱状。便于研究及使用方便，圆柱反射板的半径应统一，设为R0。为了调节激光反射板的径向误差，可以通过经验值设定一个常数C2。

判断signj是否为反射板可以通过下式判断：

满足上式的点簇，可能便是一个反射板，不妨称之为准反射板，记为Qj，得到局部地图。反射板的坐标取点簇中心的坐标，点簇中心用起始点和结束点连线的中点代替，如图3所示。

图3 特征识别原理图Fig.3 Schematic Diagram of Feature Recognition

之后，需要用准路标集合（局部地图）与实际地图（全局地图）进行匹配。

3.3 准路标在全局地图中的匹配

图4 反射板匹配示意图Fig.4 Schematic Diagram of Reflector Matching

准路标在全局地图中的匹配有很多方法，这里给出一种切实可行的方法。为了尽可能减少算法复杂度，用任意两块反射板之间的距离作为匹配项实为上策。有一个重要的前提是可以被利用的，在机器人行走过程中，每一时刻的上一时刻的位姿是已知的，可以据此找到在该位置周围的所有反射板，这样可以大大缩短匹配所耗费的时间和计算复杂程度。该区域可称之为最佳匹配区域。这种匹配原则可操作性强，只需要在开机时给定一个初始位姿，关机时保存位姿以备下次调用。反射板匹配示意，如图4所示。只需要在全局地图中匹配A、B、C三个反射板即可。

如何找到在最佳匹配区域内的路标呢？设某一时刻坐标为G0（x0，y0），激光雷达的可视范围半径为Rmax，根据实际经验设定误差常数C3，激光雷达距路标的距离为t0，则有：

且满足 Rmax-C3≤t0≤Rmax+C3。

这样，我们找到了最佳匹配区域的所有路标，如下：Mbest=｛pt｝，pt=（xt，yt），t∈N。

经过上述转换，准路标在全局地图中的匹配问题便可以简化为准路标在最佳匹配区域路标的匹配，即Mlocal在Mbest中的匹配。实际上，经过转换，结合合理的路标布置，这个匹配速度和匹配准确度是非常高的。匹配过程可采用两点距离比较循环比较法。不妨用Qj，j+m表示Mlocal中第j个与第j+m个准路标间的距离，用Pt，t+n表示Mbest中第t个和第t+n个路标之间的距离，设误差常数为C4，则匹配算法可表示：

则认为准路标 Qj、Qj+m可能一一对应路标 Pt、Pt+n或 Pt+n、Pt。式中：m，n∈N。

重复式（6）进行匹配，则可以找到准路标与路标的一一对应关系，如图5所示。

图5 准路标与路标匹配示意图Fig.5 Schematic Diagram of Road Sign Matching

找到对应关系后，此刻的坐标便可通过实际路标反向求得。

4 激光雷达在全局坐标中的坐标

由以上分析，将准路标Qj的坐标用与其对应路标Pt的全局坐标代替，根据三点定位法，以Pt（xt，yt）为圆心，以Qj（dj，θj）中dj为半径作三个圆，交点坐标即为激光雷达中心的全局坐标，如图6所示。

图6 三点定位法示意图Fig.6 Schematic Diagram of Three Point Positioning

列出公式如下：

上式解得两个值，即两个焦点：（x1，y1）和（x2，y2）。

接下来，我们需要找到三个圆的焦点，考虑到误差，我们取最接近的两个点的中点为G（x，y），列出方程组如下：

上式解得两个值，即两个焦点：（x3，y3）和（x4，y4）。

不妨用dij来表示点（xi，yi），i=1，2与点（xj，yj），j=3，4的距离，有：

依上式计算可得 d13，d14，d23，d24四个值，比较四者的大小，取最小值dmin，假定为dmn，则有：

综上，求得 G（x，y）。

5 实验分析

布置好实验环境，单位为毫米，实验环境，如图7所示。其中A（15000，10000）、B（7000，3000）、C（13000，6000）三点为反射板，G（7000，6000）为激光雷达中心位置，并初始化激光雷达的初始角度为X轴方向，反射板直径为300mm。实验的目的是利用以上定位算法，计算出激光中心雷达的全局坐标G0（x0，y0），如果G0与给定的位置G（7000，6000）相等，则该定位方法可行。

图7 实验环境示意图Fig.7 Schematic Diagram of Test Environmet

在同一位置，不同时间点，随机抽取1帧数据。经过数据分割，得到点云，如图8所示。数据分割，剔除噪点，得到准反射板的点的数据，如表1所示。

图8 实验环境拟合点云Fig.8 Point Cloud of Test Environmet

表1 准反射板点Tab.1 Data of Reflector Plate

根据上述点云，拟合出反射板坐标，匹配反射板坐标，反射板拟合示意图，如图9所示。上述过程重复9次，计算得出9组G0，具体数据，如表2所示。得出激光雷达坐标为G0（7607.7，6013.8），因精确坐标为 G（7600，6000），二者直线距离为 13.8mm，<20mm，符合定位需求。

图9 反射板拟合示意图Fig.9 Schematic Diagram of Reflector Plate

表2 激光雷达坐标Tab.2 Data of Laser Radar

6 总结

（1）针对采用二维激光雷达定位的移动机器人，提出一种基于反射板的定位算法。本质上是利用路标之间距离为匹配特征，将检测路标与实际路标进行匹配，提高了定位精度，并降低了路标检测的算法复杂性。（2）提出了一种地图构建方法以及路标的检测方法，提高了算法应用的灵活性。为二维激光雷达定位系统的大规模开发提供一种理论基础。