自动驾驶自主避障概况

文章目录

• 前言
• 1. 自主避障在自动驾驶系统架构中的位置
• 2. 自主避障算法分类
• • 2.1 人工势场法（APF）
  • • 2.1.1引力势场的构建
    • 2.1.2斥力势场的构建
    • 2.1.3人工势场法的改进
  • 2.2 TEB（Timed-Eastic-Band, 定时弹性带）
  • 2.3 栅格法
  • 2.4 向量场直方图(VFH)
  • 2.5 智能避障算法
  • • 2.5.1 模糊逻辑算法
    • 2.5.2 基于神经网络的避障算法
    • 2.5.3 基于遗传算法的避障算法
  • 2.6 强化学习算法
  • 2.7 DWA算法
  • 2.8 EM Planner
• 参考文献

前言

在GitHub上找到了路径规划与运动规划方面不错的学习资料：

• PathPlanning——https://github.com/zhm-real/PathPlanning
• MotionPlanning——https://github.com/zhm-real/MotionPlanning

1. 自主避障在自动驾驶系统架构中的位置

    Claudine Badue^[1]等人以圣西班牙联邦大学（UFES）开发的自动驾驶汽车（Intelligent Autonomous Robotics Automobile，IARA）为例，提出了自动驾驶汽车的自动驾驶系统的典型架构。如图所示，自动驾驶系统主要由感知系统（Perception System）和规划决策系统（Decision Making System）组成。感知系统主要由交通信号检测模块（Traffic Signalization Detector，TSD）、移动目标跟踪模块（Moving Objects Tracker，MOT）、定位与建图模块（Localizer and Mapper）等组成。规划决策系统主要由全局路径规划模块（Route Planner）、局部路径规划模块（Path Planner）、行为决策模块（Behavior Selector）、运动规划模块（Motion Planner）、自主避障模块(Obstacle Avoider)以及控制模块（Controller）组成。

图1-1 自动驾驶系统架构图

    自主避障功能的实现主要依赖于运动规划模块和定位与建图模块。自主避障模块接收由运动规划模块计算出的轨迹、定位模块所提供的车辆位置信息以及建图模块提供的地图信息，对轨迹进行适当的调整，并在必要时改变它（通常是降低速度），最后将该调整结束的轨迹传给控制模块进行车辆控制，以达到避障的功能。

2. 自主避障算法分类

    自主避障模块接收由运动规划模块计算出的轨迹、定位模块所提供的车辆位置信息以及建图模块提供的地图信息，对轨迹进行适当的调整，并在必要时改变它（通常是降低速度），最后将该调整结束的轨迹传给控制模块进行车辆控制，以达到避障的功能。本文将介绍一些关于自主避障算法，如向量直方图、虚拟势场法（APF）、智能避障算法以及强化学习算法等。
    Guidolini等人^[2]在自动驾驶汽车IARA中应用了一个自主避障系统，在每个运动规划周期中，接收一个在线地图，表示汽车周围的环境，在线地图中自动驾驶汽车的当前状态，以及运动规划系统规划的轨迹。自主避障系统模拟轨迹，如果预测在轨迹中发生碰撞，自主避障系统会降低自动驾驶汽车的线速度，以防止碰撞。他们的软件架构如图2-1所示。

图2-1 IARA自主避障系统构成

    自动驾驶避障依赖于地图、定位与运动规划，地图模块包括离线地图与在线地图，定位模块提供车辆的位置信息，这两者都属于感知层的任务；运动规划模块则是自主避障实现的核心。运动规划模块负责计算从当前自动驾驶汽车的状态到当前目标的轨迹， $T$。该轨迹必须尽可能遵循由行为选择模块定义的路径， $p_j$，同时满足汽车的运动学和动力学约束，并为乘客提供安全和舒适性。
    对于运动规划产生的轨迹，有两种主流的定义方式：

1. 将轨迹定义成一个由一系列控制命令组成的集合，即，$T_c=c_1,c_2,...,c_{|T|}$，其中控制命令 c k = { v k , φ k , Δ t k } c_k = \{{v_k,φ_k,Δt_k}\} ck​={vk​,φk​,Δtk​},$v_k$是无人驾驶车辆在$t=k$时的速度，${\phi }_k$是无人驾驶车辆在$t=k$时的期望方位角，$\Delta t_k$是无人驾驶车辆一个命令周期的时间。
2. 将轨迹定义成一个由一系列状态组成的集合，即， T s = { s 1 , s 2 , . . . , s ∣ T ∣ } T_s = \{s_1,s_2,...,s_{|T|}\} Ts​={s1​,s2​,...,s∣T∣​}，其中状态 c k = { p k , t k } c_k = \{p_k,t_k\} ck​={pk​,tk​},$p_k$是一个位姿点，$t_k$是无人驾驶车辆达到预期位姿点所需要的时间。

    规划模块通过输出一个轨迹，将汽车具有安全性、舒适性的带到目标点。
    对于运动规划算法，González^[3]给出了十分详细的介绍与分类，同时对常见算法的优缺点进行分析，见图2-2与表2-1。

图2-2常见运动规划与避障算法以及算法的时间线.(a)为Dijkstra算法.(b)为基于优化的方法.©为Lattice采样算法.(d)为A*算法.(e)为RRT算法.(f)用线和圆等简单几何图形去简化地图的算法.(g) Clothoid曲线.(h)多项式插值.(i)贝塞尔曲线.(j)三次样条曲线.

表2-1 常见运动规划与避障算法的优缺点

    Paden^[4]主要从算法的稳定性、时间复杂度、算法复杂度、是否最优等方面对常见的Dijkstra算法、Lattice算法、A*算法以及一些融合算法进行分析。

2.1 人工势场法（APF）

    人工势场法(artificial potential field，APF)是由Oussama Khatib博士提出的一种应用于研究机器人的路径方法。人工势场法的基本原理就是通过一系列环境感知传感器来探知环境的障碍物情况，无人车在多个斥力势场和一个引力势场的和势场环境下沿着势场下降的方向运动。人工势场法是一种广泛应用的路径规划方法，适用于已知环境或未知环境。人工势场法本质上是一种控制方法，其轨迹并非像其他规划算法一样，而是由实时的控制量产生的。

图2-3 人工势场法势场可视化图。(a)引力势场。(b)斥力势场。

2.1.1引力势场的构建

    在势场中目标位置对无人车产生吸引的影响，构成引力势场，驱动着无人驾驶汽车向着希望的目标位置运动。无人驾驶汽车所受的引力大小主要和汽车与目标位置的间隔距离有关系，若无人车与目标位置间隔越大，引力就越大；反之，引力就越小。当无人汽车到达目标位置的时候，车和目标位置的间距为零，因此，无人车所受的引力逐渐减小为零。通过查阅资料发现，无人车引力和无人车、目标点三者之间的距离是成正比关系，因此，我们可以使用幂函数来对定义引力势场：$$Uatt=\frac{1}{2}Katt{P}_0^m$$式中 $K_{att}$ —— 引力势场正比列系数；
     $P_0$ —— 无人驾驶车辆和目标点位置的直线距离；
     $m$ —— 引力势场因子。
     通常我们取引力势场因子$m=2$，引力势场正比列系数$K_{att}$可以视具体情况选取。
     无人驾驶汽车受到引力$F_{att}$作用，引力$F_{att}$是引力势场函数$U_{att}$的负导数，意义是引力势场的变化最快方向。引力的大小是引力势场函数对无人车和目标位置之间的距离取的导数，因此可得：$$Fatt(P0)=-\nabla Uatt(P0)=-KattP0$$

2.1.2斥力势场的构建

    在人工势场法中障碍物对智能驾驶汽车会产生排斥的影响，构成斥力势场，引导着无人驾驶汽车远离障碍物运动，从而顺利躲避障碍物顺利到达目标点。无人驾驶汽车所受的斥力大小和距离有关系，此距离大小就是车和障碍物相隔的距离，如果无人车和障碍物相隔的距离很小，斥力就很大；反之，斥力就很小。当无人汽车离开障碍物时，如果两者之间的距离超过斥力势场的影响范围，无人车不会受到斥力的影响。通过查阅资料可知，斥力势场和无人车、障碍物的距离是呈反比的，可以将斥力势场定义为：$$Urep(Pg)=\{\begin{array}{c}\frac{1}{2}Krep{\left(\frac{1}{Pg}-\frac{1}{\rho }\right)}^2,Pg\le \rho \\ 0,Pg>\rho \end{array}$$式中 $\rho$ —— 障碍物对无人车产生斥力的最大作用范围；
     $P_g$ —— 障碍物和无人车的距离；
     $K_{rep}$ —— 斥力势场中的正比例系数。
     由斥力势场公式可知，斥力、引力势场之间是有区别的，无人车不会一直受到障碍物的影响，只有当无人车在障碍物影响范围内即无人车和障碍物的相隔距离$P_g$小于障碍物的物影响范围$\rho$时才会受到斥力的作用，当无人车与障碍物的间距大于$\rho$时，无人车将不会受到斥力影响。需要注意的是，当无人车靠近障碍物时，斥力会慢慢的增大，相应的斥力是斥力势场的负导数，表达式为：$$Frep(Pg)=\{\begin{array}{c}Krep(\frac{1}{Pg}-\frac{1}{\rho })\frac{1}{P_g^2},Pg\le \rho \\ 0,Pg>\rho \end{array}$$     通过将引力势场和斥力势场合成即可得到合势场，无人车在合势场的作用下，将会沿着合势场梯度下降最快的方向前进。人工势场法通过实时的控制量产生轨迹，控制简单，易于实现。但存在着当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在可以忽略的情况下，物体路径上可能会碰到障碍物；当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点；在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡，发生“死锁”现象；动态避障能力差等缺点。

2.1.3人工势场法的改进

     L.Liu等人^[5]提出了一种采用中断点策略去提升人工势场法的算法速度。他们首先用$A^{\ast }$算法获得全局路径，并获得一系列路径点，将这些路径点作为中断点，来防止人工势场法陷入局部极小值。同时对人工势场法在路径规划的同时进行参数迭代，进行优化。最后利用最小二乘法对路径进行平滑处理。该方法解决了APF目标点不可达与易陷入局部极小值的问题。
     D. Wu等人^[6]提出了一种基于改进快速随机树算法（$IRR{T}^{\ast }$）与APF结合的算法。该算法同样了防止陷入局部最小值，同时与RRT*等算法相比，提高了算法的运行速度，减少了搜索时间，如表2-2所示。

表2-2 RRT*、IRRT*、A*-RRT*、P-RRT*和APF-IRRT* 在六个不同环境中独立运行20次的搜索时间。

    Song Jia等人^[7]针对超高音速的飞行器的路径规划问题，利用LSTM框架对飞行器的时间作出约束，同时将APF限定在一定范围内，以处理禁飞区的问题。
    Li Yongyi等人^[8]提出了一种改进的人工势场法，通过增加距离调整因子、动态道路排斥场、速度排斥场和加速度排斥场来完成自动驾驶轨迹的规划。为了解决传统人工势场法的缺陷，引入了入侵杂草算法。从车辆动力学模型出发，对预测模型进行线性化和离散化处理，设定相应的约束变量，建立优化目标函数，构建MPC模型控制器，实现轨迹跟踪的目标。

2.2 TEB（Timed-Eastic-Band, 定时弹性带）

    TEB 是一种经典的两轮差动机器人局部避障算法。其作用是在全局路径规划的基础上，遵守机器人动力学约束，基于最小化运动时间获取最具效率的局部避障路径^[9]。其在线实时优化的特性，支持最小转向半径和航向的修正的特点，使其具备 改进为智能车辆的局部避障算法的基础。
    TEB算法的前身是 1987 年Richard Durbin 等人提出的 EB （Eastic-Band）算法^[10]。EB算法将路径比作多点连接的橡皮筋，而每一段橡皮筋都受到来自上一段 橡皮筋的拉力和来自路径点的吸引力，皮筋的收缩特性使得每一段的皮筋的长度 都尽可能的缩短，从而“规划”出一条最具效率的通行路径。将该模型抽象化，建 立数学模型，并定义代价函数，通过最小代价获取“最优”的解。而其中的多个连 接点实际上就是被控对象在该点的姿态。而TEB就是在上叙轨迹的基础上，人为的划分时间点，从而获取规划出轨迹的运动学信息。
    在应用 TEB算法时，需要先定义汽车为位姿：$$s_i=[x_i,y_i,{\theta }_i{]}^T$$式中 KaTeX parse error: Expected group after '_' at position 3: xi_̲——智能汽车在激光雷达SLAM建立的局部代价地图中横向位置；
    $y_i$——智能汽车在激光雷达SLAM建立的局部代价地图中横向位置；
    ${\theta }_i$——智能汽车在该坐标系下的航向角。
    在避障过程中，用$Q$表示智能汽车避障过程中$n$ 个位姿点的序列： Q = { s i } , i = 0... n Q = {\rm{\{ }}{s_i}{\rm{\} }},i = 0...n Q={si​},i=0...n    TEB是基于时间来进行位置点的划分，用$\Delta T_i(i=\mathrm{0...}n-1)$表示连续位姿之间的时间步长。则整个避障过程的时间步长的序列可表示为： τ = { Δ T i } , i = 0... n − 1 \tau = \{ \Delta {T_i}\} ,i = 0...n - 1 τ={ΔTi​},i=0...n−1    将位置序列和时间步长序列合并，产生了基于时间步长的位姿,如图2-4所示：$$B=(T,\tau )$$

图2-4 TEB 算法中基于时间步长的位姿序列

    在实际的避障过程中往往需要考虑多个优化目标，比如安全性、舒适性、避障效率以及多个约束，建立 $m$个优化函数 $f_k(B)$。根据各个优化目标的权重不同最终建立优化函数： $$f(B)=\sum {\eta }_k{f}_k(B),k=\mathrm{1....}m$$式中 ${\eta }_k——f_k(B)$的权重系数，反映了该约束或优化目标的在规划中的权重。
    当代价函数 $f(B)$取最小值时，此时的位姿集合 B就是所求的“最优” $B^{\ast }$。 $$B^{\ast }={\arg }_B\min f(B)$$

    此时产生的结果实际上只是“最优”的位姿序列，而且这个“最优”序列的产 生只取决于这个位姿序列。值得注意的是，在寻优过程中，并没有将目标轨迹中所有的点都带入代价函数中进行计算。因此，“最优”序列的产生只取决于时间间隔为$\Delta T_i$的位姿——时间 序列。也正是因为这一特性，TEB 算法的求解也简化成稀疏系统矩阵的求解。
    TEB优化问题一般转化为以位姿和时间间隔为节点超图（hyper-graph）问题进行求解^[11]。超图是指其中的一条边的连接节点可以不受限制的连接多个节点，而节点之间的连接就是目标函数。图2-5 是建立的位姿、时间间隔、速度、加速度和加加速度之间的超图。图2-6 是TEB算法的一般流程图。

图2-5 位姿、时间间隔、速度、加速度和加加速度之间的超图

图2-6 TEB算法的一般流程图

    TEB 算法是基于“弹性带”的形变实现了位置的遍历，再通过代价函数来实现“最优”路径的选取。但“弹性带”的形变无法穿越障碍物，由此产生的“最优” 轨迹，很容易陷入局部最优的特性，往往导致局部避障失败。如图 2-7（a）是基于TEB 的局部避障算法正常工作的场景。其中黑色实心的圆表示障碍物，障碍物外虚线的圆环表示规划中汽车离障碍物允许的最小距离；如图2-7（b）是 TEB 规划失败的场景。其中红色部分区域，尽管在 TEB 局部避障算法中的规划是局部“最优”解，但可以看到，由于两个障碍物之间距离过近，中间的区域并不容许无人驾驶汽车顺利的通过，从而导致了避障的失败。而此时只要在这两个距离较近的外侧重新进行规划，绕开这两个障碍物，就可以完成局部的避障。

图2-7 TEB 算法的局限性

2.3 栅格法

    栅格法与人工势场法几乎同时被提出，它是用一种栅格形状的物理模型来表示障碍物出现的可能性。通过栅格法，无人驾驶汽车可以在静态环境中实现准确导航，做出有效的避障动作。然而它对工作区域的大小有一定的要求，如果区域太大，将使栅格的数量急剧增加，计算量增大不利于实时动态环境下的避障。
    王醒策等人^[12]综合势场法和栅格法的优点, 提出了一个新的全局路径规划方法——势场栅格法。算法在避免局部最优点和降低计算量方面, 有着良好的效果;并且可以自动确定栅格粒度。

2.4 向量场直方图(VFH)

    由于势场法的缺陷,Borenstein等人于1991年提出向量场直方图法。此方法将机器人的工作环境分解为一系列具有二值信息的栅格单元，每个矩形栅格都有一个累计值(CV) ,表示在此处存在障碍物的可信度^[13]。VFH算法是在用二维栅格描述环境信息之后,通过2轮的环境数据压缩,选择极坐标直方图的波谷从而确定机器人的运动方向。VFH在阈值选取得当的情况下不会陷人局部极小值,并且允许机器人快速通过障碍物而不会出现不稳定现象,至今仍被广泛应用。但是,VFH是把被控对象当做一个点来处理，没有考虑其宽度、动力学及运动学特性,使得在实际中较难完成计算出来的预定轨线。

2.5 智能避障算法

    智能避障算法是一种启发式优化算法，包括模糊逻辑算法、遗传算法、蚁群算法、粒子群算法、模拟退火算法、神经网络算法、禁忌搜索算法等。

2.5.1 模糊逻辑算法

    模糊逻辑（Fuzzy Logic，简称FL）算法是将模糊控制中感知与动作智能结合起来^[14]。关键在于建立合适的模中感知与动作智能结合起来糊控制器，有３个主要特点：
    ①用语言变量代替数学变量；
    ②用模糊控制条件语句描述变量之间的关系；
    ③用模糊算法描述复杂关系。
    模糊逻辑算法成功地处理了定位精度差的问题，但是不能够自主学习且缺乏灵活性，确定了模糊规则与隶属度后无法更改。
    Oriolo等人^[15]为了提高路径规划的实时性，在用$A^{\ast }$算法做全局路径规划的同时采用模糊逻辑算法对地图与相关参数进行迭代更新，实验结果表明，他们的算法使得机器人在陌生环境中的路径规划的实时性良好。

2.5.2 基于神经网络的避障算法

    神经网络（Neural Network，简称NN）是一种模仿生物结构与功能的模型，也是一种非线性数据建模工具。基于神经网络算法的避障方法是一个由简单处理元构成的规模宏大的并行分布式处理器，天然具有存储经验知识和使之可用的特性。神经网络算法在模糊规则与隶属度确定上具有较强灵活性。神经网络可以将传感器收集到的信息数据看作输入，无人车下一位置的运动方向看作输出，剔除冗余与对立样本，得到最终的样本集。神经网络的学习规则简单、自主学习能力强，但若从传感器得到的数据不完整，神经网络就无法进行下去，而且泛化能力差，所以在避障规划应用上并不成功。
    Kassim等人^[16]描述了一种称为波展开神经网络（Wave Expansion Neural Network, WENN）的神经网络，并表明它能够开发多种用于路径规划的APF。该算法将离散化环境包括目标配置（位置和方向）和障碍物的信息应用到WENN作为输入。产生信号以波的形式在WENN神经场传播，在平衡状态下，产生的神经活动分布形成所需求得的APF。该方法能够用于三维路径规划之中，对于更高维的状况，具有潜在的研究潜力。

2.5.3 基于遗传算法的避障算法

    遗传算法（GA）^[18]由Holland于1975年提出。在遗传算法中，问题的所有可能解都编码为形成初始种群的染色体。构建了几个基本操作∶交叉，变异和选择。生成初始种群，利用目标计算个体的适合度值，以便确定可选择的基本操作。GA虽然具有强大的搜索能力和较高的搜索效率，但容易提前收敛的问题使得它它接近问题的最优解时收敛速度降低。
    Roberge等人……[17]^提出了一种基于遗传算法的能够在复杂的现实三维环境下计算准最优轨迹的固定翼军用无人机的路径规划算法。规划出的轨迹被建立为由圆弧连接的一系列线段，如图2-8所示，并被优化以减少燃料消耗和平均高度，以增加无人机在军事行动背景下的范围和机动性。该算法采用了一种遗传算法和四种不同的交叉操作符的混合，使得无人机使用的GPU并行高效处理计算。这种多样性允许更好地探索搜索空间，并提高了路径规划模块的整体性能。

图2-8 基于遗传算法GPU并行的三维路径规划可视化图

2.6 强化学习算法

    强化学习是指智能车利用本身的传感器不断与环境相互作用来获得之前未知的环境知识，是一种仿生算法。智能车感知到环境中的一个状态并根据上一次的状态转移过程获得一个奖赏，利用学习到的策略知识，将当前环境状态映射到自身动作，并作用于环境，环境产生一个状态转移过程，使得智能车感知到一个新状态，如此循环往复。
    强化学习的４个要素为：策略、奖赏、值函数及非必需的环境模型。

• 策略是从周围环境感知的形态到在此环境下可采用一种映射；
• 奖赏，也就是立即回报，表示智 能车的眼前目标；
• 值函数是智能车获得的所有奖赏累加 的期望值，可以看作智能车的长期回报，指明了智能车在 长期运行过程中动作选择的标准；
• 环境模型给出了状态 转移过程中详细的概率分布。

    在经典强化学习算法中， Sa’rsa算法与Q-learning算法都能在有限时间内解决智能车避障问题，并给出最优解。强化学习具有在线学习与自主学习的特点。

    Chen Chaorui等人^[19]针对传统的Q-learning算法存在收敛速度慢、收敛难以得到最优解、泛化能力差等问题，提出了一种改进的Q-learning算法。，通过修改贴现率、学习率等参数，解决收敛速度慢的问题，提高价值更新的准确性；引入单链回溯算法来提高代理的学习速度；设计基于单链集的Q-learning算法来解决单链中的无效状态循环；将人工势场法与共享单链库相结合，增强代理在环境探索中的目标指导。
    Ma Tian等人^[20]针对传统的Q-learning算法在复杂环境中存在收敛速度慢的问题，出了一个连续的局部搜索Q-learning（Continuous Local Search Q-Learning, CLSQL）算法来解决这些问题，并保证规划路径的质量。首先，将全局地图逐渐划分为独立的局部地图；再利用先验知识在每个局部环境中搜索中间点；最后，在每个中间点之间的搜索，以到达目标点。通过比较其他算法，该方法在保证最优路径的同时，提高了收敛速度和计算时间。CLSQL的学习策略如图2-9所示.

图2-9 CLSQL的学习策略

    Xu Shenghua等人^[21]提出了一种基于Q-learning算法的室内应急路径规划方法。其探索因子的折扣率用于优化Q-learning算法，ε-贪婪策略中的探索因子在选择随机动作前进行动态调整，以加速大规模网格环境下Q-learning算法的收敛。他们利用模拟数据和真实室内环境数据，进行了基于Q-learning算法的室内应急路径规划实验。结果表明，在网格环境下规划最短路径时，提出的Q-learning优化算法在求解时间和收敛速度方面优于SARSA算法和经典Q-learning算法。他们所提出的Q-learning算法的收敛速度比经典的Q-learning算法快大约5倍，能够在短时间内成功规划出避开障碍物区域的最短路径。

2.7 DWA算法

DWA算法参考这篇博文自动驾驶路径规划——DWA（动态窗口法）

2.8 EM Planner

    EM Planner是Apollo面向L4的实时运动规划算法，该算法首先通过顶层多车道策略，选择出一条参考路径，再根据这条参考线，在Frenet坐标系下，进行车道级的路径和速度规划，规划主要通过Dynamic Programming和基于样条的Quadratic Programming实现。EM Planner充分考虑了无人车安全性、舒适性、可扩展性的需求，通过考虑交通规则、障碍物决策、轨迹光滑性等要求，可适应高速公路、低速城区场景的规划需求。通过Apollo仿真和在环测试，EM Planner算法体现了高度的可靠性，和低耗时性^[24]。
    EM Planner的整体架构如图2-12所示。

图2-12 EM Planner架构

    在顶层的数据中心,所有来源的信息都被收集和同步。采集数据后，参考线生成器会生成一系列携带交通规则信息和障碍物信息的参考线。这个过程要基于从Routing模块的导航信息和HD地图。在运动规划中，先构造Frenet框架。在框架中构建车辆与周围环境的关系。再将构建的关系传递给规划器。规划器模块执行速度和路径规划 ^[25]。
在路径规划过程中，周围环境的信息被投影到Frenet框架中（E-step）。然后基于投影的信息产生一条平滑的路径(M-step)。
    速度规划也是类似。在速度优化过程中，一旦路径优化器生成了一条平滑的路径，障碍物就会投影到station-time graph 中（E-step）。 然后，速度优化器将生成平滑的速度曲线（M-step）。
    结合路径和速度曲线，我们将获得指定车道的平滑轨迹。在最后一步中，所有车道级别的最佳轨迹都将发送到参考线轨迹决策器。 根据当前的汽车状态，法规和成本，最后将路径和速度结合获得一条平滑的轨迹。最后一步中将所有得到的轨迹发送到参考线决策器，根据当前汽车状态、法规和每条轨迹的代价，轨迹决策器将选择最优的轨迹交由汽车去执行。

    图2-13展示了EM Planner车道级别规划内的路径-速度EM迭代。迭代过程包括两个E-step和两个M-step。轨迹信息将在规划周期之间迭代。

图2-13 EM Planner 车道级规划

    Apollo EM Planner以分层的方式涵盖了多车道和单车道自动驾驶：
    (1)该系统的顶层是一种多车道策略，通过比较并行计算的车道级轨迹来处理变道场景。
    (2)在车道级轨迹发生器内，迭代解决基于Frenet帧的路径和速度优化。
    (3)对于路径和速度优化，提出了动态规划(Lattice采样、代价函数、动态规划搜索)和基于样条的二次规划相结合，构建一个可伸缩、易于调整的框架，同时处理交通规则、障碍决策和平滑性。
     EM Planner可扩展到高速公路和低速城市驾驶场景。 图2-14 动态规划与二次规划结合.(a)基于Lattice的动态规划.(b)基于样条的二次规划

参考文献

[1] C. Badue, et al. Self-driving cars: A survey[J]. Expert Systems with Applications, 2021, 165.
[2] Guidolini, et al. & De Souza, A. F. A simple yet effective obstacle avoider for the iara autonomous car. In 2016 IEEE 19th international conference on intelligent transportation systems (ITSC) ,2016, 1914–1919.
[3] David González, Joshué Pérez, Vicente Milanés. A Review of Motion Planning Techniques for Automated Vehicles[J]. IEEE Transactions on intelligent transportation systems, vol. 17, NO. 4, april 2016.
[4] Paden, B., Čáp, M., Yong, S. Z., Yershov, D., & Frazzoli, E. A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles, 2016, 1(1), 33–55.
[5] L. Liu, B. Wang,H. Xu. Research on Path-Planning Algorithm Integrating Optimization A-Star Algorithm and Artificial Potential Field Method[J]. Electronics, 2022, 11(22).
[6] D. Wu, et al. APF-IRRT*: An Improved Informed Rapidly-Exploring Random Trees-Star Algorithm by Introducing Artificial Potential Field Method for Mobile Robot Path Planning[J]. Applied Sciences, 2022, 12(21).
[7] Song Jia,Xu Xiaowei,Tong Xindi,Zhao Kai. A Time Cooperation Guidance for Multi-Hypersonic Vehicles Based on LSTM Network and Improved Artificial Potential Field Method[J]. Aerospace,2022,9(10).
[8] Li Yongyi,Yang Wei,Zhang Xiaorui,Kang Xi,Li Mengfei. Research on Automatic Driving Trajectory Planning and Tracking Control Based on Improvement of the Artificial Potential Field Method[J]. Sustainability,2022,14(19).
[9] 贾屿. 基于TEB算法的无人驾驶汽车路径规划与避障技术研究[D].合肥工业大学,2021.DOI:10.27101/d.cnki.ghfgu.2021.001929.
[10]Durbin R, Willshaw D. An Analogue Approach to the Travelling Salesman Problem Using an Elastic Net Method[J]. Nature, 1987,326(6114):689-691.
[11]郑凯林, 韩宝玲, 王新达. 基于改进TEB算法的阿克曼机器人运动规划系统[J]. 科学技术 与工程, 2020,20(10):3997-4003.
[12]王醒策,张汝波,顾国昌.基于势场栅格法的机器人全局路径规划[J].哈尔滨工程大学学报,2003(02):170-174.
[13]郭笑笑,刘元盛,钟启学. 基于无人驾驶汽车的避障算法综述[C]//.中国计算机用户协会网络应用分会2017年第二十一届网络新技术与应用年会论文集.,2017:145-148.
[14]任子玉.智能车自主避障路径规划研究综述[J].软件导刊,2017,16(10):209-212.
[15]Oriolo G Ulivi G Venditelli M. Real-time map building and navigation for autonomous robots in unknown environments[J]. IEEE Transactions on Systems Man and Cybernetics-Part B:B- Cybermetics, 1998, 28(3):316-333.
[16]Kassim Ashraf A.,Vijaya Kumar B.V.K… Path planners based on the wave expansion neural network[J]. Robotics and Autonomous Systems,1998,26(1).
[17]Roberge Vincent,Tarbouchi Mohammed,Labonte Gilles. Fast Genetic Algorithm Path Planner for Fixed-Wing Military UAV Using GPU[J]. IEEE Transactions on Aerospace and Electronic Systems,2018,54(5).
[18]Holland, John. Adaptation In Natural And Artificial Systems[J]. University of Michigan Press, 1975.
[19]Chen Chaorui ,Wang Dongshu. Path Planning of Mobile Robot Based on the Improved Q-Learning Algorithm[J]. 革新的コンピューティング・情報・制御に関する国際誌,2022,18(03).
[20]Ma Tian,Lyu Jiahao,Yang Jiayi,Xi Runtao,Li Yuancheng,An Jinpeng,Li Chao. CLSQL: Improved Q-Learning Algorithm Based on Continuous Local Search Policy for Mobile Robot Path Planning[J]. Sensors,2022,22(15).
[21]Xu Shenghua,Gu Yang,Li Xiaoyan,Chen Cai,Hu Yingyi,Sang Yu,Jiang Wenxing. Indoor Emergency Path Planning Based on the Q-Learning Optimization Algorithm[J]. ISPRS International Journal of Geo-Information,2022,11(1).
[22]劳彩莲,*,冯宇.基于改进A*与DWA算法融合的温室机器人路径规划[J].农业机械学报,2021,52(01):14-22.
[23]Dieter Fox,Wolfram Burgard,Sebastian Thrun. The dynamic window approach to collision avoidance.[J]. IEEE Robot. Automat. Mag.,1997,4(1).
[24]Haoyang Fan, et al. Baidu Apollo EM Motion Planner[J]. arXiv:1807.08048v1 [cs.RO] 20 Jul 2018.
[25]Travis.X.论文解读Baidu Apollo EM Motion Planner[EB/OL].https://blog.csd n.net/Travis_X/article/details/109174898