一种移动机器人SLAM问题中的实时回环检测方法技术

本发明专利技术涉及一种移动机器人SLAM问题中的实时回环检测方法，该方法包括以下步骤：1)获取移动机器人存储的子图；2)获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧‑子图对；3)对处于同一地图位置的扫描帧‑子图对进行相关性度量和复杂性度量计算；4)基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果。与现有技术相比，本发明专利技术采用相关性度量和复杂性度量对初步回环检测进行了验证，既保证了精度又保证了实时性，采用分枝定界法和预计算栅格获取相对位姿，有效减小了计算量，提高了回环检测效率。

A real-time loop detection method for SLAM problem of mobile robot

【技术实现步骤摘要】
一种移动机器人SLAM问题中的实时回环检测方法
本专利技术涉及领域，尤其是涉及一种移动机器人SLAM问题中的实时回环检测方法。
技术介绍
移动机器人执行各类任务，如导航、传递、搜索和救援等，一般都需要对其工作环境建构地图，并确定自己的在环境中的位置，这就需要同时定位与地图构建(simultaneouslocalizationandmapping,以下简称SLAM)技术。其中，基于激光雷达的SLAM技术具有较多优势，成为目前最受关注的SLAM方向之一。目前，市面上有很多开源的激光SLAM整体解决方案，如Gmapping、Hector、Karto、Cartographer等，可分为基于滤波器的和基于图优化的。然而这些方案应用在移动机器人平台和大范围工作环境时，基于滤波器的gmapping等严重依赖里程计，长时间下会出现误差的显著累积，难以保证轨迹和地图的全局一致。基于图优化的，hector对里程计要求低但对激光雷达要求高，且也没有处理回环问题，仍难以保证全局一致；karto保证了精度但保证不了实时性；cartographer是目前较好的激光SLAM方案，实现了前端多传感器融合，有回环检测模块，但在对称性较强的环境中(办公室、走廊等往往有这样的特点)，回环检测可能出现误报(false-positive)。
技术实现思路
本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供一种移动机器人SLAM问题中的实时回环检测方法。本专利技术的目的可以通过以下技术方案来实现：一种移动机器人SLAM问题中的实时回环检测方法，该方法包括以下步骤：S1：获取移动机器人存储的子图；S2：获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧-子图对；S3：对处于同一地图位置的扫描帧-子图对进行相关性度量和复杂性度量计算；S4：基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果。所述子图包括组成环境地图的若干局部地图和机器人先前存储的所有扫描帧；所述相关性度量，描述扫描帧和子图经回环变换对齐后，共有地形的数量；所述复杂性度量，考察共有地形中是否有长的直线段。进一步地，所述步骤S3中，相关性度量的计算包括以下步骤：S301：获取步骤S2回环检测中的扫描帧-子图对相对位姿，对扫描帧-子图对中的扫描帧进行位姿变换，使得扫描帧与子图对齐；S302：将对齐后的扫描帧和子图网格化为相等大小的空间区域；S303：基于网格化的扫描帧和子图，分别构建归一化的扫描帧二维直方图和归一化的子图二维直方图；S304：基于交叉核函数，计算归一化的扫描帧二维直方图和归一化的子图二维直方图的相关性度量值。进一步地，所述步骤S304中，交叉核函数的表达式为：式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。进一步地，所述步骤S3中，复杂性度量的计算包括以下步骤：S311：获取扫描帧-子图对中的共有区域；S312：在共有区域中抛弃离群点，获取内点集合；S313：基于内点集合中各内点对应的法向量，构建法向量矩阵；S314：基于法向量矩阵的特征值，计算复杂性度量。进一步地，所述步骤S313中，法向量矩阵的构建表达式为：N＝[n1,n2,...,nn]TR＝NTN式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。进一步地，所述步骤S314中，复杂性度量的计算表达式为：式中，r为复杂性度量值，λ1为扫描帧法向量矩阵的特征值，λ2为子图法向量矩阵的特征值。进一步地，所述步骤S2中，基于移动机器人存储的子图对扫描帧进行初步回环检测具体为，基于预建立的非线性最小二乘模型，获取扫描帧-子图对，所述非线性最小二乘模型的约束为相对位姿，该相对位姿为在子图坐标系下扫描帧的位姿，所述相对位姿的计算表达式为：式中，{hk}k＝1,2,...,K为扫描帧中的各点，K为点数，Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，为搜索区域，该搜索区域基于机器人当前位姿估计确定。进一步地，因为所述搜索区域比较大，因此所述相对位姿基于分枝定界法获取，分枝定界法将求最优解问题转化为树形搜索问题，该分枝定界法包括以下步骤：S201：将位姿变换的搜索区域划分为多个节点，计算每个节点的得分并将节点按照得分排序存储到栈中，使得得分最高的节点在栈顶，将最佳得分记为零；S202：从栈顶获取一个节点，判断该节点得分是否大于最佳得分，若是则执行步骤S203，否则执行步骤S204；S203：判断该节点是否为叶子节点，若是，则以该节点确定匹配结果和最佳得分，若否，则将该节点分枝为若干节点，计算每个分枝节点的得分并按照得分排序存储到栈中，使得得分最高的节点在栈顶；S204：去除该节点，重复执行步骤S202，直到栈中无节点。由于叶子节点才代表实际解，在节点的选择上，本专利技术采用深度优先搜索，以快速地考察大量叶子节点，找到合适的真实解。进一步地，所述节点由一组整数参数描述，其表达式如下：其中，ch表述节点c包含叶子节点的数量，cx,cy,cθ表述节点c的位姿信息；所述节点代表的空间的表达式为：式中，为节点c代表的空间。对于一个规模ch≥1的节点c，可将其分枝为四个规模为ch-1的子节点：步骤S201中，栈的初始化具体为：其中，需要保证覆盖整个搜索区域。用节点集对栈进行初始化。进一步地，所述节点得分计算的表达式为：式中，为基于子图预先计算的栅格地图，该地图中每个像素的取值是原子图中对应像素起始的大小的方块内像素取值的最大值。与现有技术相比，本专利技术具有以下优点：(1)本专利技术在进行回环检测的同时对得到的回环变换进行验证，采用相关性度量和复杂性度量对回环检测结果进行验证，相关性度量描述了扫描帧-子图对共有地形的数量，复杂性度量考察了共有地形是否有足够丰富的特征，两个度量简单且鲁棒的避免回环检测在对称性较强的环境下产生误报、影响全局地图，并且检测与验证两部分是分离的，既保证了精度又保证了实时性。(2)回环检测可以保证轨迹和地图的全局一致，采用非线性最小二乘进行回环检测需要获取相对位姿作为回环约束，本专利技术采用分枝定界法和预计算栅格获取相对位姿，有效减小了计算量，提高了回环检测效率。附图说明图1为本专利技术回环检测方法流程示意图；图2为本专利技术相对位姿的获取示意图；图3为本本文档来自技高网...

【技术保护点】
1.一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，该方法包括以下步骤：/nS1：获取移动机器人存储的子图；/nS2：获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧-子图对；/nS3：对处于同一地图位置的扫描帧-子图对进行相关性度量和复杂性度量计算；/nS4：基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果。/n

【技术特征摘要】
1.一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，该方法包括以下步骤：
S1：获取移动机器人存储的子图；
S2：获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧-子图对；
S3：对处于同一地图位置的扫描帧-子图对进行相关性度量和复杂性度量计算；
S4：基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果。


2.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S3中，相关性度量的计算包括以下步骤：
S301：获取步骤S2回环检测中的扫描帧-子图对相对位姿，对扫描帧-子图对中的扫描帧进行位姿变换，使得扫描帧与子图对齐；
S302：将对齐后的扫描帧和子图网格化为相等大小的空间区域；
S303：基于网格化的扫描帧和子图，分别构建归一化的扫描帧二维直方图和归一化的子图二维直方图；
S304：基于交叉核函数，计算归一化的扫描帧二维直方图和归一化的子图二维直方图的相关性度量值。


3.根据权利要求2所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S304中，交叉核函数的表达式为：



式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。


4.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S3中，复杂性度量的计算包括以下步骤：
S311：获取扫描帧-子图对中的共有区域；
S312：在共有区域中抛弃离群点，获取内点集合；
S313：基于内点集合中各内点对应的法向量，构建法向量矩阵；
S314：基于法向量矩阵的特征值，计算复杂性度量。


5.根据权利要求4所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S313中，法向量矩阵的构建表达式为：
N＝[n1,n2,...,nn]T
R＝NTN
式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。


6.根据...

【专利技术属性】
技术研发人员：周逊，
申请(专利权)人：同济人工智能研究院苏州有限公司，
类型：发明
国别省市：江苏;32