Apollo 6.0 QP（二次规划）算法解析

这篇文章主要详细地解读一下Apollo 6.0中的piecewise jerk path optimizer这个task。上一篇文章大概阐述了Apollo中的整体框架结构，是分为多场景（scenario）调度，每个scenario又包含一个或者多个阶段（stage），每个stage包含多个具有独立功能的小模块-任务（task）。
task是Apollo中的任务概念，每个阶段都会按照配置参数里的顺序，依次调用每个task，每个task都实现了一个独立的功能。在Apollo 5.0之后，task主要拆分成了两部分——decider（决策器）和optimizer（规划器），而decider和optimizer里又分为path和speed两个维度。对于path和speed两个维度的处理思路大致是一样的，都是先将障碍物映射到SL（path)和ST（speed）空间，进而通过decider决策先计算出变道、借道、靠边停车或是本车道行驶所对应的reference_line和可行驶空间。将这些信息传入进optimizer中，optimizer针对每种情况都生成一条最优path，再由path assessment decider 来挑选出一个最优path， 与随后生成的speed profile进行融合，进而得到trajectory，输出给控制模块。这篇文章主要想讲解的就是其中比较重要的一个task——piecewise jerk path optimizer，其余的task，包括速度规划等功能之后会单独再写文章进行讲解。

一、二次规划的基本形式

二次规划的一般形式如上式所示，第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到优化变量使得代价函数的值最小。许多的优化问题都是以二次优化的形式出现的，因此对二次规划的理解是非常必要的。本文不深入探讨该优化形式的具体内容，有兴趣的读者可以自行查找相关资料。**需要注意的是，二次规划只在代价函数为凸函数的时候能够收敛到最优解，因此这需要P矩阵为半正定矩阵，这是非常重要的一个条件。**这反映在Apollo中的规划算法则为需要进行求解的空间为凸空间，这样二次规划才能收敛到一条最优path。

二、Apollo 中二次规划问题的构造

在Apollo中，该task的代码在/modules/planning/tasks/optimizers/piecewise_jerk_path 下，而其中会调用/modules/planning/math/piecewise_jerk 下的类方法来帮助其完成二次规划问题的构造及求解工作。因此我们着重关注该文件夹下的几个文件，来解析Apollo对于二次规划问题的构思及实现。

可以看到该文件夹下包含三个类，piecewise_jerk_problem为基类，在这个基类中主要实现三个功能——整体最优化问题的框架、构造约束条件、求解二次优化问题。而piecewise_jerk_path_problem和piecewise_jerk_speed_problem则是这个基类的派生类，由此我们可以知道在Apollo中，path优化和speed优化的约束条件其实是一样的，都是在基类中实现的那个约束条件构造函数。而这两个派生类中又分别独自实现了代价函数的构造和一次项的构造这两个功能。下面我们详细来解读一下这三个类算法。

1，首先讲解的是piecewise_jerk_problem这个基类中的Optimize函数，该函数被task中的optimizer直接调用，因此算是一个入口函数。

该函数中会调用FormulateProblem来构造出二次规划问题的框架，再调用osqp库进行求解，从而求出最优path。需要注意的是，二次规划问题的求解方式有许多种，包括拉格朗日法，梯度下降等等，Apollo 采用的osqp这个第三方库个人感觉实际用时求解效率还是比较高的，也比较好用。唯一不太舒服的地方在于其矩阵的构造形式为csc_matrix，该种方法构造矩阵不够直观，比较复杂，后面我会把矩阵用LaTex打出来，让读者可以更直观理解。
在path优化中，优化变量x为frenet坐标系下的l坐标，l一阶导和l二阶导

2，FormulateProblem()

FormulateProblem 这个函数用于构造最优化问题具体矩阵。首先构造出P矩阵即代价函数，然后构造A矩阵即约束矩阵以及上下边界lower_bounds和upper_bounds，最后构建一次项q向量。构造完后将矩阵都存储进OSQPData这个结构体里，以便后续直接调用osqp库进行求解。以下详细解读这些矩阵的具体形式。

三、二次规划的代价函数

CalculateKernel()

CalculateKnernel 用于构造代价函数即P矩阵，由于代码过长就不全部贴出来了。我在初次看这块代码的时候，就被csc_matrix这奇葩的矩阵构造方式折磨了好久，并且我看网上许多讲解Apollo 二次规划的文章也都没有具体到矩阵的实际形式的。我这里把代价函数P矩阵实际解构了出来，我觉得对于第一次接触这块代码的朋友，能够直观的看到这个矩阵还是会对理解Apollo的算法思想有很大帮助的。可以注意到每个元素前都乘以了2，这是为了和二次优化问题的一般形式中的1/2进行抵消的。

而所对应的代价函数，写成数学公式则可表示为：

四、二次规划的限制条件

CalculateAffineConstraint()

该函数是用于构造最优化问题的限制条件，即A矩阵的。需要注意的是，该方法为基类中实现，因此对于path优化和speed优化来说，两者在Apollo 的算法思想里，所收到的约束是一样的。Apollo 该算法的精妙之处就在于，将path和speed分别在SL和ST空间中进行考虑，使得两者的优化思想非常类似，很巧妙地完成两个维度的求解。但与此同时，我感觉这也限制了speed优化，对于动态障碍物的处理就不够完备，后面我会单独再写文章详细讲这一块。
同样，我将A矩阵的具体形式用Latex表现了出来，大家可以对照着Apollo的代码，来实际自己写写看这些矩阵，会对二次规划问题的形式有更深刻的了解。


将这些矩阵表示为数学表达式，则为以下这些限制：

其中对于5，6两个约束需要尤其注意，网上一些文章提到这两个为连续性约束，我感觉讲的不是很清晰。这两个约束其实是保证优化的变量x，x一阶导，x二阶导之间的依赖关系的，具体推导过程有点复杂，我用到了泰勒展开和差分求导能够推导出来，感兴趣的朋友可以自己推导看看。

至此，二次规划的整体框架就介绍完毕了，最终求解得到的优化变量极为frenet坐标系下的path，发给控制模块前，还需转到笛卡尔坐标系下。有一点需要注意，该optimizer会生成多条path，每条path是对应前面decider生成的相应的reference_line和path_boundary，包括了有self_lane, lane_borrow, lane_change, parking这几个标签。对于每个标签，decider会生成对应的reference_line及凸空间。optimizer生成多条path后，再根据一些规则挑选出最优的一条path，用于和speed_profile进行融合。

最近在实际工作中，我也根据项目中的实际上下游接口自己实现了类似的优化算法，后面我想写出自己的思路及实际运行效果来和大家分享，喜欢我文章的朋友欢迎关注我及我的专栏，有任何问题都可以在评论区中提出来，我所知道的都会进行解答~~~ 文章中的任何问题也都可以提出，大家一起探讨，希望大家一起学习，一起进步