劃核心：Piecewise Jerk非線性優(yōu)化器Ⅱ解析)
做自動(dòng)駕駛規(guī)劃模塊的人對“速度規(guī)劃”這四個(gè)字的感情一定很復(fù)雜。路徑規(guī)劃給你一條曲線但車速怎么沿著這條曲線走才是決定乘坐體驗(yàn)和安全的下半場。Apollo 9.0 里的這套 piecewise jerk speed nonlinear optimizer Ⅱ就是速度規(guī)劃環(huán)節(jié)里最硬核的一塊它把速度曲線求解建模成一個(gè)分段加加速度piecewise jerk約束下的非線性規(guī)劃問題。這篇文章不打算從頭講概念直接聊大家最關(guān)心的幾件事——代價(jià)函數(shù)怎么設(shè)計(jì)、約束系統(tǒng)怎么搭、為什么必須用非線性求解器、以及代碼跑起來之后參數(shù)該怎么調(diào)、不收斂時(shí)怎么排查。內(nèi)容適合正在看 Apollo 規(guī)劃源碼的人、想在自己的無人車框架里實(shí)現(xiàn)類似速度規(guī)劃器的工程師以及被“NLP 求解慢/發(fā)散/結(jié)果抖”折磨過的同學(xué)。我會(huì)按我自己優(yōu)化這類問題的思路展開不堆公式標(biāo)題把拆過的模型和踩過的坑盡量說透。1. 速度規(guī)劃到底解決什么問題從 ST 圖到 piecewise jerk 模型1.1 路徑定下來之后速度為什么還要單獨(dú)做規(guī)劃在 Apollo 的規(guī)劃棧里規(guī)劃模塊大致按“感知 → 預(yù)測 → 路由 → 決策 → 規(guī)劃 → 控制”的鏈路工作。規(guī)劃這一步通常又分成兩層先解路徑Path再解速度Speed。路徑?jīng)Q定車往哪條線走速度決定車上每個(gè)時(shí)刻以多快沿著這條線走。兩者不能完全解耦因?yàn)閺澋郎系淖罡哕囁倥c曲率有關(guān)動(dòng)態(tài)障礙物又跟時(shí)間有關(guān)所以速度規(guī)劃一直是單獨(dú)拿出來重點(diǎn)處理的部分。速度規(guī)劃的輸出本質(zhì)上是一條 s-t 曲線s 表示車輛沿參考線走過的縱向弧長t 表示時(shí)間。有了 s(t)就可以求導(dǎo)得到速度 v(t)、加速度 a(t) 和加加速度 j(t)。所有跟“舒適性”“安全性”有關(guān)的約束最后都會(huì)落實(shí)到這條曲線上。1.2 ST 圖把動(dòng)態(tài)問題變成二維幾何問題處理動(dòng)態(tài)障礙物最經(jīng)典的手段就是 ST 圖。橫軸是時(shí)間縱軸是縱向位移。靜態(tài)障礙物在 ST 圖上表現(xiàn)為一段沿 s 方向的平行區(qū)域動(dòng)態(tài)障礙物則表現(xiàn)為一塊斜著的多邊形區(qū)域——前車減速時(shí)它的末端在 ST 圖上會(huì)向上彎對向車道來車看起來更陡。規(guī)劃任務(wù)就是在 ST 圖里找一條從當(dāng)前狀態(tài)出發(fā)、繞過所有障礙物區(qū)域、滿足速度和加速度上下界的三次平滑曲線。Apollo 里早期的 DP 速度規(guī)劃就是先在離散網(wǎng)格上粗搜一條可行走廊再用 QP 平滑。但粗搜和二次規(guī)劃在處理連續(xù)的非線性約束曲率限速、障礙物距離度、舒適度指標(biāo)時(shí)都需要做分段線性近似精度和光滑性都不夠理想。于是后續(xù)迭代里引入了基于 piecewise jerk 的非線性優(yōu)化器把整個(gè)問題直接建成 NLP 來做。1.3 為什么叫“piecewise jerk”簡單說piecewise jerk 模型把速度規(guī)劃的時(shí)間域離散成 N 個(gè)等長小段每一小段上的加加速度jerk被假設(shè)為常數(shù)。這樣一來加速度在每一段內(nèi)是線性變化的速度在每一段內(nèi)是二次變化的位移在每一段內(nèi)是三次變化的。所以每個(gè)小段 s(t) 都是一個(gè)三次多項(xiàng)式。如果你把相鄰若干段連起來看位移不僅能保證連續(xù)還能保證速度、加速度連續(xù)這正是乘員能明顯感知到的舒適性邊界。選擇 jerk 作為控制量而不是加速度是有講究的。如果只對加速度做約束規(guī)劃出來的速度曲線可以保證加減速過程是“平滑”的但加速度突變?nèi)匀粫?huì)讓人感到?jīng)_擊。加上 jerk 約束后加加速度被限制在一個(gè)小范圍內(nèi)車輛起步、剎停的過程才真正柔和下來。這也是 piecewise jerk 類方法在工業(yè)界被廣泛應(yīng)用的原因。1.4 先回顧一下第一版非線性優(yōu)化器做了什么既然是“Ⅱ”前面必然有一篇講基礎(chǔ)模型的。第一部分的重點(diǎn)一般是定義決策變量、寫出運(yùn)動(dòng)學(xué)遞推關(guān)系、搭出優(yōu)化問題的整體骨架。決策變量通常包含每段的位移 s_i、速度 v_i、加速度 a_i以及作為控制輸入的 jerk j_i。相鄰變量之間滿足s_{i1} s_i v_i * Δt 0.5 * a_i * Δt^2 (1/6) * j_i * Δt^3v_{i1} v_i a_i * Δt 0.5 * j_i * Δt^2a_{i1} a_i j_i * Δt這三個(gè)式子就是整個(gè)優(yōu)化問題的核心等式約束后面所有推導(dǎo)都是圍繞它展開。第二部要討論的就是在這個(gè)模型之上如何把現(xiàn)實(shí)場景里的各種目標(biāo)、約束、數(shù)值問題都塞進(jìn)一個(gè)可求解的 NLP 框架里。2. 非線性優(yōu)化器Ⅱ的核心代價(jià)函數(shù)與約束系統(tǒng)的設(shè)計(jì)2.1 代價(jià)函數(shù)不是拍腦袋定的每一項(xiàng)都對應(yīng)一種“不滿”速度規(guī)劃的本質(zhì)是在多個(gè)互相矛盾的目標(biāo)里找平衡既想跑得快又怕剎不住既要貼線又擔(dān)心太顛既想跟前車保持距離又不希望車速忽快忽慢。這些矛盾全部體現(xiàn)在代價(jià)函數(shù)里。一個(gè)典型的 piecewise jerk speed 代價(jià)函數(shù)會(huì)包含以下幾類項(xiàng)對加速度 a^2 的懲罰限制大油門、急剎車懲罰大加速度。對加加速度 j^2 的懲罰限制加速度的突變提升平順性。對參考速度偏差 (v - v_ref)^2 的懲罰讓速度盡可能貼近期望巡航速度或限速避免司機(jī)覺得“這車怎么慢吞吞的”。對橫向加速度的懲罰彎道處要考慮側(cè)向力而橫向加速度約等于 v^2 乘以曲率 κ(s)所以這項(xiàng)本質(zhì)上是把路徑曲率信息耦合進(jìn)了速度規(guī)劃。對障礙物距離的懲罰不能讓車貼著障礙物邊界走懲罰項(xiàng)給安全余量留出空間。末端代價(jià)針對停車場景期望最后一段速度接近 0、加速度接近 0讓車輛能“穩(wěn)穩(wěn)停住”。把這么多項(xiàng)寫進(jìn)目標(biāo)函數(shù)時(shí)每一項(xiàng)前面的權(quán)重系數(shù)決定了它的“話語權(quán)”。權(quán)重差一個(gè)數(shù)量級規(guī)劃出來的速度曲線風(fēng)格就完全不一樣。這就是為什么很多團(tuán)隊(duì)對同一套源碼調(diào)了幾個(gè)月參數(shù)配出來的車感千差萬別。2.2 為什么這個(gè)優(yōu)化問題必須是“非線性”的很多初學(xué)者會(huì)問這些目標(biāo)看起來有平方項(xiàng)約束看起來也不少用二次規(guī)劃QP不是更快更成熟嗎確實(shí)Apollo 早年版本里就有一個(gè) QP-based 的 piecewise jerk speed optimizer。但 QP 只能處理線性約束和二次目標(biāo)在速度規(guī)劃這個(gè)場景里有幾個(gè)東西是天然非線性的曲率限速約束。彎道允許的最大車速與曲率相關(guān)通常寫成 v ≤ sqrt(a_lat_max / |κ(s)|)。這個(gè)約束包含開方不是線性約束。橫向加速度目標(biāo)項(xiàng)。橫向加速度本身是 v^2 與 κ 的乘積是二次非線性的。障礙物距離懲罰。如果直接用分段線性約束來表示 ST 圖中的障礙物上邊界會(huì)讓可行域出現(xiàn)大量折角求解出來的速度曲線也容易出現(xiàn)轉(zhuǎn)折點(diǎn)。更好的做法是用平滑函數(shù)比如指數(shù)、對數(shù)障礙函數(shù)表達(dá)“離障礙物越遠(yuǎn)越安全”這幾乎注定是非線性問題。所以非線性優(yōu)化器Ⅱ的定位就是把這些真實(shí)約束直接用原始公式塞進(jìn)優(yōu)化模型靠 NLP 求解器去找一個(gè)滿足全部條件的局部最優(yōu)解。2.3 約束系統(tǒng)分層等式約束、變量邊界、障礙物約束實(shí)際搭約束的時(shí)候我習(xí)慣把它們分成三層來梳理。第一層是等式約束。運(yùn)動(dòng)學(xué)遞推三式必須嚴(yán)格滿足它們在 NLP 里以殘差形式出現(xiàn)比如把等式改寫成 g_i(x) 0 后交給求解器。第二層是變量邊界。位移 s 有上下界——下界來自不可倒退和動(dòng)態(tài)障礙物下邊界上界來自停車線、靜態(tài)障礙物和跟隨目標(biāo)速度 v 有上下界——下界一般是 0上界來自道路限速與曲率限速的較小者加速度和加加速度的上下界通常由車輛性能和舒適性標(biāo)定決定。第三層是障礙物相關(guān)的約束。對于每個(gè)動(dòng)態(tài)障礙物需要把它的 ST 投影變成隨時(shí)間變化的上邊界和距離邊界。最直接的做法是構(gòu)造一組分段線性邊界約束非線性優(yōu)化器里更傾向用平滑函數(shù)去逼近障礙物的“可通行區(qū)域邊界”然后把這個(gè)平滑函數(shù)引入到目標(biāo)函數(shù)或不等約束中。2.4 末端狀態(tài)約束的細(xì)節(jié)很容易被忽略很多團(tuán)隊(duì)第一次做速度規(guī)劃時(shí)重點(diǎn)都放在中間過程平滑卻忽略了終點(diǎn)狀態(tài)。結(jié)果就是車在接近終點(diǎn)或者停車線時(shí)速度曲線不收斂最后幾米突然沖一下或者剎不住。原因很簡單如果優(yōu)化目標(biāo)里沒有端點(diǎn)的速度和加速度約束那么 N 段軌跡的最后一小段狀態(tài)只是自由變量它可以被目標(biāo)函數(shù)里的其他項(xiàng)“欺負(fù)”得很狼狽。Apollo 這類規(guī)劃器在處理停車場景時(shí)通常會(huì)對末端 s、v、a 設(shè)置硬約束或者很強(qiáng)的末端代價(jià)讓末點(diǎn)速度 v_N 接近 0、加速度 a_N 接近 0。正常巡航場景則會(huì)對末端速度設(shè)置期望值保證與下一幀規(guī)劃結(jié)果銜接連續(xù)。這個(gè)細(xì)節(jié)在復(fù)現(xiàn)算法時(shí)一定要加上否則離線仿真看著沒事一上實(shí)車就是各種“點(diǎn)頭”。3. 求解器選型與數(shù)值實(shí)現(xiàn)從理論到代碼的最后一公里3.1 決策變量與問題規(guī)模小身材里的稀疏性接進(jìn)來一個(gè)實(shí)際的點(diǎn)。假設(shè)時(shí)間窗口 T 取 8 秒離散步長 Δt 取 0.1 秒那么 N 80。每一段有 s_i、v_i、a_i 三個(gè)狀態(tài)變量再加上每一段的控制變量 j_i總共大約有 4 個(gè)變量乘以 80 段也就是 320 個(gè)決策變量。再加上 240 個(gè)左右運(yùn)動(dòng)學(xué)等式約束以及各種上下界約束。這個(gè)規(guī)模在現(xiàn)代 NLP 求解器面前不算大但如果把雅可比矩陣當(dāng)成稠密矩陣來寫計(jì)算量和內(nèi)存消耗都會(huì)成倍上漲。正確的做法是利用稀疏結(jié)構(gòu)大多數(shù)約束只涉及相鄰兩三個(gè)時(shí)間節(jié)點(diǎn)雅可比矩陣呈帶狀分布。用稀疏矩陣接口來裝配 Jacobian 和 Hessian求解效率會(huì)快一個(gè)量級以上。3.2 解析雅可比永遠(yuǎn)是首選能不用差分就不用差分我在實(shí)際調(diào)試中遇到過幾次“解決問題的時(shí)候發(fā)現(xiàn)求解器很慢”排查到最后發(fā)現(xiàn)是代碼里用了有限差分去估算導(dǎo)數(shù)。有限差分在變量規(guī)模小的時(shí)候看不出問題但到了 300 多個(gè)變量時(shí)每次迭代都要多次擾動(dòng)求解浪費(fèi)時(shí)間而且差分步長選不好會(huì)引入數(shù)值噪聲導(dǎo)致優(yōu)化結(jié)果抖來抖去。對這個(gè)模型解析導(dǎo)數(shù)并不難寫。狀態(tài)遞推式對 s_i、v_i、a_i、j_i 的導(dǎo)數(shù)就那么幾個(gè)常數(shù)項(xiàng)代價(jià)函數(shù)對 a 和 j 的求導(dǎo)也很直接唯一麻煩的是橫向加速度項(xiàng)它會(huì)同時(shí)依賴 v_i 和 κ_i寫鏈?zhǔn)椒▌t時(shí)容易漏項(xiàng)。我建議每寫完一個(gè)導(dǎo)數(shù)函數(shù)先做一次數(shù)值差分對拍確認(rèn)誤差在 1e-6 量級再往下走。提示別小看這一步。數(shù)值對照是檢驗(yàn)解析雅可比最快的辦法很多“輕量級 bug”比如下標(biāo)差一、漏乘 Δt幾乎都是靠對拍抓出來的。3.3 熱啟動(dòng)解決非凸問題局部最優(yōu)的關(guān)鍵一招NLP 本質(zhì)上是個(gè)非凸優(yōu)化找到全局最優(yōu)是非常困難的。速度規(guī)劃這類問題更實(shí)際的目標(biāo)是“求一個(gè)可行且足夠好的局部最優(yōu)解”。要讓求解器穩(wěn)定找到這樣一個(gè)解初值非常重要。Apollo 的實(shí)現(xiàn)里通常會(huì)用上一步的規(guī)劃結(jié)果或者一個(gè)粗糙的 DP / QP 解來作為熱啟動(dòng)。熱啟動(dòng)的好處有兩層一是大幅減少迭代次數(shù)讓求解能在 50 毫秒量級內(nèi)完成二是讓優(yōu)化落在上一拍解的附近避免兩幀之間速度曲線發(fā)生跳變。我自己踩過的坑是某些場景下熱啟動(dòng)解已經(jīng)接近約束邊界比如貼著障礙物邊界NLP 從這種初始點(diǎn)出發(fā)很容易被障礙物懲罰項(xiàng)推到另一個(gè)分歧解里。這時(shí)候可以先把障礙物距離懲罰做大一點(diǎn)讓初值“遠(yuǎn)離”約束邊界再逐步恢復(fù)原權(quán)重。3.4 數(shù)值規(guī)范化讓量綱不同的問題不再病態(tài)速度規(guī)劃里同時(shí)存在 s、v、a、j。數(shù)值上 s 動(dòng)輒幾十上百v 十幾二十a(chǎn) 只有幾j 是零點(diǎn)幾到幾。如果直接把這些變量丟給求解器矩陣條件數(shù)很容易變差導(dǎo)致收斂緩慢或者壓根不收斂。我的做法是在進(jìn)入求解器前把每個(gè)變量除以一個(gè)特征尺度比如 s 除以 100v 除以 20a 除以 5j 除以 3。代價(jià)函數(shù)的權(quán)重也跟著重新標(biāo)定。解完之后再乘回尺度系數(shù)。這套歸一化處理整體代碼量很小收益卻非常明顯尤其是轉(zhuǎn)彎場景下橫向加速度和縱向加速度同時(shí)起作用時(shí)。4. 實(shí)操過程從配置到代碼骨架如何快速把優(yōu)化器跑起來4.1 關(guān)鍵參數(shù)與調(diào)參方向速查如果你用的是 Apollo 現(xiàn)成框架通常不希望你直接改二進(jìn)制的權(quán)重值而是通過 planning 配置里的參數(shù)去調(diào)。這里整理一個(gè)我自己常用的參數(shù)調(diào)優(yōu)速查表基本覆蓋了最容易影響速度規(guī)劃效果的幾個(gè)旋鈕參數(shù)方向?qū)?yīng)目標(biāo)項(xiàng)手感表現(xiàn)調(diào)參數(shù)值的影響jerk 權(quán)重調(diào)大加加速度懲罰速度曲線更柔起步/剎停更線性曲線變鈍但可能過分保守加速度權(quán)重調(diào)大加速度懲罰大油門/急剎減少跟車靈敏度下降參考速度權(quán)重調(diào)大貼近限速車速更接近期望速度容易忽略舒適性橫向加速度權(quán)重調(diào)大彎道車速入彎速度更早降下來出彎提速變慢障礙物距離權(quán)重調(diào)大安全距離跟車距離變大通過性變差可行域更大平滑約束靈活度提升更容易出現(xiàn)局部振蕩調(diào)參有一個(gè)順序建議先調(diào)好舒適性相關(guān)權(quán)重jerk、加速度再調(diào)限速相關(guān)權(quán)重參考速度最后調(diào)安全相關(guān)權(quán)重。原因是從目標(biāo)函數(shù)的耦合度來看安全權(quán)重加得太早容易把其他項(xiàng)的問題掩蓋住后面想排查舒適性問題就很麻煩。4.2 一個(gè)最簡 C 偽代碼骨架展示問題裝配過程接觸過 Apollo 這類 C 代碼庫的人看到大段的 Problem 對象、變量數(shù)組和約束矩陣時(shí)容易一頭霧水。其實(shí)把 NLP 的裝配過程抽象出來骨架并不復(fù)雜// 偽代碼示意構(gòu)建 piecewise jerk speed NLP const double dt 0.1; // 離散步長 const int N 80; // 離散段數(shù) // 1. 初始化決策變量 std::vectordouble s(N 1), v(N 1), a(N 1), j(N); // 2. 設(shè)定狀態(tài)初值熱啟動(dòng) s[0] current_s; v[0] current_v; a[0] current_a; // 3. 構(gòu)建代價(jià)函數(shù)項(xiàng) for (int i 0; i N; i) { cost w_ref * (v[i] - v_ref[i]) * (v[i] - v_ref[i]); cost w_acc * a[i] * a[i]; cost w_lat * kappa[i] * v[i] * v[i] * v[i] * v[i]; } for (int i 0; i N; i) { cost w_jerk * j[i] * j[i]; } // 4. 添加運(yùn)動(dòng)學(xué)等式約束寫成殘差形式 for (int i 0; i N; i) { addResidual(s[i1] - (s[i] v[i]*dt 0.5*a[i]*dt*dt j[i]*dt*dt*dt/6.0)); addResidual(v[i1] - (v[i] a[i]*dt 0.5*j[i]*dt*dt)); addResidual(a[i1] - (a[i] j[i]*dt)); } // 5. 添加變量邊界 for (int i 0; i N; i) { addLowerBound(s[i], st_s_min[i]); addUpperBound(s[i], st_s_max[i]); addLowerBound(v[i], 0.0); addUpperBound(v[i], std::min(limit_speed[i], curvature_limit_speed[i])); addLowerBound(a[i], a_min); addUpperBound(a[i], a_max); } // 6. 設(shè)置末端約束 addLowerBound(v[N], 0.0); addUpperBound(v[N], 0.0); // 停車場景示例 // 7. 交給 NLP 求解器求解 solver.solve(cost, constraints);這段代碼雖然只是骨架但已經(jīng)包含了求解一個(gè)速度曲線所需的最小要素。很多團(tuán)隊(duì)在工程落地時(shí)會(huì)再加一層對每個(gè)時(shí)間點(diǎn)把外部傳入的障礙物 ST 約束轉(zhuǎn)換成一個(gè)分段線性函數(shù)存到數(shù)組 st_s_max 里。這一步做實(shí)了整個(gè)問題才真正“接”上了感知和預(yù)測。4.3 一個(gè)典型的場景實(shí)測彎道遇靜止障礙物跑通代碼后我建議用一個(gè)簡單場景來驗(yàn)證整體效果車輛沿彎道行駛前方同一車道內(nèi)有一個(gè)靜止障礙物比如一臺拋錨的貨車。這個(gè)場景能同時(shí)暴露曲率限速、障礙物避讓、末端停車三個(gè)方面的耦合問題。從結(jié)果上你一般會(huì)看到離障礙物還有一段距離時(shí)速度先開始緩慢下降而不是到跟前才急剎。這是加加速度約束在起作用。進(jìn)入彎道之前速度會(huì)比直道限速更低因?yàn)闄M向加速度項(xiàng)的懲罰開始壓過目標(biāo)速度項(xiàng)。在障礙物位置附近s-t 曲線會(huì)留出明顯的安全余量不會(huì)貼著障礙物邊界走。接近停車目標(biāo)點(diǎn)時(shí)末段速度和加速度同時(shí)收斂到 0整條曲線看起來像“自然滑?！薄Ｈ绻麑?shí)測中看到速度曲線在彎道段出現(xiàn)抖動(dòng)多半是橫向加速度權(quán)重和曲率圖分辨率不匹配或者離散步長太大把曲率變化劇烈的地方采樣漏了。可以試試把 Δt 從 0.1 縮到 0.05代價(jià)是段數(shù)翻倍求解時(shí)間會(huì)上升但曲線的順滑度通常能明顯變好。5. 常見問題與排查技巧實(shí)錄5.1 問題一NLP 求解器一直報(bào)不收斂時(shí)間窗口被耗盡這是最常遇到的情況。排查思路按優(yōu)先級排序先看初值是否合理。如果熱啟動(dòng)解本身就在障礙物里面或者速度初值超過限速很多那求解器大概率會(huì)為了找可行解反復(fù)摸索。再看約束是否過緊。把障礙物距離約束松開一點(diǎn)點(diǎn)或者把末端速度約束從硬約束改成大權(quán)重代價(jià)看看求解時(shí)間是否急劇下降。最后檢查導(dǎo)數(shù)是否正確。在模型代碼里寫一個(gè)差分對照的開關(guān)跑一輪小規(guī)模問題如果解析雅可比與差分結(jié)果差異很大基本可以鎖定 bug 在導(dǎo)數(shù)。注意非線性優(yōu)化不收斂時(shí)不要上來就調(diào)求解器參數(shù)。先降問題難度再逐步加約束逐個(gè)排查效率高得多。這個(gè)順序我用了很多次屢試不爽。5.2 問題二速度曲線出現(xiàn)明顯的高頻振蕩這種現(xiàn)象的典型特征是 v-t 圖上有肉眼可見的紋波尤其是起步、停車和通過曲率突變點(diǎn)時(shí)。原因主要有三個(gè)jerk 權(quán)重太小對加加速度的懲罰力度不夠讓速度曲線可以肆意地在相鄰時(shí)間點(diǎn)來回彈跳。解決辦法是調(diào)大 jerk 權(quán)重。約束不光滑障礙物邊界是分段線性表達(dá)時(shí)折角會(huì)引入不光滑導(dǎo)致目標(biāo)函數(shù)的梯度在交界面附近劇烈變化。解決辦法是用平滑函數(shù)近似邊界。初值本身帶振蕩上一步 DP 解太粗直接拿它熱啟動(dòng)NLP 需要大量迭代去“磨平”初值。解決辦法是對初值先做一次輕量級平滑或者增加迭代上限。我給一個(gè)經(jīng)驗(yàn)值方向jerk 權(quán)重的量級通常要比加速度權(quán)重大 2 到 5 倍才能壓得住振蕩趨勢。具體數(shù)值受不同車型、不同整車質(zhì)量影響還是需要結(jié)合實(shí)車/仿真標(biāo)定。5.3 問題三實(shí)時(shí)性不達(dá)標(biāo)求解耗時(shí)超過 100 毫秒Apollo 這類系統(tǒng)里規(guī)劃周期一般只有 100ms 甚至更短留給 NLP 的時(shí)間最多 50 到 60ms。如果超時(shí)常見處理路徑是縮小優(yōu)化時(shí)域。把 8 秒窗口砍到 6 秒或者把 Δt 從 0.05 放大到 0.1段數(shù) N 減半。熱啟動(dòng)做扎實(shí)。同一場景連續(xù)多幀規(guī)劃解之間的差異很小熱啟動(dòng)可以把迭代次數(shù)壓到個(gè)位數(shù)。對難解場景做“多級降級”。先讓快速求解器跑一個(gè)完整解如果時(shí)間預(yù)算不夠就用上一次規(guī)劃結(jié)果結(jié)合局部修正而不是繼續(xù)死磕 NLP。預(yù)處理曲率表、限速表。在進(jìn)入優(yōu)化器之前把 s 相關(guān)的靜態(tài)信息算好避免在目標(biāo)函數(shù)里頻繁查表。實(shí)時(shí)性問題的根源通常在“建模太重”而不是求解器太慢。把不必要的大 N 和過緊的約束放掉往往比升級硬件更直接。5.4 問題四規(guī)劃結(jié)果與控制模塊對不上車實(shí)際表現(xiàn)比曲線“愣”規(guī)劃輸出的是未來若干秒的速度曲線控制模塊去跟蹤時(shí)會(huì)有滯后尤其在急轉(zhuǎn)彎或者強(qiáng)制動(dòng)場景。這時(shí)我看到很多團(tuán)隊(duì)第一反應(yīng)是去調(diào) PID但真正的問題可能出在規(guī)劃末端狀態(tài)不連續(xù)。每一幀規(guī)劃開始時(shí)初始狀態(tài)必須是上一幀規(guī)劃在“當(dāng)前時(shí)刻”的狀態(tài)值而不是直接用定位反饋的即時(shí)值。如果這兩者差得多相當(dāng)于讓優(yōu)化器從全新起點(diǎn)重新規(guī)劃每一幀都會(huì)推出一段不同形狀的曲線控制自然追不上。Apollo 這類系統(tǒng)非常強(qiáng)調(diào)上一個(gè)周期的輸出與下個(gè)周期的輸入對齊查這個(gè)問題時(shí)先看變量銜接處是否有跳變往往能找到真兇。6. 后續(xù)擴(kuò)展與一點(diǎn)個(gè)人體會(huì)非線性速度優(yōu)化器的能力邊界不止于當(dāng)前這套配置。如果你繼續(xù)往深了做會(huì)發(fā)現(xiàn)它和路徑規(guī)劃是強(qiáng)耦合的當(dāng)前方法是把路徑當(dāng)作已知輸入所以橫向加速度項(xiàng)只是“引用”曲率而不是去決策路徑形狀。更進(jìn)一步的方案會(huì)把路徑采樣和速度優(yōu)化放在同一層迭代求解但工程復(fù)雜度會(huì)成倍上漲。我個(gè)人的體會(huì)是piecewise jerk speed nonlinear optimizer 這類算法真正難的不是把數(shù)學(xué)公式寫成代碼而是在真實(shí)感知噪聲、控制偏差、限速變化頻繁切換的場景里保持解的穩(wěn)定性和連續(xù)性。針對這一點(diǎn)持續(xù)維護(hù)一個(gè)場景回歸集特別重要——把單車道跟車、多車博弈、彎道靜止障礙物、連續(xù)紅綠燈等場景錄制成離線測試包每改一次權(quán)重或者約束邏輯就全量回放一遍。這套流程跑順之后你再回去看源代碼里那些權(quán)重參數(shù)就能真正理解每一處數(shù)值背后的行車性格。最后分享一個(gè)小技巧調(diào)試時(shí)優(yōu)先看加加速度曲線而不是速度曲線。加速度和加加速度是對舒適性最敏感的指標(biāo)兩幀之間速度曲線看著差不多但加加速度曲線可能已經(jīng)出現(xiàn)了肉眼可見的毛刺。養(yǎng)成“最終判讀看 a-t 和 j-t 圖”的習(xí)慣能幫你省下大量肉眼比對速度圖的時(shí)間。 Apollo 9.0速度規(guī)劃核心：Piecewise Jerk非線性優(yōu)化器Ⅱ解析
做自動(dòng)駕駛規(guī)劃模塊的人對“速度規(guī)劃”這四個(gè)字的感情一定很復(fù)雜。路徑規(guī)劃給你一條曲線但車速怎么沿著這條曲線走才是決定乘坐體驗(yàn)和安全的下半場。Apollo 9.0 里的這套 piecewise jerk speed nonlinear optimizer Ⅱ就是速度規(guī)劃環(huán)節(jié)里最硬核的一塊它把速度曲線求解建模成一個(gè)分段加加速度piecewise jerk約束下的非線性規(guī)劃問題。這篇文章不打算從頭講概念直接聊大家最關(guān)心的幾件事——代價(jià)函數(shù)怎么設(shè)計(jì)、約束系統(tǒng)怎么搭、為什么必須用非線性求解器、以及代碼跑起來之后參數(shù)該怎么調(diào)、不收斂時(shí)怎么排查。內(nèi)容適合正在看 Apollo 規(guī)劃源碼的人、想在自己的無人車框架里實(shí)現(xiàn)類似速度規(guī)劃器的工程師以及被“NLP 求解慢/發(fā)散/結(jié)果抖”折磨過的同學(xué)。我會(huì)按我自己優(yōu)化這類問題的思路展開不堆公式標(biāo)題把拆過的模型和踩過的坑盡量說透。1. 速度規(guī)劃到底解決什么問題從 ST 圖到 piecewise jerk 模型1.1 路徑定下來之后速度為什么還要單獨(dú)做規(guī)劃在 Apollo 的規(guī)劃棧里規(guī)劃模塊大致按“感知 → 預(yù)測 → 路由 → 決策 → 規(guī)劃 → 控制”的鏈路工作。規(guī)劃這一步通常又分成兩層先解路徑Path再解速度Speed。路徑?jīng)Q定車往哪條線走速度決定車上每個(gè)時(shí)刻以多快沿著這條線走。兩者不能完全解耦因?yàn)閺澋郎系淖罡哕囁倥c曲率有關(guān)動(dòng)態(tài)障礙物又跟時(shí)間有關(guān)所以速度規(guī)劃一直是單獨(dú)拿出來重點(diǎn)處理的部分。速度規(guī)劃的輸出本質(zhì)上是一條 s-t 曲線s 表示車輛沿參考線走過的縱向弧長t 表示時(shí)間。有了 s(t)就可以求導(dǎo)得到速度 v(t)、加速度 a(t) 和加加速度 j(t)。所有跟“舒適性”“安全性”有關(guān)的約束最后都會(huì)落實(shí)到這條曲線上。1.2 ST 圖把動(dòng)態(tài)問題變成二維幾何問題處理動(dòng)態(tài)障礙物最經(jīng)典的手段就是 ST 圖。橫軸是時(shí)間縱軸是縱向位移。靜態(tài)障礙物在 ST 圖上表現(xiàn)為一段沿 s 方向的平行區(qū)域動(dòng)態(tài)障礙物則表現(xiàn)為一塊斜著的多邊形區(qū)域——前車減速時(shí)它的末端在 ST 圖上會(huì)向上彎對向車道來車看起來更陡。規(guī)劃任務(wù)就是在 ST 圖里找一條從當(dāng)前狀態(tài)出發(fā)、繞過所有障礙物區(qū)域、滿足速度和加速度上下界的三次平滑曲線。Apollo 里早期的 DP 速度規(guī)劃就是先在離散網(wǎng)格上粗搜一條可行走廊再用 QP 平滑。但粗搜和二次規(guī)劃在處理連續(xù)的非線性約束曲率限速、障礙物距離度、舒適度指標(biāo)時(shí)都需要做分段線性近似精度和光滑性都不夠理想。于是后續(xù)迭代里引入了基于 piecewise jerk 的非線性優(yōu)化器把整個(gè)問題直接建成 NLP 來做。1.3 為什么叫“piecewise jerk”簡單說piecewise jerk 模型把速度規(guī)劃的時(shí)間域離散成 N 個(gè)等長小段每一小段上的加加速度jerk被假設(shè)為常數(shù)。這樣一來加速度在每一段內(nèi)是線性變化的速度在每一段內(nèi)是二次變化的位移在每一段內(nèi)是三次變化的。所以每個(gè)小段 s(t) 都是一個(gè)三次多項(xiàng)式。如果你把相鄰若干段連起來看位移不僅能保證連續(xù)還能保證速度、加速度連續(xù)這正是乘員能明顯感知到的舒適性邊界。選擇 jerk 作為控制量而不是加速度是有講究的。如果只對加速度做約束規(guī)劃出來的速度曲線可以保證加減速過程是“平滑”的但加速度突變?nèi)匀粫?huì)讓人感到?jīng)_擊。加上 jerk 約束后加加速度被限制在一個(gè)小范圍內(nèi)車輛起步、剎停的過程才真正柔和下來。這也是 piecewise jerk 類方法在工業(yè)界被廣泛應(yīng)用的原因。1.4 先回顧一下第一版非線性優(yōu)化器做了什么既然是“Ⅱ”前面必然有一篇講基礎(chǔ)模型的。第一部分的重點(diǎn)一般是定義決策變量、寫出運(yùn)動(dòng)學(xué)遞推關(guān)系、搭出優(yōu)化問題的整體骨架。決策變量通常包含每段的位移 s_i、速度 v_i、加速度 a_i以及作為控制輸入的 jerk j_i。相鄰變量之間滿足s_{i1} s_i v_i * Δt 0.5 * a_i * Δt^2 (1/6) * j_i * Δt^3v_{i1} v_i a_i * Δt 0.5 * j_i * Δt^2a_{i1} a_i j_i * Δt這三個(gè)式子就是整個(gè)優(yōu)化問題的核心等式約束后面所有推導(dǎo)都是圍繞它展開。第二部要討論的就是在這個(gè)模型之上如何把現(xiàn)實(shí)場景里的各種目標(biāo)、約束、數(shù)值問題都塞進(jìn)一個(gè)可求解的 NLP 框架里。2. 非線性優(yōu)化器Ⅱ的核心代價(jià)函數(shù)與約束系統(tǒng)的設(shè)計(jì)2.1 代價(jià)函數(shù)不是拍腦袋定的每一項(xiàng)都對應(yīng)一種“不滿”速度規(guī)劃的本質(zhì)是在多個(gè)互相矛盾的目標(biāo)里找平衡既想跑得快又怕剎不住既要貼線又擔(dān)心太顛既想跟前車保持距離又不希望車速忽快忽慢。這些矛盾全部體現(xiàn)在代價(jià)函數(shù)里。一個(gè)典型的 piecewise jerk speed 代價(jià)函數(shù)會(huì)包含以下幾類項(xiàng)對加速度 a^2 的懲罰限制大油門、急剎車懲罰大加速度。對加加速度 j^2 的懲罰限制加速度的突變提升平順性。對參考速度偏差 (v - v_ref)^2 的懲罰讓速度盡可能貼近期望巡航速度或限速避免司機(jī)覺得“這車怎么慢吞吞的”。對橫向加速度的懲罰彎道處要考慮側(cè)向力而橫向加速度約等于 v^2 乘以曲率 κ(s)所以這項(xiàng)本質(zhì)上是把路徑曲率信息耦合進(jìn)了速度規(guī)劃。對障礙物距離的懲罰不能讓車貼著障礙物邊界走懲罰項(xiàng)給安全余量留出空間。末端代價(jià)針對停車場景期望最后一段速度接近 0、加速度接近 0讓車輛能“穩(wěn)穩(wěn)停住”。把這么多項(xiàng)寫進(jìn)目標(biāo)函數(shù)時(shí)每一項(xiàng)前面的權(quán)重系數(shù)決定了它的“話語權(quán)”。權(quán)重差一個(gè)數(shù)量級規(guī)劃出來的速度曲線風(fēng)格就完全不一樣。這就是為什么很多團(tuán)隊(duì)對同一套源碼調(diào)了幾個(gè)月參數(shù)配出來的車感千差萬別。2.2 為什么這個(gè)優(yōu)化問題必須是“非線性”的很多初學(xué)者會(huì)問這些目標(biāo)看起來有平方項(xiàng)約束看起來也不少用二次規(guī)劃QP不是更快更成熟嗎確實(shí)Apollo 早年版本里就有一個(gè) QP-based 的 piecewise jerk speed optimizer。但 QP 只能處理線性約束和二次目標(biāo)在速度規(guī)劃這個(gè)場景里有幾個(gè)東西是天然非線性的曲率限速約束。彎道允許的最大車速與曲率相關(guān)通常寫成 v ≤ sqrt(a_lat_max / |κ(s)|)。這個(gè)約束包含開方不是線性約束。橫向加速度目標(biāo)項(xiàng)。橫向加速度本身是 v^2 與 κ 的乘積是二次非線性的。障礙物距離懲罰。如果直接用分段線性約束來表示 ST 圖中的障礙物上邊界會(huì)讓可行域出現(xiàn)大量折角求解出來的速度曲線也容易出現(xiàn)轉(zhuǎn)折點(diǎn)。更好的做法是用平滑函數(shù)比如指數(shù)、對數(shù)障礙函數(shù)表達(dá)“離障礙物越遠(yuǎn)越安全”這幾乎注定是非線性問題。所以非線性優(yōu)化器Ⅱ的定位就是把這些真實(shí)約束直接用原始公式塞進(jìn)優(yōu)化模型靠 NLP 求解器去找一個(gè)滿足全部條件的局部最優(yōu)解。2.3 約束系統(tǒng)分層等式約束、變量邊界、障礙物約束實(shí)際搭約束的時(shí)候我習(xí)慣把它們分成三層來梳理。第一層是等式約束。運(yùn)動(dòng)學(xué)遞推三式必須嚴(yán)格滿足它們在 NLP 里以殘差形式出現(xiàn)比如把等式改寫成 g_i(x) 0 后交給求解器。第二層是變量邊界。位移 s 有上下界——下界來自不可倒退和動(dòng)態(tài)障礙物下邊界上界來自停車線、靜態(tài)障礙物和跟隨目標(biāo)速度 v 有上下界——下界一般是 0上界來自道路限速與曲率限速的較小者加速度和加加速度的上下界通常由車輛性能和舒適性標(biāo)定決定。第三層是障礙物相關(guān)的約束。對于每個(gè)動(dòng)態(tài)障礙物需要把它的 ST 投影變成隨時(shí)間變化的上邊界和距離邊界。最直接的做法是構(gòu)造一組分段線性邊界約束非線性優(yōu)化器里更傾向用平滑函數(shù)去逼近障礙物的“可通行區(qū)域邊界”然后把這個(gè)平滑函數(shù)引入到目標(biāo)函數(shù)或不等約束中。2.4 末端狀態(tài)約束的細(xì)節(jié)很容易被忽略很多團(tuán)隊(duì)第一次做速度規(guī)劃時(shí)重點(diǎn)都放在中間過程平滑卻忽略了終點(diǎn)狀態(tài)。結(jié)果就是車在接近終點(diǎn)或者停車線時(shí)速度曲線不收斂最后幾米突然沖一下或者剎不住。原因很簡單如果優(yōu)化目標(biāo)里沒有端點(diǎn)的速度和加速度約束那么 N 段軌跡的最后一小段狀態(tài)只是自由變量它可以被目標(biāo)函數(shù)里的其他項(xiàng)“欺負(fù)”得很狼狽。Apollo 這類規(guī)劃器在處理停車場景時(shí)通常會(huì)對末端 s、v、a 設(shè)置硬約束或者很強(qiáng)的末端代價(jià)讓末點(diǎn)速度 v_N 接近 0、加速度 a_N 接近 0。正常巡航場景則會(huì)對末端速度設(shè)置期望值保證與下一幀規(guī)劃結(jié)果銜接連續(xù)。這個(gè)細(xì)節(jié)在復(fù)現(xiàn)算法時(shí)一定要加上否則離線仿真看著沒事一上實(shí)車就是各種“點(diǎn)頭”。3. 求解器選型與數(shù)值實(shí)現(xiàn)從理論到代碼的最后一公里3.1 決策變量與問題規(guī)模小身材里的稀疏性接進(jìn)來一個(gè)實(shí)際的點(diǎn)。假設(shè)時(shí)間窗口 T 取 8 秒離散步長 Δt 取 0.1 秒那么 N 80。每一段有 s_i、v_i、a_i 三個(gè)狀態(tài)變量再加上每一段的控制變量 j_i總共大約有 4 個(gè)變量乘以 80 段也就是 320 個(gè)決策變量。再加上 240 個(gè)左右運(yùn)動(dòng)學(xué)等式約束以及各種上下界約束。這個(gè)規(guī)模在現(xiàn)代 NLP 求解器面前不算大但如果把雅可比矩陣當(dāng)成稠密矩陣來寫計(jì)算量和內(nèi)存消耗都會(huì)成倍上漲。正確的做法是利用稀疏結(jié)構(gòu)大多數(shù)約束只涉及相鄰兩三個(gè)時(shí)間節(jié)點(diǎn)雅可比矩陣呈帶狀分布。用稀疏矩陣接口來裝配 Jacobian 和 Hessian求解效率會(huì)快一個(gè)量級以上。3.2 解析雅可比永遠(yuǎn)是首選能不用差分就不用差分我在實(shí)際調(diào)試中遇到過幾次“解決問題的時(shí)候發(fā)現(xiàn)求解器很慢”排查到最后發(fā)現(xiàn)是代碼里用了有限差分去估算導(dǎo)數(shù)。有限差分在變量規(guī)模小的時(shí)候看不出問題但到了 300 多個(gè)變量時(shí)每次迭代都要多次擾動(dòng)求解浪費(fèi)時(shí)間而且差分步長選不好會(huì)引入數(shù)值噪聲導(dǎo)致優(yōu)化結(jié)果抖來抖去。對這個(gè)模型解析導(dǎo)數(shù)并不難寫。狀態(tài)遞推式對 s_i、v_i、a_i、j_i 的導(dǎo)數(shù)就那么幾個(gè)常數(shù)項(xiàng)代價(jià)函數(shù)對 a 和 j 的求導(dǎo)也很直接唯一麻煩的是橫向加速度項(xiàng)它會(huì)同時(shí)依賴 v_i 和 κ_i寫鏈?zhǔn)椒▌t時(shí)容易漏項(xiàng)。我建議每寫完一個(gè)導(dǎo)數(shù)函數(shù)先做一次數(shù)值差分對拍確認(rèn)誤差在 1e-6 量級再往下走。提示別小看這一步。數(shù)值對照是檢驗(yàn)解析雅可比最快的辦法很多“輕量級 bug”比如下標(biāo)差一、漏乘 Δt幾乎都是靠對拍抓出來的。3.3 熱啟動(dòng)解決非凸問題局部最優(yōu)的關(guān)鍵一招NLP 本質(zhì)上是個(gè)非凸優(yōu)化找到全局最優(yōu)是非常困難的。速度規(guī)劃這類問題更實(shí)際的目標(biāo)是“求一個(gè)可行且足夠好的局部最優(yōu)解”。要讓求解器穩(wěn)定找到這樣一個(gè)解初值非常重要。Apollo 的實(shí)現(xiàn)里通常會(huì)用上一步的規(guī)劃結(jié)果或者一個(gè)粗糙的 DP / QP 解來作為熱啟動(dòng)。熱啟動(dòng)的好處有兩層一是大幅減少迭代次數(shù)讓求解能在 50 毫秒量級內(nèi)完成二是讓優(yōu)化落在上一拍解的附近避免兩幀之間速度曲線發(fā)生跳變。我自己踩過的坑是某些場景下熱啟動(dòng)解已經(jīng)接近約束邊界比如貼著障礙物邊界NLP 從這種初始點(diǎn)出發(fā)很容易被障礙物懲罰項(xiàng)推到另一個(gè)分歧解里。這時(shí)候可以先把障礙物距離懲罰做大一點(diǎn)讓初值“遠(yuǎn)離”約束邊界再逐步恢復(fù)原權(quán)重。3.4 數(shù)值規(guī)范化讓量綱不同的問題不再病態(tài)速度規(guī)劃里同時(shí)存在 s、v、a、j。數(shù)值上 s 動(dòng)輒幾十上百v 十幾二十a(chǎn) 只有幾j 是零點(diǎn)幾到幾。如果直接把這些變量丟給求解器矩陣條件數(shù)很容易變差導(dǎo)致收斂緩慢或者壓根不收斂。我的做法是在進(jìn)入求解器前把每個(gè)變量除以一個(gè)特征尺度比如 s 除以 100v 除以 20a 除以 5j 除以 3。代價(jià)函數(shù)的權(quán)重也跟著重新標(biāo)定。解完之后再乘回尺度系數(shù)。這套歸一化處理整體代碼量很小收益卻非常明顯尤其是轉(zhuǎn)彎場景下橫向加速度和縱向加速度同時(shí)起作用時(shí)。4. 實(shí)操過程從配置到代碼骨架如何快速把優(yōu)化器跑起來4.1 關(guān)鍵參數(shù)與調(diào)參方向速查如果你用的是 Apollo 現(xiàn)成框架通常不希望你直接改二進(jìn)制的權(quán)重值而是通過 planning 配置里的參數(shù)去調(diào)。這里整理一個(gè)我自己常用的參數(shù)調(diào)優(yōu)速查表基本覆蓋了最容易影響速度規(guī)劃效果的幾個(gè)旋鈕參數(shù)方向?qū)?yīng)目標(biāo)項(xiàng)手感表現(xiàn)調(diào)參數(shù)值的影響jerk 權(quán)重調(diào)大加加速度懲罰速度曲線更柔起步/剎停更線性曲線變鈍但可能過分保守加速度權(quán)重調(diào)大加速度懲罰大油門/急剎減少跟車靈敏度下降參考速度權(quán)重調(diào)大貼近限速車速更接近期望速度容易忽略舒適性橫向加速度權(quán)重調(diào)大彎道車速入彎速度更早降下來出彎提速變慢障礙物距離權(quán)重調(diào)大安全距離跟車距離變大通過性變差可行域更大平滑約束靈活度提升更容易出現(xiàn)局部振蕩調(diào)參有一個(gè)順序建議先調(diào)好舒適性相關(guān)權(quán)重jerk、加速度再調(diào)限速相關(guān)權(quán)重參考速度最后調(diào)安全相關(guān)權(quán)重。原因是從目標(biāo)函數(shù)的耦合度來看安全權(quán)重加得太早容易把其他項(xiàng)的問題掩蓋住后面想排查舒適性問題就很麻煩。4.2 一個(gè)最簡 C 偽代碼骨架展示問題裝配過程接觸過 Apollo 這類 C 代碼庫的人看到大段的 Problem 對象、變量數(shù)組和約束矩陣時(shí)容易一頭霧水。其實(shí)把 NLP 的裝配過程抽象出來骨架并不復(fù)雜// 偽代碼示意構(gòu)建 piecewise jerk speed NLP const double dt 0.1; // 離散步長 const int N 80; // 離散段數(shù) // 1. 初始化決策變量 std::vectordouble s(N 1), v(N 1), a(N 1), j(N); // 2. 設(shè)定狀態(tài)初值熱啟動(dòng) s[0] current_s; v[0] current_v; a[0] current_a; // 3. 構(gòu)建代價(jià)函數(shù)項(xiàng) for (int i 0; i N; i) { cost w_ref * (v[i] - v_ref[i]) * (v[i] - v_ref[i]); cost w_acc * a[i] * a[i]; cost w_lat * kappa[i] * v[i] * v[i] * v[i] * v[i]; } for (int i 0; i N; i) { cost w_jerk * j[i] * j[i]; } // 4. 添加運(yùn)動(dòng)學(xué)等式約束寫成殘差形式 for (int i 0; i N; i) { addResidual(s[i1] - (s[i] v[i]*dt 0.5*a[i]*dt*dt j[i]*dt*dt*dt/6.0)); addResidual(v[i1] - (v[i] a[i]*dt 0.5*j[i]*dt*dt)); addResidual(a[i1] - (a[i] j[i]*dt)); } // 5. 添加變量邊界 for (int i 0; i N; i) { addLowerBound(s[i], st_s_min[i]); addUpperBound(s[i], st_s_max[i]); addLowerBound(v[i], 0.0); addUpperBound(v[i], std::min(limit_speed[i], curvature_limit_speed[i])); addLowerBound(a[i], a_min); addUpperBound(a[i], a_max); } // 6. 設(shè)置末端約束 addLowerBound(v[N], 0.0); addUpperBound(v[N], 0.0); // 停車場景示例 // 7. 交給 NLP 求解器求解 solver.solve(cost, constraints);這段代碼雖然只是骨架但已經(jīng)包含了求解一個(gè)速度曲線所需的最小要素。很多團(tuán)隊(duì)在工程落地時(shí)會(huì)再加一層對每個(gè)時(shí)間點(diǎn)把外部傳入的障礙物 ST 約束轉(zhuǎn)換成一個(gè)分段線性函數(shù)存到數(shù)組 st_s_max 里。這一步做實(shí)了整個(gè)問題才真正“接”上了感知和預(yù)測。4.3 一個(gè)典型的場景實(shí)測彎道遇靜止障礙物跑通代碼后我建議用一個(gè)簡單場景來驗(yàn)證整體效果車輛沿彎道行駛前方同一車道內(nèi)有一個(gè)靜止障礙物比如一臺拋錨的貨車。這個(gè)場景能同時(shí)暴露曲率限速、障礙物避讓、末端停車三個(gè)方面的耦合問題。從結(jié)果上你一般會(huì)看到離障礙物還有一段距離時(shí)速度先開始緩慢下降而不是到跟前才急剎。這是加加速度約束在起作用。進(jìn)入彎道之前速度會(huì)比直道限速更低因?yàn)闄M向加速度項(xiàng)的懲罰開始壓過目標(biāo)速度項(xiàng)。在障礙物位置附近s-t 曲線會(huì)留出明顯的安全余量不會(huì)貼著障礙物邊界走。接近停車目標(biāo)點(diǎn)時(shí)末段速度和加速度同時(shí)收斂到 0整條曲線看起來像“自然滑?！薄Ｈ绻麑?shí)測中看到速度曲線在彎道段出現(xiàn)抖動(dòng)多半是橫向加速度權(quán)重和曲率圖分辨率不匹配或者離散步長太大把曲率變化劇烈的地方采樣漏了。可以試試把 Δt 從 0.1 縮到 0.05代價(jià)是段數(shù)翻倍求解時(shí)間會(huì)上升但曲線的順滑度通常能明顯變好。5. 常見問題與排查技巧實(shí)錄5.1 問題一NLP 求解器一直報(bào)不收斂時(shí)間窗口被耗盡這是最常遇到的情況。排查思路按優(yōu)先級排序先看初值是否合理。如果熱啟動(dòng)解本身就在障礙物里面或者速度初值超過限速很多那求解器大概率會(huì)為了找可行解反復(fù)摸索。再看約束是否過緊。把障礙物距離約束松開一點(diǎn)點(diǎn)或者把末端速度約束從硬約束改成大權(quán)重代價(jià)看看求解時(shí)間是否急劇下降。最后檢查導(dǎo)數(shù)是否正確。在模型代碼里寫一個(gè)差分對照的開關(guān)跑一輪小規(guī)模問題如果解析雅可比與差分結(jié)果差異很大基本可以鎖定 bug 在導(dǎo)數(shù)。注意非線性優(yōu)化不收斂時(shí)不要上來就調(diào)求解器參數(shù)。先降問題難度再逐步加約束逐個(gè)排查效率高得多。這個(gè)順序我用了很多次屢試不爽。5.2 問題二速度曲線出現(xiàn)明顯的高頻振蕩這種現(xiàn)象的典型特征是 v-t 圖上有肉眼可見的紋波尤其是起步、停車和通過曲率突變點(diǎn)時(shí)。原因主要有三個(gè)jerk 權(quán)重太小對加加速度的懲罰力度不夠讓速度曲線可以肆意地在相鄰時(shí)間點(diǎn)來回彈跳。解決辦法是調(diào)大 jerk 權(quán)重。約束不光滑障礙物邊界是分段線性表達(dá)時(shí)折角會(huì)引入不光滑導(dǎo)致目標(biāo)函數(shù)的梯度在交界面附近劇烈變化。解決辦法是用平滑函數(shù)近似邊界。初值本身帶振蕩上一步 DP 解太粗直接拿它熱啟動(dòng)NLP 需要大量迭代去“磨平”初值。解決辦法是對初值先做一次輕量級平滑或者增加迭代上限。我給一個(gè)經(jīng)驗(yàn)值方向jerk 權(quán)重的量級通常要比加速度權(quán)重大 2 到 5 倍才能壓得住振蕩趨勢。具體數(shù)值受不同車型、不同整車質(zhì)量影響還是需要結(jié)合實(shí)車/仿真標(biāo)定。5.3 問題三實(shí)時(shí)性不達(dá)標(biāo)求解耗時(shí)超過 100 毫秒Apollo 這類系統(tǒng)里規(guī)劃周期一般只有 100ms 甚至更短留給 NLP 的時(shí)間最多 50 到 60ms。如果超時(shí)常見處理路徑是縮小優(yōu)化時(shí)域。把 8 秒窗口砍到 6 秒或者把 Δt 從 0.05 放大到 0.1段數(shù) N 減半。熱啟動(dòng)做扎實(shí)。同一場景連續(xù)多幀規(guī)劃解之間的差異很小熱啟動(dòng)可以把迭代次數(shù)壓到個(gè)位數(shù)。對難解場景做“多級降級”。先讓快速求解器跑一個(gè)完整解如果時(shí)間預(yù)算不夠就用上一次規(guī)劃結(jié)果結(jié)合局部修正而不是繼續(xù)死磕 NLP。預(yù)處理曲率表、限速表。在進(jìn)入優(yōu)化器之前把 s 相關(guān)的靜態(tài)信息算好避免在目標(biāo)函數(shù)里頻繁查表。實(shí)時(shí)性問題的根源通常在“建模太重”而不是求解器太慢。把不必要的大 N 和過緊的約束放掉往往比升級硬件更直接。5.4 問題四規(guī)劃結(jié)果與控制模塊對不上車實(shí)際表現(xiàn)比曲線“愣”規(guī)劃輸出的是未來若干秒的速度曲線控制模塊去跟蹤時(shí)會(huì)有滯后尤其在急轉(zhuǎn)彎或者強(qiáng)制動(dòng)場景。這時(shí)我看到很多團(tuán)隊(duì)第一反應(yīng)是去調(diào) PID但真正的問題可能出在規(guī)劃末端狀態(tài)不連續(xù)。每一幀規(guī)劃開始時(shí)初始狀態(tài)必須是上一幀規(guī)劃在“當(dāng)前時(shí)刻”的狀態(tài)值而不是直接用定位反饋的即時(shí)值。如果這兩者差得多相當(dāng)于讓優(yōu)化器從全新起點(diǎn)重新規(guī)劃每一幀都會(huì)推出一段不同形狀的曲線控制自然追不上。Apollo 這類系統(tǒng)非常強(qiáng)調(diào)上一個(gè)周期的輸出與下個(gè)周期的輸入對齊查這個(gè)問題時(shí)先看變量銜接處是否有跳變往往能找到真兇。6. 后續(xù)擴(kuò)展與一點(diǎn)個(gè)人體會(huì)非線性速度優(yōu)化器的能力邊界不止于當(dāng)前這套配置。如果你繼續(xù)往深了做會(huì)發(fā)現(xiàn)它和路徑規(guī)劃是強(qiáng)耦合的當(dāng)前方法是把路徑當(dāng)作已知輸入所以橫向加速度項(xiàng)只是“引用”曲率而不是去決策路徑形狀。更進(jìn)一步的方案會(huì)把路徑采樣和速度優(yōu)化放在同一層迭代求解但工程復(fù)雜度會(huì)成倍上漲。我個(gè)人的體會(huì)是piecewise jerk speed nonlinear optimizer 這類算法真正難的不是把數(shù)學(xué)公式寫成代碼而是在真實(shí)感知噪聲、控制偏差、限速變化頻繁切換的場景里保持解的穩(wěn)定性和連續(xù)性。針對這一點(diǎn)持續(xù)維護(hù)一個(gè)場景回歸集特別重要——把單車道跟車、多車博弈、彎道靜止障礙物、連續(xù)紅綠燈等場景錄制成離線測試包每改一次權(quán)重或者約束邏輯就全量回放一遍。這套流程跑順之后你再回去看源代碼里那些權(quán)重參數(shù)就能真正理解每一處數(shù)值背后的行車性格。最后分享一個(gè)小技巧調(diào)試時(shí)優(yōu)先看加加速度曲線而不是速度曲線。加速度和加加速度是對舒適性最敏感的指標(biāo)兩幀之間速度曲線看著差不多但加加速度曲線可能已經(jīng)出現(xiàn)了肉眼可見的毛刺。養(yǎng)成“最終判讀看 a-t 和 j-t 圖”的習(xí)慣能幫你省下大量肉眼比對速度圖的時(shí)間。