CN111427047B - 一种大场景下自主移动机器人slam方法 - Google Patents

Abstract

本发明公开了一种大场景下自主移动机器人SLAM方法，首先考虑机器人在空间平面运行所面临各种噪声的影响，由不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型；然后采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，结合位姿优化模块，得到一个精准的位姿和地图；最后将得到的位姿和地图信息传送到基于信息论方法的自主探索算法中，让机器人自主完成定位与建图任务。本发明相对其他方法计算量均很低，而且适用于结构化与非结构化环境当中，具有良好的鲁棒性，适用于更加复杂、较大的环境场合，并且能够实现机器人在未知环境下自主地大范围建图与定位。

Description

一种大场景下自主移动机器人SLAM方法

技术领域

本发明属于机器人定位与建图(SLAM)领域，涉及到自主探索和同步定位与地图构建等机器人技术，具体涉及一种大场景下自主移动机器人SLAM方法。

背景技术

自起步发展至今，SLAM技术已经有三十多年的历史，其功能以及应用范围随着科学技术的发展以及人们对生活生产的需要而逐渐完善和推广。SLAM问题可以描述为：移动机器人在没有任何先验环境的情况下，仅凭借自身传感器构建未知环境地图的同时确定自身的位姿。SLAM已成为近年来机器人领域的热点研究问题，并被认为是实现真正自主机器人的核心环节。

随着机器人应用的不断推广，SLAM技术所面临的问题与挑战也越来越大，比如机器人由初始的小范围作业变为需要适应特殊场合的大环境，在大场景下如何保证前后帧之间的关联以及数据的准确匹配；随着场景的越来越大，SLAM系统所要维护的地图数据量也越来越大，从而加大了地图构建的难度，这如何保证系统处理数据的实时性；每一次位姿估计的微小误差，机器人经过长时间与长距离的运作必然会累积成一个较大的误差，这样的误差该如何有效地减小甚至消除等等。这些问题也都是SLAM需要解决的。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种更加复杂、较大的环境场合下，机器人准确实现自主定位与建图的一种大场景下自主移动机器人SLAM方法。

为解决上述技术问题，本发明的一种大场景下自主移动机器人SLAM方法，包括如下步骤：

步骤1：采用不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型；

步骤2：首先对步骤1得到的观测数据建立栅格化地图，状态估计方程为：

将等式右边取对数，则：

将连乘运算转换成加法运算，通过上式将整帧观测数据的概率转换成当前帧数据每一个点的概率；

对观测数据建立栅格化地图：将环境划分为n×n数量的栅格，以空闲和占据表示栅格的两种状态，空闲表示栅格所表示的空间没有障碍物，用白色来填充，占据表示栅格所代表的空间有障碍物，用黑色来填充，还没有探测到的未知区域为灰色，最后采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，初步得到机器人的位姿信息与环境地图信息；

步骤3：由步骤2得到的初步机器人位姿信息以及环境地图信息，通过构建位姿以及路标点之间的图模型，利用它们之间的约束关系，采用非线性优化的方式将估计位姿与实际观测所得到的位姿构造一个误差函数，最小化误差函数以进一步求得精确位姿；

步骤4：将得到的精确的位姿和地图信息，传送到基于信息论方法的自主探索算法中，该算法的长期目标是减少机器人在探索未知环境时所构建的地图的信息熵，短期目标是最大化互信息，互信息高的位置相当于环境的未被探索的区域，利用贝叶斯优化的方法来确定互信息极值点位置，从而确定移动机器人在未知环境中的每一个运动目标位置，让机器人自主完成定位与建图任务。

本发明还包括：

1.步骤1中采用不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型具体为：

步骤1.1：建立观测模型：将雷达传感器本身的噪声、随机测量产生的误差和检测失败造成的误差观测分布进行不同权重的加权和建立一个近似高斯函数的观测模型：

其中雷达传感器本身的噪声服从高斯分布，随机测量产生的误差服从指数分布，

表示高斯分布，指数分布概率密度函数以及检测失败的概率密度，α,β,χ分别是它们所对应的权重，由于传感器噪声占主要部分，所以权重α最大，通过实验不断调整α,β,χ的大小关系来构建一个适用于实际场景的模型；

步骤1.2：建立似然域模型：将步骤1.1得到的近似高斯函数作用于所有空间点障碍物数据，首先将参考点云所占的空间划分成网格或体素并计算每个网格的均值和方差；接着由里程计给出初始化的位姿参数x_t＝(x y θ)^T；然后根据传感器所测得的数据点变换到已知的地图上的变换关系观测数据转换到参考点云图中，转换关系为：

其中x_t＝(x y θ)^T表示机器人的位姿，(x_k'y_k')^T为传感器局部坐标位置，θ_k'表示传感器光束相对于机器人航向角的偏角。

本发明的有益效果：

(1)本发明和机器人自主移动算法相结合，能够实现机器人对未知环境的自主探索，在环境中实现自动地定位与建图

(2)本发明通过考虑得到的激光数据的各种不同权重噪声的影响，构建一个似然场模型来实现帧间之间的匹配，提高了匹配的精准度，提高了定位与建图的准确性

(3)本发明利用分支定界方法对匹配进行加速处理，在保证准确性良好的情况下，提升了算法的实时性，适用于解决较大场景的SLAM问题。

附图说明

图1是分支定界法应用于搜索算法中的流程图。

图2是本发明一种基于自主移动机器人的大场景SLAM方法框图。

图3是观测数据建立栅格化地图示意图。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

结合图2，本发明的一种大场景下自主移动机器人SLAM方法，主要包括似然域模型模块、搜索匹配模块、位姿优化模块以及机器人对环境自主探索模块：

(1)似然域模型模块首先考虑到雷达传感器测量空间环境会有以下几种噪声的影响：(a)由雷达传感器本身的噪声所带来的，它主要服从高斯分布；(b)随机测量产生的误差，主要服从指数分布；(c)检测失败造成的误差。三种误差中(a)是占的比重最大，所以将三种误差的观测分布进行不同权重的加权和建立一个近似高斯函数的观测模型，然后将观测得到点云进行近似的高斯模糊化，然后激光雷达扫描到的数据点经过坐标变换映射到地图坐标系下，从而得到一定的匹配得分。

(2)搜索匹配模块通过观测数据建立好的栅格化地图和当前观测值来估计机器人的位姿。首先在参考帧时刻的位姿x_t-1的一定范围内设定一个搜索框，比如以x_t-1为中心的10×10(或者更大)的矩形区域内；然后通过分枝定界法在这个区域内进行加速筛选，生成一个候选子集；最后在这个候选子集上计算出每个位姿(位置和旋转)上对应于观测数据集合中所有坐标的概率和，寻找一个得分和最高的，就是我们所需要求的最佳位姿。

(2.1)分枝定界主要分为两部分，一是分枝，二是定界。分枝的含义就是将整个总问题(相当于根节点)不断分解成越来越小的子问题，这些子问题相当于树中的子节点，而子节点又分解子节点，直到叶子节点，每一个叶子节点就代表单个的解。分枝的过程也就可以理解为一个将大的解空间分解成一个个小的解集的过程。而定界就是在分枝的过程中在其所有子空间解集内找到一个最大值或最小值作为其父节点的值，也叫做这些解集的目标上界或下界。对于本专利的位姿搜索问题而言，首先将当前的最优解设置为z＝-∞(假设求解问题解为最大值)；其次从还没有分枝的节点中选择一个节点，进行分枝；然后计算出每一个新分出来的节点的上限；最后对每一个节点进行是否需要分枝的判断，若节点的上界大于等于z那么就将此值赋予z，否则将这个节点减掉不予考虑，返回第二步，直到左右节点遍历完为止。具体流程图如图1所示。

(3)位姿优化模块，通过构建位姿以及路标点之间的图模型，利用它们之间的约束关系，采用非线性优化的方式将估计位姿与实际观测所得到的位姿构造一个误差函数，最小化误差函数以进一步求得精确位姿，此外在此基础上加入回路闭合检测算法，检测到回环后，将历史时刻机器人位姿进行进一步调整。

(4)将得到的机器人位姿和周围环境信息传入到自主探索模块，采用一种基于信息论的方法，该方法的长期目标是减少机器人在探索未知环境时所构建的地图的信息熵，短期目标是最大化互信息(互信息高的位置相当于环境的未被探索的区域)，利用贝叶斯优化的方法来确定互信息极值点位置，从而确定移动机器人在未知环境中的每一个运动目标位置。驱动硬件模块探索完所有环境的地图信息，并绘制出栅格地图。

本发明具体实施方式还包括：

本发明涉及到自主探索和同步定位与地图构建等机器人技术。本发明采用激光雷达以及机器人里程计作为传感器，首先考虑机器人在空间平面运行所面临各种噪声的影响，由不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型；然后采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，结合位姿优化模块，得到一个精准的位姿和地图；最后将得到的位姿和地图信息传送到基于信息论方法的自主探索算法中，让机器人自主完成定位与建图任务。本发明相对其他方法计算量均很低，而且同时适用于结构化与非结构化环境当中，具有良好的鲁棒性，适用于更加复杂、较大的环境场合，并且能够实现机器人在位置环境下自主的大范围建图与定位。

本发明包括如下步骤：

步骤1：考虑机器人在空间平面运行所面临各种噪声的影响，由不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型；

步骤1.1：建立观测模型：考虑到雷达传感器测量空间环境会有以下几种噪声的影响：(1)

由雷达传感器本身的噪声所带来的，它主要服从高斯分布；(2)随机测量产生的误差，主要服从指数分布；(3)检测失败造成的误差。将三种误差的观测分布进行不同权重的加权和建立一个近似高斯函数的观测模型。

其中

表示高斯分布，指数分布概率密度函数以及检测失败的概率密度，α,β,χ分别是它们所对应的权重，由于传感器噪声占主要部分，所以权重α最大，通过实验不断调整α,β,χ的大小关系来构建一个适用于实际场景的模型。

步骤1.2：建立似然域模型：将步骤1.1得到的近似高斯函数作用于所有空间点障碍物数据，首先将参考点云所占的空间划分成一定大小的网格或体素并计算每个网格的均值和方差；接着由里程计给出初始化的位姿参数x_t＝(x y θ)^T；然后根据传感器所测得的数据点变换到已知的地图上的变换关系观测数据转换到参考点云图中，转换关系如下式所示：

其中x_t＝(x y θ)^T表示机器人的位姿，(x_k'y_k')^T为传感器局部坐标位置，θ_k'表示传感器光束相对于机器人航向角的偏角。

步骤2、首先对步骤1得到的观测数据建立栅格化地图，由于一帧雷达观测包含很多数据点，而这些数据之间是没有关联的，即它们是完全独立的，所以状态估计方程又可以写为：

考虑到实际计算的复杂性，这里我们将等式右边取个对数，则：

这样就可以将连乘运算转换成加法运算了。通过式(4)就可以将整帧观测数据的概率转换成当前帧数据每一个点的概率。接着，在计算这个概率之前，需要对观测数据建立栅格化地图。我们将环境划分为n×n数量的栅格，以空闲和占据表示栅格的两种状态，空闲表示栅格所表示的空间没有障碍物，用白色来填充，占据表示栅格所代表的空间有障碍物，用黑色来填充。如图3所示，灰色部分是还没有探测到的未知区域。最后采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，初步得到机器人的位姿信息与环境地图信息。

步骤3、由步骤2得到的初步机器人位姿信息以及环境地图信息，通过构建位姿以及路标点之间的图模型，利用它们之间的约束关系，采用非线性优化的方式将估计位姿与实际观测所得到的位姿构造一个误差函数，最小化误差函数以进一步求得精确位姿。

步骤4、将得到的精确的位姿和地图信息，传送到基于信息论方法的自主探索算法中，该算法的长期目标是减少机器人在探索未知环境时所构建的地图的信息熵，短期目标是最大化互信息(互信息高的位置相当于环境的未被探索的区域)，利用贝叶斯优化的方法来确定互信息极值点位置，从而确定移动机器人在未知环境中的每一个运动目标位置，让机器人自主完成定位与建图任务。

其中，对得到的观测数据建立栅格化地图，采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，初步得到机器人位姿以及环境地图信息。

通过最大化互信息来实现移动机器人的自主探索。在机器人的传感器有效范围内采集周围环境信息，计算出互信息最大的位置，该位置就是移动机器人的最佳运动目标位置，让机器人自主完成定位与建图任务。

Claims (1)

1.一种大场景下自主移动机器人SLAM方法，其特征在于，包括如下步骤：

步骤1：采用不同噪声的加权和，对激光雷达观测到的空间环境数据建立一个空间似然域模型，具体为：

步骤1.1：建立观测模型：将雷达传感器本身的噪声、随机测量产生的误差和检测失败造成的误差观测分布进行不同权重的加权和建立一个近似高斯函数的观测模型：

其中雷达传感器本身的噪声服从高斯分布，随机测量产生的误差服从指数分布，

表示高斯分布，指数分布概率密度函数以及检测失败的概率密度，α,β,χ分别是它们所对应的权重，由于传感器噪声占主要部分，所以权重α最大，通过实验不断调整α,β,χ的大小关系来构建一个适用于实际场景的模型；

步骤1.2：建立似然域模型：将步骤1.1得到的近似高斯函数作用于所有空间点障碍物数据，首先将参考点云所占的空间划分成网格或体素并计算每个网格的均值和方差；接着由里程计给出初始化的位姿参数x_t＝(x y θ)^T；然后根据传感器所测得的数据点变换到已知的地图上的变换关系观测数据转换到参考点云图中，转换关系为：

其中x_t＝(x y θ)^T表示机器人的位姿，(x_k' y_k')^T为传感器局部坐标位置，θ_k'表示传感器光束相对于机器人航向角的偏角；

步骤2：首先对步骤1得到的观测数据建立栅格化地图，状态估计方程为：

将等式右边取对数，则：

将连乘运算转换成加法运算，通过上式将整帧观测数据的概率转换成当前帧数据每一个点的概率；

对观测数据建立栅格化地图：将环境划分为n×n数量的栅格，以空闲和占据表示栅格的两种状态，空闲表示栅格所表示的空间没有障碍物，用白色来填充，占据表示栅格所代表的空间有障碍物，用黑色来填充，还没有探测到的未知区域为灰色，最后采用多级分辨率地图的方法进行最优位姿的搜索以及采用分枝定界法对搜索算法进行加速优化，初步得到机器人的位姿信息与环境地图信息；

步骤3：由步骤2得到的初步机器人位姿信息以及环境地图信息，通过构建位姿以及路标点之间的图模型，利用它们之间的约束关系，采用非线性优化的方式将估计位姿与实际观测所得到的位姿构造一个误差函数，最小化误差函数以进一步求得精确位姿；

步骤4：将得到的精确的位姿和地图信息，传送到基于信息论方法的自主探索算法中，该算法的长期目标是减少机器人在探索未知环境时所构建的地图的信息熵，短期目标是最大化互信息，互信息高的位置相当于环境的未被探索的区域，利用贝叶斯优化的方法来确定互信息极值点位置，从而确定移动机器人在未知环境中的每一个运动目标位置，让机器人自主完成定位与建图任务。

Images