CN109270544A

CN109270544A - 基于杆状物识别的移动机器人自定位***

Info

Publication number: CN109270544A
Application number: CN201811103396.7A
Authority: CN
Inventors: 陈广; 叶灿波; 卢凡; 余卓平; 陈凯; 熊璐; 杜嘉彤; 许仲聪
Original assignee: Tongji University
Current assignee: Tongji University
Priority date: 2018-09-20
Filing date: 2018-09-20
Publication date: 2019-01-25

Abstract

本发明公开了一种基于杆状物识别的移动机器人自定位***，其包括如下步骤：构建杆状物离线地图，利用激光雷达获取移动物体周围的点云数据，并从该点云数据中提取杆状物信息，根据杆状物信息与杆状物离线地图的匹配进行自定位，得到移动物体的定位信息。本发明使用激光雷达对杆状物进行识别跟踪，结合惯性测量单元及GPS的信息，通过相应的定位算法进行数据的处理和融合，从而实现移动物体的自定位。本发明的方法可以达到分米级的定位精度，在满足实际应用需求的前提下，有着成本较低、精度很高、实时性及鲁棒性好的优点。

Description

基于杆状物识别的移动机器人自定位***

技术领域

本发明属于机器人导航技术领域，涉及一种基于杆状物识别的移动机器人自定位***，如基于杆状物识别对移动车辆进行定位的方法。

背景技术

在智能移动机器人或自动驾驶车辆进行导航时，需要高精度的地图去降低感知识别的误差和噪声带来的不确定性，同时通过各类传感器与地图进行匹配，并通过相应的算法进行数据处理从而得到准确的自身位姿。

目前常用的定位方法一般是使用高精度的惯性测量装置配合差分全球定位***(INS/DGPS)，但是这种方法有以下几个缺点：

其一、传感器本身的成本过高。一套高精度的INS/DGPS***就需要上万甚至几十万的成本，这对于日常或工业使用的移动机器人和自动驾驶车辆是不能接受的。

其二、由于GPS本身对于环境要求较高，由于天气、位置、遮挡等原因会造成GPS的可靠性降低。

另外，也可以采用基于激光雷达或者立体摄像头的定位方案。该定位方案是通过点云或图像的自身的局部特征点或全局特征，并与地图进行匹配，进行大量的计算得到定位。这种方法的缺点一方面是采集信息特征的不确定性，另一方面是由于点云或图像的特征计算和匹配需要很大的计算量，因而对于处理器硬件的需求就比较大，成本也会提高。

基于激光雷达或者立体摄像头的定位方案是基于路标的识别和匹配进行，常见的就是通过交通规划线的识别进行定位，这种方法的主要在于适用场合较为局限，必须是有交通规划线的场合，在一些郊区道路、维修道路或内部道路上无法使用。

发明内容

针对现有技术的不足，本发明提供了一种基于杆状物识别对移动车辆进行定位的方法，其能够克服利用识别交通规划线进行定位的缺点。因为无论是何种道路，道路两侧一般都会有较多的杆状物，如树木、路灯、电线杆、路标等。杆状物由于具有普遍性、时空不变性以及易于识别的特征，是一种十分适合于地图构建和识别进行定位的路标。本发明基于这一类杆状物而实现定位。在得到路标信息后，用于定位的算法常见的有卡尔曼滤波器(KF)、扩展卡尔曼滤波器(EKF)、最大似然估计(MLE)、粒子滤波器(PF)、以及高斯滤波等。本文提出的基于杆状物识别的定位中使用到了粒子滤波器进行定位，以及使用了卡尔曼滤波进行多传感器的数据融合。

为实现上述目的，本发明采用了如下技术方案原理为：

一种基于杆状物识别对移动物体进行定位的方法，其包括如下步骤：

(1)、构建杆状物离线地图；

(2)、利用激光雷达获取移动物体周围的点云数据，并从该点云数据中提取杆状物信息；

(3)、根据杆状物信息与杆状物离线地图的匹配进行自定位，得到移动物体的定位信息。

其中，在步骤(1)中，杆状物离线地图的构建方法如下：

1-1、利用激光雷达获取一片区域内的点云数据集合；

1-2、利用拟合地面直线算法剔除点云数据集合内所有代表地面的点云数据，得到非地面点云数据；

1-3、将非地面点云数据转换为二维笛卡尔坐标系网格，每个网格内含有该网格的点云数量、最高高度和最低高度，网格的大小与该网格内点云大小呈正相关；

1-4、根据每个网格内的点云数量对该二维笛卡尔坐标系网格进行聚类分割，将该二维笛卡尔坐标系网格分割成多个不同的连通区域，每个连通区域包含多个相关的网格；

1-5、根据每个网格内的最高高度信息对不同的连通区域逐一进行聚类分割，将每个连通区域内高度差值小于20厘米的网格作为一个点云聚类，并获取每个点云聚类的宽度；高度差值为最高高度与最低高度的差值；

1-6、剔除最高高度低于70厘米的点云聚类、宽度大于最高高度的点云聚类；

1-7、利用圆杆模型算法提取剔除后的点云聚类中的杆状物信息，一片区域内所有的杆状物信息就构成了杆状物离线地图。

在步骤(2)中，利用激光雷达获取移动物体周围的点云数据，并从该点云数据中提取杆状物信息的过程如下：

2-1、利用激光雷达获取移动物体周围的点云数据集合；

2-2、利用拟合地面直线算法剔除点云数据集合内所有代表地面的点云数据，得到非地面点云数据；

2-3、将非地面点云数据转换为二维笛卡尔坐标系网格，每个网格内含有该网格的点云数量、最高高度和最低高度，网格的大小与该网格内点云大小呈正相关；

2-4、根据每个网格内的点云数量对该二维笛卡尔坐标系网格进行聚类分割，将该二维笛卡尔坐标系网格分割成多个不同的连通区域，每个连通区域包含多个相关的网格；

2-5、根据每个网格内的最高高度信息对不同的连通区域逐一进行聚类分割，将每个连通区域内高度差值小于20厘米的网格作为一个点云聚类，并获取每个点云聚类的宽度；高度差值为最高高度与最低高度的差值；

2-6、剔除最高高度低于70厘米的点云聚类、宽度大于最高高度的点云聚类；

2-7、利用圆杆模型算法提取剔除后的点云聚类中的杆状物信息，作为移动物体周围的杆状物信息。

上述的圆杆模型算法包括如下步骤：

3-1、将剔除后的点云聚类进行水平切割，水平切面的数量等于激光雷达的线数；

3-2、根据每个水平切面的圆形几何特征排除非圆形切面和不垂直于地面的物体，得到所需杆状物；

3-3、至少获取所需杆状物的位置信息和直径信息作为杆状物信息。

在步骤(3)中，至少利用杆状物的位置信息和直径信息进行匹配。

由于采用上述方案，本发明的有益效果是：

本发明的基于杆状物识别对移动车辆进行定位的方法是一种高精度的自定位方法，适用对象为移动机器人、自动驾驶车辆等。在各种交通环境中，道路两侧的树木、路灯、路标等杆状物具有时空上不变以及识别时的简易准确的特点，是一种十分适用于定位的路标。本发明使用激光雷达进行杆状物的识别跟踪，结合惯性测量单元及GPS的信息，通过相应的定位算法进行数据的处理和融合，从而实现移动机器人的自定位。本发明的自定位方法可以达到分米级的定位精度。在满足实际应用需求的前提下，有着成本较低、精度很高、实时性及鲁棒性好的优点。

附图说明

图1为本发明的定位方法总流程图。

图2为本发明的杆状物识别流程图。

图3为本发明的一片区域内的点云数据集合的三维图。

图4为本发明的一点云划分示意图。

图5为本发明的扇形区域示意图。

图6为本发明的剔除地面后的点云数据效果图。

图7为本发明的点云聚类分割后的二维平面投影图。

图8为本发明的柱状物点云分割后的图。

图9为本发明的柱状物点云被分割后对应形成的二维图。

图10为本发明的杆状物的切片示意图。

图11为本发明的近似的标准圆柱体示意图。

图12为本发明的单株树点云的树冠高、树干高和树高示意图。

图13为本发明的单株树点云的冠幅示意图。

图14为本发明的标志牌的特征信息图。

图15为本发明的展示伪代码信息的图。

具体实施方式

本发明提供了一种基于杆状物识别对移动物体进行定位的方法。该方法是建立在建立杆状物地图、提取杆状物信息和实时定位的基础上的，其详细流程图如图1所示。该方法包括如下步骤：

(1)、构建杆状物离线地图；

(3)、根据杆状物信息与杆状物离线地图进行实时定位，得到移动物体的定位信息。

其中，上述的移动物体为移动车辆或移动机器人。

在步骤(1)中，构建杆状物离线地图包括对杆状物进行识别的过程，该过程的详细流程图如图2所示。杆状物离线地图的构建方法如下：

1-1、利用激光雷达获取一片区域内的点云数据集合；

1-3、将非地面点云数据转换为水平的二维笛卡尔坐标系网格，每个网格内含有该网格的点云数量、最高高度和最低高度等信息，网格的大小与该网格内点云大小呈正相关；

1-6、剔除最高高度低于70厘米的点云聚类、宽度大于最高高度的点云聚类，从而得到疑似杆状物的点云聚类；

1-7、利用圆杆模型算法提取疑似杆状物的点云聚类中的杆状物信息，一片区域内所有的杆状物信息就构成了杆状物离线地图。

在步骤1-1中，利用激光雷达获取一片区域内的点云数据集合的三维图如图3所示。

在步骤1-2中，通过激光雷达得到点云数据集合后，先进行无关噪声数据的过滤和剔除以提高杆状物识别效率，而首先要剔除的是所有代表地面的点云，本发明采用拟合地面直线来区分剔除。该方法的优点是计算量小运算快，且精度可控，符合本发明的要求和特点。

如图4和图5所示，点云先以移动机器人或移动车辆为圆心划分为n个扇形区域(如图中S₀)，该扇形按照半径再划分为m个小块，每个点云原坐标p_i＝(x_i,y_i,z_i)会映射到一个竖直平面上，此时则扇区内的三维点云被映射到一个二维平面内且划分为m段。每一段中取其高度(即z_i)最低点，作为该段的代表点p′_bi。

随后根据增量算法，从圆心向外逐个取代表点用于拟合地面直线，其中每增加一个点，拟合得到的直线z_i＝mr_i+b需要满足以下要求：即按照图15的伪代码处理，可以去除掉非地面的代表点，并得到多段拟合直线用于代表地面。其中L_s为地面直线集合，L_last表示上一次得到的直线，P_s为当前所取代表点集合。|P_s|表示集合元素个数。其中T_set表示当前代表点与上一拟合直线的距离门限值，用于剔除障碍物、路肩等非地面代表点；T_mset、T_bset、T_err分别表示拟合直线的斜率、截距、拟合标准差的限值，用于拟合直线的分段以保证地面拟合直线的精度。

经过图15所示的伪代码处理后，最终得到一组地面直线，此时各个点云计算其在二维竖直平面内与地面直线的距离，当距离小于一设定值T_ground时，认定为地面点云并剔除。剔除地面后的点云数据效果图如图6所示。

步骤1-4的原理是基于点云数量的聚类分割，目的是根据点云数量寻找每个网格附近的连通网格，多个连通网格构成一个连通区域为初步点云聚类，从而把不相邻的物体基本分隔开。步骤1-4的点云聚类分割后的二维平面投影图如图7所示。

步骤1-5的原理是基于高度的点云聚类分割，目的是从20厘米高度差值开始，把高度差值大于20厘米的点云投影网格再分割开，与其高度相近(小于20厘米)的连通区域作为一个聚类，从而得到多个点云聚类，从每个聚类的二维形状可以得到其宽度信息。柱状物点云分割后的图如图8所示。该图表明经过基于高度的点云聚类分割后，具有一定高度的点云会被分割出来。柱状物点云被分割后对应形成的二维图如图9所示。根据二维图的柱状物点云的二维形状能够得到该柱状物的宽度信息。

在步骤1-6中，点云聚类分割后，先进行噪声物体的剔除，尽可能筛选出疑似杆状物。对于一些点云密度特别小的物体、高度低于70厘米、或者宽度比高度还要大的物体可以进行剔除。在步骤1-7中，圆杆模型可以用来对筛选后的点云聚类进行杆状物的匹配和特征信息提取。由于本发明所检测的杆状物都是垂直于地面的，所以把聚类进行水平切割后基于圆形几何特征进行计算，而聚类的水平切面数量与激光雷达的线数一致。圆形几何特征如下所示：

(x-c_x)²+(y–c_y)²＝r²①

通过整理，得到如下形式：

-r²+c_x ²+c_y ²–2xc_x–2yc_y＝-(x²+y²)②

把②应用到n个点中，得到如下线性方程组：

A·c＝b

其中：

通过解这一个方程组，每三个点可以得到一组圆心和半径的解，计算这一系列解的标准差，通过设定一个阈值把非圆形切面以及不垂直于地面的物体排除。当标准差小于阈值时，是垂直于地面的物体的圆形切面。当标准差大于或等于阈值时，是非圆形切面或者属于不垂直于地面的物体，这种情况就属于非杆状物，需要排除。计算得到一个杆状物的各个圆形切面的半径和圆心后，通过计算其平均值得到一个近似的标准圆柱体，并且得到该近似的标准圆柱体的位置信息和直径信息等特征信息。对杆状物进行切片如图10所示。通过计算圆形切片的平均值所得到的近似的标准圆柱体如图11所示。

其中，疑似杆状物的多个切面的直径的方差值s²是一个容易得到的特征信息，该特征信息可以迅速区分开路灯与树木。而激光雷达还可直接获得点云的反射强度，疑似杆状物区域点云的反射强度均值可在一定程度上反映材质信息，也可以作为一个易得的特征信息。

在分离出单个杆状物后，除去前文提到的圆心点坐标(x，y)和直径d、直径方差值s²、反射强度均值外，可以进一步对该杆状物的其他特征信息进行提取，例如树的高度和冠幅，路灯或标志牌的附属物形状等特征信息。需要指出的是，此类特征信息由于其位置的限制(如高度)，会对传感器有较高的要求，例如会需要使用线数更多的激光雷达，或需要融合其他视觉传感器如摄像头等。

具体地说，对于单株树木，树高的测定利用其最高点，树干高的测定利用点云拟合圆柱后的高度，冠幅的测量则是利用树木圆心点坐标附近的水平点云投影的分布获得。单株树点云的树冠高、树干高和树高的信息如图12所示。在图12中，树冠高表示T₁，树干高表示T₂，树高表示T，T＝T₁+T₂。单株树点云的冠幅信息如图13所示，冠幅表示T_w。

对于路灯、标志牌杆等标准圆柱物的其他特征信息提取：在分离出单个路灯、标志牌杆点云后，也可以进一步获取其他特征信息，主要是其附属物的形状特征和位置特征。标志牌杆等的特征信息提取如图14所示。

经过以上的处理，杆状物的位置信息和直径信息等特征信息已经从原始传感器数据中提取出来，可以分别用于基于杆状物的离线地图构建以及自定位。

上述的地图构建利用了上述的杆状物识别方法，其是通过搭载有高精度惯性测量仪和差分GPS(INS/DGPS)的测量移动机器人或移动车辆按照上述的杆状物识别方法，在目标区域进行基于杆状物的地图构建。地图构建的原理为：由于杆状物的特点，建立一个二维的杆状物地图，该地图实际上就是一个由杆状物特征向量的序列，每一个杆状物的特征向量存储内容包括有：坐标位置、直径值和特征信息(如圆柱直径方差、点云反射强度均值、以及前文提到的各类杆状物的特征信息)等。具体特征信息的选用应当根据激光雷达的线数和精度，以及控制器的运算能力选择。由于本发明更强调实时性和低的计算成本，且应用的粒子滤波定位算法对于特征匹配要求不高，故此处使用的是特征向量为：[坐标位置、直径值、直径方差、点云反射强度均值]，这一特征向量已经为定位时的路标匹配提供了足够的特殊性。总之，本发明利用一组杆状物特征向量的序列，描述一片区域内的杆状物信息，以用于后续的定位，具有存储内容少、但便于实时路标快速匹配的特点。

在步骤(2)中，利用激光雷达获取移动车辆周围的点云数据，并从该点云数据中提取杆状物信息的过程如下：

2-1、利用激光雷达获取移动物体周围的点云数据集合；

2-5、根据每个网格内的最高高度对不同的连通区域逐一进行聚类分割，将每个连通区域内高度差值小于20厘米的网格作为一个点云聚类，并获取每个点云聚类的宽度；高度差值为最高高度与最低高度的差值；

2-7、利用圆杆模型算法提取剔除后的点云聚类中的杆状物信息。

在离线地图的构建和定位部分都需要对杆状物进行感知和识别，并且方法一样。杆状物的感知和识别的过程主要是得到点云数据后，先进行相应的过滤和简化，再根据杆状物的特征，进行杆状物的点云聚类的识别和分割，最终得到杆状物的特征信息以用于建立离线地图或者定位。而基于杆状物的地图构建是本发明的定位***的基础，除去必要的地图更新外，只需要在初始阶段实施从而得到用于定位的离线地图。基于杆状物的定位算法和多传感器数据融合算法则是运行于各个移动机器人或车辆终端，进行实时的精确自定位。

在步骤3中，以杆状物信息离线地图作为世界坐标系中的路标信息，以实时识别到的杆状物信息作为实时测量信息，在此基础上根据这两个信息通过粒子滤波算法实现精确的自定位，然后使用卡尔曼滤波把粒子滤波得到的定位结果与惯性测量单元(IMU)的位移信息进行数据融合，得到移动物体的定位信息，从而提高定位的实时性和鲁棒性。

粒子滤波算法包括如下步骤：

(1)、使用普通的GPS进行初始化定位。根据GPS的最大误差范围确定当前定位的大致区域，该区域对应离线地图中特定的目标区域。

(2)、建立移动机器人或移动车辆模型。在全局参考坐标中，移动机器人或移动车辆的位姿用三维矢量表示：[x,y,θ]。其中x，y分别表示移动机器人或移动车辆在水平二维笛卡尔坐标系下的横坐标和纵坐标，θ表示机器人的正面朝向。

(3)、生成粒子集。在上述已经确定的离线地图的目标区域内均匀生成一定数量N的粒子(测试中设定为N＝500)，粒子的表示方式与移动机器人或移动车辆模型一致，并初始化粒子权值：

(4)、进行路标匹配。根据激光雷达返回并处理得到杆状物信息，与离线地图中的杆状物存储信息进行匹配。匹配使用的信息包括有杆状物的直径、杆状物的类型、杆状物之间的相对位置等特征信息。

(5)、进行动作更新。动作更新是通过采集移动机器人或移动车辆的惯性里程计得到的位姿变化量Δodemetry＝[Δx,Δy,Δh]，并加上的高斯噪声Δerror,其方差值取决于惯性里程计的精度。更新粒子位姿状态。如下公式所示：

(6)进行观测更新，其包括如下步骤：

(6-1)得到实际搭载的激光雷达的路标测量数据，包括移动机器人或移动车辆与路标的距离D，以及观测夹角α。

(6-2)通过离线地图路标位置与粒子位置的对比，可以得到各个粒子相对相应路标的距离d，以及夹角a。

(6-3)对于任一路标，通过高斯函数求得权值，并对所有路标求得的权值进行求积，得到粒子的最终权值ω[i]。

(7)进行重采样。本发明采用的是SIR(Sample Importance Resampling)算法进行重采样，也就是在每次迭代后立即进行重采样，使得每次迭代后，使得粒子主要集中在更有可能性的区域。具体算法内容不再赘述。

(8)进行位置估计模块。通过不断循环(4)到(6)的粒子滤波的过程，最后可以得到一组位置集中的粒子集，其位姿即是移动机器人或移动车辆位姿的估计，通过求其平均值即可得到移动机器人或移动车辆的位姿估计结果。

上述的杆状物定位的算法流程图如图15所示。

由于在普通的车载控制器上使用粒子滤波的定位更新频率只有20～30Hz，故可以使用卡尔曼滤波，把粒子滤波的定位结果与IMU的位移信息进行数据融合，以得到移动车辆的定位信息。该数据融合使得定位更新频率可以达到100Hz以上。具体的卡尔曼滤波方法不再详细描述。简单地说，IMU的位移信息用于预测更新模块，粒子滤波得到的定位结果用于测量更新模块。

总之，本发明提供了一种基于杆状物识别与粒子滤波的移动机器人高精度自定位方法，以激光雷达为主要识别传感器装置，辅助以惯性测量装置和GPS，以分割、处理、识别杆状物点云信息为建图以及实时定位的核心，以粒子滤波为移动机器人实时定位算法。本发明的自定位原理为：在进行了地面点云剔除后，把点云投影到平面内进行网格化，随后通过高度阈值和点云密度进行聚类分离和杆状物的初步分离。并通过杆状物的圆柱几何特征进行匹配，进一步分离出杆状物并获取其位置和直径信息。最后还分别对树木和人工杆状物(路灯、标志牌)的其他特征进行识别和和获取。并在此之上，以得到的杆状物点云位置和特征信息作为离线地图建立和实时定位的基础。建图时通过存储该杆状物的位置和特征信息，建立基于杆状物的二维离线地图。实时定位时，通过相同的杆状物点云处理方法，得到当前检测到的杆状物信息，并与离线地图进行杆状物路标匹配，便于实现进一步的基于粒子滤波的定位算法。

熟悉本领域技术的人员显然可以容易地对这些实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于这里的实施例，本领域技术人员根据本发明的揭示，不脱离本发明范畴所做出的改进和修改都应该在本发明的保护范围之内。

Claims

1.一种基于杆状物识别对移动物体进行定位的方法，其特征在于：包括如下步骤：

(1)、构建杆状物离线地图；

(3)、根据所述杆状物信息与所述杆状物离线地图的匹配进行自定位，得到移动物体的定位信息。

2.根据权利要求1所述的方法，其特征在于：在步骤(1)中，杆状物离线地图的构建方法如下：

1-1、利用激光雷达获取一片区域内的点云数据集合；

1-2、利用拟合地面直线算法剔除所述点云数据集合内所有代表地面的点云数据，得到非地面点云数据；

1-3、将所述非地面点云数据转换为二维笛卡尔坐标系网格，每个网格内含有该网格的点云数量、最高高度和最低高度，网格的大小与该网格内点云大小呈正相关；

1-5、根据每个网格内的最高高度信息对不同的连通区域逐一进行聚类分割，将每个连通区域内高度差值小于20厘米的网格作为一个点云聚类，并获取每个点云聚类的宽度；所述高度差值为最高高度与最低高度的差值；

1-7、利用圆杆模型算法提取剔除后的点云聚类中的杆状物信息，一片区域内所有的杆状物信息就构成了所述杆状物离线地图。

3.根据权利要求1所述的方法，其特征在于：在步骤(2)中，利用激光雷达获取移动物体周围的点云数据，并从该点云数据中提取杆状物信息的过程如下所述：

2-1、利用激光雷达获取移动物体周围的点云数据集合；

2-2、利用拟合地面直线算法剔除所述点云数据集合内所有代表地面的点云数据，得到非地面点云数据；

2-3、将所述非地面点云数据转换为二维笛卡尔坐标系网格，每个网格内含有该网格的点云数量、最高高度和最低高度，网格的大小与该网格内点云大小呈正相关；

4.根据权利要求2或3所述的方法，其特征在于：所述圆杆模型算法包括如下步骤：

3-1、将所述剔除后的点云聚类进行水平切割，水平切面的数量等于所述激光雷达的线数；

5.根据权利要求1所述的方法，其特征在于：在步骤(3)中，至少利用杆状物的位置信息和直径信息进行匹配。