轨迹规划问题的数值解法

轨迹规划问题的解法综述

《从描述驾驶体感到建立车辆运动学方程》中，我们提到，轨迹规划问题通常都能转换成一个最优控制（OCP）问题，并给出了问题形式和常见的系统方程。当对该问题进行求解时，有两个部分最为困难：

1. 对碰撞约束的处理，即如何保证安全性，这部分在《自动驾驶怎么保证安全性——碰撞约束处理》中有过详细论述；

2. 处理车辆的运动学约束，保证规划出的轨迹是能够让车辆可跟踪，即可控性。如《从描述驾驶体感到建立车辆运动学方程》中所列举的车辆的运动学方程，存在三角函数，导致其都是非线性的，这是其处理困难的来源；

上述两个都是硬约束，也就是规划的轨迹必须同时满足安全性和可控性。

那工程上是怎么解决轨迹规划这个OCP问题呢？常见有两种方法，下面分别叙述。

一种解法是使用迭代线性二次调节器（iLQR），通过迭代的方式不断逼近最优解，每次迭代中需要线性化系统并求解LQR子问题，将约束以惩罚函数的形式并入评价函数中。对于原问题中非线性部分使用泰勒展开进行简化，具体来说，将系统方程采用一阶泰勒展开，评价函数展开成二阶泰勒形式。其算法原理、步骤和案例可参考《基于cilqr的纵向跟车功能》。

另一种解决轨迹规划问题的方法是直接采用数值方法，即将最优控制问题转化为优化问题，通过离散化微分方程为代数方程，然后利用优化的方法求解。其基本原理在《从约束出发：轨迹规划问题的求解及失败处理》已有阐述，下面就Apollo方案和XP方案详细阐述下工程使用案例。

Apollo轨迹规划

Apollo的规划方案 《Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform》是典型的横纵解耦方案，先进行path规划，然后在path的基础上进行speed规划。在进行path规划时，完全使用了Frenet坐标系，优化问题的状态变量为：，控制变量为，状态方程为

变量之间的跳转关系使用了如下的piecewise-jerk method 

具体表达式为

《Frenet坐标系 or Cartesian坐标系？》文章中比较了Frenet和Cartesian坐标系的优缺点，Apollo使用上述的建模方式，虽然简化了状态方程，但是将非线性部分全都放在约束部分，尤其是曲率的约束上。其处理办法也很简单粗暴，直接进行线性化处理，所以最后构建的优化问题是个典型的二次优化问题（QP），直接使用osqp求解器进行求解。

XP轨迹规划

不同于Apollo的方案，XP轨迹规划方案《optimal vehicle trajectory planning for static obstacle avoidance using nonlinear optimization》是在Cartesian坐标下直接进行xy建模，建立经典的非线性优化模型，其优化变量和模型方程在 《从描述驾驶体感到建立车辆运动学方程》红中已有描述，离散化后的状态方程如下

对xy的积分表达其实没有解析解，文中采用Gaussian-Legendre quadrature近似

最后构建出的优化问题形式为非线性优化问题，使用了IPOPT求解器进行求解。

总结

使用数值方法解决轨迹规划问题时，优化问题的形式与所选坐标系、优化变量和近似方法息息相关，这些都直接决定了问题求解的复杂度和耗时，而耗时是自动驾驶系统中对算法的一个极为重要的指标。工程应用中需要根据场景来适当地平衡精度和复杂性之间的矛盾，在保证精度的情况下尽可能地构建简单的问题。

比较Apollo和XP的轨迹规划方案，前者使用了更简单的坐标系和更多的近似，其问题的求解也更快速更方便，在规则的道路上进行小曲率轨迹规划时，两个算法的差别可能不大，但是在大曲率的轨迹规划时，后者的效果会比前者更好。