CN110907947B - 一种移动机器人slam问题中的实时回环检测方法 - Google Patents

一种移动机器人slam问题中的实时回环检测方法 Download PDF

Info

Publication number
CN110907947B
CN110907947B CN201911230320.5A CN201911230320A CN110907947B CN 110907947 B CN110907947 B CN 110907947B CN 201911230320 A CN201911230320 A CN 201911230320A CN 110907947 B CN110907947 B CN 110907947B
Authority
CN
China
Prior art keywords
node
sub
loop detection
scanning frame
mobile robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911230320.5A
Other languages
English (en)
Other versions
CN110907947A (zh
Inventor
周逊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tongji Institute Of Artificial Intelligence Suzhou Co ltd
Original Assignee
Tongji Institute Of Artificial Intelligence Suzhou Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tongji Institute Of Artificial Intelligence Suzhou Co ltd filed Critical Tongji Institute Of Artificial Intelligence Suzhou Co ltd
Priority to CN201911230320.5A priority Critical patent/CN110907947B/zh
Publication of CN110907947A publication Critical patent/CN110907947A/zh
Application granted granted Critical
Publication of CN110907947B publication Critical patent/CN110907947B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

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

Description

一种移动机器人SLAM问题中的实时回环检测方法
技术领域
本发明涉及领域，尤其是涉及一种移动机器人SLAM问题中的实时回环检测方法。
背景技术
移动机器人执行各类任务，如导航、传递、搜索和救援等，一般都需要对其工作环境建构地图，并确定自己的在环境中的位置，这就需要同时定位与地图构建(simultaneouslocalization and mapping,以下简称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中，交叉核函数的表达式为：
Figure BDA0002303353140000021
式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。
进一步地，所述步骤S3中，复杂性度量的计算包括以下步骤：
S311：获取扫描帧-子图对中的共有区域；
S312：在共有区域中抛弃离群点，获取内点集合；
S313：基于内点集合中各内点对应的法向量，构建法向量矩阵；
S314：基于法向量矩阵的特征值，计算复杂性度量。
进一步地，所述步骤S313中，法向量矩阵的构建表达式为：
N＝[n1,n2,...,nn]T
R＝NTN
式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。
进一步地，所述步骤S314中，复杂性度量的计算表达式为：
Figure BDA0002303353140000031
式中，r为复杂性度量值，λ1为扫描帧法向量矩阵的特征值，λ2为子图法向量矩阵的特征值。
进一步地，所述步骤S2中，基于移动机器人存储的子图对扫描帧进行初步回环检测具体为，基于预建立的非线性最小二乘模型，获取扫描帧-子图对，所述非线性最小二乘模型的约束为相对位姿，该相对位姿为在子图坐标系下扫描帧的位姿，所述相对位姿的计算表达式为：
Figure BDA0002303353140000032
式中，{hk}k＝1,2,...,K为扫描帧中的各点，K为点数，
Figure BDA0002303353140000033
Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，/>
Figure BDA0002303353140000037
为搜索区域，该搜索区域基于机器人当前位姿估计确定。
进一步地，因为所述搜索区域比较大，因此所述相对位姿基于分枝定界法获取，分枝定界法将求最优解问题转化为树形搜索问题，该分枝定界法包括以下步骤：
S201：将位姿变换的搜索区域划分为多个节点，计算每个节点的得分并将节点按照得分排序存储到栈中，使得得分最高的节点在栈顶，将最佳得分记为零；
S202：从栈顶获取一个节点，判断该节点得分是否大于最佳得分，若是则执行步骤S203，否则执行步骤S204；
S203：判断该节点是否为叶子节点，若是，则以该节点确定匹配结果和最佳得分，若否，则将该节点分枝为若干节点，计算每个分枝节点的得分并按照得分排序存储到栈中，使得得分最高的节点在栈顶；
S204：去除该节点，重复执行步骤S202，直到栈中无节点。
由于叶子节点才代表实际解，在节点的选择上，本发明采用深度优先搜索，以快速地考察大量叶子节点，找到合适的真实解。
进一步地，所述节点由一组整数参数描述，其表达式如下：
Figure BDA0002303353140000034
其中，ch表述节点c包含叶子节点的数量，cx,cy,cθ表述节点c的位姿信息；
所述节点代表的空间的表达式为：
Figure BDA0002303353140000035
式中，
Figure BDA0002303353140000036
为节点c代表的空间。
对于一个规模ch≥1的节点c，可将其分枝为四个规模为ch-1的子节点：
Figure BDA0002303353140000041
步骤S201中，栈的初始化具体为：
Figure BDA0002303353140000042
Figure BDA0002303353140000043
Figure BDA0002303353140000044
Figure BDA0002303353140000045
其中，
Figure BDA0002303353140000046
需要保证覆盖整个搜索区域。
用节点集
Figure BDA0002303353140000047
对栈进行初始化。
进一步地，所述节点得分计算的表达式为：
Figure BDA0002303353140000048
式中，
Figure BDA0002303353140000049
为基于子图预先计算的栅格地图，该地图中每个像素的取值是原子图中对应像素起始的/>
Figure BDA00023033531400000410
大小的方块内像素取值的最大值。
与现有技术相比，本发明具有以下优点：
(1)本发明在进行回环检测的同时对得到的回环变换进行验证，采用相关性度量和复杂性度量对回环检测结果进行验证，相关性度量描述了扫描帧-子图对共有地形的数量，复杂性度量考察了共有地形是否有足够丰富的特征，两个度量简单且鲁棒的避免回环检测在对称性较强的环境下产生误报、影响全局地图，并且检测与验证两部分是分离的，既保证了精度又保证了实时性。
(2)回环检测可以保证轨迹和地图的全局一致，采用非线性最小二乘进行回环检测需要获取相对位姿作为回环约束，本发明采用分枝定界法和预计算栅格获取相对位姿，有效减小了计算量，提高了回环检测效率。
附图说明
图1为本发明回环检测方法流程示意图；
图2为本发明相对位姿的获取示意图；
图3为本发明实施例1中预计算栅格地图的示意图，其栅格大小从左往右，从上往下依次为1、4、16和64；
图4为本发明实施例1回环检测中相关性度量的获取示意图，(4a)为扫描帧示意图，(4b)为子图示意图，(4c)为归一化的扫描帧二维直方图，(4d)为归一化的子图二维直方图，(4e)为对扫描帧和子图进行回环变换后中的示意图，(4f)为扫描帧-子图对的交叉直方图。
图5为在地形复杂度不同的环境下的候选回环变换对比图，(5a)为一个扫描匹配来说有大量可以分辨特征的地形的回环变换示意图，(5b)为一个匹配较差的地形的回环变换示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
实施例1
本实施例为一种移动机器人SLAM问题中的实时回环检测方法，所谓回环检测简单来讲就是检测机器人是否经过了同一个地方、采集到了同样的环境信息。
如图1所示，本实施例回环检测方法对激光扫描帧和子图进行匹配，采用非线性最小二乘法进行回环优化，获取一个最优的子图，该子图包括组成环境地图的若干局部地图和机器人先前存储的所有扫描帧，首先基于机器人的位姿估计进行匹配，如果获取到足够接近的扫描帧-子图对，则在子图中搜索扫描帧的位置，搜索区域为机器人当前位姿估计附近，最终确定扫描帧相对于子图的位姿，即为相对位姿，将该相对位姿作为约束加入回环检测问题。
为了高效地获得相对位姿，本回环检测方法采用分枝定界法进行搜索，并辅以预计算栅格法进行加速。
为了处理回环检测可能出现的误报，本回环检测方法提出了两个验证度量进行回环验证，这两个验证度量分别为相关性度量和复杂性度量，相关性度量描述扫描帧和子图经回环变换对齐后，共有地形的数量；复杂性度量考察共有地形中是否有长的直线段。当这两个度量值均超过阈值，我们即认为回环变换结果是可信的，否则该回环变换结果不可信，需要重新获取。
下面对本发明非线性最小二乘法、分枝定界法、相关性度量和复杂性度量进行详细描述：
1、非线性最小二乘法
如图2所示，将回环检测问题建模为非线性最小二乘问题，建立非线性最小二乘模型，该非线性最小二乘模型的表达式为：
Figure BDA0002303353140000061
式中，ρ(·)为Huber损失函数，
Figure BDA0002303353140000062
为子图位姿，/>
Figure BDA0002303353140000063
为扫描帧位姿，子图位姿和扫描帧位姿均在同一世界坐标系下，ξij为第j个扫描帧相对于第i个子图的相对位姿，∑ij为相对位姿对应的协方差矩阵，E为残差。(1)式是对多个回环约束进行整体优化，分支定界法等操作用于寻找单个回环约束。
残差E的计算表达式为：
Figure BDA0002303353140000064
Figure BDA0002303353140000065
所述相对位姿为在子图坐标系下扫描帧的位姿，相对位姿的计算表达式为：
Figure BDA0002303353140000066
式中，ξ*指某一个ξij，{hk}k＝1,2,...,K为扫描帧中的各点，
Figure BDA0002303353140000067
Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，/>
Figure BDA0002303353140000068
为搜索区域，搜索区域为机器人当前位姿估计附近。
对子图的描述：若干连续的扫描帧可以用来构建子图M，子图M采用概率栅格的形式，包含分辨率为r的离散栅格点，每个栅格点的取值表示该栅格点被占据的概率，每个栅格点对应的像素包含所有与该栅格点距离最近的环境点。
回环检测是像素级别的匹配，Mnearest是对M的拓展，即将M中每个栅格点的取值拓展至其对应像素。
对搜索区域
Figure BDA00023033531400000615
通过以下方式获得：
Figure BDA0002303353140000069
Figure BDA00023033531400000610
Figure BDA00023033531400000611
Figure BDA00023033531400000612
Figure BDA00023033531400000613
Figure BDA00023033531400000614
Figure BDA0002303353140000071
其中，Wx,Wy,Wθ是需要在使用时进行调试的参数，示例如Wx＝Wy＝7m,Wθ＝30°，ξ0是机器人上一时刻位姿。
2、分枝定界法
回环检测问题中，搜索区域比较大，因此采用分枝定界法对相对位姿进行有效计算，分枝定界法将求最优解问题转化为树型搜索问题，树的根节点表示整个解空间，即搜索区域，对解空间层层划分，进行分枝，重新构建节点，计算节点的得分，利用分枝定界法确定最佳得分和匹配结果。
下面对节点、分枝、计算节点得分和分枝定界法进行具体描述：
1)节点
树中的每个节点由一组整数参数描述：
Figure BDA0002303353140000072
其中，ch表示节点c的规模，为需要由使用者调试的参数，cx,cy,cθ表示节点c的位姿信息。
该节点代表的空间即：
Figure BDA0002303353140000073
叶子节点为ch为零的节点。
2)分枝
将节点分枝为若干节点，具体地，对于一个规模ch≥1的节点c，将其分枝为四个规模为ch-1的子节点，即将c'h＝ch-1，将c'h赋值给ch：
Figure BDA0002303353140000074
Cc表示节点集。
3)分枝定界法
该分枝定界法包括以下步骤：
S201：将位姿的搜索区域划分为多个节点，计算每个节点的得分并将节点按照得分排序存储到栈中，使得得分最高的节点在栈顶，将最佳得分记为零；
S202：从栈顶获取一个节点，判断该节点得分是否大于最佳得分，若是则执行步骤S203，否则执行步骤S204；
S203：判断该节点是否为叶子节点，若是，则以该节点确定匹配结果和最佳得分，若否，则将该节点分枝为若干节点，计算每个分枝节点的得分并按照得分排序存储到栈中，使得得分最高的节点在栈顶；所述叶子节点为一个具体的位姿；
S204：重复执行步骤S202，直到栈中无节点。
分枝定界法的算法流程如表1所示。
表1分枝定界法算法流程表
Figure BDA0002303353140000081
步骤S201中栈的初始化具体为：
Figure BDA0002303353140000082
Figure BDA0002303353140000083
Figure BDA0002303353140000084
Figure BDA0002303353140000085
其中，
Figure BDA0002303353140000086
需要保证覆盖整个搜索区域。
用节点集
Figure BDA0002303353140000087
对栈进行初始化。
4)计算节点得分
确定节点的上界，该上界即为节点得分。
确定上界基于如下思路：
Figure BDA0002303353140000088
Figure BDA0002303353140000089
Figure BDA00023033531400000810
其中，
Figure BDA00023033531400000811
式(9)即意味着节点c的得分是以其为根节点的子树的得分的上界。为了加速式(7)的计算，Mnearest采用预先计算的栅格地图/>
Figure BDA00023033531400000812
其中每个像素的取值是原子图中对应像素起始的/>
Figure BDA0002303353140000091
大小的方块内像素取值的最大值。这样，计算得分的计算量可以控制在与扫描点的数量成线性，我们将得分函数作如下改进：
Figure BDA0002303353140000092
Figure BDA0002303353140000093
Figure BDA0002303353140000094
是在获取子图是就计算并储存的。
如图3所示，为栅格大小从左往右，从上往下依次为1、4、16和64的预计算栅格地图的示意图。
3、相关性度量
在一些对称性较强的室内环境中，回环检测可能出现误报(false-positive)，所以本回环检测方法采用两个与回环变换计算进程独立的度量来鲁棒地验证回环可靠性。
相关性度量描述扫描帧-子图对共有地形的数量。如前所述，回环检测得到扫描帧在子图中的位姿，于是可以将帧进行旋转变换与子图对齐。下一步，将其分割成相等大小的空间区域，构建一对归一化的二维直方图。最后使用交叉核函数对直方图进行关联，交叉核函数的表达式为：
Figure BDA0002303353140000095
式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。
如图4所示，示意了一个计算相关性度量的例子，(4a)和(4c)是一个扫描帧-子图对，而(4b)和(4d)分别显示了归一化的2D直方图，这些直方图由扫描点的空间分块生成。(4e)展示了一个可能的回环变换。由式(10)获得的交叉直方图，见(4f)。通过对交叉直方图进行求和计算，我们得到了相关性得分结果c＝0.6634。
4、复杂性度量
复杂性度量考察的是共有地形是否有足够丰富的特征。在共有区域中抛弃离群点，得到内点集合Df，将内点对应的法向量写入矩阵：
N＝[n1,n2,...,nn]T (11)
R＝NTN (12)
式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。
复杂性度量的计算表达式为：
Figure BDA0002303353140000101
式中，r为复杂性度量值，λ1和λ2均为法向量矩阵的特征值。
假定λ1≤λ2。当环境条件对于扫描匹配来说较差时，比如在一个长的而且缺少特征的走廊上，扫描点的法向量几乎全部沿着墙面的法向量。这个观测结果意味着λ1→0，进而r→0。反之，当环境条件较好，比如在一个有很多角落的复杂环境中，我们有λ1≈λ2，因此r→1。
图5展示了在地形复杂度不同的环境下的候选回环变换。(5a)示意的是一个复杂的环境，对于扫描匹配来说有大量可以分辨的特征，复杂性得分r＝0.4979。(5b)展示了一个对于扫描匹配来说较差的地形，较长的平行线具有主要沿着单个轴的法向量，重叠地形的比例主要由长平行线决定。复杂性得分结果为r＝0.1114，远小于(5a)所示的情况。
实施例2
本实施例为一种移动机器人SLAM问题中的实时回环检测方法，该回环检测方法对激光扫描帧和子图进行匹配，采用预建立的非线性最小二乘模型获取一个最优的子图，该子图包括组成环境地图的若干局部地图和机器人先前存储的所有扫描帧。该方法包括以下步骤：
S1：获取移动机器人存储的子图；
S2：获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧-子图对；
S3：对处于同一地图位置的扫描帧-子图对进行相关性度量和复杂性度量计算；
S4：基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果。
步骤S2中，非线性最小二乘模型的表达式为：
Figure BDA0002303353140000102
Figure BDA0002303353140000103
Figure BDA0002303353140000111
Figure BDA0002303353140000112
式中，
Figure BDA0002303353140000113
为子图位姿，/>
Figure BDA0002303353140000114
为扫描帧位姿，子图位姿和扫描帧位姿均在同一世界坐标系下，ξij为相对位姿，∑ij为相对位姿对应的协方差矩阵，E为残差；
残差E的计算表达式为：
Figure BDA0002303353140000115
Figure BDA0002303353140000116
相对位姿为在子图坐标系下扫描帧的位姿，相对位姿的计算表达式为：
Figure BDA0002303353140000117
式中，
Figure BDA0002303353140000118
为扫描帧中的各点，/>
Figure BDA0002303353140000119
Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，/>
Figure BDA00023033531400001116
为搜索区域，搜索区域为机器人当前位姿估计附近。
非线性最小二乘模型的约束为相对位姿，该相对位姿为在子图坐标系下扫描帧的位姿，相对位姿的计算表达式为：
Figure BDA00023033531400001110
式中，{hk}k＝1,2,...,K为扫描帧中的各点，
Figure BDA00023033531400001111
Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，/>
Figure BDA00023033531400001112
为搜索区域，搜索区域基于机器人当前位姿估计确定，可为机器人当前位姿估计附近。
相对位姿基于分枝定界法获取，该分枝定界法包括以下步骤：
S201：将位姿的搜索区域划分为多个节点，计算每个节点的得分并将节点按照得分排序存储到栈中，使得得分最高的节点在栈顶，将最佳得分记为零；
节点由一组整数参数描述，其表达式如下：
Figure BDA00023033531400001113
所述节点代表的空间的表达式为：
Figure BDA00023033531400001114
式中，
Figure BDA00023033531400001115
为节点c代表的空间。
节点得分计算的表达式为：
Figure BDA0002303353140000121
Figure BDA0002303353140000122
Figure BDA0002303353140000123
式中，
Figure BDA0002303353140000124
Mnearest为子图中各像素的取值，该子图采用预先计算的栅格地图/>
Figure BDA0002303353140000125
该地图中每个像素的取值是原子图中对应像素起始的/>
Figure BDA0002303353140000126
大小的方块内像素取值的最大值。
S202：从栈顶获取一个节点，判断该节点得分是否大于最佳得分，若是则执行步骤S203，否则执行步骤S204；
S203：判断该节点是否为叶子节点，若是，则以该节点确定匹配结果和最佳得分，若否，则将该节点分枝为若干节点，计算每个分枝节点的得分并按照得分排序存储到栈中，使得得分最高的节点在栈顶；
叶子节点为一个具体的位姿，叶子节点的ch为零；
将节点分枝为若干节点具体为将节点的中的ch参数减一，使得该节点分枝为四个节点。
S204：重复执行步骤S202，直到栈中无节点。
步骤S3具体为，在一些对称性较强的室内环境中，回环检测可能出现误报(false-positive)，所以本回环检测方法采用两个与回环变换计算进程独立的度量来鲁棒地验证回环可靠性。该度量包括：
1、相关性度量
相关性度量描述帧-子图对共有地形的数量。相关性度量的计算包括以下步骤：
S301：获取步骤S2回环检测中的扫描帧-子图对相对位姿，对扫描帧-子图对中的扫描帧进行位姿变换，使得扫描帧与子图对齐；
S302：将对齐后的扫描帧和子图分割成相等大小的空间区域；
S303：基于相等大小的扫描帧和子图，分别构建归一化的扫描帧二维直方图和归一化的子图二维直方图；
S304：基于交叉核函数，计算归一化的扫描帧二维直方图和归一化的子图二维直方图的相关性度量值。
交叉核函数的表达式为：
Figure BDA0002303353140000127
式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。
2、复杂性度量
复杂性度量考察的是共有地形是否有足够丰富的特征。复杂性度量的计算包括以下步骤：
S311：获取扫描帧-子图对中的共有区域；
S312：在共有区域中抛弃离群点，获取内点集合；
S313：基于内点集合中各内点对应的法向量，构建法向量矩阵；
S314：基于法向量矩阵的特征值，计算复杂性度量。
步骤S313中，法向量矩阵的构建表达式为：
N＝[n1,n2,...,nn]T
R＝NTN
式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。
步骤S314中，复杂性度量的计算表达式为：
Figure BDA0002303353140000131
式中，r为复杂性度量值，λ1和λ2均为法向量矩阵的特征值。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (8)

1.一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，该方法包括以下步骤：
S1：获取移动机器人存储的子图；
S2：获取移动机器人的当前扫描帧，基于移动机器人存储的子图对当前扫描帧进行初步回环检测，获取处于同一环境地图位置的扫描帧-子图对；
S3：对处于同一地图位置的扫描帧-子图对进行相关性度量和复杂性度量计算；
S4：基于相关性度量和复杂性度量的计算结果，获得最终回环检测结果；
所述步骤S3中，相关性度量的计算包括以下步骤：
S301：获取步骤S2回环检测中的扫描帧-子图对相对位姿，对扫描帧-子图对中的扫描帧进行位姿变换，使得扫描帧与子图对齐；
S302：将对齐后的扫描帧和子图网格化为相等大小的空间区域；
S303：基于网格化的扫描帧和子图，分别构建归一化的扫描帧二维直方图和归一化的子图二维直方图；
S304：基于交叉核函数，计算归一化的扫描帧二维直方图和归一化的子图二维直方图的相关性度量值；
所述步骤S3中，复杂性度量的计算包括以下步骤：
S311：获取扫描帧-子图对中的共有区域；
S312：在共有区域中抛弃离群点，获取内点集合；
S313：基于内点集合中各内点对应的法向量，构建法向量矩阵；
S314：基于法向量矩阵的特征值，计算复杂性度量。
2.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S304中，交叉核函数的表达式为：
Figure FDA0004207309910000011
式中，h1(i.j)为归一化的扫描帧二维直方图中第i行第j列的区域，h2(i,j)为归一化的子图二维直方图中第i行第j列的区域，若两直方图中对应的同一区域相同，则该区域度量值为1，若两直方图中对应的同一区域不同，则该区域度量值小于1，且差异越大，度量值越小。
3.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S313中，法向量矩阵的构建表达式为：
N＝[n1,n2,...,nn]T
R＝NTN
式中，R为法向量矩阵，ni为第i个内点对应的法向量，1≤i≤n。
4.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S314中，复杂性度量的计算表达式为：
Figure FDA0004207309910000021
式中，r为复杂性度量值，λ1为扫描帧法向量矩阵的特征值，λ2为子图法向量矩阵的特征值。
5.根据权利要求1所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述步骤S2中，基于移动机器人存储的子图对扫描帧进行初步回环检测具体为，基于预建立的非线性最小二乘模型，获取扫描帧-子图对，所述非线性最小二乘模型的约束为相对位姿，该相对位姿为在子图坐标系下扫描帧的位姿，所述相对位姿的计算表达式为：
Figure FDA0004207309910000022
式中，{hk}k＝1,2,...,K为扫描帧中的各点，K为点数，
Figure FDA0004207309910000023
Tξ为扫描帧的位姿变换，Mnearest为子图中各像素的取值，/>
Figure FDA0004207309910000024
为搜索区域，该搜索区域基于机器人当前位姿估计确定。
6.根据权利要求5所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述相对位姿基于分枝定界法获取，该分枝定界法包括以下步骤：
S201：将位姿变换的搜索区域划分为多个节点，计算每个节点的得分并将节点按照得分排序存储到栈中，使得得分最高的节点在栈顶，将最佳得分记为零；
S202：从栈顶获取一个节点，判断该节点得分是否大于最佳得分，若是则执行步骤S203，否则执行步骤S204；
S203：判断该节点是否为叶子节点，若是，则以该节点确定匹配结果和最佳得分，若否，则将该节点分枝为若干节点，计算每个分枝节点的得分并按照得分排序存储到栈中，使得得分最高的节点在栈顶；
S204：去除该节点，重复执行步骤S202，直到栈中无节点。
7.根据权利要求6所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述节点由一组整数参数描述，其表达式如下：
Figure FDA0004207309910000031
其中，ch表述节点c包含叶子节点的数量，cx,cy,cθ表述节点c的位姿信息；
所述节点代表的空间的表达式为：
Figure FDA0004207309910000032
式中，
Figure FDA0004207309910000033
为节点c代表的空间。
8.根据权利要求6所述的一种移动机器人SLAM问题中的实时回环检测方法，其特征在于，所述节点得分计算的表达式为：
Figure FDA0004207309910000034
式中，
Figure FDA0004207309910000035
为基于子图预先计算的栅格地图，该地图中每个像素的取值是原子图中对应像素起始的/>
Figure FDA0004207309910000036
大小的方块内像素取值的最大值。
CN201911230320.5A 2019-12-04 2019-12-04 一种移动机器人slam问题中的实时回环检测方法 Active CN110907947B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911230320.5A CN110907947B (zh) 2019-12-04 2019-12-04 一种移动机器人slam问题中的实时回环检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911230320.5A CN110907947B (zh) 2019-12-04 2019-12-04 一种移动机器人slam问题中的实时回环检测方法

Publications (2)

Publication Number Publication Date
CN110907947A CN110907947A (zh) 2020-03-24
CN110907947B true CN110907947B (zh) 2023-06-27

Family

ID=69822654

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911230320.5A Active CN110907947B (zh) 2019-12-04 2019-12-04 一种移动机器人slam问题中的实时回环检测方法

Country Status (1)

Country Link
CN (1) CN110907947B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111540005B (zh) * 2020-04-21 2022-10-18 南京理工大学 基于二维栅格地图的回环检测方法
CN111680747B (zh) * 2020-06-08 2023-09-01 北京百度网讯科技有限公司 用于占据栅格子图的闭环检测的方法和装置
CN111932600B (zh) * 2020-07-31 2023-10-03 杭州电子科技大学 一种基于局部子图的实时回环检测方法
CN112179353B (zh) * 2020-09-30 2023-07-18 深圳银星智能集团股份有限公司 自移动机器人的定位方法、装置、机器人及可读存储介质
CN112241002B (zh) * 2020-10-11 2022-10-18 西北工业大学 一种基于Karto SLAM的鲁棒闭环检测新方法
CN112668653A (zh) * 2020-12-30 2021-04-16 北京百度网讯科技有限公司 基于激光雷达地图的回环检测方法、装置、设备及介质
CN113409410B (zh) * 2021-05-19 2024-04-02 杭州电子科技大学 一种基于3d激光雷达的多特征融合igv定位与建图方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109766758A (zh) * 2018-12-12 2019-05-17 北京计算机技术及应用研究所 一种基于orb特征的视觉slam方法
CN109945856B (zh) * 2019-02-18 2021-07-06 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN110146080B (zh) * 2019-04-24 2024-01-19 佛山科学技术学院 一种基于移动机器人的slam回环检测方法及装置

Also Published As

Publication number Publication date
CN110907947A (zh) 2020-03-24

Similar Documents

Publication Publication Date Title
CN110907947B (zh) 一种移动机器人slam问题中的实时回环检测方法
US8792726B2 (en) Geometric feature extracting device, geometric feature extracting method, storage medium, three-dimensional measurement apparatus, and object recognition apparatus
JP4079690B2 (ja) 物体追跡装置及び方法
US8340400B2 (en) Systems and methods for extracting planar features, matching the planar features, and estimating motion from the planar features
CN107292234B (zh) 一种基于信息边缘和多模态特征的室内场景布局估计方法
Grisetti et al. Robust optimization of factor graphs by using condensed measurements
US10288425B2 (en) Generation of map data
US11790542B2 (en) Mapping and localization system for autonomous vehicles
US9189709B2 (en) Information processing apparatus and information processing method
US9651388B1 (en) System and method for improved simultaneous localization and mapping
CN115267796B (zh) 定位方法、装置、机器人和存储介质
CN110986956A (zh) 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN114427866A (zh) 路径规划方法、电子设备以及存储介质
CN113759928B (zh) 用于复杂大尺度室内场景的移动机器人高精度定位方法
Xia et al. Semiautomatic construction of 2-D façade footprints from mobile LIDAR data
CN112733971B (zh) 扫描设备的位姿确定方法、装置、设备及存储介质
JP2008224641A (ja) カメラ姿勢推定システム
CN111024063B (zh) 一种基于星点重提取的大机动条件下星图识别算法
CN115307641A (zh) 机器人定位方法、装置、机器人和存储介质
Yun et al. Using self-organizing map for road network extraction from ikonos imagery
Chen et al. Towards bio-inspired place recognition over multiple spatial scales
Kaneko et al. Stata Center Frame: A Novel World Assumption for Self-Localization
Matsui et al. Automatic feature point selection through hybrid metaheauristics based on Tabu search and memetic algorithm for augmented reality
Pyo et al. Development of radial layout underwater acoustic marker using forward scan sonar for AUV
CN113610700B (zh) 一种全域综合整治的图斑的自动配准方法、装置及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant