服务机器人是如何实现自主定位导航的？

      服务机器人想要自由行走，实现自主定位导航是关键，自主定位导航包括定位、建图与路径规划。SLAM作为机器人定位导航的核心技术，正不断获得行业内的重视，但SLAM只是完成定位与地图创建这两件事，它并不完全等同于自主定位导航。那么，在实际应用时，SLAM究竟是如何实现的？在实现过程中又有哪些难点？下面将为大家一一解答。

      当我们身处异常，导航软件和户外地图成为我们找路最有利的工具，服务机器人和人类一样也需要依靠地图来感知外部的环境，通过算法与传感器差异的不同机器人会采取不同的地图描述形式。而栅格地图是目前机器人使用最广泛的地图存储方式。

       栅格地图就是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。这种地图看起来和人们所认知的地图没什么区别，它最早由NASA的Alberto Elfes在1989年提出，在火星探测车上就用到过，其本质是一张位图图片，但其中每个“像素”则表示了实际环境中存在障碍物的概率分布。一般来说，采用激光雷达、深度摄像头、超声波传感器等可以直接测量距离数据的传感器进行SLAM时，可以使用该地图。这种地图也可以通过距离测量传感器、超声波（早期）、激光雷达（现在）绘制出来。

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

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

?

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

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

?

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

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

?

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

?

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

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

?

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

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

　　这个过程听起来其实并不复杂，但是要处理好有很大难度。

　　这里举几个例子，比如叫做loop closure(回环)的问题。如果匹配算法不足够优秀，或者环境中存在很不巧的干扰，当机器人绕着环境一圈后，就会发现原本是应该闭合的一个环形走廊断开了。

　　比如正常地图应该这样：

?

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

?

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

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

　　这是前两天在我们办公室进行的测试，当机器人已经绕场一周后，ROS构建的地图出现了中断，而Slamware模块构建的地图是一个完美的闭环，它与我们办公室的设计图完美重合。

?     除了算法层面的回环问题，SLAM实际应用中还会碰到其他坑，比如走廊问题与外界干扰问题。以外界干扰问题来说，通常，激光雷达作为机器人的眼睛，一般是安装在底盘上的，它能看到的视野很有限。当受到外界干扰（人类或者宠物等等）后，机器人很容易丢失定位精度，无法正常完成后续的建图工作。

　　当机器人安装SLAMWARE后，机器人受到干扰，可以完全不受影响，照样能够正常工作。目前，SLAM的开源实现代表多为学术界，实际应用有很多corner case要处理，需要传感器、系统参数、其他辅助设备的联合调优。

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

　　除配备激光雷达外，还需要机器人具有IMU（惯性测量单元）、里程计来为激光雷达提供辅助数据，否则SLAM系统也难以得到运行。总的来说，SLAM算法本身是一个对于外部系统有着多种依赖的算法，这是一个切实的工程问题。我们知道很多机器人，比如扫地机是不可能装一个PC进去的。为了让SLAM能在这类设备里运行，除了解决激光雷达成本外，还要对SLAM算法做出很好的优化。

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

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

?

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

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

?

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

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

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

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

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

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

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

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

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

?

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

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

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