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