簡(jiǎn)介面向自動(dòng)駕駛與輔助駕駛領(lǐng)域的算法學(xué)習(xí)者和開(kāi)發(fā)者這份Python項(xiàng)目演示了高速場(chǎng)景下基于Frenet優(yōu)化軌跡的無(wú)人車(chē)動(dòng)作規(guī)劃思路。通過(guò)完整可運(yùn)行的代碼讀者可以學(xué)習(xí)如何將道路坐標(biāo)轉(zhuǎn)換到Frenet系理解軌跡生成、平滑與可行性驗(yàn)證的整體流程適合具有一定Python基礎(chǔ)、希望深入局部路徑規(guī)劃的讀者。壓縮包共8個(gè)文件核心為2個(gè)py腳本——三次樣條插值工具與Frenet最優(yōu)軌跡生成主程序配套xml工程配置、iml模塊文件及pyc編譯緩存整體僅12KB輕量且易于直接運(yùn)行調(diào)試。目前已有5347人次學(xué)習(xí)瀏覽對(duì)正在學(xué)習(xí)路徑規(guī)劃、動(dòng)作規(guī)劃或準(zhǔn)備入門(mén)自動(dòng)駕駛項(xiàng)目的開(kāi)發(fā)者有一定參考價(jià)值。借助代碼讀者可直觀(guān)看到不同采樣參數(shù)對(duì)軌跡形態(tài)的影響并結(jié)合作者博客梳理代價(jià)函數(shù)設(shè)計(jì)、障礙物約束處理和橫向/縱向運(yùn)動(dòng)解耦等關(guān)鍵點(diǎn)修改車(chē)道間距、速度等參數(shù)后還能快速驗(yàn)證多種場(chǎng)景下的規(guī)劃效果是理解無(wú)人車(chē)動(dòng)作規(guī)劃從理論到實(shí)現(xiàn)的實(shí)用樣例。 作為一名在自動(dòng)駕駛行業(yè)摸爬滾打了多年的工程師我深知「Frenet」這三個(gè)字在無(wú)人車(chē)動(dòng)作規(guī)劃中的分量。我們常說(shuō)車(chē)要開(kāi)得「像老司機(jī)」本質(zhì)上就是要在任意彎曲的道路上既能把控好方向盤(pán)、保持車(chē)道又能安全平穩(wěn)地跟車(chē)、變道、避障。而這一切的起點(diǎn)就是坐標(biāo)系的選擇。坦白說(shuō)早期我用笛卡爾坐標(biāo)系做軌跡規(guī)劃時(shí)被彎道上的目標(biāo)點(diǎn)約束問(wèn)題折騰得夠嗆直到徹底吃透基于Frenet優(yōu)化軌跡的動(dòng)作規(guī)劃框架才真正體會(huì)到什么叫「豁然開(kāi)朗」。這篇文章我不打算聊教科書(shū)上那種枯燥的推導(dǎo)而是以我實(shí)際跑過(guò)的一個(gè)項(xiàng)目為例把「為什么用Frenet」「軌跡怎么生成」「代價(jià)函數(shù)怎么設(shè)計(jì)」以及「實(shí)車(chē)調(diào)試中的那些坑」拆開(kāi)講透。無(wú)論你是剛接觸動(dòng)作規(guī)劃的學(xué)生還是已經(jīng)在工程里被各種軌跡抖動(dòng)折磨的同行這篇都值得收藏慢慢看。1. 為什么無(wú)人車(chē)的動(dòng)作規(guī)劃繞不開(kāi)Frenet坐標(biāo)系1.1 笛卡爾坐標(biāo)系下的「約束困境」先從我踩過(guò)的一個(gè)坑說(shuō)起。項(xiàng)目初期我直接在全局坐標(biāo)系笛卡爾坐標(biāo)系下規(guī)劃軌跡車(chē)輛的位姿是 (x, y, yaw)規(guī)劃的每一步都要把車(chē)的位置、朝向、曲率放在統(tǒng)一的XY平面上計(jì)算。當(dāng)時(shí)遇到一個(gè)典型的場(chǎng)景車(chē)輛需要在一個(gè)曲率較大的彎道內(nèi)側(cè)變換車(chē)道目標(biāo)點(diǎn)不僅要滿(mǎn)足橫向位移還要時(shí)刻保持在道路邊界內(nèi)。麻煩就出在這里道路邊界在X-Y平面上是一個(gè)不規(guī)則的彎曲區(qū)域如果你想約束軌跡不壓線(xiàn)就得把道路邊界離散成幾百個(gè)點(diǎn)然后做大量點(diǎn)在多邊形內(nèi)部的判斷計(jì)算量陡增。更要命的是橫向偏移和縱向行駛距離在笛卡爾坐標(biāo)系下是高度耦合的。你想讓車(chē)在彎道里平緩地橫向偏移但同樣的橫向偏移量在彎道外側(cè)和內(nèi)側(cè)對(duì)應(yīng)的實(shí)際道路空間完全不同。我花了大量時(shí)間去調(diào)整約束條件結(jié)果軌跡仍然會(huì)出現(xiàn)局部抖動(dòng)就是因?yàn)樽鴺?biāo)系本身的耦合特性把問(wèn)題復(fù)雜化了。1.2 Frenet框架真正表達(dá)的是什么后來(lái)我切換到了Frenet坐標(biāo)系一句話(huà)概括它的核心思想以參考線(xiàn)通常是道路中心線(xiàn)或車(chē)道中心線(xiàn)為基準(zhǔn)把車(chē)輛的任意運(yùn)動(dòng)拆解成兩個(gè)獨(dú)立的分量——縱向位移 s沿著參考線(xiàn)走了多遠(yuǎn)和橫向偏移 d偏離參考線(xiàn)多少。這個(gè)拆解帶來(lái)的好處是革命性的。道路的彎曲形狀被「拉直」成了一個(gè)弧長(zhǎng)軸 s原本在笛卡爾坐標(biāo)系下的彎道約束變成了對(duì)橫向偏移 d 的簡(jiǎn)單區(qū)間約束。比如「車(chē)輛不能壓線(xiàn)」可以直接寫(xiě)成一個(gè)不等式d_min d d_max。而「車(chē)輛需要保持車(chē)道中心」則意味著目標(biāo)橫向偏移 d_target ≈ 0。這就很像我們平時(shí)開(kāi)車(chē)時(shí)腦子里的導(dǎo)航邏輯我不會(huì)去想「我現(xiàn)在在世界坐標(biāo)系里的絕對(duì)坐標(biāo)是多少」而是想「我離車(chē)道中心偏了多遠(yuǎn)」「前方彎道還有多少米」。Frenet坐標(biāo)系本質(zhì)上就是把這種人類(lèi)駕駛直覺(jué)數(shù)學(xué)化。所以做無(wú)人車(chē)動(dòng)作規(guī)劃時(shí)Frenet 幾乎是繞不開(kāi)的基礎(chǔ)設(shè)施。2. 從行車(chē)狀態(tài)到Frenet軌跡完整的數(shù)學(xué)建模與狀態(tài)轉(zhuǎn)換2.1 Frenet坐標(biāo)系下車(chē)輛狀態(tài)的六個(gè)關(guān)鍵量要在代碼里實(shí)現(xiàn)這個(gè)框架第一步是把車(chē)輛的實(shí)時(shí)狀態(tài)映射到Frenet坐標(biāo)系中。這里需要用到六個(gè)關(guān)鍵量它們共同描述了一輛車(chē)在當(dāng)前參考線(xiàn)上的運(yùn)動(dòng)狀態(tài)s縱向弧長(zhǎng)表示車(chē)輛在參考線(xiàn)上的投影位置。s_dot縱向速度即沿參考線(xiàn)方向的速度分量。s_ddot縱向加速度。d橫向偏移表示車(chē)輛相對(duì)參考線(xiàn)的側(cè)向距離左負(fù)右正符號(hào)約定因工程而異。d_dot橫向速度即橫向偏移的變化率。d_ddot橫向加速度。嚴(yán)格來(lái)說(shuō)完整的Frenet公式還會(huì)涉及 d 對(duì) s 的各階導(dǎo)數(shù)d、d、d用來(lái)關(guān)聯(lián)軌跡幾何與車(chē)輛運(yùn)動(dòng)學(xué)。但在實(shí)際的采樣規(guī)劃中我們通常會(huì)保留時(shí)間域上的導(dǎo)數(shù)因?yàn)楹罄m(xù)生成軌跡多項(xiàng)式時(shí)時(shí)間域積分更直觀(guān)。2.2 從笛卡爾坐標(biāo)到Frenet坐標(biāo)的轉(zhuǎn)換實(shí)踐工程上做坐標(biāo)轉(zhuǎn)換最核心的工作是找到車(chē)輛當(dāng)前位置在參考線(xiàn)上的最近投影點(diǎn)。這個(gè)點(diǎn)對(duì)應(yīng)的弧長(zhǎng)就是 s而從車(chē)輛位置到投影點(diǎn)的有向向量長(zhǎng)度就是 d。我們項(xiàng)目里的做法是把參考線(xiàn)預(yù)采樣成密集的點(diǎn)列每0.5米一個(gè)點(diǎn)然后用KD-Tree加速查找最近點(diǎn)。這里有一個(gè)容易被忽略的細(xì)節(jié)找到最近點(diǎn)之后還需要對(duì)s坐標(biāo)做線(xiàn)性插值修正。因?yàn)樽罱c(diǎn)所在的線(xiàn)段其方向可能和車(chē)輛前進(jìn)方向有夾角直接取離散點(diǎn)索引做s會(huì)導(dǎo)致縱向速度產(chǎn)生微小跳變。我試過(guò)忽略這個(gè)修正結(jié)果在彎道出口處軌跡曲率出現(xiàn)了周期性抖動(dòng)。坐標(biāo)轉(zhuǎn)換的偽代碼大致長(zhǎng)這樣def cartesian_to_frenet(x, y, ref_line): # 1. KD-Tree 查找最近點(diǎn)索引 nearest_idx kd_tree.query([x, y])[1] # 2. 在最近線(xiàn)段上做投影插值 segment_start ref_line[nearest_idx] segment_end ref_line[nearest_idx 1] dx segment_end.x - segment_start.x dy segment_end.y - segment_start.y seg_len sqrt(dx * dx dy * dy) # 3. 計(jì)算橫向偏移帶符號(hào) cross dx * (y - segment_start.y) - dy * (x - segment_start.x) d cross / seg_len # 4. 計(jì)算縱向弧長(zhǎng)考慮插值比例 proj_ratio ((x - segment_start.x) * dx (y - segment_start.y) * dy) / (seg_len * seg_len) s ref_line.cumulative_s[nearest_idx] proj_ratio * seg_len return s, d這一步如果做得扎實(shí)后面軌跡生成的所有輸入都是干凈、連續(xù)的能省下無(wú)數(shù)排查時(shí)間。3. 候選軌跡生成橫向與縱向解耦后的采樣與多項(xiàng)式擬合3.1 橫向軌跡五次多項(xiàng)式連接起終點(diǎn)狀態(tài)有了當(dāng)前狀態(tài)下一件大事就是「生成候選軌跡」。Frenet框架的核心魅力在于橫向運(yùn)動(dòng) d(t) 和縱向運(yùn)動(dòng) s(t) 可以被分別規(guī)劃然后在時(shí)間軸上重新組合。先看橫向。我在項(xiàng)目中生成橫向軌跡時(shí)使用的是五次多項(xiàng)式d(t) a0 a1·t a2·t2 a3·t3 a4·t? a5·t?為什么是五次而不是三次因?yàn)闄M向軌跡需要滿(mǎn)足的邊界條件有六個(gè)起點(diǎn)位置的橫向偏移、橫向速度、橫向加速度以及終點(diǎn)位置的橫向偏移、橫向速度、橫向加速度。六個(gè)未知數(shù)a0~a5六個(gè)方程恰好能解出唯一解。如果需要終點(diǎn)加速度可控三次多項(xiàng)式是無(wú)論如何也做不到的。實(shí)際操作中我會(huì)先把當(dāng)前車(chē)輛的 d、d_dot、d_ddot 讀出來(lái)作為起點(diǎn)狀態(tài)然后遍歷一系列目標(biāo)橫向偏移值 d_target。比如在標(biāo)準(zhǔn)雙車(chē)道場(chǎng)景下我會(huì)采樣 {-3.5, -3.0, -2.5, ..., 0, ..., 3.5} 米同時(shí)設(shè)定到達(dá)時(shí)間 T 的采樣集合常見(jiàn)的取值是 {1秒, 2秒, 3秒, 4秒, 5秒}。這樣橫向規(guī)劃就生成了一批「從當(dāng)前位置平滑挪到不同橫向位置」的軌跡簇。3.2 縱向軌跡四次多項(xiàng)式讓速度變化更平滑縱向軌跡 s(t) 采用了四次多項(xiàng)式s(t) b0 b1·t b2·t2 b3·t3 b4·t?這里只需要五個(gè)邊界條件因?yàn)榭v向規(guī)劃的終點(diǎn)通常不需要約束加速度只需要約束位置和速度即可。起點(diǎn)縱向位置 s、縱向速度 s_dot、縱向加速度 s_ddot終點(diǎn)縱向位置 s_target、終點(diǎn)縱向速度 s_dot_target。我特別強(qiáng)調(diào)一個(gè)技巧終點(diǎn)速度的選擇決定了車(chē)輛的駕駛風(fēng)格。如果你想讓車(chē)在跟車(chē)時(shí)保持舒適那么 s_dot_target 應(yīng)該朝向目標(biāo)車(chē)輛的當(dāng)前速度收斂。比如我在高速巡航場(chǎng)景下會(huì)設(shè)置一個(gè)「巡航速度」參數(shù)比如 25 m/s約90 km/h然后所有縱向采樣的終點(diǎn)速度都向這個(gè)值靠攏。而在路口或擁堵場(chǎng)景則會(huì)根據(jù)前車(chē)速度動(dòng)態(tài)調(diào)整?？v向采樣的目標(biāo)位置 s_target 不是隨意定的它由想要到達(dá)的縱向距離范圍決定。比如前方10米、30米、60米處分別采樣一個(gè)目標(biāo)位置加上目標(biāo)速度就生成了一組全速域可選的縱向軌跡。3.3 采樣組合與軌跡合并成完整運(yùn)動(dòng)軌跡橫向軌跡和縱向軌跡生成好之后就要把它們「合體」。具體的做法是在每一個(gè)時(shí)間步 t 上把橫向多項(xiàng)式給出的 d(t) 和縱向多項(xiàng)式給出的 s(t) 合并成一個(gè)Frenet坐標(biāo)點(diǎn) (s(t), d(t))然后轉(zhuǎn)換回笛卡爾坐標(biāo)系。這個(gè)合體過(guò)程在代碼里特別容易出錯(cuò)因?yàn)樗髾M向和縱向的采樣時(shí)間集合必須完全對(duì)齊。我見(jiàn)過(guò)不少新手直接把兩個(gè)獨(dú)立時(shí)間軸的結(jié)果套在一起導(dǎo)致軌跡出現(xiàn)「時(shí)間扭曲」。穩(wěn)妥的做法是先定義一個(gè)統(tǒng)一的時(shí)間軸比如 t 0, 0.1, 0.2, ..., T_max再分別對(duì)橫向和縱向多項(xiàng)式做求值最后配對(duì)組合。組合之后的軌跡還要做一次幾何轉(zhuǎn)換?？梢园褏⒖季€(xiàn)的每個(gè)點(diǎn)看成是一個(gè)局部坐標(biāo)系原點(diǎn)把 (s(t), d(t)) 映射到全局坐標(biāo)系。這個(gè)環(huán)節(jié)里s 方向決定參考線(xiàn)切向d 方向決定參考線(xiàn)法向轉(zhuǎn)換公式是def frenet_to_cartesian(s, d, ref_line): # 找到參考線(xiàn)上 s 對(duì)應(yīng)的區(qū)間插值得到位置和切線(xiàn)方向 x_ref, y_ref, yaw_ref interpolate_ref_line(ref_line, s) # 法向方向偏移 x x_ref - d * sin(yaw_ref) y y_ref d * cos(yaw_ref) return x, y到這里一條完整的候選軌跡就生成了。批量遍歷橫向目標(biāo)偏移和縱向目標(biāo)位置的笛卡爾積就能得到幾十上百條候選軌跡下一步就是從中選出最優(yōu)。4. 最優(yōu)軌跡篩選代價(jià)函數(shù)設(shè)計(jì)、碰撞檢測(cè)與權(quán)重調(diào)參4.1 代價(jià)函數(shù)的架構(gòu)與各項(xiàng)的物理意義候選軌跡生成后不能一股腦全交給控制模塊必須通過(guò)「淘汰評(píng)分」兩道關(guān)卡。淘汰靠碰撞檢測(cè)和物理約束檢查評(píng)分靠代價(jià)函數(shù)。我項(xiàng)目的代價(jià)函數(shù)長(zhǎng)這樣J_total w_safe · J_collision w_jerk · J_jerk w_lat · J_lateral_offset w_eff · J_efficiency w_consistency · J_consistency每一項(xiàng)的物理意義很直接J_collision安全代價(jià)軌跡與障礙物距離的懲罰項(xiàng)。如果距離小于安全閾值這一項(xiàng)直接給極大值距離越大懲罰越小。更精細(xì)的做法是引入「時(shí)間維度」的碰撞檢查即考慮障礙物未來(lái)一段時(shí)間的預(yù)測(cè)位置與軌跡在對(duì)應(yīng)時(shí)刻的位置比較。J_jerk舒適性代價(jià)評(píng)價(jià)軌跡的加加速度即橫向jerk和縱向jerk的加權(quán)和。這個(gè)值越低車(chē)輛加減速和方向盤(pán)動(dòng)作越柔和乘客感受越舒適。J_lateral_offset偏移代價(jià)評(píng)價(jià)軌跡與參考線(xiàn)的偏差程度。它反映了「保持車(chē)道中心」的傾向避免車(chē)輛在無(wú)必要的情況下頻繁變道或壓線(xiàn)。J_efficiency效率代價(jià)評(píng)價(jià)軌跡與期望車(chē)速的偏差。如果前方暢通效率代價(jià)會(huì)鼓勵(lì)車(chē)輛加速到巡航速度如果前方堵車(chē)效率代價(jià)會(huì)引導(dǎo)車(chē)輛跟隨前車(chē)速度。J_consistency一致性代價(jià)比較當(dāng)前候選軌跡與上一周期規(guī)劃結(jié)果的差異。這是我在實(shí)車(chē)調(diào)試后補(bǔ)充的非常重要。沒(méi)有這一項(xiàng)車(chē)輛在快速切換規(guī)劃周期時(shí)軌跡容易左右搖擺乘感極差。4.2 碰撞檢測(cè)與物理約束檢查的前置把關(guān)代價(jià)函數(shù)之前必須先做硬約束檢查。我把它分成三層靜態(tài)障礙物碰撞把柵格地圖上被占用的區(qū)域和軌跡點(diǎn)做距離檢查。這里我使用「圓-矩形」粗檢測(cè)速度快然后對(duì)候選軌跡上曲率較大的點(diǎn)做精確檢測(cè)避免軌跡弧線(xiàn)中部侵入障礙物。動(dòng)態(tài)障礙物碰撞一輛無(wú)人車(chē)不可能只面對(duì)靜止的錐桶。對(duì)于動(dòng)態(tài)車(chē)輛我會(huì)用簡(jiǎn)單的線(xiàn)性速度模型預(yù)測(cè)它未來(lái)3~5秒的包圍盒位置然后和軌跡逐時(shí)刻檢查。這里有個(gè)經(jīng)驗(yàn)預(yù)測(cè)時(shí)間窗不要設(shè)太長(zhǎng)超過(guò)5秒的預(yù)測(cè)模型誤差極大反而會(huì)誤殺很多本來(lái)安全的軌跡。車(chē)輛動(dòng)力學(xué)約束生成軌跡時(shí)可以天馬行空但最終能落地的軌跡必須滿(mǎn)足車(chē)輛本身的轉(zhuǎn)向和加速度極限。比如最大轉(zhuǎn)向角約束會(huì)轉(zhuǎn)化為軌跡最大曲率限制最大加速度/減速度約束會(huì)對(duì)應(yīng)縱向加速度限制。如果一個(gè)候選軌跡在以上任意一層被卡住就直接淘汰不再參與代價(jià)評(píng)分。這樣既保證了安全性又大幅減少了評(píng)分階段的計(jì)算量。4.3 權(quán)重調(diào)參的幾個(gè)實(shí)用方向代價(jià)函數(shù)寫(xiě)完之后真正的工程難點(diǎn)才出現(xiàn)權(quán)重怎么設(shè)。我常用的方法是「分層調(diào)參」第一層先固定安全相關(guān)權(quán)重把J_collision調(diào)成一個(gè)「一刀切」的硬門(mén)限不是靠權(quán)重壓而是靠淘汰機(jī)制兜底。第二層調(diào)舒適性和一致性。先設(shè)定一組默認(rèn)權(quán)重在仿真環(huán)境里跑標(biāo)準(zhǔn)彎道和變道場(chǎng)景觀(guān)察橫向加速度曲線(xiàn)。如果峰值加速度超過(guò)0.3g說(shuō)明J_jerk權(quán)重偏低或采樣時(shí)間窗太短我會(huì)把采樣時(shí)間窗拉長(zhǎng)1~2秒而不是一味加權(quán)重這樣更容易根治抖動(dòng)。第三層調(diào)效率和偏移。城市道路場(chǎng)景中效率和偏移經(jīng)常打架——想開(kāi)得快就難免偏離車(chē)道中心。我的建議是在限速較高的場(chǎng)景讓效率權(quán)重占優(yōu)在擁堵場(chǎng)景讓偏移和舒適權(quán)重占優(yōu)。一個(gè)我常用的初始權(quán)重方案相對(duì)值代價(jià)項(xiàng)權(quán)重比例說(shuō)明J_collision1000本質(zhì)替代淘汰碰到即死J_jerk10保證基本舒適度J_lateral_offset5趨向車(chē)道中心J_efficiency8追求合理速度J_consistency15抑制軌跡抖動(dòng)這套參數(shù)在我的項(xiàng)目里表現(xiàn)穩(wěn)定但具體數(shù)值必須在你的場(chǎng)景里重新標(biāo)定。不要迷信任何一套固定權(quán)重。5. 實(shí)車(chē)落地中被反復(fù)教育的幾個(gè)細(xì)節(jié)5.1 Frenet坐標(biāo)與笛卡爾坐標(biāo)轉(zhuǎn)換的精度陷阱仿真里跑得好好的軌跡一上實(shí)車(chē)就可能出現(xiàn)「車(chē)輛畫(huà)龍」。我排查了整整兩周最后定位到問(wèn)題根源參考線(xiàn)投影的精度不夠。當(dāng)車(chē)輛實(shí)際位置與參考線(xiàn)的最近點(diǎn)落在兩個(gè)預(yù)采樣點(diǎn)之間時(shí)線(xiàn)性插值會(huì)引入一個(gè)微小的切向誤差這個(gè)誤差在彎道曲率變化劇烈的地方會(huì)被放大導(dǎo)致轉(zhuǎn)換出的笛卡爾軌跡出現(xiàn)高頻抖動(dòng)。解決辦法是在投影點(diǎn)附近做一次局部二階插值或者把參考線(xiàn)采樣間隔從0.5米加密到0.2米。加密之后軌跡抖動(dòng)問(wèn)題基本消失。代價(jià)是內(nèi)存和查找時(shí)間略微增加但換來(lái)的是實(shí)車(chē)穩(wěn)定性這筆賬非常劃算。5.2 高速場(chǎng)景下橫向采樣的范圍收縮很多從仿真轉(zhuǎn)到實(shí)車(chē)的朋友會(huì)忽略一個(gè)問(wèn)題橫向采樣范圍應(yīng)該隨車(chē)速變化。在低速泊車(chē)場(chǎng)景橫向采樣范圍可以拉到 ±3.5 米甚至更大讓車(chē)有足夠的機(jī)動(dòng)空間。但在高速公路上車(chē)速到 80 km/h 以上時(shí)橫向采樣范圍如果還是 ±3.5 米生成的軌跡往往因?yàn)闄M向加速度過(guò)大而被動(dòng)力學(xué)約束淘汰有效軌跡稀疏甚至可能導(dǎo)致規(guī)劃無(wú)解。我實(shí)際的做法是做一個(gè)簡(jiǎn)單的線(xiàn)性收縮max_d clamp(0.5 0.04 * v, 1.0, 3.5)車(chē)速越高橫向可探索范圍越窄。這樣既能保證高速下軌跡都在車(chē)輛物理極限附近又能讓低速時(shí)的機(jī)動(dòng)靈活性不丟失。5.3 軌跡連續(xù)性防止上一幀到下一幀的跳變最后聊一個(gè)所有實(shí)車(chē)工程師都會(huì)遇到的痛點(diǎn)軌跡跳變。原因是每個(gè)規(guī)劃周期都是獨(dú)立采樣的上一幀選出的最優(yōu)軌跡是T1下一幀可能因?yàn)檎系K物位置輕微變化或權(quán)重微小波動(dòng)選出一條與前幀差異很大的軌跡T2。如果直接把T2交給控制模塊車(chē)輛會(huì)猛地打一把方向。解決這個(gè)問(wèn)題我在項(xiàng)目中引入了「上一周期的軌跡作為基準(zhǔn)」策略。具體來(lái)說(shuō)把上一幀的最優(yōu)軌跡作為一組固定的候選軌跡加入本次的候選集中。在代價(jià)函數(shù)里提高 J_consistency 的權(quán)重凡是與上幀軌跡偏離過(guò)大的候選直接扣分。這套策略實(shí)車(chē)效果非常顯著。我見(jiàn)過(guò)一個(gè)真實(shí)案例去掉軌跡連續(xù)性約束后車(chē)輛在自動(dòng)駕駛變道時(shí)平均每15秒出現(xiàn)一次明顯的橫向抖動(dòng)加上之后連續(xù)跑了30分鐘都沒(méi)有一次明顯跳變。另外還有一個(gè)細(xì)節(jié)在做軌跡平滑時(shí)我會(huì)對(duì)最終輸出的路徑點(diǎn)做一個(gè)輕量級(jí)的曲率平滑比如滑動(dòng)窗口平均或道格拉斯-普克抽稀后再插值。這個(gè)步驟不改變軌跡的整體形態(tài)但能顯著減少方向盤(pán)修正頻率對(duì)乘坐體驗(yàn)的改善非常直接。最后再分享一個(gè)我個(gè)人的調(diào)試心得每次改完代價(jià)函數(shù)或采樣參數(shù)不要只在單一場(chǎng)景下驗(yàn)證。我會(huì)把同一套參數(shù)丟到三組數(shù)據(jù)里——高速?gòu)澋?、城市擁堵、緊急避障——去跑回歸測(cè)試。只有三個(gè)場(chǎng)景都不出問(wèn)題才敢提交。因?yàn)镕renet框架的解耦特性雖然強(qiáng)大但橫縱向軌跡組合之后小概率會(huì)出現(xiàn)「橫向沒(méi)問(wèn)題、縱向也沒(méi)問(wèn)題、合在一起就是不舒服」的耦合效應(yīng)。這種時(shí)候檢查一下是不是采樣組合里漏掉了「動(dòng)態(tài)障礙物預(yù)測(cè)軌跡與候選軌跡重疊」的情況通常能找到答案。本文還有配套的精品資源點(diǎn)擊獲取