apollo 代碼代碼文檔,轉載從github
apollo文檔
侵刪
二次規劃(QP)樣條路徑優化
二次規劃(QP)+樣條插值
1. 目標函數
1.1 獲得路徑長度
路徑定義在station-lateral座標系中。s的變化區間爲從車輛當前位置點到默認路徑的長度。
1.2 獲得樣條段
將路徑劃分爲n段,每段路徑用一個多項式來表示。
1.3 定義樣條段函數
每個樣條段 i 都有沿着參考線的累加距離di。每段的路徑默認用5介多項式表示。
l=fi(s)=ai0+ai1⋅s+ai2⋅s2+ai3⋅s3+ai4⋅s4+ai5⋅s5(0≤s≤di)
1.4 定義每個樣條段優化目標函數
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公式:
minimizes.t.21⋅xT⋅H⋅x+fT⋅xLB≤x≤UBAeqx=beqAx≥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} $
且
fi′(s)2=∣∣ai0ai1ai2ai3ai4ai5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣012s3s24s35s4∣∣∣∣∣∣∣∣∣∣∣∣⋅∣∣012s3s24s35s4∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣
然後得到,
0∫difi′(s)2ds=0∫di∣∣ai0ai1ai2ai3ai4ai5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣012s3s24s35s4∣∣∣∣∣∣∣∣∣∣∣∣⋅∣∣012s3s24s35s4∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣ds
從聚合函數中提取出常量得到,
0∫dif′(s)2ds=∣∣ai0ai1ai2ai3ai4ai5∣∣⋅0∫di∣∣∣∣∣∣∣∣∣∣∣∣012s3s24s35s4∣∣∣∣∣∣∣∣∣∣∣∣⋅∣∣012s3s24s35s4∣∣ds⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣
=∣∣ai0ai1ai2ai3ai4ai5∣∣⋅0∫di∣∣∣∣∣∣∣∣∣∣∣∣000000012s3s24s35s402s4s26s38s410s503s26s39s412s515s604s38s412s516s620s705s410s515s620s725s8∣∣∣∣∣∣∣∣∣∣∣∣ds⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣
最後得到,
0∫difi′(s)2ds=∣∣ai0ai1ai2ai3ai4ai5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣0000000didi2di3di4di50di234di346di458di5610di60di346di459di5612di6715di70di458di5612di6716di7820di80di5610di6715di7820di8925di9∣∣∣∣∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣
請注意我們最後得到一個6介的矩陣來表示5介樣條插值的衍生開銷。
應用同樣的推理方法可以得到2介,3介樣條插值的衍生開銷。
2 約束條件
2.1 初始點約束
假設第一個點爲 (s0, l0), (s0, l0′) and (s0, l0′′),其中l0 , l0′ and l0′′表示橫向的偏移,並且規劃路徑的起始點的第一,第二個點的衍生開銷可以從fi(s), fi′(s), fi(s)′′計算得到。
將上述約束轉換爲QP約束等式,使用等式:
$A_{eq}x = b_{eq} $
下面是轉換的具體步驟:
fi(s0)=∣∣1s0s02s03s04s05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣=l0
且
fi′(s0)=∣∣012s03s024s035s04∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣=l0′
且
fi′′(s0)=∣∣0023×2s04×3s025×4s03∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣=l0′′
其中,i是包含s0的樣條段的索引值。
2.2 終點約束
和起始點相同,終點(se,le) 也應當按照起始點的計算方法生成約束條件。
將起始點和終點組合在一起,得出約束等式爲:
∣∣∣∣∣∣∣∣∣∣∣∣100100s010se10s022s02se22se2s033s023×2s0se33se23×2ses044s034×3s02se44se34×3se2s055s045×4s03se55se45×4se3∣∣∣∣∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣=∣∣∣∣∣∣∣∣∣∣∣∣l0l0′l0′′lele′le′′∣∣∣∣∣∣∣∣∣∣∣∣
2.3 平滑節點約束
該約束的目的是使樣條的節點更加平滑。假設兩個段segk 和segk+1互相連接,且segk的累計值s爲sk。計算約束的等式爲:
fk(sk)=fk+1(s0)
下面是計算的具體步驟:
∣∣1sksk2sk3sk4sk5∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ak0ak1ak2ak3ak4ak5∣∣∣∣∣∣∣∣∣∣∣∣=∣∣1s0s02s03s04s05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+1,5∣∣∣∣∣∣∣∣∣∣∣∣
然後
∣∣1sksk2sk3sk4sk5−1−s0−s02−s03−s04−s05∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣ak0ak1ak2ak3ak4ak5ak+1,0ak+1,1ak+1,2ak+1,3ak+1,4ak+1,5∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣=0
將s0 = 0代入等式。
同樣地,可以爲下述等式計算約束等式:
fk′(sk)=fk+1′(s0)fk′′(sk)=fk+1′′(s0)fk′′′(sk)=fk+1′′′(s0)
2.4 點採樣邊界約束
在路徑上均勻的取樣m個點,檢查這些點上的障礙物邊界。將這些約束轉換爲QP約束不等式,使用不等式:
Ax≥b
首先基於道路寬度和周圍的障礙物找到點 (sj,lj)的下邊界llb,j,且j∈[0,m]。計算約束的不等式爲:
∣∣∣∣∣∣∣∣11...1s0s1...sms02s12...sm2s03s13...sm3s04s14...sm4s05s15...sm5∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣≥∣∣∣∣∣∣∣∣llb,0llb,1...llb,m∣∣∣∣∣∣∣∣
同樣地,對上邊界lub,j,計算約束的不等式爲:
∣∣∣∣∣∣∣∣−1−1...−1−s0−s1...−−sm−s02−s12...−sm2−s03−s13...−sm3−s04−s14...−sm4−s05−s15...−sm5∣∣∣∣∣∣∣∣⋅∣∣∣∣∣∣∣∣∣∣∣∣ai0ai1ai2ai3ai4ai5∣∣∣∣∣∣∣∣∣∣∣∣≥−1⋅∣∣∣∣∣∣∣∣lub,0lub,1...lub,m∣∣∣∣∣∣∣∣