基于双向激光回环检测的SLAM算法研究

郑雪鹤，孙作雷

(上海海事大学 信息工程学院，上海 201306)

基于双向激光回环检测的SLAM算法研究

郑雪鹤1，孙作雷2

(上海海事大学 信息工程学院，上海 201306)

针对移动机器人长时间运动后无法自身修正累计误差以及传统EKF (Extended Kalman Filter)算法计算复杂度大的问题，提出了一种基于双向激光进行回环检测的方法，通过有效的相似度检测算法检测出真实的回环，及时修正机器人的位姿。同时使用精确稀疏滞后滤波算法相辅，利用信息矩阵的自然稀疏性来降低计算复杂度。通过实验结果分析，上述两种方法的结合可以有效地减少移动机器人行驶过程中的累计误差。

回环检测；精确稀疏滞后滤波；同时定位与地图构建；移动机器人

0 引言

同步定位和构图(Simultaneous Localization and Map Building,SLAM)最早由DURRANT-WHYTE H和LEONARD J J提出[1]。SLAM一般用于解决机器人在未知环境下定位导航以及构图问题。EKF(Extended Kalman Filtering)算法是SLAM算法中经典的算法之一，通过线性化及高斯分布假设对状态空间中的机器人位姿和特征位置进行估计。EKF SLAM自提出后一直被许多研究者采用，然而其计算复杂度与环境特征个数呈二次方关系，因此EKF SLAM只能在一般不超过上百个特征的较小范围内应用[2]。如何在大地图减少计算复杂度，这是SLAM算法中一个公共的难题。许多学者在EKF SLAM算法上进行改进， THRUN S通过经验观察发现，EKF SLAM 中的信息矩阵中许多非对角元素的值接近于零，具有一种近似的稀疏性。EUSTICE R对SEIF(Sparse Extended Information Filter)的稀疏过程及算法的一致性进行了分析，发现SEIF可以基本满足局部一致性，但是无法保证全局一致性[3]。而ESDF(Exactly Sparse Delayed-State Filters) 算法通过增加一个延时状态，将信息矩阵自然稀疏，可以保证局部和全局的一致性[4]。

回环检测(Loop Closure Detection)，又称闭环检测，是指机器人是否可以识别自己曾经到达过某个地点的能力。一旦检测成功，可以显著地减小累积误差[5]。回环检测实质上是一种检测观测数据相似性的算法。错误的回环会使地图变得很糟糕，优秀的回环检测算法应该尽量检测出真实回环[5]。

本文利用ESDF算法以及双向激光进行回环检测，显著地减小了机器人行驶过程中的累计误差。

1 ESDF算法推导

1.1 状态增量

当需要存储一个新视图时，通过增加一个新的状态延时来形成状态增量，以便在之后的时间里回顾之前的状态[6]。

1.1.1增加一个延时状态

假定在时间t分布下给出协方差和信息表达式：

(1)

这个分布代表着一个地图M和当前机器人的状态xt、所有的观测量zt以及输入的控制量μt。在xt+1时刻获得分布p(xt+1,xt,M|zt,μt+1)可以分解为：

p(xt+1,xt,M|zt,ut+1)=

p(xt+1|xt,M,zt,ut+1)p(xt,M|zt,ut+1)=

p(xt+1|xt,ut+1)p(xt,M|zt,ut)

(2)

下式描述的是一般非线性马尔可夫机器人运动模型以及它的一阶线性形式，F是在μt下的估计雅克比矩阵，w～N(0,Q)是高斯白噪声。

xt+1=f(xt,ut+1)+wt≈f(μt,ut+1)+F(xt-μt)+wt

(3)

1.1.2增量的协方差形式

在公式(3)的线性条件下，增加的状态分布式(2)仍然是高斯的，它的协方差形式由下式给出：

(4)

其中，

1.1.3增量的信息形式

(5)

1.2 马尔可夫精确稀疏

由上述推导的公式(5)可知，扩充机器人的运动状态到xt+1时，仅仅对先前的xt状态提供信息[7]。增广状态的一个重要的属性是用额外的滞后状态继续增加状态矢量，信息矩阵将展现出一块三对角结构连接每个滞后位姿以及先前的状态。从公式(6)中可以看出，滞后状态SLAM信息矩阵是自然的没有任何近似的。

(6)

1.3 状态更新

假设下面的一般非线性方法和它的一阶线性形式为：

(7)

(8)

这个计算中的所有非一般模型都会导致每次更新时出现二次计算复杂度。相反地，扩展信息滤波更新表达为：

(9)

1.4 预测模型

预测模型对应于机器人从t传播到t+1时刻的状态。为了得到时间分布p(xt+1,xt,M|zt,ut+1)，在公式(5)的联合分布中需要简单地边缘化先前的状态xt。最后得到信息预测模型：

(10)

这里

Ω=Λxtxt+FTQ-1

ψ=Q-1-Q-1FΩ-1F-1Q-1=

1.5 状态恢复

信息形式的高斯分布的两个参数分别为ηt和Λt。然而，公式(10)的预测模型和公式(9)的观测模型都额外地增加了状态均值矢量μt中的子块，因此，为了让延时状态中的信息形式计算高效，需要容易地恢复状态矢量均值部分。

1.5.1全局状态恢复

通过矩阵直接求逆的方法恢复初始状态会导致三次复杂性，并且会破坏在EKF上获得的任何效率。状态均值μt可以更有效地解决稀疏、对称、正定、线性方程：

Λtμt=ηt

(11)

这样的系统可以通过经典的共轭梯度迭代法来解决。通常，共轭梯度每次迭代会产生O(n)的复杂度，这里n是状态矢量的尺寸。如果是已经初始化好的，则迭代次数会更少。

1.5.2局部状态恢复

计算运动预测公式(10)和测量更新公式(8)时，只需要知道状态均值的子集，而不是总为完整的状态均值向量求解，可以将公式(11)分为两组耦合方程:

(12)

这种划分所说的就是地图的“局部”，从而能够不时地对地图的局部部分进行解析。通过目前的固定估计，可以解决公式(12)的估计：

(13)

公式(13)提供了一种恢复局部地图估计的方法，对局部部分的估计是对实际平均值的近似值。在其他情况下(例如，由于闭环的结果)，真正的全局平均值应该通过公式(11)来恢复。

2 回环检测

本文在机器人行驶的前方和后方分别使用激光传感器，为了便于区分，此处分别称之为前向激光传感器和后向激光传感器。所得到的数据则为前向激光和后向激光。回环的原理是机器人在未知环境中行走后掉头(反方向行走)，如果到达曾经到过的地方，当前时刻其后向激光传感器得到的数据和历史某时刻前向激光传感器得到的数据相似度极高。通过性状的相似度检测，检测出真实的闭环，形成闭环后可以显著地减少算法上的累积误差[8]。为了确保前后激光是在相同时刻上进行采样，需要进行前后激光传感器的时间对准[9]。

由图1可知，前后激光是在相同的时刻进行采样,可以在之后的程序中进行相同时刻上的回环检测。

图1 前后激光时间对准

3 实验结果

本文利用机器人在未知环境中行驶了两圈，机器人行走的第二圈中进行掉头的操作。其中图2为机器人行驶第一圈时的位姿，使用精确稀疏滞后算法进行位姿的估计。机器人行驶的过程中通过前后激光传感器得到激光数据。图3为机器人当前时刻的前向激光数据与机器人历史时刻的前向激光数据做回环检测后的结果，主要进行了欧氏距离检测、马氏距离检测、AdaBoost检测以及最近邻检测[10]。可以看出，机器人在掉头操作后产生了较为明显的累计误差，偏离之前的行驶路线。图4为机器人在进行前向激光回环检测后加入后向激光数据进行回环检测的结果。

图2 机器人行驶第一圈的位姿

图3 仅使用前向激光效果图

图4 使用双向激光效果图

4 结论

通过实验观测可知，使用双向激光进行回环检测，检测出真实的闭环，可以有效地减少累计误差。同时，一个好的回环检测算法应当在保证检测出真实回环的同时降低运算复杂性，提高时间运行效率。

[1] SMITH &SELF M, CHEESEMAN E. Estimating uncertain spatial relationships in robotics [J]. Uncertainty in Artificial Intelligence, 1988(2): 435-461.

[2] THMN S, LIU Y, KOLLER D. Simultaneous localization and mapping with sparse extended information filters[J]. International Journal of Robotics Research, 2004, 23(7/8):693-716.

[3] EUSTICE SINGH H, LEONARD J. Exactly sparse delayed state filters[C].IEEE International Conference on Robotics and Automation, 2005:2417-2424.

[4] EUSTICE R, WALTER M, LEONARD J. Sparse extended information filters: insights into sparsification[C].IEEE RSJ International Conference on Intelligent Robots and Systems，2005: 3281-3288.

[5] 董海巍．大范围环境下移动机器人同步定位和地图创建研究[D]．上海：上海交通大学，2008．

[6] 郭剑锋，赵春霞，石杏喜.稀疏扩展信息滤波SLAM算法的稀疏规则研究[J].系统仿真学报，2008, 20(12): 6673-6679.

[7] SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence and Pattern Recognition, 2013, 5(5): 435-461.

[8] ANGELI A, FILLIAT D, DONCIEUX S, et al. Fast and incremental method for loop-closure detection using bags of visual words[J]. IEEE Transaction on Robot,2008,24(5):1027-1037.

[9] NISHANT K, SWAGAT K, TOMOHIRO S. High performance loop closure detection using bag of word pairs[J]. Robotics and Autonomous Systems, 2015,77: 55-65.

[10] 崔大成，曾连荪.基于视觉字典的移动机器人闭环检测方法研究[J].微型机与应用,2015,34(9):85-88.

Research of SLAM algorithm based on bidirectional laser loop closure detection

Zheng Xuehe1, Sun Zuolei2

(School of Information Engineering, Shanghai Maritime Univeristy, Shanghai 201306, China)

Aiming at the problem that the mobile robot can not correct the cumulative error itself and the complexity of the traditional extended Kalman filter (EKF) algorithm, this paper proposes a method based on bidirectional laser for loop closure detection. Through the effective similarity detection algorithm, it can detect the real loop and timely correct the robot position. At the same time, we use the exactly sparse delayed state filters algorithm to supplement the natural sparsity of the information matrix to reduce the computational complexity. The experimental results show that the combination of the two methods can effectively reduce the cumulative error in the process of moving the robot.

loop closure detection; ESDF; SLAM; mobile robot

TP242

A

10.19358/j.issn.1674- 7720.2017.20.006

郑雪鹤，孙作雷.基于双向激光回环检测的SLAM算法研究[J].微型机与应用，2017,36(20)：19-22.

2017-03-31)

郑雪鹤(1992-)，女，硕士，主要研究方向：移动机器人导航。

孙作雷(1982-)，男，博士，副教授，主要研究方向：移动机器人导航。