目的
本文介绍在 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
]
^{[1]}
[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]
下面我们来看看如何计算一条直线的 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 参数值也在实时更新。
这一章主要介绍传感器的仿真。
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
30
m
m
（
1000
m
m
以内），测量距离的
30mm（1000mm以内），测量距离的
30mm（1000mm以内），测量距离的\pm$3%（1000mm~4095mm）
3.2 激光传感器仿真
下面我们就针对这款传感器进行仿真。激光传感器的基本工作方式很简单，它向四周发出很多激光束，每个激光束碰到物体就会被反射回来并被采集到。由于光速恒定，通过计算激光发射时间和反射时间的时间差就能知道物体离传感器有多远了。而每个激光束的发射角度是已知的，这样每个激光束扫描到物体上的点的坐标就知道了。
我们用直线段表示真实的激光束，只要计算出直线段与物体的交点就能得到物体到传感器的距离了。激光传感器的核心是激光束，因此我们首先定义激光束函数 laserBeamsFun，代码如下。真实传感器的角度分辨率很小（360
∘
^{\circ}
∘/1024=0.36
∘
^{\circ}
∘），为了节省仿真时间，本文选择了较大的分辨率（
π
\pi
π/180=1
∘
^{\circ}
∘），探测距离默认是5米（这些参数你可以根据具体的传感器随意修改，下面在举例时可能会使用略微不同的参数）。这个传感器扫描角度是从
−
12
0
∘
-120^{\circ}
−120∘~
12
0
∘
120^{\circ}
120∘，形成一个扇形区域，而背后有个
12
0
∘
120^{\circ}
120∘的盲区，传感器探测不到。
激光传感器会装在机器人身上，所以传感器的测量数据依赖机器人的位置和姿态
(
x
,
y
,
θ
)
(x,y,\theta)
(x,y,θ)，这就是为什么我将激光束函数 laserBeamsFun定义为pose的函数，pose也就是机器人的位置和姿态
(
x
,
y
,
θ
)
(x,y,\theta)
(x,y,θ)。记住这一点，因为后面只有激光测量数据是已知的，我们会用这些数据反过来估计机器人的位置和姿态。
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}]
经过前面的介绍，你应该已经明白了激光传感器的工作方式。对于使用激光传感器的机器人来说，它看到的世界是由一个个的点构成的，就像我们看电视上的图像其实是一个个的像素构成的。为了描述起来方便，人们给这些点起了一个诗意的名字：点云（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}}]
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函数内有效。
更方便的方式是将机器人定义成一个函数，这样我们可以随意调整它的显示效果，代码如下：
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}]]
光有路径还不够，机器人要跟踪的必须是运动的目标，为此定义机器人要跟踪的参考轨迹。
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
×
3
3 \times 3
3×3齐次变换矩阵的形式，以便于我们对点云进行刚体变换；函数TransPt的功能上用齐次变换矩阵（g）对点云做刚体变换，其定义在我的另一篇博客中给出。
5.2 ICP算法
上一节我们讨论了点云的变换问题，简直是小菜一碟嘛。可是这个问题的逆问题就有些难度了：
告诉你初始的点云和变换后的点云，如何计算对应的齐次变换矩阵呢？
解决这个问题需要一点矩阵知识，其中的数学原理可以参考文献
[
5
]
{[5]}
[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完全相等。
扫一扫
175
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
