[发明专利]基于三维点云和视觉融合的移动机器人定位方法和系统

说明书

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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