无人驾驶车核心算法—SLAM

2005年，斯坦福大学的无人驾驶车 Stanley 在 DARPA 无人驾驶车挑战赛中获得冠军。斯坦福大学教授，斯坦福人工智能实验室主任，Stanley 团队队长 Sebastian Thrun 透露说，Stanley 使用 SLAM 作为其核心驾驶算法，让 SLAM 名声大噪。如今，SLAM 已得到广泛应用，从商用扫地机器人到驰骋在美国公路上的 Google 无人驾驶汽车，都在使用 SLAM 算法。SLAM 算法也持续成为科研界的热门话题。

如果你不懂SLAM，那最好还是不要说你懂无人驾驶汽车了。今天，AI君就来科普一下高大上的SLAM算法究竟是什么？

stanley.jpg1024x609 357 KB

什么是 SLAM

SLAM 全称 Simultaneous Localization and Mapping. 即在一个静态的未知环境中，通过一个机器人的运动和测量，来学习环境地图，并且同时确定机器人在地图中的位置。

就好像打仙剑奇侠传的时候走迷宫，边走边花地图的那种体验：

gif1.gif1280x720 4.4 MB

为什么 SLAM 很难？

通过机器人的运动和测量来学习地图并不简单，这主要来自于以下几个原因：

• 一个未知环境中，所有可能的地图集合非常非常大。因为未知环境是连续的，所有可能的地图集合有无限的维度。即使考虑使用离散的环境作为近似，地图也至少会包含超过10000个变量。在如此高维空间中无法准确计算后验概率，因此使用传统的Bayes filtering 定位算法是不可行的。
• 学习地图是一个 “鸡生蛋，蛋生鸡” 的问题 。 首先，这是一个定位 (Localization) 的问题。当机器人在未知环境中运动时，每一次移动时的误差会在里程计算(Odometry)中逐渐累加，导致机器人对自己的位置越来越不确定。在一个已知地图里，我们有合适的算法来确定机器人的位置，但是在未知环境中，我们需要新的算法。 其次, 这也是一个绘制地图的问题。当机器人的位置是确定的时候，绘制地图会相对简单。但是机器人相对于探测点 (Landmark) 的位置也是不确定的时候，我们需要新的算法。在环境地图和机器人位置都缺失或不确定时，机器人需要同时完成两件事 — 学习地图和在地图中定位。下面我们来看看SLAM算法究竟是如何工作的？

pic1.png2012x716 25.9 KB

虚线点代表机器人真实运动的路线。椭圆代表机器人每次运动后，对自己位置的判断 （高斯分布）。椭圆逐渐变大代表不确定性逐渐变大。

Graph SLAM

假设机器人的起始坐标为 （0，0），第一步往右走10个单元，如果在完美的世界，机器人会到 （10，0），如下图所示。

pic2.png904x700 159 KB

但所有机器人运动都会有误差，所以他新位置是不确定的，而新位置的概率分布假设是一个高斯分布。如下图所示。

pic3.png2124x914 561 KB

从这次运动中，我们就得到了一个限制（constrain）。之后的每一次运动，以及每一次相对于landmark的测量，都会带给我们对应的限制。

pic4.png1244x775 306 KB

例如上图中，就包含了三种限制：

• I0: initial constrains
• C0, C1, C2 : relative motion constrains
• Z0, Z1, Z2 : relative motion constrains

需要找到X0, X1, X2, X3 使得所有constrains的和最小。这就将问题规约成了一个图（graph）上的 non-linear least squares problem。 这是一个标准的可解问题。

EKF SLAM

EKF是另一种SLAM的算法。它使用非线性的Kalman Filter来对当前的所有状态作出对应的高斯分布估计。并在遇到多次同一个测量点后，成功收敛。

pic5.png1676x1494 163 KB

虚线点代表机器人真实运动的路线。实心椭圆代表机器人每次运动后，对自己位置的判断 （高斯分布）。八个小点代表八个位置未知的探测点。他们周围的空心椭圆代表对他们位置的估计。

在图(a) ~ (c)中，机器人位置估计的不确定性越来越大，探测点位置估计的不确定性也越来越大。然后在图(d)中，机器人又重新遇到了第一个探测点，于是所有估计的不确定性都显著下降。

你也可以从下面的视频中直观得看到 EKF SLAM 是如何工作的。视频中线条越粗，代表不确定性越大。

gif2.gif1280x720 1.49 MB

https://discussions.youdaxue.com/t/slam/7825