自动驾驶时空联合规划_基于优化方法

自动驾驶时空联合规划:基于优化方法

这是时空联合规划专栏——基于优化方法系列文章，在Frenet坐标系下已经有很多优秀成熟的优化方法解决方案，但是对于在三维空间内不管是在S-L-T空间还是在X-Y-T空间去基于优化的方法做规划都是很困难的。一方面是维度的增加会导致优化变量的增加计算时长必然带来增长；另一方面是三维空间内可行集的凸化会涉及到横纵向的决策可能性爆炸导致非常棘手，不是很好解决.本文会介绍几篇文章，看一看大家对于这个问题提出的一些解决方案。

附赠自动驾驶最全的学习资料和量产经验：链接

第一篇:Trajectory Planning for BERTHA-a Local, Continuous Method

这篇文章根据变分法，提出一种局部连续的方法。非常巧妙的设计了约束和目标函数,确保解收敛到一个唯一的全局最优解。下面就来详细看一下具体的设计细节:

目标函数：

车体的包络圆表示

工作空间凸化：

对待距离约束的时候，需要精心处理障碍物。我们知道对于规划的状态空间，无论是欧式空间还是速度空间，它都是非凸的。对于优化算法来说，除非给出一个最优解的领域解，否则很容易陷入驻点，导致优化结果不行（如下图中(a)）。本文将离散的障碍物先给处理成凸包，然后结合驾驶通道的边缘，将障碍物凸化，使得优化空间成为凸的。

轨迹优化的驻点（a) 无决策过程（b) 有决策过程

障碍物和边界等约束，作为节点对应到图中每个节点，其中两个大的节点表示行车通道的左右边界，小的点理解成障碍物节点。从两个边界节点出发节点与节点之间的连接原则：1. 两个障碍物节点之间在几何上车辆不能通过则连接，2. 障碍物节点和对应的道路边界节点之间可以通过也要连接。采用图论中最小割方法，现在图已经被割成了两个部分，左右通道边界存在两种情况：1. 如上图中（a）最小隔集合是空的，左右边界两个部分本身就是分离的。2. 如上图（b）中，通过移除蓝色的节点把图分割开，最小割集合不是空。前一种情况车辆可通行，后一种情况车辆不可通行，采取停车动作。

对于移动的障碍物，不同时间都存在相对位置的多边形，如下图所示：

根据传感器建立约束多边形

距离函数设计:

文中设计了一个距离函数，引入具有连续可微性质的伪距离。通过对多边形角点的插值，构建伪梯度场，这样就能很快解出到多边形的距离。

伪距离和伪梯度场

重规划：

随着车辆前进，环境变化，根据新环境信息重新规划轨迹，需要保证一定连续性防止硬切换，如下图所。

C2连续重规划

红色表示车辆轨迹，蓝色表示障碍物和约束多边形，黑色表示边界约束

上图(a)显示了快速通过环形路。该方法显示出较好的动力学性能，在环道的入口，中点和出口有最小的横向加速度。上图(b)、©、(d)为通道示意图同一个右转弯，不同位置设置障碍物。蓝色的十字架是体素的位置，累积超过5帧，蓝色的多边形是它们的多边形包络。一个平滑的轨迹能够被发现，即使只有很少的自由空间。上图(e)显示面对较慢的迎面而来的车辆的一个急转向行为。预测的迎面而来的物体的轨迹用深蓝色表示，以及由此产生的淡蓝色障碍多边形。障碍物多边形是时间相关的，圆表示车辆相遇的时刻。