对于移动机器人来说，最吸引人的莫过于SLAM，堪称Moving Robot 皇冠上的明珠。Perception 服务于 SLAM，Motion Plan基于SLAM。SLAM在移动机器人整个问题框架中，起着最为核心的作用。为了专注于Mapping，此章我们假设 Location 是已知的。

1、Metric Map

　　轨迹规划任务是再Metric Map的基础上完成的。当然，层次最高的是语意图，语意图是未来研究的热点方向。获取Metric Map 的难度最大之处在于：1、传感器噪声（May be solved by PGM； 2、机器人在移动（Location 问题）；

　　常见的MAP是基于UGV而言的， 使用的地图是Occupancy Map，与之前在轨迹规划栏目中的图一样，以 0 表示无法到达的区域，以 1 表示可以达到的区域。

2、传感器噪声问题的Naive Solution

　　解决传感器噪声问题的方法是对传感器进行建模，其模型为概率模型：

　　对于给定的观测，其factor 如上，当连续N次观测到同一个xy的值时，利用 factor product，即可获得n次观测的Joint CPD ，在Joint CPD 上运行MAP算法，即可获得最终的地图了。在通解的基础上，确实是这样设计算法的，但是我们目前面对的Mapping 问题还太Naive，不需要如此复杂的手段。

　　此处定义了一个odd，表示发生与否可能性的比值。换言之，就是传感器出错的概率比，这是一个可以估计的量。比如传感器测出有物体的odd是2，没物体的odd是3.

　　依据Beyes 公式把Odd展开：

　　其中，p(m = 1|z)/p(m = 0/z) 表示的是测量完成后 odd 的值，它等于  此次测量值 + 之前的odd。

　　换言之，我们只要定义好先验图与log-odd-meas，就可以了。

3、Mapping Algorithm

　　所有的东西总是说起来容易做起来难。即使是如此Naive的Map，真正Coding还是很难的。尤其是如何高效的实现Map的更新，此外，机器人每次发出的激光线达上千条，每个位姿都要更新上前个点。对此问题，我设计了并行算法，即使在并行的条件下，生成一次Map也要近20s.

　　算法流程如下：

　　1、将激光击中的点变换到全局坐标系下

　　2、利用Breshenman 算法，获取激光通过的路径

　　3、获取Occupied Map 与 Free Map

　　4、更新Map.

　　左图是单次测量的图，也就是机器人一个位姿的测量结果。显然，在靠近机器人的地方，噪声严重。右图是机器人在空间中移动后最终的Map。

4、3D Mapping

　　对于2维地图，我们采用 free+occupy 的形式储存空间中的障碍物。然而有障碍物的地方毕竟还是少数，或者说，地图是稀疏的。

　　对于3维数据而言，我们用3维的空间来描述显然不划算，由其是当空间巨大时。此时我们选择的地图，或者说是数据结构是kd-tree 与 OcTree. 此部分内容在点云相关博客中有过介绍，不赘述。