自动驾驶的技术,从软件层面来说,分成基于嵌入式系统的软件平台部分,以及算法部分。前者提供类似OS(如Windows、MacOs,或手机上的IOS、Android)一样的运行支持和资源分配管理功能,后者更接近于提供具体功能的APP。
自动驾驶的算法一般主要分成感知、融合、规划、控制,其中规划处于承上启下的位置,作用是根据感知结果和环境信息,产生车辆的移动轨迹,从而让控制算法按照这个轨迹控制车辆的移动。
这篇文章说的就是规划算法之中一个常用的算法:EM Planner。它针对的是L4级别的自动驾驶场景。
EM planner同时覆盖多车道和单车道的自动驾驶,基于EM-type iterative algorithm。
整体思路:
并行计算每条车道的轨迹,基于Frenet frame来迭代计算最优的path和speed。
比较每条车道的轨迹,判断换道策略。
计算最优的path和speed:结合动态规划和基于样本的二次规划求解。
主要考虑的自动驾驶安全角度:
交通法规
覆盖范围:8秒或200米的轨迹
工作周期:100ms内
紧急安全:当前方输入模块出错,输出紧急保险策略
场景覆盖(基本行驶、换道、拥堵等)
交通法规
舒适度(轨迹的smoothness)
对于L4自动驾驶,必须考虑换道策略。要决定车辆在不同车道中,走哪一条车道的哪个轨迹,常规做法是在所有车道中,基于cost来搜索最优轨迹,但这样的弊端在于:
搜索空间太大成本高;
交规在不同车道是不同的;
应避免突然换道。
EM中的换道策略分成主动换道和被动换道,主动换道由routing模块触发,默认车道被阻挡则触发被动换道。
所有障碍物和环境信息都会投射到每个备选车道的Frenet frames,每个车道再各自符合交规,每个车道都会根据cost生成一条最优轨迹,再选出全局最优轨迹。
求解最有轨迹的算法:Path-Speed Iterative Algorithm
1、直接求解:
轨迹采样或栅格(lattice)搜索。受限于随着空间时间而增加的搜索复杂度,因此得出的轨迹是suboptimal次优的。path主要考虑静态障碍,然后在生成的路径上计算速度。对于动态障碍可能找不到最优解。
2、path-speed去耦求解:
首先,由最新的speed信息评估未来的、低速的动态障碍,从而得出基于这个速度的最优path。然后生成的这个path又返回给speed计算针对新的path的最优的speed。
注意,这里的speed不会再马上传给path,所以迭代不是在同一个计算周期里进行,而是在下一个周期再进行迭代的。这里最好想一想,理解一下。
对于高速障碍,EM采用换道而不是闪躲来处理,因此对于高速障碍,EM也能处理。至于为什么要换道而不是闪躲,我猜测是因为闪躲对于动态障碍物的预测精度要求更高,而去耦求解的EM的预测前提是“预测都是基于当前周期的speed和path来进行的”,并不会考虑基于这个预测作出的行动会影响到障碍物的轨迹,这种交互对于高速动态障碍物来说,足够产生无法忽略的预测偏差,导致不能进行安全闪躲。
EM中,为了平滑轨迹,在规划开始之前就进行决策。
常见决策有两种模式:
Hand-tuning decision:可以调,但受限于规模。还有可能出现超出其定义的决策规则。
Model-based decision:将车辆状态分解成有限的驾驶状态,用大量数据来训练模型。
EM的决策同时采用了以上两种模式。但说到模型应该更多在于预测模块对障碍物轨迹的预测,EM的决策目标还是提供一个大概的可选空间,决策的内容主要是换道、重用、针对每个障碍物判断是否闪躲超车等。
更抽象来说,EM中的决策,首先将ego car的移动意图描述成一个粗略但灵活的轨迹,在这条轨迹上评估与障碍的交互,从而保证scalable。然后,planner还会生成一个凸空间,用于在轨迹上平滑样点参数。最后,一个二次规划求解器依据decision生成平滑的path和speed。
(EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY )
首先,所有数据汇总到data center,生成车道级的参考线(带交规和障碍物,基于高精地图和导航信息)。对每个车道,首先基于参考线构建F坐标系,随后所有评估均基于此。
随后的决策和优化,均基于场景管理器和任务管理器进行。在一个场景下,按顺序执行定义好的tasks,其中就包含各种path和speed的决策和优化tasks。
具体来看,经过决策后将可选轨迹空间限定缩小范围,重构数据再传给Optimizer(优化器),进行path和speed优化。
在path优化时,周围信息投影到F坐标,优化的目标是在决策限定的空间内生成一条平滑的path,可以想象成闪躲障碍物往前走的游戏,玩家需要选择一条路线通过这条狭窄的通道,舒适又快捷地通过。
在speed优化时,障碍物投影到ST图(station-time graph),同样是为了生成一段平滑的speed profile(也就是尽量减少加速刹车)。
然后,组合path和speed,获取该车道的平滑轨迹。
最后,所有车道的最佳轨迹汇总,轨迹决策器基于当前车辆状态、交规、每条轨迹的成本,选择最优轨迹。
现在分析单个车道的规划过程。
首先,迭代是发生在不同的cycle之间。计算新轨迹是基于上一个周期计算的轨迹结果来开始的。
SL投影:
障碍物投影到F坐标。
静态障碍物:
直接通过Cartesian-Frenet 转换(笛卡尔坐标到F坐标)。
动态障碍物:
在Apollo中通过障碍物移动轨迹描述,结合上一个cycle计算得到的轨迹,可以评估动态障碍物和ego car之间的在不同时间点的位置关系。其中两者重叠的部分会被标记在F坐标中。
(由于动态障碍物往往会导致闪避nudge,所以SL投影里只会考虑低速和靠近中的障碍物)
根据生成的path,将所有障碍物(包含高速的)投影到station-time frame,这个ST图是基于参考线做的,但无论是障碍物的轨迹,还是自车的轨迹,都是基于每条path来存储及对比是否重叠的以及何时重叠的。
如果障碍物轨迹与规划轨迹重叠,便标记出这个区域。
通过动态规划获取一个rough solution(从此获取障碍物策略:闪躲、让道、超车等),然后基于此决定二次规划样点优化器的凸空间主体(a convex hull for the quadratic-programming-based spline optimizer)。
(虽然所有计算都基于凸空间——内部所有点的连接线都在空间内——但实际上最优轨迹在非凸空间里。)
A. SL and ST Mapping (E-step)
SL投影基于G2(continuous curvature derivative) 平滑参考线。
在笛卡尔坐标系,障碍物和自车状态通过定位和朝向(x,y,θ) ,车道曲度及曲度变化率(κ, dκ) 来描述。
在F坐标下,这些信息转换成(s, l, dl, ddl, dddl) ,即station、lateral及它的导数。
静态障碍物与时间无关,因此坐标转换能直接进行。
动态障碍物的坐标转换需要上一次计算出的自车轨迹。首先自车轨迹投射到F坐标,获取the station direction speed profile(提供自车在某个时间点的station坐标,从而评估与动态障碍物的相对关系)。自车station坐标与障碍物轨迹重叠部分就可以计算出来,如下图紫色部分:
ST投影能帮助评估自车的speed profile。
当path投影到F坐标后,若障碍物轨迹会跟自车产生交互,就会在图上显示。
上图中,一个障碍物在2秒后驶入自车道40m前;空白部分就是speed profile的可选空间。
B. M-Step DP Path
查找横向坐标l = f(s) 的最优解(在SL空间中基于station坐标计算),包含两步:
动态规划
spline-based 规划
(但实际上Apollo 6.0之中,path的DP过程被简化成获取粗略可选空间的path boundary,并不需要复杂的cost函数优化求解过程。)
包含cost计算、lattice采样、动态搜索。其中lattice采样需要在自车前获取数row的点,不同row的点平滑连接。row里撒点的间距由速度、道路结构、换道等决定,可以自定义。距离覆盖8秒或200米。
Lattice建立后,每个图边缘通过cost函数评估——基于SL投影、交规、车动态信息。总edge cost函数是“平滑性、避障、车道成本”的线性组合。
平滑cost函数中,f ′ (s)代表车道与自车的偏差角, f ′′ (s)是车道曲率。w则代表不同项的权重。
障碍物cost函数,与障碍物距离d大于设定的安全值dn时为0,小于碰撞距离dc则是无穷大。
参考线cost,代表与参考线的距离成本,其中g(s)是参考线函数。
C. M-Step Spline QP Path
QP的目标,是在DP产生的path可选空间中,通过QP spline solver对由线性化约束的目标函数进行最优化,在其内生成一条平滑的path。
QP的目标函数是平滑cost和guidance line cost的线性组合。后者用到的比较项是DP产生的可选空间里的大量path,用于评估障碍物的闪避距离。然后通过优化找最优解的方式,找出其中cost最低,也就是最优的path。
由于车辆的朝向会影响车辆边界与周围的位置关系,也就是需要确定车辆四个角与station点的关系,而不只是l=f(s)。
这样就需要知道车头朝向,其他三个角类似。
边界的cost函数都可以进行线性化处理,随后就可以用二次规划快速求解。
D. M-Step DP Speed Optimizer
DP在ST图里生成speed profile。包含cost函数,ST图栅格,动态搜索。
具体来说,首先障碍物信息分散到ST的栅格中,栅格根据间隔dt平均划分,速度的具体参数大概可以认为:
Vref 是参考速度,一般是车道的限速。第一项代表维持速度的cost,其后两项代表平滑性,最后是障碍物cost——自车与所有障碍物的距离。
F. Notes on Solving Quadratic Programming Problems
基于安全考虑,Apollo EM评估的点约100个,约束条件大于600个。
QP中计算结果包含3到5个多项式,约30个参数。
因此二次规划的目标函数较小而约束较多,求解较容易。同时为了加速计算,采用上周期的结果作为计算起点(hot start),平均3ms就能得到结果(看算力)。
G. Notes on Non-convex Optimization With DP and QP
DP和QP在非凸空间都有各自的不足。
DP:依赖于采样栅格,有限的栅格数量只能给出一个粗略的DP解。即很可能不是最优解。譬如DP能得出向左闪躲的路径,但闪躲的距离不一定是最好的。
QP:相反,QP解依赖于凸空间,因此不能脱离DP使用。譬如,没有DP的decision,QP无法决定该怎么走。
DP+QP:将各自的不足最小化。先找一个粗略的解,再在这个凸空间里找最优。
根据初始速度计算交汇点,然后得出path,同时根据约束计算speed,speed更改后交汇点发生变化于是重新计算path,根据新的path重新计算交汇之后的speed(交汇之前的若也变化,那这个迭代就需要不断进行了)。
COMPUTATIONAL PERFORMANCE
本来的三维问题(station-lateral-speed)分解成两个二维问题(station-lateral和station-speed),因此计算复杂度大大降低。对n个障碍物,M条备选path和N条备选speed,计算复杂度是O(n(M+N)) 。
EM planner是一个轻决策算法,与其他基于大量规则的算法不同,无需提前预估障碍物之间的相互影响,也不会因为规则变多导致找不到合适轨迹而规划失败。
安全性和通过性互相矛盾,安全规则增加后,可选的路径就会减少,而且很容易导致走到一半的换道突然被中断,而EM Planner则更可能保持连贯性,且得到可用的轨迹。
EM的计算复杂度较低。
EM已在北京和CA实地测试过,截止20180516,跑了3380小时、68000公里。并在仿真环境跑了10w小时和100w公里。
以上是论文的结论。
EM Planner整个框架能处理L4的自动驾驶场景,而且采用场景管理和任务管理,在去耦的角度看处理的也比较好,将三维问题分解成两个二维问题,是这个算法最大的特点了。整个算法中,最核心的部分在于要理解Cost函数的构造和求解。
apollo中包含了两种轨迹规划方法:Lattice和EM Planner,其中Lattice主要是采样和剪枝的思想,EM主要是优化的思想。二者的目标都是求取代价最小的路径
lattice planner只要设计了六个代价函数,考虑了到达目标,平滑,避免碰撞,向心加速度,横向偏移,舒适性
em planner将轨迹规划分为两部分优化:path即d-s(轨迹的横向偏移量和纵向行驶距离),speed即s-t(轨迹的纵向行驶距离与时间的关系);优化分为两个步骤:DP动态规划求大致解,更像是给出nudge或者overtake这样的决策,把优化问题变为凸优化问题,为后续的QP二次规划提供参考。QP则求精细平滑的轨迹。