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

一种基于点云的室内动态场景slam方法及系统的制作方法
【专利摘要】本发明提出一种基于点云的室内动态场景SLAM方法，包括进行基于多尺度的激光帧和概率图配准，结合当前帧的信息对每一层历史概率图进行更新，包括根据配准结果，将当前帧转换至历史概率图所在的坐标系，得到转换后的激光帧，进行概率图生成，生成对应的概率图；结合当前帧概率图和历史概率图，对历史概率图进行更新，得到当前历史概率图；直至所有激光帧处理完毕，得到激光器的运行轨迹和室内动态环境的二维地图。本技术方案能够快速准确的对动态场景进行构图，并完成自定位。
【专利说明】
一种基于点云的室内动态场景SLAM方法及系统
技术领域
[0001] 本发明属于机器人SLAM技术领域，特别是涉及一种基于点云的室内动态场景SLAM 方法。
【背景技术】
[0002] 对机器人而言，在其做出下一个决定前，必须知道其自身所在的位置。在户外场 景，全球定位系统(GPS)可以为机器人提供精确的定位和导航服务。然而在室内场景，GPS信 号很微弱，机器人难以接收。因此，在室内场景，机器人需要进行自定位。这一问题在机器人 领域中被定义为即时定位与地图构建(SLAM)，是实现真正全自主移动机器人的关键所在。
[0003] SLAM问题，作为机器人领域中最为经典和基础的问题，根据所使用的传感器的不 同，其主要解决方法主要有三种:基于RGBD相机的SLAM，基于视觉的SLAM以及基于激光点云 的SLAM 〇
[0004] 基于RGBD相机的方法，如Kinect，该方法适用于较小的场景，可以高效准确的构建 地图和进行实时定位。但是由于其传感器的有效范围有限，因此，并不适用于较大的场景。
[0005] 基于视觉的方法其成本较低，仅仅需要一个普通相机或者立体相机即可。但是当 场景中纹理较为缺乏时，该方法很容易失败。
[0006] 基于激光点云的方法，不存在上述两种方法所存在的缺陷，该方法以其优异的稳 定性和有效性，被广泛的应用于无人车，无人机等智能机器人上。
[0007] 考虑到基于激光点云方法的优异性，本发明拟采用基于激光点云的方法解决室内 动态场景的定位和构图问题。

【发明内容】

[0008] 为了稳定准确的对室内陌生的动态场景进行构图，并进行自定位，本发明提供了 一种基于激光点云的动态场景下进行SLAM的技术方案。
[0009] 本发明所采用的技术方案包括一种基于点云的室内动态场景SLAM方法，包括以下 步骤：
[0010] 步骤1，首先判断是否t = 0,表示当前激光扫描帧是否为第一帧，是则生成当前帧 的概率图M纟，并直接设置历史概率图Mt1 =M0S，然后等待下一帧，当t不为0时，进入步骤 2;所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模型， 针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0011] 步骤2，基于多尺度的激光帧和概率图配准，实现如下，
[0012] 设待配准的当前帧为St，各层历史概率图分别为Mb，其中S = 1，2~S，S为概率图 当前层数，S为总层数，最顶层初始转换矩阵为It1，采用上一帧最顶层的转换矩阵，
[0013] 将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能 量函数，寻找到两者间的最优转化矩阵存，
[0014] 能量函数如下所示，
[0015]
[0016] 其中，x表示激光帧S t中的一个点，T/ <g) x表示x经过尨转换后的点， P(7^? XlMlL1)表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值， ML 1为某一层的历史概率图；
[0017] 步骤3,概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历 史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；
[0018] 对每一层的概率图进行更新，实现方式如下，
[0019] 设当前层数为s，令腿η = Mh，Mlt =厕Mt = Mf;根据步骤2的配准结果，将 当前帧St转换至历史概率图M^1所在的坐标系，得到转换后的激光帧；，针对舄进行概率 图生成，生成对应的概率图Mt;
[0020] 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯 模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0021] 结合当前帧概率图Mt和历史概率图，对历史概率图进行更新，得到当前历史 概率图Mt;
[0022] 步骤4,返回步骤1，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨 迹和室内动态环境的二维地图。
[0023]而且，步骤2的实现方式包括如下步骤，
[0024]步骤2.1，点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方图 对，直方图个数为B;
[0025] 步骤2.2，权值估算，每个点的权值Wb计算公式如下所示，
[0026]
[0027] 其中^表示第b个直方图的大小，其中b = l，2，~B;
[0028] 步骤2.3,通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始 转换矩阵为上一帧传递过来的转换矩阵;若不是最顶层，则该层的初始转换矩阵为上一 层传递下来的转换矩阵
[0029] 步骤2.4，根据步骤2.3确定的转化矩阵获得LM算法初始值；
[0030] 步骤2.5,采用LM算法优化能量函数以心M^1)，求解得到最优转换矩阵7?'，并估 计得到激光器的速度Ff;
[0031 ] 步骤2 · 6，当s不等于S时，令s = s+1，返回步骤2 · 5，当s等于S时，结束迭代进入步骤 3〇
[0032] 而且，步骤3中，结合当前帧概率图Mt和历史概率图Ut-i，对历史概率图进行更新， 实现方式如下，
[0033] 针对概率图中的每一个格网g，表示为M(g) = fMI.p(g), M:气gl Mn(g)j，其中 RT(g) e: [0,1]和e [ο,ι]，分别表示概率图中该格网的概率值，和过去所达到的最高 的概率值，RT(g)表示该格网在过去被观察到的次数;概率图的更新分为4种情况，分别如 下所示，
[0034] 如果该格网g在当前帧概率图Mt和历史概率图·上，概率值Mf(g)和M^1Cg)都 为〇,则不进行任何更新；
[0035] 如果在当前帧概率图Mt上，Mf(g) > OjmM^(g) 2 1，则按照下述公式进行更新，
[0036]
[0037] 其中，^是概率值增长率，计算方式如下，
[0038]
[0039]其中，W1是预设的学习率，τ是控制参数；
[0040] 如果在当前帧概率图Mt上，蹲(g) = 〇,同时，厕Ug) S nift，其中nth为预设的阈 值，则按照下述公式进行更新，
[0041]
[0042] 其中，Wd表示概率下降速率，其计算方式如下，
[0043]
[0044] 针对剩下的未处理的格网，采用内插的方式进行更新，包括用《^和@^表示概率值 增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率 值Pu和Pd，计算公式如下所示：
[0045]
[0046] 如果|pu| > |pd|，则p=pu，反;2l，p=pd，通过十述公式对格N概率值进行更新：
[0047]
[0048] 其中，d(g，gk)表不格网g和格网gk之间的距离，其中gk代表*Sti或者:中的一个格 网，S 1表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。
[0049] 本发明提供一种基于点云的室内动态场景SLAM系统，包括以下模块：
[0050] 第一模块，用于首先判断是否t = 0,表示当前激光扫描帧是否为第一帧，是则生成 当前帧的概率图M0S，并直接设置历史概率图Mfq = M^然后等待下一帧，当t不为0时，命 令第二模块工作；
[0051] 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯 模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0052]第二模块，用于基于多尺度的激光帧和概率图配准，实现如下，
[0053] 设待配准的当前帧为St，各层历史概率图分别为Mt1，其中S = 1，2~S，S为概率图 当前层数，S为总层数，最顶层初始转换矩阵为7t:1，采用上一帧最顶层的转换矩阵，
[0054] 将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能 量函数，寻找到两者间的最优转化矩阵疗，
[0055] 能量函数如下所示，
[0056]
[0057] 其中，X表示激光帧St中的一个点，疗?^：表示X经过:rf转换后的点， Ρ0-/? χ|Μ乙J表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值， Mlf-I为某一层的历史概率图；
[0058]第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每 一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；
[0059] 对每一层的概率图进行更新，实现方式如下，
[0060] 设当前层数为S，令離= Mt =轉，Mt = MM艮据第二模块的配准结 果，将当前帧St转换至历史概率图蘭t_：t所在的坐标系，得到转换后的激光帧茹，针对S?进行 概率图生成，生成对应的概率图M t;
[0061] 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯 模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0062] 结合当前帧概率图Mt和历史概率图，对历史概率图进行更新，得到当前历史 概率图Mt;:
[0063]第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光 器的运行轨迹和室内动态环境的二维地图。
[0064]而且，第二模块的实现方式包括如下子模块，
[0065] 第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分 布直方图:H%直方图个数为B;
[0066] 第二子模块，用于权值估算，每个点的权值Wb计算公式如下所示，
[0067]
[0068] 其中A表示第b个直方图的大小，其中b = l，2，'"B;
[0069] 第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该 层的初始转换矩阵为上一帧传递过来的转换矩阵Tt 1;若不是最顶层，则该层的初始转换矩 阵为上一层传递下来的转换矩阵-1;
[0070] 第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值；
[0071] 第五子模块，用于采用LM算法优化能量函数求解得到最优转换矩阵 符，并估计得到激光器的速度F/;
[0072]第六子模块，用于当s不等于S时，令s = s+l，命令第五子模块工作，当s等于S时，结 束迭代命令第三模块工作。
[0073] 而且，第三模块中，结合当前帧概率图Mt和历史概率图Mt-P对历史概率图进行更 新，实现方式如下，
[0074] 针对概率图中的每一个格网g，表示为
，其中 M〃(g) e丨〇,1]和K#(g) e 分别表示概率图中该格网的概率值，和过去所达到的最高 的概率值，Mra(g)表示该格网在过去被观察到的次数;概率图的更新分为4种情况，分别如 下所示，
[0075] 如果该格网g在当前帧概率图Mt和历史概率图顯卜:上，概率值Mf(g)和_|_ Jg)都 为0,则不进行任何更新；
[0076] 如果在当前帧概率图Mt上，Mf (g) > O并且Mtn(g)乏：U则按照下述公式进行更新，
[0077]
[0078] 其中，^是概率值增长率，计算方式如下，
[0079]
[0080]其中，W1是预设的学习率，τ是控制参数；
[0081] 如果在当前帧概率图Mt上，Mf (g) = 0,同时，MUg) S 其中nth为预设的阈 值，则按照下述公式进行更新，
[0082]
[0083] 其中，Wd表示概率下降速率，其计算方式如下，
[0084]
[0085] 针对剩下的未处理的格网，采用内插的方式进行更新，包括用表示概率值 增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率 值Pu和Pd，计算公式如下所示：
[0086]
[0087] 如果|pu| > |pd|，贝>Jp=pu，反之，p=pd，通过下述公式对格网概率值进行更新： [0088]
[0089] 其中，d (g，gk)表不格网g和格网gk之间的距离，其中gk代表Gu.或者Gd _中的一个格 网，S1表示高斯函数的核大，用p代表该格网增长或下降的概率值大小。
[0090] 相对于现有技术，本发明的有益效果是:本技术方案能够快速准确的对动态场景 进行构图，并完成自定位。在概率图生成过程中，本技术方案对传统的轮廓模型进行了改 进，使其能够更好的对场景进行表达。在概率图配准过程中，本技术方案结合了常用的穷举 法和梯度下降法解求旋转平移矩阵，即利用穷举法选择在较大的窗口中选择较为准确的初 始值，再采用梯度下降法求得精确值，在保证结果的准确性的基础上尽可能的避免落入局 部最小。此外，还采用了由粗到精的配准策略，现在最顶层(分辨率最低)的概率图上进行配 准，将结果传递至下一层，直至底层，多尺度的配准方法很好的提高了配准效率和精度。为 了补偿由于机器人速度所带来的激光点云的变形，在配准过程中进行了速度补偿。在概率 图更新过程中，本技术方案借鉴背景建模的方法，通过控制概率图中每个格网的概率值大 小的变化，即静态目标所在的格网概率值增大，动态目标所在的格网概率值减小，从而减少 动态目标对概率图准确性的影响。
【附图说明】
[0091 ]图1为本发明实施例的整体流程图。
[0092]图2为本发明实施例的最终概率示意图。
【具体实施方式】
[0093]具体实施时，本发明技术方案所提供流程可由本领域技术人员采用计算机软件技 术实现自动运行。为了更好地理解本发明的技术方案，下面结合附图和实施例对本发明做 进一步的详细说明。
[0094]参见附图1，本发明实施例包括以下步骤：
[0095]步骤1:首先判断是否t = 0,即当前激光扫描帧是否为第一帧，是则生成当前帧的 概率图MJ，并直接设置历史概率图M^1 = Mg，然后等待下一帧，当t不为0时，进入步骤2。
[0096] 具体实施时，可以在流程开始首先判断当前激光扫描帧，判断是否t = 0。若当前帧 为第一帧，也即t = 0,则转换矩阵为0,则生成当前帧各层概率图，也即Mg，并直接设置历 史概率图Mt1 = 步骤2的配准和步骤3的历史概率图更新实际上不需执行。若当前帧不 是第一帧，则正常执行步骤2将当前帧和历史概率图进行配准，然后进入步骤3进行历史概 率图更新。
[0097] 本发明进一步提供概率图生成方式。在传统的激光帧配准算法中，其所采用的策 略多为两两配准。配准过程中不可避免的会存在误差，而在传统的两两匹配中，误差会进行 积累，最终，导致"漂移"现象的产生。本发明为了避免误差的累积，将当前帧和历史所有帧 进行配准，而存储所有帧的数据，其所消耗的计算机内存巨大。因此，本发明，拟采用概率图 对历史帧进行存储。
[0098] 实施例所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二 维高斯模型，针对不同分辨率的概率图，计算出每个格网的概率值大小，具体包括以下子步 骤：
[0099] 步骤1.1:激光帧轮廓提取。在室内场景中，其主要的场景可认为由多个面组成，在 二维激光扫描帧中，则可以使用轮廓信息对室内场景进行更加精确的表达。首先，需要从激 光帧中精确的提取出轮廓信息。实施例具体包括以下子步骤：
[0100] 步骤1. 1.1:相邻点间距离计算。针对激光帧中的所有相邻点对(HXk)，计算所 有点对之间的欧式距离，记为{cy Cift = Iixft -Xnim，其中κ为激光帧中点的个数。
[0101] 步骤1.1.2:种子点对选择。当点对(Xk-^xk)满足如下两个条件时，则认为这是一 个种子点对:第一，d k为最小的距离且小于预定的距离阈值dth(具体实施时本领域技术人员 可预设取值，本实施例为150毫米）；第二，点 Xk-dPXkF属于任何一条轮廓。
[0102] 步骤1.1.3:轮廓增长。针对当前种子点对，根据其邻域关系，得到其邻域点{Xl}， 若满足下述增长条件，则进行增长，否则，不增长。增长条件如下所示：
[0103]
[0104]其中，Cl1表示点X1和点XH之间的欧式距离，X谦示当前轮廓中和X1相邻的其中一 个端点，dj表示Xj和点Xj-i之间的欧式距离，Xi是Xj的下一个点或者上一个点，dth和rth分别是 距离阈值和比例阈值(具体实施时本领域技术人员可预设取值，本实施例中dth = 150毫米 rth=2)。迭代的进行增长，即当前点增长完之后，再以增长后的点作为起点，再进行增长，直 至不能再增长，则当前轮廓提取完成。
[0105] 步骤1.1.4:对下一种子点对，重复步骤1.1.2和步骤1.1.3,直至所有的轮廓提取 完成。
[0106] 步骤1.1.5:当轮廓线长度小于Lth时(具体实施时本领域技术人员可预设取值，本 实施例设为5)，考虑其为噪声，并将其去除，最终得到所有的有效的轮廓，记为C。
[0107] 步骤1.2:格网概率值内插。根据所获得的轮廓模型，结合二维高斯模型，针对不同 分辨率的概率图，计算出其每个格网的概率值大小，得到多尺度的概率图。
[0108] 设置不同格网大小，可以分别生成不同分辨率下的概率图。具体实施时本领域技 术人员可预设多种不同的金字塔分辨率，实现多尺度格网大小设置。本实施例中，生成三层 不同分辨率的概率图，其格网大小从大到小在本实施实例中分别设为40,20,10毫米，因此 可以在每一层生成得到总共三层概率图。
[0109] 实施例中，对每种格网大小，分别具体执行包括以下子步骤：
[0110] 步骤1.2.1:根据当前的格网大小，确定X轴和y轴，针对同一条轮廓线的一对点 (Xi,Xj)，设定X轴为Xi到Xj的直线方向，垂直于X轴的方向为y轴。
[0111] 步骤1.2.2:计算格网概率值。其计算公式如下所示：
[0112]

[0113] 其中，e为数学常量，X为当前格网中心点，代表其概率值，Pmax为最大的概率 值(具体实施时，本领域技术人员可自行设定取值，本实施例设为0.5)，δ4Ρδγ分别代表沿X 轴和y轴的高斯核大小（具体实施时，本领域技术人员可自行设定取值，本实施例分别设置 为1.5和10)。之(\([:)和40,￡)表示当前格网沿1轴和 7轴到轮廓的距离，计算公式如下所 示：
[0114]
[0115] 其中，dx(x，xk)表示X沿X轴到轮廓上的点紅的距离，表示X到轮廓的距离。
[0116] 步骤2:基于多尺度的激光帧和概率图配准。假设当前待配准帧为St，各层历史概 率图分别为厕!_:1，其中 S = 1，2~S，S为概率图当前层数，S为总层数(本实施例中为3)，最顶 层初始转换矩阵为FtL 1，即为上一帧最顶层的转换矩阵。
[0117] 为了将当前帧配准至历史概率图上，本发明采用了一种由粗到精的配准方式，在 每一层中通过结合穷举法和梯度下降法优化能量函数，寻找到两者间的最优转化矩阵疗， 转换矩阵主要由三个参数进行控制，分别是旋转角θ，χ轴方向的偏移量d x，y轴方向的偏移 量dy。能量函数如下所示：
[0118]
[0119] 其中，X表示激光帧St中的一个点，疗?X表示X经过7/转换后的点， 表示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值， 屬L1为某一层的历史概率图。
[0120] 实施例具体包括以下子步骤:步骤2.1:点云法向量直方图统计。为了计算每个点 的权值大小，即计算其对能量函数的贡献。首先，需要统计激光帧中所有点的法向量分布直 方图況'，直方图个数为B(本实施例中B = 12)。为了排除诸如噪声等外点对法向量估算的影 响，本发明采用Robust Diagnostic PCA(RDPCA)算法鲁棒的计算每个点的法向量。
[0121 ]步骤2.2:权值估算。每个点的权值Wb计算公式如下所示：
[0122]
[0123]其中私表示第b个直方图的大小，也即落在该直方条内的点的个数，其中b = l， 2, .·_Β〇
[0124]通过步骤2.1和步骤2.2，可以求得当前帧每一个点的权值，该权值可以应用于每 一层的概率图配准中。为了防止解算得到的配准结果为局部最优，采用了由粗到精的配准 方式，因此，在实施例中，对每一层概率图的配准，迭代的执行以下步骤，直至最底层。
[0125] 步骤2.3:判断当前层是否为最顶层，也即s是否等于1。若为最顶层，则该层的初始 转换矩阵为上一帧传递过来的转换矩阵，也即为7?^。若不是最顶层，则该层的初始转换矩 阵为上一层传递下来的转换矩阵，也即ΓΓ 1 〇
[0126] 步骤2.4:根据步骤2.3确定的转化矩阵采用穷举法获得LM算法初始值：
[0127] 由于梯度下降法较易于落入局部最优，因此，在采取梯度下降法优化配准能量函 数前，先采用穷举法搜索到一个较好的初始值，提高梯度下降法的效率和稳定性。根据步骤 2.3所得转换矩阵的三个参数(Θ，d x，dy)，建议设定一个较窄的搜索窗口（ Θ ± WO，dx± Wx，dy 土 wy)(具体实施时本领域技术人员可预设取值，本实施例中旋转平移矩阵中的三个参数其搜 索窗口大小分别设置如下:we = 5°，wx=10mm，wy=10mm)，再设定一个较大的搜索歩距(se, sx，S y)(具体实施时本领域技术人员可预设取值，本实施例中se = 0.5°，Sx= I，Sy = 1)。通过 在这个窗口内搜索，找到一个最好的初始转换矩阵Ivt，作为梯度下降法的初始值。在一个 较窄的搜索空间中，以一个较大的搜索歩距进行搜索，因此可以减少搜索的次数，从而提高 算法的效率，减少初始值搜索的时间。
[0128] 步骤2.5:梯度下降法精确计算转换矩阵并进行速度估计和补偿。为了获得更加精 确的转换矩阵，并对速度进行估计和补偿，本发明采用Levenberg-Marquardt (LM)算法(梯 度下降算法的一种)优化上述能量函数求解得到最优转换矩阵T/，并估计得到 激光器的速度Vts。实施例具体包括以下子步骤：
[0129] 步骤2.5.1:速度估算。为了补偿由于激光器速度所造成的激光帧畸变，需要在配 准的过趕中对激光器的谏庶讲杆估笪和补偿。当前激光器速度V?估算公式如下所示：
[0130]
[0131]其中，Δ t为激光器扫描一帧所需要的时间（本实施例激光器频率为50Hz，所以
表示上一帧的转换矩阵的逆。
[0132] 步骤2.5.2:速度补偿。通过估算的速度，在进行一次LM优化迭代之前，按照下述公 式对激光帧St进行速度补偿：
[0133]
[0134] 其中，4代表未补偿的激光帧St中的一点，藏代表补偿后的激光帧矣中的一点，其 中k=l，2,3…K，其中K为一个激光扫描帧中的点的个数(本实施例中K = 540)，而Ats = ·ρ 表不激光器扫描一个点所需要的时间。
[0135] 步骤2.5.3 :LM优化。针对补偿后的激光帧皂和历史概率图ML1，采用LM算法优化 其配准能量函数，一次迭代之后，得到新的转换矩阵疗，且迭代次数加1。
[0136] 步骤2.5.4:根据新得到的转换矩阵疗，根据步骤2.5.1对速度进行更新，并用新的 速度根据步骤2.5.2对速度进行补偿，再进行步骤2.5.3。即返回重复步骤2.5.1-2.5.3，直 到满足迭代结束条件，例如LM算法收敛，或迭代次数大于预设的最大迭代次数I max(具体实 施时，本领域技术人员可自行预设取值，本实施例设为1000次），进入步骤2.6。
[0137] 步骤2.6:为实现基于多尺度的概率图配准，当s不等于S时，令s = s+l，返回步骤 2.5,当s等于S时，结束迭代进入步骤3。迭代步骤2.3到步骤2.5,也即完成每一层的配准。将 当前层的配准结果，也即当前层得到的旋转平移矩阵的三个参数(9，d x，dy)传递到下一层， 作为下一层的初始值，直至传递至最底层，从而完成由粗到精的配准，结束迭代进入步骤3。
[0138] 步骤3:概率图更新。当前激光扫描帧和历史概率图配准后，需要结合当前帧的信 息对每一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中。
[0139] 对每一层的概率图更新，实施例具体包括以下子步骤：
[0140] 步骤3.1:层数设置。在概率图更新中，需要对每一层的概率图进行更新，每一层的更新 策略完全一致。假设当前层数为8，在步骤3.1和步骤3.2中，麗4-1. =麗纟-1.，腿《；=麗?，.吨::=_财|0
[0141] 步骤3.2:当前帧概率图生成。通过步骤2,可以将当前帧St转换至历史概率图Mt-I 所在的坐标系，得到转换后的激光帧筘，针对$，采用如步骤1中的概率图生成方法生成对应 的概率图M t。
[0142] 步骤3.3:概率图更新。结合当前帧概率图Mt和历史概率图Mp1，对历史概率图进 行更新，得到当前历史概率图Μ ?:。针对概率图中的每一个格网g，本发明实施例将其表示为
其中MP(g) e [0,1]和S#(g) e [0 lj，分别表示概率图中该 格网的概率值，和过去所达到的最高的概率值，Mn (g)表示该格网在过去被观察到的次数。 概率图的更新主要分为4种情况，分别如下所示：
[0143] 1.不更新。如果该格网在当前帧概率图Mt和历史概率图，其概率值Mf(g) 和M^g)都为〇,也即蚪(g) = ML(g) = 0,则不进行任何更新。
[0144] 2.概率值增加。如果在当前帧概率图Mt上，有点落在格网g中，也即Mfig) > 0并且 Mtn(g) 2 1，则在历史概率图M^1上，增加该格网的概率值，并按照下述公式进行更新：
[0145：
[0146] 其中^是概率值增长率，计算方式如下：
[0147]
[0148] 其中，W1是预设的学习率，τ是控制参数(具体实施时本领域技术人员可预设取值， 本实施例中Wi = O .01，τ = 3)。
[0149] 3.概率值下降。如果在当前帧概率图Mt上，不存在点落在格网g中，也即 Mf(g) = 〇,同时，在以往帧中，不存在足够的点落于格网g中，也即其中nth 为预设的阈值(具体实施时本领域技术人员可预设取值，本实施例中设为30)，则在历史概 率图Mt-il，降低该格网的概率值，计算方式如下所示：
[0150]
[0151] 其中，Wd表示概率下降速率，其计算方式如下：
[0152]
[0153] 4.概率值增长和下降值内插。针对剩下的未处理的格网，采用内插的方法进行更 新。用％和^表示概率值增长了和下降了的格网，针对待内插的格网g，首先根据高斯函数 内插其增长和下降的概率值Pu和Pd，计算公式如下所示：
[0154]
[0155] 其中，d(g，gk)表不格网g和格网gk之间的距离，其中gk代表或者心中的一个格 网，31表示高斯函数的核大小（具体实施时，本领域技术人员可自行设定，本实施例中设为 1.5)。用P代表该格网增长或下降的概率值大小，如果I pu I > I Pd I，则p = Pu，反之，P = Pd，通 过下述公式对格网概率值进行更新：
[0156]
[0157] 根据步骤3.1到步骤3.3,可以对单尺度的概率图进行更新。本发明在步骤2中采用 的是一种多尺度的概率图配准方法，因此，需要按照步骤3.1和步骤3.3对多尺度的概率图 分别进行更新，可初始化s = 1，执行完毕后令s = s+1，再次执行，直到s = S。
[0158] 步骤4:返回步骤1，可以对后续各帧迭代步骤2到步骤3,直至所有激光帧处理完 毕，则终止迭代，并得到激光器的运行轨迹和室内动态环境的二维地图。参见图2,走廊场景 最终概率图。走廊中间的环线为机器车运行轨迹，其余区域，灰度值越大(影像越白），概率 值越大，反之，概率值越小。
[0159] 具体实施时，本发明所提供方法可基于软件技术实现自动运行流程，也可采用模 块化方式实现相应系统。本发明实施例提供一种基于点云的室内动态场景SLAM系统，包括 以下模块：
[0160] 第一模块，用于首先判断是否t = 0,表示当前激光扫描帧是否为第一帧，是则生成 当前帧的概率图M〗'，并直接设置历史概率图Mf_i 然后等待下一帧，当t不为0时，命 令第二模块工作；
[0161] 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯 模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0162] 第二模块，用于基于多尺度的激光帧和概率图配准，实现如下，
[0163] 设待配准的当前帧为St，各层历史概率图分别为ML1，其中S=1，2-_S，s为概率图 当前层数，S为总层数，最顶层初始转换矩阵为F tL1，采用上一帧最顶层的转换矩阵，
[0164] 将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能 量函数，寻找到两者间的最优转化矩阵:rf，
[0165] 能量函数如下所示，
[0166]
[0167] 其中，X表示激光帧St中的一个点，Γ/ ? X表示X经过g转换后的点，代沒? XlMU 表示转换后的点在历史概率图上的概率值，W(X)表示每个点的权重值，Mt 1为某一层的历 史概率图；
[0168] 第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每 一层历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中；
[0169] 对每一层的概率图进行更新，实现方式如下，
[0170] 设当前层数为s，令= MU，Mi = Mf，= M&根据第二模块的配准结 果，将当前帧St转换至历史概率图Mp1所在的坐标系，得到转换后的激光帧发，针对客进行 概率图生成，生成对应的概率图M t;
[0171]所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯 模型，针对不同分辨率的概率图，计算出每个格网的概率值大小；
[0172] 结合当前帧概率图Mt和历史概率图对历史概率图进行更新，得到当前历史 概率图Mt:;
[0173] 第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光 器的运行轨迹和室内动态环境的二维地图。
[0174]进一步地，第二模块的实现方式包括如下子模块，
[0175] 第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分 布直方图:?%直方图个数为B;
[0176] 第二子樽块，用干权倌估筧,毎个点的权倌wb计算公式如下所示，
[0177]
[0178] 其中叫表示第b个直方图的大小，其中b = l，2，~B;
[0179] 第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该 层的初始转换矩阵为上一帧传递过来的转换矩阵Tt 1;若不是最顶层，则该层的初始转换矩 阵为上一层传递下来的转换矩阵疗一1;
[0180] 第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值；
[0181] 第五子模块，用于采用LM算法优化能量函数MU求解得到最优转换矩阵 拉，并估计得到激光器的速度咬；
[0182] 第六子模块，用于当s不等于S时，令s = s+l，命令第五子模块工作，当s等于S时，结 束迭代命令第三模块工作。
[0183] 各模块具体实现可参见相应步骤，本发明不予赘述。
[0184] 需要强调的是，本发明所述的实施例是说明性的，而不是限定性的。因此本发明包 括并不限于【具体实施方式】中所述的实施例，凡是由本领域技术人员根据本发明的技术方案 得出的其他实施方式，同样属于本发明保护的范围。
【主权项】
1. 一种基于点云的室内动态场景SLAM方法，其特征在于，包括以下步骤： 步骤1，首先判断是否t = 0,表示当前激光扫描帧是否为第一帧，是则生成当前帧的概 率图M0S，并直接设置历史概率图= M0S，然后等待下一帧，当t不为0时，进入步骤2; 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模 型，针对不同分辨率的概率图，计算出每个格网的概率值大小； 步骤2，基于多尺度的激光帧和概率图配准，实现如下， 设待配准的当前帧为St，各层历史概率图分别为，其中8 = 1，2~5,8为概率图当前 层数，S为总层数，最顶层初始转换矩阵为，采用上一帧最顶层的转换矩阵， 将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函 数，寻找到两者间的最优转化矩阵7?， 能量函数如下所示，其中，X表示激光帧st中的一个点，疗(g) X表示X经过哎转换后的点，POT (gi XIMLJ表 示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，Μ?ιΛ某一层的历史 概率图； 步骤3,概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层历史概 率图进行更新，从而可以将新的概率图用于下一帧的配准中； 对每一层的概率图进行更新，实现方式如下， 设当前层数为s，令= 根据步骤2的配准结果，将当前 帧St转换至历史概率图所在的坐标系，得到转换后的激光帧说，针对发进行概率图生 成，生成对应的概率图Mt; 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模 型，针对不同分辨率的概率图，计算出每个格网的概率值大小； 结合当前帧概率图Mt和历史概率图，对历史概率图进行更新，得到当前历史概率图 步骤4,返回步骤1，直至所有激光帧处理完毕，则终止迭代，得到激光器的运行轨迹和 室内动态环境的二维地图。2. 根据权利要求1所述基于点云的室内动态场景SLAM方法，其特征在于:步骤2的实现 方式包括如下步骤， 步骤2.1，点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直方间冗.直 方图个数为Β; 步骤2.2，权值估算，每个点的权值wb计算公式如下所示，其中1^表示第b个直方图的大小，其中b=l，2,…B; 步骤2.3,通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的初始转换 矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为上一层 传递下来的转换矩阵ΤΓ1;: 步骤2.4，根据步骤2.3确定的转化矩阵获得LM算法初始值； 步骤2.5，采用LM算法优化能量函数f (? MU，求解得到最优转换矩阵7V，并估计得到 激光器的速度以_; 步骤2.6，当s不等于S时，令s = s+1，返回步骤2.5，当s等于S时，结束迭代进入步骤3。3.根据权利要求2所述基于点云的室内动态场景SLAM方法，其特征在于:步骤3中，结合 当前帧概率图Mt和历史概率图，对历史概率图进行更新，实现方式如下， 针对概率图中的每一个格网g，表示为蘭(g) = (g),(g),蘭#(g)}，其中厕p(g) e L〇,i]和l#(g) e ?〇,?]，分别表示概率图中该格网的概率值，和过去所达到的最高的概率值， Mn(g)表示该格网在过去被观察到的次数;概率图的更新分为4种情况，分别如下所示， 如果该格网g在当前帧概率图Mt和历史概率图Mg上，概率值Mf(g)和腿^(g)都为0， 则不进行任何更新； 如果在当前帧概率图Mt上，Mf(g) > 0并且M?(g) 2 1，则按照下述公式进行更新，其中，^是概率值增长率，计算方式如下， A 二 Μ (1 + τΜ丨f-"g)) 其中，W1是预设的学习率，τ是控制参数； 如果在当前帧概率图Mt上，Mf(g) = 〇,同时，，其中nth为预设的阈值，则 按照下述公式进行更新， M?(g) = (1 - 其中，Wd表示概率下降速率，其计算方式如下，针对剩下的未处理的格网，采用内插的方式进行更新，包括用@?和《^表示概率值增长 了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值Pu 和Pd，计算公式如下所示：如果|pu| > Ipd|，贝ljp = pu，反之，P = Pd，通过下述公式对格网概率值进行更新：其中，d(g，gk)表不格网g和格网gk之间的距离，其中gk代表心或者％中的一个格网，δι 表示高斯函数的核大，用Ρ代表该格网增长或下降的概率值大小。4. 一种基于点云的室内动态场景SLAM系统，其特征在于，包括以下模块： 第一模块，用于首先判断是否t = 0,表示当前激光扫描帧是否为第一帧，是则生成当前 帧的概率图_，并直接设置历史概率图爾4 = M0S，然后等待下一帧，当t不为0时，命令第 二模块工作； 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模 型，针对不同分辨率的概率图，计算出每个格网的概率值大小； 第二模块，用于基于多尺度的激光帧和概率图配准，实现如下， 设待配准的当前帧为St，各层历史概率图分别为Mfq，其中8 = 1，2 - 5,8为概率图当前 层数，S为总层数，最顶层初始转换矩阵为7〗*^.采用上一帧最顶层的转换矩阵， 将当前帧配准至历史概率图时，在每一层中通过结合穷举法和梯度下降法优化能量函 数，寻找到两者间的最优转化矩阵疗， 能量函数如下所示，其中，X表示激光帧S冲的一个点，G ? X表示X经过砭转换后的点，/>(纪^xlMU表 示转换后的点在历史概率图上的概率值，w(x)表示每个点的权重值，为某一层的历史 概率图； 第三模块，用于概率图更新，当前帧和历史概率图配准后，结合当前帧的信息对每一层 历史概率图进行更新，从而可以将新的概率图用于下一帧的配准中； 对每一层的概率图进行更新，实现方式如下， 设当前层数为=鋪f-i，厕t = ;根据第二模块的配准结果，将当 前帧St转换至历史概率图糜所在的坐标系，得到转换后的激光帧旯，针对进行概率图 生成，生成对应的概率图Mt; 所述进行概率图生成，包括进行激光帧轮廓提取，根据激光帧轮廓，结合二维高斯模 型，针对不同分辨率的概率图，计算出每个格网的概率值大小； 结合当前帧概率图Mt和历史概率图Mm，对历史概率图进行更新，得到当前历史概率图 第四模块，用于命令第一模块，直至所有激光帧处理完毕，则终止迭代，得到激光器的 运行轨迹和室内动态环境的二维地图。5. 根据权利要求4所述基于点云的室内动态场景SLAM系统，其特征在于:第二模块的实 现方式包括如下子模块， 第一子模块，用于点云法向量直方图统计，包括统计激光帧中所有点的法向量分布直 方图兄直方图个数为B; 第二子模块，用于权值估算，每个点的权值wb计算公式如下所示，其中％表示第b个直方图的大小，其中b=l，2,…B; 第三子模块，用于通过s是否等于1，判断当前层是否为最顶层，若为最顶层，则该层的 初始转换矩阵为上一帧传递过来的转换矩阵若不是最顶层，则该层的初始转换矩阵为 上一层传递下来的转换矩芦I 了广1; 第四子模块，用于根据第三子模块确定的转化矩阵获得LM算法初始值； 第五子模块，用于采用LM算法优化能量函数求解得到最优转换矩阵疗，并 估计得到激光器的速度松； 第六子模块，用于当s不等于S时，令s = s+1，命令第五子模块工作，当s等于S时，结束迭 代命令第三模块工作。6.根据权利要求5所述基于点云的室内动态场景SLAM系统，其特征在于:第三模块中， 结合当前帧概率图Mt和历史概率图Mtd，对历史概率图进行更新，实现方式如下， 针对概率图中的每一个格网8，表示为賊8) = {1^(8),1#(5),爾"(8)；}，其中耐(8)€ [0,1]和腿气g) e [0,1]，分别表示概率图中该格网的概率值，和过去所达到的最高的概率值， Mn(g):表示该格网在过去被观察到的次数;概率图的更新分为4种情况，分别如下所示， 如果该格网g在当前帧概率图Mt和历史概率图上，概率值,V/f (g)和M^g)都为0， 则不进行任何更新； 如果在当前帧概率图Mt上，Mf(g) > 0并且M?(g) 2 1，则按照下述公式进行更新，其中，^是概率值增长率，计算方式如下， % (1 + 1(g)) 其中，W1是预设的学习率，τ是控制参数； 如果在当前帧概率图Mt上，Mf(g) = 0,同时，MKg) S知，其中nth为预设的阈值，则 按照下述公式进行更新， Mf(g) = (1 - wd)FyD|；_1(g) 其中，Wd表示概率下降速率，其计算方式如下，针对剩下的未处理的格网，采用内插的方式进行更新，包括用^表示概率值增长 了和下降了的格网，针对待内插的格网g，首先根据高斯函数内插其增长和下降的概率值Pu 和Pd，计算公式如下所示：如果|pu| > Ipd|，贝ljp = pu，反之，P = Pd，通过下述公式对格网概率值进行更新：其中，d(g，gk)表不格网g和格网gk之间的距离，其中gk代表必U,或者中的一个格网，δι 表示高斯函数的核大，用Ρ代表该格网增长或下降的概率值大小。
【文档编号】G06T7/00GK106056643SQ201610268859
【公开日】2016年10月26日
【申请日】2016年4月27日
【发明人】姚剑, 李礼, 刘康, 谢仁平, 涂金戈
【申请人】武汉大学