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