基于Mathematica的机器人仿真环境（SLAM篇）

目的

　　本文介绍在 Mathematica 中实现一个 SLAM 仿真的步骤，主要面向初学者。这个 SLAM 基于线特征，并使用 EKF 方法更新状态。
　　为什么研究 SLAM？
　　SLAM（Simultaneous Localization and Mapping）的意思是“同步定位和地图创建”，它是移动机器人领域研究的一个问题。没有接触过SLAM的同学可能会觉得它高深莫测，实际上现在很多介绍SLAM的文章对于初学者确实不那么容易理解，不够“接地气”，可能是它们隐藏了太多背景细节，又或者是作者本人理解得就不深刻。本文面向没有基础的初学者，所以采用了启发式的表达方式，此外在背景介绍时的思维更发散一些、眼光更长远一些（“废话”也更多一些）。下面，我先用直白的语言介绍下为什么要研究 SLAM。
　　笔者小时候喜欢看“动物世界”和“人与自然”这样的节目，同时对机器人也有着强烈的兴趣。因此，生物、机器人、大自然也顺理成章地成为我思考的对象。一个有趣的问题是：植物与动物最大的区别是什么？答案当然见仁见智，不过如果结合本文讨论的主题，那我认为区别最大的就是：动物有神经系统，而植物没有。如果再问：神经系统是干什么的？答案很明显：是加工处理信息的。进一步追问下去：为什么信息对动物那么重要，反观植物却不需要一个专门处理信息的系统。答案似乎很简单：因为动物要“运动”（更准确地说是“移动”）。千万不要小瞧“运动”，要实现高效的运动非常困难，不管是动物还是机器人。运动是个复杂又深刻的话题，它不仅牵扯到空间和时间的变化，还与环境有着复杂的相互作用，最关键的是它需要很多的信息。想象一下，一只兔子与一只狐狸在野外不期而遇，兔子要想活命，需要知道自己离狐狸有多远，该往哪个方向逃跑，逃跑的路线大概是什么，逃跑的过程怎么躲避障碍物，如何利用上一次“死里逃生”的经验。兔子每一步关于运动的决策都离不开“信息”。这么多的信息必然需要一个“信息处理器”，对于动物就是神经系统，对于机器人就是 CPU。如果根据重要性对信息排排序，恐怕没有什么比知道自己的“空间位置信息”更重要的了，2014诺贝尔奖生理学奖就是关于“表示动物位置和方向的神经细胞”的新发现。正是因为位置信息如此重要，经过上亿年的进化，动物的大脑中出现了专门编码位置信息的神经细胞。
　　我费了半天劲谈论“信息”对动物的作用，你自然想问：站在“信息”的角度对理解SLAM有帮助吗？我们可以思考一下，机器人自身的位置是信息、地图本身携带着重要的环境信息、里程计的读数反映的是局部环境的信息、激光传感器或者摄像头采集的还是信息，“信息”的身影出现在SLAM整个研究过程中，甚至可以说SLAM就是一个信息处理的过程。遗憾的是，在现有的文章中，几乎没有人强调信息这个概念的。本文既然面向非专业人士，似乎应该尽量避免使用抽象的概念才对。不过对于 SLAM 来说，把众多看似不同的概念用“信息”统一起来反而会更容易理解。因此，我建议读者在脑海中时刻具有“信息是成功的前提”的意识。

1. 定位

　　SLAM解决两个问题，第一个问题是如何定位（Localization）。简单地说，定位就是确定位置。解释地更具体一点就是：确定自己在空间中的位置，有时候还要确定方向，合起来叫位姿（位置和姿势 pose）。
　　
1.1 机器人为什么要定位？

　　知道自己的位置很重要吗？位置信息不是必需的，机器人可以在不知道自己位置的前提下完成简单的任务，可是如果想让机器人更高效的完成更复杂的任务，就有必要知道位置信息了。这一点不只适用于机器人，你自己就可以体会位置信息的重要性。设想你在家里的客厅，我交给你个任务：把卫生间里的毛巾拿到卧室。为了给这个简单的任务增加点难度，我要求你在走动时闭上眼睛。如果你对你家的房间布局非常熟悉，那么这个任务并不是很难。因为你已经对家这个环境“心中有数”，你清楚每个房间的面积、每个门的朝向和位置、每一件家具的摆放。如果再增加点难度呢？我要求你不仅始终保持眼睛紧闭，而且还要在动身之前快速原地旋转十圈，然后再执行任务。此时你还能像上一次那么顺利吗？我们可以继续加大难度，把前面的环境换成你朋友家试试。
　　上面这组“实验”是个不断剥夺你信息的过程，你逐渐丢失了自身的位置信息、方向信息、环境信息。随着你所掌握的信息的减少，做出正确决策的难度在增大。反之，信息越充裕，越有可能做出正确而且高效的决策。人类信仰的上帝不就是全知全能的吗，全知（掌握所有信息）是全能（正确决策）的前提。
　　如果从信息的角度看，在《机器人学、机器视觉与控制》（Robotics, Vision and Control）一书中，机器人导航这一章其实就是按照机器人的信息从少到多的思想展开的。
　　因此，“定位”被认为是实现真正自主的机器人（而不是靠人遥控的玩具）的首要步骤。定位这么有价值的课题已经得到了广泛的研究，人们发明了很多种定位方法。一种常用的定位方法是使用GPS，但是GPS的无线电信号太弱不能穿透混凝土墙壁，所以无法用于室内定位。而且GPS的精度不高，误差在几十厘米到几米之间。在机器人领域，最简单、廉价的获取位置信息的方法是“里程定位”（Odometry）$^{[1]}$。
　　“里程定位”（Odometry）的意思是：
　　Odometry is the use of data from the movement of actuators to estimate change in position over time through devices such as rotary encoders to measure wheel rotations.
　　“里程定位是指使用传感器测量执行器（例如通过旋转编码器测量机器人车轮的旋转量）得到的运动数据来估计机器人的位置随时间的变化。”
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　—— Wiki : Visual odometry
　　还有一个相似的概念：航迹推测（Dead reckoning），它的意思是：
　　Dead reckoning is the process of calculating one’s current position by using a previously determined position, and advancing that position based on known or estimated speeds over elapsed time and course.
　　“航迹推测是指在前一时刻已经确定的位置的基础上根据已知的或者估计的运动速度来计算机器人当前的位置。”
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　—— Wiki : Dead reckoning
　　二者的意思差不多，很容易混淆，区别只在于使用的数据来源——是用传感器测得的运动机构的数据估计机器人的位置还是直接用已知或测得的机器人整个身体的运动速度来估计机器人的位置。
　　有些时候，机器人的控制量（比如车轮的转速）很容易得到，那么我们使用里程定位更方便；可是当机器人在复杂的地形中运动时，机器人的车轮可能经常打滑，那么依靠车轮的速度估计运动了多远可能不准确。如果机器人携带了加速度计等传感器，那么我们就能得到机器人身体的运动速度，此时使用航迹推测更准确、方便。航迹推测不只限于机器人领域，在生物界也使用地非常多。如果睁着眼睛走路，你能很容易地确定自己的位置；当你闭着眼睛走路时，虽然不能准确地判断自己的位置，但是你能凭感觉大概估计出自己走了多远，方向变化了多少，这个原理就是航迹推测。
　　我们能完全相信航迹推测吗？想知道答案，我们就仿真看一下，下图所示的例子是一个小车依靠航迹推测定位的过程，红色的轨迹是小车真实的轨迹，而黑色虚线的轨迹是靠航迹推测估计的。为了模拟传感器的误差，我添加了一点随机噪声。可以看到刚开始它们相差不大，但是随着时间的增加，偏差越来越大，以至于最后估计完全不能信赖了。所以答案是：不能，航迹推测会随着误差的累计而逐渐偏离真实的位置。我们还知道的一点是：航迹推测在短时间内还是比较可靠的，从这个例子看就是，局部观察每一部分轨迹，它们还是非常相似，但是累加起来就不靠谱了。

2. 室内环境表达
　　
2.1 直线特征
　　
　　为了简单，本文假设环境中的物体全部由直线段组成。对于室内环境来说，这个假设还是比较符合实际的，因为随便抬眼一看便会发现人造环境中充满了直线特征，如下图所示的两个例子。

　

　　利用 Mathematica 自带的ImageLines函数，我们可以找出图片中的直线，代码如下。找到的直线如下图中的红线所示，结果还是挺准确的。

image = Import["C:\\indoor.jpg"]; (*导入图片，这里的地址是图片的存储位置*)
lines = ImageLines[EdgeDetect[image, 5], 0.1];
HighlightImage[image, Line /@ lines]

　　本文针对2D SLAM，所以只关心室内环境的平面投影。一般的室内环境是什么样子的呢？只需要上网搜索“户型图”就知道了，如下图所示的例子。

　　我们只考虑环境中的墙壁，所以可以将环境简化为用直线段表示的墙壁。定义墙壁如下，它由数段直线段构成，而每条直线段又由两个二维点组成：

walls = {{{0, 0}, {0, 15}}, {{0, 15}, {25, 15}}, {{25, 15}, {25, 0}}, {{25, 0}, {0, 0}}, {{0, 0}, {0, 15}}, {{8.5, 15}, {8.5, 10}}, {{8.5, 10}, {7.5, 10}}, {{0, 7.5}, {5, 7.5}}, {{5, 7.5}, {5., 10}}, {{0., 10}, {2.5, 10}}, {{4.5, 10}, {5, 10}}, {{7.5, 7.5}, {8.5, 7.5}}, {{8.5, 7.5}, {8.5, 4.5}}, {{8.5, 2.5}, {8.5, 0}}, {{8.5, 1.5}, {9, 1.5}}, {{8.5, 5.5}, {14.5, 5.5}}, {{11, 5.5}, {11, 1.5}}, {{11, 1.5}, {10.5, 1.5}}, {{16.5, 5.5}, {20, 5.5}}, {{17.5, 5.5}, {17.5, 0}}, {{10.5, 1.5}, {11, 1.5}}, {{11, 1.5}, {11., 0}}, {{15.5, 15}, {15.5, 12.5}}, {{15.5, 10.5}, {19.5, 10.5}}, {{19.5, 10.5}, {19.5, 8}}, {{19.5, 8}, {15.5, 8}}, {{15.5, 8}, {15.5, 10.5}}};
Graphics[{Thickness[0.005], Line[walls]}, Frame -> True]