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