在自動駕駛的 “駕駛技能” 里，橫向控制算法就像司機(jī)的雙手 —— 它決定車輛能否精準(zhǔn)沿著規(guī)劃好的路線行駛。而在眾多橫向控制算法中，純跟蹤算法(PurePursuit)憑借 “簡單好用” 的特質(zhì)，成為低速場景(比如園區(qū)物流車、自動泊車)的首選方案。人類開車時，從不會死盯著車頭，而是會看向前方幾十米的目標(biāo)點，然后自然轉(zhuǎn)動方向盤對準(zhǔn)它。純跟蹤算法的核心邏輯，就是把這個過程 “數(shù)學(xué)化”。

一、化繁為簡：自行車模型

真實汽車的轉(zhuǎn)彎原理很復(fù)雜(比如阿克曼轉(zhuǎn)向，四個車輪繞同一圓心轉(zhuǎn)動，內(nèi)側(cè)車輪轉(zhuǎn)角更大)，但純跟蹤算法用了個 “偷懶” 的辦法 —— 自行車模型。把汽車簡化成 “兩個輪子”(前軸和后軸各一個)，只考慮平面運(yùn)動。這樣一來，前輪轉(zhuǎn)角 δ 和轉(zhuǎn)彎半徑 R 的關(guān)系就變得簡單：tan (δ)= 軸距 L / 轉(zhuǎn)彎半徑 R。也就是說，只要算出目標(biāo)轉(zhuǎn)彎半徑 R，就能直接得到需要轉(zhuǎn)多少方向盤 (δ=arctan (L/R))。這個模型在 60km/h 以下的低速場景非常好用，實測橫向偏差能控制在 15cm 以內(nèi)(相當(dāng)于一個拳頭的寬度)。

二、核心操作：盯準(zhǔn)目標(biāo)點，畫個圓弧沖過去

純跟蹤算法的工作步驟，像極了新手司機(jī)練車。

選目標(biāo)點：以車輛后軸為起點，沿規(guī)劃路線向前找一個 “預(yù)瞄點”(距離自己 ld 米)。

算轉(zhuǎn)彎半徑：計算從當(dāng)前位置到目標(biāo)點，需要畫一個多大半徑的圓弧。

打方向盤：根據(jù)圓弧半徑，用上面的公式算出前輪該轉(zhuǎn)多少角度。

整個過程就像用圓規(guī)畫圓 —— 后軸是圓心，預(yù)瞄距離 ld 是半徑，目標(biāo)點是圓上的一點。

三、工程落地：把公式變成 “能跑的代碼”

理論好懂，但落地時藏著不少細(xì)

。我們按算法運(yùn)行的步驟，一步步拆解關(guān)鍵操作。

給目標(biāo)點 “定位”：規(guī)劃好的路線坐標(biāo)是 “全局地圖” 里的(比如經(jīng)緯度)，而車輛需要知道目標(biāo)點在 “自己的坐標(biāo)系” 里的位置(比如 “前方 5 米，偏右 0.3 米”)。這就需要做坐標(biāo)轉(zhuǎn)換。簡單說，就是把 “地圖上的點” 轉(zhuǎn)化成 “相對于自己的位置”，這樣才能算清楚 “該往左轉(zhuǎn)還是右轉(zhuǎn)”。節(jié)預(yù)瞄距離怎么定：速度快就看遠(yuǎn)點預(yù)瞄距離 ld(就是看前方多遠(yuǎn)的點)是個關(guān)鍵參數(shù)，直接影響控制效果。太慢的車(比如泊車時 <5km/h)：看 1-2 米就行。園區(qū)里跑的車(30-40km/h)：看 3-5 米。工程上常用速度自適應(yīng)策略：速度 v 越快，預(yù)瞄距離 ld 越遠(yuǎn)(比如 ld=k×v，k 取 1.1-1.5)。車速單位要轉(zhuǎn)成 m/s(CAN 總線常給 km/h，別忘除以 3.6)。最低預(yù)瞄距離留 3-5 米(防止車速為 0 時出錯)。選目標(biāo)點時，要找到 “剛好超過預(yù)瞄距離 ld” 的那個點，步驟很簡單：

先在規(guī)劃路線上，找到離自己最近的點(相當(dāng)于 “當(dāng)前位置”)。

從這個點往前數(shù)，累加路線長度，直到超過 ld，這個點就是目標(biāo)點。

轉(zhuǎn)向角 δ=arctan (2× 軸距 L×sin (α)/ 預(yù)瞄距離 ld)：

轉(zhuǎn)成方向盤角度：前輪轉(zhuǎn)角 × 轉(zhuǎn)向傳動比(比如傳動比 16，就是前輪轉(zhuǎn) 1 度，方向盤要轉(zhuǎn) 16 度)。

平滑處理：用一階低通濾波消除抖動(比如濾波系數(shù) 0.2，新角度 = 0.8× 舊角度 + 0.2× 計算值)。

應(yīng)用場景：

計算快：單幀處理 < 5ms，普通單片機(jī)都能跑。

自動泊車：10km/h 以內(nèi)的泊車場景，短預(yù)瞄距離能精準(zhǔn)入位。

農(nóng)業(yè)機(jī)械：農(nóng)田里直線作業(yè)，結(jié)合 PID 補(bǔ)償能提高精度。

純跟蹤算法就像自動駕駛領(lǐng)域的 “老黃?！薄? 它不算最先進(jìn)，但勝在簡單可靠。在低速封閉場景里，它的表現(xiàn)甚至不輸復(fù)雜的模型預(yù)測控制(MPC)。理解它的原理，不僅能幫你快速實現(xiàn)一個可用的橫向控制模塊，更能讓你明白：好的算法不是越復(fù)雜越好，而是能在特定場景下解決問題。