2018.03.14 号更新:
- 关于 Radar 的数据关联。我与同事就 Radar 的关联方式进行了讨论，我们认为传感器输出的应该就是已经做过关联的数据。因此 Apollo 可以直接从裸数据中读到 track ID。Apollo 另外计算了几何距离，猜测只是为了过滤波动较大的点。
- 为了便于大家理解，这里更新一张图片来描述整体的融合框架。
以下为原文：
前言
本篇为上一篇感知模块的续篇，建议大家先阅读上一篇中关于感知框架的介绍，有助于对障碍物感知部分有一个概览性的了解 [1]。
Apollo 2.0 框架及源码分析(二) | 感知模块 | Lidarzhuanlan.zhihu.com
在上一篇文章中我们探讨了 Apollo 2.0 障碍物感知中 Lidar 部分的内容，本篇我将继续与大家分享障碍物感知部分 (Obstacle Perception) 中 Radar 和 Fusion 相关内容的分析。
Radar 部分
入口函数位置：obstacle/onboard/radar_process_subnode.cc
void 相比 Lidar 部分的令人惊艳，Apollo 2.0 中 Radar 就稍显诚意不足了。Apollo 2.0 中 Radar 探测器模块名为“ModestRadarDetector”(“适度的雷达探测器”，从起名上就感觉Apollo 2.0 对 Radar 这部分有些不自信）。
Radar 部分大体的实现方式与 Lidar 类似，可以看做是其简化版，其大体流程如下图所示。
从代码实现上来看，Radar 的工作可分为以下4步：
一. 计算 Radar 的坐标转换矩阵
这里有个细节十分有趣。雷达的世界坐标的转换矩阵是由以下代码来实现的。
*这段代码描述了这样一个式子：
Radar 世界坐标 = Lidar 世界坐标 * 短摄像头参数* radar参数。
这个式子隐隐透露着， Apollo 2.0 的坐标体系是以 Lidar 为基准的。Apollo 可能认为 Velodyne 的位置是最准确的，因此 Camera 的位置标定参考 Velodyne, Radar 的标定参考 Camera。
二. 获取 ROI 区域
从高精度地图读取 ROI 区域，并将其转化为地图多边形 (map_polygons) 备用。Radar 和 Lidar 使用了相同的函数来获取 ROI 区域和地图多边形，获得的 ROI 地图多边形处于世界坐标下。
函数位置: obstacle/onboard/hdmap_input.cc
void 函数位置: obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc
void 三. 计算汽车线速度
四. 探测障碍物
核心函数位置：obstacle/radar/modest/modest_radar_detector.cc
bool 这部分为 Radar 的核心部分，这段又可分为如下4步。
1. 从原始数据构造 object
实现函数位置: obstacle/radar/modest/object_builder.cc
void 该部分涉及到了以下方面的内容
- 判断该障碍物是否为 background
- object 锚点 (anchor_point) 的坐标系转换: Radar 坐标 -> 世界坐标
- 速度的转化: 相对速度-> 绝对速度；
我们重点阐述下第一项内容。Apollo 提供了三种方式用来判断该障碍物是否为 background：
a. 根据障碍物出现的次数
当障碍物出现次数小于 delay_frames_ （默认值为4）时，认为它是 background。
b. 根据障碍物出现的 存在概率 和 均根方值（rms）
实现函数位置: obstacle/radar/modest/conti_radar_util.cc
bool 当满足以下两个条件之一时，认为该障碍物是 background
- 障碍物的存在概率小于 Radar 参数预设值
- 当障碍物在车身横纵两个方向上的 距离 或 速度 的 rms 大于 Radar 参数预设值
代码中对这个功能是这样实现的（为方便说明，这里隐去部分代码）。
bool 从中可以看出，Apollo 对不同类型的障碍物分别进行了处理，也就是说，Apollo 使用的 Radar 可以分辨出车、行人和自行车 等不同的障碍物。
Apollo 推荐使用的毫米波为Continental 的 ARS 408-21。ARS 408-21的介绍文档里也有简单提到，它可以对障碍物进行分类。
但实际上，我对于毫米波雷达的障碍物类型的分辨结果的可靠性和可用性仍存有疑惑。笔者也咨询过同事和朋友，大家认为毫米波雷达也许可以根据回波特性和被检测物的速度来区分障碍物。但这种方式的分辨率、可靠性和可用性都有待提高。另外文档中括号内 “> 120 single cluster”这个条件，我一直不能理解，希望可以和大家一起探讨一下。
c. 根据 障碍物速度 和 车身速度 的夹角 (均在绝对速度下)
当车速和障碍物速度都大于阈值 (velocity_threshold, 默认值 1e-1) 时，计算两个速度的夹角。当夹角在（1/4 Pi, 3/4 Pi）或者 (-3/4 Pi, -1/4 Pi )的范围时，认为该障碍物是 background。
这里还有一个槽点，Apollo 2.0 在构造 object 外形的时候的代码如下:
object_ptr从代码中可以看到，Apollo 将障碍物长宽高都设成1米。毫米波雷达确实很难获取到物体的轮廓信息，但这样处理，还是觉得有点简单粗暴了。
另外这里的障碍物类型直接设置为 UNKNOW ，这就让人有些疑惑了。既然之前 Apollo 在构造 object 时已经可以直接从 Radar 获得障碍物类型，为什么这里不使用呢？难道 Apollo 自己其实也认为 Radar 获得的物体类型的可靠性不高么？这样处理岂不是有点前后矛盾？
2. 调用 ROI 滤波器
实现函数位置: obstacle/radar/modest/modest_radar_detector.cc
void Radar 的 ROI 滤波器与 Lidar 有所不同，被简化了许多。
- Radar 没有使用位图和扫描线，而是直接判断每个 object 是否处于地图多边形内
- Radar ROI 滤波器中使用的点和地图多边形的坐标均为世界坐标，而 Lidar 的 ROI 使用了地方坐标。
3. 障碍物追踪
入口函数位置: obstacle/radar/modest/radar_track_manager.cc
// @brief: process radar obstacles
Radar 的数据关联相对简单，只考虑 目标 object 和 tracked object 两者的 track_id 和 几何距离 两个因素。
当两者的 track_id 相同，且两者之间距离小于 RADAR_TRACK_THRES （默认 2.5）时，认为两者为同一目标，即目标 object 属于该 track。
几何距离 = 两object 的中心点的距离+object 的速度*时间差。
我对障碍物的 track_id 这个属性有一点疑惑。在障碍物构造中可以看到，track_id 属性来源于 Radar 的原始数据 raw_obstacles。原始数据中为什么会有关于轨迹的id, 对这一点我不太理解。
另外对比 Lidar 在进行数据关联时，计算了障碍物的关联距离且使用了匈牙利算法进行最优分配，Radar 部分的数据关联显得太简单和太单薄了。
除此之外，在进行障碍物追踪时，Radar 的 tracker 也并未存储 object 的历史数据，只保存最新的一个 object，并且也没有使用卡尔曼滤波器对 objects 进行状态估计。
4. 收集 Objects
收集objects， 为最后一步融合做准备。
总得来说，Radar 部分“无愧于" modest 的名称，与 Lidar 部分相比，Radar 的实现显得过于单薄。虽然与 Lidar 相比，Radar 的精确度确实相对较低，并且我也相信 Lidar 是未来的趋势。但现阶段 Radar 在恶劣天气下的表现，在速度的探测上和探测距离上都比 Lidar 要好。在许多量产方案中 (比如奥迪A8，特斯拉) Radar 也都发挥了极其重要的作用。Radar 也可以作为 Lidar 的冗余，在 Lidar 出故障时，用来保证系统的安全。
我期望百度能够让 Radar 发挥更大的作用，让 Apollo 系统得以进一步完善。
融合部分
入口函数位置: obstacle/onboard/fusion_subnode.cc
apollo核心函数位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc
// @brief: fuse objects from multi sensors(64-lidar, 16-lidar, radar...)
融合部分总体来看，没有特别大的惊喜，仍是使用了传统的卡尔曼滤波。
Apollo 介绍中说到，他们使用了 object-level 的数据融合，该部分的输入为各传感器处理后的得到的object。
这里顺便一提，多源信息的数据融合中，根据数据抽象层次，融合可分为三个级别[2]：
- 数据级融合 传感器裸数据融合，精度高、实时性差，要求传感器是同类的
- 特征级融合 融合传感器抽象的特征向量(速度，方向等)，数据量小、损失部分信息
- 决策级融合 传感器自身先做出决策，融合决策结果，精度低、通信量小、抗干扰强
Apollo 应该是在特征层面对 objects 进行了融合。每当节点收到新的一帧数据的时候，融合部分就被调用。融合部分的输入为 SensorObjects, 输出为融合后的 object， 其大体的流程如下图所示。
这其中，最重要的步骤自然就是融合了，Apollo 依照传感器的测量结果，根据is_background 标志位，将 objects 分为 ForegroundObjects 和 BackgroundObjects 两类, 融合时只处理 ForegroundObjects。
融合函数位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc
void 从之前的经验我们可以看出，传感器的数据融合有两部分内容比较重要，即 数据关联 和 动态预估。
数据关联
数据关联的接口定义如下：
// @brief match sensor objects to global tracks build previously
数据关联的实现是在:
obstacle/fusion/probabilistic_fusion/pbf_hm_track_object_matcher.cc
bool 其实从文件名称中的"hm"就可以看出一些端倪了，与 Lidar 相似，Fusion 部分的数据关联还是使用了匈牙利算法（Hungarian Matcher）来进行分配的。
但是在计算关联距离时，两者有比较大的不同。Fusion 部分计算的只是两个 object 中心的几何距离。我们稍微看一下这里的代码，有个细节比较有趣。
Fusion 在计算关联距离时，使用了以下代码。
函数位置：obstacle/fusion/probabilistic_fusion/pbf_track_object_distance.cc
float 上面这段代码表明了，Fusion 在计算几何距离时，要求计算的两个 obj 中至少有一个是来自于 Lidar 的，并且以 Lidar 为基准来测量距离。
但只靠几何距离来进行数据关联，容易出现 Miss Match 的问题。即，当两条航迹发生交叉时，只靠几何距离是无法为 object 关联合适的航迹的。我现在还不清楚 Apollo 2.0 打算如何规避这个问题。
动态预估
动态预估还是使用的卡尔曼滤波，这部分有两点比较有趣：
- Apollo 的融合部分为了可扩展性使用的是标准卡尔曼滤波
- Apollo 融合部分的卡尔曼滤波使用了非简化的估计误差协方差矩阵
更新公式
我们对比上图标准卡尔曼滤波的公式，来说明我为什么觉着上述两点比较有趣。
1. 使用标准卡尔曼滤波
从代码细节上看，Apollo 使用了标准卡尔曼滤波，并且在更新来自 Lidar 和 Radar 的 object 的时候，Apollo 对这两者使用了相同的观测矩阵
这里有趣的地方在于，一般而言 Lidar 和 Radar 的观测矩阵是不同的, 因为两者得到的数据不同 。为了更好地说明这个问题，我建议大家先阅读一下下面这篇文章[3]。
扩展卡尔曼滤波EKF与多传感器融合 - CSDN博客blog.csdn.net
这篇文章描述了使用卡尔曼滤波融合 Lidar 和 Radar 的一种方式，为了方便阅读理解，我这里做个简述。
- Lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值z=(x, y)，使用卡尔曼滤波 (KF)。
- Radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值为 z=(ρ,ϕ,
)，非线性，使用扩展卡尔曼滤波(EKF)。
- lidar和radar的参数更新部分是不同的，不同的原因在于不同传感器得到的测量值是不同的
- 其传感器融合步骤如下图所示：
反观 Apollo 这里，在融合部分出乎意料的简单。
除了预测部分，因为观测得到的数据不同，Lidar 和 Radar 有些许不同。整体看来就是一个标准的 Kalman 滤波器。之所以 Apollo 没有使用扩展卡尔曼滤波器 （EKF）来处理 Radar 数据, 根据 Apollo 社区自己的回答，其原因在于 Apollo 使用的毫米波雷达输出的位置和速度信息是在笛卡尔坐标系下的，即 (x , y) 的格式，而非常见的极坐标的格式[4]。
2. 使用了非简化的估计误差协方差矩阵
我们简单看一下区别
- 标准卡尔曼滤波：
- Apollo：
对于这一点，我一开始并不太明白，特地去 Apollo 技术开发群中进行了询问。不得不说，百度对 Apollo 的推广还是十分用心的，我当天就得到了答复[4]。
【Apollo直答号】Apollo高精地图用的是标准的OpenDRIVE吗？还是自己定义的格式？mp.weixin.qq.com
结合 Wikipedia 上关于卡尔曼滤波的介绍，我先总结下该问题的背景 [5]:
- Apollo 使用的估计误差协方差矩阵
Joseph form，而标准卡尔曼滤波通常使用的是简化版的更新公式
的更新公式是所谓的
- 简化版的更新公式计算量小，实践中应用广，但只在 卡尔曼增益为最优 时有效
- 必须使用 Joseph form 的两种情况：
- 使用了非最优卡尔曼增益
- 算法精度过低，造成了数值稳定性相关的问题
Apollo 社区回答了两点原因，一是出于算法精度的考虑；二是由于计算单元的强大（且昂贵），非简化版的卡尔曼滤波在计算时也不会消耗太久的时间。综合考虑，Apollo 选择了 Joseph form 的更新方程。
最后，我们用一个表格对障碍物感知部分做个简单的小结
模块入口的选择
我在分析 Apollo 时将重点放在了模块的实现和解决方案上，而非代码的编写技巧。因此关于代码只在本篇最后简单说一下上篇里提到过的函数入口的问题。
Preception 模块的入口
Preception 模块的主入口其实还是很明确的，为：
modules/perception/main.cc
APOLLO_MAIN这是一个宏，展开之后为
int 关于这段代码的具体含义，强烈建议参考下列的链接，这篇文章对此说得很清楚，这里就不再展开了[6]。值得一提的是，Apollo 大部分模块的都是以类似的形式开始的，分析的方式也类似。
Apollo Planning模块源代码分析blog.csdn.net
感知模块的初始化代码如下所示：
Status 程序启动代码为：
Status 从这两段中可以看到有向无环图 ( directed acyclic graph , DAG )的身影。其实在官方文档里就有提到，感知模块的框架是基于 DAG 图的，每一个功能都以 DAG 中的 sub-node 的形式出现[7]。
The perception framework is a directed acyclic graph (DAG). There are three components in DAG configuration, including sub-nodes, edges and shared data. Each function is implemented as a sub-node in DAG. The sub-nodes that share data have an edge from producer to customer.
DAG 图由以下几个 Subnode 构成
障碍物感知：
- LidarProcessSubnode
- RadarProcessSubnode
- FusionSubnode
交通灯检测
- TLPreprocessorSubnode 、
- TLProcSubnode
DAG 图如下图所示：
那么到现在为止，其实各部分的程序实现的入口应该比较明朗了，只要找到各个Subnode的执行代码即可。
Doxygen
百度提供了 Doxygen 的文档系统，十分有助于我们了解具体各部分代码之间的关系[8]。
Doxygen是一种开源跨平台的，以类似JavaDoc风格描述的文档系统，完全支持C、C++、Java、Objective-C和IDL语言，部分支持PHP、C#。注释的语法与Qt-Doc、KDoc和JavaDoc兼容。Doxygen可以从一套归档源文件开始，生成HTML格式的在线类浏览器，或离线的LATEX、RTF参考手册 Doxygen_百度百科
Apollo 的 Doxygen 的网址见下[9]：
Apollo: Main Pageapolloauto.github.io如图为障碍物感知 （obstacle） 的 Directory dependency graph。
由图片可见，障碍物感知调用了 Radar, Lidar 和 Fusion 的相关函数，其入口在 onboard 文件夹中。我们来看下 onboard 文件夹中有哪些文件。
.
├── BUILD
├── fusion_subnode.cc
├── fusion_subnode.h
├── hdmap_input.cc
├── hdmap_input.h
├── hdmap_input_test.cc
├── lidar_process.cc
├── lidar_process.h
├── lidar_process_subnode.cc
├── lidar_process_subnode.h
├── lidar_process_test.cc
├── object_shared_data.h
├── obstacle_perception.cc
├── obstacle_perception.h
├── radar_process_subnode.cc
├── radar_process_subnode.h
└── sensor_raw_frame.h可以推断，这些文件就是障碍物感知涉及到的 Subnode 的执行代码了。我是选取的以下3个文件作为分析的入口。
- Radar -> http://radar_process_subnode.cc
- Lidar -> http://lidar_process_subnode.cc
- Fusion -> http://fusion_subnode.cc
这里其实也遗留了一些问题，由于我并不打算进行交通灯感知的分析，直接跳到这里开始分析障碍物感知部分。因而造成一个问题是， 我不太清楚某些文件的具体作用。
比如 http://obstacle_perception.cc，这个文件看着很像入口，描述也很像入口, 内容也很像入口，但是和 Subnode 的框架似乎没什么关系。
位置: obstacle/onboard/obstacle_perception.cc
/**
这个函数中直接调用了障碍物感知的3个核心函数
1. 再比如 http://lidar_process.cc，这个文件内容与 http://lidar_process_subnode.cc 的内容相似，但也不完全相同，也令人感到疑惑。十分希望大家可以给我一些指点。
后记
据 Apollo 技术交流群的消息， Apollo 2.5 将会在4月左右发布。
百度 Apollo 更新迭代的速度的确令人感到佩服，相信同时也会给竞争对手带来不小的压力。Apollo Github 上日益增长的活跃度，众多开发者对 Apollo 高涨的热情，Apollo 对开发者问题的迅速反馈，无疑虑都彰显了开源的魅力。
如果 Apollo 能以现在的频率保持更新，我相信再迭代一到两个版本之后，Apollo 会成为一个不错的、可用的开源无人驾驶的 Demo 系统，这对学习者和许多中小型公司都极具诱惑力。
但是从 Demo 到量产，两者之间的鸿沟是巨大的。百度为了构建自己的 Apollo 生态, 势必要争取与主机厂合作的机会，证明自己的量产实力。但落地之路如此艰难，百度是否拥有量产的能力？传统的车企巨头、Tier1中，有多少愿意给百度证明自己的机会？自动驾驶的未来又会以何种方式到来呢？我很期待！
以上，感谢阅读。
如需转载，请注明出处。
注：文章中有部分观点来自于与 mentor 、同事以及朋友的讨论，感谢你们提供的指导和帮助！
该系列的汇总链接见：
Apollo 2.0 框架及源码分析(零) | 引言
扫一扫
7533
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
