一种不同传感器数据的自适应多重融合SLAM方法

技术领域

本发明属于移动机器人自主导航领域，具体是一种在未知室内环境下能充分利用不同传感器数据的自适应多重融合SLAM方法。

背景技术

随着科学技术的进步，机器人得到了快速的发展。机器人技术从最初的工业生产，逐渐地向物流、会议和医疗等领域普及。移动机器人可以通过获取所处环境数据和自己在所处环境中的位置，自主地进行导航，因此在多个领域中得到了普遍的使用。随着传感器和人工智能技术的发展，移动机器人同时定位与建图(SLAM)方法成为了近些年移动机器人自主导航领域中研究的热点。

2D激光雷达由于成本较低、精度高、抗干扰能力强且实时性好，在SLAM中得到了广泛的应用。2D激光雷达SLAM目前存在激光点云配准计算量大、位姿估计累计误差大、闭环检测效果差、容易产生噪声和泄漏以及传感器数据利用不充分等问题。

当前已经有许多方法来解决移动机器人2D激光雷达SLAM问题，比如Gmapping算法、Karto算法和Hector算法等。Gmapping利用RaoBlackwellized粒子滤波器(RBPF)来构建SLAM系统，但算法的计算量大，且不适用于大场景。Karto算法引入了后端优化和闭环检测，有效减少了累计误差，但算法比较耗时。Cartographer算法是一种基于图优化的2D激光雷达SLAM算法，分为前端和后端两部分。前端利用滤波方法融合激光雷达点云数据和IMU数据获得机器人位姿，同时利用帧间匹配技术构建子地图，后端根据子地图间的约束关系进行优化，来修正子地图的位姿节点完成地图构建。该算法构建的地图在准确度上没有明显提升，且定位精度较低。

发明内容

本发明针对现有移动机器人2D激光雷达SLAM存在的问题，以及移动机器人在室内环境中进行定位建图的需求，在Cartographer算法的基础上进行改进，提出一种不同传感器数据的自适应多重融合SLAM方法。

所述不同传感器数据的自适应多重融合SLAM方法，具体步骤如下：

步骤1：针对处于某未知室内环境中的移动机器人，获取其传感器数据并进行处理，同时提取到深度相机获得的图像的特征，生成全局深度视觉信息和局部深度视觉信息。

1)生成全局深度视觉信息的过程为：

首先，获得深度相机的灰度图，并利用GCNv2神经网络对灰度图进行处理，生成特征图。

然后，将特征图上的特征点进行均匀化处理。

最后，通过损失函数训练和二值描述分别对均匀化的特征点进行处理，生成特征点云和描述符并保存，作为全局深度视觉信息。

损失函数如下：

L

其中，(x

二值描述公式如下：

其中，b(x)是根据f(x)的值区分得到的特征值，f(x)是特征点的观察概率。

2)生成局部深度视觉信息的过程如下：

首先，以2D激光雷达圆心为原点，利用IMU(Inertial Measurement Unit，惯性测量装置)数据、深度相机平面投影与激光雷达二维坐标的一致性，构建移动机器人的全局坐标系并得到IMU的观测数据。

假设移动机器人做匀变速运动，在全局坐标系中，移动机器人的状态变量如下：

上式中

利用全局坐标系，得到IMU的观测数据，由于IMU输出频率很高，[t

上式中

同时，获取深度相机数据，利用改进后的LPP算法提取深度相机获得的图像的局部特征。

结合移动机器人运动一致性改进后的LPP算法公式如下：

其中，i,j是全局坐标系中移动机器人经过的两个不同位置，X

然后，图像的局部特征结合IMU观测数据对移动机器人运动的描述，用引入加权目标函数的R-CNN神经网络生成局部深度视觉信息。

加权目标函数如下：

其中，L

步骤2：利用改进的IMLS-ICP算法将激光雷达数据、全局深度视觉信息和局部深度视觉信息进行点云匹配，用于数据融合和生成点云地图。

点云匹配的具体步骤如下：

步骤201：利用局部深度视觉信息得到的移动机器人状态量之间的变换，去除激光雷达数据中的运动畸变。

步骤202：删去地面点云和聚类，根据移动机器人和激光雷达数据点云、全局深度视觉信息的特征点云以及局部深度视觉信息点云之间的运动变换，去除全局坐标系中的移动物体。

步骤203：利用激光点云的中心点到视觉参考点云的之间一定范围内的点云来构建曲面，最后对曲面进行降维处理，生成二维地图。

视觉参考点云即为未经归一化处理的视觉点云；

曲面公式如下：

其中，((x,y)-p

步骤204：选取一定数量的激光点云和视觉点云，并根据构建的曲面，对选取的激光点云和视觉点云进行匹配求解，得到点云配准结果。

点云匹配公式如下：

其中，(u

步骤3：利用改进的模糊自适应UKF算法融合IMU数据和点云配准数据，得到移动机器人的位姿。

具体步骤如下：

步骤301：IMU数据和点云配准数据进行χ

其中，q

步骤302：进一步引入模糊自适应规则对IMU数据和点云配准数据的过程噪声和观测噪声进行估计，其公式如下：

其中，

步骤303：将经过χ

移动机器人的位姿计算公式如下：

其中，Z

步骤4：对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图。

具体步骤如下：

步骤401：将全局深度视觉信息中的描述符和联合优化后的位姿进行匹配，检测移动机器人的路径是否为一个闭合环。若路径为一个闭合环，则从点云配准中得到移动机器人的位姿变换，形成闭环约束，执行步骤402。若不是一个闭合环，则继续搜索起始点位置，直至达到闭环约束。

步骤402：利用高斯牛顿法的收敛性，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图；

优化函数的公式如下：

其中，F(x

本发明的优点在于：

(1)本发明为一种不同传感器数据的自适应多重融合SLAM方法，充分利用IMU数据并引入全局深度视觉信息和局部深度视觉信息，为点云配准、闭环优化和地图更新提供了更精准的位姿和地图数据。

(2)本发明为一种不同传感器数据的自适应多重融合SLAM方法，通过改进的IMLS-ICP算法，将具有代表性的2D激光雷达点云和3D相机点云进行匹配，有效降低了点云配准计算量，提升了定位和建图的精度。

(3)本发明为一种不同传感器数据的自适应多重融合SLAM方法，利用改进的模糊自适应UKF算法，将IMU和点云配准数据进行融合，充分利用了传感器数据，减少了噪声和泄漏，得到的位姿状态量更精确，提升了移动机器人对所处环境的鲁棒性。

(4)本发明为一种不同传感器数据的自适应多重融合SLAM方法，将位姿约束、点云配准约束和闭环检测约束进行联合优化，修正子地图的位姿节点，减小了移动机器人的位姿估计误差，使移动机器人所建地图更加准确。

附图说明

图1为本发明基于一种不同传感器数据的自适应多重融合SLAM方法的原理框架示意图；

图2为本发明利用GCNv2神经网络生成全局深度视觉信息的过程示意图；

图3为本发明利用改进LPP算法提取相机数据局部特征点，结合IMU描述用改进的R-CNN神经网络生成局部深度视觉信息的过程示意图；

图4为本发明在全局坐标系中利用改进的IMLS-ICP算法将激光点云和视觉点云进行点云匹配的示意图；

图5为本发明利用改进的模糊自适应UKF算法融合IMU和点云匹配数据，得到移动机器人的位姿的过程示意图；

图6为本发明机器人位姿、点云地图和闭环检测结果之间的约束关系示意图。

具体实施方式

下面结合附图对本发明进行详细说明。

本发明寻找一种融合激光雷达、深度相机、惯性测量单元(IMU)等多个传感器数据的SLAM方法，可以改善移动机器人定位建图的性能，减小移动机器人位姿估计误差，提升移动机器人的闭环检测效果，使移动机器人所建地图更加准确，提升移动机器人对所处环境的鲁棒性。

针对现有2D激光雷达SLAM激光点云配准计算量大、闭环检测效果差和位姿估计误差大的问题，本发明引入了全局深度视觉信息和局部深度视觉信息，用于点云配准、闭环检测和地图更新。针对2D激光雷达点云配准数据容易出现遗漏和建图精度较低的问题，利用改进的IMLS-ICP算法将激光雷达点云、全局深度视觉信息点云和局部深度视觉信息点云进行匹配，用于数据融合和生成点云地图。针对2D激光雷达SLAM容易产生噪声和泄漏以及传感器数据利用不充分的问题，利用改进的模糊自适应UKF算法融合IMU和点云匹配数据，得到移动机器人的位姿。本发明将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，性能得到了显著的提升，减小了移动机器人的位姿估计误差，检测到的闭合路径更加接近真实的闭合路径，移动机器人所建地图更加准确，提升了移动机器人对所处环境的鲁棒性。

一种不同传感器数据的自适应多重融合SLAM方法，原理如图1所示，具体步骤如下：

步骤1：针对处于某未知室内环境中的移动机器人，获取其传感器数据，并进行处理，同时提取深度相机获得的图像的特征，生成全局深度视觉信息和局部深度视觉信息。

1)生成全局深度视觉信息的过程，如图2所示，具体为：

首先，获得深度相机的灰度图，并利用GCNv2神经网络对灰度图进行处理，生成1280×720×256像素的特征图。

然后，将特征图上的特征点进行均匀化处理。

最后，通过损失函数训练和二值描述分别对均匀化的特征点进行处理，生成特征点云和描述符并保存，作为全局深度视觉信息。

损失函数如下：

L

其中，(x

二值描述公式如下：

其中，b(x)是根据f(x)的值区分得到的特征值，f(x)是特征点的观察概率。

2)生成局部深度视觉信息的过程如图3所示，具体如下：

首先，以2D激光雷达圆心为原点，利用IMU(Inertial Measurement Unit，惯性测量装置)数据、深度相机平面投影与激光雷达二维坐标的一致性，构建移动机器人的全局坐标系并得到IMU的观测数据。

假设移动机器人做匀变速运动，在全局坐标系中，移动机器人的状态变量如下：

上式中

利用全局坐标系，得到IMU的观测数据，由于IMU输出频率很高，[t

上式中

同时，获取深度相机数据，利用改进后的LPP算法提取深度相机获得的图像的局部特征。

结合移动机器人运动一致性改进后的LPP算法公式如下：

其中，i,j是全局坐标系中移动机器人经过的两个不同位置，X

然后，图像的局部特征结合IMU观测数据对移动机器人运动的描述，用引入加权目标函数的R-CNN神经网络生成局部深度视觉信息。

加权目标函数如下：

其中，L

步骤2：利用改进的IMLS-ICP算法将激光雷达数据点云、全局深度视觉信息点云和局部深度视觉信息进行点云匹配，用于数据融合和生成点云地图。

点云匹配原理如图4所示，图中的坐标系是移动机器人的全局坐标系，圆点是激光雷达点云，曲面是视觉点云，具体步骤如下：

步骤201：利用局部深度视觉信息得到的移动机器人状态量之间的变换，去除激光雷达数据中的运动畸变。

步骤202：删去地面点云和聚类，根据移动机器人和激光点云、视觉点云之间的运动变换，去除全局坐标系中的移动物体。

步骤203：选取具有丰富特征、曲率较小且分布均衡的激光点云和视觉点云，而且要保证选取的激光点云和视觉点云契合度高，实现激光点云和视觉点云的充分利用。

步骤204：利用激光点云的中心点到视觉参考点云(未经归一化处理的视觉点云)之间的一定范围内的点云来构建曲面，最后对曲面进行降维处理，生成二维地图。

曲面公式如下：

/>

其中，((x,y)-p

步骤205：根据构建的曲面，对选取的激光点云和视觉点云进行匹配求解，得到点云配准结果；

点云匹配公式如下：

其中，(u

步骤3：利用改进的模糊自适应UKF算法融合IMU数据和点云配准数据，得到移动机器人的位姿。

原理如图5所示，本过程是先将IMU数据和点云配准数据进行χ

步骤301：IMU数据和点云配准数据进行χ

其中，q

步骤302：进一步引入模糊自适应规则对IMU数据和点云配准数据的过程噪声和观测噪声进行估计，其公式如下：

其中，

步骤303：将经过χ

/>

其中，Z

步骤4：对移动机器人的运动轨迹进行闭环检测，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图。

各状态量之间的约束关系如图6所示，图中X

步骤401：将全局深度视觉信息中的描述符和步骤三联合优化后的位姿进行匹配，检测移动机器人的路径是否为一个闭合环。若路径为一个闭合环，则从点云配准中得到移动机器人的位姿变换，形成闭环约束，执行步骤402。若不是一个闭合环，则继续搜索起始点位置，直至达到闭环约束。

步骤402：利用高斯牛顿法的收敛性，将机器人位姿、点云地图和闭环检测结果作为约束进行联合优化，得到移动机器人的路径地图

优化函数的公式如下：

其中，F(x