小鹏轨迹规划方案：Optimal Vehicle Trajectory Planning for Static Obstacle Avoidance using Nonlinear Opt

小鹏轨迹规划方案：Optimal Vehicle Trajectory Planning for Static Obstacle Avoidance using Nonlinear Opt

Optimal Vehicle Trajectory Planning for Static Obstacle Avoidance using Nonlinear Optimization，Yajia Zhang* Hongyi Sun Ruizhi Chai Daike Kang Shan Li Liyun Li。非线性规划解决path plan问题，XP方案。基本就是将path plan问题描述成经典的非线性优化问题，不做任何简化，然后IPOPT求解。

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

文章开头作者强调了两点：

为了保证控制的连续性，我们要求优化问题考虑到比control conmmand至少要高一阶的程度，这里车辆的control commands一般会是加速度和曲率，所以优化问题中的控制量升一阶，变成jerk and curvature change rate.

以前apollo操作path plan基于两点，第一个是frenet , 这个坐标系下算出来东西之后要用的时候还得转回xy，这个转换过程要求frenet坐标系本身的参考系要至少3阶连续，才能保证“to accurately decouple the motion to the second-order derivative level ”，这个要求是很高的。且apollo是对约束做了各种简化表达的QP形式，所以不是对原问题的精确描述，更别说状态转移方程那些都考虑不到。

本文方案，就是非线性优化，不做任何简化。优化问题的基本元素如下：

优化变量：

运动学状态转移函数则采用自行车模型离散积分：

这里的积分都是完整表达，要是想简化点表达也不是不行，比如

这个里面只留下前两项，做个线性化表达也是可以的，如果离散轨迹的点密集，那这个简化不会损失太多精度。

同样，对xy的积分表达其实没有闭解，所以只能采用Gaussian-Legendre quadrature：

文章说采用了十阶，其实如果想简化就选少点：

如果选1，那就是线性化表达了。

目标函数：

这里强调了一个东西，对于到参考线举例，以及到desired speed的difference这两项cost，我们没有直接采用简单的二次罚函数，而是采用了huber loss function:

这个说实话挺合理的，比如作者举的例子是一个变道行为，如果直接二次罚函数惩罚便宜，则会让横向error这个cost瞬间占主导地位从而产生很大的横向运动。原文如下：

“Take the cost term of distance to reference line as an example, when autonomous vehicle initiates a lane change, a natural approach is to switch the reference line from ego lane (the lane where the ego vehicle is in) to the target lane. If a quadratic form of the cost is used, at the beginning of the lane change, this cost factor will become excessively large numerically, which causes other cost factors essentially become ineffective and leads to drastic lateral motions. The same idea applies to the closeness to target speed factor during ego vehicle starts from a static state to avoid drastic longitudinal motions.”

约束：

首先是状态转移，需要按照上面给出的自行车模型下的离散化表达把优化变量之间联系起来，简写成如下：

运动学限制和姿态限制：

作者强调了一个点，这个最后一项，theta attraction在U型弯中有助于收敛：

“The first six constraints are from vehicle’s kinematic limits. The heading difference constraint is not necessary but we find it is helpful on convergence rate in large curvature scenarios such as U-turns as it reduces the volume of the search space.”

一些应用细节

热启动：

热启动可以减少一些迭代次数。这里采用p controller tracking reference line and desired speed生成初始initial guess:

如图，浅蓝色的部分就是tracking reference line 的结果。

光滑化边界

作者发现光滑的边界也有助于收敛，一个直觉上的解释就是光滑的边界可以给到连续变化的梯度信息：

“Our intuitive explanation is that dense resolution of the drivable corridor (and reference line) can reduce projection error from the polyline projection function P , and smoother edge can lead to smoother derivative changes. ”

R tree提升索引速度

因为迭代过程中轨迹在发生变化，与之对应的约束也得进行实时匹配，又因为ipopt里面采用了filtered line search strategy, 这会导致我们需要非常频繁的去匹配约束，所以作者这个整了一个R-tree，帮忙快速索引，虽然多了indexing的消费，不过整体耗时减少了70%。