自動駕駛決策規劃算法第二章第二節(中) 參考線算法_嗶哩嗶哩_bilibili

動態規劃開辟的凸空間如下，兩條橙色線之間：

黃色的點就意味著L的上下界，物理意義是當軌跡s=s1時，L的范圍應該是(Lmin1,Lmax1)之間，這個范圍就是開辟的凸空間。

規劃的路徑要滿足：

求解L = f(s)使用Apollo3.5中的算法——分段加加速優化算法：

假設在f(s)的三階導數恒為一個常數，那么f(s)四階以上的導數都是0；

?上面的方程組就是二規劃的等式約束；除了對Li有等式約束，還有不等式約束，不等式約束主要就是L不能超過凸空間的范圍，如下：

這樣近似是安全的，大角度時只會把車估計大，而不是估計小，是一個比較安全的。

約束時：

使用上面的約束方法不合適，因為上圖的Lp1已經碰到凸空間的邊界。需要做的保守一些，如下：

具體就是尋找車輛在凸空間上附近(si-d2,si+d1)所有lmaxi都囊括進來進行比較，選擇最小值，記未ubi，使得四個腳點都要小于它，對于lmini也進行近似的處理，找到(si-d2,si+d1)所有lmini中的最小值，是四個角點都小于它。

不等式約束可以寫成如下形式：

由上面可以推出總的不等式約束：

?二次規劃的等式約束和不等式約束都已經寫出來了，接下來就寫其cost function：