CN109541612A

CN109541612A - 基于单线激光雷达识别柱状物的机器人室内自定位系统

Info

Publication number: CN109541612A
Application number: CN201811329207.8A
Authority: CN
Inventors: 陈广; 叶灿波; 瞿三清; 杨谦益; 陈凯; 杜嘉彤; 胡瑞恩
Original assignee: Tongji University
Current assignee: Tongji University
Priority date: 2018-11-09
Filing date: 2018-11-09
Publication date: 2019-03-29

Abstract

本发明公开了一种基于单线激光雷达识别柱状物的机器人室内自定位系统，其包括如下步骤：构建柱状物离线地图；利用单线激光雷达获取柱状物的点云数据，并从该点云数据中提取柱状物特征信息；根据柱状物特征信息与柱状物离线地图的匹配进行自定位，得到柱状物的定位信息。本发明使用的传感器主要为单线激光雷达，并利用粒子滤波器进行数据的处理，最终得到移动机器人的高精度定位。本发明的自定位方法可以达到分米级的定位精度，在满足实际应用需求的前提下，有着成本较低、精度很高、实时性及鲁棒性好的优点。

Description

基于单线激光雷达识别柱状物的机器人室内自定位系统

技术领域

本发明属于机器人导航技术领域，涉及一种基于单线激光雷达识别柱状物的机器人室内自定位系统，如基于单线激光雷达识别对柱状物进行定位的方法。

背景技术

对于在室内环境中工作的智能机器人，如何进行高精度的定位一直都是其最为关键的任务。高精度以及高鲁棒性的定位，是室内机器人完成其感知、规划、控制等任务的基础。

然而在室内环境中，由于GPS失去作用，机器人的自定位并不是一项简单的任务，尤其是在需要高精度定位时，成本与精度之间的矛盾会成为一个难以解决的问题。现有的室内自定位方法包括基于WiFi的技术，射频识别设备(RFID)、超宽带(UWB)和蓝牙等，但大多数这些定位方法都存在高成本，不稳定或低精度的缺陷。

发明内容

针对现有技术的不足，本发明提供了一种基于单线激光雷达识别对柱状物进行定位的方法，其使用单线激光雷达检测室内的柱状物，利用柱状物的信息进行建图和定位。在合适的应用场景下如室内停车场中，可以以较低的成本实现很高的定位精度。

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

一种基于单线激光雷达识别对柱状物进行定位的方法，其包括如下步骤：

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

(2)、利用单线激光雷达获取柱状物的点云数据，并从该点云数据中提取柱状物特征信息；

(3)、根据柱状物特征信息与柱状物离线地图的匹配进行自定位，得到柱状物的定位信息。

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

1-1、利用单线激光雷达获取一片区域内的初始的二维点云数据；

1-2、通过密度聚类算法(Density-Based Spatial Clustering of Applicationswith Noise，DBSCAN)对二维点云数据进行聚类分割，得到聚类；

1-3、对聚类分别采用L型检测算法或圆形检测算法，识别并获取的柱状物特征信息就构成了柱状物离线地图。

优选地，在步骤(2)中，利用单线激光雷达获取柱状物点云数据，并从该点云数据中提取柱状物特征信息的过程如下：

2-1、利用单线激光雷达获取一片区域内的初始的二维点云数据；

2-2、通过密度聚类算法对二维点云数据进行聚类分割，得到聚类；

2-3、对聚类分别采用L型检测算法或圆形检测算法，识别并获取柱状物特征信息。

优选地，在步骤(3)中，至少利用柱状物的位置信息和形状信息进行匹配。

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

本发明的基于单线激光雷达识别对柱状物进行定位的方法是一种高精度的自定位方法，应用环境为柱状物常见的室内自定位，例如室内停车场、仓库等。在此类环境中柱状物具有时空上不变以及识别时的简易准确的特点，是一种十分适用于定位的路标。本发明使用的传感器主要为单线激光雷达，并利用粒子滤波器进行数据的处理，最终得到移动机器人的高精度定位。本发明的自定位方法可以达到分米级的定位精度，在满足实际应用需求的前提下，有着成本较低、精度很高、实时性及鲁棒性好的优点。

附图说明

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

图2为本发明的点云聚类分割所使用的DBSCAN算法效果示意图(Core Points为核心点、Non-core Points为非核心点和Outliers为外部点)。

图3为本发明的二维点云数据的L型检测算法伪代码。

图4为本发明的粒子滤波算法的流程图。

具体实施方式

本发明提供了一种基于单线激光雷达识别对柱状物进行定位的方法。该方法是建立在构建柱状物地图、提取柱状物特征信息和实时定位的基础上的，其详细流程图如图1所示。该方法包括如下步骤：

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

其中，柱状物特征信息包括柱状物的截面形状：矩形、圆形；以及特殊点的位置信息：矩形的角点、圆形的圆心与截面半径。

在步骤(1)中，构建柱状物离线地图包括对柱状物进行识别的过程，故柱状物离线地图的构建方法如下：

1-2、通过密度聚类算法对二维点云数据进行聚类分割，得到聚类；

在步骤1-2中，通过单线激光雷达获得实时的二维点云数据后，需要先对二维点云数据做聚类分割，才能做后续的特征提取。由于室内柱状物分布较为离散，本发明采用的是一个经典的聚类分割算法：DBSCAN算法(具有噪声的基于密度的聚类方法)。图2为该算法的效果示意图，核心内容为遍历输入的点云数据，统计在一个给定半径范围内的点云数量，并依此来把点云划分为核心点(Core Points)、非核心点(Non-core Points)和外部点(Outliers)，其中核心点和非核心点会最终构成一个聚类，外部点被认定为噪声点。

在步骤1-3中，二维点云数据被分割为聚类后，分别对每个聚类进行特征信息的提取。由于室内常见的柱状物截面形状都是矩形或圆形，根据场景可以认为规定识别目标为矩形或圆形，从而分别采用L型检测算法或圆形检测算法，并得到相应的柱状物特征向量。

实际上，L型检测算法的伪代码如图3所示。其基本思想是迭代矩形的所有可能方向，并找到包含所有扫描点的矩形。对每一个聚类内的点，分别作出以其为交点的两条垂直相交的直线cosθ*x+sinθ*y＝q以及cosθ*y-sinθ*x＝p，继而可以获得每个点到这两条直线的距离向量D＝[d₁,…,d_m]，目标函数是所有点到两条垂线的距离d_i的倒数之和伪代码中的CalculateCloseness函数具体指的便是使每一个点的该目标函数最大，得到一组以该点作为角点的最合适的垂线。遍历了所有点之后，每一个点都会有一组垂线，其中目标函数得分最高的便是最终拟合的L型垂线。在这之后，取这一组垂线上最边缘的点，作出矩形的另外两条边，从而最终得到一个拟合的矩形。矩形的四个角点的坐标(x_i,y_i)_i＝1,2,3,4也可以得到。四个角点的坐标不仅包含了柱状物的位置信息，也包含了柱状物的形状信息。

另外，圆形检测算法如下：

圆形几何特征：

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

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

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

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

A·c＝b

其中：

通过解这一个方程组，每三个点可以得到一组圆心和半径的解，计算这一系列解的标准差，通过设定一个阈值把非圆形切面以及不垂直于地面的物体排除。计算得到一个柱状物的各个圆形切面的半径和圆心后，通过计算其平均值得到一个近似的标准圆柱体，且其位置信息和直径信息也已得到。

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

上述的地图构建利用了上述的柱状物识别方法，由于室内高精度定位较为困难，而室内场景的柱状物常在建筑物设计时就有精确的位置信息和形状信息，可以直接根据该信息建立基于柱状物的地图。当然也可以使用搭载高精度惯性导航装置以及室内定位装置的信号采集机器人，根据上述的柱状物识别算法，在目标区域进行基于柱状物的地图构建。地图构建的原理为：由于柱状物的特点，建立一个二维的柱状物地图，该地图实际上就是一个由柱状物特征向量的序列，每一个柱状物的特征向量存储内容根据柱状物类型分为两种，对于矩形截面柱状物是各个柱状物截面四个角点的坐标的集合；对于圆形截面柱状物是圆形截面的圆心坐标和半径[x_i,y_i,r_i]。最终可以得到一个由柱状物特征向量的序列。总之，本发明利用一组柱状物特征向量的序列，描述一片区域内的柱状物信息，以用于后续的定位，具有存储内容少、但便于实时路标快速匹配的特点。

在步骤(2)中，利用单线激光雷达获取柱状物点云数据，并从该点云数据中提取柱状物特征信息的过程如下：

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

在步骤3中，以柱状物信息离线地图作为世界坐标系中的路标信息，以实时识别到的柱状物信息作为实时测量信息，在此基础上通过粒子滤波算法实现精确的自定位。若使用惯性测量装置进行辅助定位，还可使用卡尔曼滤波把粒子滤波得到的定位结果与惯性测量装置的位移信息进行数据融合，得到柱状物的定位信息，从而提高定位的实时性和鲁棒性。

如图4所示，粒子滤波算法包括如下步骤：

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

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

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

(4)、进行路标匹配。根据激光雷达返回并处理得到柱状物信息，与离线地图中的柱状物存储信息进行匹配。

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

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

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

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

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

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

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

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

总之，本发明提供了一种基于单线激光雷达，通过相应的柱状物特征检测算法获取柱状物信息，并利用粒子滤波进行位姿估计的机器人室内自定位系统，以单线激光雷达为主要识别传感器装置，通过L型检测算法或圆形检测算法识别相应柱状物的同时用于建图或定位，以粒子滤波为移动机器人实时定位算法。此外还可选地使用惯性测量装置进行辅助定位。本发明的自定位原理为：先对获取的二维点云数据进行聚类分割，随后对各个聚类应用L型检测算法或圆形检测算法，识别并获取柱状物的特征信息。并在此之上，把得到的柱状物特征信息作为离线地图建立和实时定位的基础。建图时通过存储该柱状物的位置信息和特征信息，建立基于柱状物的二维离线地图。实时定位时，通过相同的柱状物点云处理方法，得到当前检测到的柱状物信息，并与离线地图进行柱状物路标匹配，便于实现进一步的基于粒子滤波的定位算法。

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

Claims

1.一种基于单线激光雷达识别对柱状物进行定位的方法，其特征在于：其包括如下步骤：

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

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

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

1-2、通过密度聚类算法对所述二维点云数据进行聚类分割，得到聚类；

1-3、对所述聚类分别采用L型检测算法或圆形检测算法，识别并获取的柱状物特征信息就构成了所述柱状物离线地图。

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

2-2、通过密度聚类算法对所述二维点云数据进行聚类分割，得到聚类；

2-3、对所述聚类分别采用L型检测算法或圆形检测算法，识别并获取柱状物特征信息。

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