基于SLAM的机器人的自主定位导航解析

点击上方“小白学视觉”，选择加"星标"或“置顶”

重磅干货，第一时间送达

本文来源于新机器视觉，文章仅供学术交流，已获得授权。

来源：

https://blog.csdn.net/weixin_37251044/article/details/78515909

作者：Jack_Kuo@CSDN

背景介绍： 机器人的自主定位导航

正如图中所示，机器人自主定位导航技术中包括：定位和地图创建（SLAM）与路径规划和运动控制两个部分，而SLAM本身只是完成机器人的定位和地图创建，二者有所区别。

首先明确一下自主导航的两大解决方案：

1.传统方案：SLAM+路径规划+运动控制；

2.深度学习：这一两年，深度学习成为人工智能的代名词。在这个行业，前几年还是以比较传统的概率学，或者控制论的方式进行机器人自主定位导航。但这里展现的是完全通过深度学习，直接通过摄像头数据作为信号输入，再通过神经网络直接产生出机器人的控制信号。其中像 SLAM 的过程、路径规划的过程，完全通过学习的方式就可以进行实现。

一、SLAM

SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写，最早由Hugh Durrant-Whyte 和 John J.Leonard提出。

其实，SLAM更像是一个概念而不是一个算法，它本身包含许多步骤，其中的每一个步骤均可以使用不同的算法实现。主要用于解决移动机器人在未知环境中运行时即时定位与地图构建的问题。

当你身处异地，怎么准确找到想去的地方？在户外迷路时，怎么找到回家的路？没错，我们有导航软件和户外地图。

1.选择地图

和人类绘制地图一样，机器人描述环境、认识环境的过程主要就是依靠地图。它利用环境地图来描述其当前环境信息，并随着使用的算法与传感器差异采用不同的地图描述形式。

机器人学中地图的表示方法有四种：栅格地图、特征地图、直接表征法以及拓扑地图。

（1）栅格地图

机器人对环境地图的描述的方式最常见的为栅格地图(Grid map)或者称为Occupancy Map。栅格地图就是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。

这种地图看起来和人们所认知的地图没什么区别，它最早由NASA的Alberto Elfes在1989年提出，在火星探测车上就用到过，其本质是一张位图图片，但其中每个“像素”则表示了实际环境中存在障碍物的概率分布。

一般来说，采用激光雷达、深度摄像头、超声波传感器等可以直接测量距离数据的传感器进行SLAM时，可以使用该地图。这种地图也可以通过距离测量传感器、超声波（早期）、激光雷达（现在）绘制出来。

（2）特征点地图

特征点地图，是用有关的几何特征（如点、直线、面）表示环境，常见于vSLAM（视觉SLAM）技术中。

相比栅格地图，这种地图看起来就不那么直观了。它一般通过如GPS、UWB以及摄像头配合稀疏方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。

（3）直接表征法

直接表征法中，省去了特征或栅格表示这一中间环节，直接用传感器读取的数据来构造机器人的位姿空间。

上图就是直接记录了屋子内天花板画面的图像地图。这种方法就像卫星地图一样，直接将传感器原始数据通过简单处理拼接形成地图，相对来说更加直观。

（4）拓扑地图

拓扑地图，是一种相对更加抽象的地图形式，它把室内环境表示为带结点和相关连接线的拓扑结构图，其中结点表示环境中的重要位置点（拐角、门、电梯、楼梯等），边表示结点间的连接关系，如走廊等。这种方法只记录所在环境拓扑链接关系，这类地图一般是由前几类地图通过相关算法提取得到。

比如扫地机器人要进行房间清扫的时候，就会建立这样的拓扑地图：

（5）小结

在机器人技术中，SLAM的地图构建通常指的是建立与环境几何一致的地图。

一般算法中建立的拓扑地图只反映了环境中的各点连接关系，并不能构建几何一致的地图，因此，这些拓扑算法不能被用于SLAM。

直接表征法类似卫星地图，它是直接使用传感器（一般是图像传感器）构建得到。这种方法的信息冗余度最大，对于数据存储是很大的挑战，同时，机器人要从中提取出有用的数据也要耗费一番周折，因此在实际应用中很少使用。

特征点地图又是另一个极端，虽然数据量少，但是它往往不能反应所在环境的一些必须的信息，比如环境中障碍物的位置。vSLAM技术中，多采用这种地图来解决机器人定位问题。想让机器人进行自主避障和路径规划，还需要额外配置距离传感器，如激光雷达、超声波来完成。

栅格地图，或者Occupancy Map（占据地图）恰好介于其中，一方面它能表示空间环境中的很多特征，机器人可以用它来进行路径规划，另一方面，它又不直接记录传感器的原始数据，相对实现了空间和时间消耗的最优。因此，栅格地图是目前机器人所广泛应用的地图存储方式。

2.定位及传感器选择

当你打开手机中的导航软件，在选择前往目的地的最佳路线之前，首先要做的动作是什么呢？没错，就是定位。我们要先知道自己在地图中的位置，才可以进行后续的路径规划。

在机器人实时定位问题中，由于通过机器人运动估计得到的机器人位置信息通常具有较大的误差，我们还需要使用测距单元得到的周围环境信息更正机器人的位置。

关于定位方案的选择，具体看这里：干货|服务机器人常用的定位导航技术及优缺点分析

目前，常见的测距单元包括激光测距、超声波测距以及图像测距三种。其中，凭借激光良好的指向性和高度聚焦性，激光雷达已经成为移动机器人的核心传感器，同时它也是目前最可靠、最稳定的定位技术。

自1988年被提出以来，SLAM的理论研究发展十分迅速。在实际应用时，除配备激光雷达外，还需要机器人具有IMU（惯性测量单元）、里程计来为激光雷达提供辅助数据，这一过程的运算消耗是巨大的，传统上需要PC级别的处理器，这也成为限制SLAM广泛应用的瓶颈之一。

3.传感器数据预处理

这是一个完整的SLAM和导航系统的主要架构图：

其中，SLAM核心过程包括3个步骤，第一步称为预处理。我们知道，激光雷达和其他雷达设备一样，某一个时刻只能获取它所在位置的环境信息。

这就是我们所说的点云，它只能反映机器人所在环境中的一个部分。第一步预处理就是对激光雷达原始数据进行优化，剔除一些有问题的数据，或者进行滤波。

4.匹配

第二步是匹配，也就是说把当前这一个局部环境的点云数据在已经建立地图上寻找到对应的位置，这个步骤非常关键。

这个是ICP的点云匹配算法，用于实现匹配。说这个过程关键，就是因为它的好坏，直接影响了SLAM构建地图的精度。这个过程和我们玩拼图游戏有点类似，就是在已经拼好的画面中找到相似之处，确定新的一个拼图该放在哪里。

在SLAM过程中，需要将激光雷达当前采集的点云（红色部分）匹配拼接到原有地图中。

如果不进行匹配过程，所构建的地图可能就乱成一团，变成这样。

5.地图融合

在这个部分完毕以后，就进行第三步，地图融合，也就是将这一轮来自激光雷达的新数据拼接到原始地图当中，最终完成地图的更新。

就像这个图一样，这个过程是永远伴随SLAM过程的。

数据融合和简单的贴图是有很大的差异的。因为实际上传感器描绘的世界存在一定的误差，或者正巧在这个时间环境有了变化，例如机器人旁边闯入了一只小猫。

因此，实际要进行的过程会更加复杂，需要用很多概率算法，并采用滤波的方式进行融合。将上述这个过程逐次执行，就最终产生了我们看到的栅格地图。

6.Loop Closure（回环）问题

这个过程听起来其实并不复杂，但是要处理好有很大难度。这里举几个例子，比如叫做Loop Closure（回环）问题。如果匹配算法不足够优秀，或者环境中存在很不巧的干扰，当机器人绕着环境一圈后，就会发现原本是应该闭合的一个环形走廊断开了。

比如正常地图应该这样：

如果处理不好，实际地图就成这样：

对于环境比较大的场景，回环问题是不得不面对的，但现实总是不完美的，即使是激光雷达这种高精度传感器，也难免存在误差。而回环问题的难点恰恰在于在一开始出现少许误差的时候，并不会被发觉，直到机器人绕着环路一圈，随着误差的累加，发现环路已经无法闭合时，此时已经酿成大错，一般很难回天。

当然这个问题并不是无解，一个好的商用化SLAM系统，回环问题是否能很好的解决，就成为评判这个系统实力的指标了。

这是前两天在『思岚科技』办公室进行的测试，左边的视频是基于开源的ROS机器人操作系统进行的地图构建，右边的是基于SLAMWARE构建的地图。

『视频暂缺』

当机器人已经绕场一周后，ROS构建的地图出现了中断，而SLAMWARE构建的地图是一个完美的闭环，它与我们办公室的设计图完美重合。

除了算法层面的回环问题，SLAM实际应用中还有很多这种坑，比如走廊问题与外界干扰问题。

以外界干扰问题来说，通常，激光雷达作为机器人的眼睛，一般是安装在底盘上的，它能看到的视野很有限。当受到外界干扰（人类或者宠物等等）后，机器人很容易丢失定位精度，无法正常完成后续的建图工作。

当机器人安装SLAMWARE后，机器人受到干扰，可以完全不受影响，照样能够正常工作。

『视频暂缺』

目前，SLAM的开源实现代表多为学术界，实际应用有很多Corner Case要处理，需要传感器、系统参数、其他辅助设备的联合调优。

一般来说，上述的SLAM过程对于运算消耗是巨大的，虽然并没有达到像训练神经网络动用服务器集群那种地步，但传统上需要PC级别的处理器。

除配备激光雷达外，还需要机器人具有IMU（惯性测量单元）、里程计来为激光雷达提供辅助数据，否则SLAM系统也难以得到运行。总的来说，SLAM算法本身是一个对于外部系统有着多种依赖的算法，这是一个切实的工程问题。

7.算法优化及商用

很多机器人，比如扫地机是不可能装一个PC进去的，为了让SLAM能在这类设备里运行，除了解决激光雷达成本外，还要对SLAM算法做出很好的优化。

扩展：

这也是我们思岚科技SLAMTEC主要的努力方向，一方面，我们这7年多很好的解决了各类实际SLAM算法难点，另一方面，我们把SLAM这个复杂的系统做了很大的优化，可以放到一个硬币那么大的模块内部，降低尺寸功耗。此外，它还集成了IMU等配套传感器，力求做到对于SLAM的使用便捷性。

二、路径规划

那么，机器人利用SLAM技术得到了有效的空间信息后，它是怎样实现路径规划的？SLAM和路径规划之间关系是怎样的？

实际上，SLAM算法本身只是完成了机器人的定位和地图构建两件事情，与我们说的导航定位并不是完全等价的。这里的导航，其实是SLAM算法做不了的。它在业内叫做运动规划(Motion Planning)。

运动规划是一个很大的概念，从机械臂的运动、到飞行器的飞行，再到这里我们说的扫地机的清扫路径规划，都是运动规划的范畴。

我们先谈谈针对扫地机这类轮式机器人的运动规划。这里所需的基础能力就是路径规划，也就是一般在完成SLAM后，要进行一个叫做目标点导航的能力。通俗的说，就是规划一条从A点到B点的路径出来，然后让机器人移动过去。

1.全局规划

要实现这个过程，运动规划要实现至少两个层次的模块，一个叫做全局规划，这个和我们车载导航仪有一点像，它需要在地图上预先规划一条线路，也要有当前机器人的位置。这是由我们的SLAM系统提供出来的。行业内一般会用叫做A*的算法来实现这个过程，它是一种启发式的搜索算法，非常优秀。它最多的应用，是在游戏中，比如像星际争霸、魔兽争霸之类的即时战略游戏，都是使用这个算法来计算单位的运动轨迹的。

2.局部规划

当然，仅仅规划了路径还是不够的，现实中会有很多突发情况，比如正巧有个小孩子挡道了，就需要调整原先的路径。当然，有时候这种调整并不需要重新计算一遍全局路径，机器人可能稍微绕一个弯就可以。此时，我们就需要另一个层次的规划模块，叫做局部规划。它可能并不知道机器人最终要去哪，但是对于机器人怎么绕开眼前的障碍物特别在行。

3.已知地图（A*）与未知地图（D*）算法

这两个层次的规划模块协同工作，机器人就可以很好的实现从A点到B点的行动了，不过实际工作环境下，上述配置还不够。比如A＊算法规划的路径是根据已知地图，预先规划好的，一旦机器人前往目的地的过程中遇到了新的障碍物，就只好完全停下来，等待障碍物离开或者重新规划路径了。如果扫地机器人买回家，必须先把屋子都走一遍以后才肯扫地，那用户体验就会很差。

为此，也会有针对这类算法的改进，比如SLAMWARE内我们采用改良的D*算法进行路径规划，这也是美国火星探测器采用的核心寻路算法。这是一种动态启发式路径搜索算法，它让机器人在陌生环境中行动自如，在瞬息万变的环境中游刃有余。

D*算法的最大优点是不需要预先探明地图，机器人可以和人一样，即使在未知环境中，也可以展开行动，随着机器人不断探索，路径也会时刻调整。

4.空间覆盖（space coverage）

以上是目前大部分移动机器人都需要的路径规划算法，而扫地机器人作为最早出现在消费市场的服务机器人之一，它需要的路径规划算法更为复杂。

一般来说，扫地机需要这么几个规划能力：贴边打扫、折返的工字形清扫以及没电时候自主充电。单单依靠前面介绍的D*这类算法，无法满足这些基础需要。

扫地机器人还需要有额外的规划算法，比如针对折返的工字形清扫，有很多问题要处理。扫地机如何最有效进行清扫而不重复清扫？如何让扫地机和人一样，理解房间、门、走廊这种概念？

针对这些问题，学术界长久以来有一个专门的研究课题，叫做空间覆盖（space coverage），同时也提出了非常多的算法和理论。其中，比较有名的是Morse Decompositions，扫地机通过它实现对空间进行划分，随后进行清扫。

20世纪70年代，卡内基梅陇大学（CMU）完全依靠超声波做到了现在我们扫地机的行为，当然造价也十分昂贵。

前面介绍的从A点到B点移动路径规划也是实现这类更高级路径规划的基础。实际上，要从SLAM实现到扫地机器人所需要的这些功能，还是有非常多的工作要做的。

扩展：

针对扫地机器人，我们将其特有的路径规划功能预先内置在SLAMWARE中，方便厂家进行整合，不需要进行二次开发。

好消息！

小白学视觉知识星球

开始面向外开放啦👇👇👇

下载1：OpenCV-Contrib扩展模块中文版教程

在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。


下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。


下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。


交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~