复杂三维环境中的路径规划方法(翻译).pdf

复杂三维环境中的路径规划方法复杂三维环境中的路径规划方法 晏飞 刘一莎 肖继忠 摘要：本文介绍了在复杂环境中的无人机（UAV）三维路径规划算法。在该算法 中，环境是由八叉树算法分为体素。为了满足 UAV 的安全要求，自由空间被自 由体素，其具有足够的空间余量为 UAV 穿过表示。边界框数组在整个三维空间 来评价自由体素连通创建。概率路线图方法（PRM）由边界框阵列中随机抽样改 进，以确保路线节点的在 3D 空间中的更有效的分配。根据连接评估，路线图是 用来通过使用 A *算法来规划的可行路径。实验结果表明，该算法在复杂的 3D 环境有效。 关键词：3D 路径规划，复杂的环境中，无人机（UAV） ，概率路线图法（PRM） ， 八叉树 1 引言引言 近年，无人机（UAV）正被越来越多地用于许多领域，如消防1，救灾难度太 大或危险的任务2，搜救3，目标跟踪4，以及面向空间的应用5。对于这些应用， 所述 UAV 应计划在其工作空间中的非碰撞路径的能力。路径规划的中心问题是 根据一些规定诸如能源，时间和安全性，以产生从初始点到目标点与障碍物的环 境中的无冲突路径和优化路径6。 规划算法已经开发几十年。在7中，波前算法被用作路径规划三维正态分布 变换（3D-NDT）地图。这个算法是直观的，易于实现，但它会无法找到在复杂 环境中的最佳路径。在另一方面，*A（见8-10）是找到一个给定的起始节点到目 的节点的最低成本路径最佳优先启发式搜索算法。不幸的是，随着工作空间变得 越来越大，所花费的时间以指数寻找最优路径的增加5。路线图通常适用于帮助 这些启发式搜索算法，这往往是在配置空间表示。各种路线图的方法已被用于如 能见度线11，虚拟力9，迅速-探索随机树木12 ，13和概率路线图方法（PRM）14。 路径规划将在 3D 环境中，这是特别是在复杂的环境中的一大挑战来进行。 通常用于高维配置空间的方法是基于基于采样的方法，包括在 PRM 15,16和它的 变体。PRM 有两个阶段：一个学习阶段和一个查询相15。在学习阶段，一个数 据结构被称为路线图被构造为给定的场景。该路线图是一个无向图),(ENR 。 N为一组节点，这是随机无冲突配置空间采样点。有前途的节点对被选择在图 表和一个本地筹办被用于尝试这种安排有边缘连接。如果它们可以被连接，则该 边缘加到大肠杆菌好的图形应覆盖自由空间很好。在查询阶段中，路径通常是 由路径查询诸如 Dijkstra 算法或*A找到。 基本 PRM 的瓶颈在于它在寻找通过狭窄的通道路径很麻烦。使太少节点被 放置在狭窄的通道，这是由于在自由空间中的随机抽样的节点。一个解决方案是 增加接近障碍物边界17 ，18的配置。虽然这种方法增加取样狭窄的通道节点的概 率，在狭窄的通道以外的许多节点不改善路线图的连通性帮助。Hsu 等人19 ，20 提出一种混合策略：桥试验在狭窄的通道施加增加采样密度，它也使用了在开放 的自由空间均匀的采样策略。 在文献21中， 作者用一个慵懒的显著边缘算法在尚 未导航区域来放置新样本改善慵懒的概率路径规划。在这些方法中，环境的障碍 是由几何模型呈现。但在实践中，实际的实验环境不能由几何模型描述。对于复 杂的 3D 场景，最流行的表示是三维点群22-27或体素28 ，29。当使用基本 PRM 或 变种以上这些 3D 表示所提到的，碰撞检查是因为障碍物（点或体素） ，数量庞 大沉重的计算负担。为了解决这个问题，细胞分解是一种可以把自由空间到子空 间的好选择。 该方法是由伯格和奥维马斯30为指导采样向现场最有趣的地区分离 大开放的区域31。提出了完整的运动计划，结合近似细胞分解与 PRM 混合的方 法。但在这些方法，碰撞检查用于获得路线图构成的无碰撞的本地路径也复杂 3D 环境的一个大问题。 在本文中，我们提出了基于三维复杂环境下的无人机路径规划 PRM 方法的 算法。环境可以通过激光扫描仪捕获的 3D 点云来表示。然而，从激光扫描器的 原始数据为任何路径规划算法实时实现望而却步巨大。 因此我们使用八叉树算法 来划分作业空间成体素，这是环境的 3D 网格表示。以提取用于 UAV 导航自由空 间， 我们记录期间八叉树生成过程的中心点和自由体素尺寸。路径规划的最小自 由体素的尺寸被设定为比 UAV 的物理尺寸较大，以确保它能够通过通路安全。 最终路径连接自由体素的序列。为了应对窄通道问题，环境等分为称为边界框几 个子区域。 从我们以前的论文32不同， 则节点随机从游离体素中的每个边界框根 据自由和占用的体素的体积比采样。游离体素的连通性的评价在每个边界框进 行。 那么局部规划将搜索边界框节点无碰撞路径。为了使路线图覆盖整个可用空 间， 则加入另外的边界框重叠相邻边界框在相邻的区域中的局部路线图的连接评 估。最后，我们使用*A方法从开始搜索素在路线图的目标体素路径。我们已经 实现了该算法在实际户外环境中，由 3D 点云表示。 在本文的其余部分安排如下：第二节介绍了复杂的 3D 环境表示。第 3 节介 绍了实时路径规划所提出的方法。 实验结果在第 4 节提出的结论和未来的工作将 在第 5 给出。 2 环境表示环境表示 3D 网格表示提供最多的信息，这将在本文中被应用为 3D 路径的规划。对于 无人机的路径规划，环境的自由空间是必需的信息。因此，我们的方法的重点是 要找到在环境中的可用空间。 加快建立三维网格结构的过程中， 使用八叉树算法。 八叉树是在三维33用于 空间细分的分层数据结构。在八叉树的每个节点代表包含在立方容积的空间中， 通常被称为体素。此卷递归细分成八个子体积直至体素的尺寸满足预期的分辨 率。在其最基本的形式，八叉树可用于布尔属性模型29：占用或免费。不失一 般性，在子体积，其中包含的 3D 数据，被加到代表环境作为占领体素。我们专 注于免费的体素，这是在八叉树生成过程还内置。如图 1 所示，被占领的体素将 是细分在下一步骤。我们记录每个步骤中的自由体素的位置和大小。在图 1 中， 光体素是自由的和暗的体素都被占用。在免费的代表在环境中的自由空间。为了 准确描述环境，研究人员通常把场景到高分辨率像素。莱德在 0.02 米分辨率28 以 3D 形式绘制的办公环境。 在文献29中， 地图的体素尺寸为 0.04 米或 0.05 米。 以这种方式，有两个问题：首先，它从显著计算负担遭受在检测障碍物或计划在 复杂的环境中的路径。第二，用于 UAV 路径规划（我们的 UAV 的大小约为 0.4 米）时小的自由体素不能确保安全性。在我们的八叉树方法中，如果一个体素的 子体积是不够大，UAV 通过，此体素将永远不会被细分。所以分辨率是依赖于无 人机的物理尺寸。因为自由体素从不同水平的八叉树算法的萃取，它们的 尺寸并不 统一。 图 1所占用的体素和自由体素提取三维数据在八叉树的构建 3 三维三维 PRM 算法算法 我们的路径规划策略是首先有效地品尝节点根据自由， 占领素子区域的体积 比所谓的包围盒。 第一阶段后， 局部规划在每个子区域中进行构建路线图。 最后， 搜索方法被用来获得从所述路线的路径。 3.1 节点选择节点选择 节点选择是 PRM 的基本的，但重要的过程。在基本算法，随机取样策略被 应用在整个自由空间，这导致狭窄的通道区域抽样节点的低成功率选择的节点。 为了应对窄通道的问题， 我们把环境空间分成称为边界框， 其大小是m次的 自由体素分辨率大小相等的子区域。因此，在一个边界框体素的最大数量为 )2( 3 mmnvoxel。有三种包围盒：空盒，满箱和混合箱。在满箱，也有只占用 体素使得没有体素可以被选择用于路线图施工。与此相反，在体素属于空盒都是 免费的体素，其可以确保 UAV 路径规划的安全性。我们可以随机选择任何像素 的路线图的节点。虽然更多的节点可以使路标覆盖自由空间更好，计算负担也是 在这种情况下，增加了。如何选择空箱子节点将在下一节讨论。不失一般性，混 合框的意思的狭窄通道也包括在这种类型的边界框的障碍的边界。在这些框，我 们将根据被占领和自由体素的比率增加的随机节点的密度。 由于游离体素尺寸是 不是在我们的算法均匀，数量是成正比的体素的体积比。然后选定的节点在一个 框中的数量是 i free occupied node v v kn （1） 其中 occupied v是一个被占用的体素的体积， i free v是第 i 个自由体的体积，k是 一个预先设定的系数。 图 2 显示计算节点数的策略选择。 图上有 10 个 k 的值， 从 voxel n1 . 0到 N voxel n。 选定节点的数目曲线陡峭表示计算负担快速增加。所以在应用中，我们把 k 设置 为 voxel n1 . 0。如图所示，当占用小于 voxel n625. 0，选择自由体素为节点。如果占用 像素数大于 voxel n73. 0，会选择全部自由体素。在其他情况下，节点的选择是根据 公式（1）进行的。 图 2节点选择策略 我们比较我们的策略与随机抽样方法，因为像素块彼此通常在三维场景中， 为了说明差异明显，我们测试了二维仿真环境的方法。环境是由两大自由区和一 个狭窄的通道组成，如图 3 所示。我们将自由空间到二维网格的四叉树的方法。 总共有 329 个自由网格，其中 55 个在通道中。边界框的大小是自由网格分辨率 的 4 倍。然后根据我们的策略，在这个环境中选择了 109 个节点。为了比较的目 的，我们选择 109，155 个和 220 个节点在相同的环境中的随机抽样方法。然后 分别对每种情况分别进行 100 个独立运行。 我们把选定的网格在狭窄的通道和结 果如图 4 所示数。每一个盒子显示的最小和最大，正方形是平均值。上面的文本 框显示了建设路线图的成功率，利用我们的计划（见第 3.2 节）连接两个大的自 由空间的成功率。在随机抽样方法的 220 个自由网格的选择，结果与我们的想法 相似。 实例 109 节点如图 3 所示。在图 3（b） ，在通道中间没有节点，路线图不能 连接两大自由空间。此外，大多数的随机节点是最小的自由网格，因为最小的自 由网格的数目远远大于大的。 这些最小的障碍边界附近的节点不帮助改善的连通 性的路线图 19 。采用随机抽样的方法相比，我们的策略是更好的（图 3（a） ） 。 图 3我们用均匀采样方法比较实例的节点选择策略。 （a）109 个节点根据 我们的策略选择； （b）相同的节点数在同一环境中随机采样 图 4在狭窄的通道选择的网格数量 3.2 局部规划局部规划 在一些 PRM 方法中， 局部规划选择一组邻居的每个新节点被添加到路线图。 在本文中，包围盒分工作空间分成若干子区域。所以，不像其他的 PRM 方法， 我们不需要搜索的近邻但考虑所有选择的节点的包围盒的邻居。在包围盒中，如 果节点的数目很小，一些节点没有通过使用直线局部规划连接。这将影响到建立 路线。图 5 包围盒有三个节点。当使用直线局部规划时，每两个节点之间的边缘 与障碍物相撞。 为了解决这个问题， 我们提出了一个简单的方法， 作为局部规划， 它可以搜索一个无碰撞的路径节点。 图 5包围盒的局部规划 正如前面提到的，自由体素的八叉树算法提取尺寸大于无人机的物理尺寸， 以确保它可以通过安全。 为了创造一个无碰撞路径， 自由体必须连接一个接一个。 所以像素之间的连接必须进行评估。在本文中，两体素谁分享同一个面是相邻的 体素。如果两体素的位置满足 2 )( 2 )( 2 )( ba ba ba ba ba ba SS zz SS yy SS xx 在 a S和 b S是两个像素大小，像素在 x 轴连接，如图 6 所示。这样，两个像 素共享区可以保证无人机安全度过。 图 6两自由体素通过他们的位置连接坐标 基于连通性的评估，我们用*A算法搜索选定的节点之间的局部路径，方法 如图 5 所示。局部所有的路径是由连接自由体素，可以保证无人机安全。但只有 在边界框中的局部路径是不够建设路线图的。 要计划一个完整的路径从起始位置 到目标位置，在不同的相邻包围盒的局部路径的连接状态是必要的。这个问题将 通过插入相邻的另一个包围盒来解决。如图 7 所示，A和B是两个邻居包围盒， a和b是两个自由体分别属于不同的盒子。 因此， 在上述评价的过程中， 体素a和 b之间没有任何连通性，因为它们在不同的包围盒中评估。但是当有另一个边界 框C插入A和B之间，C可以对连接的a和b进行评估。 图 7个包围盒C插入相邻的人之间 （A和B） 是用来评价两自由体连接 （A 和B）在不同的包围盒 在盒的连接的局部路径A和B的第一个条件是，节点A和B都属于框C。由 于A的一半的部分是在框C中，复选框中的随机选择的一个节点时，该节点的概 率属于到C是2/1。假设A是一个空盒， voxel n节点都在框A。选择为了确保至 少一个节点被放置在框C中，概率需要满足以下的条件 1 22 3 mnvoxel （2） 我们可以得到 3 2 m 。考虑到计算量，我们选择 3 2 m 个节点。以这种方 式， 在一个盒子里的两个节点可以足够用于连接的本地路径。但它是不可靠的路 线图创建。因此，在实践中，我们设置为 3 2 2 ，这意味着有四分之一的自由像 素空边界框被选中。这种策略也可用于混合箱（图 2） 。 3.3 查询阶段查询阶段 假设路线图是建立和起始和目标游离体素中给出。*A方法用于在查询阶段， 协助路径规划 PRM。*A是广泛用于寻找路径。在这里，我们使用*A从起始节 点在路线图的目标节点搜索的最短路径。在这个过程中，我们考虑的直线距离， 并在路线作为搜索成本连接两个节点游离体素的数目。具体的步骤如下： isi NNArcsND,. 1 iss isi i NNArcsN NNArcsN NP , , . 2 isii NNHNDNW,. 3 )min( satisfiesthat Find. 4WNWN ik )(_. 5 k NbackpushS gk NN ! While. 6 )min( satisfies that Find14. End13. End.12 ;.11 ;,.10 ;,. 9 ,. 8 1For . 7 WNWN NNP NNHNDNW NNArcsNDND NNArcsNDNDIf ntoj ik kj gjjj jkkj jkkj End.16 )(_.15 k NbackpushS 弧 ji NN ,表示 i N和 j N节点之间的连接。该值是 ENNNNNNND ENN NNArcs jljiji jl ji , , ,（3） 其中，),( ji NND描述了 i N和 j N的距离，),( ji NNN为游离体素连接的数目 基于 i N j N由局部规划体素通过使用*A搜索算法，所述路径从目标节点存储在 矢量P通过检索。 4 实验结果实验结果 这部分是由两部分组成。首先，我们实现我们的方法，并测试其大小不同在 室外环境包围盒的性能。根据结果，我们将确定m的值。在第二部分中，较大的 和更复杂的环境将被用来测试该方法的有效性。 所有的环境中的原始数据是三维 点云。为方便起见，该数据是由一个三维激光测距仪收集。三维激光测距仪是由 一个二维激光扫描仪（LMS291）和倾斜机制的一个平台。虽然，没有被 UAV 获 得的数据，这是非常类似于由 UAV 用激光测距仪在27收集的数据。我们的无人 机的大小4 . 0米。在八叉树生成过程中，被占领体素的分辨率被设定为 0.2 米和 自由体素的分辨率为m5 . 0。该计算与英特尔（R）酷睿（TM）i5-2520M 的 CPU 和 4GB 内存，这是一个非车载电脑在个人电脑上的所有执行。 4.1 实验实验m的选择的选择 实验在三个室外环境进行如图 8 所示。第一环境是简单，它在中间并在通道 的每一侧衬套的两排的路径（图 8（a） ） 。在第二个，所述稀疏树木使环境为 UAV 路径规划（图 8（b） ）的更有挑战性。最后环境是更复杂的，这是由致密的树木 和灌木（图 8（c） ）的的。对于环境的一些参数可以在表 1 中找到。 表 1实验环境参数 环境 大小),(zyx 三维点数目自由体素数 1mmm71615127794584 2mmm92715855571223 3mmm53015129238584 边界框的尺寸是由m的值决定。如果m被设置为 4，边界框的尺寸是 m25 . 04。我们运行在三种环境随着m（从 2 到 10）的不同值的路径规划。 对于每一个值，该方法进行 100 次。发现从开始体素的目标体素的路径的成功率 在图 9 中描绘。并且用于每个步骤的平均时间示于图 8)(ig 中。上面的条数是 选自由体素的数目。最后一排中的数字显示路径规划（图 8)(lj ）的结果。 (a)(b) (c)(d) (e)(f) (j) (k) (l) 图 8室外环境中的路径规划。a-c 室外环境；d-f 的三维点云为三 环境；g-i 为不同的米的每一步的时间统计；j-l 路径规划的结果 当2m时，边界框的尺寸太小，自由体素在一个边界框选择。如在第 3.2 节所提到的，如果节点的数量是在包围盒小，它是不可靠的路线图创建。所以成 功率不是对所有的三种环境的 100。另一方面，当m值越大，速率要低得多。 这是因为在路线图的节点是过少（参见上述图 8） 。事实上，在图 2 中得到的 n 个节点的最小自由体素的体积的倍数。当使用八叉树方法来划分的自由空间，自 由体素的体积并不一致。这样的节点数目通常小于 node n。在该边界框包括大自由 体素的m的值越大，越可能。所以节点的数目较少。在图 9 中，当m设定为 3 和 4 时，成功率是 100。m的值应设置为 4。 图 9 成功率为m在三环境中的不同的值 4.2 大环境大环境 此外，我们还测试在一个更大的户外环境所提出的方法。如在图 10（a）所 示，这是一个树木茂密的环境中，它是由 10 激光扫描的结果。场景的尺寸是 mmm103247，和原始激光指针的数目是 444030.在该实验中，自由空间被分 成 8169 游离体素。当m设定为 4，我们选择 1194 免费像素为节点建设路线图。 该实验被重复 100 次。路径规划的最终结果示于图 10（b）和图 10（c）所示。 实验表明，该算法可以找到在复杂的环境中无人机安全的路径。每个步骤中的平 均计算时间示于表 2 中。 表 2每个步骤中的平均计算时间 步骤时间(s) 八叉树1.71 边框制作0.322 建设路线图14.135 *A算法0.224 总共16.391 图 10的路径在大型树木繁茂的环境规划设计。 （a）一个复杂的室外场景 组成的 3D 点;（b）该路径规划结果的一个俯视图; （c）路径的侧面图 5 结论结论 在本文中，我们提出了在复杂环境中的无人机导航三维路径规划方法。在这 项研究中，我们使用八叉树算法来划分作业空间成体素，它是环境的 3D 网格表 示。游离体素，其中有足够的空间容差以满足 UAV 的安全需求，是排序的表示 环境的自由空间。为了有效地得到的游离体素的连通性，环境被分成称为边界框 若干区域，和自由体素连通性的评价在每个边界框，而不是在整个空间中进行。 基本 PRM 使用随机采样策略来选择节点中的全部可用空间，这将导致窄通道问 题，即可能导致路径规划算法的故障。我们改善随机抽样的 PRM 的包围盒，以 确保在 3D 空间中的分布更加合理。最后，基于体素的连通性，选择的节点组成 一个路线图，它是由可行路径*A算法应用于路径搜索。路径规划的实验，在实 际的户外环境中进行。结果表明，该算法可以建立一个无人机无冲突的道路。 参考文献 1 J. Moore. UAVFire-fighting System, U.S. Patent 20130134254 A1, May 2013. 2 M. B. Wang, A. Chu, L. A. Bush, B. C. Williams. Active detection of drivable surfaces in support of robotic disaster relief missions. In Proceedings of Aerospace Conference, IEEE, Big Sky, MT,USA, pp.113, 2013. 3 D. Erdos, A. Erdos, S. E. Vatkins. An experimental UAV system for search and rescue challenge. IEEE Aerospace and Electronic Systems Magazine, vol.28, no.5, pp.3237, 2013. 4 S. Q. Zhu, D. W. Wang, C. B. Low. Ground target tracking using UAV with input constraints. Journal of Intelligent & Robotic Systems, vol.69, no.14, pp.417429, 2013. 5 Y. G. Fu, M. Y. Ding, C. P. Zhou. Phase angle-encoded and quantum-behaved particle swarm optimization applied to three-dimensional route planning for UAV. IEEE Transactions on Systems, Man, and Cybernetics Part A: Systems and Humans, vol.42, no.2, pp.511526, 2012. 6 O. Hachour. Path planning of autonomous mobile robot.International Journal of Systems Applications, Engineering & Development, vol.2, no.4, pp.178190, 2008. 7 T. Stoyanov, M. Magnusson, H. Andreasson, A. J. Lilienthal. Path planning in 3D environments using the normal distributions transform. In Proceedings of 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Taipei, Taiwan, China, pp.32633268, 2010. 8 Z. Qi, Z. H. Shao, Y. S. Ping, L. M. Hiot, Y. K. Leong. An improved heuristic algorithm for UAV path planning in 3D environment. In Proceedings of the 2nd International Conference on Intelligent Human-machine System and Cybernetics, IEEE, Nanjing, China, pp.258261, 2010. 9 Z. N. Dong, Z. J. Chen, R. Zhou, R. L. Zhang. A hybrid approach of virtual force and A search algorithm for UAV path re-planning. In Proceedings of the 6th IEEE International Conference on Industrial Electronics and Applications, IEEE, Beijing, China, pp.11401145, 2011. 10 S. Hrabar. 3D path planning and stereo-based obstacle avoidance for rotorcraft UAVs. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Nice, France, pp.807814, 2008. 11 R. Omar, D. W. Gu. Visibility line based methods for UAV path planning. In Proceedings of ICROS-SICE International Joint Conference, IEEE, Fukuoka, Japan, pp.31763181, 2009. 12 H. L. Yu, R. Beard, J. Byrne. Vision-based navigation frame mapping and planning for collision avoidance for miniature air vehicles. Control Engineering Practice, vol.18, no.7, pp.824836, 2010. 13 K. Yang, S. Sukkarieh. 3D smooth path planning for a UAV in cluttered natural environments. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, Nice, France, pp.794800, 2008. 14 K. Belghith, F. Kabanza, L. Hartman, R. Nkambou. Anytime dynamic path-planning with flexible probabilistic roadmaps. In Proceedings of IEEE International Conference on Robotics and Automation, IEEE, Orlando, USA, pp.23722377, 2006. 15 L. E. Kavraki, P. Svestka, J. C. Latombe, M. H. Overmars. Probabilistic roadmaps for path planning in highdimensional configuration spaces. IEEE Transactions on Robotics and Automation, vol.12, no.4, pp.566580, 1996. 16 M. H. Overmars. A Random Approach to Motion Planning. Technical Report RUU-CS-92-32, Department Computer Science, Utrecht University,The Netherlands, 1992. 17 V. Boor, M. H. Overmars, A. Frank, A. F. van der Stappen. The Gaussian sampling strategy for probabilistic roadmap planners. In Proceedings of the 1999 IEEE International Conference on Robotics & Automation, IEEE, Detroit, USA, pp.10181023, 1999. 18 N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, D. Vallejo. OBPRM: An obstacle-based PRM for 3D workspaces. In Proceedings of the 3rd Workshop on the Algorithmic Foundations of Robotics on Robotics, A. K. Peters, Ltd., Natick, MA, USA, pp.155168, 1998. 19 D. Hsu, T. T. Jiang, J. Reif, Z. Sun. The bridge test for sampling narrow passages with probabilistic roadmap planners. In Proceedings of 2003 IEEE International Conference on Robotics & Automation, IEEE, Taipei, Taiwan, China, pp.44204426, 2003. 20 D. Hsu, G. Sanchez-Ante, Z. Sun. Hybrid PRM sampling with a cost-sensitive adaptive strategy. In Proceedings of 2005 IEEE International Conference on Robotics and Automation, IEEE, Barcelona, Spain, pp.38743880, 2005. 21 J. Polden, Z. X. Pan, N. Larkin, S. Van Duin. Path planning with a Lazy Significant Edge Algorithm (LSEA). International Journal of Advanced Robotic Systems, vol.10, pp.18, 2013. 22 M. Bosse, R. Zlot. Place recognition using regional point descriptors for 3D mapping. In Proceedings of the 7th International Conference on Field and Service Robotics, Cam-bridge, Massachusetts, Springer, Berlin, Germany, pp.195204, 2009. 23 H. Men, B. Gebre, K. Pochiraju. Color point cloud registration with 4D ICP algorithm. In Proceedings of 2011 IEEE International Conference on Robotics and Automation, IEEE, Shanghai, China, pp.15111516, 2011. 24 A. Gressin, C. Mallet, N. David. Improving 3D Lidar point cloud registration using optimal neighborhood knowledge.In Proceedings of the ISPRS Annals of the Photogrammetry, Remote Sensing and Spatial Information Sciences, ISPRS, Melbourne, Australia, volI-3, pp.111 116, 2012. 25 H. Andreasson, A. J. Lilienthal. 6D scan registration using depth-interpolated local image features. Robotics and Autonomous Systems, vol.58, no.2, pp.157165, 2010. 26 P. Henry, M. Krainin, E. Herbst, X. F. Ren, D. Fox. RGB-D m