Robotics: Estimation and Learning.WEEK 4

$WEEK4$

4.1 里程计模型

\qquad 里程计会给出机器人已经移动多远，当然里程计定位只是获取机器人世界位置的一种方法。以汽车导航这个定位的具体应用为例，我们将看到许多定位方法，信息源包括GPS全球定位系统、蜂窝网络以及Wi-Fi热点。然而，每个信息源都包含不同程度的噪声，因此有不同程度的精度。
\qquad 以无人车为例，它需要高于GPS所能提供的3.5米的精度，这个精度决定了车是开在路上还是会冲上人行道。上一种信息源提供了全局位置信息，即绝对的坐标、里程及其他信息源可以增强这种全局定位。本地的信息源更加精确，可以提供厘米级的精度。但是对例如编码器或陀螺仪这样的信息源，进行积分会产生漂移。这是由误差的累积产生的，例如轮胎在地面打滑就使编码器不准。另一些本地传感器，如激光雷达、色彩、深度相机可以帮助我们校正这些误差。
\qquad 应用里程计更新机器人的位置信息，首先就要对机器人进行建模。以滑动转向四轮机器人为例，里程数据来自编码器脉冲，它能计量在给定时间内轮子旋转了多少度，将脉冲的数量映射到机器人的平动和转动上，就可以测量机器人转动角度和运动距离。
$$图1四轮机器人转动$$ \qquad 在四轮机器人的转向过程中，由编码器的测量值得到$e_o$(外轮)和$e_i$(内轮)的移动距离，测得长度即为所经过的弧度，按照弧度公式可知：
$$e_i=\theta r_i{e}_o=\theta r_o(1)$$由小车的宽度：$$w=r_0-r_i(2)$$计算旋转角度：$$\theta =\frac{e_o-e_i}{w}(3)$$ \qquad 对小车两轮经过的弧度取平均值，通过与转过的角度进行计算的位置变换：
$$\begin{gathered} y=\frac{e_o+e_i}{2}cos\theta (4) \\ x=\frac{e_o+e_i}{2}sin\theta (5) \end{gathered}$$ \qquad 上式得到小车的位置变换，对于小的移动是一个很好的位置计算。但编码器的测量会包含轮子的打滑引入的噪声，角度测量的误差又会引起位移估计的误差。一种解决办法是利用陀螺仪来获得更加精确的角度变化，在一个小的时间段内陀螺仪的测量是非常准确的。因此，角度的测量将只由陀螺仪测得的角速度对时间积分得到，这种测量方式同样可以提高平移的测量精度。这种简单的通过测量来定位的技术，需要机器人的起点来作为参考位置。本地的编码器及陀螺仪测量仍然包含噪声，所以我们希望加入更多的测量来校正误差。

4.2 定位和地图匹配

\qquad 地图配准是在以测量为基础的定位非常精确地补充。通过激光雷达可以提供对距离的测量，提供二维数据。在示例的应用中，对激光雷达进行建模，测量极坐标系下的深度信息，在离散的角度$\theta$上返回一个连续的距离$r$。
$$图2激光雷达扫描$$ \qquad 将测量结果绘制成栅格地图。亮栅格概率很大为障碍，深色栅格表示概率很小为障碍。可以利用地图信息，在给定雷达数据时得到机器人在地图中的姿态。这种同时进行地图构建和定位的方法称为SLAM。
$$图3生成栅格地图$$ \qquad 在定位过程中获得两套信息。占据栅格地图提供了期望机器人获得观测的准确信息，激光雷达的测量提供机器人当前时刻观测到的信息。通过以上两个信息找到最优的与观测结果相匹配的机器人姿态。
$$图4栅格地图和测量结果$$ \qquad 在所有可能姿态中搜索是困难的，可以使用基于编码器读数的测量信息来简化搜索。通过上一刻的姿态以及关于机器人走了多远的里程信息，将搜索限制预测姿态上。因此，机器人的最大似然姿态是在给定的激光雷达数据并依据里程信息预测的。这意味着我们可以将搜索限制在预测位置附近，对搜索结果的有效性更加有信心。
$$图4直接搜索姿态$$
$$图5结合测量信息简化搜索$$ \qquad 匹配姿态的代价函数：$$\underset{p}{\max }\sum _r(\delta (p_x+rcos(p_{\theta }+r_{\theta }),p_y+rsin(p_{\theta }+r_{\theta }))\cdot m(x,y))(6)$$
$$图6成功匹配$$

4.3 粒子滤波

\qquad 使用以采样为基础的分布，利用概率估计状态的技术称为粒子滤波器，其使用粒子群表示一组采样的分布。粒子滤波就是使用一系列来自该分布的样本表示该分布。
\qquad 实际上一群粒子表征高斯混合模型。每个粒子包含一个姿态和姿态的权重，权重就是这种情况下姿态在某分布下的概率。一个运动模型会使潜在的分布发生变化，粒子的变化是基于机器人获得的距离测量的变化。伴生的不确定性模型表征了运动模型的噪声例如轮子打滑或摩擦力的变化。在粒子滤波中，我们不追踪运动模型的具体参数，并通过运动噪声模型增加采样噪声。
\qquad 摘自《概率机器人》
粒子滤波器算法(基于重要性采样的贝叶斯滤波)：
\qquad 1.基于粒子$x_{t-1}^{[m]}$和控制$u_t$产生时刻t的假想状态$x_t^{[m]}$，粒子集中包括M个粒子作为粒子集的先验置信度。
\qquad 2.计算粒子的重要性因子，重要因子是将测量$z_t$合并到粒子集中。重要性是测量$z_t$在粒子$x_t^{[m]}$下的概率，使用$w_t^{[m]}=p(z_t|x_t^{[m]})$给定。如果将$w_t^{[m]}$解释为粒子的权值(weight)， 则加权的粒子集(近似)表示贝叶斯滤波的后验$bel(x_t)$ 。
\qquad 3.重采样或者重要性采样，算法由从暂时粒子集中抽取替换M个粒子。抽取每个粒子的概率由其权值给定。重采样将M个粒子的粒子集变换成同样大小的粒子集。通过将重要性权重合并到再采样过程，粒子的分布发生变化：在采样前，它们按$\overline{bel}(x_t)$分布；在重采样后，它们（近似）按照后验$bel(x_t)=\eta p(z_t|x_t^{[m]})\overline{bel}(x_t)$(加权过程)分布。事实上，得到的样本集通常有许多重复，因为粒子是替换得到的。更重要的是，不包含在$x_t$的粒子往往就是具有较低权重的粒子。
\qquad 4.粒子滤波的另一个版本（通常是略次的）并不进行重采样，而是为每个粒子维护一个权重，权重初始化为1并按$w_t^{[m]}=p(z_t|x_t^{[m]})w_{t-1}^{[m]}$更新。
\qquad 重采样步骤是一种某于达尔文适者生存(survival of the fittest) 思想的概率实现：它将粒子集重新聚集在状态空间中具有高后验概率的区域。这样，它就将滤波算法的计算资源集中在状态空间中最受关注的区域。

4.4 最近点迭代法(ICP)

\qquad 机器人要处理许多带有噪声的测量值，找到匹配模式并不简单。
$$图7测量值和匹配模型$$ICP算法(另一种匹配方法)：
\qquad 1.初始化旋转（R）和平移（t），通过旋转和平移实现测量和地图的匹配。
\qquad 2.找到$x_i$对应地图上的最近点$y_i$，算法方程为 (E-step):
$$y_i=arg\underset{y_j\in Y}{\max }||x_i-y_j||(7)$$找到所有测量点在地图上的相关点。
$$图8求最近匹配点$$ \qquad 3.找到更好的R和t (M-step):
$$R,t=argmin\sum _{x_i,y_j\in C}||d(x_i,y_i)|{|}^2(8)$$ \qquad 4.直到R和t变化幅度低于阈值，得出最优R和t
$$图9匹配结果$$
$$图103D匹配结果$$