CN111429574B - 基于三维点云和视觉融合的移动机器人定位方法和系统 - Google Patents

Abstract

本发明提供了一种基于三维点云和视觉融合的移动机器人定位方法和系统，通过建立环境地图、点云与视觉特征融合、匹配地图并进行定位。其中，环境地图即建立环境的特征栅格地图，特征栅格地图中每个栅格存储从点云中提取的特征点和视觉图像的特征点组成的点集，并提取了高度值、强度值、法向量投影值；点云与视觉特征融合是将图像提取的特征点投影到点云空间，与点云特征组成联合特征点；匹配地图并进行定位是把联合特征点投影到二维栅格，并提取得到特征向量，将特征栅格与地图进行匹配，采用直方图滤波器，确定每个候选位姿的后验概率，基于各后验概率确定机器人在地图中的位置。

Description

基于三维点云和视觉融合的移动机器人定位方法和系统

技术领域

本发明涉及移动机器人定位导航技术领域，具体地，涉及一种基于三维点云和视觉融合的移动机器人定位方法和系统，尤其是涉及移动机器人基于多传感器融合的特征栅格地图匹配定位方法。

背景技术

移动机器人通常具有自主定位导航的功能，需要完成环境地图的构建，并基于构建的地图实现高精度的定位，定位问题是机器人领域的关键性问题。

定位系统在自动驾驶车辆中起着举足轻重的作用。其它模块，例如感知、路径规划等模块都不同程度地基于定位系统产生的定位结果来进行相应的操作。定位的准确性更是直接影响无人驾驶车成败的关键之一。

现有基于激光雷达和摄像头的定位技术中，主要是通过激光雷达构建点云地图或者通过视觉SLAM构建特征点地图，从而通过地图匹配进行定位。然而，现有点云地图方案需要存储大量点云数据，并将实时点云与整个地图进行匹配定位。而视觉SLAM构建地图在室外特征稀疏的场景中难以实现准确的定位。

例如专利文献CN109814572A公开移动机器人定位建图方法、装置、移动机器人和存储介质，获取点云数据及其对应的位姿估计，并对所述点云数据进行滤波处理；将滤波处理后的所述点云数据与当前关键帧进行配准，若配准结果满足预设条件则将当前点云数据作为新的当前关帧参与下一轮配准；计算两帧相邻所述关键帧间地面法向量的差值；根据所述关键帧、地面法向量的差值以及所述关键帧对应的位姿估计构建位姿图并对所述位姿图进行优化，根据优化后的所述位姿图生成全局地图。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于三维点云和视觉融合的移动机器人定位方法系统。

根据本发明提供的一种基于三维点云和视觉融合的移动机器人定位方法，包括：

构建离线地图步骤：采集激光点云数据和图像数据组成联合特征点，通过俯视图投影到二维平面，并提取出高度、强度、法向量投影的数值，构建特征向量，将特征向量存储在预先划分的二维栅格中，得到特征栅格地图；

在线位姿估计步骤：通过里程计预测机器人的当前时刻位置，基于当前时刻位置确定待搜索的候选栅格范围，令实时采集的激光点云数据和图像数据组成实时特征栅格，令实时特征栅格与特征栅格地图匹配，基于候选栅格范围确定每个栅格的匹配概率，基于匹配概率和每个栅格的位置确定机器人在特征栅格地图中的位置。

优选地，所述构建离线地图步骤包括：

空间投影步骤：将点云空间范围投影为二维栅格平面，划分为M×N个地图网格，所有的地图网格具有相同的大小和形状，用于存储特征向量；

特征点转换步骤：同步运行通过激光SLAM与视觉SLAM，建立点云地图与特征点地图，对于特征点地图中的点通过坐标变换得到其在点云地图坐标系下的第二坐标，根据第二坐标找到点云地图中的相同点或相近点，将所述相同点或相近点记为图像特征点；

地图生成步骤：将图像特征点与点云地图中的点共同投影到二维栅格平面，提取并计算每个栅格中点的特征均值和方差，以及栅格内点的数量，作为几何特征的表示，将整个栅格地图的几何特征采用二维数组的形式存储在文本文件中，形成特征栅格地图，所述文本文件内二维数组的行数、列数分别代表栅格坐标系的横方向、纵方向的栅格数量。

优选地，所述在线位姿估计步骤包括：

确定搜索范围步骤：以预先通过里程计确定的先验定位位置为初始位置，以初始位置为中心确定位置搜索栅格范围，记为候选栅格范围；

确定匹配概率步骤：确定候选栅格范围内每个候选栅格的预测概率，通过实时特征栅格与特征栅格地图的匹配距离确定候选栅格范围内每个栅格的权重，通过权重计算每个栅格的匹配概率；

位置确定步骤：根据预测概率和匹配概率，利用直方图滤波器得到候选栅格的后验概率，根据后验概率和候选栅格所处位置，计算得到机器人在特征栅格地图中的位置。

优选地，所述坐标变换是通过以下公式将特征点地图中的点变换到点云地图坐标系中：

P′＝(x′,y′,z′)^T＝RP+T

其中，R为从所述移动机器人的视觉地图坐标系向点云地图坐标系转化的旋转矩阵；

P＝(x,y,z)为所述移动机器人建立的特征点地图中的特征点坐标；

T为所述移动机器人的视觉地图坐标系向点云地图坐标系转化的平移矩阵；

(x′,y′,z′)表示特征点地图中的点变换到点云地图坐标系中的坐标；

P′表示特征点地图中的点。

优选地，所述候选栅格范围通过下式确定：

x_i＝x₀+i·d

y_j＝y₀+j·d

其中，(x₀,y₀)表示里程计确定的先验定位位置，也即预测的初始位置，以其作为中心确定m×n个栅格为位置搜索栅格范围，对于每一个栅格(i,j)，其代表的机器人位置为(x_i,y_j)，d为栅格的分辨率；

优选地，所述预测概率通过下式确定：

其中，(x_i,y_j)表示机器人位于候选栅格位置，p(x₀,y₀)为预测机器人处于初始位置(x₀,y₀)的概率，所述概率由上一时刻(x₀,y₀)对应栅格的后验概率决定，η为归一化常量。

优选地，所述匹配距离通过下式确定：

其中，

为(m,n)栅格在地图中对应栅格存储的特征向量，f_m,n为该栅格在线点云与图像组成的特征向量，SSD(x_i,y_j)表示对于位置为(x_i,y_j)的栅格在实时特征栅格与特征栅格地图的匹配距离；

所述匹配概率通过下式确定：

其中，N表示特征栅格内的点的个数，α为一预设常数。

优选地，所述后验概率通过下式确定：

其中，

与p(z|x_i,y_j,m)分别表示候选栅格(x_i,y_j)根据里程计预测得到的预测概率和根据激光点云、图像观测结果与地图匹配得到的匹配概率。

优选地，所述机器人在特征栅格地图中的位置通过下式确定：

以及估计的方差为：

其中，p(x_i,y_j|z,m)候选栅格(x_i,y_j)的后验概率。

根据本发明提供的一种基于三维点云和视觉融合的移动机器人定位系统，包括：

构建离线地图模块：采集激光点云数据和图像数据组成联合特征，通过俯视图投影到二维平面，构建特征向量，将特征向量存储在预先划分的二维栅格中，得到特征栅格地图；

在线位姿估计模块：通过里程计预测机器人的当前时刻位置，基于当前时刻位置确定待搜索的候选栅格范围，令实时采集的激光点云数据和图像数据组成实时特征栅格，令实时特征栅格与特征栅格地图匹配，基于候选栅格范围确定每个栅格的匹配概率，基于匹配概率和每个栅格的位置确定机器人在特征栅格地图中的位置。

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

本发明将激光点云特征与图像特征构成联合特征，与特征栅格地图匹配进行定位，避免了点云定位中需要存储大量点云数据的问题，同时避免了视觉构建地图中室外特征场景稀疏的问题，实现了三维点云和视觉融合的高精度移动机器人定位。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明中对特征栅格地图的构建过程示意图；

图2为本发明中基于特征栅格地图对移动机器人的定位过程示意图；

图3为本发明中特征栅格与地图匹配的过程说明示意图，(x₀,y₀)为通过里程计预测得到的预测位置，(x_i,y_j)为任意一候选栅格，虚线框表示将实时点云移动到此栅格的位置处，此时由框内的栅格和地图对应栅格匹配可计算匹配概率。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

针对现有技术中的缺陷，本发明提供一种结合激光点云特征与图像特征构成联合特征，并与特征栅格地图匹配进行定位，分别涉及建立环境地图、点云与视觉特征融合、匹配地图，其中，所述的环境地图即建立环境的特征栅格地图，特征栅格地图中每个栅格存储从点云中提取的特征点和视觉图像的特征点组成的点集，并提取了高度值、强度值、法向量投影特征；所述的点云与视觉特征融合是将图像提取的特征点投影到点云空间，与点云特征组成联合特征点；所述的匹配地图并进行定位是把联合特征点投影到二维栅格，并提取得到特征，将特征栅格与地图进行匹配，采用直方图滤波器，确定每个候选位姿的后验概率，基于各后验概率确定机器人在地图中的位置。

一方面，融合点云与图像信息的特征栅格构建地图，通过激光雷达和摄像头数据建立先验特征栅格地图，其大小与空间点云的大小在水平方向上一致。具体方法为：同时运行激光SLAM的构图和视觉SLAM的构图过程，将视觉构建的特征点地图中的特征点通过坐标变换关系转换到点云地图坐标系中，组成联合点集，并将联合点集投影到二维平面，落在不同的栅格内，对每个栅格内的点，计算其特征(高度、强度、法向量投影)的均值，构成一组特征向量，以txt文件的形式存储所有栅格的特征向量。

一方面，实时特征与特征栅格地图匹配进行定位，基于贝叶斯状态估计理论，包含预测和更新两个部分。在预测部分，通过里程计传感器获得机器人位姿增量，累计到上一时刻的位姿，得到当前时刻位姿的预测值。根据预测值确定精确位姿的可能范围，也即是特征栅格的搜索范围。将实时点云在该范围内滑动，对每个位置计算其匹配概率。计算时同样按照存储特征时的处理方法，将两种点组成联合的点集，并计算其特征向量，然后通过特征向量与地图对应栅格的特征向量的差进行求和，并以高斯函数形式表示所求的匹配概率。基于计算的匹配概率，得到候选栅格的后验概率，并将搜索范围内的每个栅格位置进行加权，得到机器人最终的位姿估计结果。

如图1所示，移动机器人结合点云与图像特征的栅格地图定位方法中的地图生成部分,即M1。特征栅格地图构建的过程中，采集了激光点云数据和图像数据组成联合特征，并通过俯视图投影到二维平面，构建特征向量，并将特征向量存储在预先划分的二维栅格中，得到特征栅格地图。建立结合三维点云特征和视觉特征点的特征栅格地图，将空间点投影到二维栅格，每个栅格存储三维点云和视觉的联合特征点，并提取出特征向量，保存成地图文件。具体方法为：基于预先通过激光SLAM与视觉SLAM同步建立的点云地图与特征点地图，将特征点地图中的点通过坐标变换转化到点云空间中，得到其在点云地图坐标系下的坐标，并根据坐标找到点云中的相同点或最近点，将转化得到的点与原始点云中的点共同投影到二维栅格平面上，每个栅格存储一定数量的点。提取并计算每个栅格中点的典型特征的均值，特征包括高度、强度、法向量投影等，以及该网格内点的数量，作为几何特征的表示。将整个栅格地图的几何特征采用二维数组的形式存储在文本文件中，所述文本内二维数组的行数、列数分别代表栅格坐标系的横、纵方向的栅格数量。

该部分分为以下步骤：

步骤1：将点云空间范围投影为二维平面，并划分为M×N个地图网格，所有的地图网格具有相同的大小和形状，用于保存特征向量，也即存储空间中点云和视觉特征点的投影。

步骤2：同步运行通过激光SLAM与视觉SLAM算法，建立点云地图与视觉的特征点地图，对于特征点地图中的点，通过以下方法转化到点云地图坐标系中：

P′＝(x′,y′,z′)^T＝RP+T(1)

通过公式(1)，可以将图像中的特征点转化到点云地图坐标系中。R为从所述移动机器人的视觉地图坐标系向点云地图坐标系转化的旋转矩阵，P＝(x,y,z)为所述移动机器人建立的特征点地图中的特征点坐标，T为所述移动机器人的视觉地图坐标系向点云地图坐标系转化的平移矩阵。根据P′点的位置可以找到点云地图中P′点的最近点为Q(P′和Q可能重合为一点)，Q即为点云地图中的“图像特征点”。

步骤3：将转化得到的点与原始点云中的点共同投影到二维栅格平面上，每个栅格存储一定数量的点。然后提取每个栅格中点的特征均值和方差，特征包括高度、强度、法向量投影等，以及该网格内点的数量，作为几何特征的表示。将整个栅格地图的几何特征采用二维数组的形式存储在文本文件中，所述文本内二维数组的行数、列数分别代表栅格坐标系的横、纵方向的栅格数量。

例如，对于坐标为(m,n)的栅格，提取的特征向量为f_m,n＝(u₁,u₂,u₃)，其中u₁为高度特征的均值，u₂为强度特征的均值，u₃为法向量在Z轴投影大小的均值。

如图2所示，移动机器人结合点云与图像特征的栅格地图定位方法中的实时定位部分，即M2，是基于特征栅格地图确定移动机器人在地图中位置的过程。在预测部分，通过里程计预测机器人的当前时刻位置，基于此位置确定候选栅格的范围(机器人的最终位置通过这部分栅格确定)，通过实时的特征栅格与特征栅格地图匹配，确定每个候选栅格的匹配概率，基于匹配概率和每个栅格位置确定最终的机器人位置。实时定位过程中，对于在线点云中的点，保持其空间位置不变。对于视觉的图像原始数据，提取ORB特征点，并将特征点转化到点云坐标系中，得到“图像特征点”，与点云中的特征点组成联合点集。对于联合点集中的点，将其投影到二维平面，按照栅格地图的生成方式，得到联合点集的特征栅格表示，以特征向量的形式进行实时的存储。该部分分为以下步骤：

步骤1：在每个定位周期根据里程计的预测信息，确定机器人处于每个栅格中的预测概率。

例如，假设里程计预测的机器人位置为(x₀,y₀)，以其为初始位置，位姿搜索的栅格范围为以初始位置为中心，m×n个栅格为搜索范围，对于每一个栅格(i,j)，其代表的机器人位置为(x_i,y_j)，其中：

x_i＝x₀+i·d

y_j＝y₀+j·d (2)

公式(2)中，(x₀,y₀)为预测的初始位置，d为栅格的分辨率。

步骤2：确定候选栅格的预测概率，根据里程计的预测信息，假设噪声满足高斯分布，则机器人位于候选栅格位置(x_i,y_j)的预测概率用公式(3)表示：

公式(3)中，p(x₀,y₀)为预测机器人处于初始位置(x₀,y₀)的概率，一般的，这个概率由上一时刻(x₀,y₀)对应栅格的后验概率决定。η为归一化常量。

步骤3：确定搜索栅格范围内每个栅格的权重，该权重通过将实时点云滑动到该栅格上时，形成的特征栅格与地图的匹配距离，并计算匹配概率来表征。

例如，对于位置为(x_i,y_j)的栅格，确定实时特征栅格与地图的匹配距离(用特征的平方差总和表示)为：

其中，

为(m,n)栅格在地图中对应栅格存储的特征向量，f_m,n为该栅格在线点云与图像组成的特征向量。

依据匹配距离确定栅格(x_i,y_j)的匹配概率，匹配概率的具体计算方法为：

公式(5)中，N表示特征栅格内的点的个数，α为一预设常数，该公式通过计算以(x_i,y_j)为中心的m×n个栅格的特征值的差的平方，并以类似高斯函数的形式计算并表示出该栅格位置的的匹配概率。

步骤4：确定每个候选栅格的后验概率，根据步骤2和步骤3中计算得到的预测概率与匹配概率，利用直方图滤波器更新得到候选栅格的后验概率如下所示：

其中，

与p(z|x_i,y_j,m)分别表示候选栅格(x_i,y_j)根据里程计预测得到的预测概率和根据激光、图像观测得到的观测概率。

步骤5：确定机器人在特征栅格地图中的精确位置，根据公式(5)确定的候选栅格的后验概率和栅格所处的位置，计算如下：

公式(7)用于确定机器人最终处于特征栅格中的位置，用精确的坐标

表示。该方法估计机器人位置，估计的方差可以用如下公式(8)表示：

公式(8)中，p(x_i,y_j|z,m)为选栅格(x_i,y_j)的后验概率。根据此公式可以得到移动机器人在特征栅格地图中的位置的不确定度。

如图3所示，特征栅格与地图匹配的过程说明中，(x₀,y₀)为通过里程计预测得到的预测位置，(x_i,y_j)为任意一候选栅格，红色虚线框表示将实时点云移动到此栅格的位置处，此时由框内的栅格和地图对应栅格匹配可计算匹配概率。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (9)

1.一种基于三维点云和视觉融合的移动机器人定位方法，其特征在于，包括：

构建离线地图步骤：采集激光点云数据和图像数据组成联合特征点，通过俯视图投影到二维平面，提取出高度、强度、法向量投影的数值，构建特征向量，将特征向量存储在预先划分的二维栅格中，得到特征栅格地图；

在线位姿估计步骤：通过里程计预测机器人的当前时刻位置，基于当前时刻位置确定待搜索的候选栅格范围，令实时采集的激光点云数据和图像数据组成实时特征栅格，令实时特征栅格与特征栅格地图匹配，基于候选栅格范围确定每个栅格的匹配概率，基于匹配概率和每个栅格的位置确定机器人在特征栅格地图中的位置；

所述构建离线地图步骤包括：

空间投影步骤：将点云空间范围投影为二维栅格平面，划分为M×N个地图网格，所有的地图网格具有相同的大小和形状，用于存储特征向量；

特征点转换步骤：同步运行通过激光SLAM与视觉SLAM，建立点云地图与特征点地图，对于特征点地图中的点通过坐标变换得到其在点云地图坐标系下的第二坐标，根据第二坐标找到点云地图中的相同点或相近点，将所述相同点或相近点记为图像特征点；

地图生成步骤：将图像特征点与点云地图中的点共同投影到二维栅格平面，提取并计算每个栅格中点的特征均值和方差，以及栅格内点的数量，作为几何特征的表示，将整个栅格地图的几何特征采用二维数组的形式存储在文本文件中，形成特征栅格地图，所述文本文件内二维数组的行数、列数分别代表栅格坐标系的横方向、纵方向的栅格数量。

2.根据权利要求1所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述在线位姿估计步骤包括：

确定搜索范围步骤：以预先通过里程计确定的先验定位位置为初始位置，以初始位置为中心确定位置搜索栅格范围，记为候选栅格范围；

确定匹配概率步骤：确定候选栅格范围内每个候选栅格的预测概率，通过实时特征栅格与特征栅格地图的匹配距离确定候选栅格范围内每个栅格的权重，通过权重计算每个栅格的匹配概率；

位置确定步骤：根据预测概率和匹配概率，利用直方图滤波器得到候选栅格的后验概率，根据后验概率和候选栅格所处位置，计算得到机器人在特征栅格地图中的位置。

3.根据权利要求1所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述坐标变换是通过以下公式将特征点地图中的点变换到点云地图坐标系中：

P'＝(x',y',z')^T＝RP+T

其中，R为从所述移动机器人的视觉地图坐标系向点云地图坐标系转化的旋转矩阵；

P＝(x,y,z)为所述移动机器人建立的特征点地图中的特征点坐标；

T为所述移动机器人的视觉地图坐标系向点云地图坐标系转化的平移矩阵；

(x',y',z')表示特征点地图中的点变换到点云地图坐标系中的坐标；

P'表示特征点地图中的点。

4.根据权利要求2所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述候选栅格范围通过下式确定：

x_i＝x₀+i·d

y_j＝y₀+j·d

其中，(x₀,y₀)表示里程计确定的先验定位位置，也即预测的初始位置，以其作为中心确定m×n个栅格为位置搜索栅格范围，对于每一个栅格(i,j)，其代表的机器人位置为(x_i,y_j)，d为栅格的分辨率。

5.根据权利要求2所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述预测概率通过下式确定：

其中，(x_i,y_j)表示机器人位于候选栅格位置，p(x₀,y₀)为预测机器人处于初始位置(x₀,y₀)的概率，该概率由上一时刻(x₀,y₀)对应栅格的后验概率决定，η为归一化常量。

6.根据权利要求2所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述匹配距离通过下式确定：

其中，

为(m,n)栅格在地图中对应栅格存储的特征向量，f_m,n为该栅格在线点云与图像组成的特征向量，SSD(x_i,y_j)表示对于位置为(x_i,y_j)的栅格在实时特征栅格与特征栅格地图的匹配距离；

所述匹配概率通过下式确定：

其中，D表示特征栅格内的点的个数，α为一预设常数。

7.根据权利要求2所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述后验概率通过下式确定：

其中，

与p(z|x_i,y_j,b)分别表示候选栅格(x_i,y_j)根据里程计预测得到的预测概率和根据激光点云、图像观测结果与地图匹配得到的匹配概率。

8.根据权利要求2所述的基于三维点云和视觉融合的移动机器人定位方法，其特征在于，所述机器人在特征栅格地图中的位置通过下式确定：

以及估计的方差为：

其中，p(x_i,y_j|z,b)表示候选栅格(x_i,y_j)的后验概率。

9.一种基于三维点云和视觉融合的移动机器人定位系统，其特征在于，采用权利要求1所述的基于三维点云和视觉融 合的移动机器人定位方法，包括：

构建离线地图模块：采集激光点云数据和图像数据组成联合特征，通过俯视图投影到二维平面，构建特征向量，将特征向量存储在预先划分的二维栅格中，得到特征栅格地图；

在线位姿估计模块：通过里程计预测机器人的当前时刻位置，基于当前时刻位置确定待搜索的候选栅格范围，令实时采集的激光点云数据和图像数据组成实时特征栅格，令实时特征栅格与特征栅格地图匹配，基于候选栅格范围确定每个栅格的匹配概率，基于匹配概率和每个栅格的位置确定机器人在特征栅格地图中的位置。

Images