本文基于Piecewise Jerk Path Optimizer方法进行python实现，Piecewise Jerk Path Optimizer的相关知识点可参考文末的两篇文章，本文主要是进行该算法的问题构造和实现。

Piecewise Jerk Path Optimizer方法的核心思想是使用二次规划的方法寻找最优路径。

上图为qp求解器的输入输出，其中输入包括参考线refline和路径的规划边界boundary。其中refline在NOA模式下和pilot模式下来源有所区别。当系统在NOA模式下时（有高精地图），参考线需要从地图中获取；pilot模式下则从感知识别到的车道线中直接提取。这两种中心线都或多或少存在不平滑或者上下拍不连续的问题。由于目前主流的地图中对地图中心线并未做比较好的路径平滑（有时间写一篇关于中心线的平滑方法），因此规划拿到中心线后需要做平滑处理，然后再输入给路径求解器；boundary则是提取出可行路径，综合考虑车道线、障碍物提取出有用的边界作为qp求解器的约束条件，这里其实是将非凸问题转化为凸问题。

对于二次规划问题，一般的问题构造步骤为：

对于路径规划，我们需要求解的变量是路径点，为了达到舒适稳定的效果，需要优化的变量包括与参考的偏移量、偏移量一阶导、二阶导等（优化变量可自行设定）。因此，x定义如下：

x=\begin{bmatrix} x_0 \\[4pt] x_1\\[4pt]\vdots \\[4pt]x_{n-1}\\[4pt]x'_0\\[4pt]x'_1\\[4pt]\vdots\\[4pt]x'_{n-1}\\[4pt]x''_0\\[4pt]x''_1\\[4pt]\vdots\\[4pt]x''_{n-1} \end{bmatrix}\in R^{3n \times 1}

目前函数设计为所有优化变量与目标偏差的平方和形式，如下所示：

J=w_x\sum^{n-1}_{i=0}(x_i-r_i)^2+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^2+w_{x'''}\sum^{n-2}_{i=0}(x'''_i)^2

其中 x_i 表示侧向偏移量， r_i 为参考偏移量。 x'_i,x''_i,x'''_i 分别表示侧向偏移量对路径 s 的一阶导、二阶导和三阶导。该式表明，我们的目标优化指标是使得与参考线的偏差量、侧向偏移的一阶导、二阶导、三阶导足够小。

约束主要包含四种：偏移量，一阶导，二阶导，三阶导。即规划出的路径上的每个点偏移参考线的约束，不能超过一定的范围；一阶导、二阶导、三阶导则是对侧向速度、加速度的进一步限制。其实这里最好是描述成曲率的形式，因为曲率与运动学、动力学是直接相关的，偏移量的n阶导并不能直观反应到运动学上。

min\{\frac{1}{2}x^TPx+qx\}\\l\le Ax\le u

如上为qp问题的形式，本文使用osqp进行二次规划问题的求解。

结合我们的目标函数和qp问题，我们可以拆解出P和q矩阵。

由 x'''_i\Delta s=x''_{i+1}-x''_i ，得到 x'''_i=\frac{x''_{i+1}-x''_i}{\Delta s} ，将其带入J中，可得

J=w_x\sum^{n-1}_{i=0}[(x_i)^2+(r_i)^2-2x_ir_i]+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^2+\frac{w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}[(x''_{i+1})^2+(x''_i)^2-2x''_{i+1}x''_i]

整理可得（去掉常数项）：

J=w_x\sum^{n-1}_{i=0}[(x_i)^2-2x_ir_i]+w_{x'}\sum^{n-1}_{i=0}(x'_i)^2+(w_{x''}+\frac{2w_{x'''}}{\Delta s^2})\sum^{n-1}_{i=0}(x''_i)^2-\frac{w_{x'''}}{\Delta s^2}((x''_0)^2+(x''_{n-1})^2)-\frac{2w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}(x''_{i+1}x''_i)

由于

x=\begin{bmatrix} x_0 \\[4pt] x_1\\[4pt]\vdots \\[4pt]x_{n-1}\\[4pt]x'_0\\[4pt]x'_1\\[4pt]\vdots\\[4pt]x'_{n-1}\\[4pt]x''_0\\[4pt]x''_1\\[4pt]\vdots\\[4pt]x''_{n-1} \end{bmatrix}\in R^{3n \times 1}

令 J=x^TPx+2qx ，则可构建P和q矩阵（这里比较抽象，需要对照目标函数和qp问题进行确认）：

P_x=\begin{bmatrix} w_x &0& \ldots&0\\0&w_x&\ldots&0\\\vdots&\vdots&\ddots&\vdots\\0&0&\ldots&w_x \end{bmatrix}\in R^{n \times n} ， P_{x'}=\begin{bmatrix} w_{x'} &0& \ldots&0\\0&w_{x'}&\ldots&0\\\vdots&\vdots&\ddots&\vdots\\0&0&\ldots&w_{x'} \end{bmatrix}\in R^{n \times n}

P_{x''}=\begin{bmatrix} w_{x''}+\frac{w_{x'''}}{\Delta s^2} &0& \ldots&0&0\\[8pt]-\frac{2w_{x'''}}{\Delta s^2}&w_{x''}+\frac{2w_{x'''}}{\Delta s^2}&\ldots&0&0\\[8pt]\vdots&\vdots&\ddots&\vdots&\vdots\\[8pt]0&0&\ldots&w_{x''}+\frac{2w_{x'''}}{\Delta s^2}&0\\[8pt]0&0&\ldots&-\frac{2w_{x'''}}{\Delta s^2}&w_{x''}+\frac{w_{x'''}}{\Delta s^2} \end{bmatrix}\in R^{n \times n}

P=\begin{bmatrix} P_x &0&0\\0&P_{x'}&0\\0&0&P_{x''} \end{bmatrix}\in R^{3n \times 3n} ，q=\begin{bmatrix} -r_0\\-r_1\\\vdots\\-r_{n-1}\\0\\\vdots\\0 \end{bmatrix}^T\in R^{1 \times 3n}

不等式形式为 l\le Ax\le u ，我们需要求解A和l、u。

约束有四种：偏移量，一阶导，二阶导，三阶导，未知数有三种：偏移量，一阶导，二阶导。由于三阶导不是未知数，所以通过辛普森公式建立它与未知数间的关系（可以不使用辛普森，如 x'''_i=\frac{x''_{i+1}-x''_i}{\Delta s} ）：

x_{i+1}=x_i+x'_i\Delta s+\frac{1}{2}\Delta s^2x''_i+\frac{1}{6}\Delta s^3x'''_i\\ x'_{i+1}=x'_i+x''_i\Delta s+\frac{1}{2}\Delta s^2x'''_i

将 x'''_i 提取出来，可以得到：

x'''_i\frac{\Delta s^3}{6}=x_{i+1}-x_i-x'_i\Delta s-\frac{1}{2}\Delta s^2x''_i\\x'''_i\frac{\Delta s^2}{2}=x'_{i+1}-x'_i-x''_i\Delta s

此处，对于三阶导。我们构造的其实是一个等式约束，此时 l=u 。

观察矩阵的维度，对号入座进行匹配，可以得到：

A_x=I \in R^{3n\times3n}, A_{ds}=-\Delta sI \in R^{n\times n}, A_{ds^2}=-\frac{1}{2}\Delta s^2I \in R^{n\times n}\\ A_{I}=\begin{bmatrix} -1 &1& 0&\ldots&0 \\0&-1&1&\ldots&0 \\\vdots&\vdots&\ddots&\ddots&\vdots \\0&0&\ldots&-1&1 \\0&0&\ldots&0&-1 \end{bmatrix}\in R^{n \times n} , A=\begin{bmatrix} &A_x\\A_I&A_{ds}&A_{ds^2} \\0&A_I&A_{ds} \end{bmatrix}\in R^{5n \times 3n}

上下边界：

l=\begin{bmatrix} x_{0l}\\[8pt]\vdots \\[8pt]x_{(n-1)l}\\[8pt]x'_{0l}\\[8pt]\vdots\\[8pt]x'_{(n-1)l}\\[8pt]x''_{0l}\\[8pt]\vdots\\[8pt]x''_{(n-1)l}\\[8pt]-\frac{\Delta s^3}{6}x'''_{bound}\\[8pt]\vdots\\[8pt]-\frac{\Delta s^3}{6}x'''_{bound}\\[8pt]-\frac{\Delta s^2}{2}x'''_{bound}\\[8pt]\vdots\\[8pt]-\frac{\Delta s^2}{2}x'''_{bound} \end{bmatrix}\in R^{5n \times 1} , u=\begin{bmatrix} x_{0u}\\[8pt]\vdots \\[8pt]x_{(n-1)u}\\[8pt]x'_{0u}\\[8pt]\vdots\\[8pt]x'_{(n-1)u}\\[8pt]x''_{0u}\\[8pt]\vdots\\[8pt]x''_{(n-1)u}\\[8pt]\frac{\Delta s^3}{6}x'''_{bound}\\[8pt]\vdots\\[8pt]\frac{\Delta s^3}{6}x'''_{bound}\\[8pt]\frac{\Delta s^2}{2}x'''_{bound}\\[8pt]\vdots\\[8pt]\frac{\Delta s^2}{2}x'''_{bound} \end{bmatrix}\in R^{5n \times 1}

以上，我们的qp问题就已构造完毕。

构造好矩阵后，使用Python进行实现（注：需要安装osqp库才能求解）。