Apollo的Planning分为参考线平滑、决策、路径规划、速度规划等部分。从整体上来说,规划模块的架构分为两个部分:一部分负责对数据的监听、获取和预处理;另一部分负责管理各个优化模块。数据进入后,对其综合处理为规划模块的内部数据结构,由任务管理器调度合适的优化器进行各个优化任务。综合优化的结果,经过最终的验证后,输出给控制模块。 在设计上,实现了策略的可插拔,使得各个优化器可以灵活配置不同策略,提升迭代效率。 EM-Planner是具体的规划实施类,它基于高精地图、导航路径及障碍物信息作出实际的驾驶决策,包括路径、速度等方面。首先使用DP(动态规划)方法确定初始的路径和速度,再利用QP(二次规划)方法进一步优化路径和速度,以得到一条更平滑的轨迹,既满足舒适性,又方便车辆操纵。 基于样条的车辆轨迹优化二次规划,为了寻求更优质更平滑,体感更好的路径,需要使用二次规划的方法寻找。需要的限制条件有:曲率和曲率连续性、贴近中心线、避免碰撞。 今天,就让阿波君和开发者们一起了解二次规划样条路径是如何实现的。 以下,ENJOY: 路径定义在 station-lateral坐标系中。 参数s的取值范围为车辆的当前位置到默认规划路径的长度。 将路径划分为n段,每段路径用一个多项式来表示。 每个样条段 i 都有沿着参考线的累加距离。每段的路径默认用5阶多项式表示: QP公式如下所示: 下面是将开销(cost)函数转换为QP公式的例子: 且 且 然后得到, 从聚合函数中提取出常量得到, 最后得到, 请注意我们最后得到一个6阶的矩阵来表示5阶样条插值的衍生开销。 应用同样的推理方法可以得到2阶,3阶样条插值的衍生开销。 假设第一个点为, 和。 其中 ,表示横向的偏移,并且规划路径的起始点的第一,第二个点的衍生开销可以从, 计算得到。 将上述约束转换为QP约束等式,使用等式: 下面是转换的具体步骤: 且 且 其中,i 是包含的样条段的索引值。 和起始点相同,终点也应当按照起始点的计算方法生成约束条件。 将起始点和终点组合在一起,得出约束等式为: 该约束的目的是使样条的节点更加平滑。假设两个段和互相连接,且的累计值s为。计算约束的等式为: 下面是计算的具体步骤: 然后, 将代入等式。 同样地,可以为下述等式计算约束等式: 在路径上均匀的取样m个点,检查这些点上的障碍物边界。将这些约束转换为QP约束不等式,使用不等式: 首先基于道路宽度和周围的障碍物找到点的下边界,且。计算约束的不等式为: 同样地,对上边界,计算约束的不等式为:
|
|