Frenet坐标系下动态街道场景的最优轨迹生成

0 前言

有两个主要算法已经在实际中使用：
（1）大多数研究组采用插值来解决规划问题，如奥迪、斯坦福最近演示中使用了回旋曲线，贝塞尔以及多项式曲线也被其他研究组使用。主要原因是在结构化环境中增强映射可以提供所需路点，在平滑性、连续性、车辆约束、速度以及舒适性方面提供了保证；
（2）图搜索技术是实际应用中第二个比较常用的算法，尤其是状态格。选择该技术主要原因是保证了舒适性安全性并且满足了车辆约束条件，可以实时快速搜索最优路径。
当前主要挑战是动态环境中的实时运动规划问题，实际中有许多不明意图的其他交通参与者，如何获取它们的状态并且进行准确的预测会减少规划层的压力，并且在有限时间内生成多重动态障碍的自由碰撞轨迹仍然是一个尚未解决的问题。

由于规划阶段是在感知与控制之间的环节，当前的研究在开发新的路径规划算法时既考虑了感知的不确定性又考虑了控制约束。路径规划最新发展旨在正确处理动态环境下数据采集过程中的不确定性，这需要有一个更好的实时环境理解，提高规划能力，以防止感知的不确定所带来的危险情况。从控制的角度来看，必须考虑多种约束从车辆运动学动力学限制到乘客的舒适感。最近的发展旨在规划阶段考虑其中一些限制，从而实现顺利以及可实现的轨迹，减少控制阶段的限制。下一步应该专注于开发能够将感知不确定性与控制约束融合在一起的算法。

并且将驾驶员加入控制回路是汽车自动化研究的新趋势，这带来了新的挑战，即生成安全平稳可实现轨迹时，将感知不确定性、控制约束以及驾驶员知识进行多重融合。

论文解读

论文题目：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

1 摘要

（1）提出了一种semi-reactive的轨迹生成算法，可以很好得集成进行为决策层。Lattice Planner方法通过最优控制策略，结合Frenet坐标系实现速度保持，塞车，跟随以及停车等目标。适用于低速场景以及高速场景的自动驾驶规划算法。
（2）在无约束优化问题的最优解集合中寻找合理的近似来解决受约束的优化问题，并选择最佳解决方案，以满足约束条件
（3）高速横纵向互相解耦；低速则不同

2 Introduction

2.1 Motivation

传统的算法存在一些局限性，难以适用于动态复杂的环境以及高速场景。作者提出的方法：
（1）很好的适应于公路场景，并且能够生成保持速度不变（指变道、并道等操作与行驶速度独立）的驾驶动作以及将速度和距离控制传递给规划层面。
（2）通过结合转向、制动/加速来实现reactive的障碍物避障

2.2 Related work

① [11], [19], [2], [4]: 使用一个连接起点和一个远的可能的目标状态的全局轨迹，但这类方法没有考虑自动驾驶车辆行车过程中各类不确定性，依赖于其他交通参与者的未来行为轨迹的精准预测。

② [16], [1], [7]:使用了离散优化的方法：通过对车辆动力学微分方程的前向积分来获得有限轨迹库(控制空间的离散化)。利用轨迹库，使用cost函数进行评价、打分，作局部路径规划。轨迹的类型可以有很多种形式。但是这种方式可能会非最优。而且这种方式可能会产生超调甚至不稳定性，如下图所示

重规划频率高的轨迹可以避免或减少重规划频率低的轨迹所遇到的超调甚至是不稳定问题。

③ [9]: 通过RRT模拟闭环系统对轨迹树进行采样[10]

④ [17]：与本文类似的思想，但没有考虑障碍物。提到了Galerkin base这个概念

（1）Galerkin base：Galerkin基是一种基函数，可以在函数空间中描述一个系统的动态行为。在优化控制中，使用Galerkin基来表示系统的状态和控制空间，以便找到最优的控制轨迹。简单来说，Galerkin基是一种数学方法，用于将一个系统的动态行为表示为一组基函数的线性组合，从而简化系统的建模和控制。

（2）Bellman最优性原理：动态规划的基本原则之一，如果我们在一个问题中找到了最优决策序列，则这个序列中的每个决策都是这个问题的最优决策，无论之前的决策序列如何。这个原理是动态规划算法的基础，将复杂问题逐步分解为简单问题，并进行逐步求解。

3 最优控制方法

作者使用在轨迹生成中引入最优控制的思想，用以保证轨迹的时间一致性。

通常来说，采用代价函数的方式可能不会遵守Bellman最优性原理，在下一次规划的时候，规划出的轨迹会有不同。那么本文是如何做到的呢？

本文的方法符合Bellman最优性原理，是因为在轨迹生成过程中，将轨迹跟踪问题优化为最优控制问题，并选择一个符合最优性原理的代价函数。
最优性原理：当一个最优解被找到后，它将一直被保留，直到问题的某些方面发生变化。
本文的方法：一旦找到最优解，车辆就会在每个规划步骤中沿着预先计算的轨迹的剩余部分行驶，从而保证了轨迹的时间一致性。

• 在最优控制方面，主要是遵循Bellman的最优化原则去选取cost function，轨迹在最小化cost的基础上也能最大程度的接近车辆理想的开车形式。
• 在解决一般受限制的优化问题时，由于问题不限于某个函数类，因此问题变得非常复杂，最多只能进行数值求解。作者的方法是用无约束优化问题下求得的最优解序列去近似受约束的优化问题。

理想的自动驾驶车辆运动行为：在偏离车道后，能够很好地平衡舒适度（jerk）以及回到期望位置的时间（$t_{end}-t_{start}$）

这意味着一旦找到最佳解决方案是有效的（约束条件不再活跃），non-reactive轨迹的时间一致性就得到保证。

4 在Frenet坐标系下的运动规划

轨迹跟踪控制里比较好的方法是Frenet坐标系方法

• $SE(2):=SO(2)$ X $R^2$特殊欧式子群里有很好的跟踪性能

特殊欧式子群SO(2)是指二维旋转群，它由所有二维旋转矩阵组成，形式为：

特殊欧式子群SE(2)是指二维欧氏群，它由所有二维刚体运动矩阵组成，包括平移和旋转，形式为：

其中$\theta$表示旋转角度，x和y表示平移量。二维欧氏群可以用于描述机器人在平面上的运动。

• Frenet坐标系下的规划问题中，参考线十分重要。参考线可以是道路中心线、或者是非结构化道路下path planning产生的粗轨迹。

基于动态参考系分别生成横纵向上的1维轨迹。相对关系如下所示：


本文采用五次多项式作为轨迹，可以达到最小化jerk的效果，构建了经典的BVP/OBVP问题。

起点：$P_0=[p_0,\dot{p_0},\ddot{p_0}]$，起点时间：$t_0$
终点：$P_0=[\dot{p_1},\ddot{p_1}]$，终点时间：$t_1=$