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

权利要求书

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₀)对应栅格的后验概率决定，η为归一化常量。