CN112525202A - 一种基于多传感器融合的slam定位导航方法及系统 - Google Patents

Abstract

本发明技术方案提供了一种基于多传感器融合的SLAM定位导航方法及系统，结合深度相机、激光雷达和全向轮里程计信息，输出障碍物点云、二维栅格地图和稠密点云以用于导航。通过改进贝叶斯数据算法的融合方法，设计基于A*和DWA的导航算法和异常处理方法并实时性。本发明的优点是通过对贝叶斯方法进行改进，融合深度相机、激光雷达和全向轮里程计信息，实现多传感器辅助自动定位、全局二维栅格地图构建和具有避障能力的移动机器人自主导航的目的。

Description

一种基于多传感器融合的SLAM定位导航方法及系统

技术领域

本发明涉及机器人自主导航领域，尤其涉及一种基于多传感器融合的SLAM定位导航方法及系统。

背景技术

目前在移动机器人领域，为了在人所不及的环境中作业，给人们日常生活提供便利，把人类从琐事中解放出来，移动机器人的自主导航正逐渐成为研究热点。实现移动机器人自主运行要解决三个问题：“此刻在哪”、“此刻周围环境如何”、“如何到达目的地”，分别对应：定位、地图构建、导航，机器人需要通过采集自身携带的传感器数据，确定运动轨迹，同时构建出环境地图。因此，建立一种便宜、有效且准确的定位与建图系统，实现机器人自主导航是非常有意义的。

同步定位和地图构建(SLAM)由R.C.Smith和P.Cheeseman在1986年国际机器人和自动化会议上提出，主流SLAM分为激光SLAM和视觉SLAM。激光SLAM基于激光传感器，能准确测量障碍物的角度与距离，具备对环境光线要求不高、能够生成导航地图等优点，得到广泛应用。激光SLAM代表性算法有GMapping和Hector SLAM，GMapping算法结合激光数据和里程计信息，使用粒子滤波器估算机器人轨迹，广泛用于2D占用网格地图的获取。Hector SLAM利用高斯牛顿方法解决扫描匹配问题，可以在计算资源较少的条件下通过2D激光雷达快速创建2D占用网格地图，定位漂移低。视觉SLAM基于视觉传感器，视觉图像易获取，且图像信息量大、特征丰富，视觉SLAM正逐渐成为研究热点。代表性算法有ORB-SLAM2、RGBiD-SLAM和Elastic Fusion。ORB-SLAM2结合RGB-D相机、立体相机获得环境的稀疏点云，使用词袋模型检测闭环，采用光束法平差(BA)优化地图，减少相机跟踪的帧速率对结果的影响，较好的实现了定位，但该方法构建的地图无法辨别出环境中的障碍物，难以直接用于后续的避障与路径规划等工作。RGBiD-SLAM使用光度误差和深度误差对RGB-D图像进行运动估计，可生成环境的稠密点云，但该算法假设跟踪时有足够的视觉特征，不适合移动机器人自主导航。Elastic Fusion采用迭代最近点(Iterative Closest Point，ICP)算法在GPU的支持下在线重建基于表面元素的地图，能在小型环境中实时处理RGB-D相机帧，但每帧的处理时间会随着地图的表面元素数量的增加而变长，实时性下降。以上方法存在障碍物边界探测精确度较低、因场景特征少导致位姿丢失、随运行时间增加实时性下降等问题，位姿准确度较低。

为解决上述问题，许多研究者对多传感器数据融合的SLAM技术进行研究，2017年提出一种集成激光雷达和惯性测量单元的在线SLAM算法，能够并行处理两种传感器信息，融合时仅记录和传递滤波器预测时的误差状态，在不损失定位精度的情况下解决时间滞后问题，有效地提高系统实时性。2019年针对激光雷达进行大范围扫描时误差积累快的缺点，将激光与视觉传感器相结合，采用基于图优化的SLAM框架，设计一种综合考虑扫描数据和图像数据的代价函数，构建了能同时显示障碍物和视觉特征的2.5D地图，可快速对机器人进行重定位。2020年融合激光雷达数据和单目相机信息，使用激光雷达的稀疏深度代替视觉特征，在多个关键帧下对每个测量值进行联合优化，通过严格的边缘化保持滑动窗口的关键帧不变，保证系统的实时性，提高了在稀疏深度测量条件下SLAM方法的精度和鲁棒性。

以上方法基本都是致力于在保证实时性的同时，提高SLAM方法的定位精度和鲁棒性，输出稠密地图进行导航，部分方法进行了障碍物的检测。但很多情况下，机器人只需构建出环境的二维栅格地图就可满足自主移动要求，并不需要输出稠密地图进行导航，所以如何提供一种只需构建环境的二维栅格地图成为亟待解决的问题。

发明内容

本发明提供一种基于多传感器融合的SLAM定位导航方法及系统，采用改进的贝叶斯方法融合深度相机、激光雷达和全向轮里程计信息，实现多传感器辅助自动定位、全局二维栅格地图构建和具有避障能力的移动机器人自主导航，无需输出稠密地图进行导航。

为了实现上述目的，本发明技术方案提供了一种基于多传感器融合的SLAM定位导航方法，包括：深度相机采集图像信息，激光雷达采集激光数据，全向轮移动底盘采集里程计信息。进行闭环检测，判断是否发生回环，若发生则将回环作为新约束用于修正位姿误差，当无法根据所述图像信息和激光数据修正位姿误差时，全向轮里程计信息参与进行所述闭环检测。将图像信息和激光数据进行预处理后得到的数据与闭环检测后得到的检测结果进行结合，得到修正后的位姿。深度相机和激光雷达分别构建二维局部栅格地图，之后二者将各自的二维局部栅格地图融合后，根据占用栅格单元概率设定融合规则。采用改进后的贝叶斯数据算法结合融合规则计算融合后栅格的占据概率：

从而得到全局二维栅格地图，基于全局二维栅格地图规划导航路线，并进行局部调整，使得机器人能够避障。其中，

为占用网格的先验概率，

为观测后的确认占用网格是否被占据的条件概率。

作为上述技术方案的优选，较佳的，首先根据图像信息和激光数据进行闭环检测，若能够获取位姿数据则判断是否发生回环；若无法获取位姿数据，则将全向轮里程计信息与图像信息和激光数据信息结合进行闭环检测。

作为上述技术方案的优选，较佳的，对图像信息和激光数据均进行特征提取及匹配的数据预处理，对图像信息的预处理数据结果进行最小化视觉图像信息误差；对激光数据的预处理数据结果进行最小化激光总误差处理；结合最小化视觉图像信息误差的数据结果和闭环检测结果对位姿进行修正。

作为上述技术方案的优选，较佳的，深度相机生成三维点云地图，进一步得到二维离散障碍图，从而构建深度相机的二维局部栅格地图。激光雷达根据激光数据构建一个二维局部栅格地图。

作为上述技术方案的优选，较佳的，对深度相机构建的二维栅格地图和激光雷达构建的二维局部栅格地图进行融合，包括：将激光雷达和深度相机各自构建的二维局部栅格地图整合。获取整合后二维局部栅格地图中激光雷达和深度相机占用网格单元的概率，根据概率设定融合规则。基于融合规则，对不能确定是否被占用的栅格采用改进后的贝叶斯数据算法获取该栅格的占据概率，根据占据概率判断该栅格是否被占据。O代表被激光传感器观测到障碍物事件，E表示障碍物存在事件，观测模型的条件概率表示为：

其中，P(E)为占用网格单元的先验概率，初始值0.5。从占用网格G中的任何一个单元点:

结合当前观测值r_t＝(r_t,r_t-n,r₀)，得到融合概率计算公式：

根据融合概率的数值将所述全局二维栅格地图构建完毕。

作为上述技术方案的优选，较佳的，在获取观测模型的条件概率表示公式之前，还包括：观测Z_k＝{z_i,i＝1,2,3...k}和现在时刻的概率x_k，则先验分布为：

作为上述技术方案的优选，较佳的，基于全局二维栅格地图规划导航路线，包括；采用A*算法对全局二维栅格地图进行全局路径规划，将全局二维栅格地图划分为若干区域，根据评估准则，评估每个区域中各节点的代价，得到权值最小的父节点，得到终点后评估完毕后，将各父节点、终点和起点连接后得到的路径为导航路线。

作为上述技术方案的优选，较佳的，局部调整使用DWA算法，包括：在机器人的控制空间中进行离散采样得到采样速度。计算所述机器人受自身最大速度最小速度限制的速度数据。计算所述机器人受电机性能影响的干扰数据。计算所述机器人的预计速度范围。对每个采样速度进行模拟得到模拟轨迹，根据速度数据、干扰数据和预计速度范围对模拟轨迹进行评估，选择得分最高的轨迹发送至机器人。

本发明还提供一种基于多传感器融合的SLAM定位导航系统，包括：采集组件，用于采集图像信息、激光数据和全向轮里程计信息。预处理模块，用于对所述图像信息、所述激光数据进行特征提取及匹配。闭环检测单元，用于根据所述采集组件采集的所述图像信息、激光数据和全向轮里程计信息获取位姿数据。激光数据处理模块，用于对所述预处理模块得到的激光预处理数据结果进行最小化激光总误差处理。图像信息处理模块，用于对所述预处理模块得到的图像信息预处理数据结果进行最小化视觉图像信息误差。位姿输出模块，用于构建一个非线性最小二乘问题，对得到的最小化激光数据误差和位姿优化视觉数据进行恢复，得到精确位姿图像，其中所述位姿图像包括所述激光数据生成的激光位姿栅格图像和所述图像信息生成的视觉位姿栅格图像。地图构建模块，用于分别构建所述图像信息和所述激光数据各自对应的二维局部栅格地图，通过使用改进的贝叶斯数据融合算法计算不能确定融合后是否被占用的栅格的融合后栅格占据概率，从而构建全局二维栅格地图。导航模块，用于初始化所述全局二维栅格地图后，获取起点和终点信息，然后使用A*算法进行全局路径规划、使用DWA算法进行局部路径规划。

作为上述技术方案的优选，较佳的，地图构建模块，还包括：融合规则设定单元，用于对图像信息和激光数据各自的二维局部栅格地图进行整合，整合后二维局部栅格地图中激光雷达和深度相机占用网格单元的概率，根据所述概率设定融合规则。融合单元，用于根据融合规则，对整合后二维局部栅格地图中不能确定融合后是否被占用的栅格采用改进后的贝叶斯数据算法获取该栅格的占据概率，根据占据概率判断该栅格是否被占据。

本发明技术方案提供了一种基于多传感器融合的SLAM定位导航方法及系统，结合深度相机、激光雷达和全向轮里程计信息，输出障碍物点云、二维栅格地图和稠密点云以用于导航。通过改进贝叶斯数据算法的融合方法，设计基于A*和DWA的导航算法和异常处理方法并实时性。本发明的优点是通过对贝叶斯方法进行改进，然后深度相机、激光雷达和全向轮里程计信息进行融合，最后实现多传感器辅助自动定位、全局二维栅格地图构建和具有避障能力的移动机器人自主导航的目的。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的一种基于多传感器融合的SLAM定位导航方法的实施流程图。

图2为图1中步骤109的流程图。

图3为图1中步骤110的流程图。

图4为本发明技术方案所涉及的算法整体架构图。

图5为RTAB-MAP系统结构框图。

图6为机器人恢复行为流程图。

图7为SLAM姿态示意图。

图8为局部二维栅格地图的融合过程流程图。

图9为本发明提供的一种基于多传感器融合的SLAM定位导航系统的结构示意图。

图10为图9中地图构建模块的结构示意图。

图11为本发明所用的基于ROS&RTAB-Map的移动机器人节点架构的示意图。

图12为模拟实验二中地图构建实验截图。

图13为模拟实验三的实验截图。

图14为模拟实验三中将激光构建的栅格地图与本发明方法构建的栅格地图进行对比。

图15为模拟实验四的真实环境避障过程。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

一种基于多传感器融合的SLAM定位导航方法，具体实施步骤如图1所示，包括：

步骤101、深度相机采集图像信息，激光雷达采集激光数据，全向轮移动底盘采集里程计信息。

步骤102、对图像信息和激光数据进行预处理得到二者特征提取及匹配结果。

对图像信息预处理包括：利用ORB(oriented FAST and rotated BRIEF)特征提取算法提取图像特征，通过最近邻距离比率法(Nearest Neighbor Distance Ratio,NNDR)比较最近邻特征和次近邻特征的距离进行特征点匹配，应用随机采样一致性算法(RandomSample Consensus，RANSAC)消除误匹配，采用PnP(Perspective-N-Point)完成运动估计。

具体的：视觉传感器获取彩色图像和深度图像作为输入，通过对图像进行ORB特征提取，通过特征匹配算法得到点对，根据点对机器人的运动进行估计。ORB特征使用的关键点是一种添加了旋转不定性的FAST角点，它的描述子是BRIEF(Binary RobustIndependent Elementary Feature)。通过比较最近邻特征和次近邻特征的距离可以有效甄别局部特征是否正确匹配，即(Nearest Neighbor Distance Ration，NNDR)，消除引起干扰的噪点。待匹配特征点为D_A，最邻近特征为D_B，次邻近特征为D_C，判断该特征匹配的条件为：

满足条件的特征点即为可匹配的特征点，不满足条件的特征点进行剔除。运动估计PnP有很多方法，但是与其他方法相比，EPnP方法的复杂度为O(n)。对于点对数目较多的PnP问题，极其高效。EPnP的基本思想是将三维点表示为4个控制点的组合，而优化仅针对4个控制点，因此速度很快；在求解过程中考虑了4个奇异向量，因此精度也很高。

求解相机姿态ξ的最小二乘问题为：

其中si表示空间点Pi在图像Ii所在相机坐标系的深度，ui表示空间点对应投影点的像素坐标，K表示相机内参数，exp

表示相机从图像Ii到图像Ii-1的姿态变换阵。对激光数据预处理包括，采用迭代最近点(Iterative Closest Point，ICP)方法进行匹配并估计位姿，具体的：

激光雷达采集的点云数据没有明显的几何特征，本发明采用迭代最近点进行匹配。通过激光雷达获得第一组点云P＝{p₁,p₂,...,p_n}，激光雷达经过位姿变换(旋转加平移)后又获得第二组点云Q＝{q₁,q₂,...,q_n}，通过计算两点之间最近邻距离调整点云的存储顺序，使得P和Q中的点一一对应，两个点云通过一次旋转R的矩阵计算和一次平移t的矩阵运算可以得到以下优化目标：

其中，J为目标函数。

步骤103、判断图像信息和激光数据估计位姿是否成功，若是则执行步骤104、否则调取全向轮里程计信息后执行步骤104。

步骤104、进行闭环检测，判断是否发生回环，若是则执行步骤108，否则结束。其中，闭环检测根据传感器信息判断机器人是否访问过先前的某个区域，采用词袋模型(Bag-of-Words，BoW)判断是否发生回环。其中词袋模型BoW通过聚类形成词袋，每个特征根据词袋形成向量，通过计算每个特征向量的相似度就可以找到回环数据，认为机器人经过了之前的同一个位置，之后添加新的约束从而消除因噪声造成的累计误差。

步骤105、最小化图像信息总误差。

步骤106、最小化激光数据总误差。

对于步骤105和106应用g2o优化方法构建一非线性最小二乘问题，最小化激光数据误差和视觉图像信息误差，恢复出更精确的位姿，具体的：

g2o是执行非线性最小二乘问题优化的一般框架，它可以表示为一个图。这里的图是图论意义上的图。多个顶点和每个顶点连接形成的边构成了这个图。此外，这些顶点集成了优化变量，连接顶点的这些边集成了误差项。于是，对任意形式的非线性最小二乘问题，我们可以构建对应的一个图。对步骤106进行详细说明，如图7所示，深度相机的位姿节点由三角形表示，路标点由圆表示，他们为图优化的顶点；相机模型的运动模型由实线表示，观测模型由虚线表示，他们为图优化的边。g2o优化边的误差函数为：

x^*＝argminF(x) (5)

其中，X为全部位姿变量x的集合，C代表估计位姿的时间序列，x_i和x_j是i，j时刻估计出的机器人位姿，

一个参数向量，其中每个x_i表示一个通用参数块，z_ij和Ω_ij分别表示与参数x_i和x_j相关的约束的平均值和信息矩阵，e(x_i,x_j,z_ij)为一个向量误差函数，用于测量参数块x_i和x_j对z_ij约束的满意度，当x_i和x_j完全匹配约束时，它为0，此时公式(4)为最小值，从而实现最小化图像信息总误差，步骤106的计算与步骤105相同，此处不再赘述。

步骤107、将步骤104得到的回环数据作为新的边约束添加到位姿中修正累积误差，之后执行步骤108。

步骤108、生成位姿。

根据步骤105和步骤106的最小化误差结果生成一位姿，若输入数据有新的边约束则结合步骤107生成位姿。

步骤109、构建全局二维栅格地图。

具体的，通过分别生成激光雷达的二维局部栅格地图和图像信息的二维局部栅格地图后将二者进行融合从而构建全局二维栅格地图。

步骤109的具体步骤如图2所示

步骤201、获取激光数据。

步骤202、构建二维局部栅格地图。

激光雷达数据进行滤波处理生成局部栅格地图。

步骤203、深度相机获取一定高度的三维点云数据。

步骤204、构建二维局部栅格地图。

应用贝叶斯方法对三维点云数据提取光束投影形成二维的离散障碍图，从而构建步骤204中的二维局部栅格地图。

步骤205、将两个二维局部栅格地图融合。

步骤206、获取整合后二维局部栅格地图中激光雷达和深度相机占用网格单元的概率。

具体的，激光雷达和深度相机的占用栅格如下表1所示，根据下表1概率设定融合规则，如表1所示：

表1融合规则

根据概率设定融合规则计算图像信息和激光数据占用栅格的概率，基于表1的融合规则，对不能确定是否被占用的栅格采用改进后的贝叶斯数据算法获取该栅格的占据概率，根据占据概率判断该栅格是否被占据。

步骤207、根据占用网格单元的概率构建全局二维栅格地图。

对于步骤205至207：

具体的，全局二维栅格地图构建采用改进的贝叶斯数据融合方法。

贝叶斯数据融合方法是基于贝叶斯定理的条件或后验概率的统计融合算法，用条件概率表达各个信息要素之间的相关性，有效地进行多元信息的表达和融合。观测Z_k＝{z_i,i＝1,2,3...k}和现在时刻的概率x_k，先验分布为：

式中：P(z_k|x_k)为传感器观测模型的似然函数，P(x^k|Z^k-1)为机器人动力学模型的先验分布函数，P(z^k|Z^k-1)为保证归一化概率密度的规格化术语。

使用贝叶斯方法更新占用网格地图的概率，O代表被激光传感器观测到障碍物事件，E表示障碍物存在事件，经过贝叶斯推理，观测模型的条件概率表示为：

式中：P(E)为占用网格单元的先验概率，初始值0.5。

进一步的，融合过程如图8所示，从占用网格G中的任何一个单元点：

开始,结合当前观测值r_t＝(r_t,r_t-n,r₀)，根据贝叶斯递推法得到改进的融合公式计算估计值：

式中：

为占用网格的先验概率，

为观测后的确认占用网格是否被占据的条件概率。

当占用网格都为不确定时，应用贝叶斯方法式(8)求得融合后的占用网格占据概率

步骤110、进行全局路径和局部路径的规划。

具体的，步骤110的详细实施步骤如图3所示：

步骤301、对全局二维栅格地图初始化。

步骤302、A*算法规划全局路径。

A*算法结合了BFS算法和Dijkstra方法的优点，最佳路径由代价大小决定，代价大小的评估准则为：F(n)＝g(n)+h(n) (10)

其中n为待扩展的节点，g(n)为初始节点到此节点的真实代价值，h(n)为此节点到终节点的估计值。A*算法主要逻辑为根据估计准则，评估周围节点的代价，然后选择权值最小的周围节点，直到到达终结点。

A*算法的步骤如下：把区域划分成大小一致的形状，形状的中心被称为一个节点；建立open list和close list，在open list中放入起点。

步骤303、相邻节点设为局部目标并移动。具体的，

1)找到open list中F值最小的节点，把它作为待处理节点。

2)把这个节点移动到close list。

3)对当前方格的8个相邻方格每一个方格作如下判断：

a)如果它是不可抵达的或者它在close list中，忽略。否则作如下操作。

b)如果它不在open list中，把它放入open list，并把现节点当作父节点。

c)如果已经在open list中，检查经由当前节点到此相邻节点是否更好，如果是，把当前节点作为父节点，否则重新计算父节点。

4)当终点加入到open list中或者open list为空时，停止。

5)从终点开始，把每个父节点连接起来直到起点，连接路径为要找路径。

步骤304、传感器是否感知障碍物，若是，则执行步骤305，否则执行步骤306。步骤305为局部路径规划。

步骤305、采用动态窗口法规划局部路径，并将结果反馈至步骤303。

当机器人遇到移动障碍物时无法避免，为避免移动障碍物还需要局部路径规划，本发明使用动态窗口法(DWA)进行路径规划，详细的：

(1)在机器人的控制空间(dx,dy,dθ)中进行离散采样；机器人本身和环境会限制机器人的速度。

a、计算移动机器人受自身最大速度最小速度限制：

V_m＝{v∈[v_min,v_max],ω∈[ω_min,ω_max]} (11)，式中：ν为速度，ω为角度。

b、计算移动机器人受电机性能的影响：

式中：v_c为当前速度，ω_c为当前角度。

为保证机器人能顺利到达，结合步骤a和b的结果计算其速度范围：

式中：dist(ν,ω)为(ν,ω)对应轨迹上离障碍物最近的距离。

(2)对于每个采样的速度ν，从机器人的当前状态执行正向模拟，对在某个(短)时间段内使用该速度会发生的情况进行轨迹预测。

(3)使用包含以下特征的指标来给由正向模拟产生的每个轨迹打分：障碍物的接近程度、目标点的接近程度、全局路径的接近程度和速度的大小。清除与障碍物碰撞的轨迹。其评估函数为：

G(v,w)＝s×(a×heading(v,w)+b×dist(v,w)+g×velocity(v,w)) (14)

式中：heading(v,w)为机器人到轨迹末端和目标的角度差，相差角度越小评分越高，dist(v,w)为轨迹与障碍物的距离，velocity(v,w)为评价当前采样速度的大小，其余的为增益系数。

为保证评价合理性，对函数进行归一化处理：

(4)选择得分最高的轨迹并将对应的速度发送到移动底盘(步骤304)。

(5)清除并重复。

步骤306、判断是否达到全局目标节点，若是，结束，否则返回步骤304。

图4为本发明技术方案所涉及的算法整体架构图，图5为RTAB-MAP系统结构框图，现结合图4和图5对上述具体实施步骤进行进一步说明。

如图4所示，本发明所涉及的算法架构由定位与建图模块和导航模块组成，定位与见图模块包括前端、后端和建图三部分，具体的：

定位与建图模块采用RTAB-MAP框架，RTAB-Map是一种融合RGB-D相机图像、激光雷达数据和全向轮里程计信息的SLAM方法，整体结构如图5所示。

如图5所示，RTAB-Map采取工作-短期-长期记忆的内存管理机制，在短期记忆(Short-Term Memory，STM)获取新数据，观察连续图像的相似度，当STM不能再容纳新节点时，将在STM时间最久的节点移动到工作记忆(Working Memory，WM)；WM中保留最近和经常观察到的位置，将其他位置转移到长期记忆(Long-Term Memory，LTM)；当在当前位置和存储在工作记忆中的位置之间找到匹配时，记住并更新存储在长期记忆中的相关位置。使用后用于处理的节点数被合理限制，确保处理时间不会超过实时约束，大幅度提高在平台计算能力有限时SLAM的实时性。

RTAB-MAP的主要工作在WM中完成，数据同步模块利用RGB-D图像配准技术、附带里程计信息的立体图像校准技术对RGB-D图像、激光扫描数据、里程计信息进行同步：具体的，即为定位与建图模块的前端采集RGB-D相机图像、激光数据和全向轮里程计信息，对视觉图像，利用ORB特征提取算法提取图像特征，通过最近邻距离比率法比较最近邻特征和次近邻特征的距离进行特征点匹配，应用随机采样一致性算法消除误匹配，采用PnP完成运动估计。对激光数据，采用迭代最近点方法进行匹配并估计位姿。STM创建2D/3D局部栅格地图后，进行邻近检测和闭环检测，判断信息采集方向，确定新图像来自先前位置或新位置的可能性；当接受闭环假设时，将新约束添加到地图图像中，采用图优化器g2o修正地图错误，拼接全局地图。闭环检测根据传感器信息判断机器人是否之前访问过某个区域，采用词袋模型判断是否发生回环，将回环作为新的边约束添加到位姿中修正累积误差，当视觉和激光估计位姿失败时，全向轮里程计信息直接参与闭环校正。定位与构图模块的后端应用g2o优化方法构建一个非线性最小二乘问题，最小化激光数据误差和视觉图像信息误差，恢复出更精确的位姿。在优化位姿的基础上，地图构建采用改进的贝叶斯数据融合方法，确定融合后栅格的占据概率，构建全局二维栅格地图。

导航模块主要包括全局规划器、局部规划器和异常处理机制。全局规划器在二维栅格地图上规划出从起点位置到终点位置的路线，局部规划器进行局部调整，使机器人在导航时避开障碍物，保障机器人安全。通过使用A*算法和动态窗口法，将局部路径规划结合到全局路径规划之中，以保证动态规划路径的全局最优性：首先记录不包含移动障碍物时地图中每个位置可能与机器人发生冲突的概率，生成全局成本地图传给全局规划器规划导航路径。感知机器人周围在传感器范围内的移动障碍物，在动态场景下实时获取由激光传感器标记的障碍物点云和深度相机的3D点云，并投影到2D平面上，记录包含移动障碍物的局部地图中每个位置可能与机器人发生冲突的概率，生成局部成本地图，与全局路线一起传给局部规划器，以全局路径为基础做出调整，及时完成避障操作。最后局部规划器通过调整后的路径和机器人的性能参数计算机器人将要采取的运动速度指令，传送给移动底盘进行运动。

当出现导航异常时，导航模块主要的异常处理机制启动，通过异常处理方法恢复行为，继续导航，具体的，恢复行为流程如图6所示，机器人导航过程中可能因为多种原因陷入无法运动或无法导航的状况，一旦陷入这种状况，机器人需要立即采取恢复措施，通过清除地图信息和位姿调整试图恢复导航，机器人将依次进行清除路径附近区域外的障碍物、原地旋转找到没有障碍的方向、清除可以在其中旋转的矩形区域之外的所有障碍物、再一次就地旋转直至恢复导航，如果最后依然不能摆脱，机器人将认为无法继续进行导航并通知用户导航已经中止，则中止点为终点。

本发明还提供一种基于多传感器融合的SLAM定位导航系统，如图9所示：

采集组件91，用于采集图像信息、激光数据和全向轮里程计信息。

预处理模块92，用于对图像信息、所述激光数据进行特征提取及匹配。

闭环检测单元93，用于根据采集组件采集的图像信息、激光数据和全向轮里程计信息获取位姿数据。其中，若无法根据图像信息、激光数据获取位姿数据进行闭环检测，则从采集组件91中调取全向轮里程计信息参与闭环检测。

激光数据处理模块94，用于对预处理模块92得到的激光预处理数据结果进行最小化激光总误差处理。

图像信息处理模块95，用于对预处理模块92得到的图像信息预处理数据结果进行最小化视觉图像信息误差。

位姿输出模块96，用于构建一个非线性最小二乘问题，对得到的最小化激光数据误差和位姿优化视觉数据进行恢复，得到精确位姿图像，其中位姿图像包括激光数据生成的激光位姿栅格图像和图像信息生成的视觉位姿栅格图像。

地图构建模块97，用于分别构建图像信息和激光数据各自对应的二维局部栅格地图，通过使用改进的贝叶斯数据融合算法计算不能确定融合后是否被占用的栅格的融合后栅格占据概率，从而构建全局二维栅格地图。

导航模块98，用于初始化所述全局二维栅格地图后，获取起点和终点信息，然后使用A*算法进行全局路径规划和DWA算法进行局部路径规划。

如图10所示，地图构建模块97，还包括：

融合规则设定单元971，用于对图像信息和激光数据各自的二维局部栅格地图进行整合，整合后二维局部栅格地图中激光雷达和深度相机占用网格单元的概率，根据概率设定融合规则。

融合单元972，用于根据融合规则，对整合后二维局部栅格地图中不能确定融合后是否被占用的栅格采用改进后的贝叶斯数据算法获取该栅格的占据概率，根据占据概率判断该栅格是否被占据。

现进一步结合基于本发明技术方案进行的模拟实验进行说明：

在对模拟实验进行说明之前，首先对本发明所采用的基于ROS&RTAB-Map的移动机器人节点架构进行说明：

本文算法在机器人操作系统(Robot Operating System，ROS)架构下实现，系统应用navigation软件包、RTAB-Map软件包、ros_arduino_bridge软件包完成地图构建和导航功能，并发送运动速度指令控制机器人三轮全向移动底盘向目标点运动，节点设计架构如图11所示。

图中，实线框内的节点提供SLAM功能：数据同步、生成实时视野点云和检测障碍物，并为导航模块提供二维占用网格地图。其中包括主节点rtabmap、相机数据同步节点rgbd_sync、数据可视化节点Rviz、相机与里程计数据同步节点data_odom_sync、点云生成节点point_cloud_xyzrgb和障碍物识别节点obstacles_detection。主节点rtabmap将RGB-D相机数据经过节点rgbd_sync发送的同步相机数据和里程计信息、激光扫描数据进行近似同步，减轻由于传输延迟导致的不同步问题。节点Rviz接收节点data_odom_sync同步的里程计信息和相机数据进行数据可视化。节点point_cloud_xyzrgb输入相机的同步数据，生成3D点云发送给节点obstacles_detection，节点obstacles_detection用来标注点云中处于机器人高度范围内的障碍物并投影到地面，发布障碍物点云主题。

虚线框内为导航模块节点，全局成本地图接收SLAM模块输出的二维占用网格地图和障碍物点云，结合激光数据记录静态障碍物信息。局部成本地图用来实时更新动态障碍物信息。为防止机器人导航时陷入无法运动的状况，设计一种恢复行为节点，利用成本地图调整位姿恢复导航。通过全局成本地图和目标点位姿信息，全局规划器采用A*算法规划全局导航路径，局部规划器在全局路径的基础上，结合局部成本地图，使用DWG算法实现避障操作。局部规划器通过调整后的路径和机器人的性能参数计算得到机器人运动速度指令，用来控制机器人三轮全向移动底盘的运动。

模拟实验一：

本发明所提供的系统基于公开数据集TUM进行实验。TUM数据集是2012年TUM计算机视觉组提出的RGB-D数据集，自带真实轨迹与测量误差的Python脚本，其目标场景是机器人救援，使用手持Kinect v1录制。本次选取TUM数据集中的fr1/desk2和fr1/room数据集，其中fr1/desk2包含对四个办公桌的扫描，fr1/room包含机器人从一个屋子通往另一个屋子的所有路程信息。

绝对轨迹误差(ATE)用于衡量估计轨迹与真实轨迹的一致程度，即轨迹准确度。故本发明选取位姿绝对误差评价位姿估计精度，在fr1/desk2、fr1/room数据集上的ATE为0.0207m、0.0517m。与ORB-SLAM2及RGBD SLAM在相同数据集上的ATE比较，如表2所示，单位为米(m)，可以看出本发明方法的位姿绝对误差均优于另外两种方法，具有更高的定位准确度。

表2三种方法的位姿绝对误差(单位：米)

模拟实验二：

本次在真实环境中进行自主定位实验，机器人从图12中的起点A点依次到达B、C、D点，图中A、B、C、D各点处的箭头方向表示各点机器人的朝向。

根据地图和实际测量数据可以获得机器人在地图上的坐标位置及各点角度，实验结果如表3和表4所示。与仅使用激光雷达估计的位姿比较，本发明方法在点A、B、C、D的位置误差分别为0.48m、0.145m、0.265m、0.331m，角度误差分别为1.968°、0.03°、0.443°、1.105°，均低于仅使用激光雷达估计的位置误差和角度误差，可以看出本发明方法由于能够纠正闭环，位姿更为准确。

实验中，当闭环检测出现障碍时，机器人根据最新回环假设位姿得到的传感器数据，发生位置上的漂移，但随后会通过激光扫描数据进行修正，即地图构建时机器人将对不同角度的图像信息进行全面的收集，用以增加闭环检测的匹配率，提高自动定位速度。

表3仅使用激光雷达的位姿估计

表4融合后的位姿估计

其中，表3和表4中真实位置、位置估计和位置误差的单位为米(m)，真实角度、角度估计和角度误差的单位为角度(°)。

模拟实验三：

为进一步分析本发明算法在真实环境中地图构建时的效果，利用机器人在室内进行地图构建实验。实验过程的截图如图13所示。

上方的截图是RViz工具显示的地图构建情况，可以看到通过激光雷达生成的2D占用网格图；下方的截图为RTAB-Map自带的可视化工具rtabmapviz，在画面左侧显示闭环检测的匹配情况。从图13左侧图可以看出，地图构建开始时尽管图像只有一张，但是机器人的侧后方已经产生了占用网格，基于激光雷达的占用网格生成发生了作用。从图13右侧图可以看出，场地中的墙壁良好地贴合在占用网格的障碍边缘，视觉图像与激光雷达构建的信息得到了充分的融合。

本实验中，存在机器人面对白墙无法获取特征点或特征点少的场景，但由于本算法融合了全向轮里程计信息，能够克服因视觉特征少导致位姿丢失的问题，使得机器人不出现丢失位姿的问题，实现了闭环检测。实验中深度相机传来的图像帧以稳定的速率得到处理，没有发现随着运行时间增加，处理时间变慢、实时性下降的问题，表明通过内存管理机制本发明算提供的方法满足SLAM的实时性。

同时，为了验证本发明多传感器融合的SLAM方法可以在地图中准确标记出障碍物边界，在室内放置一把高于激光雷达所在高度的椅子，将激光构建的栅格地图与本发明方法构建的栅格地图进行对比，结果如图14所示。由于用激光雷达只能扫面激光所在平面的障碍物高度，而深度相机的垂直范围为60°，能够探测到激光平面以外的障碍物高度，获取椅子信息，从图中可以明显看出本发明方法得到的地图更加精确。

模拟实验四：

导航避障实验旨在验证当地图中有障碍物时，机器人能否按规划路径避开障碍物。图15为真实环境避障过程，障碍物为黑色矩形物体。

从图15可以发现，机器人在遇到障碍物时可以及时调整位姿，与障碍物之间保持安全的距离。同时，机器人还能保持向目标点运动的方向，最大限度避免过度旋转移动对运动速度的干扰。最终机器人准确地到达目标点，位姿也没有受到干扰，成功避开障碍物。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种基于多传感器融合的SLAM定位导航方法，其特征在于，所述方法包括：

深度相机采集图像信息，激光雷达采集激光数据，全向轮移动底盘采集里程计信息；

进行闭环检测，判断是否发生回环，若发生则将回环作为新约束用于修正位姿误差，当无法根据所述图像信息和所述激光数据修正位姿误差时，所述全向轮里程计信息参与进行所述闭环检测；

将所述图像信息和激光数据进行预处理后得到的数据与所述闭环检测后得到的检测结果进行结合，得到修正后的位姿；

所述深度相机和所述激光雷达分别构建二维局部栅格地图，之后二者将各自的所述二维局部栅格地图融合后，根据占用栅格单元概率设定融合规则；

采用改进后的贝叶斯数据算法结合融合规则计算融合后栅格的占据概率：

从而得到全局二维栅格地图；

基于所述全局二维栅格地图规划导航路线，并进行局部调整，使得机器人能够避障；

其中，

为占用网格的先验概率，

为观测后的确认占用网格是否被占据的条件概率。

2.根据权利要求1所述的基于多传感器融合的SLAM定位导航方法，其特征在于，首先根据所述图像信息和所述激光数据进行闭环检测，若能够获取位姿数据则判断是否发生回环；若无法获取所述位姿数据，则将所述全向轮里程计信息与所述图像信息和所述激光数据信息结合进行所述闭环检测。

3.根据权利要求1所述的基于多传感器融合的SLAM定位导航方法，其特征在于，对所述图像信息和所述激光数据均进行特征提取及匹配的数据预处理，对所述图像信息的预处理数据结果进行最小化视觉图像信息误差；对所述激光数据的预处理数据结果进行最小化激光总误差处理；

结合所述最小化视觉图像信息误差的数据结果和所述闭环检测结果对位姿进行修正。

4.根据权利要求1所述的基于多传感器融合的SLAM定位导航方法，其特征在于，所述深度相机生成三维点云地图，进一步得到二维离散障碍图，从而构建所述深度相机的二维局部栅格地图；

所述激光雷达根据所述激光数据构建一个二维局部栅格地图。

5.根据权利要求4所述的基于多传感器融合的SLAM定位导航方法，其特征在于，对所述深度相机构建的二维栅格地图和所述激光雷达构建的二维局部栅格地图进行融合，包括：

将所述激光雷达和所述深度相机各自构建的所述二维局部栅格地图整合；

获取整合后二维局部栅格地图中所述激光雷达和所述深度相机占用网格单元的概率，根据所述概率设定融合规则；

基于所述融合规则，对不能确定是否被占用的栅格采用所述改进后的贝叶斯数据算法获取该栅格的占据概率，根据所述占据概率判断该栅格是否被占据：

O代表被激光传感器观测到障碍物事件，E表示障碍物存在事件，观测模型的条件概率表示为：

其中，P(E)为占用网格单元的先验概率，初始值0.5；

从占用网格G中的任何一个单元点:

结合当前观测值r_t＝(r_t,r_t-n,r₀)，得到融合概率计算公式：

根据融合概率的数值将所述全局二维栅格地图构建完毕。

6.根据权利要求5所述的基于多传感器融合的SLAM定位导航方法，其特征在于，在获取所述观测模型的条件概率表示公式之前，还包括：

观测Z_k＝{z_i,i＝1,2,3...k}和现在时刻的概率x_k，则先验分布为：

7.根据权利要求1所述的基于多传感器融合的SLAM定位导航方法，其特征在于，所述基于所述全局二维栅格地图规划导航路线，包括；

采用A*算法对所述全局二维栅格地图进行全局路径规划，将所述全局二维栅格地图划分为若干区域，根据评估准则，评估每个区域中各节点的代价，得到权值最小的父节点，得到终点后评估完毕后，将各所述父节点、终点和起点连接后得到的路径为所述导航路线。

8.根据权利要求1所述的基于多传感器融合的SLAM定位导航方法，其特征在于，所述局部调整使用DWA算法，包括：

在机器人的控制空间中进行离散采样得到采样速度；

计算所述机器人受自身最大速度最小速度限制的速度数据；

计算所述机器人受电机性能影响的干扰数据；

计算所述机器人的预计速度范围；

对每个所述采样速度进行模拟得到模拟轨迹，根据所述速度数据、所述干扰数据和所述预计速度范围对所述模拟轨迹进行评估，选择得分最高的轨迹发送至所述机器人。

9.一种能够实现如权利要求1-8任一项所述的基于多传感器融合的SLAM定位导航系统，其特征在于，包括：

采集组件，用于采集图像信息、激光数据和全向轮里程计信息；

预处理模块，用于对所述图像信息、所述激光数据进行特征提取及匹配；

闭环检测单元，用于根据所述采集组件采集的所述图像信息、激光数据和全向轮里程计信息获取位姿数据；

激光数据处理模块，用于对所述预处理模块得到的激光预处理数据结果进行最小化激光总误差处理；

图像信息处理模块，用于对所述预处理模块得到的图像信息预处理数据结果进行最小化视觉图像信息误差；

位姿输出模块，用于构建一个非线性最小二乘问题，对得到的最小化激光数据误差和位姿优化视觉数据进行恢复，得到精确位姿图像，其中所述位姿图像包括所述激光数据生成的激光位姿栅格图像和所述图像信息生成的视觉位姿栅格图像；

地图构建模块，用于分别构建所述图像信息和所述激光数据各自对应的二维局部栅格地图，通过使用改进的贝叶斯数据融合算法计算不能确定融合后是否被占用的栅格的融合后栅格占据概率，从而构建全局二维栅格地图；

导航模块，用于初始化所述全局二维栅格地图后，获取起点和终点信息，然后使用A*算法进行全局路径规划、使用DWA算法进行局部路径规划。

10.根据权利要求9所述的基于多传感器融合的SLAM定位导航系统，其特征在于，所述地图构建模块，还包括：

融合规则设定单元，用于对所述图像信息和所述激光数据各自的二维局部栅格地图进行整合，整合后二维局部栅格地图中所述激光雷达和所述深度相机占用网格单元的概率，根据所述概率设定融合规则；

融合单元，用于根据融合规则，对整合后二维局部栅格地图中不能确定融合后是否被占用的栅格采用所述改进后的贝叶斯数据算法获取该栅格的占据概率，根据所述占据概率判断该栅格是否被占据。

Images