五、从无到有自己动手写个主流无人驾驶汽车框架（如apollo）（CSDN独创）
注1：必须先看完前四章再看这一章，如果想看得轻松请看《概率机器人》与《机器人学状态估计》完再看
注2：本文力图用白话说明这个原理，不扯面条，希望大家能学会/font>
5.1、框架概述
写这个主要是博主之前看过很多大佬写的CSDN，发现就是一个问题，不是写得不好，有的甚至给出了代码，博主也跑过这些代码，但是跑完仍然不知道如何做一个无人驾驶汽车，发现全网确实没有从代码上细致介绍如何手写一个无人驾驶汽车框架，我写的是自己用的框架，除了算法细节其他介绍足以让大家自己写一个了。先给出Apollo的框架，然后提出自己的框架：
及硬件框图
实际上有很多东西没有给出，毕竟商业机密。
我们先回顾在第四章讲的小车框架，由此引出我们今天的2.5D框架，2D小车框架如下：
1.底盘控制
2.轮式里程计
3.IMU里程计
4.VO及语义slam信息加入
5.GPS里程计
6.定位融合优化算法（如EKF）
7.配置激光雷达
8.栅格建图（若是3D建图，需保存成一个文件，这时候不能用rviz显示，要自己做个软件显示）
9.导航算法。
我们要改的部分很多，几乎都要改，改完后的框架如下：
1.主从分布式服务器配置（类似zookeeper）
2.车辆状态读取及报警系统配置（为了安全，还要连接到云端）
3.底盘控制（连入CAN网络）（这里也可以用模拟器在云端服务器进行行为克隆训练，可见我写的强化学习博客，这个是逆向强化学习/模仿学习）
4.轮式里程计（由汽车轨迹方程）
5.高精度IMU里程计配置
6.VO里程计输入
7.前方图像获取及场景语义分析（如基于canny/滑动窗口的车道线检测，场景3D语义分割，3Dbox车辆检测，2D交通信号识别等）（我会另外开一篇文章细节介绍原理）。
8.差分GPS配置
9.定位融合算法（不同的是这个是反馈融合）（当然还可以结合毫米波和超声波）
10.配置激光雷达
11.cubic建图/栅格2.5D建图（这个可以交给服务器）（HDmap要先建，不能像小车一样边导航边建）
12.地图语义融合（根据7）
13.车道语义地图抽取
14.根据全局语义地图地图由A*生成全局规划路线（此可在云端完成）
15.车辆/行人行为预测。
16.由激光雷达数据及全局规划路线生成局部路线（或由行为克隆完成）
17.由局部路线或者行为克隆进行局部导航
18.安全问题解决方案（分物理与网络攻击）
19.时序与多机协调
现在开始！
5.2、主从分布式服务器配置
这主要分为以下几点：
1.车辆端PC配置
2.云端PC配置
3.车辆备份端PC配置（也可以云端备份）
4.用一个软件将三个联系成一个"cluster"
其实说白了这个部分主要是解决roscore卡死的问题，即主节点卡死，所以需要一种备份机制：zookeeper是一种方法：之后我会写出我自用的方法，zookeeper如下：
配置可以见zookeeper配置，教程自行百度，我觉得这个算通用框架，就具体应用来说，大家可以自行尝试这种方法，但博主希望DIY且错误可控于是自己写了个。目前其使用起来是很不错的。先把思想给大家，其实大家可以自己先写，而博主希望路跑测试几个月后再传github。先按顺序来说：
1.车辆端PC配置，本人采用X86基于ubuntu系统，而不是ARM，装上ROS，这个自行百度以及各种语言环境，编写与云端通信的软件。
2.云端PC配置，同ubuntu，装上ROS，这些自行百度
3.备份端PC配置，同车辆端
4.用一个软件将三个联系成一个"cluster"
5.3、车辆状态读取及报警系统配置
为了安全，台式机还要把车辆状态信息用ROS定时器发给云端，云端可以发给移动设备端，云端能直接对台式机进行控制，这个很重要，这可以用C++写，另外还有非常重要的一点，云端对台式机的控制不能通过ros！，这个极其重要，其实在ros中我们也是通过ROS定时器与CAN通信，这里我们不能经过ros，而要直接用C++与CAN通信，这样就不会受ROS卡死的影响了，（但是台式机本身可能会卡死，这时候只能开两个ros，就是我说的第一种方法），当时本机卡死的时候，车辆是不会动的，这在普通路面是没问题，但是高速公路就需要用S型加减速机制处理本机卡死的状况。实际上现在无人汽车还不敢真正上高速。
这里的发送用5G，博主写此博客的时候很多5G基站及服务已经开始实验商用了，不过V2X网络的建立还需要一定时间（大公司可实现），属于信息编码问题，跑题了，不是本章论述的重点，本架构暂时不考虑V2X。
5.4、底盘控制
在2D小车中我们已经写得很清楚了，但汽车不同，你需要给汽车运动模型建模，之后汽车轨迹方程做轮式里程计，控制参数可以通过模拟仿真得到，博主用的是carsim/simulink，方法见CarsimSimulink联合仿真，当然光靠这个是不够的，其他请自行百度。当然，我们接受的肯定还是cmd_vel信息，但是计算产生cmd_vel的方法（比如行为克隆）及解析用于控制的时候和小车是完全不一样的。要根据仿真参数设置，然后CAN通信自行百度CAN通信手册，最后也是写个ROS定时器，然后用自动控制理论建模控制(参数也可以通过强化学习得到)，方法和我第一章讲的差不多。这个属于机械工程，这个不是本文的重点，但是参数可以通过强化学习得到，见本人写的强化学习文章，而行为克隆用逆向强化学习方法，见逆向强化各大算法介绍。Udacity有行为克隆源码。用的是Udacity的仿真器见Udacity行为克隆介绍及GITHUB。
ROS节点如何写见我第4章
5.5、轮式里程计
根据汽车轨迹方程建立里程计模型及误差模型，发布方法见我写的第一章及第四章，建立误差模型的参数可以通过强化学习得到，博主自用是DDPG，可以见我的强化学习文章这里不赘述。ROS节点如何写见我第4章
5.6、高精度IMU里程计配置
Apollo的IMU型号为NovAtel SPAN-IGM-A1。一些可以用的比如LPMS,AH100B等，不要买几十块那种，记得。买回来就是矫正及根据误差模型产生协方差矩阵。见我的第3-4章。ROS节点如何写见我第4章
5.7、VO里程计输入
VO我会单独开一系列论述，比如深度摄像头用ICP产生位姿估计，见ICP原理及实现，单目摄像头可以用PNP，博主这里用的是轮式+imu+单目融合估计，博主在论文里把轮式，imu当一组静态位姿组，GPS为Deterministic组，摄像头，激光雷达SCAN TO MAP为动态估计组，大家可以先尝试用ICP产生一个VO及一个6*6的误差模型协方差矩阵。这里我在第四章也说过，ROS节点如何写见我第4章
5.8、前方图像获取及场景语义分析
1.激光雷达摄像头外参数标定（为了激光雷达和摄像头深度学习能结合成为3D）
见Autoware激光雷达摄像头外参标定。
2.车道线检测
可以根据传统的方法，选区域+色彩阈值+梯度算子提取梯度+霍夫变换/滑动窗口多项式拟合，见：传统车道线检测方法，效果如图：
Udacity也提供了车道线检测的源码，见Udacity检测。
而也有深度学习的方法，如laneNet方法与SCNN方法，博主对三者进行评价，速度当然是传统最快，效果推荐laneNet，如果计算机资源够，合理安排时序，可以用laneNet。
发布lanemap话题
3.3D车辆检测
注意不能用2D，单纯用地面方程是无法适应坡度的情况的。这里提供3种方法：SqueezeSeg方法，VoxelNet方法，以及刚出的SqueezeSegV2，V2我没测试过，但是我对比下SqueezeSeg和VoxelNet，VoxelNet要快些且效果好于SqueezeSeg。效果如图：
最后可以发布各个车辆的位姿(自定义的carpose，其中有一个pose[]用于存储位姿)，在局部规划会用到。
4.交通信号检测
见交通信号检测思想。当然不止这些，还有些交通信号线。这些用深度学习或者机器学习可以自己去完善。最后要发布标志的位置及类型用于构建sigmap。引用自上链接效果如图：
5.地图语义分割
本人用的是FCIS，见FCIS原理，也可以用Panoptic FPN，这是为了进行像素级分类。本人会在日后在深度学习系列详细介绍语义分割，比如得到某个点的类别是1、2、3、4等。实验效果如下：
还有一张引用图片：Semantic Segmentation of Point Clouds using Deep Learning ：
最后发布的是目前检测/分割的结果（就是一个结构体数组），需要自定义rosmsg，定义方法见ROS消息及服务原理之后用ROS定时器发送，这个在建图后进行接收，这个其实可以分PC做。ROS节点如何写见我第4章。
这里发布segmentationmap_P，segmentationmap_L，segmentationmap_D等话题。
5.9、RTK输入
也就是差分GPS输入，不能用普通的GPS，这是因为GPS在定位融合的时候其实是非常关键的一个模块，
ROS节点如何写见我第4章。
5.10、定位融合算法
你可以选择UKF或者EKF，方法见我第四章叙述，但是这个需要反馈融合，之前说了博主在论文里把轮式，imu当一组静态位姿组，GPS为Deterministic组，摄像头，激光雷达SCAN TO MAP为动态估计组，协调这三组传感器是值得研究的，如果你只是为了做个demo，那么EKF就够了，方法见我第四章叙述，ROS节点如何写见我第4章。发布的是融合后的里程计
5.11、配置激光雷达
这些我第四章写的很多了，就是配置好，然后测量激光雷达到base link的TF,这里还要autoware标定，ROS节点如何写见我第4章。
5.12、cubic建图/栅格2.5D建图
这里先用cartographer，注意，先不用loam，loam在室外缺陷很大，cartographer我会在下一章详细介绍，值得一提的是本文先假设车辆行驶不能上急坡（如高架桥），要根据订阅的5.9融合后的里程计对点云进行修正后再建图，节点名pointfix，以适应3D的情况，有人可能问了，来回上下坡因为激光雷达的匹配肯定是无法精确匹配的，那之后我会提到会用到一些修正方案对于缓坡够了，栅格地图原理见我第四章，ROS节点如何写见我第4章。效果如下图：
（而3D建图显示，目前常用点云扩张大小显示，而算法的情况，2D是栅格概率，3D只不过变成了cube）如下图：
5.13、地图语义融合（建立语义HDmap可在服务器完成）
结合5.11,5.8的信息，这里博主融合后发布了四类2D地图，车道线检测地图（lanemap），交通标志地图（sigmap）及相应visual marker，语义分割地图（每一类一个地图）（segmentationmap_P）（segmentationmap_L），cartographer栅格地图。其中道路语义分割地图后面会用到。以及自定义了一个结构体用于存储车道3D坡度分布（highdistribution话题），最后自定义了一个rosmsg用于发布highdistribution话题，这个非常重要，其组成元素类似于occupancy grid，加入了type，每一栅格高度的均值及方差，这个方差是很重要的。当然，为了显示效果也发布了3DBOX话题。（注意：HDmap要先建，不能像小车一样边导航边建），你也可以用服务器完成此项工作，录制后打包给服务器就可以，当然5G时代可以实时，之后建好之后需要人工标定，这个也是极其重要的。总之，目前研究主流目标就是在这里，处理3D问题时很重要的。ROS节点如何写见我第4章。当然博主在测试的时候发现还有一点比较麻烦，就是时序协调，要对这几个算法分别统计运行速度后决定，可能还需要加入另外的PC和FPGA代替几个模块。
5.14、车道语义地图抽取
这里我订阅的是segmentationmap_P话题（车道地图），highdistribution话题（车道坡度分布），sigmap话题（交通标志地图），cartographer地图话题，博主目前还没有订阅segmentationmap_L等其他话题，这个会在后续加入，大家可以自己加。然后还要订阅里程计信息，发布segmentationmap_P_part（局部车道分割地图），highdistribution_part信息，sigmap_part信息。用于局部路径规划，这里局部地图的大小是param。
3D效果如下：（引用自deepmotion）
不过本文写的是2.5D。3D之后写。
5.15、根据全局语义地图地图由 A* 生成全局规划路线（可在服务器完成）
根据5.13订阅的话题由 A* 生成全局规划路线，而语义约束，博主是通过逻辑的方法实现对A*的约束的，表现为比如，检测到实线（那么不可转向），比如 A星探路检测到公交车道，那么不能走这个道。博主目前考虑直接用百度API规划路线，这是一种方法，但在一些偏远地区或者实验场所，肯定还是要自己规划。
5.16、车辆/行人行为预测
博主这里直接订阅了里程计和各车辆位姿信息进行了2帧线性预测，实际上还有更好的方法，但是考虑到从这一帧决策产生到决策完，又会重新计算，这个线性预测我给出了个scale参数用于放大预测，所以使用起来够了，但是大家可以用更好的方法，比如Future Person Localization in First-Person Videos这个算法，效果如图：
这个节点最后发布的是pointcloudpredict点云信息。
5.17、由激光雷达数据及全局规划路线生成局部路线
其实和小车不同，汽车的全局路线确定了，局部路径规划只做微调，博主是这样做的，每隔一段时间（这个时间经常为2-5秒，故不需要担心延迟）从HDmap提取（可在服务器上放HDmap）一段"大局部地图"即订阅5.13的xx_part及订阅上述pointcloudpredict点云信息，车辆检测信息，特定交通标志信息（之前已经用5.8.4）。最后生成局部路线（这个和小车不同），经常和全局路线重合，但遇到交通标志会抑制路线生成（主要是这个作用），博主是用的是逻辑方法（也可以行为克隆），目前已写完了十几种逻辑了。当然还有基于FredNet的路径规划方法，大家可以参考。此时不建议dwa，
发布的局部路径是个数组，见我的四章
5.18、由局部路线或者行为克隆进行局部导航
博主仅用强化学习训练了运动参数，然后根据订阅的局部路线（自定义结构logic_path）发布cmd_vel信息，之后就能进行控制了，底盘会订阅cmd_vel根据控制参数去进行控制汽车，见第四章及5.4行为克隆。
5.19、安全问题解决方案
见无人驾驶安全方案。
5.20、时序与多机协调
这个是整个系统的核心之一，如果协调不好，算法再好也没用，并且还要给出某些节点卡死的解决方案，本人用的是ROStime一个个实际去计算时间进行协调，并采用了信号量机制及ros time 的header->stamp，这个着实耗了我不少时间。
好了架构就完成了，这篇文章改日还会更新。
