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

一、化繁為簡(jiǎn)：自行車(chē)模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

應(yīng)用場(chǎng)景：

計(jì)算快：?jiǎn)螏幚?< 5ms，普通單片機(jī)都能跑。

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

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

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