CN106056643A - 一种基于点云的室内动态场景slam方法及系统 - Google Patents

Abstract

本发明提出一种基于点云的室内动态场景SLAM方法，包括进行基于多尺度的激光帧和概率图配准，结合当前帧的信息对每一层历史概率图进行更新，包括根据配准结果，将当前帧转换至历史概率图所在的坐标系，得到转换后的激光帧，进行概率图生成，生成对应的概率图；结合当前帧概率图和历史概率图，对历史概率图进行更新，得到当前历史概率图；直至所有激光帧处理完毕，得到激光器的运行轨迹和室内动态环境的二维地图。本技术方案能够快速准确的对动态场景进行构图，并完成自定位。

Description

一种基于点云的室内动态场景SLAM方法及系统

技术领域

本发明属于机器人SLAM技术领域，特别是涉及一种基于点云的室内动态场景SLAM方法。

背景技术

对机器人而言，在其做出下一个决定前，必须知道其自身所在的位置。在户外场景，全球定位系统(GPS)可以为机器人提供精确的定位和导航服务。然而在室内场景，GPS信号很微弱，机器人难以接收。因此，在室内场景，机器人需要进行自定位。这一问题在机器人领域中被定义为即时定位与地图构建(SLAM)，是实现真正全自主移动机器人的关键所在。

SLAM问题，作为机器人领域中最为经典和基础的问题，根据所使用的传感器的不同，其主要解决方法主要有三种：基于RGBD相机的SLAM，基于视觉的SLAM以及基于激光点云的SLAM。

基于RGBD相机的方法，如Kinect，该方法适用于较小的场景，可以高效准确的构建地图和进行实时定位。但是由于其传感器的有效范围有限，因此，并不适用于较大的场景。

基于视觉的方法其成本较低，仅仅需要一个普通相机或者立体相机即可。但是当场景中纹理较为缺乏时，该方法很容易失败。

基于激光点云的方法，不存在上述两种方法所存在的缺陷，该方法以其优异的稳定性和有效性，被广泛的应用于无人车，无人机等智能机器人上。

考虑到基于激光点云方法的优异性，本发明拟采用基于激光点云的方法解决室内动态场景的定位和构图问题。

发明内容

为了稳定准确的对室内陌生的动态场景进行构图，并进行自定位，本发明提供了一种基于激光点云的动态场景下进行SLAM的技术方案。

本发明所采用的技术方案包括一种基于点云的室内动态场景SLAM方法，包括以下步骤：

步骤1，首先判断是否t＝0，表示当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，进入步骤2；所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

步骤2，基于多尺度的激光帧和概率图配准，实现如下，

设待配准的当前帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数，最顶层初始转换矩阵为采用上一帧最顶层的转换矩阵，

将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵

能量函数如下所示，

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图；

步骤3，概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；

对每一层的概率图进行更新，实现方式如下，

设当前层数为s，令根据步骤2的配准结果，将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对进行概率图生成，生成对应的概率图M_t；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图

步骤4，返回步骤1，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和室内动态环境的二维地图。

而且，步骤2的实现方式包括如下步骤，

步骤2.1，点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图直方图个数为B；

步骤2.2，权值估算，每个点的权值w_b计算公式如下所示，

其中表示第b个直方图的大小，其中b＝1,2,…B；

步骤2.3，通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵

步骤2.4，根据步骤2.3确定的转化矩阵获得LM算法初始值；

步骤2.5，采用LM算法优化能量函数求解得到最优转换矩阵并估计得到激光器的速度

步骤2.6，当s不等于S时，令s＝s+1，返回步骤2.5，当s等于S时，结束迭代进入步骤3。

而且，步骤3中，结合当前帧概率图M_t和历史概率图对历史概率图进行更新，实现方式如下，

针对概率图中的每一个格网g，表示为其中和分别表示概率图中该格网的概率值，和过去所达到的最高的概率值，表示该格网在过去被观察到的次数；概率图的更新分为4种情况，分别如下所示，

如果该格网g在当前帧概率图M_t和历史概率图上，概率值和都为0，则不进行任何更新；

如果在当前帧概率图M_t上，并且则按照下述公式进行更新，

其中，w_u是概率值增长率，计算方式如下，

其中，w_l是预设的学习率，τ是控制参数；

如果在当前帧概率图M_t上，同时，其中n_th为预设的阈值，则按照下述公式进行更新，

其中，w_d表示概率下降速率，其计算方式如下，

针对剩下的未处理的格网，采用内插的方式进行更新，包括用和表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值p_u和p_d，计算公式如下所示：

如果|p_u|＞|p_d|，则p＝p_u，反之，p＝p_d，通过下述公式对格网概率值进行更新：

其中，d(g,g_k)表示格网g和格网g_k之间的距离，其中g_k代表或者中的一个格网，δ_l表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。

本发明提供一种基于点云的室内动态场景SLAM系统，包括以下模块：

第一模块，用于首先判断是否t＝0，表示当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，命令第二模块工作；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

第二模块，用于基于多尺度的激光帧和概率图配准，实现如下，

设待配准的当前帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数，最顶层初始转换矩阵为采用上一帧最顶层的转换矩阵，

将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵

能量函数如下所示，

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图；

第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；

对每一层的概率图进行更新，实现方式如下，

设当前层数为s，令根据第二模块的配准结果，将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对进行概率图生成，生成对应的概率图M_t；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图

第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和室内动态环境的二维地图。

而且，第二模块的实现方式包括如下子模块，

第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图直方图个数为B；

第二子模块，用于权值估算，每个点的权值w_b计算公式如下所示，

其中表示第b个直方图的大小，其中b＝1,2,…B；

第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵

第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值；

第五子模块，用于采用LM算法优化能量函数求解得到最优转换矩阵并估计得到激光器的速度

第六子模块，用于当s不等于S时，令s＝s+1，命令第五子模块工作，当s等于S时，结束迭代命令第三模块工作。

而且，第三模块中，结合当前帧概率图M_t和历史概率图对历史概率图进行更新，实现方式如下，

针对概率图中的每一个格网g，表示为其中和分别表示概率图中该格网的概率值，和过去所达到的最高的概率值，表示该格网在过去被观察到的次数；概率图的更新分为4种情况，分别如下所示，

如果该格网g在当前帧概率图M_t和历史概率图上，概率值和都为0，则不进行任何更新；

如果在当前帧概率图M_t上，并且则按照下述公式进行更新，

其中，w_u是概率值增长率，计算方式如下，

其中，w_l是预设的学习率，τ是控制参数；

如果在当前帧概率图M_t上，同时，其中n_th为预设的阈值，则按照下述公式进行更新，

其中，w_d表示概率下降速率，其计算方式如下，

针对剩下的未处理的格网，采用内插的方式进行更新，包括用和表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值p_u和p_d，计算公式如下所示：

如果|p_u|＞|p_d|，则p＝p_u，反之，p＝p_d，通过下述公式对格网概率值进行更新：

其中，d(g,g_k)表示格网g和格网g_k之间的距离，其中g_k代表或者中的一个格网，δ_l表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。

相对于现有技术，本发明的有益效果是：本技术方案能够快速准确的对动态场景进行构图，并完成自定位。在概率图生成过程中，本技术方案对传统的轮廓模型进行了改进，使其能够更好的对场景进行表达。在概率图配准过程中，本技术方案结合了常用的穷举法和梯度下降法解求旋转平移矩阵，即利用穷举法选择在较大的窗口中选择较为准确的初始值，再采用梯度下降法求得精确值，在保证结果的准确性的基础上尽可能的避免落入局部最小。此外，还采用了由粗到精的配准策略，现在最顶层(分辨率最低)的概率图上进行配准，将结果传递至下一层，直至底层，多尺度的配准方法很好的提高了配准效率和精度。为了补偿由于机器人速度所带来的激光点云的变形，在配准过程中进行了速度补偿。在概率图更新过程中，本技术方案借鉴背景建模的方法，通过控制概率图中每个格网的概率值大小的变化，即静态目标所在的格网概率值增大，动态目标所在的格网概率值减小，从而减少动态目标对概率图准确性的影响。

附图说明

图1为本发明实施例的整体流程图。

图2为本发明实施例的最终概率示意图。

具体实施方式

具体实施时，本发明技术方案所提供流程可由本领域技术人员采用计算机软件技术实现自动运行。为了更好地理解本发明的技术方案，下面结合附图和实施例对本发明做进一步的详细说明。

参见附图1，本发明实施例包括以下步骤：

步骤1：首先判断是否t＝0，即当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，进入步骤2。

具体实施时，可以在流程开始首先判断当前激光扫描帧，判断是否t＝0。若当前帧为第一帧，也即t＝0，则转换矩阵为0，则生成当前帧各层概率图也即并直接设置历史概率图步骤2的配准和步骤3的历史概率图更新实际上不需执行。若当前帧不是第一帧，则正常执行步骤2将当前帧和历史概率图进行配准，然后进入步骤3进行历史概率图更新。

本发明进一步提供概率图生成方式。在传统的激光帧配准算法中，其所采用的策略多为两两配准。配准过程中不可避免的会存在误差，而在传统的两两匹配中，误差会进行积累，最终，导致“漂移”现象的产生。本发明为了避免误差的累积，将当前帧和历史所有帧进行配准，而存储所有帧的数据，其所消耗的计算机内存巨大。因此，本发明，拟采用概率图对历史帧进行存储。

实施例所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小，具体包括以下子步骤：

步骤1.1：激光帧轮廓提取。在室内场景中，其主要的场景可认为由多个面组成，在二维激光扫描帧中，则可以使用轮廓信息对室内场景进行更加精确的表达。首先，需要从激光帧中精确的提取出轮廓信息。实施例具体包括以下子步骤：

步骤1.1.1：相邻点间距离计算。针对激光帧中的所有相邻点对(x_k-1,x_k)，计算所有点对之间的欧式距离，记为其中K为激光帧中点的个数。

步骤1.1.2：种子点对选择。当点对(x_k-1,x_k)满足如下两个条件时，则认为这是一个种子点对：第一，d_k为最小的距离且小于预定的距离阈值d_th(具体实施时本领域技术人员可预设取值，本实施例为150毫米)；第二，点x_k-1和x_k不属于任何一条轮廓。

步骤1.1.3：轮廓增长。针对当前种子点对，根据其邻域关系，得到其邻域点{x_i}，若满足下述增长条件，则进行增长，否则，不增长。增长条件如下所示：

$\left\{\begin{array}{c}{d}_i\≤d_{th}\\ \frac{max(d_i,d_j)}{min(d_i,d_j)}\≤r_{th}\end{array}\right.$

其中，d_i表示点x_i和点x_i-1之间的欧式距离，x_j表示当前轮廓中和x_i相邻的其中一个端点，d_j表示x_j和点x_j-1之间的欧式距离，x_i是x_j的下一个点或者上一个点，d_th和r_th分别是距离阈值和比例阈值(具体实施时本领域技术人员可预设取值，本实施例中d_th＝150毫米r_th＝2)。迭代的进行增长，即当前点增长完之后，再以增长后的点作为起点，再进行增长，直至不能再增长，则当前轮廓提取完成。

步骤1.1.4：对下一种子点对，重复步骤1.1.2和步骤1.1.3，直至所有的轮廓提取完成。

步骤1.1.5：当轮廓线长度小于L_th时(具体实施时本领域技术人员可预设取值，本实施例设为5)，考虑其为噪声，并将其去除，最终得到所有的有效的轮廓，记为

步骤1.2：格网概率值内插。根据所获得的轮廓模型，结合二维高斯模型，针对不同分辨率的概率图，计算出其每个格网的概率值大小，得到多尺度的概率图。

设置不同格网大小，可以分别生成不同分辨率下的概率图。具体实施时本领域技术人员可预设多种不同的金字塔分辨率，实现多尺度格网大小设置。本实施例中，生成三层不同分辨率的概率图，其格网大小从大到小在本实施实例中分别设为40，20，10毫米，因此可以在每一层生成得到总共三层概率图。

实施例中，对每种格网大小，分别具体执行包括以下子步骤：

步骤1.2.1：根据当前的格网大小，确定x轴和y轴，针对同一条轮廓线的一对点(x_i,x_j)，设定x轴为x_i到x_j的直线方向，垂直于x轴的方向为y轴。

步骤1.2.2：计算格网概率值。其计算公式如下所示：

其中，e为数学常量，x为当前格网中心点，代表其概率值，P_max为最大的概率值(具体实施时，本领域技术人员可自行设定取值，本实施例设为0.5)，δ_x和δ_y分别代表沿x轴和y轴的高斯核大小(具体实施时，本领域技术人员可自行设定取值，本实施例分别设置为1.5和10)。和表示当前格网沿x轴和y轴到轮廓的距离，计算公式如下所示：

其中，d_x(x,x_k)表示x沿x轴到轮廓上的点x_k的距离，表示x到轮廓的距离。

步骤2：基于多尺度的激光帧和概率图配准。假设当前待配准帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数(本实施例中为3)，最顶层初始转换矩阵为即为上一帧最顶层的转换矩阵。

为了将当前帧配准至历史概率图上，本发明采用了一种由粗到精的配准方式，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵转换矩阵主要由三个参数进行控制，分别是旋转角θ，x轴方向的偏移量d_x，y轴方向的偏移量d_y。能量函数如下所示：

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图。

实施例具体包括以下子步骤：步骤2.1：点云法向量直方图统计。为了计算每个点的权值大小，即计算其对能量函数的贡献。首先，需要统计激光帧中所有点的法向量分布直方图直方图个数为B(本实施例中B＝12)。为了排除诸如噪声等外点对法向量估算的影响，本发明采用Robust Diagnostic PCA(RDPCA)算法鲁棒的计算每个点的法向量。

步骤2.2：权值估算。每个点的权值w_b计算公式如下所示：

其中表示第b个直方图的大小，也即落在该直方条内的点的个数，其中b＝1,2,…B。

通过步骤2.1和步骤2.2，可以求得当前帧每一个点的权值，该权值可以应用于每一层的概率图配准中。为了防止解算得到的配准结果为局部最优，采用了由粗到精的配准方式，因此，在实施例中，对每一层概率图的配准，迭代的执行以下步骤，直至最底层。

步骤2.3：判断当前层是否为最顶层，也即s是否等于1。若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵，也即为若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵，也即

步骤2.4：根据步骤2.3确定的转化矩阵采用穷举法获得LM算法初始值：

由于梯度下降法较易于落入局部最优，因此，在采取梯度下降法优化配准能量函数前，先采用穷举法搜索到一个较好的初始值，提高梯度下降法的效率和稳定性。根据步骤2.3所得转换矩阵的三个参数(θ,d_x,d_y)，建议设定一个较窄的搜索窗口(θ±w_θ,d_x±w_x,d_y±w_y)(具体实施时本领域技术人员可预设取值，本实施例中旋转平移矩阵中的三个参数其搜索窗口大小分别设置如下：w_θ＝5°，w_x＝10mm，w_y＝10mm)，再设定一个较大的搜索歩距(s_θ,s_x,s_y)(具体实施时本领域技术人员可预设取值，本实施例中s_θ＝0.5°，s_x＝1，s_y＝1)。通过在这个窗口内搜索，找到一个最好的初始转换矩阵T′_t，作为梯度下降法的初始值。在一个较窄的搜索空间中，以一个较大的搜索歩距进行搜索，因此可以减少搜索的次数，从而提高算法的效率，减少初始值搜索的时间。

步骤2.5：梯度下降法精确计算转换矩阵并进行速度估计和补偿。为了获得更加精确的转换矩阵，并对速度进行估计和补偿，本发明采用Levenberg-Marquardt(LM)算法(梯度下降算法的一种)优化上述能量函数求解得到最优转换矩阵并估计得到激光器的速度实施例具体包括以下子步骤：

步骤2.5.1：速度估算。为了补偿由于激光器速度所造成的激光帧畸变，需要在配准的过程中对激光器的速度进行估算和补偿。当前激光器速度估算公式如下所示：

$V_t^s=\frac{1}{\mathrm{\Δ}t}log{T}_{t-1}^{-1}{T_{t-1}^s}^{-1}{T}_t^s$

其中，Δt为激光器扫描一帧所需要的时间(本实施例激光器频率为50Hz，所以秒)，表示上一帧的转换矩阵的逆。

步骤2.5.2：速度补偿。通过估算的速度，在进行一次LM优化迭代之前，按照下述公式对激光帧S_t进行速度补偿：

${\hat{x}}_k^t=e^{{\mathrm{k\Δt}}_s{V}_t^s}{x}_k^t$

其中，代表未补偿的激光帧S_t中的一点，代表补偿后的激光帧中的一点，其中k＝1,2,3…K，其中K为一个激光扫描帧中的点的个数(本实施例中K＝540)，而表示激光器扫描一个点所需要的时间。

步骤2.5.3：LM优化。针对补偿后的激光帧和历史概率图采用LM算法优化其配准能量函数，一次迭代之后，得到新的转换矩阵且迭代次数加1。

步骤2.5.4：根据新得到的转换矩阵根据步骤2.5.1对速度进行更新，并用新的速度根据步骤2.5.2对速度进行补偿，再进行步骤2.5.3。即返回重复步骤2.5.1-2.5.3，直到满足迭代结束条件，例如LM算法收敛，或迭代次数大于预设的最大迭代次数I_max(具体实施时，本领域技术人员可自行预设取值，本实施例设为1000次)，进入步骤2.6。

步骤2.6：为实现基于多尺度的概率图配准，当s不等于S时，令s＝s+1，返回步骤2.5，当s等于S时，结束迭代进入步骤3。迭代步骤2.3到步骤2.5，也即完成每一层的配准。将当前层的配准结果，也即当前层得到的旋转平移矩阵的三个参数(θ,d_x,d_y)传递到下一层，作为下一层的初始值，直至传递至最底层，从而完成由粗到精的配准，结束迭代进入步骤3。

步骤3：概率图更新。当前激光扫描帧和历史概率图配准后，需要结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中。

对每一层的概率图更新，实施例具体包括以下子步骤：

步骤3.1：层数设置。在概率图更新中，需要对每一层的概率图进行更新，每一层的更新策略完全一致。假设当前层数为s，在步骤3.1和步骤3.2中，

步骤3.2：当前帧概率图生成。通过步骤2，可以将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对采用如步骤1中的概率图生成方法生成对应的概率图M_t。

步骤3.3：概率图更新。结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图针对概率图中的每一个格网g，本发明实施例将其表示为 其中和分别表示概率图中该格网的概率值，和过去所达到的最高的概率值，表示该格网在过去被观察到的次数。概率图的更新主要分为4种情况，分别如下所示：

1.不更新。如果该格网在当前帧概率图M_t和历史概率图上，其概率值和都为0，也即则不进行任何更新。

2.概率值增加。如果在当前帧概率图M_t上，有点落在格网g中，也即并且则在历史概率图上，增加该格网的概率值，并按照下述公式进行更新：

其中w_u是概率值增长率，计算方式如下：

其中，w_l是预设的学习率，τ是控制参数(具体实施时本领域技术人员可预设取值，本实施例中w_l＝0.01，τ＝3)。

3.概率值下降。如果在当前帧概率图M_t上，不存在点落在格网g中，也即同时，在以往帧中，不存在足够的点落于格网g中，也即其中n_th为预设的阈值(具体实施时本领域技术人员可预设取值，本实施例中设为30)，则在历史概率图上，降低该格网的概率值，计算方式如下所示：

其中，w_d表示概率下降速率，其计算方式如下：

4.概率值增长和下降值内插。针对剩下的未处理的格网，采用内插的方法进行更新。用和表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值p_u和p_d，计算公式如下所示：

其中，d(g,g_k)表示格网g和格网g_k之间的距离，其中g_k代表或者中的一个格网，δ_l表示高斯函数的核大小(具体实施时，本领域技术人员可自行设定，本实施例中设为1.5)。用p代表该格网增长或下降的概率值大小，如果|p_u|＞|p_d|，则p＝p_u，反之，p＝p_d，通过下述公式对格网概率值进行更新：

根据步骤3.1到步骤3.3，可以对单尺度的概率图进行更新。本发明在步骤2中采用的是一种多尺度的概率图配准方法，因此，需要按照步骤3.1和步骤3.3对多尺度的概率图分别进行更新，可初始化s＝1，执行完毕后令s＝s+1，再次执行，直到s＝S。

步骤4：返回步骤1，可以对后续各帧迭代步骤2到步骤3，直至所有激光帧处理完毕，则终止迭代，并得到激光器的运行轨迹和室内动态环境的二维地图。参见图2，走廊场景最终概率图。走廊中间的环线为机器车运行轨迹，其余区域，灰度值越大(影像越白)，概率值越大，反之，概率值越小。

具体实施时，本发明所提供方法可基于软件技术实现自动运行流程，也可采用模块化方式实现相应系统。本发明实施例提供一种基于点云的室内动态场景SLAM系统，包括以下模块：

第一模块，用于首先判断是否t＝0，表示当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，命令第二模块工作；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

第二模块，用于基于多尺度的激光帧和概率图配准，实现如下，

设待配准的当前帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数，最顶层初始转换矩阵为采用上一帧最顶层的转换矩阵，

将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵

能量函数如下所示，

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图；

第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；

对每一层的概率图进行更新，实现方式如下，

设当前层数为s，令根据第二模块的配准结果，将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对进行概率图生成，生成对应的概率图M_t；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图

第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和室内动态环境的二维地图。

进一步地，第二模块的实现方式包括如下子模块，

第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图直方图个数为B；

第二子模块，用于权值估算，每个点的权值w_b计算公式如下所示，

其中表示第b个直方图的大小，其中b＝1,2,…B；

第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵

第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值；

第五子模块，用于采用LM算法优化能量函数求解得到最优转换矩阵并估计得到激光器的速度

第六子模块，用于当s不等于S时，令s＝s+1，命令第五子模块工作，当s等于S时，结束迭代命令第三模块工作。

各模块具体实现可参见相应步骤，本发明不予赘述。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的。因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (6)

1.一种基于点云的室内动态场景SLAM方法，其特征在于，包括以下步骤：

步骤1，首先判断是否t＝0，表示当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，进入步骤2；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

步骤2，基于多尺度的激光帧和概率图配准，实现如下，

设待配准的当前帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数，最顶层初始转换矩阵为采用上一帧最顶层的转换矩阵，

将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵

能量函数如下所示，

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图；

步骤3，概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；

对每一层的概率图进行更新，实现方式如下，

设当前层数为s，令根据步骤2的配准结果，将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对进行概率图生成，生成对应的概率图M_t；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图

步骤4，返回步骤1，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和室内动态环境的二维地图。

2.根据权利要求1所述基于点云的室内动态场景SLAM方法，其特征在于：步骤2的实现方式包括如下步骤，

步骤2.1，点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图直方图个数为B；

步骤2.2，权值估算，每个点的权值w_b计算公式如下所示，

其中表示第b个直方图的大小，其中b＝1,2,…B；

步骤2.3，通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵

步骤2.4，根据步骤2.3确定的转化矩阵获得LM算法初始值；

步骤2.5，采用LM算法优化能量函数求解得到最优转换矩阵并估计得到激光器的速度

步骤2.6，当s不等于S时，令s＝s+1，返回步骤2.5，当s等于S时，结束迭代进入步骤3。

3.根据权利要求2所述基于点云的室内动态场景SLAM方法，其特征在于：步骤3中，结合当前帧概率图M_t和历史概率图对历史概率图进行更新，实现方式如下，

针对概率图中的每一个格网g，表示为其中 和分别表示概率图中该格网的概率值，和过去所达到的最高的概率值，表示该格网在过去被观察到的次数；概率图的更新分为4种情况，分别如下所示，

如果该格网g在当前帧概率图M_t和历史概率图上，概率值和都为0，则不进行任何更新；

如果在当前帧概率图M_t上，并且则按照下述公式进行更新，

其中，w_u是概率值增长率，计算方式如下，

其中，w_l是预设的学习率，τ是控制参数；

如果在当前帧概率图M_t上，同时，其中n_th为预设的阈值，则按照下述公式进行更新，

其中，w_d表示概率下降速率，其计算方式如下，

针对剩下的未处理的格网，采用内插的方式进行更新，包括用和表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值p_u和p_d，计算公式如下所示：

如果|p_u|＞|p_d|，则p＝p_u，反之，p＝p_d，通过下述公式对格网概率值进行更新：

其中，d(g,g_k)表示格网g和格网g_k之间的距离，其中g_k代表或者中的一个格网，δ_l表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。

4.一种基于点云的室内动态场景SLAM系统，其特征在于，包括以下模块：

第一模块，用于首先判断是否t＝0，表示当前激光扫描帧是否为第一帧，是则生成当前帧的概率图并直接设置历史概率图然后等待下一帧，当t不为0时，命令第二模块工作；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

第二模块，用于基于多尺度的激光帧和概率图配准，实现如下，

设待配准的当前帧为S_t，各层历史概率图分别为其中s＝1,2…S，s为概率图当前层数，S为总层数，最顶层初始转换矩阵为采用上一帧最顶层的转换矩阵，

将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵

能量函数如下所示，

其中，x表示激光帧S_t中的一个点，表示x经过转换后的点，表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史概率图；

第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；

对每一层的概率图进行更新，实现方式如下，

设当前层数为s，令根据第二模块的配准结果，将当前帧S_t转换至历史概率图所在的坐标系，得到转换后的激光帧针对进行概率图生成，生成对应的概率图M_t；

所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；

结合当前帧概率图M_t和历史概率图对历史概率图进行更新，得到当前历史概率图

第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和室内动态环境的二维地图。

5.根据权利要求4所述基于点云的室内动态场景SLAM系统，其特征在于：第二模块的实现方式包括如下子模块，

第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图直方图个数为B；

第二子模块，用于权值估算，每个点的权值w_b计算公式如下所示，

其中表示第b个直方图的大小，其中b＝1,2,…B；

第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层传递下来的转换矩阵

第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值；

第五子模块，用于采用LM算法优化能量函数求解得到最优转换矩阵并估计得到激光器的速度

第六子模块，用于当s不等于S时，令s＝s+1，命令第五子模块工作，当s等于S时，结束迭代命令第三模块工作。

6.根据权利要求5所述基于点云的室内动态场景SLAM系统，其特征在于：第三模块中，结合当前帧概率图M_t和历史概率图对历史概率图进行更新，实现方式如下，

针对概率图中的每一个格网g，表示为其中 和分别表示概率图中该格网的概率值，和过去所达到的最高的概率值，表示该格网在过去被观察到的次数；概率图的更新分为4种情况，分别如下所示，

如果该格网g在当前帧概率图M_t和历史概率图上，概率值和都为0，则不进行任何更新；

如果在当前帧概率图M_t上，并且则按照下述公式进行更新，

其中，w_u是概率值增长率，计算方式如下，

其中，w_l是预设的学习率，τ是控制参数；

如果在当前帧概率图M_t上，同时，其中n_th为预设的阈值，则按照下述公式进行更新，

其中，w_d表示概率下降速率，其计算方式如下，

针对剩下的未处理的格网，采用内插的方式进行更新，包括用和表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值p_u和p_d，计算公式如下所示：

如果|p_u|＞|p_d|，则p＝p_u，反之，p＝p_d，通过下述公式对格网概率值进行更新：

其中，d(g,g_k)表示格网g和格网g_k之间的距离，其中g_k代表或者中的一个格网，δ_l表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。