如下圖所示，frenet坐標系下，給定邊界，使用優化的思想求解軌跡
在frenet坐標系下，規划一條在平滑（舒適）、安全（限定空間）的軌跡。
定義代價函數包括平滑性、靠近中心線、終點，在上下邊界內求解使代價函數最小的目標軌跡
Optimality Modeling
We consider the following factors in modeling the optimality of a path:
- Collision-free The path must not intersect with obstacles in the environment.
- Minimal lateral deviation If there is no collision risk, the vehicle should stay as close to the center of the lane as possible.
- Minimal lateral movement The lateral movement and its rate of change must be minimized. These two terms implicitly represent how quickly the vehicle moves laterally.
- (Optional)Maximal obstacle distance Maximize the distance between the vehicle and the obstacles to allow clearance for the vehicle to pass through safely. This factor turns out can be represented as the distance between the vehicle and the center of its corresponding path boundary.
從以下角度出發來規划和評價path：
1 無碰撞
2 最小橫向偏差 - 沿中心線行駛
3 最小橫向位移 - 降低橫向位移和位移頻率
4 （可選）遠離障礙物
從而設計cost:
\[minimize\ f(l(s)) = w_l\cdot \int l(s)^2ds + w_{l'}\cdot \int l'(s)^2ds + w_{l''}\cdot \int l''(s)^2ds + w_{l'''}\cdot \int l'''(s)^2ds +\\ w_{obs} \cdot\int (l(s) - 0.5*(l_B(s)_{min} + l_B(s)_{max}))^2ds + \\ w_{end_l}\cdot (l_{n-1} - l_{endref})^2 + w_{end_{dl}}\cdot ({l}'_{n-1}-{l_{endref}}')^2 + w_{end_{ddl}}\cdot ({l}''_{n-1} - {l_{endref}}'')^2\\ \]
subject to the safety constraint:
\[l(s)\in l_B(s), \forall s\in [0,s_{max}] \]
在cost中：
第一行表示平滑，l,l',l'',l'''最小
第二行表示中心線，盡量沿中心線行駛
第三行表示終點權重
Formulation
To effectively formulate an optimization problem and evaluate constraint satisfaction in practice, our approach discretizes the path function l(s) according to the spatial parameter s to a certain resolution ∆s, and uses these discretized points to control the shape of the path, and approximately evaluate constraint satisfactions. Same ap- proach has been used in our previously published work . The key idea is to discretize the 1-dimensional function to second-order derivative level, and use constant third-order derivative terms to “connect” consecutive discretized points. Third order derivative of a positional variable is commonly named jerk. Thus this formulation is named piecewise-jerk method.
frenet坐標系下，沿固定距離△s將路徑均勻采樣
使用常三階多項式連接各個點（每段曲線的三階導數jerk為常量）
\[\begin{matrix} l_0\\l_0'\\l_0'' \end{matrix} \overset{\Delta s}{\rightarrow} \begin{matrix} l_1\\l_1'\\l_1'' \end{matrix} \overset{\Delta s}{\rightarrow} \begin{matrix} l_2\\l_2'\\l_2'' \end{matrix} ... \begin{matrix} l_{n-2}\\l_{n-2}'\\l_{n-2}'' \end{matrix} \overset{\Delta s}{\rightarrow} \begin{matrix} l_{n-1}\\l_{n-1}'\\l_{n-1}'' \end{matrix} \]
三階導數是常量，那么有:
\[l_{i\rightarrow i+1}''' = \frac{l_{i+1}'' - l_i''}{\Delta s} \]
積分得到：
\[{l_{i+1}}'' = \int_{0}^{\Delta s}{l_{i->i+1}}'''ds={l_i}''+{l_{i->i+1}}'''\cdot\Delta s\\ {l_{i+1}}'={l_i}'+\int_{0}^{\Delta s}{l}''(s)ds={l_i}'+{l_i}''\cdot\Delta s+\frac{1}{2}{l_{i->i+1}}'''\cdot\Delta s^2\\ l_{i+1}=l_i+\int_{0}^{\Delta s}{l}'(s)ds\\=l_i+(l_i)'\cdot\Delta s+\frac{1}{2}{l_i}''\cdot s^2+\frac{1}{6}{l_{i->i+1}}'''\cdot\Delta s^3 \]
qp problem
qp的一般形式
\[minimize \frac{1}{2} \cdot x^T \cdot P \cdot x + q \cdot x \\ s.t. LB \leq A\cdot x \leq UB \]
將cost轉為qp格式
以4個點P1,P2,P3,P4為例
我們要求解的矩陣x為：
\[\begin{vmatrix} l_1\\ l_2\\ l_3\\ l_4\\ l_1'\\ l_2'\\ l_3'\\ l_4'\\ l_1''\\ l_2''\\ l_3''\\ l_4''\\ \end{vmatrix} \]
先看cost中的平滑項：
\[w_l\cdot \sum_{i=0}^{n-1} l_i^2 + w_{{l}'}\cdot \sum_{i=0}^{n-1} {l_i}'^2 + w_{{l}''}\cdot \sum_{i=0}^{n-1} {l_i}''^2 + w_{{l}'''}\cdot \sum_{i=0}^{n-2}(\frac{{l_{i+1}}'' - {l_i}''}{\Delta s})^2 \\ \]
轉為P矩陣
\[\begin{vmatrix} w_l&0&0&0&0&0&0&0&0&0&0&0\\ 0&w_l&0&0&0&0&0&0&0&0&0&0\\ 0&0&w_l&0&0&0&0&0&0&0&0&0\\ 0&0&0&w_l&0&0&0&0&0&0&0&0\\ 0&0&0&0&w_{{l}'}&0&0&0&0&0&0&0\\ 0&0&0&0&0&w_{{l}'}&0&0&0&0&0&0\\ 0&0&0&0&0&0&w_{{l}'}&0&0&0&0&0\\ 0&0&0&0&0&0&0&w_{{l}'}&0&0&0&0\\ 0&0&0&0&0&0&0&0&w_{{l}''}+\frac{w_{{l}'''}}{\Delta s^2}&0&0&0\\ 0&0&0&0&0&0&0&0&-2\frac{w_{{l}'''}}{\Delta s^2}&w_{{l}''}+2\frac{w_{{l}'''}}{\Delta s^2}&0&0\\ 0&0&0&0&0&0&0&0&0&-2\frac{w_{{l}'''}}{\Delta s^2}&w_{{l}''}+2\frac{w_{{l}'''}}{\Delta s^2}&0\\ 0&0&0&0&0&0&0&0&0&0&-2\frac{w_{{l}'''}}{\Delta s^2}&w_{{l}''}+\frac{w_{{l}'''}}{\Delta s^2}\\ \end{vmatrix}\]
ref項的cost：
\[w_{obs}\cdot \sum_{i=0}^{n-1}(l_i-l_{ref})^2 \\ \]
轉為P矩陣：
\[\begin{vmatrix} w_{ref_1}&0&0&0&0&0&0&0&0&0&0&0\\ 0&w_{ref_2}&0&0&0&0&0&0&0&0&0&0\\ 0&0&w_{ref_3}&0&0&0&0&0&0&0&0&0\\ 0&0&0&w_{ref_4}&0&0&0&0&0&0&0&0\\ \end{vmatrix}\]
和q矩陣：
\[\begin{vmatrix} -2w_{ref_1}\cdot l_{ref_1}\\ -2w_{ref_2}\cdot l_{ref_2}\\ -2w_{ref_3}\cdot l_{ref_3}\\ -2w_{ref_4}\cdot l_{ref_4}\\ \end{vmatrix}\]
end_state的cost：
\[w_{end_l}\cdot (l_{n-1} - l_{endref})^2 + w_{end_{dl}}\cdot ({l}'_{n-1}-{l_{endref}}')^2 + w_{end_{ddl}}\cdot ({l}''_{n-1} - {l_{endref}}'')^2 \]
轉為P矩陣：
\[\begin{vmatrix} 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&w_{end_l}&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&w_{end_{dl}}&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&0&0&w_{end_{ddl}} \end{vmatrix} \]
和q矩陣：
\[\begin{vmatrix} 0\\ 0\\ 0\\ -2w_{end_l}\cdot end_l\\ 0\\ 0\\ 0\\ -2w_{end_dl}\cdot end_dl\\ 0\\ 0\\ 0\\ -2w_{end_ddl}\cdot end_ddl\\ \end{vmatrix}\]
上面三個矩陣加在一起，就得到了我們最終需要的P矩陣和q矩陣。
（省略了code中的scale_factor 項
將bound轉為qp格式
bound主要由三部分組成：
- 在path bound decider中獲取的上下邊界
- l' l'' l'''的約束
- 起點約束
- 數學約束，上述的l,l',l'',l'''的數學關系
拆開來看,
1.道路邊界(l)約束：
\[\begin{bmatrix} lb_{s1} \\ lb_{s2} \\ lb_{s3} \\ lb_{s4} \end{bmatrix} \leq \begin{bmatrix} 1&0&0&0&0&0&0&0&0&0&0&0&\\ 0&1&0&0&0&0&0&0&0&0&0&0&\\ 0&0&1&0&0&0&0&0&0&0&0&0&\\ 0&0&0&1&0&0&0&0&0&0&0&0&\\ \end{bmatrix} x \leq \begin{bmatrix} ub_{s1} \\ ub_{s2} \\ ub_{s3} \\ ub_{s4} \end{bmatrix} \]
- tan(heading)(l'，)約束，目前為固定值2.0：
\[ \begin{bmatrix} -2.0 \\ -2.0 \\ -2.0 \\ -2.0 \end{bmatrix} \leq \begin{bmatrix} 0&0&0&0&1&0&0&0&0&0&0&0\\ 0&0&0&0&0&1&0&0&0&0&0&0\\ 0&0&0&0&0&0&1&0&0&0&0&0\\ 0&0&0&0&0&0&0&1&0&0&0&0\\ \end{bmatrix} x \leq \begin{bmatrix} 2.0 \\ 2.0 \\ 2.0 \\ 2.0 \end{bmatrix} \]
3.kappa(l'')約束，道路kappa±自車允許的最大kappa：
\[\begin{bmatrix} -lat\_acc\_bound + kappa_{s1} \\ -lat\_acc\_bound + kappa_{s2} \\ -lat\_acc\_bound + kappa_{s3} \\ -lat\_acc\_bound + kappa_{s4} \end{bmatrix} \leq \begin{bmatrix} 0&0&0&0&0&0&0&0&1&0&0&0\\ 0&0&0&0&0&0&0&0&0&1&0&0\\ 0&0&0&0&0&0&0&0&0&0&1&0\\ 0&0&0&0&0&0&0&0&0&0&0&1\\ \end{bmatrix} x \leq \begin{bmatrix} lat\_acc\_bound + kappa_{s1} \\ lat\_acc\_bound + kappa_{s2} \\ lat\_acc\_bound + kappa_{s3} \\ lat\_acc\_bound + kappa_{s4} \end{bmatrix} \]
- jerk約束：
由上面的jerk表達式：
\[l_{i\rightarrow i+1}''' = (l_{i+1}'' - l_i'') / \Delta s \]
jerk bound的計算沒有看懂，用的是kappa/vehicle_speed?
double PiecewiseJerkPathOptimizer::EstimateJerkBoundary(
const double vehicle_speed, const double axis_distance,
const double max_yaw_rate) const {
return max_yaw_rate / axis_distance / vehicle_speed;
}
轉為矩陣形式：
\[\begin{bmatrix} -jerk_1 * \Delta s \\ -jerk_2 * \Delta s \\ -jerk_3 * \Delta s \end{bmatrix} \leq \begin{bmatrix} 0&0&0&0&0&0&0&0&-1&1&0&0\\ 0&0&0&0&0&0&0&0&0&-1&1&0\\ 0&0&0&0&0&0&0&0&0&0&-1&1\\ \end{bmatrix} x \leq \begin{bmatrix} jerk_1 * \Delta s \\ jerk_2 * \Delta s \\ jerk_3 * \Delta s \end{bmatrix} \]
5.起點約束
\[\begin{bmatrix} ego_l \\ ego_{dl} \\ ego_{ddl} \\ \end{bmatrix} \leq \begin{bmatrix} 1&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&1&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&1&0&0&0\\ \end{bmatrix} x \leq \begin{bmatrix} ego_l \\ ego_{dl} \\ ego_{ddl} \\ \end{bmatrix} \]
- 數學關系 - l'' -> l'約束
對該式
\[{l_{i+1}}'={l_i}'+\int_{0}^{\Delta s}{l}''(s)ds={l_i}'+{l_i}''\cdot\Delta s+\frac{1}{2}{l_{i\rightarrow i+1}}'''\cdot\Delta s^2\\ \]
將jerk式代入，有：
\[l_{i+1}' - l_i' - 0.5\Delta s *l_i'' - 0.5\Delta s *l_{i+1}'' = 0 \]
也就是：
\[\begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \leq \begin{bmatrix} 0&0&0&0&-1&1&0&0&-0.5\cdot\Delta s&-0.5\cdot\Delta s&0&0\\ 0&0&0&0&0&-1&1&0&0&-0.5\cdot\Delta s&-0.5\cdot\Delta s&0\\ 0&0&0&0&0&0&-1&1&0&0&-0.5\cdot\Delta s&-0.5\cdot\Delta s\\ \end{bmatrix} x \leq \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \]
- 數學關系 - l' -> l 約束
和上面的方法一樣，
由
\[l_{i+1}=l_i+\int_{0}^{\Delta s}{l}'(s)ds=l_i+(l_i)'\cdot\Delta s+\frac{1}{2}{l_i}''\cdot s^2+\frac{1}{6}{l_{i\rightarrow i+1}}'''\cdot\Delta s^3 \]
得到：
\[l_{i+1} - l_i - \Delta s \cdot l_i' - \frac{1}{2} \Delta s ^2 \cdot l_i'' - \frac{1}{6} \Delta s^3 \cdot l_{i+1}'' = 0 \]
轉為矩陣：
\[\begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \leq \begin{bmatrix} -1&1&0&0&-\Delta s&0&0&0&-\frac{1}{2}\Delta s^2&-\frac{1}{6}\Delta s^3&0&0\\ 0&-1&1&0&0&-\Delta s&0&0&0&-\frac{1}{2}\Delta s^2&-\frac{1}{6}\Delta s^3&0\\ 0&0&-1&1&0&0&-\Delta s&0&0&0&-\frac{1}{2}\Delta s^2&-\frac{1}{6}\Delta s^3\\ \end{bmatrix} x \leq \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \]
以上就是整個formulate problem的過程，通過以上步驟完成了P,q,A,LB,UB五個矩陣，接下來使用osqp求解器進行求解
Reference
Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform (2020 IEEE Intelligent Vehicles Symposium (IV) October 20-23, 2020. Las Vegas, USA)