百度 Apollo 轨迹规划技术分享笔记

老师简介
张雅嘉，百度高级架构师，百度 Apollo 平台规划方向技术负责人。印第安纳大学计算机科学博士，从事机器人运动规划方向。有多年机器人运动规划方向的研发经验，曾带领团队参加 DARPA 机器人竞赛。2016年加入百度，从事无人驾驶平台规划方向的研发工作。

1. 第一个问题：整个规划过程的输入个输出在 planning 代码中提现在哪个位？

A1：输入输出请参见PublicRoadPlanner这个类的实现

2. 第二个问题：每个实时规划的初始状态量，比如 s、速度、加速度等是以车体底盘实时反馈为主还是从组合导航获得，还是说通过一定方式从上帧规划结果获得参考量？交给控制去执行的连续两帧轨迹如何联系起来，才能保证控制模块在连接处速度、加速度、曲率等不发生突变？

A2：这个问题非常好，在今天的分享中没有专门的介绍。我这里简要描述一下，车辆的状态是由上游的定位模块获得的，融合了多种传感器的数据，包括当前地图坐标系下的坐标，朝向，转向角度，速度，加速度等等。
轨迹规划模块以固定的频率进行，我们使用了轨迹拼接的算法（Trajectory Stitching）保证相邻帧的轨迹在控制器看来是平滑的。假设我们的周期时间是dt秒，如果我们没有上一周期的轨迹，那我们使用运动学模型， 对当前从定位模块获得的车辆状态进行外推，获得dt时间之后的状态作为规划起始点，我们称之为重新规划（Replan）；如果上一周期的轨迹存在，我们会根据当前系统时间T，在上一周期的轨迹中找到相对应的轨迹点，然后，我们进行一个比较，比较这个轨迹点与定位模块获得的当前车辆状态的差异，如果这个差异在一定范围内，我们找到T+dt时间的上一周期轨迹点作为规划起始点；如果这个差异超过设定范围，说明控制器有了较大的误差，我们会做第一种情况的replan。

这种机制保证了在控制误差允许的情况下，做到相邻帧轨迹的平滑拼接。在控制器看起来，规划模块发出的轨迹是一小段一小段dt长度的轨迹光滑拼接起来的。

补充问题：为什么每次规划时，不以车辆当前的状态为规划起始点呢，而是“找到T+dt时间的上一周期轨迹点作为规划起始点”？

因为规划结果真正送到控制器是T+dt时刻

3. 第三个问题：在路径规划中常三阶导数的物理含义以及在算法中的作用是什么？

A3:三阶导数是加加速度，表示加速度的变化率。在物理上，是舒适性的一个重要指标。在算法中，相邻离散点之间的连接就是通过常三阶导数实现的。

4. 第四个问题：张老师在文章中提到：将速度规划问题分解成启发式的速度规划与分段加加速度算法的目的在于，需要进行离散的维度与时间的解耦。请问这里离散的维度是指什么？

A4：可能我写的不是很清楚，在速度规划里，离散的维度指的是时间。

5. 第五个问题：我们在实际使用PublicRoadPlanner时发现车辆总是倾向于以最大加速度加速，以最大减速度减速。这对于超车和紧急避障是合适的，但对于一般起步、过弯、调头、停车可能就不太合适。能否提供一套可以动态配置加减速力度的参数的方法？

A5：理论上是可以的，可以通过上游的决策层根据规划目标动态调整参数。

6. 第六个问题：人工驾驶车辆，遇到前方较远处有障碍物时，一般会松开油门，让车辆惯性前进，如果障碍物在离车辆距离较近时仍存在，则刹车减速，否则加速前行。请问规划算法如何描述松开油门靠惯性前行这一行为？

A6：这个主要靠设计代价函数来实现。如果我们惩罚二阶导数的话，意味着我们希望加速度尽可能小，来实现我们的目标。

7. 第七个问题：路径规划是否考虑了地形因素对车辆状态的影响？

A7：请问地形因素是否能举例？

8. 第八个问题：apollo lattice规划时，看到了车辆状态采集，横向为-0.5，0，0.5，这种状态下，拟合出的轨迹还能实现超车或躲避大型障碍物吗？

A8：不能，Apollo Lattice需要通过变换指引线来实现较大幅度的超车或躲避障碍物。

9. 第九个问题： 路径规划的分段加加速度优化算法中，架设障碍物在车道中间，可通行区间被分割成了上下两个区间，约束条件不再是简单的一个[lmin，lmax]，而变为或条件，请问这种情况要怎么处理？

A9：非常好的问题- 对于同一个离散点，我们会有不同的区间。我们设计了一个类，称为PathBoundsDecider，会处理不连续的区间。在实现中，我们会将不同的区间使用启发式方法加以排序，在时间允许的情况下，对每一种离散的区间都做路径规划。

10. 第十个问题： 在优化的迭代过程中，l_(i→i+1)^‴ 会随着相邻点 l_i^″ 与 l_(i+1)^″ 的变化而变化，但 l_(i→i+1)^‴ 在两个离散点之间会保持不变。请解释下这句话的含义？

A10：含义是加速度是优化的变量，会在优化迭代中变化；相邻点的加速度决定了加加速度，即一旦加速度确定，加加速度也就通过差分的方式唯一确定。

11. 第十一个问题：在第N次规划中规划出轨迹后，按照轨迹运行了一段距离。进入第N+1次循环后，如何利用上一次循环生成的轨迹？是保留一小段，后面的重新生成，还是直接舍弃之前的轨迹重新规划，还是从上一条轨迹中选择距离自己最近的点，从这个点开始规划？

A11：这个问题请参见Q2的解答。

12. 第十二个问题：决策层作为运动规划的上层，比如决策做出来了超车决定，运动规划层尝试进行规划，结果发现不行，这下该怎么办？类似的决策和规划层做出不一致的决定时，有没有在实际过程中比较好架构的来应对这样的问题？

A12：非常好的问题，在规划层面是个非常重要的问题。这个超出了今天分享的范围，我就简答说下可能的思路：规划层需要设计一个反馈的信号，反馈到决策层，通知决策层是否能够实现所做的决定。

13. 第十三个问题：Apollo 轨迹规划的起始条件之一是光滑的指引线，也就是说在获得指引线的情况下才开始做轨迹规划吗？如果是的话，那么该指引线本身是曲率可导，符合运动学规律的，为什么不在世界坐标系下作为无人车的可行轨迹，给到控制中呢？而要进一步在弗雷那坐标系下在进行一次规划呢？这样做有什么样的好处？

A13：是的，必须在有光滑指引线的情况下做规划。在Frenet坐标系下做规划，是为了避免直接考虑朝向，曲率等问题，避免在高维度上做规划。

14. 第十四个问题：GitChat的文章中主要介绍了apollo2.5里EMPlanner的设计思路，后续apollo版本中还有lattice planner以及open space环境下的planner。能对em planner和lattice planner在public road场景下的表现进行比较吗？对于open space中常见的倒车等轨迹规划，em planner和lattice planner能解决吗？

A14：对我们还没有综合性的对em与lattice进行比较。设计上，em适合解决复杂的城市道路，lattice适合解决相对简单的高速场景。倒车的话，em和lattice是不能解决的。

15. 第十五个问题：在弗雷那坐标系下，横向距离对于纵向路径的求导，是通过分段拟合后直接求导，还是离散化之后通过差分的形式近似呢？ppt关于三阶导数求解似乎是后者哦

A15：三阶导数是通过差分的形式直接计算出的。

16. 第十六个问题：EM Planner规划器中，通过惩罚路径、速度曲线的一阶导、二阶导、三阶导来进行规划，这种方法会在哪些场景下失效，有没有必要将这些约束换成车辆的动力学约束来进行运动规划？还是说将动力学约束下放到控制环节去做？

A16：我们会根据车辆的运动学模型，对一阶、二阶、三阶导数的取值范围做出估计，找出符合运动学的取值范围，约束求解轨迹的空间。

17. 第十七个问题：在EMplan中，路径规划的起点initpoint，会先计算该点的dl和ddl，而在实车调试时，这两个值可能会因为前端输入的heading等数据有一些波动，导致这两个值偏大，最终规划的路径为沿参考路径呈s型。如何规避这个问题？

A17：我觉得这是个综合的问题，如果heading出现抖动，不仅planning会受到影响，control受到的影响更大，而且这个是上游模块来的信息，这个问题不好，我个人认为也不应该，在planning中解决。

18. 第十八个问题：应当如何定义TTC？FTTI 对 planning 的影响是什么？

A18：请问TTC，FTTI的含义是什么？我不是很清楚。

19. 第二十个问题：车辆位置、障碍物位置投影到指引线上。请问具体的怎样确定投影后的 s 值？

A19：具体请参见相关代码：CartesianToFrenetConversion这个类的实现。

20. 第二十个问题：之前的em规划器无论路径、速度规划点与点之间以五次多项式相连，本文中所描述的方法貌似完全是在离散状态下计算与优化（包括目标函数），而不考虑点与点之间连接的函数表达，所以纵向点需要很密（路径规划中纵向点间距大概多远？）。这种完全离散的方法与之前em中qp规划方法相比好处在哪里？

A20：好问题。路径规划中，离散的s在0.5m以下，路径有200m长。好处在于我们获得了对路径更大的控制权，能够在非常复杂的问题中计算出可行轨迹。

21. 第二十一个问题：“每两个离散点之间，有一个常三阶导数项”，是不是可以理解为以函数形式为三次多项式？或者说是上个问题中表述的一样，是在完全离散的状态下计算的？

A21：可以这么理解，本质上也是一个多项式。

22. 第二十二个问题：在如图的 Uturn 情况，主车在大概 s1 处，感知到了障碍物 box，但同一个 box 的四个 corner 点被映射到了不同的 s 处，重构障碍物 sl 边界时就出错了，构造了以最大最小 s 及最大最小 l 构成的错误 sl_boundary，最后造成障碍物未占据车道而无法通行，请问你们是如何解决的？

A22：好问题，这个情况比较极端，现在我们是根据不同的情况做特殊处理，也没能很好的综合性解决方案。

23. 第二十三个问题：DpPolyPathOptimizer、QpSplinePathOptimizer、QpPiecewiseJerkPathOptimizer 、PiecewiseJerkPathOptimizer 这几个优化器有什么区别，应用的有什么不同？

A23：前面的3个已经被deprecate掉了，使用PiecewiseJerkPathOptimizer替代。应用商PiecewiseJerkPathOptimizer能够覆盖前面的优化器。

24. 第二十四个问题：在路径规划算法中，保证三阶导是每段的常数，那每一段的三阶导这个数是怎么确定的？

A24：通过差分二阶导数

25. 第二十五个问题：文章中提到“路径规划避让静态障碍物，速度规划避让动态障碍物”，而 2018 年的论文“Baidu Apollo EM Motion Planner”实验中路径规划中可以避让对向来车，这两个描述是否矛盾，怎样理解静态障碍物？对于速度较低的动态障碍物是否合适在路径规划中考虑的？

A25：静态障碍物可以以速度大小来区分。对于速度较低的障碍物，范围只要能够bound到一定的大小，也可以在路径规划中考虑。

26. 第二十六个问题：控制 kappa 相对控制角速度有什么优势吗？另外，纵向有考虑加速度，横向不考虑角加速度的原因？

A26：角速度涉及了路径和速度两个方面的综合因素。在我们的实现中，路径和速度分开考虑是为了降低难度。 直接考虑角加速度的算法我们正在开发，不久会发布。

27. 第二十七个问题：非和谐车辆系统就是非完整约束车辆系统吗？在路径规划时也需要考虑到当前车辆的运动状态而适当的调整车辆的几何形状，几何形状是指什么？多种平滑指引线的计算方法，有哪些常见方法？

A27：是的。应该是根据车辆状态来调整路径的几何形状吧？平滑算法可以参见我们的实现：QP Spline，spiral curve …

28. 第二十八个问题：如何判断车与障碍物是否相撞？

A28：请问是指在我们的实现中，如何计算避免碰撞的轨迹还是几何上怎么判断？

29. 第二十九个问题：之前做控制看到用 MPC 的思想去做避障的轨迹规划，请问这种方法和现在这种在图空间中采样的方法有什么优缺点？这两种方法有分类么？

A29：这个是个大问题，MPC是control-based的方法，在地图坐标系做规划，与我们在Frenet坐标系下规划不同。具体我们可以线下交流。

30. 第三十个问题：如果控制使用 MPC 的话，MPC 无解怎么办？

A30：无解了就没办法了，planning也有无解的时候啊

31. 第三十一个问题：路径规划的求解用的什么方法？另外这种求解方式耗费算力吗？

A31：使用的OSQP，计算路径的话，0.5m间隔200m总长，少于10ms

32. 第三十二个问题：规划算的参数很多都是相互耦合的, 应给如何调节? 有什么智能化的方法吗?

A32：我们以前尝试过auto tuning，现在还没有很好的进展。

33. 第三十三个问题：为什么每次规划时，不以车辆当前的状态为规划起始点呢，而是“找到 T+dt 时间的上一周期轨迹点作为规划起始点”？

A33：好想已经有参与者代为回答了

34. 第三十四个问题：如果在做路径规划的时候只考虑静态障碍物，那么在该路径上规划出的轨迹是否无法对动态障碍物进行合适的处理？

A34：有这个可能，这个是个为了降低规划难度做的牺牲。

35. 第三十五个问题：请问在路径规划模块中需要考虑障碍物运动的不确定性么。对于和其他车辆有交互的场景，例如堵车时加塞这样对路权有争夺的情况，如何去设计我们的决策方案？在实际路测中当前 Apollo 规划系统不能解决的主要有哪些问题呢？

A35：好问题。这需要上游模块，比如预测模块提供比较好的预测。现在planning中还没有相应的解决方案。

36. 第三十六个问题：做路径规划之前如何做出应该从某个障碍物上方绕过、下方绕过或者跟在障碍物后面的决策？

A36：通过用户设置代价函数，比如加速度的权重，等等，通过代价来进行判断。

37. 第三十七个问题：Apollo 基于场景、阶段和任务的理念进行规划，那么阶段（stage）该如何理解？这个过程做了什么？能否举例说明？

A37：Stage简要的说是一个场景中的一个阶段，几个stage顺序组合起来形成一个完整的场景。

38. 第三十八个问题：在 Apollo 中，关于 MPC 的控制和基于 frenet 的横纵向控制，二者之间什么关系？

A38：具体控制中的MPC实现，我不是很了解，我可以把问题转到我们做控制的同学。

39. 第三十九个问题：路径加载轨迹点，一次性全部读入缓冲区里放到ram里面吗？如果是这样做的话是不是有点浪费内存？

A39：不是很清楚您的问题，路径可能一共就400个点，ram的空间应该不需要多少吧。

40. 第四十个问题：未来无人车的规划决策以及控制会不会被强化学习取代？

A40：这问题太大… 我们线下讨论吧。

41. 第四十一个问题：路径规划算法怎样在脱离实际车辆进行测试？测试通过需要哪些量化指标之后再上实车验证？

A41：我们现在会通过仿真平台做初步验证，在planning成功率，轨迹质量加以检测之后，再做实车测试。

42. 第四十二个问题：在实际使用过程中规划路径经常出现跳变，导致控制不稳定，请问可以如何减小规划结果的跳动？

A42：这个和实际的实现有关系，合理的跳变表明对动态环境做出了反应；不合理的跳变说明在设计代价函数的时候有欠缺，需要改进设计。

43. 第四十三个问题：对于档位切换时，加减速度会滞后的车辆实施自动驾驶时，如何改进控制跟不上规划步调导致频繁重规划的问题？

A43：这个好像主要是控制的问题。在planning层面，我们可以适当的减小加速度的搜索范围，生成尽可能和缓的轨迹。

44. 第四十四个问题：能够根据生产的轨迹和当前雷达测到的其它车辆位置速度，预测碰撞风险吗? 如果其它车辆突然加速，实际是很困难感觉

A44：是的，这个是预测模块非常头疼的问题…

45. 第四十五个问题：指引线在道路居中规划时是否可以理解为车道中心线？

A45：可以这么理解，现在的实现也是这样；不排除将来我们有更好的方法找到不严格为道路中心线的更好的指引线。

46. 第四十六个问题：能否分享一下指引线的生成方法。Apollo里的cos_theta_reference_smoother和spiral_reference_line_smoother是如何工作的？（Apollo似乎还未提供文档进行解释）

A46：好的，如果有机会，我们可以专门有指引线生成方法的分享。具体技术细节很多，涉及了非线性优化的问题。

47. 第四十七个问题：智能车是如何控制速度的？如何让车辆达到预定的速度？

A47：这个超出了我的范围，需要控制的同学来回答 A48 纵向速度闭关控制，通过对ESC和EMS发送目标加减速度或扭矩。

48. 第四十八个问题：这个规划方案在哪个版本中有提现？可以离线运行吗？

A48：这个规划方案会在7月Apollo新版本正式发布。离线运行是指？

49. 第四十九个问题：目前针对前方低速前行（比自车速度低），这时候要不要换道是由上层的决策模块决定的吗？如果是，假如现在决策要换道，左右两边还有其它车辆的情况下，规划层如何根据左右两边的车辆运动情况进行规划？是先定一个沿参考路径方向的换道距离，确定当前决策下换道路径，后续不再改变这条路径，还是实时根据左右两边车辆运动轨迹，每一帧都规划一条与左右两边车辆没有碰撞的路径？

A49：是后一种情况，实时根据左右两边车辆运动轨迹，每一帧都规划一条与左右两边车辆没有碰撞的路径

50. 第五十个问题：车辆沿着光滑的道路指引线运动时，突然出现紧急情况时(比如cut in工况)，如何处理？决策层会优先执行预设的规避动作然后重新进行规划么？

A50：不会，我们以10HZ的方式进行规划就是为了能够及时更新轨迹，处理紧急情况。

51. 第五十一个问题：针对行人这类低速且方向随机的障碍物是否有专门的处理方式，举个例子，园区场景，单车道有行人自右侧向左横穿，走几步又停下再走，车辆如果按避障处理可能会在向左拐弯途中发现行人障碍物变到左侧，于是又试图向右避障，请问老师针对这种场景是否有对应解决办法？

A51：这个就是预测的问题了，好像这个问题折磨了预测同学很久，我可以问问他们最新进展。planning层面不适合对这样的情况做假设与处理。

52. 第五十二个问题：此处指的地形是指比如矿区、越野环境下，路面非常不平，若不考虑地形对车辆动力学的影响，规划的结果可能无法使用，在规划模块如何考虑比如土坡、水坑的影响？

A52：这个问题我还没考虑过，可能最先会做的是减小轨迹计算的搜索范围，让轨迹不要贴合车辆动力学的边界，提高能够执行的概率。

53. 第五十三个问题：在路径规划中是怎么计算几何上，本车与障碍物是否碰撞，实时性能如何？通过什么方法求解？

A53：二维bounding box碰撞，大概需要十几个向量点乘运算，可以参见Separate Axis theorem，中文可能叫超平面分割定理。

54. 第五十四个问题：请问这种算法已经发布在git上了吗？未来会代替lattice和em吗？

A54：是的，这是我们的计划，尽可能使用一套算法解决问题。

55. 第五十五个问题：请问是指在我们的实现中，如何计算避免碰撞的轨迹还是几何上怎么判断？

是几何上的。

A55：请参见A53

56. 第五十六个问题：目前对无保护左转弯用什么方法规划的？

A56：这个是决策层面的问题，具体请参见我们专门设计的场景

57. 第五十七个问题：如果目的地是在车后方不远处(eg: 10m)，规划的话是先掉头，还是直接倒车？这个权重如何取？

A57：在我们的规划中（onlane planner），我们假设的是一般公路场景，算法中不允许向后移动。Open space planner可以允许这样的操作。

58. 第五十八个问题：当规划路径，出现几种损失函数彼此不能兼顾，该优先选择那些损失函数？

A58：代价函数是几个因素加权的和。所以我觉得您指的是应该提高那些的权重？这个可能需要根据不同的应用场景、车辆类型来决定。

59. 第五十九个问题：请问规划时在停车点时的规划策略是怎样的 ？

A59：不是很明白您的问题。您指的是到了停车点，我们的后续策略吗？

60. 第六十个问题：这个规划方案在哪个版本中有提现？可以离线运行吗？ 离线运行就是 在电脑上模拟

A60: 可以离线运行，在电脑上模拟。

61. 第六十一个问题：请问强化学习在目前规划中有使用吗？主要应用在什么场景？

A61: 目前还没有使用，我们现在还在探索。大家敬请期待哦！

62. 第六十二个问题：open space planner的规划算法能具体介绍一下吗？

A62: 我没有参与open space planner的具体开发，技术上是以一篇论文为基础，可以联系apollo小姐姐领取。

63. 第六十三个问题：针对前方突发的障碍物（比如虚警或横穿马路的动物），按照技术文档的方式可以规划出避让的路径，但会有较大的离心加速度，实际最优选择似乎应该是紧急制动，就是与障碍物相交碰撞的路径，但是横向路径规划似乎一开始就剔除了这些可能碰撞的区间，不知道我的理解是否有问题

A63: 如果突发障碍物的话，我们会当作动态障碍物处理，在生成路径的时候是不考虑的。所以在这个场景下，我们会在路径上继续直行，在速度上紧急制动。