做自動駕駛規(guī)劃的人繞不過Lattice Planner。就算現(xiàn)在很多團隊轉(zhuǎn)向了更復雜的采樣方案或者直接走learning-based路線Lattice Planner依然是理解軌跡規(guī)劃到底在解什么問題最好的教材。我第一次啃開源代碼里lattice_planner模塊時最先勸退我的不是各種模板類和工廠模式而是一大坨Frenet坐標換算好好的x、y不用為什么非要搞一個s和一個d出來這篇文章打算把這條鏈路徹底捋一遍參考線怎么定義Frenet坐標Frenet坐標下的軌跡怎么用多項式擬合生成Lattice Planner如何在采樣—生成—評分—篩選的循環(huán)里跑起來最后給出我在實車和仿真項目里沉淀下來的調(diào)參思路與踩坑記錄。內(nèi)容適合剛接手規(guī)劃模塊的工程師也適合那些已經(jīng)能把軌跡跑起來、但總感覺差點意思的調(diào)參選手。1. 先搞懂參考線Frenet坐標系的地基到底怎么打1.1 直角坐標系在道路場景下的尷尬回到最基本的提問為什么不在笛卡爾坐標系里直接做規(guī)劃在開闊的靜態(tài)場景里直角坐標系確實夠用。但一旦車在高速公路上跑前方是一個接一個的彎道麻煩就來了。車道線在x-y平面上是一條復雜的曲線你想表達車輛保持在車道中間就需要實時計算車道中心線每個點的朝向再把車的橫向偏差換算成世界坐標下的約束。更麻煩的是道路結(jié)構(gòu)天然是沿著某個方向延伸的這個方向在彎道里每時每刻都在變用固定不動的x、y軸描述一輛順著道路走的車等于硬把一條蛇拉直了量長度。Frenet坐標系解決的就是這個不匹配。它把車輛的位姿從全局坐標轉(zhuǎn)換到以參考線為骨架的局部坐標規(guī)劃問題因此被分解成兩個更直覺的子問題縱向——沿著路走了多遠、多快橫向——偏離路的中心線多遠。1.2 參考線如何定義s和dFrenet坐標系的兩個坐標量是sstation和dlateral offset它們完全依附于參考線存在s車輛當前位置在參考線上的投影點從參考線起點沿弧長方向量到投影點的距離。d從投影點指向車輛實際位置的有向垂距通常約定參考線行進方向的左側(cè)為正、右側(cè)為負。具體做法是在參考線上找到離車輛位置最近的點作為投影點記參考線在該點的切線方向角為θ單位切線向量為t(cosθ, sinθ)單位左法向量為n(-sinθ, cosθ)。那么從Frenet坐標s, d反算回笛卡爾坐標的關(guān)系是x x_r(s) - d * sin(θ(s)) y y_r(s) d * cos(θ(s))其中(x_r(s), y_r(s))是參考線在弧長s處的點。反過來從(x, y)求(s, d)則需要做一次投影即找到一個s使得參考線上點與車輛位置的連線垂直于參考線切線這個s對應(yīng)的距離就是d。關(guān)鍵點在于整個Frenet坐標系的精度上限取決于參考線的質(zhì)量。參考線一旦有毛刺投影位置就會抖動s、d跟著跳下游所有軌跡都跟著遭殃。這就是為什么網(wǎng)上搜frenet坐標系和參考線的關(guān)系最終都會落到同一個結(jié)論——參考線不是規(guī)劃模塊的附屬品它就是Frenet坐標系的坐標軸。坐標軸本身不光滑你在坐標系里做的所有運算都不可信。1.3 參考線質(zhì)量決定Frenet坐標質(zhì)量實際項目中參考線通常來自兩條路徑全局路由Routing給出的粗參考線以及局部規(guī)劃模塊自己做的平滑參考線。HD地圖里的道路中心線點云往往比較密但曲率不連續(xù)路由模塊輸出的是折線角點處曲率直接突變。這兩類輸入都不能直接當Frenet坐標系的骨架用。我常用的處理方式是先對參考線做平滑常見方案有兩種一是用二次規(guī)劃做曲率約束下的平滑目標函數(shù)里同時懲罰參考線與原始線偏離、相鄰點間距變化以及曲率變化率二是用樣條曲線對離散點做擬合強制保證C2連續(xù)。兩種方案我都驗證過前者在參考線需要貼合車道中心并避開障礙物時更靈活后者在計算速度上有優(yōu)勢。無論哪種最后都得看一眼曲率曲線是否連續(xù)這是最容易偷懶也最容易埋雷的一步。實操提示判斷參考線平滑度別只看位置點連起來好不好看直接畫出s-曲率曲線。曲率曲線如果像鋸齒說明平滑沒到位這時候做的Frenet轉(zhuǎn)換、軌跡生成都沒意義。2. 多項式擬合的數(shù)學本質(zhì)五次多項式為什么是規(guī)劃界的默認答案2.1 從邊界條件到多項式系數(shù)拿到了Frenet坐標系之后Lattice Planner的核心工作就變成給定起點狀態(tài)和一系列候選終點狀態(tài)生成一條滿足邊界條件的軌跡曲線。在Frenet框架下橫向運動d(t)和縱向運動s(t)被解耦各自獨立擬合。橫向軌跡d(t)用五次多項式d(t) a0 a1*t a2*t2 a3*t3 a4*t? a5*t?為什么是五次因為一條橫向軌跡需要滿足6個邊界條件起點的位置d(0)、速度?(0)、加速度d?(0)以及終點的位置d(T)、速度?(T)、加速度d?(T)。六個未知系數(shù)對應(yīng)六個約束方程五次多項式正好是能唯一確定一條滿足全部邊界條件曲線的最低階次。更深一層說五次多項式不是拍腦袋選的。在最優(yōu)控制理論里最小化加速度變化率jerk平方積分min ∫ (d3d/dt3)2 dt在給定兩端位置、速度、加速度約束的條件下最優(yōu)解恰好就是五次多項式。這解釋了為什么幾乎所有基于采樣的規(guī)劃器都把五次多項式當默認曲線——它不是恰好能用而是等價于在追求乘坐舒適性jerk最小化意義上的最優(yōu)解?？v向軌跡的邏輯相同但終點約束通常少一個。比如巡航場景只約束終點的速度不約束終點位置這時候四階多項式就夠五個系數(shù)對應(yīng)起點s、?、s?和終點?、s?五個條件。如果是定點停車場景需要同時給定停車位置和速度為0邊界條件變成六個縱向軌跡也得上五次多項式。這個細節(jié)很多入門教程沒講透等你自己寫代碼時會發(fā)現(xiàn)系數(shù)矩陣維度對不上就是因為在糾結(jié)到底用四階還是五階。2.2 五次多項式系數(shù)的實際求解很多資料把公式擺出來就結(jié)束了這里我把求解過程寫完整方便你直接照抄。設(shè)橫向軌跡起終點條件為起點d(0)d0, ?(0)?0, d?(0)d?0 終點d(T)d1, ?(T)?1, d?(T)d?1帶入五次多項式前三個系數(shù)直接由起點條件給出a0 d0 a1 ?0 a2 d?0 / 2剩下三個系數(shù)由終點條件聯(lián)立求解T3*a3 T?*a4 T?*a5 d1 - d0 - ?0*T - 0.5*d?0*T2 3T2*a3 4T3*a4 5T?*a5 ?1 - ?0 - d?0*T 6T*a3 12T2*a4 20T3*a5 d?1 - d?0這是一個3×3的線性方程組系數(shù)矩陣只和時間T有關(guān)起終點狀態(tài)只影響右端項。所以工程實現(xiàn)上可以預先對不同的T求矩陣的逆或者做LU分解運行時只需要做幾次乘加運算幾百條候選軌跡的系數(shù)求解在毫秒級完成。實操提示在實現(xiàn)時建議把系數(shù)求解封裝成獨立函數(shù)入?yún)⑹瞧瘘c狀態(tài)、終點狀態(tài)、時間T出參是六個系數(shù)。寫完之后一定要做一次反向驗證——把算出的系數(shù)代回原函數(shù)檢查t0和tT時的位置、速度、加速度是否和輸入一致。這類矩陣求解代碼最容易出符號錯誤反向驗證能省下大量調(diào)試時間。2.3 多項式擬合做不到的事必須承認多項式擬合只是補間它自己不知道障礙物在哪、不知道路面限速、不知道車輛轉(zhuǎn)彎半徑上限。它保證的只有一件事從起點狀態(tài)平滑過渡到終點狀態(tài)。所以它生成的曲線可能在中間出現(xiàn)很大的橫向位移過沖overshoot也可能在道路曲率大的地方提出一個根本轉(zhuǎn)不過去的彎。所以在Lattice Planner里多項式擬合得出的曲線之后還有兩道工序碰撞檢測和可行性檢查。這兩道工序把數(shù)學上光滑的曲線變成物理上可執(zhí)行且安全的軌跡。另外一條多項式曲線只在[t0, T]區(qū)間內(nèi)定義實際使用時需要按固定時間步長通常0.1秒離散成一系列路徑點再逐個點判斷車輛位姿和周圍障礙物的關(guān)系。3. Lattice Planner完整流水線從離散采樣到軌跡篩選3.1 在Frenet框架下的采樣策略Lattice Planner的核心思想是采樣終點、擬合曲線、評估選擇。它不直接隨機生成整條曲線而是把問題簡化成規(guī)劃周期開始時車輛狀態(tài)已知接下來采樣未來某個時刻車輛應(yīng)該在什么位置、什么速度然后用多項式把當前狀態(tài)和候選終點連起來。橫向采樣圍繞車道中心線展開。假設(shè)車道寬3.5米以車道中心線為基準在[-3.5, 3.5]米范圍內(nèi)每隔0.5米采一條橫向目標線就得到15個候選橫向終點d值每個終點d的橫向速度?和橫向加速度d?都設(shè)為0表示車輛到達目標橫向位置時已經(jīng)穩(wěn)定?？v向采樣則根據(jù)場景不同分成幾類巡航場景采樣目標速度比如[0, 1, 2, ..., V_max]跟車場景采樣與前方障礙物保持的相對距離定點停車場景采樣停車位置。每一組橫向目標縱向目標時間T組合起來就是一條候選軌跡的終點狀態(tài)。這里最關(guān)鍵的參數(shù)是時間T。同一個橫向目標d0用T2秒和用T8秒擬合出來的軌跡性格完全不同T短意味著車輛激烈地打方向快速回正T長意味著緩緩靠過去。所以Lattice Planner的時間采樣不是單值而是在一個范圍內(nèi)分層采樣比如2秒到8秒每0.5秒一層。3.2 軌跡生成、坐標回投與離散化拿到一組候選終點狀態(tài)后用上一節(jié)的多項式擬合方法橫向、縱向各解出一條曲線再把它們逐點合成。合成不是簡單地相加而是利用Frenet與笛卡爾坐標的轉(zhuǎn)換關(guān)系把每個時刻的(s(t), d(t))回投到世界坐標系得到(x(t), y(t))和航向角?；赝稌r要用到參考線在s(t)處的切線方向角θ和曲率κ參考線平滑度在這里第三次影響結(jié)果。軌跡生成完畢按時間步長離散化。比如時間采樣T6秒步長0.1秒一條軌跡就有60個路徑點。每個路徑點左右要掛上車輛輪廓——常見做法是把車輛包圍成沿車身均勻分布的若干個圓用圓和障礙物做距離判斷比精確的矩形碰撞檢測快得多安全裕量通過圓的半徑來控制。3.3 代價函數(shù)如何反映駕駛偏好碰撞檢測只負責淘汰不能走的軌跡候選軌跡里可能仍有幾十條能走但性格不同的代價函數(shù)負責給它們排序。典型代價函數(shù)由以下幾項加權(quán)求和代價項含義影響方向橫向jerk積分∫(d )2dt值越大轉(zhuǎn)向越猛舒適性越差縱向jerk積分∫(s )2dt值越大加減速越突兀橫向偏移代價(d_end)2或∫d2dt懲罰偏離車道中心速度偏差代價(v_end - v_target)2懲罰達不到目標車速時間代價(T - T_expected)2傾向于盡快到達障礙物距離代價1/d_obstacle2 類函數(shù)越靠近障礙物代價越高每一項的權(quán)重系數(shù)直接編碼了這輛車在道路上應(yīng)該表現(xiàn)出的性格。權(quán)重調(diào)大jerk項軌跡會更平順但車輛會表現(xiàn)出不愿變道的遲鈍感權(quán)重調(diào)大偏移項車輛會死死貼在車道中心哪怕旁邊有障礙物也傾向于硬著頭皮走權(quán)重調(diào)大速度項車輛會盡量頂?shù)侥繕塑囁俪嚒⒉⑷雸鼍跋卤憩F(xiàn)得激進。評估時還要考慮可行性約束軌跡上任一點曲率不能超過車輛最小轉(zhuǎn)彎半徑對應(yīng)的曲率上限加速度不能超過輪胎附著極限jerk不能超過人體舒適閾值。超出約束的候選軌跡直接丟棄不參與排序。3.4 一次完整的周期流程把上面所有環(huán)節(jié)串起來一個規(guī)劃周期大概是這樣的獲取當前車輛狀態(tài)位置、速度、加速度、航向角轉(zhuǎn)換到Frenet坐標。確定可行車道考慮變道意圖、前方障礙物、交通規(guī)則。在s-t平面和d-t平面分別采樣終點狀態(tài)。對每個終點狀態(tài)做橫向、縱向多項式擬合和坐標回投生成完整軌跡。逐條做碰撞檢測、可行域約束檢查。對剩余軌跡計算代價函數(shù)選出代價最低的一條。把選中的軌跡前一小段比如0.5秒交給控制模塊執(zhí)行下一個周期重新規(guī)劃。這種滾動窗口機制保證車輛總能基于最新環(huán)境信息重新規(guī)劃即使上一個周期的軌跡已經(jīng)過時影響也只有半個控制周期這正是Lattice Planner在動態(tài)環(huán)境中能穩(wěn)定工作的原因。4. 調(diào)參這件事參數(shù)、權(quán)重的物理意義與一套推薦調(diào)法4.1 時間域參數(shù)規(guī)劃時域與采樣粒度Lattice Planner最核心的時間參數(shù)是時域下界T_min和上界T_max以及時間采樣步長ΔT。T_min決定車輛最早能在什么時候到達某個橫向目標它直接限制了轉(zhuǎn)向的激進程度。T_min設(shè)得太小比如1秒車輛會為了快速變道而拉出極大的橫向速度jerk爆表T_min設(shè)得太大比如4秒碰到近距離障礙物時可能來不及規(guī)劃出避讓軌跡。我的經(jīng)驗是城區(qū)場景T_min取2至2.5秒比較平衡高速場景可以適當放大到2.5至3秒因為車速高時小幅橫向位移就會帶來很大的橫擺角速度給車輛留更多時間更安全。T_max決定規(guī)劃器看得多遠。T_max越大規(guī)劃越有前瞻性但候選軌跡數(shù)量也越多計算量線性上漲。更重要的是T_max超過一定值后遠期的軌跡點依賴參考線外推參考線一旦有偏差遠期軌跡反而不可信。T_max不建議超過10秒高速公路場景8秒就夠城區(qū)場景6秒是常用值。時間采樣步長ΔT影響的是候選軌跡在時域上的分辨率。步長0.5秒意味著每個橫向終點會搭配十幾條不同時長的縱向軌跡。注意候選數(shù)量是乘數(shù)關(guān)系15條橫向線乘以10層時域再乘以8檔目標速度輕輕松松上千條軌跡。如果每周期都暴力跑這么多條CPU和延遲都受不了。實際項目里城區(qū)簡單場景可以把橫向采樣減到7條時域只采4層速度只取3檔先把計算量壓下來再逐步加密。4.2 空間采樣參數(shù)橫向范圍、橫向間距、速度步長橫向采樣范圍和道路寬度強相關(guān)。城市單車道寬度約3.5米車道線到路肩還有個緩沖區(qū)采樣范圍取[-3.5, 3.5]米是合理的。但如果是窄路掉頭或者園區(qū)場景道路寬度不到5米采樣范圍還取這么寬就會有一半軌跡直接撞路沿白算。我推薦先量一下實際道路寬度把橫向采樣范圍設(shè)成比車道寬度單側(cè)多出0.5米。橫向采樣間距決定車輛能精細到多少厘米地選擇橫向位置。間距0.5米是大多數(shù)場景的甜點值能覆蓋車道內(nèi)大部分可行位置而且候選數(shù)量可控。間距調(diào)到0.2米會讓軌跡更貼合障礙物邊緣窄縫通行成功率提高但候選數(shù)量增長明顯評估階段計算量漲得很快。間距調(diào)到1米以上會漏掉關(guān)鍵路徑比如兩輛靜止障礙物之間只有1.2米的間隙1米間距很可能兩邊都采不到規(guī)劃器直接判定無路可走?？v向速度采樣步長影響車速選擇的精度。步長1米/秒意味著候選速度以3.6公里/小時為粒度對巡航控制夠用跟車場景建議細分到0.5米/秒因為前車速度波動時太粗的速度檔位會讓本車加減速呈階梯狀乘坐體驗很差。4.3 代價權(quán)重一組可以直接上手的起點值和調(diào)整策略代價函數(shù)權(quán)重沒有放之四海皆準的數(shù)值但有一個直接可用的起點組合。以下是我在多個項目里驗證過的基礎(chǔ)權(quán)重歸一化后權(quán)重項歸一化起始值調(diào)整方向參考橫向jerk權(quán)重1.0變道太生硬則調(diào)大響應(yīng)太慢則調(diào)小縱向jerk權(quán)重1.0剎車點頭/啟動竄動則調(diào)大橫向偏移代價1.5頻繁壓線則調(diào)大繞障空間不足則調(diào)小速度偏差代價1.0車速長期達不到規(guī)劃值則調(diào)大時間代價0.4希望更快到達則調(diào)大障礙物距離代價3.0貼近障礙物時感到心慌則調(diào)大這套起始值的含義是障礙物距離是安全底線權(quán)重先給足橫向偏移其次保證正常行駛時車輛不壓線jerk權(quán)重起舒適性調(diào)節(jié)作用不要一開始就把舒適性權(quán)重拉滿否則車輛會變得過于佛系在需要果斷變道的場景里猶猶豫豫。調(diào)整權(quán)重的正確姿勢是每次只動一個參數(shù)在固定場景下對比前后軌跡差異。我在實踐中發(fā)現(xiàn)一個特別有用的技巧把每一條候選軌跡的各項代價子項都記錄下來畫出直方圖。如果發(fā)現(xiàn)某一項代價在所有候選軌跡上都大得離譜說明該項權(quán)重過高或者采樣范圍本身就不對。比如橫向偏移代價普遍偏高可能不是權(quán)重問題而是橫向采樣范圍根本沒覆蓋到車道中心線附近——這種時候調(diào)權(quán)重是南轅北轍。4.4 一套可落地的調(diào)參順序我總結(jié)的調(diào)參順序按重要性排序先修參考線。參考線光滑度不達標后面全白調(diào)。畫出曲率曲線確認沒有鋸齒。再定采樣邊界。根據(jù)道路寬度、限速、場景類型確定橫向范圍、速度范圍、時域范圍確保采樣覆蓋所有理論上可走的軌跡。后調(diào)可行性約束。最大加速度、最大jerk、最大曲率按車輛手冊填寫不要憑感覺放大否則選出的軌跡控制模塊執(zhí)行不了。最后調(diào)權(quán)重。在一個典型場景集上跑仿真對比基準權(quán)重和調(diào)整后的軌跡差異確認每個權(quán)重改變帶來的行為變化符合預期。做回歸。跑一遍全場景回歸測試防止某個權(quán)重調(diào)好了一個場景卻破壞了一批場景。這套順序的關(guān)鍵邏輯是采樣范圍決定可選空間約束決定可行空間權(quán)重只在可行空間內(nèi)部排序。如果前面兩步?jīng)]做好權(quán)重再怎么調(diào)都是矮子里拔將軍。5. 實戰(zhàn)中躲不開的坑從坐標換算到可視化調(diào)試5.1 參考線不光滑導致的規(guī)劃抖動我踩過最深的坑就是參考線曲率不連續(xù)?，F(xiàn)象是車輛在直線行駛前方有一個輕微弧度的彎規(guī)劃出的軌跡會突然往一側(cè)甩一下然后又回正?？创a邏輯完全沒問題最后定位到是參考線在某個點處曲率發(fā)生了突變Frenet轉(zhuǎn)換后的d值在同一段路上出現(xiàn)不連續(xù)多項式擬合被迫用一個跳躍的起點去生成軌跡。排查方法很簡單把所有規(guī)劃輸入和輸出畫到同一張圖上包括原始參考線、平滑后參考線、當前車輛的Frenet坐標s, d、規(guī)劃出的軌跡。只要看到軌跡和參考線之間出現(xiàn)莫名的橫向拉扯十有八九是參考線平滑問題。我在這個坑里浪費了整整兩天后來養(yǎng)成了先畫參考線曲率曲線再談其他的習慣。5.2 符號約定不一致左右方向的隱形炸彈Frenet坐標的d值正負號約定在不同代碼庫里不統(tǒng)一。有的約定參考線行進方向左側(cè)為正有的約定右側(cè)為正笛卡爾坐標系的航向角有的用atan2(y, x)相對x軸有的用相對參考線切線方向。這兩處一混軌跡會以鏡像方式生成看起來方向反了。實車表現(xiàn)是規(guī)劃器明明想向右變道控制出來的動作卻向左。這類問題在仿真里不容易暴露因為左右對稱場景下代價幾乎一致車會隨機選一邊一旦上了帶語義的真實道路比如必須在右側(cè)行駛問題立刻放大。我的經(jīng)驗是在代碼里統(tǒng)一用左側(cè)為正約定并在Frenet與笛卡爾轉(zhuǎn)換函數(shù)入口處寫一條注釋標明約定再用一個簡單的起點重合、目標偏移單元測試鎖定行為。5.3 離散化太粗導致碰撞檢測漏檢一條軌跡在數(shù)學上連續(xù)穿過障礙物但因為離散點間隔過大每個離散點都恰好避開了障礙物碰撞檢測返回安全結(jié)果實車撞上去。這是典型的tunneling問題。舉例車速20米/秒72公里/小時軌跡離散步長0.5秒兩個相鄰路徑點間距10米。一個2米寬的障礙物完全可能落在兩個路徑點之間圓形車輛包絡(luò)掃過時恰好擦到它但檢測點都沒落在障礙物內(nèi)。解決辦法有兩個方向一是把時間步長縮小到0.1秒但高速場景下相鄰點間距仍有2米不保險二是改用位置步長離散比如每隔0.2米取一個檢測點同時對相鄰兩個路徑點做線段與障礙物圓的相交檢測。我在項目里兩者都做了才把這類漏檢清零。5.4 可視化調(diào)試三板斧最后分享我認為最有效的三套可視化手段每套都在實際調(diào)試中救過我**第一板斧候選軌跡簇全景圖。**把所有候選軌跡畫在笛卡爾坐標系下按代價從低到高著色低代價綠色、高代價紅色。這張圖能一眼看出代價函數(shù)是不是瞎了眼——比如有一堆軌跡明明在繞遠路代價卻最低說明權(quán)重該調(diào)了。**第二板斧s-t圖和d-t圖。**把選中軌跡的縱向運動畫成s-t圖橫向運動畫成d-t圖。s-t圖的斜率就是速度曲線陡峭說明加速猛d-t圖的彎曲程度對應(yīng)橫向jerk。這兩張圖比笛卡爾軌跡更直觀地暴露舒適性問題因為笛卡爾圖會把縱向和橫向運動混在一起看不清誰在發(fā)力。**第三板斧代價分解表。**每周期把最低代價軌跡的各項子代價打成日志。如果某天車輛行為異常拉出日志一看就能定位到底是障礙物距離代價突然升高還是速度偏差代價失控。調(diào)參時我也依賴這張表它讓每個權(quán)重調(diào)整都有了可量化的反饋而不是靠感覺好像更順了。我在實際項目中的體會是Lattice Planner的難點從來不是某個環(huán)節(jié)的公式有多難而是整條鏈路每個環(huán)節(jié)之間的相互影響。參考線影響FrenetFrenet影響擬合擬合影響代價代價影響行為——任何一環(huán)的微小誤差都會在下一環(huán)被放大。所以調(diào)試時永遠從上游開始查先看參考線再看坐標轉(zhuǎn)換再看軌跡生成最后才是代價權(quán)重。按這個順序走一遍大多數(shù)莫名其妙的規(guī)劃問題都會現(xiàn)出原形。