apollo 代碼代碼文檔，轉載從github

apollo文檔

侵删

文章目錄

• 二次規劃（QP）樣條路徑優化
• • 1. 目标函數
  • • 1.1 獲得路徑長度
    • 1.2 獲得樣條段
    • 1.3 定義樣條段函數
    • 1.4 定義每個樣條段優化目标函數
    • 1.5 将開銷（cost）函數轉換為QP公式
  • 2 限制條件
  • • 2.1 初始點限制
    • 2.2 終點限制
    • 2.3 平滑節點限制
    • 2.4 點采樣邊界限制

二次規劃（QP）樣條路徑優化

二次規劃（QP）+樣條插值

1. 目标函數

1.1 獲得路徑長度

路徑定義在station-lateral坐标系中。s的變化區間為從車輛目前位置點到預設路徑的長度。

1.2 獲得樣條段

将路徑劃分為n段，每段路徑用一個多項式來表示。

1.3 定義樣條段函數

每個樣條段 i 都有沿着參考線的累加距離 d i d_i di​。每段的路徑預設用5介多項式表示。

l = f i ( s ) = a i 0 + a i 1 ⋅ s + a i 2 ⋅ s 2 + a i 3 ⋅ s 3 + a i 4 ⋅ s 4 + a i 5 ⋅ s 5 ( 0 ≤ s ≤ d i ) l = f_i(s) = a_{i0} + a_{i1} \cdot s + a_{i2} \cdot s^2 + a_{i3} \cdot s^3 + a_{i4} \cdot s^4 + a_{i5} \cdot s^5 (0 \leq s \leq d_{i}) l=fi​(s)=ai0​+ai1​⋅s+ai2​⋅s2+ai3​⋅s3+ai4​⋅s4+ai5​⋅s5(0≤s≤di​)

1.4 定義每個樣條段優化目标函數

c o s t = ∑ i = 1 n ( w 1 ⋅ ∫ 0 d i ( f i ′ ) 2 ( s ) d s + w 2 ⋅ ∫ 0 d i ( f i ′ ′ ) 2 ( s ) d s + w 3 ⋅ ∫ 0 d i ( f i ′ ′ ′ ) 2 ( s ) d s ) cost = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big) cost=∑i=1n​(w1​⋅0∫di​​(fi′​)2(s)ds+w2​⋅0∫di​​(fi′′​)2(s)ds+w3​⋅0∫di​​(fi′′′​)2(s)ds)

1.5 将開銷（cost）函數轉換為QP公式

QP公式:

m i n i m i z e 1 2 ⋅ x T ⋅ H ⋅ x + f T ⋅ x s . t . L B ≤ x ≤ U B A e q x = b e q A x ≥ b \begin{aligned} minimize & \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. \qquad & LB \leq x \leq UB \\ & A_{eq}x = b_{eq} \\ & Ax \geq b \end{aligned} minimizes.t.​21​⋅xT⋅H⋅x+fT⋅xLB≤x≤UBAeq​x=beq​Ax≥b​

下面是将開銷（cost）函數轉換為QP公式的例子：

$f_i(s) ＝

\begin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 \end{vmatrix}

\cdot

\begin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} \end{vmatrix} $

且

$f_i’(s) =

\begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix}

\cdot

\begin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} \end{vmatrix} $

且

f i ′ ( s ) 2 = ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ⋅ ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ ⋅ ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ f_i'(s)^2 = \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} fi′​(s)2=∣∣​ai0​​ai1​​ai2​​ai3​​ai4​​ai5​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​012s3s24s35s4​∣∣∣∣∣∣∣∣∣∣∣∣​⋅∣∣​0​1​2s​3s2​4s3​5s4​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​

然後得到，

∫ 0 d i f i ′ ( s ) 2 d s ＝ ∫ 0 d i ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ⋅ ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ ⋅ ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ d s \int\limits_{0}^{d_i} f_i'(s)^2 ds ＝ \int\limits_{0}^{d_i} \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} ds 0∫di​​fi′​(s)2ds＝0∫di​​∣∣​ai0​​ai1​​ai2​​ai3​​ai4​​ai5​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​012s3s24s35s4​∣∣∣∣∣∣∣∣∣∣∣∣​⋅∣∣​0​1​2s​3s2​4s3​5s4​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​ds

從聚合函數中提取出常量得到，

∫ 0 d i f ′ ( s ) 2 d s ＝ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ⋅ ∫ 0 d i ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ ⋅ ∣ 0 1 2 s 3 s 2 4 s 3 5 s 4 ∣ d s ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ \int\limits_{0}^{d_i} f'(s)^2 ds ＝ \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} 0∫di​​f′(s)2ds＝∣∣​ai0​​ai1​​ai2​​ai3​​ai4​​ai5​​∣∣​⋅0∫di​​∣∣∣∣∣∣∣∣∣∣∣∣​012s3s24s35s4​∣∣∣∣∣∣∣∣∣∣∣∣​⋅∣∣​0​1​2s​3s2​4s3​5s4​∣∣​ds⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​

＝ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ⋅ ∫ 0 d i ∣ 0 0 0 0 0 0 0 1 2 s 3 s 2 4 s 3 5 s 4 0 2 s 4 s 2 6 s 3 8 s 4 10 s 5 0 3 s 2 6 s 3 9 s 4 12 s 5 15 s 6 0 4 s 3 8 s 4 12 s 5 16 s 6 20 s 7 0 5 s 4 10 s 5 15 s 6 20 s 7 25 s 8 ∣ d s ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ＝\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 & 0 &0&0&0&0\\ 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4\\ 0 & 2s & 4s^2 & 6s^3 & 8s^4 & 10s^5\\ 0 & 3s^2 & 6s^3 & 9s^4 & 12s^5&15s^6 \\ 0 & 4s^3 & 8s^4 &12s^5 &16s^6&20s^7 \\ 0 & 5s^4 & 10s^5 & 15s^6 & 20s^7 & 25s^8 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} ＝∣∣​ai0​​ai1​​ai2​​ai3​​ai4​​ai5​​∣∣​⋅0∫di​​∣∣∣∣∣∣∣∣∣∣∣∣​000000​012s3s24s35s4​02s4s26s38s410s5​03s26s39s412s515s6​04s38s412s516s620s7​05s410s515s620s725s8​∣∣∣∣∣∣∣∣∣∣∣∣​ds⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​

最後得到，

∫ 0 d i f i ′ ( s ) 2 d s = ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ⋅ ∣ 0 0 0 0 0 0 0 d i d i 2 d i 3 d i 4 d i 5 0 d i 2 4 3 d i 3 6 4 d i 4 8 5 d i 5 10 6 d i 6 0 d i 3 6 4 d i 4 9 5 d i 5 12 6 d i 6 15 7 d i 7 0 d i 4 8 5 d i 5 12 6 d i 6 16 7 d i 7 20 8 d i 8 0 d i 5 10 6 d i 6 15 7 d i 7 20 8 d i 8 25 9 d i 9 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ \int\limits_{0}^{d_i} f'_i(s)^2 ds =\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 & 0 & 0 & 0 &0&0\\ 0 & d_i & d_i^2 & d_i^3 & d_i^4&d_i^5\\ 0& d_i^2 & \frac{4}{3}d_i^3& \frac{6}{4}d_i^4 & \frac{8}{5}d_i^5&\frac{10}{6}d_i^6\\ 0& d_i^3 & \frac{6}{4}d_i^4 & \frac{9}{5}d_i^5 & \frac{12}{6}d_i^6&\frac{15}{7}d_i^7\\ 0& d_i^4 & \frac{8}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{16}{7}d_i^7&\frac{20}{8}d_i^8\\ 0& d_i^5 & \frac{10}{6}d_i^6 & \frac{15}{7}d_i^7 & \frac{20}{8}d_i^8&\frac{25}{9}d_i^9 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} 0∫di​​fi′​(s)2ds=∣∣​ai0​​ai1​​ai2​​ai3​​ai4​​ai5​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​000000​0di​di2​di3​di4​di5​​0di2​34​di3​46​di4​58​di5​610​di6​​0di3​46​di4​59​di5​612​di6​715​di7​​0di4​58​di5​612​di6​716​di7​820​di8​​0di5​610​di6​715​di7​820​di8​925​di9​​∣∣∣∣∣∣∣∣∣∣∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​

請注意我們最後得到一個6介的矩陣來表示5介樣條插值的衍生開銷。

應用同樣的推理方法可以得到2介，3介樣條插值的衍生開銷。

2 限制條件

2.1 初始點限制

假設第一個點為 ( s 0 s_0 s0​, l 0 l_0 l0​), ( s 0 s_0 s0​, l 0 ′ l'_0 l0′​) and ( s 0 s_0 s0​, l 0 ′ ′ l''_0 l0′′​)，其中 l 0 l_0 l0​ , l 0 ′ l'_0 l0′​ and l 0 ′ ′ l''_0 l0′′​表示橫向的偏移，并且規劃路徑的起始點的第一，第二個點的衍生開銷可以從 f i ( s ) f_i(s) fi​(s), f i ′ ( s ) f'_i(s) fi′​(s), f i ( s ) ′ ′ f_i(s)'' fi​(s)′′計算得到。

将上述限制轉換為QP限制等式，使用等式：

$A_{eq}x = b_{eq} $

下面是轉換的具體步驟：

f i ( s 0 ) = ∣ 1 s 0 s 0 2 s 0 3 s 0 4 s 0 5 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ = l 0 f_i(s_0) = \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5}\end{vmatrix} = l_0 fi​(s0​)=∣∣​1​s0​​s02​​s03​​s04​​s05​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​=l0​

且

f i ′ ( s 0 ) = ∣ 0 1 2 s 0 3 s 0 2 4 s 0 3 5 s 0 4 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ = l 0 ′ f'_i(s_0) = \begin{vmatrix} 0& 1 & 2s_0 & 3s_0^2 & 4s_0^3 &5 s_0^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l'_0 fi′​(s0​)=∣∣​0​1​2s0​​3s02​​4s03​​5s04​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​=l0′​

且

f i ′ ′ ( s 0 ) = ∣ 0 0 2 3 × 2 s 0 4 × 3 s 0 2 5 × 4 s 0 3 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ = l 0 ′ ′ f''_i(s_0) = \begin{vmatrix} 0&0& 2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l''_0 fi′′​(s0​)=∣∣​0​0​2​3×2s0​​4×3s02​​5×4s03​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​=l0′′​

其中，i是包含 s 0 s_0 s0​的樣條段的索引值。

2.2 終點限制

和起始點相同，終點 ( s e , l e ) (s_e, l_e) (se​,le​) 也應當按照起始點的計算方法生成限制條件。

将起始點和終點組合在一起，得出限制等式為：

∣ 1 s 0 s 0 2 s 0 3 s 0 4 s 0 5 0 1 2 s 0 3 s 0 2 4 s 0 3 5 s 0 4 0 0 2 3 × 2 s 0 4 × 3 s 0 2 5 × 4 s 0 3 1 s e s e 2 s e 3 s e 4 s e 5 0 1 2 s e 3 s e 2 4 s e 3 5 s e 4 0 0 2 3 × 2 s e 4 × 3 s e 2 5 × 4 s e 3 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ = ∣ l 0 l 0 ′ l 0 ′ ′ l e l e ′ l e ′ ′ ∣ \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 0&1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4 \\ 0& 0&2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \\ 1 & s_e & s_e^2 & s_e^3 & s_e^4&s_e^5 \\ 0&1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4 \\ 0& 0&2 & 3\times2s_e & 4\times3s_e^2 & 5\times4s_e^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = \begin{vmatrix} l_0\\ l'_0\\ l''_0\\ l_e\\ l'_e\\ l''_e\\ \end{vmatrix} ∣∣∣∣∣∣∣∣∣∣∣∣​100100​s0​10se​10​s02​2s0​2se2​2se​2​s03​3s02​3×2s0​se3​3se2​3×2se​​s04​4s03​4×3s02​se4​4se3​4×3se2​​s05​5s04​5×4s03​se5​5se4​5×4se3​​∣∣∣∣∣∣∣∣∣∣∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​=∣∣∣∣∣∣∣∣∣∣∣∣​l0​l0′​l0′′​le​le′​le′′​​∣∣∣∣∣∣∣∣∣∣∣∣​

2.3 平滑節點限制

該限制的目的是使樣條的節點更加平滑。假設兩個段 s e g k seg_k segk​ 和 s e g k + 1 seg_{k+1} segk+1​互相連接配接，且 s e g k seg_k segk​的累計值s為 s k s_k sk​。計算限制的等式為：

f k ( s k ) = f k + 1 ( s 0 ) f_k(s_k) = f_{k+1} (s_0) fk​(sk​)=fk+1​(s0​)

下面是計算的具體步驟：

∣ 1 s k s k 2 s k 3 s k 4 s k 5 ∣ ⋅ ∣ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 ∣ = ∣ 1 s 0 s 0 2 s 0 3 s 0 4 s 0 5 ∣ ⋅ ∣ a k + 1 , 0 a k + 1 , 1 a k + 1 , 2 a k + 1 , 3 a k + 1 , 4 a k + 1 , 5 ∣ \begin{vmatrix} 1 & s_k & s_k^2 & s_k^ 3 & s_k^ 4 & s_k^ 5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & s_{0} & s_{0}^2 & s_{0}^3 & s_{0}^4&s_{0}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix} ∣∣​1​sk​​sk2​​sk3​​sk4​​sk5​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ak0​ak1​ak2​ak3​ak4​ak5​​∣∣∣∣∣∣∣∣∣∣∣∣​=∣∣​1​s0​​s02​​s03​​s04​​s05​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ak+1,0​ak+1,1​ak+1,2​ak+1,3​ak+1,4​ak+1,5​​∣∣∣∣∣∣∣∣∣∣∣∣​

然後

∣ 1 s k s k 2 s k 3 s k 4 s k 5 − 1 − s 0 − s 0 2 − s 0 3 − s 0 4 − s 0 5 ∣ ⋅ ∣ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 a k + 1 , 0 a k + 1 , 1 a k + 1 , 2 a k + 1 , 3 a k + 1 , 4 a k + 1 , 5 ∣ = 0 \begin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 & -1 & -s_{0} & -s_{0}^2 & -s_{0}^3 & -s_{0}^4&-s_{0}^5\\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \\ a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix} = 0 ∣∣​1​sk​​sk2​​sk3​​sk4​​sk5​​−1​−s0​​−s02​​−s03​​−s04​​−s05​​∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣​ak0​ak1​ak2​ak3​ak4​ak5​ak+1,0​ak+1,1​ak+1,2​ak+1,3​ak+1,4​ak+1,5​​∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣​=0

将 s 0 s_0 s0​ = 0代入等式。

同樣地，可以為下述等式計算限制等式：

f k ′ ( s k ) = f k + 1 ′ ( s 0 ) f k ′ ′ ( s k ) = f k + 1 ′ ′ ( s 0 ) f k ′ ′ ′ ( s k ) = f k + 1 ′ ′ ′ ( s 0 ) f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} (s_0) \\ f'''_k(s_k) = f'''_{k+1} (s_0) fk′​(sk​)=fk+1′​(s0​)fk′′​(sk​)=fk+1′′​(s0​)fk′′′​(sk​)=fk+1′′′​(s0​)

2.4 點采樣邊界限制

在路徑上均勻的取樣m個點，檢查這些點上的障礙物邊界。将這些限制轉換為QP限制不等式，使用不等式：

A x ≥ b Ax \geq b Ax≥b

首先基于道路寬度和周圍的障礙物找到點 ( s j , l j ) (s_j, l_j) (sj​,lj​)的下邊界 l l b , j l_{lb,j} llb,j​，且 j ∈ [ 0 , m ] j\in[0, m] j∈[0,m]。計算限制的不等式為：

∣ 1 s 0 s 0 2 s 0 3 s 0 4 s 0 5 1 s 1 s 1 2 s 1 3 s 1 4 s 1 5 . . . . . . . . . . . . . . . . . . 1 s m s m 2 s m 3 s m 4 s m 5 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ≥ ∣ l l b , 0 l l b , 1 . . . l l b , m ∣ \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 1 & s_1 & s_1^2 & s_1^3 & s_1^4&s_1^5 \\ ...&...&...&...&...&... \\ 1 & s_m & s_m^2 & s_m^3 & s_m^4&s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix}a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix} ∣∣∣∣∣∣∣∣​11...1​s0​s1​...sm​​s02​s12​...sm2​​s03​s13​...sm3​​s04​s14​...sm4​​s05​s15​...sm5​​∣∣∣∣∣∣∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​≥∣∣∣∣∣∣∣∣​llb,0​llb,1​...llb,m​​∣∣∣∣∣∣∣∣​

同樣地，對上邊界 l u b , j l_{ub,j} lub,j​，計算限制的不等式為：

∣ − 1 − s 0 − s 0 2 − s 0 3 − s 0 4 − s 0 5 − 1 − s 1 − s 1 2 − s 1 3 − s 1 4 − s 1 5 . . . . . . − . . . . . . . . . . . . − 1 − s m − s m 2 − s m 3 − s m 4 − s m 5 ∣ ⋅ ∣ a i 0 a i 1 a i 2 a i 3 a i 4 a i 5 ∣ ≥ − 1 ⋅ ∣ l u b , 0 l u b , 1 . . . l u b , m ∣ \begin{vmatrix} -1 & -s_0 & -s_0^2 & -s_0^3 & -s_0^4&-s_0^5 \\ -1 & -s_1 & -s_1^2 & -s_1^3 & -s_1^4&-s_1^5 \\ ...&...-&...&...&...&... \\ -1 & -s_m & -s_m^2 & -s_m^3 & -s_m^4&-s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix} ∣∣∣∣∣∣∣∣​−1−1...−1​−s0​−s1​...−−sm​​−s02​−s12​...−sm2​​−s03​−s13​...−sm3​​−s04​−s14​...−sm4​​−s05​−s15​...−sm5​​∣∣∣∣∣∣∣∣​⋅∣∣∣∣∣∣∣∣∣∣∣∣​ai0​ai1​ai2​ai3​ai4​ai5​​∣∣∣∣∣∣∣∣∣∣∣∣​≥−1⋅∣∣∣∣∣∣∣∣​lub,0​lub,1​...lub,m​​∣∣∣∣∣∣∣∣​