行业干货|机器人如何实现自主移动与路径规划？

SLAMTEC思岚科技

智能服务机器人正成为行业的风口浪尖，从清扫机器人开始，家庭陪伴机器人、送餐机器人等陆续进入公众视线。
# A) }" |' x9 s1 _

+ ]. u3 b: Q; L* V- ]5 u4 `3 b: F' g/ n1 {

在讨论这类机器人是否能解决实际问题时，自主定位导航技术作为机器人智能化的第一步正不断引起行业内的重视。同时，作为自主定位导航技术的重要突破口，SLAM 技术也成为关注焦点。

  u) o1 d; y5 J5 L& Z6 V  S

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

6 ^! Z$ _  D6 z% h

那么，SLAM 技术究竟是如何实现的？它有哪些难点？机器人如何实现自主移动和路径规划？到底什么样的扫地机器人才算智能？

这些问题，小编将为你一一解答，带领大家了解机器人自主移动的秘密。

今天，我们先从 SLAM 说起。

9 X( [' d: I; ~/ I- x

什么是SLAM技术?

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

. N# U# Z7 }1 u: T3 K! J* j

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

. {! D* O  T" \0 D

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

4 k$ e( w% I: w- J) u  @

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

四种常见的地图表示方法是什么？

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

+ U- G3 _0 \2 c7 b1 `" |1 J

栅格地图

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

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

: o- L( N1 n1 j2 M% V1 o/ ]! L" m; c

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

特征点地图

, ^1 c' |# J7 Z7 p3 j# e

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

! I# B! ^5 o* P4 \; o4 S

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

& `7 h9 S( s0 B. f# }

直接表征法

# A4 }+ d& j7 `5 _, q

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

! F1 E& R. {$ H0 t$ O# E# Q/ D

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

9 q" h1 d: x9 l, R/ A

拓扑地图

4 Z2 E+ b+ U* a- Z' V

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

' @2 A8 {$ o% r' W7 u$ b) n

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

4 u& A7 ]  j' x' t8 E/ V

小结

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

" R6 ^: ~2 {& J+ }

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

% T# `! A5 U$ a* F% I% T  Y

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

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

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

. Z! U$ q: x+ x2 K7 F; U

思岚科技的 SLAMWARE 系统内部也采用这种地图方式。

" J, F: K4 T" s' v+ G8 C

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

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

; V% y5 _) }" W) g

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

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

" W0 R5 G2 i) R1 B0 d6 T) e

那么，实现 SLAM 的过程中，都会遇到哪些坑呢？

9 Z& s4 u, \$ I. m

SLAM 实现过程中的难点有哪些？

SLAM 主要解决机器人的实时定位与自动建图问题，那么，在实际应用时，SLAM 究竟是如何实现的呢？在实现过程中有哪些难点？

) U* V- E# P. _5 Q3 ~% S

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

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

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

9 M# s# s$ t+ Z6 i4 S1 o5 }

: ]5 A* H% K# C: K

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

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

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

) t2 C) `5 }% B+ f; R
& q) I+ Q0 J7 S

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

2 [) r# q. S2 }

7 h# e0 H! C8 M) x7 |6 ?0 O" h4 K

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

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

7 u7 Y5 |% u' N3 E8 W2 _' S: Y9 F

$ Z: Y; d. F  n& m  v4 b. K' l  A! H

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

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

! O; x% g. [* c. R- z, q' B! Q4 C7 G

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

* k+ k* y* \' w7 S

比如正常地图应该这样：

; \% I, |- {8 \9 O2 Q. y* A8 r

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

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

0 [3 i& ^) f& M$ ^- x$ P

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

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

1 [  {, I- c3 g
( A3 [$ k8 S5 C0 L

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

; {# y( A* o& J& B7 S

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

9 J5 `0 f5 s. ~$ P7 U& G

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

8 l' L+ C; I6 g6 F2 `2 E

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

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

/ V( b8 z/ a: f9 l

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

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

0 A/ F4 b$ X# v$ N

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

机器人如何实现自主移动和路径规划？

0 z5 S8 t* i- @4 O4 Y

上面我们已经知道了 SLAM 与路径规划的关系。

, J. V/ u. l; @0 o

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

. }) _  b& A  g3 k, a# m

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

) Z% X7 d$ X5 M) }" H; Z. T

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

  T1 n0 v2 M* }: `# Q% o4 u

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

! z$ k9 ?; A" K

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

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

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

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

8 U/ o' x/ ]! R- b2 h* O

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

+ D% t( D; r4 h5 d' e

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

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

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

: S' V: m( o. T% D0 A( E- |  C

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

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

% p. }5 H' j6 ~, ]# \

8 \9 V* l. L$ `0 v5 `

未来第一站

谢谢分享!

韩寒11

家里就有扫地机器人，很好用的

天天来一回

谢谢分享

simpleplanfx

干货，学习了

qwchenhao

楼主是否可以谈谈陀螺仪在机器人导航中的应用。。

nangonglyj

; f; }7 ?" K) p0 g, x