一、高精度地图

制作一张高精度地图可以大概分为3个过程：采集、加工、转换。

如何采集地图？
我们需要需要一些传感器来获取数据，下面是需要的传感器列表：
lidar、摄像头、gnss、imu

lidar主要是来采集点云数据，因为激光雷达可以精确的反应出位置信息，所以激光雷达可以知道路面的宽度，红绿灯的高度，以及一些其他的信息，当然现在也有厂家基于视觉SLAM（纯摄像头测距）来制作地图的，有兴趣的也可以看下相关介绍。
摄像头主要是来采集一些路面的标志，车道线等，因为图像的像素信息更多，而位置信息不太精确，所以采用摄像头来识别车道线，路面的一些标志等。
gnss记录了车辆的位置信息，记录了当前采集点的坐标。
imu用来捕获车辆的角度和加速度信息，用来校正车辆的位置和角度。

用apollo的录制bag功能，可以把传感器的数据都录制下来，提供生成高精地图的原始数据。其实在录制数据之前，需要对上面所说的传感器进行校准工作，这部分的工作比较专业，涉及到坐标系转换，也涉及到一些传感器的知识，所以对非专业人士来说不是那么好理解。或者开发一系列工具来实现校准。

接下来就是采集了，采集过程中需要多次采集来保证采集的数据比较完整，比如你在路口的时候，从不同的角度开车过去看到的建筑物的轮廓是不一样的，这些轮廓就是激光雷达扫描到的数据。所以遇到路口，或者多车道的情况，尽可能的多采集几次，才能收集到比较完整的地图信息。并且速度不要太快，apollo上的介绍是不超过60km/h（这里没有特别说明会出现什么问题）。

加工
如何加工上述地图？

首先需要生成一张原始的地图，这里我们采用点云生成原始的地图，因为点云的距离位置信息比较准确，因为点云数据是0.1s采集一帧，下面我们可以做一个计算。如果车速是100km/h，对应27.8m/s。即0.1s车行驶的距离是2.78m，而激光雷达的扫描距离大概是150m，所以前后2帧大部分地方是重合的。因为数据是一帧一帧的，我们需要把上面的说的每一帧进行合并，生成一张完整的地图，有点类似全景照片拼接，这样我们就可以得到一张原始的采集路段的地图。这里用到了点云的配准技术，有2种算法ICP和NDT，基于上面的算法，可以把点云的姿态进行变换并且融合。具体的介绍可以参考。

点云拼接好了之后，我们就需要在道路上标出路沿，车道线，红绿灯，路口，一些交通标识等。大部分的工作都可以用深度学习结合图像的方法去解决，查找出上面的一些信息并且标识出来，目前有些场景还是需要人工标识出来，比如路口停止线和红绿灯的关系，如果一些特殊场景的车道线等，需要人工去做一些校正。

上面的过程可以说是一个简易的制图过程。实际上这里还需要讲下高精地图的格式，因为如果没有一个统一的格式，高精度地图是没有太多意义的。我们可以把高精度地图分为三层。

地图图层 地图图层主要是道路的信息，比如道路的路沿，车道线，路口信息，主要是道路的一些基本信息。
定位图层 定位图层主要是具备独特的目标或特征，比如红绿灯，交通标志，道路的点云数据等。
动态图层 动态图层主要是一些实时路况，修路或者封路等需要实时推送或者更新的数据。

通过下面的加工流程：

点云地图校准 -> 地图标注加工 -> 高精度地图
这样就生成了一张高精度地图，当然加工过程中首要的目标是提高效率和质量，尽量的采用算法自动化处理会很大的提高效率，这可能是后面地图厂家的核心竞争力。因为地图需要实时更新，谁的效率更高，谁的图就越新，用的人越多，之后的数据也越完善。

转换
转换主要是得到一个通用的自动驾驶系统可以使用的高精度地图。

上面的高精地图格式可能还是原始的数据格式，需要转换为apollo中高精度地图的格式，apollo中高精度地图采用了opendrive的格式，并且做了改进，总之这是一个通用的标准，这个很重要，否则每个厂家的数据如果不兼容，会导致很大的问题，你需要开发一系列的转换工具，去处理不同地图的差异，并且不同的自动驾驶系统和不同的地图厂家采用的方式不一样，会带来很多兼容性问题。

二、gamapping slam 建图

https://blog.csdn.net/luohuiwu/article/details/92787237

粒子滤波的思想是通过一组粒子来估计近似状态概率。理论上，粒子集中一个假设状态粒子的概率应该与贝叶斯滤波器t时刻后验概率成一定比例关系。

粒子滤波主要步骤如下：

（1）初始化阶段：
规定粒子数量，将粒子平均的分布在规划区域，规划区域需要人为或者通过特征算法计算得出，比如人脸追踪，初始化阶段需要人为标出图片中人脸范围或者使用人脸识别算法识别出人脸区域。对于SLAM来说，规划区域一般为用来进行定位的地图，在初始化时，将需要设置的特定数量粒子均匀的撒满整张地图。

（2）转移阶段：
这个阶段所做的任务就是对每个粒子根据状态转移方程进行状态估计，每个粒子将会产生一个与之相对应的预测粒子。这一步同卡尔曼滤波方法相同，只是卡尔曼是对一个状态进行状态估计，粒子滤波是对大量样本（每个粒子即是一个样本）进行状态估计。

（3）决策阶段：
决策阶段也称校正阶段。在这一阶段中，算法需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为重采样做准备。在SLAM中权重计算方式有很多，比如机器人行走过程中，激光雷达或者深度摄像头会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。

（4）重采样阶段：
根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；权重小的粒子有些会被淘汰，为了保证粒子总数不变，一般会在权值较高的粒子附近加入一些新的粒子。

（5）滤波：
将重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述转移、决策、重采样过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置，实现定位。

（6）地图生成：
每个粒子都携带一个路径地图，整个过程下来，我们选取最优的粒子，即可获得规划区域的栅格地图。

（1） 数据输入
在ROS GMapping包中，获取激光和里程计数据传入openslam GMapping包中，为新一时刻的建图做准备。

（2）运动模型
根据t-1时刻的粒子位姿以及里程计数据，预测t时刻的粒子位姿，在初始值的基础上增加高斯采样的noisypoint。

（3）扫描匹配
对每个粒子执行扫描匹配算法，GMapping默认采用30个采样粒子。扫描匹配的作用是找到每个粒子在t时刻位姿的最佳坐标。为后面每个粒子权重更新做准备。如果此处扫描匹配失败，则粒子权重更新则采用默认的似然估计。

（4）建议分布
混合了运动模型和观测模型的建议分布，根据上一步扫描匹配获得的最佳坐标，围绕该坐标取若干位置样本（距离差值小于某阈值）计算均值与方差，使得当前粒子位置满足该均值方差的高斯分布。

（5）权重计算
对各个粒子的权重进行更新，更新之后还需进行归一化操作。注意：重采样前更新过一次，重采样后又更新过一次。

（6）重采样
使用Neff判断是否进行重采样（重采样频率越高，粒子退化越严重，即粒子多样性降低，导致建图精确度降低，有必要设定一个判定值改善粒子退化问题）。

（7）粒子维护地图
每个粒子都维护了属于自己的地图，即运动轨迹。该步骤执行的操作是更新每个粒子维护的地图。

（8）地图更新
在ros中进行地图更新。先得到最优的粒子（使用权重和 weightSum判断 ），得到机器人最优轨迹，地图膨胀更新。

三、ydlidar

https://cloud.tencent.com/developer/news/215695
从github下载ydlidar x4的ROS下驱动源码，该源码需要放在ros工作空间下的src目录下进行编译，
当下载完源码后，接下来就可以在ros的工作空间根目录下执行catkin_make来编译该源码软件包，当编译完成后记得source devel/setup.bash，然后就可以来配置udev规则了，以后就不用来看雷达挂载点是ttyUSB0还是ttyUSB1这些了，因为每次开机时usb设备的加载顺序是随机的，这样就导致挂载点也随机，为了保证我们的启动雷达的代码统一，因此就需要为该设备配置一个别名，

四、路径规划算法

ROS Navigation包里面的GlobalPlanner自带是提供了两种全局路径规划的方法，dijkstra和A*
https://www.pianshen.com/article/9625893826/

A*算法简单来说就是一个寻找一条最小代价路径的算法
核心公式为：
F = G + H
F：总损失
G：从起点到当前位置的实际移动代价
H：从当前位置到终点的估算代价

算法维护两个list，正确而简单的理解就是
open list：该list中的点还要被搜索
closed list：该list中的点不需要被搜索了

算法步骤如下

open list中最开始只有一个点——路径的起点

把路径起点周围的点放入open list中，计算他们的F值，并将他们的父节点设为起点，将起点放入closed list中

选取open list中F值最小的点，把它自己放入closed list中

重要的一步：

(1) 第3步中选取的F值最小的点，我们称他为“小明”，称小明的周围点为“小明的同桌们”，同桌们有的已经在open list中了，有的还没有在open list中；

(2) 没有在open list中的点，自然没计算过F值，那么现在计算他们的F值，并把小明设为这些同桌的父节点

(3) 已经在open list中的点，以小明作为父亲，重新计算一下已经在open list 的同桌们的G值（注意，是G值）；

(4) 如果新计算的G值小于他们固有的G值，那么“小明是这位同桌的爸爸”。哈哈，意思就是说：从小明移动到这位同桌的路径，比之前计算得到的、能够移动到这位同桌的路径要好。把小明设置为这些同桌的父节点。

(5) 如果新计算的G值大于他们固有的G值，那么“小明不是这些同桌的爸爸”。小明移动到这些同桌，不如之前计算的路径代价小，那么这些同桌们的爸爸不变。什么操作都不要有…小明，别想当人家爹了，人家的爹还是不变的

(6) 进行过“父亲辨认”后，把同桌们都放入open list中；

循环，请执行第3步，直到第3步在 open list 中选取的F值最小的点其实就是终点。

从终点开始，倒推每个节点的父节点，直到倒推回起点，这就是你要的路径

A*算法终极理解
简单几句话：

A*，就是在已知位置中不断地搜索代价最小的点，并且更新一下所有已知位置的最优到达路径，通过在搜索过程中明确点与点之间的父与子关系，最终确定最优路径。

A在移动机器人路径规划的应用思考
A算法用在移动机器人路径规划是没有问题的，在程序层面需要设计的部分包括

open list的维护：很显然，一个最简单的方法是维护一个有排好序的open list，取值时只取F值最小的位置就行了。小地图这样可以，大地图列表有可能过大影响效率，二叉堆是一个优化效率的好方法（TODO）

多机器人的规划：多机器人怎么处理互相的碰撞，都在运动怎么计算出避免碰撞的路径（设定都首先向一个方向躲避还是？）

寻路速度：

使用小地图（呵呵，我就想在大地图里翱翔就没办法了）
不要多个机器人一起去规划（没办法的办法）
地图分辨率（重要：地图分辨率决定了你的路径规划速度，比如你有100平方米，地图分辨率2cm和2mm规划起来效率肯定不一样啊）
路径点系统（重要：大区域规划出来的路径，应该是一系列主要的路径点构成的，不可能移动时你要记住路径的每一个点）
设定一些不能到达的区域（重要：因为这是很实际的，移动机器人的工作区域往往只是地图的一小部分，剩余部分是它绝对不能进入的）
不同的地形损耗（比如地势高的地方代价就要大，不好走的地方代价就要大。其实这很有必要，只不过目前移动机器人不太需要这一条，实际很少人去用。原因？因为依靠SLAM的移动机器人现在没那么厉害，能够平地不出错就万事大吉了）
维护未探测的区域（需要这个时可能偏游戏了，因为移动机器人在路径规划时，已经靠SLAM拥有了一张工作区域完整的地图）
平滑路径（重要：A*算法得到的路径并不平滑，移动机器人需要另外的算法去平滑这些路径点。这样机器人走起来不会摆来摆去）
非方形搜索区域（也就是不用正方形的格子计算F值了。暂时不知道这个在移动机器人里的应用。因为一般，移动机器人是将地图按照像素划分的）