本文來(lái)源：智車科技

在自動(dòng)駕駛的“駕駛技能”里，橫向控制算法就像司機(jī)的雙手——它決定車輛能否精準(zhǔn)沿著規(guī)劃好的路線行駛。而在眾多橫向控制算法中，純跟蹤算法(PurePursuit)憑借“簡(jiǎn)單好用”的特質(zhì)，成為低速場(chǎng)景（比如園區(qū)物流車、自動(dòng)泊車）的首選方案。今天我們就用最通俗的語(yǔ)言，拆解這個(gè)算法的底層邏輯：它如何模仿人類駕駛？工程落地時(shí)要避開(kāi)哪些坑？又適合哪些場(chǎng)景？

從人類駕駛到算法：純跟蹤的“模仿秀”

人類開(kāi)車時(shí)，從不會(huì)死盯著車頭，而是會(huì)看向前方幾十米的目標(biāo)點(diǎn)，然后自然轉(zhuǎn)動(dòng)方向盤對(duì)準(zhǔn)它。純跟蹤算法的核心邏輯，就是把這個(gè)過(guò)程“數(shù)學(xué)化”。

先給汽車“瘦身”：從四輪到兩輪的簡(jiǎn)化

真實(shí)汽車的轉(zhuǎn)彎原理很復(fù)雜（比如阿克曼轉(zhuǎn)向，四個(gè)車輪繞同一圓心轉(zhuǎn)動(dòng)，內(nèi)側(cè)車輪轉(zhuǎn)角更大），但純跟蹤算法用了個(gè)“偷懶”的辦法——自行車模型：

圖 真實(shí)車輛的阿克曼轉(zhuǎn)向幾何（上）及其簡(jiǎn)化的自行車模型（下）

把汽車簡(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)多少方向盤（δ=arctan(L/R)）。

這個(gè)模型在60km/h以下的低速場(chǎng)景非常好用，實(shí)測(cè)橫向偏差能控制在15cm以內(nèi)（相當(dāng)于一個(gè)拳頭的寬度）。

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

純跟蹤算法的工作步驟，像極了新手司機(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ò)程就像用圓規(guī)畫圓——后軸是圓心，預(yù)瞄距離ld是半徑，目標(biāo)點(diǎn)是圓上的一點(diǎn)。

圖 純跟蹤算法的幾何關(guān)系推導(dǎo)

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

理論好懂，但落地時(shí)藏著不少細(xì)節(jié)。我們按算法運(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)換：

簡(jiǎn)單說(shuō)，就是把“地圖上的點(diǎn)”轉(zhuǎn)化成“相對(duì)于自己的位置”，這樣才能算清楚“該往左轉(zhuǎn)還是右轉(zhuǎn)”。

圖 坐標(biāo)轉(zhuǎn)化

第二步：預(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米

速度再快？抱歉，純跟蹤算法不太擅長(zhǎng)（后面會(huì)說(shuō)原因）

工程上常用速度自適應(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ò)）

圖 預(yù)瞄距離與速度關(guān)系

第三步：目標(biāo)點(diǎn)怎么選？別找錯(cuò)了！

選目標(biāo)點(diǎn)時(shí)，要找到“剛好超過(guò)預(yù)瞄距離ld”的那個(gè)點(diǎn)，步驟很簡(jiǎn)單：

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

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

最后一步：方向盤轉(zhuǎn)角怎么算？還要做什么處理？

有了目標(biāo)點(diǎn)，用下面的公式直接算前輪轉(zhuǎn)角：

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

（α是車輛當(dāng)前航向與目標(biāo)點(diǎn)方向的夾角，用坐標(biāo)轉(zhuǎn)換后的局部坐標(biāo)就能算）

但工程落地時(shí)，這一步還要做兩件事：

轉(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ì)算值）

純跟蹤算法：哪些場(chǎng)景“能用”，哪些場(chǎng)景“頭疼”？

優(yōu)勢(shì)：簡(jiǎn)單就是王道

計(jì)算快：?jiǎn)螏�處�?lt;5ms，普通單片機(jī)都能跑

魯棒性強(qiáng)：定位有小誤差（比如10cm）也不影響

參數(shù)少：主要調(diào)預(yù)瞄距離相關(guān)的2-3個(gè)參數(shù)，新手也能上手

缺點(diǎn)：高速場(chǎng)景“hold不住”

忽略動(dòng)力學(xué)：沒(méi)考慮輪胎打滑（比如雨天路面滑，算法算出來(lái)的轉(zhuǎn)角會(huì)不準(zhǔn)）

高速易跑偏：車速超過(guò)60km/h后，穩(wěn)定性明顯下降（急彎容易“切彎”）

參數(shù)敏感：預(yù)瞄距離差1米，控制效果可能天差地別

最適合的場(chǎng)景

低速園區(qū)車：廠區(qū)、校園里的配送車（速度<40km/h）

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

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

工程落地避坑指南

預(yù)瞄距離不是越大越好：

低速（<20km/h）：3-5米

中速（20-40km/h）：5-8米

急彎處：比直道縮短20%（防止切彎）

濾波參數(shù)別亂調(diào)：

抑制抖動(dòng)選強(qiáng)濾波（系數(shù)0.1-0.3）

高速場(chǎng)景選弱濾波（系數(shù)0.5-0.8），避免響應(yīng)滯后

別忽略單位轉(zhuǎn)換：

車速單位：km/h→m/s（除以3.6）

角度單位：弧度→度（算法算的是弧度，CAN總線發(fā)的是度）

簡(jiǎn)單算法的“生存智慧”

純跟蹤算法就像自動(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)題。

下次坐園區(qū)無(wú)人車時(shí)，不妨觀察一下它過(guò)彎的路線——說(shuō)不定，你就能看出純跟蹤算法的“影子”呢。

- End -

免責(zé)聲明：

凡本公眾號(hào)注明“來(lái)源：XXX（非智車科技）”的作品，均轉(zhuǎn)載自其它媒體，轉(zhuǎn)載目的在于傳遞和分享更多信息，并不代表本平臺(tái)贊同其觀點(diǎn)和對(duì)其真實(shí)性負(fù)責(zé)，版權(quán)歸原作者所有，如有侵權(quán)請(qǐng)聯(lián)系我們刪除。

       原文標(biāo)題 : 從原理到落地：純跟蹤算法如何讓自動(dòng)駕駛車“走直線”？