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

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

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

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

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

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

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

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

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

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

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

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

給目標(biāo)點(diǎn) “定位”：規(guī)劃好的路線坐標(biāo)是 “全局地圖” 里的(比如經(jīng)緯度)，而車輛需要知道目標(biāo)點(diǎn)在 “自己的坐標(biāo)系” 里的位置(比如 “前方 5 米，偏右 0.3 米”)。這就需要做坐標(biāo)轉(zhuǎn)換。簡單說，就是把 “地圖上的點(diǎn)” 轉(zhuǎn)化成 “相對于自己的位置”，這樣才能算清楚 “該往左轉(zhuǎn)還是右轉(zhuǎn)”。節(jié)預(yù)瞄距離怎么定：速度快就看遠(yuǎn)點(diǎn)預(yù)瞄距離 ld(就是看前方多遠(yuǎn)的點(diǎn))是個(gè)關(guān)鍵參數(shù)，直接影響控制效果。太慢的車(比如泊車時(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 時(shí)出錯(cuò))。選目標(biāo)點(diǎn)時(shí)，要找到 “剛好超過預(yù)瞄距離 ld” 的那個(gè)點(diǎn)，步驟很簡單：

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

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

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

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

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

應(yīng)用場景：

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

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

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

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