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

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

（上海工程技術大學，上海 201620）

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

1 前言

近年來，隨著自動駕駛和無人駕駛技術的發展，車輛自身控制的安全性和穩定性研究一直備受關注。路徑跟蹤是其中的重要環節之一，直接決定了車輛自身控制的安全性和穩定性。

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

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

2 轉向幾何模型

2.1 自行車運動模型

無人車系統的運動學模型相對于無人機或機械臂運動模型來說更簡潔，但并非線性系統，應用于具體的控制算法時，有必要對原始的運動學模型進行線性化。本文使用自行車運動學模型，如圖1所示。

圖1 自行車運動學模型

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

2.2 純跟蹤控制模型

純追蹤算法的運動模型基于自行車模型得到，如圖2所示。

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

圖2 純跟蹤算法運動學模型

式中，為后輪轉向時所對應的曲率。

由自行車運動模型可得：

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

2.3 Stanley控制模型

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

圖3 Stanley算法運動學模型

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

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

由式（6）可看出，當橫向誤差不存在或者無窮小時，有：

當航向角偏差不存在或者無窮小時，有：

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

Stanley 方法的特點是跟蹤效果主要受到增益的影響，并且擁有前饋控制，能夠提前補足轉彎過程中的車輛轉角，可根據設計需求對前饋值進一步增加增益系數。但當在合適取值范圍內且速度不變時，與跟蹤精度正相關，與平滑度負相關。

3 融合控制算法

3.1 融合算法原理

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

圖4 融合算法運動學模型

對模型的車輛行駛環境進行如下假設：不考慮阿克曼轉向幾何，忽略輪胎硬度和側滑，不考慮車輛懸架特性、道路阻力以及俯仰角，近似認為汽車自身驅動與制動之間的相互作用是線性平滑的理想狀態。

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

當橫向誤差不存在或者無窮小時，有：

當航向角偏差不存在或者無窮小時，的計算與式（7）相同。

則融合算法的車輪轉角為：

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

3.2 融合算法仿真

由于該算法受橫向誤差影響較大，當路徑點不連續時，跟蹤路徑的平滑性會受到更大影響，對于外界刺激較為敏感。為此，直接以影響較大的點路徑（Point Way）作為試驗對象，測試該融合算法的性能。

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

圖5 車速20 km/h仿真結果

圖6 車速40 km/h仿真結果

圖7 車速60 km/h仿真結果

圖8 車速80 km/h仿真結果

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

圖9 k、Ld與速度的關系

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

圖10 權重與速度的關系

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

4 實車試驗

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

圖11 試驗車輛

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

圖12 記錄軌跡示意

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

表1 20 km/h車速下Stanley算法的試驗結果

表2 20 km/h車速下融合算法的試驗結果

實車試驗時和的實際值與仿真值存在差異，如表3所示。

表3 20 km/h車速下的實車參數

試驗結果表明，2種算法均存在一些車輪轉角突變值，這是由于攝像頭在采集車道信息時的一些外在因素，如車道線磨損嚴重等，導致識別不到較為準確的車道線，影響了航向角的判斷。為了抑制突變產生的突然轉向，設置轉角變化范圍超過6°的轉角不會發送到控制端。

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

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

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

圖13 軌跡示意

圖14 實測軌跡

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

5 結束語

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