基于改進(jìn)Stanley算法的無人車路徑跟蹤融合算法研究*

王鑫 凌銘 饒啟鵬 劉暢 翟樹龍

（上海工程技術(shù)大學(xué)，上海 201620）

主題詞：無人駕駛 路徑跟蹤 純跟蹤算法 斯坦利算法 融合算法

1 前言

近年來，隨著自動(dòng)駕駛和無人駕駛技術(shù)的發(fā)展，車輛自身控制的安全性和穩(wěn)定性研究一直備受關(guān)注。路徑跟蹤是其中的重要環(huán)節(jié)之一，直接決定了車輛自身控制的安全性和穩(wěn)定性。

自1980 年卡內(nèi)基梅隆大學(xué)無人駕駛車出現(xiàn)至今，路徑跟蹤控制模型的研究有了長足的發(fā)展。早期的控制算法以PID 反饋控制為主，后來出現(xiàn)了多種控制算法，如：模型預(yù)測控制（Model Predictive Control，MPC）算法能夠較為有效地解決線性和非線性控制系統(tǒng)中的約束問題，線性二次型調(diào)節(jié)器（Linear Quadratic Regulator，LQR）算法也能利用模型進(jìn)行行為預(yù)測，然后實(shí)現(xiàn)對預(yù)測結(jié)果的跟蹤。也有學(xué)者提出基于幾何關(guān)系的模型控制算法：文獻(xiàn)[9]提出了一種基于斯坦利（Stanley）算法的自適應(yīng)最優(yōu)預(yù)瞄模型，文獻(xiàn)[10]利用GPS 獲取位置信息，采用純跟蹤（Pure Pursuit）算法實(shí)現(xiàn)路徑跟蹤。以上控制算法均以控制跟蹤精度為主要目標(biāo)，忽略了無人車對跟蹤平滑性的要求，乘坐舒適性有待提高。

本文基于純跟蹤算法在路徑跟蹤時(shí)對車輪轉(zhuǎn)角的計(jì)算原理，在Stanley算法的控制模型上進(jìn)行改進(jìn)，提出一種融合算法，在不同車速下，依據(jù)橫向距離、航向角、合適的預(yù)瞄距離等車輛參數(shù)，得到當(dāng)前狀態(tài)下合適的車輪轉(zhuǎn)角，在保證跟蹤精確度的情況下，能夠保證良好的跟蹤路徑平滑性，以提高乘坐舒適性。

2 轉(zhuǎn)向幾何模型

2.1 自行車運(yùn)動(dòng)模型

無人車系統(tǒng)的運(yùn)動(dòng)學(xué)模型相對于無人機(jī)或機(jī)械臂運(yùn)動(dòng)模型來說更簡潔，但并非線性系統(tǒng)，應(yīng)用于具體的控制算法時(shí)，有必要對原始的運(yùn)動(dòng)學(xué)模型進(jìn)行線性化。本文使用自行車運(yùn)動(dòng)學(xué)模型，如圖1所示。

圖1 自行車運(yùn)動(dòng)學(xué)模型

圖1中，為車輛前輪的轉(zhuǎn)向角，為軸距，為后輪對應(yīng)的轉(zhuǎn)彎半徑。該運(yùn)動(dòng)模型實(shí)際上由阿克曼轉(zhuǎn)向幾何簡化得到，將四輪車輛模型簡化為兩輪單軌模型。該運(yùn)動(dòng)模型一般采取以下假設(shè)：車輛只在水平面上運(yùn)動(dòng)，不考慮空氣阻力、輪胎的側(cè)滑和車輛的俯仰角，車輛的前輪轉(zhuǎn)向時(shí)，內(nèi)、外車輪轉(zhuǎn)動(dòng)相同的角度。采用自行車模型能夠簡化無人車的前輪轉(zhuǎn)向角與后輪轉(zhuǎn)向曲率之間的幾何關(guān)系，其關(guān)系可表示為：

2.2 純跟蹤控制模型

純追蹤算法的運(yùn)動(dòng)模型基于自行車模型得到，如圖2所示。

該算法的核心思想是：以車輛后輪中心點(diǎn)為參考點(diǎn)，通過控制前輪的轉(zhuǎn)向角度來控制車輛沿一段圓弧行駛，使后輪的中心點(diǎn)經(jīng)過路徑上的預(yù)瞄點(diǎn)。圖2中，()為預(yù)瞄點(diǎn)坐標(biāo)，為車輛后軸心與預(yù)瞄點(diǎn)的連線與當(dāng)前航向之間的夾角，為車輛后軸中心點(diǎn)到預(yù)瞄點(diǎn)的距離，即前視距離。根據(jù)正弦定理可得：

圖2 純跟蹤算法運(yùn)動(dòng)學(xué)模型

式中，為后輪轉(zhuǎn)向時(shí)所對應(yīng)的曲率。

由自行車運(yùn)動(dòng)模型可得：

由此建立了與、、之間的關(guān)系。該算法主要影響參數(shù)為，其與車速總體呈正相關(guān)，且需約束其取值范圍。

2.3 Stanley控制模型

Stanley算法的控制模型原理如圖3所示。

圖3 Stanley算法運(yùn)動(dòng)學(xué)模型

該算法核心思想也是對車輛前輪進(jìn)行控制，通過消除橫向偏差來實(shí)現(xiàn)路徑跟蹤。其控制原理可表示為：

式中，為航向角，是車輛的前后軸心連線方向與距離車前輪軸心最近的軌跡點(diǎn)的切線方向之間的夾角；為車速；為橫向誤差；為可進(jìn)行調(diào)節(jié)的控制量增益。

由式（6）可看出，當(dāng)橫向誤差不存在或者無窮小時(shí)，有：

當(dāng)航向角偏差不存在或者無窮小時(shí)，有：

由此可見，此算法對于被控量的影響主要在于航向角和橫向誤差。在值一定，車速不變的前提下，橫向誤差和航向角增大，則算法會(huì)增大車輪的轉(zhuǎn)角控制量，進(jìn)而減小誤差實(shí)現(xiàn)軌跡跟蹤。

Stanley 方法的特點(diǎn)是跟蹤效果主要受到增益的影響，并且擁有前饋控制，能夠提前補(bǔ)足轉(zhuǎn)彎過程中的車輛轉(zhuǎn)角，可根據(jù)設(shè)計(jì)需求對前饋值進(jìn)一步增加增益系數(shù)。但當(dāng)在合適取值范圍內(nèi)且速度不變時(shí)，與跟蹤精度正相關(guān)，與平滑度負(fù)相關(guān)。

3 融合控制算法

3.1 融合算法原理

融合算法的控制模型原理如圖4所示。

圖4 融合算法運(yùn)動(dòng)學(xué)模型

對模型的車輛行駛環(huán)境進(jìn)行如下假設(shè)：不考慮阿克曼轉(zhuǎn)向幾何，忽略輪胎硬度和側(cè)滑，不考慮車輛懸架特性、道路阻力以及俯仰角，近似認(rèn)為汽車自身驅(qū)動(dòng)與制動(dòng)之間的相互作用是線性平滑的理想狀態(tài)。

分析Stanley 算法和純跟蹤算法可知，式（4）和式（6）中的車輪轉(zhuǎn)角都是由于航向角所引起的控制量的變化。控制量的影響因素相同，則為兩者分配不同的權(quán)重、，且+=1，只要找到合適的、即可使效果疊加后，既能保證車輪轉(zhuǎn)角的輸出結(jié)果不變或變化甚微，又能使平滑性相對可控。

當(dāng)橫向誤差不存在或者無窮小時(shí)，有：

當(dāng)航向角偏差不存在或者無窮小時(shí)，的計(jì)算與式（7）相同。

則融合算法的車輪轉(zhuǎn)角為：

由Stanley 算法可知，增大以提高精確度，但平滑度降低；由純跟蹤算法可知，增大可增加平滑度，但精確度會(huì)降低。則式（9）可以通過同時(shí)增大和使平滑性和精確度能夠相互得到補(bǔ)償，并調(diào)整影響因子和的值使平滑性和精確度維持在相對平衡的水平上，在不損失精度或精度損失甚微的情況下，保證較好的平滑性。同時(shí)由式（6）和式（8）可知，融合算法能夠減弱航向角在檢測過程中因存在較大誤差導(dǎo)致車輪轉(zhuǎn)角發(fā)生突變的負(fù)面影響。

3.2 融合算法仿真

由于該算法受橫向誤差影響較大，當(dāng)路徑點(diǎn)不連續(xù)時(shí)，跟蹤路徑的平滑性會(huì)受到更大影響，對于外界刺激較為敏感。為此，直接以影響較大的點(diǎn)路徑（Point Way）作為試驗(yàn)對象，測試該融合算法的性能。

基于點(diǎn)路徑分別測試原Stanley 算法和融合算法在相同路徑和速度下的車輛的最優(yōu)跟蹤效果，比較分析不同車速下2 種算法在最優(yōu)跟蹤效果前提下的跟蹤精確度和平滑性的優(yōu)劣，在20～80 km/h 車速范圍內(nèi)，車速每變化10 km/h 進(jìn)行一次仿真，部分仿真跟蹤結(jié)果如圖5～圖8 所示。

圖5 車速20 km/h仿真結(jié)果

圖6 車速40 km/h仿真結(jié)果

圖7 車速60 km/h仿真結(jié)果

圖8 車速80 km/h仿真結(jié)果

在原Stanley 算法和融合算法均處于最優(yōu)跟蹤效果的前提下，當(dāng)速度不超過40 km/h 時(shí)，Stanley 算法中的值隨速度提高呈增長趨勢，但速度在40 km/h 以上時(shí)，的值隨著速度的提高而減小。當(dāng)速度不同時(shí)，融合算法中值并無明顯變化規(guī)律，但前視距離與速度正相關(guān)。圖9 所示為不同車速下各參數(shù)的變化情況。

圖9 k、Ld與速度的關(guān)系

融合算法中隨著速度增大而減小，隨著速度增大而增大，二者變化較為規(guī)律，車速每變化10 km/h時(shí)，和的取值相應(yīng)變化約0.05，如圖10所示。

圖10 權(quán)重與速度的關(guān)系

仿真結(jié)果表明，2種算法均處于最精確的跟蹤條件時(shí)，不同速度下的仿真結(jié)果平滑性差異較大。Stanley算法在不連續(xù)的路徑點(diǎn)之間的跟蹤路徑呈折線化趨勢，振蕩頻率較高，點(diǎn)與點(diǎn)之間的過渡并不理想，想要維持較好的平滑性稍有困難。而在曲率較大的路徑段，平滑效果更差，甚至出現(xiàn)跟蹤精確度損失較大的情況。速度越高時(shí)，這一趨勢越明顯。而改進(jìn)后的融合算法在相同速度和相同路徑段能夠在維持其精確度的前提下，或者以微小的精確度損失為補(bǔ)償，使平滑性得到明顯提升。在曲率較大的路徑段，過渡更為自然平滑，精確度也極大改善。并且在路徑跟隨的初始階段，融合算法在減小振蕩的功能方面也能保持良好效果，也能夠快速進(jìn)入穩(wěn)定階段。

4 實(shí)車試驗(yàn)

試驗(yàn)采用的車輛如圖11 所示。車輛配有CAN 總線、工控機(jī)、攝像頭等。車輛的軸距為2.9 m，通過使用攝像頭采集路況信息，提取視頻流中采集到的車道線信息，得到融合算法中的航向角、橫向偏差等參數(shù)，然后通過融合算法得到實(shí)時(shí)車輪轉(zhuǎn)角，將轉(zhuǎn)角信息和速度信息通過CAN 總線發(fā)送給工控機(jī)，由工控機(jī)對轉(zhuǎn)向系統(tǒng)發(fā)送轉(zhuǎn)向指令，以實(shí)現(xiàn)路徑跟隨。

圖11 試驗(yàn)車輛

在直線道路上分別對Stanley 算法和改進(jìn)后的融合算法路徑跟蹤效果進(jìn)行試驗(yàn)。使車輛在車道線內(nèi)行駛，完成車道保持，車輛在跟蹤路徑過程中，質(zhì)心始終在車道線中心線附近，車輛未偏離車道線，如圖12 所示。

圖12 記錄軌跡示意

記錄每個(gè)時(shí)刻得到的車輪轉(zhuǎn)角，其正、負(fù)分別代表左、右轉(zhuǎn)向，統(tǒng)計(jì)其正負(fù)值交替次數(shù)，若在一定時(shí)間內(nèi)交替次數(shù)多，則說明車輛在跟隨路徑時(shí)振蕩頻率高，搖擺次數(shù)多，平滑性不佳。通過測量橫向偏差來判斷車輛的跟蹤精確度，正值表示車輛質(zhì)心在車道中心線左側(cè)，反之在右側(cè)。同時(shí)，在車輛穩(wěn)定跟蹤后，車輪轉(zhuǎn)角的變化范圍也能側(cè)面反映跟蹤精確度，變化范圍越小，說明橫向誤差越小，跟蹤越精確。受試驗(yàn)場地車速限制，并考慮無人車的道路安全性，試驗(yàn)速度設(shè)為20 km/h。Stanley 算法和融合算法每秒末的試驗(yàn)結(jié)果分別如表1、表2 所示，2 次試驗(yàn)的道路段相同，車輛初始位置相同。

表1 20 km/h車速下Stanley算法的試驗(yàn)結(jié)果

表2 20 km/h車速下融合算法的試驗(yàn)結(jié)果

實(shí)車試驗(yàn)時(shí)和的實(shí)際值與仿真值存在差異，如表3所示。

表3 20 km/h車速下的實(shí)車參數(shù)

試驗(yàn)結(jié)果表明，2種算法均存在一些車輪轉(zhuǎn)角突變值，這是由于攝像頭在采集車道信息時(shí)的一些外在因素，如車道線磨損嚴(yán)重等，導(dǎo)致識(shí)別不到較為準(zhǔn)確的車道線，影響了航向角的判斷。為了抑制突變產(chǎn)生的突然轉(zhuǎn)向，設(shè)置轉(zhuǎn)角變化范圍超過6°的轉(zhuǎn)角不會(huì)發(fā)送到控制端。

由表1 和表2 可知：試驗(yàn)前3 s 內(nèi)，在車輛啟動(dòng)的初始階段，由于車速不高，且車輛質(zhì)心偏離車道中心線較遠(yuǎn)，車輪校正角度較大，車輪轉(zhuǎn)角隨著車輛向車道中心線附近移動(dòng)而變??；在第3 s后，車輛質(zhì)心首次越過車道中心線，車輪轉(zhuǎn)角開始反向變化，以反饋控制的方式來修正跟蹤路徑，不會(huì)使車輛過于偏離預(yù)定軌跡，以達(dá)到精確跟蹤的目的。且車輪轉(zhuǎn)角在第5 s 后，融合算法的橫向偏差的絕對值明顯較原Stanley算法小，且車輪轉(zhuǎn)角變化幅度明顯減小，說明車輛質(zhì)心與車道中心線的距離較小，精確度較好。由此可見，改進(jìn)后的融合算法在維持跟蹤精確度方面表現(xiàn)較好。

但在第5 s 后，2 種算法在平滑度方面差異較大。Stanley 算法實(shí)車試驗(yàn)中除車輪轉(zhuǎn)角突變引起的車輪轉(zhuǎn)角正負(fù)值交替外，總有效次數(shù)為11次，而改進(jìn)后的融合算法總有效次數(shù)僅為5 次。且在后5 s 的時(shí)間段內(nèi)，車輪轉(zhuǎn)角突變只出現(xiàn)3次。由此可知，改進(jìn)后的融合算法相比原Stanley算法減弱了航向角檢測的大誤差直接帶來的車輪轉(zhuǎn)角的變化影響，不僅在一定程度上抑制了車輪轉(zhuǎn)角突變，而且平滑度有了顯著提升。

在實(shí)車試驗(yàn)過程中，為了得到較為清晰的車輛軌跡，通過在車輛右后輪的輪胎上撒水的方式來記錄軌跡，如圖13、圖14所示。其中，路段為校正階段，路段為穩(wěn)定跟蹤階段。

圖13 軌跡示意

圖14 實(shí)測軌跡

由圖14 可看出，改進(jìn)后的融合算法車輛軌跡振蕩頻率小，相對平滑且跟蹤精確。在較遠(yuǎn)的路徑中，也能維持一定的精度，不會(huì)很快脫離軌道。仿真結(jié)果和實(shí)車試驗(yàn)結(jié)果均能說明改進(jìn)后融合算法的可行性和優(yōu)越性。

5 結(jié)束語

本文通過分析Stanley 算法中航向角和橫向偏差對車輪轉(zhuǎn)角的影響，在斯坦利控制模型的基礎(chǔ)上，采用純跟蹤算法中航向角對車輪轉(zhuǎn)角的計(jì)算方式，提出了一種融合算法，基于航向角、橫向偏差、車輛軸距、前視距離來得到合適的車輪轉(zhuǎn)角信息。仿真和實(shí)車試驗(yàn)結(jié)果表明，改進(jìn)后的融合算法在精確度和平滑性方面均有更好的表現(xiàn)，在保證安全性的同時(shí)，提升了乘坐舒適性。