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