CN109358316A - 基于结构单元编码和多假设跟踪的线激光全局定位方法 - Google Patents

Abstract

基于结构单元编码和多假设跟踪的线激光全局定位方法。针对利用线激光为移动机器人提供全局定位的任务，本发明提出一个新颖的基于SUES(结构单元编码机制，structural unit encoding scheme)和扩展MHT(多假设跟踪,multiple hypothesis tracking)的全局定位方法。首先通过SUES对有向端点特征进行编码；再利用扩展MHT逐步恢复全局位姿。相比现有方法，本发明所提的SUES无需离线训练，不依赖于词典质量；同时，所提的扩展MHT方法具有收敛更快和不依赖特征数量的优点。实验结果表明，相比于自适应蒙特卡洛定位，本发明具有更高的成功率和效率。

Description

基于结构单元编码和多假设跟踪的线激光全局定位方法

技术领域

本发明属于环境辨识和移动机器人定位的技术领域，特别是涉及一种基于结构单元编码和多假设跟踪的线激光全局定位方法。

背景技术

SLAM(同时定位与建图，simultaneous localization and mapping)作为智能机器人领域不可或缺的关键技术，通过机载传感器构建周围环境地图同时给机器人提供定位信息[1]。借助目前的SLAM技术，可以获得环境地图；在获得先验地图的基础上，通过视觉或者激光传感器为移动机器人提供鲁棒定位从而完成复杂任务，是当前移动机器人不可或缺的功能[2]。然而定位问题不仅仅包含位姿跟踪，在很多情况下需要解决没有初始位姿下的定位问题，比如发生绑架，这就是所谓的全局定位问题。相比于位姿跟踪，全局定位问题更具有挑战性，因为需要解决环境辨识的问题，并且在整个环境中可能存在多个相似的场景。

在全局定位的过程中，解决环境辨识和表达是至关重要的一步。在视觉领域，Galvez-López等[3]提出了一种新颖的词袋模型环境辨识方法，用于回环检测。之后，在文献[4]中，Mur-Artal等人提出了将词袋模型用于回环检测的ORB-SLAM。文献[5]提出利用全景gist(梗概)描述符来解决城市的定位问题。不同于视觉传感器，线激光可以提供更稳定的环境信息以及更大的观测视野，从而被大量用于环境辨识。Tipaldi等人[6]提出第一种激光特征—FLIRT(快速激光感兴趣区域变换，Fast Laser Interest Region Transform)，被成功应用于环境辨识。文献[7]利用FLIRT特征和弱化的结构检验解决了大场景下的环境辨识问题。Deray等人[8]提出一种新颖的回环检测方法，该方法结合了改进的BoW机制和HMM(隐马尔可夫模型，Hidden Markov Model)。上述方法仍需要离线训练产生词典。

全局定位可分为基于滤波和基于特征两种方法，都被广泛应用于移动机器人领域。在文献[9]中，Martín等人提出一种基于KLD(KL散度，Kullback-Leibler divergence)的全局定位方法；Fox[10]提出了一种AMCL(自适应蒙特卡洛定位，adaptive Monte Carlolocalization)方法，该方法通过利用KLD采样提高了粒子滤波的效率。不同于传统MCL方法，Valencia等人在文献[11]中提出了NDT-MCL(正态分布变换-蒙特卡洛定位方法，NormalDistribution Transform Monte Carlo Localisation)，在较少的粒子和更快的收敛速度下获得更好的性能。上述基于滤波的方法，不需要对当前环境进行辨识，通过粒子收敛达到全局定位的效果，但粒子收敛导致较低的效率，同时在相似区域容易引发收敛错误。Tully等人[12]将混合地图表达技术与MHT相结合，提出了一种统一的机器人全局定位滤波框架。He等人[13]利用LRM(线段关系匹配，Line-segment Relation Matching)提出了一种移动机器人全局定位算法。在文献[14]中，Liu等人提出了一种可行的房间级定位解决方案，通过结构化核稀疏编码模型实现。Park等人在文献[15]中提出一种基于支持向量机(SVM)的位置识别和粒子滤波的全局定位方法，该研究方案通过离线训练提高了全局定位的效率。

虽然上述方法取得了很大的进展，但对全球定位的研究还远未成熟。对于基于滤波器的方法，通常需要大量粒子来实现全局定位，这导致较高的时间开销和存储成本以及低收敛性能。此外，基于滤波的方法在具有许多相似局部场景的环境中极有可能收敛到错误的结果。对于基于特征的方法，特征类型和描述符的选择非常重要，通常需要离线训练产生词典用于描述环境。在动态环境中尤其具有挑战性，因为特征可能出现在动态对象上，导致数据关联困难。总结文献可以得知，现有方法或者在环境辨识，或者在全局定位的成功率和效率上存在欠缺，难以实际应用。

发明内容

本发明的目的是解决现有全局定位技术存在的上述不足，提供一种基于结构单元编码和多假设跟踪的线激光全局定位方法。

本发明提出了一种新颖的基于结构单元编码和多假设跟踪的线激光全局定位方法。该方法最大的特点包括不需要离线训练产生词典，因而避免了离线词典的质量不佳影响全局定位效果。其次，本发明提出了一种扩展多假设跟踪算法，区别传统多假设跟踪算法，将独立观测作为先验信息，并将连续的候选位姿和里程计增量作为似然信息，相比于传统MHT方法，具有收敛速度更快和不依赖于特征数量的优点。具体而言，首先通过线段特征提取获得有向端点特征，然后通过结构单元描述有向端点特征之间的几何关系；之后，利用所提出的SUES对结构单元进行编码获得激光数据的特征向量，与先验地图匹配得到候选关键帧和位姿信息；最后通过所提出的扩展MHT逐步恢复全局位姿。整体算法流程如附图1所示。通过公测集和自录数据集的实验结果表明，相比于AMCL(自适应蒙特卡洛定位,adaptive Monte Carlo localization)，本发明具有更高的成功率和效率。

本发明提供的基于结构单元编码和多假设跟踪的线激光全局定位方法包括：

第1，利用SUES对有向端点特征之间的几何关系进行编码

第1.1构造产生结构单元

对激光数据进行线段特征提取之后，整个环境能够由有向端点特征进行描述(具体可以见附图2)。定义P＝{p₁,...,p_N}为有向端点特征点集，其中N表示特征点数量。第i个有向端点特征以p_i＝[x_i y_i θ_i]^T表示，其中(x_i,y_i)定义为特征位置，θ_i定义为特征方向。同一线段两侧的有向端点特征的方向相差180°。定义ρ_j为特征点p_l＝[x_l y_l θ_l]^T与特征点p_n＝[x_n y_n θ_n]^T之间的欧式距离，其中j为索引编号，

当满足ρ_j≤ξ时，定义结构单元s_j＝{ρ_j,θ_j1,θ_j2}表示特征点p_l和p_n之间的几何关系(具体描述可以见附图3)，其中ξ为给定阈值，θ_j1和θ_j2为特征点p_l和p_n之间的方向信息，计算如下：

其中θ′_l,θ′_n为方向信息，位于区间[0,360°)。定义S＝{s₁,…,s_M}为构造得到的结构单元集合，其中M定义为结构单元数量，根据上述构造方式获得。这样的结构单元构造方式在保证旋转不变性的同时也能区分相似结构，如附图4所示，四类结构单元具有相同的欧式距离，但是仍能通过角度信息区分这四类相似结构。

第1.2通过SUES对结构单元进行编码

本发明提出一种新颖的编码机制SUES(结构单元编码机制，structural unitencoding scheme)，用于对上述得到的结构单元进行编码。SUES不需要先验数据的离线训练产生词典，同时SUES的性能也不依赖于词典质量。定义δ_a和δ_d分别为角度分辨率和距离分辨率，将整个角度空间和距离空间以上述分辨率划分成小栅格。在此基础上，结构单元可以通过角度和距离对应的特定栅格进行描述，然而这样的硬编码机制容易将分割线附近的角度和距离划分到不同的栅格。如附图5所示，本发明提出一种软编码机制，角度θ能够引起分割线1·δ_a和2·δ_a的响应，同样距离ρ也能够引起同样的效果。定义w_i为由2个角度分割线和1个距离分割线组合形成的结构单词，其中i为索引值，总共能够产生L＝b²d个结构单词，其中采用基于分割线的软投票机制，将产生的结构单元s_j＝{ρ_j,θ_j1,θ_j2}进行编码，首先根据软投票机制产生的分割线计算如下：

其中φ_i(i＝1,...,6)为具体分割线索引值。然后根据距离分割线的距离大小决定每个分割线的权重，分割线所对应的权重计算如下：

其中ω_φi(i＝1,...,6)为分割线所对应的权重。最后通过分割线之间的不同组合，每个结构单元s_j＝{ρ_j,θ_j1,θ_j2}可以产生8个结构单词，每个结构单词对应的索引值和权重如下表1所示，其中a_ij表示对应结构单词w_i在结构单元s_j的权重，i为结构单词在词典中的索引值。

在第k时刻激光数据中，会存在多个结构单元，产生多个结构单词，因此定义a_ijk为第k时刻激光数据中结构单词w_i在结构单元s_j中的权重，a_ik则表示结构单词w_i在第k时刻激光数据的权重，a_ik计算如下：

表1.结构单元s_j对应的8个结构单词

如是将所有结构单元进行编码再归一化，就能够得到第k时刻激光数据所对应的特征向量，计算如下：

因此，定义特征向量v_k＝[tf_1k,tf_2k,...,tf_Lk]^T(L为结构单词数量)用于描述第k时刻激光数据的结构信息。之后可以利用下式求取特征向量v₁和特征向量v₂之间的相似度，计算如下：

第2，通过扩展MHT逐步恢复全局位姿

第2.1建立多假设概率模型

通过SUES对当前激光数据进行单元结构特征编码，然后通过相似度计算能够从离线位姿地图中获取候选关键帧并得到候选全局位姿。区别于传统MHT方法，本发明将独立观测获得的候选全局位姿信息作为先验项，将连续的候选位姿和里程计增量信息作为似然项。所提出的扩展MHT(多假设跟踪,multiple hypothesis tracking)方法具有以下两个显著的优点：(1)收敛速度更快；(2)候选位姿的获得独立于特征数量。关于k时刻机器人位姿序列的概率分布模型表示如下：

p(X_1:k|u_1:k-1,z_1:k,m), (10)

其中u_1:k-1定义为里程计信息序列；z_1:k表示观测序列，同时X_1:k和m分别表示候选机器人位姿序列和先验地图。为了减少计算量，整个分布使用如下递归进行计算：

p(X_1:k|u_1:k-1,z_1:k,m)＝p(X_k|u_k-1,z_k,m,X_1:k-1)·p(X_1:k-1|u_1:k-2,z_1:k-1,m), (11)

其中p(X_1:k-1|u_1:k-2,z_1:k-1,m)表示k-1时刻的位姿序列概率分布，p(X_k|u_k-1,z_k,m,X_1:k-1)表示k时刻机器人位姿的概率分布。具体推导如下：

利用贝叶斯规则对p(X_k|u_k-1,z_k,m,X_1:k-1)进行展开，得到如下：

其中p(u_k-1|X_k-1,X_k)和p(X_k|z_k,m)分别表示似然项和先验项，p(u_k-1|z_k,m,X_1:k-1)为归一化项。具体推导如下：

能够得到概率分布函数正比于似然函数：p(X_k|u_k-1,z_k,m,X_1:k-1)∝p(u_k-1|X_k-1,X_k)。

第2.2利用里程计模型进行似然估计

利用里程计模型能够得到两个连续位姿之间的增量参数，包括两个角度和一个距离。如附图6所示，定义δ′_rot1、δ′_tran和δ′_rot2为连续候选全局位姿X_k-1,X_k的增量参数；定义δ_rot1、δ_tran和δ_rot2为里程计信息u_k-1的增量参数。利用里程计模型获得的增量参数之间相互独立，因此p(u_k-1|X_k-1,X_k)∝p₁·p₂·p₃，其中

其中，α_i(i＝1,...,4)为里程计模型中的噪声参数，prob()为正态分布函数。机器人候选位姿序列的概率分布模型通过独立观测和里程计模型进行计算。在实际操作的时候，每一次更新概率分布之后，都要检测去除概率小于特定阈值的位姿序列，直至剩下唯一一个位姿序列对应全局定位结果。整个扩展MHT框架如附图7所示。

本发明的优点和有益效果

本发明提出了一种基于结构单元编码和多假设跟踪的线激光全局定位方法。所提出的结构单元编码机制SUES不需要离线训练产生词典，避免了离线词典的质量不佳影响全局定位效果。其次，本发明提出了一种扩展MHT方法，相比于传统MHT方法，具有收敛速度更快和不依赖于特征数量的优点。对比实验结果表明，相比于AMCL，本发明具有更高的成功率和效率。

附图说明：

图1为整体算法流程；

图2有向端点特征表达环境示意图。通过线段特征提取获得有向端点特征，环境能够使用稀疏的特征进行描述，有向端点特征在图中以箭头和方块进行表示；

图3为结构单元构造示意图。构造产生的结构单元包含两个角度和一个距离信息；

图4区别相似结构单元示意图。(a)、(b)、(c)和(d)分别表示相似结构单元；

图5角度空间和距离空间分割示意图。其中(a)表示角度空间在δ_a分辨率下进行分割，(b)表示距离空间在δ_d分辨率下进行分割；

图6里程计模型示意图。其中(a)和(b)分别表示连续的独立观测和里程计信息在时刻k-1和时刻k之间的增量模型；

图7为扩展MHT框架示意图；

图8实验***与环境；其中(a)和(b)分别表示实验***和实验环境；

图9本发明所提的全局定位方法在数据集Intel-sorted-01中的全局定位示意图。图中黑色三角形为候选机器人位姿，三角形之间的连线为位姿之间的合理连接关系；子图(a)-(d)表示了整个全局定位过程；

图10 AMCL全局定位示意图。子图(a)-(d)表示了整个全局定位过程；

图11关于数据集Intel-sorted-01全局定位效率对比示意图；

图12关于自录数据集全局定位效率对比示意图。

具体实施方式：

实施例1：

第1，利用SUES对有向端点特征之间的几何关系进行编码

第1.1构造产生结构单元

在全局定位具体实施过程中，以室内走廊环境为例(如附图8(b)所示)。首先对激光数据进行线段特征提取，整个环境能够由有向端点特征进行描述(具体可以见附图2)。定义P＝{p₁,...,p_N}为特征点集，其中N表示特征点数量。第i个有向端点特征以p_i＝[x_i y_iθ_i]^T表示，其中(x_i,y_i)定义为特征位置，θ_i定义为特征方向。ρ₁为特征点p₁＝[1m 2m 0°]^T与特征点p₂＝[2m 1m 270°]^T之间的欧式距离，将ξ定为5m,满足ρ₁≤ξ条件，得到结构单元s₁＝{ρ₁,θ₁₁,θ₁₂}，结构单元中方向信息计算如下：

θ′₁＝atan2(1-2,2-1)-0°,

θ′₂＝atan2(2-1,1-2)-270°,

θ₁₁＝max(θ′₁,θ′₂)＝315°,

θ₁₂＝min(θ′₁,θ′₂)＝225°,

其中需要将角度信息θ′₁,θ′₂归一化到区间[0,360°)，得到通过这样的构造方式，得到激光数据对应的结构单元集合S＝{s₁,…,s_M}。

第1.2通过SUES对结构单元进行编码

将角度分辨率δ_a和距离分辨率δ_d分别设置为20°和0.5m，然后将整个角度空间和距离空间以上述分辨率划分成小栅格(如附图5所示)。定义w_i为由2个角度分割线和1个距离分割线组合形成的结构单词，其中i为索引值；然后计算分割线数量， 总共能够产生L＝b²d＝3564个结构单词。采用基于分割线的软投票机制，将产生的结构单元进行编码，首先根据软投票机制产生的分割线计算如下：

然后根据距离分割线的距离大小决定每个分割线的权重，分割线所对应的权重计算如下：

最后通过分割线之间的不同组合，结构单元能够产生8个结构单词，每个结构单词对应的索引值和权重如下表2所示。

表2.每个结构单词对应的索引值和权重

在第k时刻激光数据中，会存在多个结构单元，产生多个结构单词，因此定义a_ijk为第k时刻激光数据中结构单词w_i在结构单元s_j中的权重，a_ik则表示结构单词w_i在第k时刻激光数据的权重，a_ik计算如下：

a_ik＝Σ_ja_ijk. (7)

如是将所有结构单元进行编码再归一化，就能够得到第k时刻激光数据所对应的特征向量，计算如下：

因此根据公式(8)，得到特征向量v_k＝[tf_1k,tf_2k,...,tf_Lk]^T(L为结构单词数量)用于描述第k时刻激光数据的结构信息。之后可以利用下式求取v₁和v₂的相似度：

第2，通过扩展MHT逐步恢复全局位姿

第2.1建立多假设概率模型

通过SUES对当前激光数据进行单元结构特征编码，然后通过相似度计算能够从离线位姿地图中获取候选关键帧并得到候选全局位姿。区别于传统MHT方法，本发明所提的扩展MHT方法将独立观测获得的候选全局位姿信息作为先验项，将连续的候选位姿和里程计增量信息作为似然项。具有以下两个显著的优点：(1)收敛速度更快；(2)候选位姿的获得独立于特征数量。关于k时刻机器人位姿序列的概率分布模型表示如下：

p(X_1:k|u_1:k-1,z_1:k,m), (10)

其中u_1:k-1定义为里程计信息序列；z_1:k表示观测序列，同时X_1:k和m分别表示候选机器人位姿序列和先验地图。为了减少计算量，整个分布使用如下递归进行计算：

p(X_1:k|u_1:k-1,z_1:k,m)＝p(X_k|u_k-1,z_k,m,X_1:k-1)·p(X_1:k-1|u_1:k-2,z_1:k-1,m), (11)

其中p(X_1:k-1|u_1:k-2,z_1:k-1,m)表示上一时刻的位姿序列概率分布，p(X_k|u_k-1,z_k,m,X_1:k-1)表示当前机器人位姿的概率分布。

利用贝叶斯规则对p(X_k|u_k-1,z_k,m,X_1:k-1)进行展开，得到如下：

其中p(u_k-1|X_k-1,X_k)和p(X_k|z_k,m)分别表示似然项和先验项，p(u_k-1|z_k,m,X_1:k-1)为归一化项；能够得到概率分布函数正比于似然函数p(X_k|u_k-1,z_k,m,X_1:k-1)∝p(u_k-1|X_k-1,X_k)。

第2.2利用里程计模型进行似然估计

利用里程计模型能够得到两个连续位姿之间的增量参数，包括两个角度和一个距离。如附图6所示，定义δ′_rot1，δ′_tran和δ′_rot2为连续候选全局位姿X_k-1,X_k的增量参数；定义δ_rot1，δ_tran和δ_rot2为里程计信息u_k-1的增量参数。利用里程计模型获得的增量参数之间相互独立，因此p(u_k-1|X_k-1,X_k)∝p₁·p₂·p₃，其中

其中，α_i(i＝1,...,4)为里程计模型中的噪声参数，prob()为正态分布函数。机器人候选位姿序列的概率分布模型通过独立观测和里程计模型进行计算。在实际操作的时候，每一次更新概率分布之后，都要检测去除概率小于特定阈值的位姿序列，直至剩下唯一一个位姿序列对应全局定位结果。整个扩展MHT框架如附图7所示。

第3，实验效果描述

为验证上述基于结构单元编码和扩展多假设跟踪的全局定位方法的有效性，本发明使用两个公开可用的数据集(Intel-sorted-01 and seattle)和同一走廊(hallway)录制的数据集进行了实验验证。实验***由两轮移动机器人(Pioneer3-DX)和2D激光传感器(ODM30MR2000-B23)组成，如附图8(a)所示。对比实验包括MHT+SUES、AMCl+SUES和原始AMCL三种方法，每种方法都使用成功率和效率进行评估。所提全局定位方法在不同环境下的***参数如表3所示。所提出的方法是用C/C++编程的，所用计算机的软硬件规格如列出的表4所示。

表3.***参数

表4.软硬件规格

第3.1，公测集全局定位结果

数据集Intel-sorted-01的传感数据从Intel Jones Farms Campus,Oregon获得。它是一个典型的办公室结构，大小为40m×40m，包含有许多相似的场景。实验从8030个原始激光扫描帧中选取162个关键帧作为离线位姿地图进行全局定位测试。

所提出的全局定位方法的过程如附图9(a)-(d)所示。在附图9(a)中，通过第一次激光扫描，得到十一个初始候选位姿序列；在此基础上，通过第二次观测修剪三个候选位姿序列，如附图9(b)；在附图9(c)中，我们只修剪了一组候选位姿序列；通过第四次观测，去除了六个候选位姿序列，得到了正确的位姿序列。与所提出的方法不同，附图10(a)-(d)表示出AMCL进行全局定位的过程。在附图10(a)中，在全局定位开始时，在整位姿空间上随机且均匀地获得大量的位姿粒子(22200个粒子)。然后，随着移动机器人的移动，这些位姿粒子逐渐收敛到正确的位姿，如附图(b)-(d)所示。

通过对全局定位结果的比较，可以看出，所提出的方法不需要在整个位姿空间中布置大量的位姿粒子，同时在较短的移动距离下实现成功的全局定位。由于在场景没有显著变化(如长廊和类似走廊)时，AMCL进行全局定位的收敛速度一般较低，因此该方法的收敛速度比AMCL快。

为了评估成功率，我们从数据集中选择间隔10s作为全局定位的起始点。使用不同的起始点进行全局定位的结果如表5所示。结果表明，在三种方法中，MHT+SUES的性能最好，并且AMCL+SUES的性能优于AMCL。

表5.全局定位成功率结果(Intel-sorted-01)(成功次数/测试次数)

在全局定位过程中，使用移动距离来表示全局定位的效率。我们成功地计算了AMCL、AMCL+SUES和MHT+SUES的平均移动距离，结果如附图11所示。从图附图11可以看出，MHT+SUES和AMCL+SUES的平均移动距离远小于AMCL，并且所提出的MHT+SUES比AMCL+SUES表现更好。对比全局定位关于成功率和效率的实验结果表明，所提的全局定位方法比AMCL具有更好的性能。

全局定位实验的第二个公共数据集是Seattle，实验从241个原始激光扫描帧中选择102个作为关键帧。由于公测集中包含的数据太少，AMCL和AMCL+SUES很难获得满意的结果。然而，MHT+SUES的成功率高达98.25％(112/114)。此外，我们还计算了平均移动距离来反映MHT+SUES的效率，结果为2.8m。

第3.2，自录数据集定位结果

我们在同一个走廊环境中(附图8(b))录制了五个激光数据集用于全局定位实验，由四个静态数据集和一个动态数据集组成。四个静态数据集从静态环境中获得，包括两个顺时针数据集和两个逆时针数据集。其中一个数据集用于生成关于关键帧的位姿地图，而其他三个数据集用于全局定位实验。当移动机器人沿着相同的路径移动时，同时会有两个人在移动机器人周围行走造成动态环境，从而录制产生动态数据集。

同样从三个静态数据集中选择间隔10s作为全局定位的不同起始点。使用不同起始点的全局定位结果显示在表6中，结果表明MHT+SUES在三种方法中具有最好的性能，并且由于使用本发明所提出的SUES，AMCL+SUES获得了比AMCL更好的性能。对于全局定位成功的例子，我们计算AMCL、AMCL+SUES和MHT+SUES的平均移动距离，结果如附图12所示。附图12表明，在三个不同数据集中，MHT+SUES在平均移动距离方面具有最好的性能，并且AMCL+SUES的性能也优于AMCL。

表6.全局定位成功率结果(hallway)(成功次数/测试次数)

此外，为了验证本发明对环境噪声的鲁棒性，还进行了所提出的SUES+MHT和AMCL在动态环境下的对比实验。关于成功率和效率的对比实验结果如表7所示，结果表明，环境噪声对SUES+MHT和AMCL的全局定位成功率没有影响。然而，环境噪声降低了AMCL的效率，而SUES+MHT仍然保持高效率。

表7.动态环境下的成功率和效率对比实验结果(成功次数/测试次数)

上述关于在走廊环境中成功率和效率的对比实验结果表明本发明比AMCL具有更好的性能。具体而言，在较高成功率的前提下，所提出的方法提高了全局定位的效率。

参考文献

1.H.Durrant-Whyte,T.Bailey,“Simultaneous localization and mapping:Part I,”IEEE Robotics andAutomation Magazine,vol.13,no.2,pp.99-110,2006.

2.C.Cadena,L.Carlone,H.Carrillo,Y.Latif,D.Scaramuzza,J.Neira,I.Reid,J.J.Leonard,“Past,present,and future of simultaneous localization andmapping:Toward the robust-perception age,”IEEE Transactions on Robotics,vol.32,no.6,pp.1309-1332,2016.

3.D.Galvez-López,J.D.Tardos,“Bags of binary words for fast placerecognition in image sequences,”IEEE Transactions on Robotics,vol.28,no.5,pp.1188-1197,2012

4.R.Mur-Artal,J.M.M.Montiel,J.D.Tard′os,“ORB-SLAM:A versatile andaccurate monocular SLAM system,”IEEE Transactions on Robotics,vol.31,no.5,pp.1147-1163,2015.

5.A.Murillo,G.Singh,J.J.Guerrero,“Localization in urbanenvironments using a panoramic gist descriptor,”IEEE Transactions onRobotics,vol.29,no.1,pp.146-160,2013.

6.G.D.Tipaldi,K.O.Arras,“FLIRT-Interest regions for 2D range data,”inIEEE International Conference on Robotics andAutomation,2010,pp.3616-3622.

7.G.D.Tipaldi,L.Spinello,W.Burgard,“Geometrical flirt phrases forlarge scale place recognition in 2D range data,”in IEEE InternationalConference on Robotics and Automation,2013,pp.2693-2698.

8.J.Deray,J.Solá,J.Andrade-Cetto,“Word ordering and documentadjacency for large loop closure detection in 2D laser maps,”IEEE Roboticsand Automation Letters,vol.2,no.3,pp.1532-1539,2017.

9.F.Martín,L.Moreno,D.Blanco,et al.“Kullback-Leibler divergence-basedglobal localization for mobile robots,”Robotics and Autonomous Systems,vol.62,no.2,pp.120-130,2014.

10.D.Fox,“Adapting the sample size in particle filters through KLD-sampling,”International Journal of Robotics Research,vol.22,no.12,pp.985-1003,2003.

11.R.Valencia,J.Saarinen,H.Andreasson,et al.“Localization in highlydynamic environments using dual-timescale NDT-MCL,”in IEEE InternationalConference on Robotics andAutomation,2014,pp.3956-3962.

12.S.Tully,G.Kantor,H.Choset,“A unified Bayesian framework for globallocalization and SLAM in hybrid metric/topological maps,”InternationalJournal ofRobotics Research,vol.31,no.3,pp.271-288,2012.

13.T.He,S.Hirose,“A global localization approach based on line-segment relation matching technique,”Robotics andAutonomous Systems,vol.60,no.1,pp.95-112,2012.

14.H.Liu,F.Sun,B.Fang,X Zhang,“Robotic room-level localization usingmultiple sets of sonar measurements,”IEEE Transactions on Instrumentation andMeasurement,vol.66,no.1,pp.2-13,2016.

15.S.Park,K.S.Roh,“Coarse-to-fine localization for a mobile robotbased on place learning with a 2-D range scan,”IEEE Transactions on Robotics,vol.32,no.3,pp.1-17,2016.

Claims (1)

1.一种基于结构单元编码和多假设跟踪的线激光全局定位方法，其特征在于该方法包括：

第1，利用SUES对有向端点特征之间的几何关系进行编码

第1.1构造产生结构单元

对激光数据进行线段提取，定义P＝{p₁,...,p_N}为有向端点特征点集，其中N表示特征点数量；第i个有向端点特征以p_i＝[x_i y_i θ_i]^T表示，其中(x_i,y_i)定义为特征位置，θ_i定义为特征方向；定义ρ_j为特征点p_l＝[x_l y_l θ_l]^T与特征点p_n＝[x_n y_n θ_n]^T之间的欧式距离，其中j为索引编号，

当满足ρ_j≤ξ时，定义结构单元s_j＝{ρ_j,θ_j1,θ_j2}表示特征点p_l和p_n之间的几何关系，其中ξ为给定阈值，θ_j1和θ_j2为特征点p_l和p_n之间的方向信息，计算如下：

其中θ′_l,θ′_n为方向信息，位于区间[0,360°)；定义S＝{s₁,…,s_M}为构造得到的结构单元集合，其中M定义为结构单元数量，根据上述构造方式获得；

第1.2通过SUES对结构单元进行编码

定义δ_a和δ_d分别为角度分辨率和距离分辨率，将整个角度空间和距离空间以上述分辨率划分成小栅格；定义w_i为由2个角度分割线和1个距离分割线组合形成的结构单词，其中i为索引值，总共能够产生L＝b²d个结构单词，其中采用基于分割线的软投票机制，将产生的结构单元s_j＝{ρ_j,θ_j1,θ_j2}进行编码，首先根据软投票机制产生分割线，其中φ_i、i＝1,...,6定义为具体分割线索引值；然后根据距离分割线的距离大小决定每个分割线的权重；最后通过分割线之间的组合，每个结构单元能够产生8个结构单词，如是将所有结构单元进行编码再归一化，就能够得到激光数据所对应的特征向量；

第2，通过扩展MHT逐步恢复全局位姿

第2.1建立多假设概率模型

通过SUES对当前激光数据进行单元结构特征编码，然后通过相似度计算能够从离线位姿地图中获取候选关键帧并得到候选全局位姿；区别于传统MHT方法，本发明将独立观测获得的候选全局位姿信息作为先验项，将连续的候选位姿和里程计增量信息作为似然项；关于k时刻机器人位姿序列的概率分布模型表示如下：

p(X_1:k|u_1:k-1,z_1:k,m), (10)

其中u_1:k-1定义为里程计信息序列；z_1:k表示观测序列，同时X_1:k和m分别表示候选机器人位姿序列和先验地图；为了减少计算量，整个分布使用如下递归进行计算：

p(X_1:k|u_1:k-1,z_1:k,m)＝p(X_k|u_k-1,z_k,m,X_1:k-1)·p(X_1:k-1|u_1:k-2,z_1:k-1,m), (11)

其中p(X_1:k-1|u_1:k-2,z_1:k-1,m)表示k-1时刻的位姿序列概率分布，p(X_k|u_k-1,z_k,m,X_1:k-1)表示k时刻机器人位姿的概率分布；

利用贝叶斯规则对p(X_k|u_k-1,z_k,m,X_1:k-1)进行展开，得到如下：

其中p(u_k-1|X_k-1,X_k)和p(X_k|z_k,m)分别表示似然项和先验项，p(u_k-1|z_k,m,X_1:k-1)为归一化项；能够得到概率分布函数正比于似然函数p(X_k|u_k-1,z_k,m,X_1:k-1)∝p(u_k-1|X_k-1,X_k)；

第2.2利用里程计模型进行似然估计

利用里程计模型能够得到两个连续位姿之间的增量参数，包括两个角度和一个距离；定义δ′_rot1、δ′_tran和δ′_rot2为连续候选全局位姿X_k-1,X_k的增量参数；定义δ_rot1、δ_tran和δ_rot2为里程计信息u_k-1的增量参数；利用里程计模型获得的增量参数之间相互独立，因此p(u_k-1|X_k-1,X_k)∝p₁·p₂·p₃，其中

其中，α_i(i＝1,...,4)为里程计模型中的噪声参数，prob()为正态分布函数；机器人候选位姿序列的概率分布模型通过独立观测和里程计模型进行计算；在实际操作的时候，每一次更新概率分布之后，都要检测去除概率小于特定阈值的位姿序列，直至剩下唯一一个位姿序列对应全局定位结果。