机器人建图及定位概述（Robotic Mapping and Localization）

-*一.引言（Introduction to Mapping）

我们将学习机器人地图构建，更确切地说，是理解一种基于距离测量的地图构建算法——占据栅格地图（Occupancy Grid Mapping）。

机器人的地图是它所环境的模型，我们称建立一个地图的过程为mapping（地图构建）。为了帮助我们的机器人建立一张地图，常见的地图类型有：
1). 尺度地图 (Metric Map)

通常，在尺度地图中，位置表示一个坐标值，这是地图最基本的形式。因为大部分的移动机器人都在自我定位时使用了一些坐标系。
2). 拓扑地图（Topological map）

地铁路线图其尺度和大小跟实际情况是有差别的，但是这张图的目的是为了展示哪列车会在哪站停以及哪些站点连接了不同的线路，我们称这种地图为拓扑地图。在图中位置表示为节点，他们的链接表示为弧。精确到坐标在这种图上并不重要，重要的是节点之间的连接。下图中左边的拓扑图和右边的拓扑图是等价的，弧被用来表述节点间的连接代价或是限制条件，这种图在轨迹规划测验中很有用。

3). 语义地图（Semantic map）

语义地图在机器人的高层规划以及人机交互中有重要作用，通过物体之间位置关系描述出其所在位置。

机器人建图的难点：
1.地图构建本质上是一个感知问题，即确定从传感器获取的数据代表什么，同时，基于传感器就意味这以下俩件事：
a.我们的测量是有噪声的，因此我们需要可靠的估计算法。
b.我们的测量往往是在相对坐标系下进行的，它需要被转化到绝对坐标系中。
2.地图构建还涉及到其他机器人问题，例如轨迹规划和导航问题。因为地图构建往往发生在一个机器人在漫游的时候。
3.现实世界的物体可能会随时间变化，理论上，地图应该被实时更新来反映现实。

二.占据栅格地图（Occupancy Grid Map）

首先我们使用激光作为我们的传感器，如何将激光数据转换为地图？
术语“占据”被定义为一个二进制随机变量，随机变量是对真实事物采样空间的函数，这里的“占据”被定义在有俩个可能状态的概率空间内，空闲及被占据。因此占据随机变量有俩个可能取值0和1。

占据栅格地图就是占据变量组成的数组，栅格的每个元素可以用一个相应的占据变量描述，下图展示了一个2D的占据栅格地图。

构建占据栅格地图需要贝叶斯滤波器算法来维护，贝叶斯滤波器意味着一个递归的地图更新。机器人永远无法对周围的环境有确定的认知，因此我们用占据的概率标记而非二值占据变量本身。

现在来看看传感器的测量，占据栅格构建算法往往利用一个距离传感器，传感器提供距离信息，然而，在我们的地图上，每个栅格只有俩种可能的测量结果。当小格可以被光线通过，意味着它是一个空旷的空间。下图中浅蓝色的栅哥就是一个开放栅格的例子。当然，一些小格子还可能被光线击中，这意味着它被某些东西占据了，黄色的栅格就是一个被占据的例子。我们将用0代表开放，1代表测量结果被占据。

现在我们来思考一个测量的概率模型，给定每个栅格的占据状态，测量只有四种可能的条件概率。变量$z$和$m_{x,y}$，有四种情况：在m为1的条件下z也为1的概率$p(z=1|m_{x,y}=1)$, 即是我们对一个被占据的栅格获得测量结果为占据的概率；在给定m为1下z为0的概率，即为我们对一个被占据的栅格获得的测量结果为开放的概率。当m为0时我们也可以用同样方法定义概率。
我们需要设定一些测量参数，错误的测量可能来自于传感器噪声，离散空间的表达，移动的对象还有对机器人移动信息的不确定。所以我们有四个参数，对于条件概率，我们会发现其实我们只有俩个参数。

如果我们队栅格有一些先验信息，根据贝叶斯法则，我们也可将他们考虑进来。

三.Log-odd Update

我们希望在贝叶斯框架下根据传感器数据更新每个栅格的占据概率，但是，直接跟踪概率是困难的，介绍新的表示方法。如果有一个某件事发生的概率，记做$p(X)$赢率(odd)可以被记为一个比例，这个比例是这件事发生的概率与它不发生的概率之比:
$Odd:\frac{(X happens)}{(x \ not \ happens)}=\frac{p(X)}{p(X^{c})}$
我们将利用栅格被占据的赢率表示为图上所示的后验概率形式：$Odd((m_{x,y}=1) \ given \ z) = \frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}$

引用贝叶斯法则 $p(m_{x,y}=1|z)=\frac{p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z)}$，我们可以包含进传感器模型项，$Odd((m_{x,y}=1) \ given \ z) = \frac{p(z|m_{x,y}=1)p(m_{x,y}=1)/p(z)}{p(m_{x,y}=0|z)} \ .... ... (1)$,

再次引入贝叶斯法则 $p(m_{x,y}=0|z)=\frac{p(z|m_{x,y}=0)p(m_{x,y}=0)}{p(z)}$,可以将公式(1)化为：

$Odd((m_{x,y}=1) \ given \ z) = \frac{p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z|m_{x,y}=0)p(m_{x,y}=0)} \ .... ... (2)$,

然后对$Odd$取对数，我们得到：

$Log-Odd: \ log\frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=log \frac{p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z|m_{x,y}=0)p(m_{x,y}=0)} = log \frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)}+log\frac{p(m_{x,y}=1)}{p(m_{x,y}=0)}$

$log \ odd^{+} (后验)= log \ odd \ meas (似然) + log \ odd^{-} (先验)$
这是对占据栅格地图更新的对数赢率(log-odds)公式。

地图存储了每一个栅格的对数赢率，测量模型也表示成为对数赢率。地图的更新计算变成了求和这些对数赢率，

当在使用这个更新法则时需要记住俩件事，1.只更新被观察到的栅格 2.更新后的值将变成先验信息当你在后续点获取了新的测量时，整个过程是递归发生的。

那么如何更新赢率（odd）呢？
首先探讨这个测量模型：$log \frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)}$，并考虑俩种情况，

1.测量结果为占据的 $log \ odd \ occ: =log \frac{p(z=1|m_{x,y}=1)}{p(z=1|m_{x,y}=0)}$

2.测量结果为空闲的
$log \ odd \ free: =-log \frac{p(z=0|m_{x,y}=0)}{p(z=0|m_{x,y}=1)} （注意这里公式是颠倒的，表明m为0的时候同时z为0）$

记住这个更新算法: $log \ odd \ +=log \ odd \ mean$

让我们看一个占据栅格地图构建的简单例子：
在一个被初始化为全0对数赢率的占据栅格地图中，意味着这种初始化等价于网格被占据和开放的概率是一样的，

$p(m_{x,y}=1)=p(m_{x,y}=0)=0.5, 则log \ odd=log \frac{p(m_{x,y}=1)}{p(m_{x,y}=0)}=0$

这里，我们的距离传感器发出一道光线，我们获得了一个新的测量，黄色栅格被测成被占据，而浅蓝色栅格的结果为空置的。对那些被占据的栅格，我们将其对数赢率加上占据的对数赢率的测量参数来对其更新，在这里就是加上0.9。对那些空置的栅格，我们将其空置的对数赢率减去空置赢率测量参数来对其更新，这里是减去0.7。

以此类推，换个角度继续测量，进而更新权值，数字越小就越反应越不相信是被占据的。

四.处理激光数据（Handling Range Sensor）

首先，激光传感器是固定在机器人身上的，如何将激光数据从机器人身上转移到世界坐标系上，这需要一个转换关系：

我们给定的栅格地图是具有一定分辨率的方格地图，如何从连续的地图（即没有栅格）的位置转换到栅格地图上的位置，考虑下一维的情况呢，如下：

对于给定的$r$点(左边图) $x$的索引(右边图)，可以计算为不低于$x/r$的最小整数。例如$1.7r$则其所以为$2$。
举个例子，对于一个长为1米的一维地图，若含有20个格子，则每个格子的长度为 1[meter]/20[cell]=0.05[meter]/[cell]。
更一般情况下，如果一条线从非0开始，我们只需要在计算索引时从x减去最小值，

那么如何获得被占据栅格的2维索引呢？
首先，我们知道的是距离 $d$ 和机器人的姿态$(x,y,\theta)$, 根据这些，我们可以首先计算出阻挡光线的点在世界坐标系下的坐标，同时也是在连续域中的坐标，然后我们计算在分辨率为$r$的离散地图中的索引

对于他们中的点，我们将使用Bresenham画线算法。
在实际情况下，距离传感器将往不同方向发射多条射线，其中，$\alpha$代表每条射线基于机器人$x$轴的方向。

建三维地图的时候，可以使用一种称为树的高效数据结构，来从3D点云构建3D地图，有kd树和八叉树。

五.里程计模型（Odometry Modeling）

这里，我们将考虑一个里程计量的模型来作为机器人位置的一阶近似（first order approximation to the robot‘s location），就像你的车里，里程表会记下你已经开了多远，里程计会给出机器人已经移动多远，里程计量只是获取机器人在世界中位置的一种方法。如果我们看一个定位的具体应用，汽车导航，我们将看到许多定位方法，信息源包括GPS，全球定位系统（global positioning system），窝峰网络（cellular networks）和WiFi热点，然而这些传感器都包含不同程度的噪声，因此有不同程度的精度。

里程计的更新开始于对机器人进行建模，不同模型，例如人形机器人和飞行器将使用不同的模型。在我们的例子中，我们将建模一个滑动转向四轮机器人，里程数据来自于编码器脉冲(ticks from the encoder)，它能计量在给定时间内轮子旋转了多少角度，可以将脉冲的数量映射到机器人的平动和转动上，首先让我们来看一下转动里程计的计算。

对于滑动转向机器人，左右轮是单独控制的，当转弯的时候，俩边的轮子将分别组成同心圆的内外圆弧，我们可以根据编码器读数确定转动角度。
首先，我们希望将编码器($e$)读数转变为内外圈轮子绕他们相应圆弧走过的距离，$e_{i}=\theta r_{i} (内圈圆弧),\ e_{o}=\theta r_{o}(外圈圆弧)$，内外圆弧我们都是已知的（可以从编码器上获得），但是他们有相同的圆心角，在知道机器人宽度的情况下，我们可以利用弧长的差来计算这个相同的角度，我们知道小车的宽度$w=r_{o}-r_{i}$, 则我们可以计算其夹角$\ \theta=\frac{e_{o}-e_{i}}{w}$。

接着，我们将考虑机器人位置变换，位置变换需要我们已经计算得到的旋转信息。在测量位移时，我们可以构造一个包含一只转角的三角形，接着对内圈和外圈的轮胎轮子的位移分别取平均，来得到x方向的位移，同时y方向的位移也可以用相同的方法得到。

但不幸的是，编码器的测量会包含轮子的打滑引入的噪声，角度测量的误差又会引起位移估计的误差。一种解决的办法是利用陀螺仪，来获得更精确的角度变化测量。在一个小的时间段内，陀螺仪是非常准确的，因此，角度的测量将由陀螺仪测得的角速度对时间积分得到，这种测量方式同样可以提高平移的测量精度。

六.地图配准（Map Registeration）

考虑在给定激光测距数据时，基于相关性的定位及地图匹配策略。这种地图配准可以提供对以测量为基础的定位非常精确的补充。
首先，我们先介绍一下深度激光雷达传感器（lidar depth sensor），说起激光雷达，就想到光检测和测距，它可以提供对距离的测量，通常使用激光雷达来提供二维的数据。如何将建模的激光雷达可以测量在极坐标系下的深度信息，在离散的角度$\theta$上返回一个连续的距离 $r$,图中的$\theta$的范围只有270度，不是一个完整的圆。激光雷达只能测量10到30米的距离，这个距离意味着以黑色点表示的测量结果，只能是在这个绿色的区域内。因为光是从一点出发并且范围被限制，机器人只能看到虚线而不能看到棕色的实线。

一个二维的占据栅格地图将被用在定位中，其中亮色栅格表示这格很大概率为障碍，而深色栅格表示概率很小。当机器人接近走廊的中间时，栅格地图应该能与上一张图片中激光雷达。测量到的边界对应上，因为机器人是在一个有限栅格空间中活动的，当机器人离开边界后栅格必须被扩展，除了用激光构建地图外，我们可以利用地图信息在给定雷达数据时得到机器人在地图中的姿态，即同时定位与构图。

在定位问题中，我们有俩套信息，首先，占据栅格地图提供了我们期望机器人获得观测的准确信息，第二，激光雷达的测量提供了机器人当前时刻观测到的信息。根据地图的表示方式，激光雷达的数据必须是离散的，才能与占据栅格地图相比较。

有了这俩个目标后，我们的目标是找到最优的与测量结果相匹配的机器人姿态。在所有可能姿态中搜索是困难的。但是由于我们有编码器读数来简化搜索，我们可以将搜索限制在一些由里程信息获得的姿态上，因为我们在跟踪机器人，我们有机器人上一刻的姿态以及关于机器人走了多远的里程信息。因此，机器人的最大似然姿态是在给定新的激光雷达数据的，很可能的与根据里程信息预测的很接近，这意味着我们可以将搜索限制在预测位置附近，并且对搜索结果的有效性更加有信心。

我们在搜索时根据地图配准的标准考核每一个姿态，一种度量是考虑在坐标$xy$激光雷达返回距离r时，真的被击中点的地图中记录值的和，这种相关性度量可以被修改来适应你手头的应用，在我们的例子中，地图记录的是对数赢率，所以雷达返回的点在地图上有高概率被占据的话，将大大增加度量的分值，如果返回的点在地图上是开放点则会减少度量分值。另外，相关性可以被调整为远处点对度量计算的影响将小于近处的点。

我们将机器人与地图配准，使姿态能够最大化配准度量，因此当里程被计算后，它将用来预测机器人的新状态。
除了只考虑雷达返回的点外，我们还可以考虑被激光穿过的点，可以进一步证实我们的地图配准同时也引起了大量的计算。

七.粒子滤波器(Particle Filter)

在这个环节中我们将探讨一种利用概率估计状态的技术，该技术使用一种以采样为基础的分布，称为粒子滤波器。

不同于完整定义的函数，粒子滤波器用粒子群表示了一组采样的分布，粒子群表示了分布，采样点的统计特性与分布的统计特性是一致的，例如均值和标准差等。在这种情况下，将没有高斯模型中出现的均值和协方差等参数，反而，所有的粒子将被跟踪。

事实上，一群例子表征了一个我们在机器人学统计建模中的高斯分布（Gaussian distribution in robotics statistic model）文章中提到过的高斯混合模型，这里的方差为将是0，在0方差情况下，高斯分布变成了狄拉克函数(Dirac Delta function)。起初，一群粒子代表了潜在的状态，每一个粒子包含了一个姿态和姿态的权重，这与用概率函数表示很相似，这里的权重就是这种情况下姿态在某种分布中的概率。这里越深的颜色表示了越高的权重，越浅的颜色则表示权重低。

就像卡尔曼滤波器一样，一个运动模型会在潜在的分布发生变化，这里粒子的变化是基于机器人获得的距离测量。

一个伴生的不确定性模型表征了运动模型的噪声，例如轮子打滑或者摩擦力的变化等等。在粒子滤波器中，我们不追踪运动模型的具体参数，而是通过运动噪声模型增加采样噪声。在本节中，我们使用高斯分布来建模0均值但方差非0的噪声，每个粒子的的噪声都是单独添加的，所以每个粒子也是单独采样的，在增加噪声之后，粒子的分离就表征运动产生的不确定性。

就像卡尔曼滤波一样，我们可以使用独立的观察，来限制我们的噪声并且更新分布。这里我们将使用上一节提到的激光雷达相关性地图配准，我们将使用当前粒子权值作为先验，并更新粒子的权值来反映地图配准中获得的分数。

新的粒子表征了在里程计量和传感器测量之后的分布，但这可能不是表征分布的最优粒子群。你可以看到这里只有几个粒子有很高的权值，大部分的例子颜色很浅，他们并没有提供很多分布的信息。为了使得粒子群更好的表示状态的分布，我们检验有效的粒子数量，有效粒子的数量作为是否进行重采样的依据，重采样过程是一种从概率角度出发的，除去低权值粒子的方法。

当同时有高权值和低权值粒子时，可以使用累计概率函数来帮助采样，权值和归一化为1，并且可以表示为单调递增的累积函数。我们在一个累计范围0到1之间随机采样，并查找哪个权值包含这个数，权值大的被采样到的几率就大很多。

在重采样中被找到的那些粒子，将被成为新的粒子群，被用来下一次里程更新，粒子可能会被复制，但里程误差会将他们分开，这种方法提供了一种很好的处理多模型状态分布和运动模型非线性的途径。