基于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
　　二者的意思差不多，很容易混淆，区别只在于使用的数据来源——是用传感器测得的运动机构的数据估计机器人的位置还是直接用已知或测得的机器人整个身体的运动速度来估计机器人的位置。
　　有些时候，机器人的控制量（比如车轮的转速）很容易得到，那么我们使用里程定位更方便；可是当机器人在复杂的地形中运动时，机器人的车轮可能经常打滑，那么依靠车轮的速度估计运动了多远可能不准确。如果机器人携带了加速度计等传感器，那么我们就能得到机器人身体的运动速度，此时使用航迹推测更准确、方便。航迹推测不只限于机器人领域，在生物界也使用地非常多。如果睁着眼睛走路，你能很容易地确定自己的位置；当你闭着眼睛走路时，虽然不能准确地判断自己的位置，但是你能凭感觉大概估计出自己走了多远，方向变化了多少，这个原理就是航迹推测。
　　我们能完全相信航迹推测吗？想知道答案，我们就仿真看一下，下图所示的例子是一个小车依靠航迹推测定位的过程，红色的轨迹是小车真实的轨迹，而黑色虚线的轨迹是靠航迹推测估计的。为了模拟传感器的误差，我添加了一点随机噪声。可以看到刚开始它们相差不大，但是随着时间的增加，偏差越来越大，以至于最后估计完全不能信赖了。所以答案是：不能，航迹推测会随着误差的累计而逐渐偏离真实的位置。我们还知道的一点是：航迹推测在短时间内还是比较可靠的，从这个例子看就是，局部观察每一部分轨迹，它们还是非常相似，但是累加起来就不靠谱了。

　　 我们该怎么办呢？问题既然出在了信息上，那就从信息的角度解决。一种方案是设计精度极高的传感器，从而最大限度地避免引入误差。但是，传感器是昂贵的玩意儿，这样的传感器价格想必也是上了天的。第二种方案则另辟蹊径，既然自身的（传感器）信息不可靠，为什么不依靠外部信息呢？机器人的工作场所一般不是空旷无物的环境，里面会有各种各样的物体，而且通常都有固定的物体。如果这些物体的位置是不动的，那么我们就可以利用这些固定物来定位。比如说，你闭着眼睛四处乱走，过了很长一段时间后你完全不知道自己在哪了。但是如果你睁开眼看到了一栋很熟悉的建筑或者地标（比如超市），你马上就知道自己在哪里了。换句话说，走的时间越长，你的位置不确定度越大，可是在睁开眼的那一刻，你的位置不确定度立马坍缩成一个非常确定的值了。
　　总之，我们的原则是：如果有能利用的信息绝不浪费。我们要让自己变得像一个饥饿的旅行者一样，不能放过任何的食物。只不过，我们的机器人是以“信息”为食的。
　　航迹推测长时间不靠谱，依靠环境地标定位只能确定它周围一定范围内的位置，那么把它们合起来怎么样呢？沿着这个思路，SLAM 就诞生了。

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]

　　
2.2 直线的Hough参数
　　
　　 既然我们假设环境中只存在直线段，那么一个很基本的问题是：如何表示一条直线段呢？要表示一条直线段只需要两个端点即可，对于2维平面环境来说只需要4个参数。而要表示一条直线有许多种方法，在高中时大家最熟悉的方法是使用“斜率-截距”形式，只需要两个参数：
$$y=kx+b\tag{1}$$ 　　但是这种方法无法表示垂直的直线，因为此时斜率无穷大。所以本文使用 Hough 参数方法来描述一条直线。Hough 参数也有两个：原点到直线的最短距离是第一个参数；过原点做直线的垂线，这条垂直线与 $x$ 轴的夹角是第二个参数。这两个参数能够唯一地确定一条直线，而一条直线具有唯一的 Hough 参数。换句话说，直线和一个二维的点是一一对应的。这样做的一个好处是，数据关联问题变的很简单$^{[2]}$。

　　 用 Hough 参数表示的直线方程如下： $$x cos(\alpha)+y sin(\alpha)=r\tag{2}$$ 　　从几何的角度理解这个方程更容易：向量$[cos(\alpha),sin(\alpha)]$与$x$轴的夹角是$\alpha$，也就是垂线指向的方向。而点或者说向量$[x,y]$向$[cos(\alpha),sin(\alpha)]$向量的投影就是计算内积：$x cos(\alpha)+ysin(\alpha)$。而只有在直线上的点向$[cos(\alpha),sin(\alpha)]$向量的投影才等于 $r$，也就是满足等式$(2)$，如下图所示。

　　下面我们来看看如何计算一条直线的 Hough 参数。
　　pointProjectOnLine函数用来计算一个点在一条直线上的投影。hough函数调用了pointProjectOnLine函数，计算原点在直线上的投影，然后计算投影点的极坐标， 也就是直线的 Hough 参数。

pointProjectOnLine[pt_, line_] := Module[{pt1, pt2, v1, v2},
  {pt1, pt2} = line;
  v1 = pt2 - pt1;
  v2 = pt - pt1;
  Projection[v2, v1] + pt1]
hough[line_] := Module[{pt},
  pt = pointProjectOnLine[{0, 0}, line];
  ToPolarCoordinates[pt](*注意变换到极坐标后，角度的范围是-Pi~Pi*)]

　　下图所示的例子展示了hough函数的计算结果，代码如下。随着图中直线的变化，对应的 Hough 参数值也在实时更新。

Manipulate[line = {pt1, pt2};
 pt = pointProjectOnLine[{0, 0}, line];
 {d, \[Theta]} = hough[line];
 Graphics[{Line@line, Red, Point@pt, Line[{{0, 0}, pt}], Blue, Circle[{0, 0}, 0.1, {0, \[Theta]}], Text[d, 1.2 pt], Text[\[Theta], {0.12, 0.12}]}, Axes -> True, PlotRange -> 1], {{pt1, {-1, 0}}, Locator}, {{pt2, {0.5, 0.5}}, Locator}]

　　
3. 机器人的眼睛

　　这一章主要介绍传感器的仿真。
　
3.1 激光测距传感器

　　有个成语叫“画龙点睛”，可见眼睛对生物有多么重要。其实重要的不是眼睛本身，而是眼睛获取的东西——信息。机器人获取环境信息可以借助很多传感器，其中最常用的是激光测距传感器（Laser Range Sensor）。本文采用的激光传感器原型是日本北阳公司（Hokuyo）的 URG-04LX-UG01，如下左图所示。这是一款面向机器人领域的学生、科研人员设计的入门级传感器，精度不是很高，但用于 SLAM 研究还是够用的。与 SICK TIM510 或者 Velodyne VLP-16 这样动不动就上万的传感器来说，URG-04LX-UG01 的价格还是很亲民的。当然，现在也有一些优秀的国产激光传感器可以选择，其价格更便宜，比如思岚科技的 RPLIDAR A2。机器人是个烧钱的行业，主要成本基本都花在硬件上，其中传感器就是一项昂贵的开支。不过随着参与者的增加，激光雷达的价格将会很快走低。
　　URG-04LX-UG01 的主要参数如下：
　　●　探测距离：20mm~5600mm
　　●　探测角度：240$^{\circ}$
　　●　角度分辨率：0.36$^{\circ}$
　　●　距离分辨率：1mm
　　●　扫描时间：100ms/scan（每秒扫描10次）　　
　　●　精度：$\pm$30mm（1000mm以内），测量距离的$\pm$3%（1000mm~4095mm）

3.2 激光传感器仿真

　　下面我们就针对这款传感器进行仿真。激光传感器的基本工作方式很简单，它向四周发出很多激光束，每个激光束碰到物体就会被反射回来并被采集到。由于光速恒定，通过计算激光发射时间和反射时间的时间差就能知道物体离传感器有多远了。而每个激光束的发射角度是已知的，这样每个激光束扫描到物体上的点的坐标就知道了。
　　我们用直线段表示真实的激光束，只要计算出直线段与物体的交点就能得到物体到传感器的距离了。激光传感器的核心是激光束，因此我们首先定义激光束函数 laserBeamsFun，代码如下。真实传感器的角度分辨率很小（360$^{\circ}$/1024=0.36$^{\circ}$），为了节省仿真时间，本文选择了较大的分辨率（$\pi$/180=1$^{\circ}$），探测距离默认是5米（这些参数你可以根据具体的传感器随意修改，下面在举例时可能会使用略微不同的参数）。这个传感器扫描角度是从$-120^{\circ}$~$120^{\circ}$，形成一个扇形区域，而背后有个$120^{\circ}$的盲区，传感器探测不到。
　　激光传感器会装在机器人身上，所以传感器的测量数据依赖机器人的位置和姿态$(x,y,\theta)$，这就是为什么我将激光束函数 laserBeamsFun定义为pose的函数，pose也就是机器人的位置和姿态$(x,y,\theta)$。记住这一点，因为后面只有激光测量数据是已知的，我们会用这些数据反过来估计机器人的位置和姿态。

laserBeamsFun[pose_: {0, 0, 0}, distanceRange_: 5, angleRange_: 2 Pi/3]:= Table[{AngleVector[pose[[1 ;; 2]], {0.02, \[Theta] + pose[[3]]}], AngleVector[pose[[1 ;; 2]], {distanceRange, \[Theta] + pose[[3]]}]}, {\[Theta], -angleRange, angleRange, Pi/180.}]
Animate[Graphics[{Red, Line /@ laserBeamsFun[][[1 ;; i]], Black, Disk[{0, 0}, 0.1]}, PlotRange -> 7], {i, 0, Length[laserBeamsFun[]], 1}]

　　为了计算激光束到达环境中物体的距离，我们需要计算两条直线段的交点，这是个基本的几何问题。函数segsegintersection用于计算两条直线段的交点，它的输入是两条直线段（每条线段由两个端点表示），输出是它们的交点坐标（如果不相交则返回空列表：{}）。这里有个容易出错的地方：如果两条直线段共线而且有重叠如何定义交点呢？为了简单，我们认为这种情况下两条直线段没有交点。

segsegintersection[linePair_] := Module[{md = Subtract @@ (Plus @@ # & /@ linePair), sub = Subtract @@ # & /@ linePair, det}, det = -Det[sub];
  If[det == 0, Return[{}]];
  If[And @@ (Abs[#] <= 1 & /@ #), (Plus @@ #[[1]] - Subtract @@ #[[1]] Last@#[[2]])/2 &@{First@linePair, #}, {}] &@(Det[{#[[1]], md}]/det & /@ ({#, Reverse@#} &@sub))]
Manipulate[
 {line1, line2} = {{p1, p2}, {p3, p4}};
 intersectPt = segsegintersection[{line1, line2}];
 Graphics[{Line/@{line1, line2}, Green, Point[intersectPt]}, PlotRange -> 4], {{p1, {2, 1}}, Locator}, {{p2, {-2, -1}}, Locator}, {{p3, {-2, 1}}, Locator}, {{p4, {3, -1}}, Locator}]

　　 对于每一束激光，我们计算它和物体几何边界的交点，如下左图中的绿点所示。传感器的激光没有穿透能力，所以每个激光束只能扫射到一个交点，也就是离激光发射头最近的那个，如下右图所示。

scan[laserBeams_, walls_] := 
 Module[{intersectionPt, firstIntersectionPt},
  Flatten /@ Table[
    intersectionPt = segsegintersection[{beam, #}] & /@ walls;
    intersectionPt = Select[intersectionPt, # =!= {} &];
    If[intersectionPt == {}, Nothing,
     firstIntersectionPt = Keys[TakeSmallest[<|Thread[# -> EuclideanDistance[#, beam[[1]]] & /@ #] &@intersectionPt|>, 1]]], {beam, laserBeams}]]

　　定义环境中的障碍物为多边形几何体：

polys = {{{4.0,-2.75},{3.0,-3.85},{1.74,-3.16},{3.85,-1.18}},{{-1.49,2.61},{-4.17,0.18},{-3.22,2.09},{-2.6,3.35}},{{0.73,1.34},{3.73,2.},{0.9,4.},{1.55,2.36}},{{4.02,1.77},{4.9,-0.2},{4.37,-1.1},{2.88,1.}},{{-3.4,0.23},{-2.08,-1.42},{-2.7,-3.},{-4.13,-0.93}},{{0.83,2.23},{-1.13,2.34},{-1.64,4.1},{0.88,3.35}},{{-2.17,-2.77},{-0.64,-2.13},{1.64,-2.34},{0.54,-3.87}}};
polySegs = Flatten[Partition[#,2,1,1]&/@polys,1];

Manipulate[
 laserBeams = laserBeamsFun[Flatten@{pt, 0}, 4, Pi];
 scanPts = scan[laserBeams, polySegs];
 Graphics[{Black, Disk[pt, 0.1], FaceForm[LightGray], Polygon/@polys, Red, Line/@laserBeams, Green, PointSize[0.005], Point /@ scanPts}], {{pt, {0, 0}}, Locator}]

　　 在实际情况下，如果激光束没有照射到物体，它就不会被反射回来，所以不能被传感器接收到。这时我们需要将超出量程的激光束丢掉，下面左图展示了这种情况。

　　 其实，我们对激光的仿真在计算机图形学中已经被广泛研究过了，它被称为：Ray Casting。这个例子展示了Ray Casting。借助激光仿真，我们可以实现一些好玩的东西，比如Visibility polygon（可视多边形）。假设环境中只有直线组成的物体，可视多边形定义为可以被照亮的多边形区域。这个概念在机器人的运动规划中会用到。
　　前面我们计算的仿真激光采集点是用笛卡尔坐标表示的，但是真实传感器的激光数据点是用极坐标表示的——激光传感器的位置就是激光束的原点，每隔固定的角度就对应一个距离值。我们可以用ToPolarCoordinates函数将数据点的笛卡尔坐标$(x,y)$转换成极坐标$(r,\theta)$，代码如下。注意这个函数得到的点的极坐标中距离在前、角度在后。善意的提醒一句：ToPolarCoordinates函数在原点处会报错（也就是不能输入{0,0}），在使用时要小心。

scanPtsPolar = ToPolarCoordinates[scanPts];
ListPolarPlot[Reverse /@ scanPtsPolar, PlotStyle -> Black]
Graphics[Point[scanPts]]

　　转换后的极坐标数据可以用ListPolarPlot函数画出来，注意ListPolarPlot函数绘图时要求角度在前、距离在后（和前面刚好是反的），所以这里用Reverse函数将顺序颠倒一下。不出意外，显示结果和笛卡尔坐标是完全一样的，如下左图所示。FromPolarCoordinates函数可以逆转：将极坐标转换为笛卡尔坐标。

　
　　 真实的世界总是不完美的，激光传感器不可能没有误差，这就意味着采集到的激光数据会包含噪声。为了模拟噪声，我们采用最常用的高斯噪声模型。高斯噪声由两个参数定义：均值和标准差。均值我们就取为$0$，标准差怎么确定呢？在传感器的参数表中可以找到它的距离误差：30mm，角度误差则未知。所以距离的标准差就定义为${\sigma}_{r}=0.03$，角度我只好估摸着来了，就定义为分辨率的十分之一吧：${\sigma}_{\alpha}=0.036^{\circ}$。它们组成了传感器的噪声矩阵：
$$W=\left[ \begin{matrix} {\sigma}^{2}_{r} & 0\\ 0 & {\sigma}^{2}_{\alpha}\\ \end{matrix} \right]$$ 　　我们将噪声添加进采集到的数据中，并画出图像（如上右图所示），代码如下。现在，我们虚拟的传感器终于比较符合实际了，下面就可以利用它来对SLAM进行仿真了。

{\[Sigma]r, \[Sigma]\[Alpha]} = {0.03, (0.36/10)*Pi/180};
noiseofLaser = Transpose[{RandomReal[{-\[Sigma]r, \[Sigma]r}, Length[scanPts]], RandomReal[{-\[Sigma]\[Alpha], \[Sigma]\[Alpha]}, Length[scanPts]]}];
scanPtsPolar = ToPolarCoordinates[scanPts] + noiseofLaser;
scanPts = FromPolarCoordinates[scanPtsPolar];

　
3.3 点云

　　经过前面的介绍，你应该已经明白了激光传感器的工作方式。对于使用激光传感器的机器人来说，它看到的世界是由一个个的点构成的，就像我们看电视上的图像其实是一个个的像素构成的。为了描述起来方便，人们给这些点起了一个诗意的名字：点云（Point Cloud）。
　　Mathematica 没有提供专门处理点云的函数，不过它倒是提供了一个处理文字的函数：WordCloud（词云）。要想在 Mathematica 中显示点云只需要用通用的Point函数即可。不过如果你想从点云中还原出物体的形状，我建议你使用 SolidWorks 的 ScanTo3D 插件，它可以根据三维点云数据自动建立网格并生成三维曲面图形，比如下面的例子。

　

　　当然，你可以直接使用点云表示环境，但问题是点云包含的点太多了，这对计算机是一个很大的负担，既占用大量的存储空间，还会消耗很多宝贵的计算资源。一种更简单、紧凑的方式是从点云中还原出物体的轮廓。
　　我们面临的第一个问题是：点云中的点是属于一个物体呢，还是属于不同的物体呢？
　　从前面的示例中，我们知道，结果传感器扫描得到的点是可能属于不同的物体的。那么我们就要对这些点进行划分。划分的依据就是：相邻两个点之间的距离，如果间距大于一定的阈值，我们就认为这两个点在不同的物体上。findClusters函数用于对点云进行分类，其中的输入dmax就是这个阈值，它是由用户自己设置的，到底应该定为多少取决于具体的环境。

findClusters[pts_, dmax_:0.5] := Module[{cornerPtsIdx, cornerPtsIdices, abs, a, b, segs},
  If[Length[pts] < 3, Return[pts]];
  cornerPtsIdx = Table[If[EuclideanDistance[pts[[i]], pts[[i - 1]]] > dmax, i - 1, Nothing], {i, 2, Length[pts], 1}];
  cornerPtsIdices = Flatten@{1, cornerPtsIdx, -1};
  If[cornerPtsIdices == {}, Return[pts],
   abs = Partition[cornerPtsIdices, 2, 1];
   segs = Table[{a, b} = ab; pts[[a+1 ;; b]], {ab, abs}]];
  Select[segs, Length[#] > 2 &]
  ]

　　对于前面的例子，点云可以分为 9 组，如下图所示（我用颜色标记不同的组）。简单起见，我们暂时不考虑数据中的噪声。

clusters = findClusters[scanPts, 0.2];
nc = Length[clusters]
Graphics[{Transpose@{RandomColor[nc], Point /@ clusters}}]

　
　　 将点划分为不同的组之后，随之而来的第二个问题是：如何找到其中的直线段？
　　从上图中可以看到，有些颜色的点代表不止一段直线段，我们要还原出所有的直线段。本文采用的方法是“迭代端点拟合”（IEPF：Iterative End Point Fit）$^{[2,3,4]}$，它的原理并不难：
　　1. 连接点集中的第一个点和最后一个点，形成一条直线段；
　　2. 计算所有点到这条直线段的距离，找到距离最大的点（记为 p）；
　　3. p 点将点集分为两部分；
　　4. 对两部分点集分别重复步骤 1~3，直到点到直线段的距离小于某个给定的阈值；
　　实现代码可见findLineSeg函数，它是一个嵌套函数，常数threshold表示点到直线的距离阈值，用来判断点集是不是继续迭代；其中，pointToLineDistance函数计算点到直线的距离。

findLineSeg[pts_, threshold_: 0.1] := Module[{dis, cornerPtsIdx, line, result},
  If[Length[pts] > 2, line = pts[[{1, -1}]], Return[Nothing]];
  dis = pointToLineDistance[#, line] & /@ pts;
  result = If[Max[dis] < threshold, {pts}, {cornerPtsIdx} = FirstPosition[dis, Max[dis]];
  {findLineSeg[pts[[1 ;; cornerPtsIdx]]], findLineSeg[pts[[cornerPtsIdx ;; -1]]]}];
  If[Depth[result] > 4, Flatten[result, 1], result]
  ]

pointToLineDistance[pt_, line_] := Norm[Det[{line[[2]] - line[[1]], line[[1]] - pt}]]/Norm[line[[2]] - line[[1]]];

　　 现在我们考虑点云存在噪声。对于给定的掺杂噪声的离散点，如何得到它们所对应的直线呢？这个问题可以用“最小二乘法”来解决。fitLine函数使用最小二乘法得到拟合的直线，它的原理可以参考${[4,5]}$。

fitLine[pts_] := 
 Module[{x, y, n, s1, s2, s3, s4, xm, ym, S, values, vectors, minv, a, b, d, \[Alpha]},
  {x, y} = Transpose[pts];
  n = Length[x];
  {xm, ym} = Mean /@ {x, y};
  s1 = x.x - n xm^2;
  s2 = s3 = x.y - n xm ym;
  s4 = y.y - n ym^2;
  S = {{s1, s2}, {s3, s4}};
  {values, vectors} = {Eigenvalues[S], Eigenvectors[S]};
  {{minv}} = Position[values, Min[values]];
  {a, b} = vectors[[minv]];
  d = {a, b}.{xm, ym};
  \[Alpha] = ArcTan @@ (Sign[d]*{a, b});
  houghToLine[{Abs[d], \[Alpha]}]
  ]
houghToLine[houghPt_] := Module[{x, y, \[Rho], \[Alpha]},
  {\[Rho], \[Alpha]} = houghPt;
  {x, y} = \[Rho] {Cos[\[Alpha]], Sin[\[Alpha]]};
  {{x, y}, {x, y} + {-y, x}}
  ]

myatan[x_, y_] := Module[{result, epsilon = 10^-10},
  If[(x^2 < epsilon) && (y^2 < epsilon), Return[0]]; 
  If[(x^2 < epsilon), If[Re[y] > 0, Return[Pi/2], Return[-Pi/2]]]; 
  result = ArcTan[y/x]; If [Re[result] > 0,
  If[Re[x] > 0, Return[result], Return[result + Pi]],
  If[Re[x] > 0, Return[result + 2 Pi], Return[result + Pi]]
 ]
]
Manipulate[
 pts = TransPt[poseToH[Flatten@{pt, \[Theta]}]] /@ RandomPoint[Rectangle[{-4, -0.3}, {4, 0.3}], 100];
 {xs, ys} = Transpose[pts];
 {xm, ym} = Mean /@ {xs, ys};
 Sxx = Total@Table[(x - xm)^2, {x, xs}];
 Syy = Total@Table[(y - ym)^2, {y, ys}];
 Sxy = Total@Table[(xs[[i]] - xm)*(ys[[i]] - ym), {i, Length[xs]}];
 \[Alpha] = 1/2 myatan[Syy - Sxx, -2 Sxy];
 \[Rho] = xm Cos[\[Alpha]] + ym Sin[\[Alpha]];
 If[\[Rho] < 0, \[Alpha] = \[Alpha] - Pi; \[Rho] = Abs[\[Rho]];];
 Graphics[{PointSize[0.007], Point[pts], Red, InfiniteLine[houghToLine[{\[Rho], \[Alpha]}]]}, PlotRange->{{-1.5, 1.5}, {-1, 1}}4, ImageSize -> 500], {{pt, {-1, 1}}, Locator}, {{\[Theta], Pi/4}, 0, 2 Pi, 0.01}, TrackedSymbols :> True]

　　Mathematica 有一个函数专门用于拟合直线：LinearModelFit，它得到的直线是用“斜率-截距”形式描述的。

4. 机器人模型

4.1 机器人的外观模型

　　为了显示机器人，需要定义它的外形模型，如下图所示。尖头对应机器人的前进方向，黑色的圆盘表示激光传感器。改变变量r的值可以调整机器人的大小。把r放在With函数里的目的是将其作为局部变量，也就是说它的值只在With函数内有效。

robot = With[{r = 0.5}, {Red, Rectangle[-{r/1.5, r/2.3}, +{r/1.5, r/2.3}], Triangle[{{r/1.5, r/2.3}, {r/1.5, -r/2.3}, {r/5 + r/1.5, 0}}], Black, Disk[{0, 0}, 0.1]}];
Graphics[robot]

　　更方便的方式是将机器人定义成一个函数，这样我们可以随意调整它的显示效果，代码如下：

Clear[robot]
robot[r_: 0.5, color_: Yellow] := Module[{g, mesh, pts0, pts},
   g = Graphics[{Rectangle[-{r/1.5, r/2.3}, +{r/1.5, r/2.3}], Triangle[{{r/1.5, r/2.3}, {r/1.5, -r/2.3}, {r/5 + r/1.5, 0}}]}];
   mesh = DiscretizeGraphics[g];
   pts0 = MeshCoordinates[mesh];
   pts = pts0[[Last[FindShortestTour[pts0]]]];
   {EdgeForm[Thickness[0.008]], color, Polygon[pts], Gray, Disk[{0, 0}, r/5]}]
Manipulate[Graphics[robot[r, color], PlotRange->3, Frame->True], {r, 0.5, 2, 0.01}, {color, {Yellow, Red, Green, Orange, Blue}}]

4.2 机器人的运动模型

　　机器人的运动方式对于SLAM也有影响。定义机器人的运动学模型为。

robotMotionModel[robotPose_, robotControl_, dt_: 0.01] := Module[{x, y, \[Theta], v, \[Omega]},
  {x, y, \[Theta]} = robotPose;
  {v, \[Omega]} = robotControl;
  x = x + v Cos[\[Theta]]*dt;
  y = y + v Sin[\[Theta]]*dt;
  \[Theta] = \[Theta] + \[Omega]*dt;
  {x, y, \[Theta]}]

　　机器人运动模型的输入是当前的位姿和控制，输出是新的位姿。我们可以给定输入控制函数，观察机器人的运动轨迹。角速度对机器人的运动轨迹影响更显著，我们将线速度设置为1，改变角速度，代码如下。

{time, dt} = {20, 0.1};
DynamicModule[{pts = Table[{i, 0.1}, {i, 0, time, 2}]},
 p1 = LocatorPane[Dynamic[pts], Dynamic@Graphics[{{
       f = Interpolation[pts];
       pose = {0, 0, 0};
       poseList = Table[control = {1, f[t]}; pose = robotMotionModel[pose, control, dt], {t, 0, time, dt}]; 
       First@Plot[f[x], {x, 0, time}, PlotStyle -> Red, PlotRange -> All]}}, Frame -> True, PlotRange -> {{0, 20}, {-2, 2}}, ImageSize -> 500]];
 p2 = Dynamic@Graphics[{Black, Thickness[0.003], Line@poseList[[;; , 1 ;; 2]]}, ImageSize -> 500, PlotRange -> {{-5, 5}, {-3, 3}}*3, Frame -> True];
 Column[{p1, p2}]]

　　 机器人的运动应该是自己规划出来的，但是这里为了简单直接人工生成机器人的参考路径。

waypts={{1.24,8.97},{0.02,8.18},{4.63,7.46},{3.1,11.75},{0.75,10.9},{1.35,14.25},{7.3,13.65},{6.05,3.6},{9.52,4.08},{9.34,-1.1},{10.13,3.2},{6.2,4.05},{5.75,1.15},{1.5,1.5},{1.55,6.2},{6.05,6.1},{5.75,9.25},{11.45,8.75},{10.15,13.25},{13.6,13.7},{13.4,11.25},{17.95,11.7},{17.8,13.5},{23.55,14.2},{24.8,0.1},{18.6,0.7},{18.4,4.1},{20.75,3.8},{23.,6.7},{14.65,7.55},{16.,0.95},{12.5,1.5},{12.5,3.5},{15.75,4.05},{15.5,7.},{13.5,7.},{13.5,9.5}};
Manipulate[Graphics[{Black, Thickness[0.003], Line@walls, Blue, Thick, BSplineCurve[pts]}, PlotRange -> All, ImageSize -> Large], {{pts, waypts}, Locator}]

　　光有路径还不够，机器人要跟踪的必须是运动的目标，为此定义机器人要跟踪的参考轨迹。

refTrajectory=BSplineFunction[waypts];
Animate[Graphics[{Blue,BSplineCurve[waypts],Black,Thickness[0.003],Line@walls,Red,PointSize[0.008],Point@refTrajectory[t]},PlotRange->All],{t,0,1,0.001}]

　　定义轨迹跟踪控制器。

trackController[pose_, referencePosition_] :=
  Module[{xt, yt, \[Theta]t, xr, yr, v, \[Omega], \[Theta]e, de, v`max = 1, \[Omega]`max = 0.5404, kv = 1, k\[Omega] = 1},
   {xt, yt, \[Theta]t} = pose;
   {xr, yr} = referencePosition;
   de = Norm[{xr - xt, yr - yt}];
   \[Theta]e = myatan[xr - xt, yr - yt];
   v = Clip[kv*de, {-v`max, v`max}];
   \[Omega] = Clip[k\[Omega]*modToPiPi[\[Theta]e-\[Theta]t], {-\[Omega]`max, \[Omega]`max}];
   {v, \[Omega]}]

　　随着机器人一步步地探索，环境地图逐渐成型。

5. 数据关联
　　
5.1 数据关联问题

　　数据关联（Data Association）就是回答“机器人观测到的数据对应环境的哪个部分”这个问题的。由于这是个比较难的问题，我先从简单的问题入手，考虑这样一个问题：
　　●　如何对点云进行变换（本文我们只考虑刚体变换，比如转动、移动）？
　　这个问题并不难，利用刚体变换矩阵很容易就能做到。下面我们在 Mathematica 中实现这个基本的功能。
　　①　首先生成一些用于变换的示例点，我选择从图形中提取点。被提取的图形如下面左图所示（你可以“右键另存为”保存到自己的电脑硬盘上，比如 C 盘）。然后将图片导入到 Mathematica 中，并利用软件自带的图像处理函数，对导入的图像进行一系列的变换，目的是得到离散的点（如右图所示），代码如下：

　　 $\Rightarrow$　　

JiQiMaoImage = Import["C:\\JiQiMao.png"]; (*注意修改文件名为 JiQiMao*)
JiQiMaoData = ImageData@Binarize@ImageRotate[ImageResize[JiQiMaoImage, Scaled[1/2]], -90 Degree];
JiQiMaoPts = Position[JiQiMaoData, 0]/120; (*得到点的坐标*)
Graphics[Point[JiQiMaoPts]]

　　②　我们可以对点进行任意地移动、旋转（如下图所示），对应的代码如下：

　

pts1 = JiQiMaoPts;
Manipulate[
 g = poseToH[{x, y, \[Theta]}];
 pts2 = TransPt[g] /@ pts1;
 Graphics[{Text[MatrixForm[g], {0.5, 0}], PointSize[0.003], Point@pts1, Red, Point@pts2}, Frame -> True, PlotRange -> {{-1.5, 1.5}, {-0.5, 1.5}}], {\[Theta], 0, Pi, 0.01}, {x, -1, 1, 0.01}, {y, 0, -1, 0.01}, Initialization :> {x = y = \[Theta] = 0}]

　　其中，poseToH函数的功能是将位姿向量（{x, y, \[Theta]}）变换为$3 \times 3$齐次变换矩阵的形式，以便于我们对点云进行刚体变换；函数TransPt的功能上用齐次变换矩阵（g）对点云做刚体变换，其定义在我的另一篇博客中给出。

5.2 ICP算法

　　上一节我们讨论了点云的变换问题，简直是小菜一碟嘛。可是这个问题的逆问题就有些难度了：
　　告诉你初始的点云和变换后的点云，如何计算对应的齐次变换矩阵呢？
　　解决这个问题需要一点矩阵知识，其中的数学原理可以参考文献${[5]}$。实现函数是registerPts（在这里 register 表示“对齐”）。

registerPts[pts1_, pts2_] := Module[{n, center1, center2, M, U, W, V, R, p},
  n = Min[Length /@ {pts1, pts2}];
  {center1, center2} = Mean /@ {pts1, pts2};
  M = Total@Table[KroneckerProduct[pts2[[i]] - center2, pts1[[i]] - center1], {i, n}];
  {U, W, V} = SingularValueDecomposition[M];
  R = V.Transpose[U];
  p = center1 - R.center2;
  RPToH[R, p]
  ]

　　Mathematica 自带的FindGeometricTransform函数也可以实现相同的功能。为了方便使用，我给它“包装”了一下：

findRigidTrans[pts1_, pts2_] := Last@Last@FindGeometricTransform[pts1, pts2, TransformationClass -> "Rigid", Method -> "Linear"]

　　点云对齐的演示过程如下图所示，可见我们的变换矩阵矩阵g和还原得到的矩阵g2完全相等。

Manipulate[
 pts1 = JiQiMaoPts;
 g = poseToH[{x, y, \[Theta]}];
 pts2 = TransPt[g] /@ pts1;
 g2 = registerPts[pts2, pts1](*findRigidTrans[pts2,pts1]*);
 Graphics[{Text[MatrixForm[Chop[g]], {0.5, 0}], Text[MatrixForm[Chop[g2]], {0.5, -0.3}], PointSize[0.003], Point@pts1, Red, PointSize[0.004], Point@pts2}, PlotRange -> {{-1.5, 1.5}, {-0.5, 1.5}}, ImageSize -> 500, Frame -> True], {\[Theta], 0, Pi, 0.01}, {x, -1, 1, 0.01}, {y, 0, -1, 0.01}, Initialization :> {x = y = \[Theta] = 0.}]

　　下面我们再增加一点难度。如果要对齐的点云内部各个点存储的顺序是不一样的，这时registerPts函数还有效吗？
　　你可以试一下，把上面代码中的pts2 = TransPt[g] /@ pts1;换成pts2 = Reverse[TransPt[g] /@ pts1];，也就是颠倒点排列的顺序，运行一下看看g是否还等于g2。你会发现它们不相等了，这说明，registerPts函数不管用了。
　　在实际应用中遇到的情况比上面的问题还要复杂，两组点云可能对应不同的照射位置，包含的点的数量也可能不相等，此外它们还会有噪声。为了解决这样的问题，我们需要使用 ICP 算法（Iterative Closest Point）$^{[6]}$。 ICP 算法是一种迭代算法，它从最近的点对开始，逐渐变换，最后得到想要的变换矩阵。ICP 方法的迭代过程如下图所示，绿色点是参考点云，红色点是要和绿色点云对齐的采样点云。ICP 方法的一个缺点是它容易陷入局部极小值，你能通过试验很容易感受到，当两组点云偏离比较大时，传统的 ICP 方法就无能为力了。

　　

ICP[pts1_, pts2_, maxIter_: 50] := Module[{pts1N, pts2T, g, dim},
  dim = Last@Dimensions[pts1];
  g = IdentityMatrix[dim];
  Do[pts2T = TransPt[g] /@ pts2;
   pts1N = Flatten /@ Nearest[pts1, pts2T, 1];
   g = g.registerPts[pts1N, pts2T];
   , maxIter];
  g]

[1]　Where am I? Sensors and Methods for Mobile Robot Positioning, Johann Borenstein, 1996.
[2]　Linear Feature Based EKF-SLAM, Filippo Arrichiello.
[3]　SLAM with Salient Line Feature Extraction in Indoor Environments, Su-Yong An, Int. Conf. Control, Automation, Robotics and Vision, 2010.
[4]　Least Squares Fitting, MathWorld.
[5]　Line Fitting, PPT.
[6]　Evaluation of surface registration algorithms for PET motion correction, Hans Martin Kjer, Bachelor thesis, 2010.
[7]　Iterative Closest Point, PPT.
[8]　Introduction to Autonomous Robots, Nikolaus Correll, 2016.