无人驾驶系统的基本结构：感知，规划和控制

无人驾驶系统的核心可以概述为三个部分：感知（Perception），规划（Planning）和控制（Control），这些部分的交互以及其与车辆硬件、其他车辆的交互可以用下图表示： 

感知是指无人驾驶系统从环境中收集信息并从中提取相关知识的能力。其中，环境感知（Environmental Perception）特指对于环境的场景理解能力，例如障碍物的位置，道路标志/标记的检测，行人车辆的检测等数据的语义分类。 一般来说，定位（Localization）也是感知的一部分，定位是无人车确定其相对于环境的位置的能力。

规划是无人车为了某一目标而作出一些有目的性的决策的过程，对于无人驾驶车辆而言，这个目标通常是指从出发地到达目的地，同时避免障碍物，并且不断优化驾驶轨迹和行为以保证乘客的安全舒适。规划层通常又被细分为任务规划（Mission Planning），行为规划（Behavioral Planning）和动作规划（Motion Planning）三层。

最后，控制则是无人车精准地执行规划好的动作的能力，这些动作来源于更高的层。

▌感知

环境感知

为了确保无人车对环境的理解和把握，无人驾驶系统的环境感知部分通常需要获取周围环境的大量信息，具体来说包括：障碍物的位置，速度以及可能的行为，可行驶的区域，交通规则等等。无人车通常是通过融合激光雷达（Lidar），相机（Camera），毫米波雷达（Millimeter Wave Radar）等多种传感器的数据来获取这些信息，本节我们简要地了解一下激光雷达和相机在无人车感知中的应用。

激光雷达是一类使用激光进行探测和测距的设备，它能够每秒钟向环境发送数百万光脉冲，它的内部是一种旋转的结构，这使得激光雷达能够实时的建立起周围环境的3维地图。

通常来说，激光雷达以10Hz左右的速度对周围环境进行旋转扫描，其扫描一次的结果为密集的点构成的3维图，每个点具备（x,y,z）信息，这个图被称为点云图（Point Cloud Graph），如下图所示，是使用Velodyne VLP-32c激光雷达建立的一个点云地图：

激光雷达因其可靠性目前仍是无人驾驶系统中最重要的传感器，然而，在现实使用中，激光雷达并不是完美的，往往存在点云过于稀疏，甚至丢失部分点的问题，对于不规则的物体表面，使用激光雷达很难辨别其模式，在诸如大雨天气这类情况下，激光雷达也无法使用。

为了理解点云信息，通常来说，我们对点云数据进行两步操作：分割（Segmentation）和分类（Classification）。其中，分割是为了将点云图中离散的点聚类成若干个整体，而分类则是区分出这些整体属于哪一个类别（比如说行人，车辆以及障碍物）。分割算法可以被分类如下几类：

基于边的方法，例如梯度过滤等；

基于区域的方法，这类方法使用区域特征对邻近点进行聚类，聚类的依据是使用一些指定的标准（如欧几里得距离，表面法线等），这类方法通常是先在点云中选取若干种子点（seed points），然后使用指定的标准从这些种子点出发对邻近点进行聚类；

参数方法，这类方法使用预先定义的模型去拟合点云，常见的方法包括随机样本一致性方法（Random Sample Consensus，RANSAC ）和霍夫变换(Hough Transform，HT)；

基于属性的方法，首先计算每个点的属性，然后对属性相关联的点进行聚类的方法；

基于图的方法；

基于机器学习的方法；

在完成了点云的目标分割以后，分割出来的目标需要被正确的分类，在这个环节，一般使用机器学习中的分类算法，如支持向量机（Support Vector Machine，SVM）对聚类的特征进行分类，最近几年由于深度学习的发展，业界开始使用特别设计的卷积神经网络（Convolutional Neural Network，CNN）对三维的点云聚类进行分类。

然而，不论是提取特征-SVM的方法还是原始点云-CNN的方法，由于激光雷达点云本身解析度低的原因，对于反射点稀疏的目标（比如说行人），基于点云的分类并不可靠，所以在实践中，我们往往融合激光雷达和相机传感器，利用相机的高分辨率来对目标进行分类，利用Lidar的可靠性对障碍物检测和测距，融合两者的优点完成环境感知。

在无人驾驶系统中，我们通常使用图像视觉来完成道路的检测和道路上目标的检测。道路的检测包含对道路线的检测（Lane Detection），可行驶区域的检测（Drivable Area Detection）；道路上路标的检测包含对其他车辆的检测（Vehicle Detection），行人检测（Pedestrian Detection），交通标志和信号的检测（Traffic Sign Detection）等所有交通参与者的检测和分类。

车道线的检测涉及两个方面：第一是识别出车道线，对于弯曲的车道线，能够计算出其曲率，第二是确定车辆自身相对于车道线的偏移（即无人车自身在车道线的哪个位置）。一种方法是抽取一些车道的特征，包括边缘特征（通常是求梯度，如索贝尔算子），车道线的颜色特征等，使用多项式拟合我们认为可能是车道线的像素，然后基于多项式以及当前相机在车上挂载的位置确定前方车道线的曲率和车辆相对于车道的偏离。

可行驶区域的检测目前的一种做法是采用深度神经网络直接对场景进行分割，即通过训练一个逐像素分类的深度神经网络，完成对图像中可行驶区域的切割。 

交通参与者的检测和分类目前主要依赖于深度学习模型，常用的模型包括两类：

以RCNN为代表的基于Region Proposal的深度学习目标检测算法（RCNN，SPP-NET，Fast-RCNN，Faster-RCNN等）；

以YOLO为代表的基于回归方法的深度学习目标检测算法（YOLO，SSD等）

▌定位

在无人车感知层面，定位的重要性不言而喻，无人车需要知道自己相对于环境的一个确切位置，这里的定位不能存在超过10cm的误差，试想一下，如果我们的无人车定位误差在30厘米，那么这将是一辆非常危险的无人车（无论是对行人还是乘客而言），因为无人驾驶的规划和执行层并不知道它存在30厘米的误差，它们仍然按照定位精准的前提来做出决策和控制，那么对某些情况作出的决策就是错的，从而造成事故。由此可见，无人车需要高精度的定位。

目前使用最广泛的无人车定位方法当属融合全球定位系统（Global Positioning System，GPS）和惯性导航系统（Inertial Navigation System）定位方法，其中，GPS的定位精度在数十米到厘米级别之间，高精度的GPS传感器价格也就相对昂贵。融合GPS/IMU的定位方法在GPS信号缺失，微弱的情况下无法做到高精度定位，如地下停车场，周围均为高楼的市区等，因此只能适用于部分场景的无人驾驶任务。

地图辅助类定位算法是另一类广泛使用的无人车定位算法，同步定位与地图构建（Simultaneous Localization And Mapping，SLAM）是这类算法的代表，SLAM的目标即构建地图的同时使用该地图进行定位，SLAM通过利用已经观测到的环境特征确定当前车辆的位置以及当前观测特征的位置。

这是一个利用以往的先验和当前的观测来估计当前位置的过程，实践上我们通常使用贝叶斯滤波器（Bayesian filter）来完成，具体来说包括卡尔曼滤波（Kalman Filter），扩展卡尔曼滤波（Extended Kalman Filter）以及粒子滤波（Particle Filter）。

SLAM虽然是机器人定位领域的研究热点，但是在实际无人车开发过程中使用SLAM定位却存在问题，不同于机器人，无人车的运动是长距离的，大开放环境的。在长距离的运动中，随着距离的增大，SLAM定位的偏差也会逐渐增大，从而造成定位失败。

在实践中，一种有效的无人车定位方法是改变原来SLAM中的扫描匹配类算法，具体来说，我们不再在定位的同时制图，而是事先使用传感器如激光雷达对区域构建点云地图，通过程序和人工的处理将一部分“语义”添加到地图中（例如车道线的具体标注，路网，红绿灯的位置，当前路段的交通规则等等），这个包含了语义的地图就是我们无人驾驶车的高精度地图（HD Map）。

在实际定位的时候，使用当前激光雷达的扫描和事先构建的高精度地图进行点云匹配，确定我们的无人车在地图中的具体位置，这类方法被统称为扫描匹配方法（Scan Matching），扫描匹配方法最常见的是迭代最近点法（Iterative Closest Point ，ICP），该方法基于当前扫描和目标扫描的距离度量来完成点云配准。

除此以外，正态分布变换（Normal Distributions Transform，NDT）也是进行点云配准的常用方法，它基于点云特征直方图来实现配准。基于点云配准的定位方法也能实现10厘米以内的定位精度。

虽然点云配准能够给出无人车相对于地图的全局定位，但是这类方法过于依赖事先构建的高精度地图，并且在开放的路段下仍然需要配合GPS定位使用，在场景相对单一的路段（如高速公路），使用GPS加点云匹配的方法相对来说成本过高。

▌规划

任务规划

无人驾驶规划系统的分层结构设计源于2007年举办的DAPRA城市挑战赛，在比赛中多数参赛队都将无人车的规划模块分为三层设计：任务规划，行为规划和动作规划，其中，任务规划通常也被称为路径规划或者路由规划（Route Planning），其负责相对顶层的路径规划，例如起点到终点的路径选择。 

我们可以把我们当前的道路系统处理成有向网络图（Directed Graph Network），这个有向网络图能够表示道路和道路之间的连接情况，通行规则，道路的路宽等各种信息，其本质上就是我们前面的定位小节中提到的高精度地图的“语义”部分，这个有向网络图被称为路网图（Route Network Graph），如下图所示：

这样的路网图中的每一个有向边都是带权重的，那么，无人车的路径规划问题，就变成了在路网图中，为了让车辆达到某个目标（通常来说是从A地到B地），基于某种方法选取最优（即损失最小）的路径的过程，那么问题就变成了一个有向图搜索问题，传统的算法如迪科斯彻算法（Dijkstra’s Algorithm）和A*算法（A* Algorithm）主要用于计算离散图的最优路径搜索，被用于搜索路网图中损失最小的路径。

行为规划

行为规划有时也被称为决策制定（Decision Maker），主要的任务是按照任务规划的目标和当前的局部情况（其他的车辆和行人的位置和行为，当前的交通规则等），作出下一步无人车应该执行的决策，可以把这一层理解为车辆的副驾驶，他依据目标和当前的交通情况指挥驾驶员是跟车还是超车，是停车等行人通过还是绕过行人等等。

行为规划的一种方法是使用包含大量动作短语的复杂有限状态机（Finite State Machine，FSM）来实现，有限状态机从一个基础状态出发，将根据不同的驾驶场景跳转到不同的动作状态，将动作短语传递给下层的动作规划层，下图是一个简单的有限状态机：

如上图所示，每个状态都是对车辆动作的决策，状态和状态之间存在一定的跳转条件，某些状态可以自循环（比如上图中的循迹状态和等待状态）。虽然是目前无人车上采用的主流行为决策方法，有限状态机仍然存在着很大的局限性：首先，要实现复杂的行为决策，需要人工设计大量的状态；车辆有可能陷入有限状态机没有考虑过的状态；如果有限状态机没有设计死锁保护，车辆甚至可能陷入某种死锁。

动作规划

通过规划一系列的动作以达到某种目的（比如说规避障碍物）的处理过程被称为动作规划。通常来说，考量动作规划算法的性能通常使用两个指标：计算效率（Computational Efficiency）和完整性（Completeness），所谓计算效率，即完成一次动作规划的处理效率，动作规划算法的计算效率在很大程度上取决于配置空间（Configuration Space），如果一个动作规划算法能够在问题有解的情况下在有限时间内返回一个解，并且能够在无解的情况下返回无解，那么我们称该动作规划算法是完整的。

配置空间：一个定义了机器人所有可能配置的集合，它定义了机器人所能够运动的维度，最简单的二维离散问题，那么配置空间就是[x, y]，无人车的配置空间可以非常复杂，这取决于所使用的运动规划算法。

在引入了配置空间的概念以后，那么无人车的动作规划就变成了：在给定一个初始配置（Start Configuration），一个目标配置（Goal Configuration）以及若干的约束条件（Constraint）的情况下，在配置空间中找出一系列的动作到达目标配置，这些动作的执行结果就是将无人车从初始配置转移至目标配置，同时满足约束条件。

在无人车这个应用场景中，初始配置通常是无人车的当前状态（当前的位置，速度和角速度等），目标配置则来源于动作规划的上一层——行为规划层，而约束条件则是车辆的运动限制（最大转角幅度，最大加速度等）。

显然，在高维度的配置空间来动作规划的计算量是非常巨大的，为了确保规划算法的完整性，我们不得不搜索几乎所有的可能路径，这就形成了连续动作规划中的“维度灾难”问题。目前动作规划中解决该问题的核心理念是将连续空间模型转换成离散模型，具体的方法可以归纳为两类：组合规划方法（Combinatorial Planning）和基于采样的规划方法（Sampling-Based Planning）。

运动规划的组合方法通过连续的配置空间找到路径，而无需借助近似值。由于这个属性，它们可以被称为精确算法。组合方法通过对规划问题建立离散表示来找到完整的解，如在Darpa城市挑战赛（Darpa Urban Challenge）中，CMU的无人车BOSS所使用的动作规划算法，他们首先使用路径规划器生成备选的路径和目标点（这些路径和目标点事融合动力学可达的），然后通过优化算法选择最优的路径。

另一种离散化的方法是网格分解方法（Grid Decomposition Approaches），在将配置空间网格化以后我们通常能够使用离散图搜索算法（如A*）找到一条优化路径。

基于采样的方法由于其概率完整性而被广泛使用，最常见的算法如PRM（Probabilistic Roadmaps），RRT（Rapidly-Exploring Random Tree），FMT（Fast-Marching Trees），在无人车的应用中，状态采样方法需要考虑两个状态的控制约束，同时还需要一个能够有效地查询采样状态和父状态是否可达的方法。后文我们将详细介绍State-Lattice Planners，一种基于采样的运动规划算法。

▌控制

控制层作为无人车系统的最底层，其任务是将我们规划好的动作实现，所以控制模块的评价指标即为控制的精准度。控制系统内部会存在测量，控制器通过比较车辆的测量和我们预期的状态输出控制动作，这一过程被称为反馈控制（Feedback Control）。

反馈控制被广泛的应用于自动化控制领域，其中最典型的反馈控制器当属PID控制器（Proportional-Integral-Derivative Controller），PID控制器的控制原理是基于一个单纯的误差信号，这个误差信号由三项构成：误差的比例（Proportion），误差的积分（Integral）和误差的微分（Derivative）。

PID控制因其实现简单，性能稳定到目前仍然是工业界最广泛使用的控制器，但是作为纯反馈控制器，PID控制器在无人车控制中却存在一定的问题：PID控制器是单纯基于当前误差反馈的，由于制动机构的延迟性，会给我们的控制本身带来延迟，而PID由于内部不存在系统模型，故PID不能对延迟建模，为了解决这一问题，我们引入基于模型预测的控制方法。

预测模型：基于当前的状态和控制输入预测未来一段时间的状态的模型，在无人车系统中，通常是指车辆的运动学/动力学模型；

反馈校正：对模型施加了反馈校正的过程，使预测控制具有很强的抗扰动和克服系统不确定性的能力。

滚动优化：滚动地优化控制序列，以得到和参考轨迹最接近的预测序列。

参考轨迹：即设定的轨迹。

下图表示模型预测控制的基本结构，由于模型预测控制基于运动模型进行优化，在PID控制中面临的控制延时问题可以再建立模型考虑进去，所以模型预测控制在无人车控制中具有很高的应用价值。

▌小结 

在本结我们概述了无人驾驶系统的基本结构，无人驾驶软件系统通常被划分为三层：感知，规划和控制。从某种程度上而言，无人车在这种分层体系下就可以看作是一个“载人机器人”，其中，感知具体包括环境感知和定位，近年来深度学习的突破，使得基于图像和深度学习的感知技术在环境感知中发挥了越来越重要的作用，借助人工智能，我们已经不再局限于感知障碍物，而逐渐变成理解障碍物是什么，理解场景，甚至预测目标障碍物的行为，机器学习和深度学习的内容我们将在后面两章详细了解。

在实际的无人车感知中，我们通常需要融合激光雷达，相机和毫米波雷达等多种测量，这里涉及到的如卡尔曼滤波，扩展卡尔曼滤波等融合算法以及激光雷达。

无人车和机器人的定位方法众多，目前主流的方法一是使用GPS+惯性导航系统融合的方法，二是基于Lidar点云扫描匹配的方法，将重点介绍ICP，NDT等基于点云匹配的算法。

规划模块内部也被分成三层：任务规划（也被称为路径规划），行为规划和动作规划。后文会介绍基于路网和离散路径搜索算法的任务规划方法，在行为规划中，我们将重点介绍有限状态机在行为决策中的应用，在动作规划算法层，重点介绍基于采样的规划方法。

无人车的控制模块我们往往会使用基于模型预测的控制方法，但是在了解模型预测控制算法之前，作为对基础反馈控制的了解，我们前面了解了PID控制器。接着我们学习两类最简单的车辆模型——运动学自行车模型和动力学自行车模型，最后，我们介绍模型预测控制。

虽然将无人车理解为机器人并且使用机器人开发的思维处理无人车系统是目前工业界的共识，但是也不乏一些单纯使用人工智能或者是智能体来完成无人驾驶的案例。其中基于深度学习的端到端无人驾驶和基于强化学习的驾驶智能体是目前的研究热点。