机器人导航认知随想- 大数跨境

秘塔科技

2018-11-19

导读：从机器人身上学习人的认知

眨眼之间，2018年快要结束了。这个年份我一直很有印象，因为我是电影《终结者》的铁杆粉丝，其中有一部《终结者2018》，虽然没有施瓦辛格亲自参演，但是有他老人家年轻时候的高保真三维渲染，还是很棒的。科幻电影很尴尬的一点就是，在编剧的时候容易高估人类的科技发展水平。《终结者2018》是09年的电影，也没能逃脱这个尴尬。事实上人类在2018年了还是远远没有达到能造出天网的水平。不过在导航的问题上，机器人与人的差距相比其他问题来说，不算太大，而且与人的导航方式有很大的相似性。本文就来简单地聊聊机器人是如何通过认识外境，实现导航的，这和人的认知过程又如何相似，而我们又能从中得到怎样的启发。

要让机器人能实现自主导航，必须完成以下几件事情：

1. 让机器人认识面前的空间

要实现这一点，就需要实现三维地图的绘制(Mapping)。这个地图绘制必须是三维的，因为咱们这讨论的是高端的人形机器人（例如终结者T-800），不是扫地机器人。人形机器人面临的很多障碍都分布在不同的高度（例如走着走着头被树干打到）。

2. 让机器人知道自己在地图中的位置

也就是要解决机器人自我定位的问题(Localization)，从而决定自己的位置和朝向。如果不知道自己的定位，就没法根据地图进行导航。

3. 让机器人知道自己的速度大小及方向(Odometry)。

如果不知道自己的当前速度，就无法规划路径（想象一下跑得太快来不及转弯而撞墙）。

4. 进行路径规划(Navigation)

这个跟我们平常使用的高德地图的路径导航差不多，就是根据起点和目标点，计算一条最优路径，让机器人沿着路径走。但是要注意临时蹦出来的障碍，要能够避让。

认识面前的空间(Mapping)

如何让机器人像人一样对空间产生概念？最简单的方法就是提前做好一张地图下载到机器人身上。但这太不高端了，连扫地机器人都不如，因为一旦走出了地图区域，没地图时就傻了。因此，机器人要和人一样，有自主绘制地图的能力，一边自己走，对自己周围的空间就越来越有概念，地图就逐步画出来了。

要画三维地图，首先要有一对眼睛。抛开360度雷达的话（终结者T-800是纯爷们机器人，不用雷达这么无聊的东西，所以你在它身后偷袭它，它也是硬抗，然后转过身来揍你），机器人的眼睛，一般是用Kinect一类的结构光深度摄像头，或者双目立体摄像头。无论哪种，归根结底和人眼非常相似，都能得到眼睛所看到的每一个像素的RGBD的四维信息，也就是红绿蓝三色，以及景深(Depth)。我们人眼也是这样，除了颜色，还有景深。机器人的眼睛无时无刻在收集最新的RGBD信息，大概一秒收集几十次，相当于一秒钟拍几十张照片，其中每一张照片可以认为是由三维空间中的点构成的云，叫点云(Point Cloud)，大概长这样：

这张图看起来像是照片实际上是由许多三维空间的点组成的，只是每个点上加了RGB的贴图。绘制地图的关键，在于寻找两个略微不同的点云之间的相对旋转和位移（Transform）矩阵。寻找两个点云之间的Transform矩阵最常见的算法叫ICP(Iterative Closest Point)，大概思路就是用一种迭代的方式不断地尝试更优的Transform矩阵，使得错误函数尽量减小。

有了两个点云之间的Transform之后，就可以将点云B中的点，通过坐标系变换投影到点云A的坐标系中，从而实现两个点云的融合，如下图：

这其实就是三维场景重建的技术。机器人自己绘制三维地图，就是不断地对自己身边的环境进行三维重建。机器人刚进入一个新环境时，脑子中的地图是空的，睁开了第一眼，就有了第一个点云：

随着机器人不断地走动，通过拼接点云，脑子中的地图逐渐扩大：

直到一个关键状态：闭环(Loop Closure)的发现：

所谓闭环(Loop Closure)就是指的机器人从第一个点云A开始，经过拼接B,C,D,…等等若干不同的点云照片，最后有一张点云照片X与之前的某一个点云（例如点云A）匹配上了，并且根据坐标系转换，发现X中的点与A中的点坐标是基本一致的，这就形成了一个闭环。形成闭环的意义在于机器人对于整个环境算是心里有谱了，有底了，有信心了。试想一下，假设点云A代表房间内的一扇门。在没形成闭环之前，假设某一次做点云匹配时误配了，这会导致机器人将大量的点的坐标搞错，在此基础上后续的点云的坐标也将全错，最终导致当机器人再次看到房间中这扇门时，虽然照片能与A匹配上，但点云的坐标经过若干矩阵的计算，已经和门上的点的坐标相差甚远了。因此，只有形成了闭环，才能基本保证机器人脑子中的地图是靠谱的（这都怪ICP这类算法，不是百分之百保险）。

有了三维点云模型之后，还需要对其进行简化，否则点太多运算量太大。这一步一般是将点云转换成一个一个的小方块，小方块的数量远远小于点的数量。转换的结果叫做Occupancy Grid，大概张这样：

这样的三维地图，可以帮助机器人在任意高度范围内都不会碰到障碍（画地图时没遇到的东西除外），但是为了后面的导航，一般还需要一张二维的地图。此时只需要将三维地图投影到平面即可。二维地图一般长这样：

至此，机器人脑子里就构建出了面前空间的地图了。

机器人的自我定位(Localization)

机器人的自我定位，和人的自我定位是很相似的。假设一个人去逛一个陌生的公园，刚走进去时是不知道自己在哪里的。走着走着，发现一个公园地图。此时要搞清楚自己在哪里，就是先观察周围的特征，有什么墙，有什么道路，有什么湖之类的，然后在地图上面去进行匹配，得出一个位置和朝向。这个过程是一个反复试错的过程，第一次不一定就对了，有时候同行的小伙伴也可能有完全不同的看法，这个时候可以先不着急决定，保留几种可能性，再走走看。一般再多走几步路，看到前面一个更为明显的特征，就能决定正确的位置了。机器人的定位算法也是这样。最常用的算法叫Monte Carlo Localization。大概就是随时维护一个可能的位置向量集合，通过机器人对周围环境的扫描与地图进行比对，不断地更新位置向量集合中每一个向量的概率。机器人走动得越多，这个概率越确定。一开始算法会随机分配一些位置向量，如下图中的每一个蓝色点：

图中绿色箭头是所有位置向量加权平均的结果，代表了机器人目前自己认为最有可能的位置。红线则是机器人的眼睛看到的障碍。可以看出目前机器人完全是懵的。经过一定的迭代之后，大部分不靠谱的位置向量都会被丢弃：

此时机器人能够确定自己大概的位置范围了。如果机器人继续在地图范围内活动，会继续排除掉不可能的位置向量，运气好的话最终可以得到一个较为精准的估计：

可以看出此时机器人眼睛看到的障碍和地图中的障碍高度对齐，定位效果非常好。

感知速度大小及方向(Odometry)

人最主要的信息来源是视觉，通过视觉人们可以估计出自己的运动速度及方位，也很容易因为视觉错误而受骗。试想一下高速公路上由于汽车的减速玻璃，让人觉得开得比较慢，实际上已经超速了。如果人的双眼被蒙住，那就主要得靠身体的感觉了。例如有的电影中，一个被绑架了的特工，坐在车上靠计算每一次车辆的转弯来保持方位感，靠车的震动情况和风声来估计速度，最终在脑子里维护了一条完整的路径。再例如人可以靠步数来衡量距离。由此可见，人的空间速度感知是多个子系统配合产生的。

机器人的方法与人类似，首先也是要用视觉，这类算法叫做Visual Odometry。大体原理就是将连续看到的两个画面P1与P2进行匹配，求解出一个最有可能的坐标变换矩阵T，使得机器人先拍摄下P1,然后经过T的变换后就会拍摄到P2的图像。具体做法可以分为3D-3D方法和3D-2D方法。前者就是用相邻的两张点云数据直接进行ICP匹配(类似于前文地图绘制的算法)，而后者是只考虑二维图片，通过图片的特征点提取，计算出P1与P2中相同的特征点，再用一个迭代算法(Bundle Adjustment)估算出机器人眼睛的位移，使得特征点匹配的错误最小。下图是车载双目摄像头进行Visual Odometry的例子。其中第一排的黄线是P1图片中左右摄像头中共同的特征点，第二排左边是P2图片的左摄像头的画面，竖着的黄线就是P1左图到P2左图的匹配点对的连线。

通过P1与P2中相同特征点的位置变化关系，计算出变换矩阵T后，再加上拍摄两张照片的时间差即可推算出机器人当前的速度向量。

除了依靠视觉，机器人还可以将人的体感也模仿过来。最简单方法是装一个陀螺仪(Gyroscope)和加速度计(Accelerometer)。这两个仪器可以提供x,y,z三个方向上的角加速度和线加速度。这就像是配备了人的内耳一样。有了这两个神器，机器人只需要不断地累加它们的读数，就能维护一个三维速度向量。进一步地对这个速度向量进行累加，还能得到三维的位置向量。

此外，类似人可以数步数，人形双足机器人也可以数步数，而且可以数得比人精确得多。如果是用轮子或履带行走的机器人(例如Walle)，直接记录轮子转了多少圈就行了，类似于汽车的里程表的计算。机器人的车轮一般是电机带动的，所以这个问题就是数电机输出轴的转动圈数。电机输出轴只要加装了光电或者磁感编码器就能读到非常高精度的转动信息。如果是双轮以上的机器人，可以通过左右两边轮子的瞬时转速差来计算自身的旋转角速度，将这个角速度累加就能维护方向信息。

以上三种方法（视觉、陀螺仪/加速度计、里程表）都能够计算机器人的速度和方向。那么为什么还需要同时采用三种方法呢？因为每一种方法都不完美，都很容易出问题。视觉方法容易受到环境光、天气等因素影响；陀螺仪/加速度计的数据噪声较大；里程表计算问题也很多，例如如果是基于车轮的机器人，当车轮驶过不完全平整的路面时，就会造成里程误差。且这三种方法的误差都是越积越多的，因为位置向量是瞬时速度向量累加起来的。因此，机器人运行得越久，它的位置信息就越不靠谱。如果机器人始终处在有地图的环境，例如室内，那么还可以结合前面提到的定位算法进行修正，但如果是在非常空旷的地带，又没有GPS之类的信息反复提供校准，就会越来越找不到北了。

为了尽量提高定位精度，就有必要明确地计算出每一种传感器的累计误差，叫做Bias。如果有模型能随着机器人的运转而不断计算出最新的Bias值，那就可以将误差抵消掉，这就需要将多种传感器相结合。单个传感器，自己是无法计算出自己的误差的，就像人们自己往往意识不到自己犯的错误。但是不同的传感器类型是互补的，因此当有多种传感器时，可以借用其他传感器的数据来估算出某个传感器的误差。这种做法叫做Sensor Fusion。具体的常见算法叫Extended Kalman Filter，这是利用非线性模型估算每一种传感器的Bias的方法。

导航(Navigation)

讲了那么多，终于到了最终环节：如何动态地决定从一个点到另一个点的路径？有了前面的各项准备，这一步就简单了。下面一张图即可说明整个过程：

首先，机器人不断地绘制地图的同时，会同时计算一个费用地图。这个地图标出了哪些位置是绝对的障碍，哪些位置是由于机器人自身尺寸而不能进入的区域，哪些位置是机器人可以通行的区域。例如机器人自身宽0.5米，那它就不能进入离墙壁0.5米范围以内的任何区域。此外，可以通行的区域也不是平等的。例如离墙壁0.6米的地方可能不如离墙壁0.8米的地方更适合机器人通行。计算整个费用地图的过程叫做充气（Inflation），可以理解为障碍点的费用是最高的，然后越远离障碍点，费用就越低，这就像是从障碍点开始进行了充气。从上图可以看到，障碍点是黄色的，其次是紧挨着障碍点的亮蓝色，再次是红色，最后是紫色。紫色以外的区域基本就是一马平川随便走，费用近乎为0。通过调整充气的范围，可以对机器人导航策略进行微调。例如，充气充得很大，会导致机器人尽可能地远离任何障碍，走阳关大道，但是同时这就使机器人计算路径时，可选择的空间变小，搞不好连条路都算不出来。如果充气太小，机器人就可能喜欢贴着墙走，老是往多事之秋一个劲的钻。

细心的同学会发现，上图的地图其实包括了两张图，一张是Local Map，一张是Global Map。Global Map是可以事先提供给机器人的全局地图。如果没有的话，机器人就只能全靠自己的点云拼一张Global Map出来。另外一张是Local Map，这一般只包含机器人周围几米的范围之内的障碍。上图中的小矩形就是机器人的Local Map。之所以要区分Global和Local，是因为机器人导航一般由两个路径规划模块构成：Global Planner和Local Planner。其中Global Planner就和高德地图导航差不多，随时根据当前点和目标点，以及费用地图，用最短路算法计算一条路径出来。这就是上图中的绿色路径。有Global Planner不就行了吗，为什么要Local Planner呢？因为机器人面前随时会有各种人或物临时蹦出来，如果机械地按照Global Planner的路径去走，很可能会撞上。机器人的双眼随时将面前看到的障碍写入Local Map中，Local Planner根据Local Map的费用情况，随时计算出眼前两三米内的路径，这就是图中的红色部分。只不过图中刚好Local Path和Global Path重合了。机器人最终的行动方向，是Local Path和Global Path的加权平均。例如，机器人面前有一个垃圾桶，刚好处在Global Path上，为了躲避这个垃圾桶，Local Path指挥机器人朝左走。这时由于垃圾桶就在面前，费用很高，机器人不得不往左走。当走过垃圾桶后，如果没别的特别的障碍在身边，Local Planner就会让机器人往Global Path靠拢，因此机器人又会逐渐走回到Global Path上。一般来说Global Planner和Global Map的计算频率要低于Local Planner和Local Map，因为前者计算量大，且大环境相对稳定，没有必要不断地计算最优路径。但是Local Planner和Local Map计算频率要尽量地高，才能及时避开障碍。

人与机器人在空间认知上有多大差别？

至此，机器人的自主导航所需要的主要技术就介绍完了。在整个过程中，机器人是同时不断地进行地图绘制、自我定位、以及导航的。这叫做SLAM and Navigation。其中SLAM是同时进行自我定位与地图绘制(Simultaneous Localization and Mapping)的意思。听起来很高端，但就是我们逛公园看地图干的事情。仔细想想，上面介绍的每一项技术，其实现思路都是借鉴了人的经验的。我们自己到一个陌生环境，就是在脑子里面拼接地图，不断地完善一个三维场景；我们的定位，就是根据眼前所见与地图进行匹配，然后去猜一个方向和位置；我们的速度感，大部分来自于视觉（火车停站时，旁边的火车开了，人就会有自己火车发动的错觉）；我们每天上班的路上，也是一个Global Planner加一个Local Planner。Global Planner为我们计算上班路线：乘坐几号线转几号线，且换乘时选最近的车厢。但是一般最近的那节车厢根本挤不上去，于是Local Planner告诉我们去尝试旁边的车厢。可见学习了那么多机器人的算法，同时也是对我们自身的“算法”有所了解。只不过我们不去注意它们，它们只是在潜意识中默默运行。

然而，我们与机器人类似的还不仅仅是算法的实现过程，更深刻的相似性是算法的输入信息。例如，机器人获得空间认知的流程是：

景深信息->点云->点云合成的三维场景地图

这个认知过程的根本信息来源是带有景深信息的数码照片，而数码照片的本质是一组数字。我们人获得空间感的根本来源是什么？也是景深信息，只是比起机器人的数码相机，人眼更像是胶片机。如果是盲人，也是需要用手摸或者用一根棍子来探索空间，探索得到的直接信息并不是空间本身，而是盲人假想的不同空间坐标上，有没有碰到障碍的触感，有触感就是1，无触感就是0。所以盲人获取空间信息的输入数据也是类似于一组数字信号。不管哪种方式，人都是在大脑中根据输入的信号形成点云数据，然后和先前的点云数据做合并，得到越来越大的三维场景地图。只不过这个过程太熟练了，我们毫不在意。

如果你闭上眼睛，回忆一下每天上班的办公室的三维场景张什么样，那脑子中浮现的是不是会和下图差不多？

我们的记忆总是模糊的，就跟点云是残缺的一样。在我们没注意到的细节的位置，我们是没有空间信息的，所以我们得到的三维场景和机器人得到的类似，都是残缺的，只要够我们使用就行了。我们并不会专门去为一个从来没去过的“安全通道”建模。因此，每个人脑中的世界都是不同的，都是一个自己不断拼接和维护的一系列细碎零散的三维场景地图。例如家的残缺场景模型、上班地点的残缺场景模型、平常经常走的路段的残缺场景模型（路边的小巷子永远不进去的话就永远不会去建模）、不同旅游地点的残缺场景模型。当我们与其他人共处一个场景时，我们各自的场景地图都包含当前的场景，因此我们感觉都在一个共同的空间中活动。甲看有一张桌子，乙在同一个地方也看到一张桌子，因为彼此的地图中都有这张桌子。但是我们对于自己地图外的情况毫无概念。

那么问题来了：在自己地图以外，有没有空间存在？例如，如果我从没去过美国，美国到底存不存在？再进一步，我自己的三维场景地图所代表的空间是否存在?

通过给机器人传输景深数据（这个景深数据完全可以是人随意构造的），机器人脑（内存）中就可以产生一个三维空间的模型，这个模型如果是一栋房子，本质上说只是机器人以为有一栋房子，实际上这是机器人的一个妄想。我们人的信息来源：视觉、触觉、听觉等等，也都只是每个人的主观感觉信号，假设有某种机制给人提供了关于一栋房子的信号（参考电影《黑客帝国》），我们就会在脑子里构建出房子的三维场景，并且通过这些信号，我们会计算出自己运动的速度、加速度、位置感等等，并对此深信不疑，真的以为我们在这栋房子里生活。