CN111551953A - 基于slam算法的室内建图优化方法 - Google Patents

Abstract

基于SLAM算法的室内建图优化方法，包括SPA优化步骤、分支定界扫描匹配优化步骤，所述SPA优化步骤中，对scan和submap的位姿进行优化；所述分支定界扫描匹配优化步骤中，在新加入地图的scan的估计位姿附近选取一个窗口，然后在该窗口中搜索scan的可能匹配项，将匹配项的臂环约束添加到位姿优化中。其有益效果是：修复了运动畸变和地图漂移问题，提高了算法对复杂环境的适应能力，对环境中的细节特征描述更清晰。

Description

基于SLAM算法的室内建图优化方法

技术领域

本发明涉及移动机器人同步定位与地图构建领域，特别是一种基于SLAM算法的室内建图优化方法。

背景技术

近年来，国内外研究学者就移动机器人同步定位与地图构建(SLAM)算法提出了众多方案。移动机器人构建地图的效果与SLAM算法密切相关，但是不同参数对建图质量有很大的影响，本文旨在通过在室内环境下创建地图，对比不同参数下的建图效果，并表明参数配置的方法，提高室内环境下Cartographer 2D SLAM算法建图的准确度。

发明内容

本发明的目的是为了解决上述问题，设计了一种基于SLAM算法的室内建图优化方法。具体设计方案为：

SLAM算法：

在2D SLAM中，移动机器人的位姿ξ由激光雷达观察获得的平移(x,y)和旋转(ξ_θ)三个参数确定,表示为ξ＝(ξ_x,ξ_y,ξ_θ)。在Cartographer算法中，当前姿态中的激光观测值为scan,一定数量的scan构成一个submap(子地图)。

局部SLAM是利用扫描匹配(Scan matching)将一系列连续scan插入到submap中。

submap的构建过程是scan与子submap坐标系反复匹配的迭代过程。scan原点位于O∈R²，scan点集表示为H＝{h_k}_k＝1,...,k，h_k∈R²。

通过转换系数T_ξ把scan坐标系下的点转换为在submap坐标系下的点，转换公式为：

一些连续的scan组成submap，submap采用概率网格M:rZ×rZ→[p_min,p_max]的形式，这表示分辨率为r的离散网格点被占用的概率。对于每个网格点，用相关的像素去近似表示。

当scan插入submap时，会产生一组称为hits的网格点和一组称为misses的网格点。

其中命中的网格点是hit，加入hits集合经过的网格点是miss，加入misses集合。每个hits中的网格点被赋予初始值M＝p_hit，每个misses中的网格点被赋予初始值M＝p_miss。如果网格点已经有p值，则用下述公式更新：

M_new(x)＝clamp(odds^-1(odds(M_old(x)·odds(p_hit))))

将scan插入submap前，使用Ceres库的扫描匹配器对scan的位姿ξ进行优化，将求解scan位姿的问题转变为求解非线性最小二乘的问题，其公式为：

其中T_ξ将h_ξ从scan坐标系转换到submap坐标系，函数M_soomth:R²→R对局部submap中的概率值使用双三次插值法进行平滑处理，将值限制在范围内。

scan仅与当前submap匹配，而地图是由多个submap构成，随着更多的submap被创建，扫描匹配过程中的累计误差会越来越大，因此采取稀疏姿态调整(Sparse PoseAdjustment)的方法，优化所有的scan和submap的位姿。

具体设计方案为：

一种基于SLAM算法的室内建图优化方法，包括SPA优化步骤、分支定界扫描匹配优化步骤，

所述SPA优化步骤中，对scan和submap的位姿进行优化；

所述分支定界扫描匹配优化步骤中，在新加入地图的scan的估计位姿附近选取一个窗口，然后在该窗口中搜索scan的可能匹配项，将匹配项的臂环约束添加到位姿优化中，具体包括选择节点步骤、分支规则步骤、计算上线步骤，所述节点步骤、分支规则步骤、计算上限步骤依次进行。

所述SPA优化步骤中，其优化公式为：

其中，ρ为损失函数，使用损失函数的目的是减少加入到优化问题中的离群点对于系统的影响(损失函数(loss function)或代价函数(cost function)是将随机事件或其有关随机变量的取值映射为非负实数以表示该随机事件的“风险”或“损失”的函数)，

和

分别为submap和scan在世界坐标系中的位姿，在给定的相对位姿ξ_ij和相关协方差矩阵∑_ij的约束下对它们进行优化，对于成组的submapi和scanj，scan在对应的submap坐标系下的相对位姿用ξ_ij表示，协方差矩阵∑_ij采用Ceres库进行特征估计。submap在世界坐标系中的位姿，i，j表示个数。

所述分支定界扫描匹配优化步骤中，搜索公式(扫描匹配公式)为：

其中，w表示搜索窗口,M_nearest是M中参数的最近网格点到对应像素(R²)的扩展。为提高搜索效率，选择分支定界法计算搜索窗口内的ξ^*。在该方法中，概率树上的节点用子集的概率表示，其中根节点ω代表所有的可能解，一个叶节点代表一个可能解。该方法主要包括三个部分内容:选择节点，分支规则，计算上限。分支定界法能够精确寻找窗口内得分最高的节点score(c)。

所述选择节点步骤中，使用深度优先搜索(DFS)方法对节点进行评估，首先计算每个子节点的得分并引入分数阈值，利用优先访问得分最高子节点的搜索规则，将小于该阈值的节点舍弃，然后根据得分将每一个节点进行排序，选择出分数最高的节点。

深度优先搜索所遵循的搜索策略是尽可能“深”地搜索图。在深度优先搜索中，对于最新发现的顶点，如果它还有以此为起点而未探测到的边，就沿此边继续汉下去。当结点v的所有边都己被探寻过，搜索将回溯到发现结点v有那条边的始结点。这一过程一直进行到已发现从源结点可达的所有结点为止。如果还存在未被发现的结点，则选择其中一个作为源结点并重复以上过程，整个进程反复进行直到所有结点都被发现为止。

和广度优先搜索类似，每当扫描已发现结点u的邻接表从而发现新结点v时，深度优先搜索将置v的先辈域π[v]为u。和宽度优先搜索不同的是，前者的先辈子图形成一棵树，而后者产生的先辈子图可以由几棵树组成，因为搜索可能由多个源顶点开始重复进行。因此深度优先搜索的先辈子图的定义也和宽度优先搜索稍有不同：

Gπ＝(V,Eπ),Eπ＝{(π[v],v)∈E:v∈V∧π[v]≠NIL}

深度优先搜索的先辈子图形成一个由数个深度优先树组成的深度优先森林。Eπ中的边称为树枝。

和宽度优先搜索类似，深度优先在搜索过程中也为结点着色以表示结点的状态。每个顶点开始均为白色，搜索中被发现时置为灰色，结束时又被置成黑色(即当其邻接表被完全检索之后)。这一技巧可以保证每一顶点搜索结束时只存在于一棵深度优先树上，因此这些树都是分离的。

除了创建一个深度优先森林外，深度优先搜索同时为每个结点加盖时间戳。每个结点v有两个时间戳：当结点v第一次被发现(并置成灰色)时记录下第一个时间戳d[v]，当结束检查v的邻接表时(并置v为黑色)记录下第二个时间截f[v]。许多图的算法中都用到时间戳，他们对推算深度优先搜索进行情况是很有帮助的。

下列过程DFS记录了何时在变量d[u]中发现结点u以及何时在变量f[u]中完成对结点u的检索。这些时间戳为1到2|V|之间的整数，因为对每一个v中结点都对应一个发现事件和一个完成事件。对每一顶点u，有d[u]<f[u]，在时刻d[u]前结点u为白色，在时刻d[u]和f[u]之间为灰色，以后就变为黑色。

所述SPA优化步骤中，残差E的计算公式为：

所述分支规则步骤中，

概率树中的每个节点都可以成作一个整数元组c＝(c_θ,c_θ,c_θ,c_θ)∈Z⁴，节点高度为c_h，具有

种可能的转换，具体的特征转换如公式为：

叶节点的高度为c_h＝0时，对应的可行解如公式：

将父节点分为一系列的初始化节点，初始化节点的高度为h₀并且覆盖了整个搜索窗口，分化公式为：

给定节点高度为c_h＞1的节点c，并将其分支为四个高度为c_h-1的子节点：

所述计算上限步骤中，

计算每个内部节点的上限是分支定界法的最后一步，上限计算公式为：

计算时

搜索区域的边界可能会大于相邻分支中的

计算内部节点得分如公式：

其中ξ_c为之前的叶节点，网格

与M_nearest有相同的像素结构，每个像素中存储的是当前2^h×2^h像素窗口中的最大值。

通过本发明的上述技术方案得到的基于SLAM算法的室内建图优化方法，其有益效果是：

修复了运动畸变和地图漂移问题，提高了算法对复杂环境的适应能力，对环境中的细节特征描述更清晰。

附图说明

图1是网格点与相关像素的示意图；

图2是Hits网格点与misses网格点的示意图；

图3是默认参数下建立的地图；

图4是默认配置下关闭global SLAM建立的地图；

图5是修改参数后关闭global SLAM建立的地图

图6是加入global SLAM优化后建立的地图。

具体实施方式

下面结合附图对本发明进行具体描述。

一种基于SLAM算法的室内建图优化方法，包括SPA优化步骤、分支定界扫描匹配优化步骤，

所述SPA优化步骤中，对scan和submap的位姿进行优化；

所述分支定界扫描匹配优化步骤中，在新加入地图的scan的估计位姿附近选取一个窗口，然后在该窗口中搜索scan的可能匹配项，将匹配项的臂环约束添加到位姿优化中，具体包括选择节点步骤、分支规则步骤、计算上线步骤，所述节点步骤、分支规则步骤、计算上限步骤依次进行。

所述SPA优化步骤中，其优化公式为：

其中，ρ为损失函数，使用损失函数的目的是减少加入到优化问题中的离群点对于系统的影响(损失函数(loss function)或代价函数(cost function)是将随机事件或其有关随机变量的取值映射为非负实数以表示该随机事件的“风险”或“损失”的函数)，

和

分别为submap和scan在世界坐标系中的位姿，在给定的相对位姿ξ_ij和相关协方差矩阵∑_ij的约束下对它们进行优化，对于成组的submapi和scanj，scan在对应的submap坐标系下的相对位姿用ξ_ij表示，协方差矩阵∑_ij采用Ceres库进行特征估计。submap在世界坐标系中的位姿，i，j表示个数。

所述分支定界扫描匹配优化步骤中，搜索公式(扫描匹配公式)为：

其中，w表示搜索窗口,M_nearest是M中参数的最近网格点到对应像素(R²)的扩展。为提高搜索效率，选择分支定界法计算搜索窗口内的ξ^*。在该方法中，概率树上的节点用子集的概率表示，其中根节点ω代表所有的可能解，一个叶节点代表一个可能解。该方法主要包括三个部分内容:选择节点，分支规则，计算上限。分支定界法能够精确寻找窗口内得分最高的节点score(c)。

所述选择节点步骤中，使用深度优先搜索(DFS)方法对节点进行评估，首先计算每个子节点的得分并引入分数阈值，利用优先访问得分最高子节点的搜索规则，将小于该阈值的节点舍弃，然后根据得分将每一个节点进行排序，选择出分数最高的节点。

深度优先搜索所遵循的搜索策略是尽可能“深”地搜索图。在深度优先搜索中，对于最新发现的顶点，如果它还有以此为起点而未探测到的边，就沿此边继续汉下去。当结点v的所有边都己被探寻过，搜索将回溯到发现结点v有那条边的始结点。这一过程一直进行到已发现从源结点可达的所有结点为止。如果还存在未被发现的结点，则选择其中一个作为源结点并重复以上过程，整个进程反复进行直到所有结点都被发现为止。

和广度优先搜索类似，每当扫描已发现结点u的邻接表从而发现新结点v时，深度优先搜索将置v的先辈域π[v]为u。和宽度优先搜索不同的是，前者的先辈子图形成一棵树，而后者产生的先辈子图可以由几棵树组成，因为搜索可能由多个源顶点开始重复进行。因此深度优先搜索的先辈子图的定义也和宽度优先搜索稍有不同：

Gπ＝(V,Eπ),Eπ＝{(π[v],v)∈E:v∈V∧π[v]≠NIL}

深度优先搜索的先辈子图形成一个由数个深度优先树组成的深度优先森林。Eπ中的边称为树枝。

和宽度优先搜索类似，深度优先在搜索过程中也为结点着色以表示结点的状态。每个顶点开始均为白色，搜索中被发现时置为灰色，结束时又被置成黑色(即当其邻接表被完全检索之后)。这一技巧可以保证每一顶点搜索结束时只存在于一棵深度优先树上，因此这些树都是分离的。

除了创建一个深度优先森林外，深度优先搜索同时为每个结点加盖时间戳。每个结点v有两个时间戳：当结点v第一次被发现(并置成灰色)时记录下第一个时间戳d[v]，当结束检查v的邻接表时(并置v为黑色)记录下第二个时间截f[v]。许多图的算法中都用到时间戳，他们对推算深度优先搜索进行情况是很有帮助的。

下列过程DFS记录了何时在变量d[u]中发现结点u以及何时在变量f[u]中完成对结点u的检索。这些时间戳为1到2|V|之间的整数，因为对每一个v中结点都对应一个发现事件和一个完成事件。对每一顶点u，有d[u]<f[u]，在时刻d[u]前结点u为白色，在时刻d[u]和f[u]之间为灰色，以后就变为黑色。

所述SPA优化步骤中，残差E的计算公式为：

所述分支规则步骤中，

概率树中的每个节点都可以成作一个整数元组c＝(c_θ,c_θ,c_θ,c_θ)∈Z⁴，节点高度为c_h，具有2^ch×2^ch种可能的转换，具体的特征转换如公式为：

叶节点的高度为c_h＝0时，对应的可行解如公式：

将父节点分为一系列的初始化节点，初始化节点的高度为h₀并且覆盖了整个搜索窗口，分化公式为：

给定节点高度为c_h＞1的节点c，并将其分支为四个高度为c_h-1的子节点：

所述计算上限步骤中，

计算每个内部节点的上限是分支定界法的最后一步，上限计算公式为：

计算时

搜索区域的边界可能会大于相邻分支中的

计算内部节点得分如公式：

其中ξ_c为之前的叶节点，网格

与M_nearest有相同的像素结构，每个像素中存储的是当前2^h×2^h像素窗口中的最大值。

实施例1

如图3所示，首先bobac机器人实地运行，使用Cartographer算法自带的revo_lds.lua文件建图，并用rosbag记录观测的数据，其余实验使用记录的数据进行仿真，以保证试验时数据一致。

根据bobac机器人硬件配置修改算法追踪的ROS坐标系ID:tracking_map＝“base_link”，发布位姿子坐标系的ROS坐标系ID:published_frame＝“odom”。

指定激光雷达数据的有效范围:

TRAJECTORY_BUILDER_2D.min_range＝0.15；TRAJECTORY_BUILDER_2D.max_ran-ge＝12。

移动机器人选择激光雷达作为唯一的传感器，因此启用RealTimeCorrelativeSanMatcher扫描匹配策略，设置在线相关扫描匹配:TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matchi-ng＝true。

实验采用2D SLAM,距离数据可以被实时处理而不需要额外的信息来源，因此不需要IMU信息，设置:

TRAJECTORY_BUILDER_2D.use_imu_data＝false。其余参数使用默认配置。

观察地图发现，方框1出现了大面积的位置漂移，方框2处障碍物消失，方框3出现了运动畸变，因此需要对算法参数进行优化。Cartographer 2D SLAM算法参数优化被分为两部分，首先进行local SLAM优化，以便于产生较好的子地图，减小扫描数据插入子地图时产生的误差，然后在此基础上进行global SLAM优化，实现更好的回环约束并构建更准确的全局地图。

实施例2

在实施例1的基础上，

local SLAM构建子地图时出现子地图滑动和地图细节描述不清晰的问题，因此需要关闭glob-al SLAM，避免调节参数时对local SLAM产生影响。

根据Cartographer算法调参规则，设置:

POS-E_GRAPH.optimize_every_n_nodes＝0。

子地图必须足够的小以保证漂移低于分辨率，另一方面它们需要足够的大以保证闭环检测时能够区分它们，因此设置:

TRAJECTORY_BUILDER_2D.submap-s_num_range_data＝30。

realTimeCorrelativeScanM-atcher的工作原理是从搜索窗口中搜索相似的扫描数据。根据地图中细节描述不够清晰，需要修改搜索窗口或扫描数据的平移和旋转分量的权重。

设置:

TRAJECTORY_BUILDER_2D.real_time_co-rre-lative_scan_matcher.linear_search_window＝0.1；

TRAJECTORY_BUILDER_2D.real_time_correlati-ve_scan_matcher.translation_weight＝20；

TRAJECT-ORY_BUILDER_2D.real_time_correlative_scan_m-atcher.rotation_weight＝10。

一旦扫描匹配器找到了两个扫描数据之间的运动关系，就会转到运动滤波器(motion filter)，为了避免运动滤波器中的每个子地图插入过多的扫描数据，通过设置移动机器人运动的距离、角度或者时间阈值来确定一次扫描数据是否可以被插入到当前子地图中，因此设置:

TRAJECTORY_BUILDER_2D.motion_filter.m-ax_rangle_radians＝0.35。

如图4、图5所示，

由于所收集的机器人位姿数据的误差，两图中方框1部分都在水平方向上出现倾斜，观察发现图4中方框1的部分比图5中方框1的部分倾斜更严重。对比图4和图5中方框2、3部分，在输入相同的数据下，优化后的算法可以更好地匹配障碍物的轮廓，不会出现重要扫描数据被丢弃的现象。

实施例3

在实施例1的基础上：

在local SLAM优化获得质量较好的子地图基础上，继续进行global SLAM优化。

一旦子地图插入一定数量的轨迹节点后，global SLAM就会开始进行批量进行优化，根据算法运行的的频率调整子地图批次的大小，

设置:POSE_GRAPH.optimize_e-very_n_nodes＝35。

节点和子地图构建约束时会优先经过FastCorrelativeScanMatcher扫描匹配器，实现实时闭环扫描匹配。

一旦FastCorrelativeScanMatc-her有足够好的提议(超过最小匹配分数)，构建的约束就会被送入Ceres扫描匹配器进行位姿细化，

因此设置:POSE_GRAPH.constraint_builder.min_sc-ore＝0.5。

如图5、图6所示，加入global SLAM优化后图6的地图轮廓比图5更平滑，其中图6方框1部分尤为明显，此外该优化也减小了方框2，3部分子地图之间位置误差。

上述技术方案仅体现了本发明技术方案的优选技术方案，本技术领域的技术人员对其中某些部分所可能做出的一些变动均体现了本发明的原理，属于本发明的保护范围之内。

Claims (7)

1.一种基于SLAM算法的室内建图优化方法，包括SPA优化步骤、分支定界扫描匹配优化步骤，其特征在于，

所述SPA优化步骤中，对scan和submap的位姿进行优化；

所述分支定界扫描匹配优化步骤中，在新加入地图的scan的估计位姿附近选取一个窗口，然后在该窗口中搜索scan的可能匹配项，将匹配项的臂环约束添加到位姿优化中，具体包括选择节点步骤、分支规则步骤、计算上线步骤，所述节点步骤、分支规则步骤、计算上限步骤依次进行。

2.根据权利要求1中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述SPA优化步骤中，其优化公式为：

其中，ρ为损失函数，

和

分别为submap和scan在世界坐标系中的位姿，对于成组的submapi和scanj，scan在对应的submap坐标系下的相对位姿用ξ_ij表示，协方差矩阵∑_ij采用Ceres库进行特征估计。

3.根据权利要求1中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述分支定界扫描匹配优化步骤中，搜索公式为：

其中，w表示搜索窗口,M_nearest是M中参数的最近网格点到对应像素(R²)的扩展。为提高搜索效率，选择分支定界法计算搜索窗口内的ξ^*。在该方法中，概率树上的节点用子集的概率表示，其中根节点ω代表所有的可能解，一个叶节点代表一个可能解。该方法主要包括三个部分内容:选择节点，分支规则，计算上限。分支定界法能够精确寻找窗口内得分最高的节点score(c)。

4.根据权利要求1中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述选择节点步骤中，首先计算每个子节点的得分并引入分数阈值，利用优先访问得分最高子节点的搜索规则，将小于该阈值的节点舍弃，然后根据得分将每一个节点进行排序，选择出分数最高的节点。

5.根据权利要求1中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述SPA优化步骤中，残差E的计算公式为：

6.根据权利要求3中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述分支规则步骤中，

概率树中的每个节点都可以成作一个整数元组c＝(c_θ,c_θ,c_θ,c_θ)∈Z⁴，节点高度为c_h，具有

种可能的转换，具体的特征转换如公式为：

叶节点的高度为c_h＝0时，对应的可行解如公式：

将父节点分为一系列的初始化节点，初始化节点的高度为h₀并且覆盖了整个搜索窗口，分化公式为：

给定节点高度为c_h＞1的节点c，并将其分支为四个高度为c_h-1的子节点：

7.根据权利要求6中所述的基于SLAM算法的室内建图优化方法，其特征在于，所述计算上限步骤中，

计算每个内部节点的上限是分支定界法的最后一步，上限计算公式为：

Images