机器人的SLAM和导航系统技术分析

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

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

2.特征点地图

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

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

3.直接表征法

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

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

4.拓扑地图

拓扑地图，是一种相对更加抽象的地图形式，它把室内环境表示为带结点和相关连接线的拓扑结构图，其中结点表示环境中的重要位置点（拐角、门、电梯、楼梯等），边表示结点间的连接关系，如走廊等。

这种方法只记录所在环境拓扑链接关系，这类地图一般是由前几类地图通过相关算法提取得到。

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

5.小结

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

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

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

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

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

二、定位及传感器选择

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

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

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

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

三、传感器数据预处理

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

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

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

四、匹配

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

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

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

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

五、地图融合

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

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

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

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

六、Loop Closure（回环）问题

这个过程听起来其实并不复杂，但是要处理好有很大难度。这里举几个例子，比如叫做Loop Closure（回环）问题。

如果匹配算法不足够优秀，或者环境中存在很不巧的干扰，当机器人绕着环境一圈后，就会发现原本是应该闭合的一个环形走廊断开了。

比如正常地图应该这样：

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

对于环境比较大的场景，回环问题是不得不面对的，但现实总是不完美的，即使是激光雷达这种高精度传感器，也难免存在误差。

而回环问题的难点恰恰在于在一开始出现少许误差的时候，并不会被发觉，直到机器人绕着环路一圈，随着误差的累加，发现环路已经无法闭合时，此时已经酿成大错，一般很难回天。

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

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

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

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

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

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

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

七、算法优化及商用

很多机器人，比如扫地机是不可能装一个PC进去的，为了让SLAM能在这类设备里运行，除了解决激光雷达成本外，还要对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实现到扫地机器人所需要的这些功能，还是有非常多的工作要做的。

编辑：黄飞