做自動駕駛規(guī)劃這兩年多我一直有個判斷路徑規(guī)劃的問題大多“想得明白但寫起來煩”速度規(guī)劃則恰恰相反——“看起來簡單跑起來全是坑”。尤其在 Apollo 9.0 平臺里planning 模塊中最容易被低估、也最經(jīng)得起折騰的就是那個負責把一條已定路徑變成可執(zhí)行速度曲線的 piecewise jerk speed nonlinear optimizer分段 jerk 速度非線性優(yōu)化器。我為什么單獨把它拎出來寫因為最近這段時間我在做 speed planning 的工程落地碰到的幾個最詭異的線上問題最后全都定位到這個優(yōu)化器身上一是障礙物 ST 約束被硬生生卡死導致頻繁無意義減速二是目標函數(shù)權(quán)重失衡加減速來回震蕩乘客體感非常差三是某些 cut-in 場景下舊版 QP 求解器直接報無解。Apollo 9.0 里帶“非線性”三個字的這個版本就是沖著這些問題來的。這篇文章會把它的建模原理、求解鏈路、調(diào)參手段、以及那些不寫在文檔里的坑一次性講透適合正在讀 Apollo planning 源碼、或者有意自己搭速度規(guī)劃模塊的工程師參考。1. 速度規(guī)劃為什么繞不開“非線性”先看上一代 QP 版本卡在哪1.1 速度規(guī)劃在這條鏈路里的角色先對齊一下基本流程。任何一個規(guī)劃模塊通常都是先解決“往哪走”再解決“怎么走”上游路徑優(yōu)化器給出一條平滑的幾何曲線只包含位置信息速度規(guī)劃的任務(wù)就是在這條曲線上找到一段隨時間變化的關(guān)系即 s(t)、v(t)、a(t)最后拼成帶時間戳的軌跡交給控制模塊。整個過程里ST 圖是核心中間產(chǎn)物——把障礙物投影到以時間為橫軸、縱向距離為縱軸的圖里速度規(guī)劃本質(zhì)上是在這張圖上找一條合理的 s(t) 曲線。這一步的產(chǎn)出對整車體驗的影響往往比路徑規(guī)劃更大。橫向路徑不平滑乘客頂多覺得“有點晃”但縱向速度曲線只要有一點抖動人體對縱向加速度的感知會立刻被放大——前仰后合、推背頓挫全是這里來的。所以速度規(guī)劃一直是規(guī)劃模塊里對數(shù)值精度和實時性要求最苛刻的部分。1.2 上一代 QP 版本三個讓人頭疼的毛病Apollo 在非線性版本之前默認的 speed optimizer 走的是二次規(guī)劃路線時間軸離散成若干段狀態(tài)量是距離、速度、加速度jerk 作為控制量障礙物在 ST 圖上用上下邊界框出來寫成 s_min(t) ≤ s(t) ≤ s_max(t) 的線性約束再配 slack 變量防止硬性無解。這套框架在教科書上無懈可擊但工程上跑起來我總結(jié)出了三個典型問題。第一盒子約束天生保守。ST 圖里的障礙物往往不是一個干凈矩形而是帶軌跡預(yù)測不確定度的區(qū)域。QP 版本為了安全通常取完整包絡(luò)于是“前方一輛車在變道但實際離我挺遠”也會被壓成一個很緊的可行走廊。表現(xiàn)出來就是車提前一兩百米開始減速后車瘋狂按喇叭乘客也不知道你在躲什么。第二slack 和速度跟蹤項打架。為了避免無解每個障礙物約束都配了 slack權(quán)重卻不能設(shè)太大否則約束形同虛設(shè)設(shè)小了一旦約束被頂?shù)竭吔鐑?yōu)化器寧可產(chǎn)生巨大減速度也不愿破約束。我見過速度曲線在一個周期內(nèi)從 72 掉到 35 km/h 再拉回 70 的情況就是 slack 權(quán)重與 v_ref 跟蹤權(quán)重互相拉扯的結(jié)果。第三無解場景處理太粗糙。旁車不打燈直接切到車道這類場景ST 圖上兩個障礙物的可行區(qū)間在時間軸上互相擠壓QP 的可行域直接為空。代碼只能返回失敗整個規(guī)劃幀拿不到新曲線只能沿用上一幀甚至觸發(fā)保守剎停。這在高速場景尤其危險因為安全留給你的反應(yīng)時間本來就不多。1.3 非線性版本到底改變了什么以及“Ⅱ”迭代了什么非線性版本的思路換了個角度把“必須落在障礙物邊界一側(cè)”的硬邏輯變成“離障礙物邊界越近代價越高”的軟懲罰。優(yōu)化器自己權(quán)衡“貼邊加速通過”和“減速讓行”哪個綜合代價更低而不是被布爾條件一票否決。這帶來兩個直接好處可行域幾乎不會為空因為任何時刻都能通過增大懲罰代價找到一條偏保守但存在的解速度曲線也不會因為單個 ST 盒子的抖動而劇烈變化因為懲罰函數(shù)是光滑的。另外非線性問題天然能表達更多耦合關(guān)系——彎道里限速和橫向加速度的乘積關(guān)系、坡道上重力分量對加速度的抵消這些東西寫進 QP 里要么線性化失真要么根本寫不進去。至于標題里的“Ⅱ”以我對 Apollo 9.0 規(guī)劃的跟蹤理解這標記了該優(yōu)化器的第二代實現(xiàn)。第一代更像是把原本的 QP 框架硬塞給 NLP 求解器的過渡品固定時間網(wǎng)格、障礙物懲罰簡單粗暴第二代則真正按 NLP 思路重新設(shè)計了代價與約束結(jié)構(gòu)引入非均勻時間離散、更精細的懲罰函數(shù)和更穩(wěn)健的初始化策略。后面章節(jié)我會把這些差異具體展開。2. piecewise jerk 建模全解從分段常 jerk 假設(shè)到代價函數(shù)推導2.1 為什么是“分段常 jerk”先說一個反直覺的點速度規(guī)劃里真正被優(yōu)化的對象其實不是速度而是 jerk——加速度的變化率。乘客暈不暈車主要看 jerk 是否一直在大幅跳變。工程里常用的舒適性指標是縱向 jerk 不超過 2~4 m/s3超過這個范圍人體很快會產(chǎn)生不適感?！胺侄纬?jerk”就是把控制量簡化成把時間軸切成 N 段每一段內(nèi) jerk 保持恒定。這樣做最大的好處是第 i 段到第 i1 段的狀態(tài)轉(zhuǎn)移可以用閉式公式直接寫出來優(yōu)化變量只有每段的起始狀態(tài)和該段的 jerk 值問題規(guī)?？煽?。標準遞推式是s_{i1} s_i v_i·Δt ?·a_i·Δt2 (1/6)·j_i·Δt3v_{i1} v_i a_i·Δt ?·j_i·Δt2a_{i1} a_i j_i·Δt其中 s 是縱向距離v 是速度a 是加速度j 是 jerkΔt 是這一段的時長。N 個時間段狀態(tài)變量約 3N 個再加 N 個 jerk 控制量整體是 4N 量級的小規(guī)模問題對現(xiàn)代求解器來說負擔并不大。2.2 代價函數(shù)每一項都在表述一種“駕駛性格”優(yōu)化器的性格完全由代價函數(shù)決定。我把它的核心拆成四部分。第一部分是參考速度跟蹤項w_v·Σ(v_i ? v_ref,i)2。v_ref 來自限速、交通流速度、決策模塊給的目標速度。這一項權(quán)重越大車越“急著”貼限速走權(quán)重越小車越“肉”會優(yōu)先照顧其他目標。高速巡航和城市低速跟車場景對這項權(quán)重的需求完全不同。第二部分是加速度懲罰w_a·Σa_i2。它抑制大起大落讓車盡量保持勻速或平緩加減速。注意這里懲罰的是加速度本身所以長時間緩加速和短時間急加速之間優(yōu)化器會傾向于前者。第三部分是 jerk 懲罰w_j·Σj_i2。這是舒適性的直接體現(xiàn)。jerk 權(quán)重高時優(yōu)化器寧可早一點開始減速也不愿意在最后關(guān)頭猛踩一腳。實話說這一項才是速度曲線“柔軟度”的真正來源。第四部分是障礙物軟懲罰也是非線性版本的精髓。對每個時間點 iST 圖給出安全區(qū)間 [s_low,i, s_high,i]然后定義一個門檻懲罰函數(shù)落在安全區(qū)間內(nèi)時懲罰近乎為零一旦逼近甚至越過邊界懲罰迅速上升。一種常見寫法是單側(cè)二次懲罰加緩沖區(qū) Δp(s_i) ?·w_obs·max(0, s_low,i Δ ? s_i)2 ?·w_obs·max(0, s_i ? (s_high,i ? Δ))2這里有個工程細節(jié)必須注意max(0, ·) 會在觸發(fā)點造成一階不光滑梯度在那里跳變求解器容易在邊界附近來回振蕩。穩(wěn)妥做法是把轉(zhuǎn)折處做一個微小圓弧或線性過渡保證函數(shù)一階光滑甚至二階光滑。這一點后面調(diào)參部分還會再提。最后還有一類末端代價比如紅燈場景要求某個時間點后速度歸零或者要求在規(guī)劃末段達到目標速度。我建議能寫成軟約束就寫在代價里比寫死約束更穩(wěn)。硬約束在求解器數(shù)值誤差下容易出現(xiàn)“差一點點不滿足”然后整解被判失敗的情況。2.3 約束體系哪些必須硬哪些可以軟我的原則很簡單物理極限必須硬安全與舒適可以軟。硬約束包括狀態(tài)轉(zhuǎn)移方程本身、速度上下限、加速度上下限由輪胎附著和縱向控制能力決定、jerk 上下限、初始狀態(tài)等式約束——當前位置、速度、加速度必須等于車輛實際值。末端如果明確要求停車比如紅燈或前方靜止障礙物終點速度等于零也要寫成強約束或用足夠大的末端權(quán)重逼近。軟的方面則有兩類。一類是障礙物邊界用軟懲罰代替硬約束前面已反復說明。第二類是舒適性邊界比如“正常行駛加速度不超過 2 m/s2”。我會把它做成懲罰而非硬約束因為緊急避險時突破它是合理的硬約束會直接導致無解。還有一個經(jīng)常被忽略的非線性約束彎道限速的耦合關(guān)系。彎道上速度必須滿足 v_i2 ≤ a_lat_max / κ(s_i)其中 κ 是路徑在 s_i 處的曲率。κ 是 s 的函數(shù)而 s_i 本身是決策變量所以這個約束本質(zhì)上是非線性的。QP 版本只能預(yù)先按路徑位置算好一張限速表當常量用轉(zhuǎn)彎前把速度壓得很死NLP 版本可以在迭代過程中根據(jù)當前 s_i 修正曲率取值限速曲線與真實位置嚴格對應(yīng)。這也是非線性版本在彎道場景體驗更好的原因之一。3. 非線性問題怎么解Apollo 9.0 求解鏈路與工程細節(jié)3.1 求解器選型內(nèi)點法還是序列二次規(guī)劃NLP 問題規(guī)模雖小但選錯求解器照樣會跑不完一個規(guī)劃周期。工程實現(xiàn)里主流是兩條路一條用 IPOPT 這類內(nèi)點法通用求解器一條是自研序列二次規(guī)劃SQP。Apollo 9.0 的規(guī)劃模塊具備多種求解器調(diào)度能力工程上通常以外接 IPOPT 或等價實現(xiàn)為主。內(nèi)點法對“變量少、約束結(jié)構(gòu)稀疏且固定”的問題非常友好每次迭代只需要解析目標函數(shù)值、梯度以及約束雅可比矩陣。但內(nèi)點法有一個大坑對數(shù)值尺度極其敏感。s 是幾十米量級v 是十幾 m/sa 是幾個 m/s2jerk 則可能小到 0.01 量級。如果不做變量縮放Hessian 條件數(shù)會非常難看輕則收斂慢重則根本不收斂。我建議在做這類優(yōu)化器時先把變量歸一化或者在調(diào)用求解器時顯式設(shè)置變量 scale。很多規(guī)劃代碼里 solver 配置那欄有一堆 tolerance 參數(shù)調(diào)來調(diào)去根因往往就是尺度沒處理干凈。梯度也要盡量給解析式。分段常 jerk 的狀態(tài)轉(zhuǎn)移是多項式求導是簡單到不能再簡單的事手寫 Jacobian 完全可行。數(shù)值微分雖然在開發(fā)期省事但在線運行時每次多算幾十次函數(shù)求值耗時會明顯增加而且精度誤差會在 KKT 判定時反咬你一口。3.2 warm start能不能收斂一半看初始猜測NLP 求解的是局部最優(yōu)初始點往哪給結(jié)果就落在哪片山谷里。速度規(guī)劃常見的三種初始化方式一是直接用上一幀的解平移二是先用動態(tài)規(guī)劃DP在 ST 圖上跑一個粗糙解插值到 NLP 的網(wǎng)格上三是用勻速外推。我強烈推薦做 DP warm start。原因很簡單DP 雖粗糙但天然“可行”——它避開了障礙物、滿足基本限速。把這些信息喂給 NLP 后求解器基本只需在可行點附近做打磨把加速度和 jerk 修平滑。直接拿上一幀解做 warm start低速場景還好一旦上一幀是緊急剎車狀態(tài)平移過來的初值會讓本幀解偏向剎車產(chǎn)生“剎一下、停一下、再剎一下”的怪曲線。勻速外推更糟糕障礙物前的初始點完全不滿足約束求解器光是把點拉回可行域就要耗掉大半迭代預(yù)算。這里要特別說一句第二代實現(xiàn)里 warm start 的改進是肉眼可見的它把 DP 結(jié)果按非均勻網(wǎng)格重新采樣近端插值更密遠端稀疏讓求解器在關(guān)鍵決策區(qū)域更快起步。這一點在實際測試中顯著降低了迭代次數(shù)。3.3 收斂判據(jù)與失敗回退一個規(guī)劃幀只有 100 毫秒實時規(guī)劃對耗時零容忍一個 planning cycle 通常只有 100 毫秒級別速度優(yōu)化器必須在這個時間內(nèi)交卷。工程上要設(shè)幾道保險最大迭代次數(shù)要卡死比如 100~200 次超時就停求解器狀態(tài)碼要單獨判斷不是 SUCCESS 一律視為失敗失敗后不能把速度曲線直接丟掉否則控制端瞬間失去目標。我的回退鏈分三級先嘗試沿用當前迭代點因為迭代到后期的點哪怕沒收斂往往也八九不離十不行就回退到上一周期成功的速度曲線再不行才用保守的緊急剎停 profile但這是最低優(yōu)先級選項。實測中真正走到第三步的場景極少大部分失敗在第一步就被接住了。還有一個隱蔽的坑判斷“成功”要看 KKT 條件殘差是否真正滿足而不是只看求解器返回值。我踩過一次求解器每次返回“最優(yōu)”但速度曲線末端每隔幾個周期就抖一下。排查很久才發(fā)現(xiàn)是約束 Jacobian 里某個動態(tài)約束對 s 的偏導寫錯了導致拉格朗日乘子假收斂。這類問題在仿真里基本看不出來實車只要跑到那個速度區(qū)間就會周期性頓挫。強烈建議把 KKT 殘差、約束違例量這些診斷信息全部落盤做長里程數(shù)據(jù)回放后再上線。4. 調(diào)參與排障讓非線性優(yōu)化器在真實道路上跑穩(wěn)4.1 權(quán)重標定從默認值出發(fā)的三步法調(diào)權(quán)重沒有銀彈但我有一套固定的起步流程保證不會從零開始亂試。第一步先固定加速度和 jerk 的相對關(guān)系。讓 w_a 約等于 w_j 的 0.1 到 0.2 倍也就是允許適度加速度但強烈抑制 jerk 突變。這個比例決定了“平順優(yōu)先”的底色。第二步只調(diào)參考速度權(quán)重 w_v觀察高速巡航和跟車距離是否滿足要求。w_v 太小車跟在慢車后面懶得超w_v 太大遇障剎車過晚乘客被嚇到。第三步再調(diào)障礙物軟懲罰的形狀參數(shù)比如懲罰函數(shù)上升的陡峭程度以及從多近開始起作用。我常用的量級參考如下但注意不同實現(xiàn)里 s/v/a 的量綱處理不一樣直接抄數(shù)字沒意義關(guān)鍵看相對比例參數(shù)量級參考影響方向w_v1 ~ 10速度跟蹤緊迫感、跟車距離w_a100 ~ 1000加減速激進程度w_j1000 ~ 10000速度曲線平滑度、舒適性障礙物懲罰斜率視函數(shù)形式貼邊通過 vs 保守避讓舉一個實際例子低速跟車場景我把 w_v 從 5 降到 2同時把障礙物懲罰的起始距離從 1.5 米擴到 3 米車輛在擁堵路段的“點頭”頻率立刻下降。代價是跟車距離稍微拉大容易被加塞但乘客舒適度明顯提升。4.2 時間分辨率dt 的選擇與分段設(shè)計速度規(guī)劃的預(yù)測時域通常取 8~10 秒但到底切多少段、每段多長直接關(guān)系響應(yīng)速度。dt 太大優(yōu)化器對 cut-in 這類突發(fā)障礙反應(yīng)遲鈍因為第一個決策點離當前太遠dt 太小變量個數(shù)膨脹求解時間不可控。工程上我推薦基礎(chǔ)網(wǎng)格用 0.5~1 秒近端前 1~2 秒加密到 0.2~0.4 秒。別小看這一段加密它決定了緊急制動能不能在第一個周期內(nèi)把減速度拉起來。舉個例子8 秒時域全用 0.5 秒網(wǎng)格是 17 段改成近端 0.2 秒 × 1 秒、中段 0.5 秒 × 3 秒、遠端 1.0 秒 × 4 秒總段數(shù) 13 段左右變量反而少了關(guān)鍵區(qū)域的決策精度卻高了。NLP 版本比 QP 版本更愿意接受非均勻網(wǎng)格這個紅利值得吃滿。4.3 真實道路上的四個邊界 case 與排查套路分享四個我在測試中真實撞過的場景每一個都值得記進自己的排查手冊。第一個是綠燈倒計時。這個場景的坑在于決策模塊給了“可通行”語義但 ST 圖里又有匯聚的車流邊界兩次規(guī)劃之間語義切換速度曲線會瞬間從“保持 50”變成“急?！薄Ｅ挪闀r要先看決策層語義是否滯后半拍再看終點速度約束是不是從“自由”突然變成 0。我的處理是給倒計時場景單獨設(shè)置末端速度軟約束用權(quán)重過渡代替硬切換。第二個是旁車 cut-in。非線性軟懲罰大多時候表現(xiàn)驚艷但如果懲罰函數(shù)設(shè)計得太陡旁車一進來代價在邊界附近出現(xiàn)陡峭梯度優(yōu)化器給出的速度剖面會有明顯的“下探”再“回拉”體感就是一腳急剎然后松開。解決思路是限制懲罰函數(shù)的最大斜率這等價于給避讓行為設(shè)置縱向減速度上限。第三個是限速突變比如隧道入口從 80 限到 60。v_ref 如果直接給跳變優(yōu)化器為了滿足速度跟蹤項可能在一個 segment 內(nèi)強行產(chǎn)生巨大負 jerk。根子在上游但優(yōu)化器這邊可以兜底給 v_ref 序列做滑動平均同時把 jerk 硬上限設(shè)到舒適值以下讓它無論如何都突破不了物理極限。第四個是高架上坡。加速度上限必須考慮重力沿坡道的分量平地上能剎住的減速度在坡上要額外抵消一個 g·sinθ。如果不把這項寫進約束優(yōu)化器算出的速度剖面在坡道會“超能力”實車表現(xiàn)為速度比規(guī)劃值越掉越多控制端反復補油門出現(xiàn)追趕振蕩。這個約束是典型的非線性紅利QP 版本很難優(yōu)雅處理。每個 case 的排查我都有固定動作先把 s/v/a/jerk 四個 profile 導出 CSV畫出來看曲線形態(tài)再查約束違例量看哪些約束在圖上最緊的位置被激活最后核對求解器狀態(tài)和迭代軌跡。這套動作走下來80% 的速度異常能在十分鐘內(nèi)定位到是上游還是優(yōu)化器本身的問題。5. 與上下游模塊的配合一次完整的速度規(guī)劃實戰(zhàn)復盤5.1 上游輸入的質(zhì)量決定優(yōu)化器一半的命運很多人遇到 NLP 優(yōu)化結(jié)果抽風時第一反應(yīng)是調(diào)參但我發(fā)現(xiàn)相當比例的“優(yōu)化器抽風”根源在上游。路徑曲率連續(xù)性是最典型的例子如果路徑優(yōu)化器輸出一條 κ 不連續(xù)的曲線那么 v2 ≤ a_lat/κ 這個曲率限速會在拐點處突變速度優(yōu)化器被迫在極短時間內(nèi)從 70 剎到 40這本身就是一個大 jerk。所以排查速度問題時先檢查路徑曲率曲線的連續(xù)性再談其他。參考速度也一樣。決策模塊給的目標速度如果是步進式的比如前方路段限速從 80 在某處突變?yōu)?60喂給優(yōu)化器的 v_ref 就是一個階躍信號。我的習慣是在優(yōu)化器前面加一個速度參考平滑層用一階慣性或滑動窗口把跳變抹平。這比在代價函數(shù)里硬調(diào)權(quán)重省事得多而且對下游控制更友好。5.2 輸出對接控制端的三個細節(jié)速度優(yōu)化器交出的是一組時間軸上的 s/v/a 序列但控制模塊真正需要的是更高采樣率、且三階可導的速度曲線。實際工程要做三件事一是把離散序列插值成高分辨率 profile。這一點在分段常 jerk 假設(shè)下非常省事任意時刻 τ 在第 i 段內(nèi)的位置可以直接用閉式公式算s(τ) s_i v_i·τ ?·a_i·τ2 (1/6)·j_i·τ3不需要重新擬合也不會引入插值誤差。二是裁剪時域到真實可執(zhí)行范圍保證末端速度不為負、且對靜止目標準確歸零。三是和控制周期對齊避免多項式插值在控制 tick 之間引入相位差。我踩過一個很典型的坑只把 s 和 v 發(fā)給控制a 由控制端自己差分來算結(jié)果高頻噪聲被放大車里聽到持續(xù)的細碎頓挫。后來把優(yōu)化器里的 a 和 jerk 一并下發(fā)控制端直接用于前饋問題立刻消失。輸出接口別省a 和 jerk 的精度對下游控制不是錦上添花是剛需。5.3 復盤一次高架匯入場景的對比最后講一次讓我印象深刻的實測對比。某條高架匯入口主路車流密集我們在同一段測試道路上分別跑了 QP 舊版和 nonlinear 新版。場景是左前方一輛慢車壓著車道右側(cè)匯入車流緩緩并入視線里幾百米外路牌寫著匝道結(jié)束。舊版 QP 給出的速度曲線在進入?yún)R入段時先降一截等匯入車流離開可行域后猛加速再因為幾百米后的限速又降——三段“平臺—急剎—急加速”的階梯體感很差。非線性版本則不同它把“貼邊跟住慢車”和“為匯入車讓點空間”放進同一個代價函數(shù)權(quán)衡輸出曲線先建立在一個溫和減速度上貼著可行域邊緣穩(wěn)定通過隨后緩慢回到目標速度。兩臺車跑下來乘客對橫向表現(xiàn)評價沒差別縱向評價差距巨大。那次之后我把一條經(jīng)驗定成了團隊內(nèi)部原則速度優(yōu)化器里的安全邊界能軟則軟盡量不要用硬約束去做避讓決策硬約束留給物理極限就夠了。這條原則聽起來簡單真正實踐起來需要針對每個約束做反復的仿真與實車 A-B 測試。我的個人體會是這個優(yōu)化器真正值錢的地方不在“非線性”這個標簽本身而在于它給了規(guī)劃工程師一個連續(xù)調(diào)優(yōu)的旋鈕把很多從前只能靠規(guī)則硬編碼的駕駛行為變成了目標函數(shù)里幾個有物理意義的權(quán)重系數(shù)。想用好它先忘掉默認參數(shù)表回到“駕駛性格”四個字上想清楚每一項代價在表達什么然后一條路一條路地跑數(shù)據(jù)、回放、微調(diào)。