基于改進Stanley算法的目標假車路徑跟蹤控制

李文禮，易 帆，封坤，王 戡，張智勇

（1.重慶理工大學汽車零部件先進制造技術教育部重點實驗室，重慶 400054；2.招商局檢測車輛技術研究院有限公司，重慶 401122）

0 引言

近幾年，自動駕駛產業發展迅猛，自動駕駛功能測試需求也在不斷增長，在智能車場地測試中，基于安全性和測試效率的考慮，通常使用軟目標車作為目標車輛參與測試，軟目標車由目標假車與目標假車搭載平板組成［1］。

國內外軟目標車是隨著ADAS測試需求的增長而不斷發展的。2013年6月，Euro NCAP指定的AEB系統測試規程中規定了軟目標車選用標準以及對AEB測試場景的要求，如圖1所示Euro NCAP最初使用的軟目標車是一種具有車輛外形的氣球車［2］，該軟目標車主要用來模擬前車尾部，具有一定的柔韌性，在一定的測試范圍內可以避免對測試車輛造成破壞。由導航車提供動力，在發生碰撞時導軌能按照預定軌跡滑行，以此吸收碰撞產生的能量和沖擊。為提升測試效率，英國Moshon Data公司在軟目標車上加裝了拉繩和拉桿以提升軟目標車安裝速度。2018年5月，Euro NCAP提高了軟目標車的測試要求［3］。如圖2所示為美國DRI公司生產的軟目標車［4］，相比于EVT，美國DRI公司生產的軟目標車的動力系統由底部的移動平臺車提供，該移動平臺車裝置由高精度GPS設備和控制裝置組成，能夠實現高精度的路徑跟蹤，且當測試車輛與軟目標車發生碰撞時（如圖3所示），被試車輛能夠很容易碾壓上移動平臺車，保護被試車輛不與軟目標車金屬結構發生碰撞，從而避免對被試車輛造成損壞。

圖2 DRI公司生產的軟目標車

圖3 被試車輛與軟目標車發生碰撞

軟目標車眾多技術指標中最重要的一環就是能夠在場地測試中較真實地模擬出各種測試場景和工況，即目標假車搭載平臺能夠配合搭載的3D軟體目標假車，通過相應的控制策略使軟目標車能夠按照預設的軌跡，完成交叉路口穿行、切入切出等多種復雜的測試工況。

軟目標車的路徑跟蹤主要包括縱向的速度跟隨控制以及橫向的軌跡跟隨控制。在縱向控制方面，常用的控制方法有比例積分微分控制（PID）、滑?？刂疲⊿MC）和模型預測控制（MPC）等。其中，PID控制因其簡單實用且不需要依賴具體模型的優點，廣泛應用于智能車的縱向控制。尹燕莉等［5］提出一種基于PID與Q-Learning的混合動力汽車隊列分層控制策略，并通過在線仿真驗證了該控制策略的有效性和工況適應性；Zennir等［6］在控制器設計中引入車輛橫擺角速度，顯著提高了控制性能；Han等［7］提出一種自適應PID神經網絡路徑跟蹤控制策略，通過遺忘因子最小二乘算法辨識模型參數，通過神經網絡算法調控控制器參數。在橫向控制方面，常用的控制方法可大致分為3類：第1類是以幾何學模型為基礎的控制算法，有Stanley算法、純跟蹤算法等，第二類是以運動學為基礎的控制算法，有模型預測控制、反饋線性化等，第3類則是以運動學與動力學為基礎的控制算法，有滑?？刂?、線性二次調節以及模型預測控制等［8］。靳欣宇［9］綜合考慮車輛橫向誤差、航向角偏差以及側向加速度等因素，提出了一種基于Stanley的自適應最優預瞄模型；王鑫等［10］在Stanley算法模型的基礎上提出了一種融合純跟蹤計算前輪轉角方式的融合算法，該算法基于航向角、橫向偏差、車輛軸距、前視距離等得到前輪轉角信息；Shen等［11］基于運動學和動力學模型，設計了基于LPV的模型預測控制（MPC）和基于線性矩陣不等式（LMI）的線性二次型調節器（LQR），分別以運動學變量和動力學變量的形式跟蹤期望路徑。

面向智能車場地測試開發了一種軟目標車，用來代替實車作為目標車輛參與測試，用于評估ADAS車輛的主動安全性。同時，對軟目標車的軌跡跟蹤控制方法進行了研究，在縱向控制方面，采用粒子群優化算法優化PID控制器，得到最優控制參數，對軟目標車速度跟蹤效果進行研究；在橫向控制方面，以橫向誤差與前輪轉角控制量組成適應度函數，采用遺傳算法優化Stanley控制器參數，得到最優知識庫，采用模糊控制的方法來自適應調節控制器參數以提高路徑跟蹤的精準性與穩定性，并進行了軟目標車實車實驗，以驗證算法的有效性。

1 軟目標車軌跡跟蹤控制方法研究

軟目標車的軌跡跟蹤控制根據上位機軟件制定并下發的目標路徑信息規定軟目標車的運行軌跡。按照軟目標車通過慣導等傳感器獲得的自身位姿信息，通過優化算法調整車輛輸入即前輪轉角與驅動電機扭矩的大小，通過CAN通訊將計算所得的期望前輪轉角與電機扭矩傳遞至執行機構，實現對目標軌跡的跟蹤。

1.1 目標假車搭載平臺轉向系統建模

智能車的路徑跟蹤首先需要對車輛模型進行分析，按照當前車輛參數，給予一定的優化算法控制調節轉向執行系統，使智能車能夠沿著規劃好的期望路徑平穩行駛，同時盡可能地減少誤差，確?？刂凭?。復雜的模型并不一定是最好的選擇，對車輛運動學模型進行合理簡化并且選擇行使工況的約束條件才是研究的重點，能夠表達控制對象的真實物理限制即可，因此，需要對軟目標車的車輛模型進行相應的模型簡化。

結合軟目標車的結構特點，做以下假設：1）軟目標車通過轉向推桿來控制轉向輪，假定轉向輪的左右側車輪轉角大致相同，左右輪等效為單個輪子，將軟目標車模型簡化為兩輪模型；2）忽略軟目標車在緊急制動及急加速等情況下造成的軸荷轉移；3）軟目標車后輪無法偏轉，僅通過前輪控制轉向，因此，后輪轉角δr近似零；4）不考慮軟目標車在垂直方向的運動，假設車輛的運動是一個二維平面上的運動；簡化后軟目標車轉向模型如圖4所示。

圖4 軟目標車轉向模型示意圖

結合后輪轉角δr輸入大致為零，軟目標車對于轉角的輸入反饋到前輪，前輪轉角可表達為

式中：L為軟目標車的軸距，R為軟目標車的轉彎半徑。

1.2 軟目標車橫向控制算法研究

1.2.1 Stanley算法

Stanley算法最開始被應用到斯坦福大學的智能駕駛車上，是一種基于橫向跟蹤誤差的非線性反饋函數，也被稱為前輪反饋控制，能夠實現跟蹤誤差指數收斂于0。

Stanley算法原理如圖5所示，輸出的前輪轉角的控制量由橫向誤差引起的轉角θe與航向誤差引起的轉角δδe兩部分組成。

圖5 Stanley原理示意圖

在不考慮橫向誤差ey的前提下，要使車輛能夠沿著期望路徑行駛，則前輪轉角應與參考路徑點的切線方向一致。則在不考慮橫向誤差的情況下，有：

式中：θe為車輛航向與期望路徑最近路徑點切線方向之間的夾角。

在不考慮航向誤差的前提下，橫向跟蹤誤差越大，即ey越大，則控制量轉角越大。假設經過d（t）距離，車輛與期望路徑重合，則根據圖中幾何關系得出如下非線性比例函數：

綜合考慮航向及橫向誤差，可得前輪轉角：

由圖5可知橫向誤差變化率：

當橫向誤差ey很小時，由上式可以得：

因此，橫向誤差收斂于指數e（t），增益參數k決定了收斂速度，是系統收斂的關鍵參數。

1.2.2 基于遺傳算法的橫向控制器參數優化

如上文所分析，說明了增益參數k會對控制器的跟蹤效果具有重要影響，增益參數k的值越大，控制精度越高，但過高時會引起劇烈的擺動。針對Stanley控制器，選擇合適的增益參數k至關重要。因此，引入全局搜索能力較高的遺傳算法來解決對增益參數k的優化問題。

1）遺傳算法概述

遺傳算法（genetic algorithm，GA）是受自然進化理論啟發由進化論和遺傳學機理而產生的搜索算法，從任一初始種群出發，通過隨機選擇、交叉和變異操作，產生一群更適合環境的個體，使群體進化到搜索空間中越來越好的區域，這樣一代一代的不斷繁衍進化，最后收斂到一群最適應環境的個體，從而求得問題的優質解［12］，其流程如圖6所示。

圖6 遺傳算法優化流程示意圖

遺傳算法主要有以下4個操作步驟：

①在規定的區域內，隨機產生初試初始種群；

②計算當前種群內的個體是否適合當前的環境，即得到適應度值；

③依據得到的適應度值選擇、淘汰一些個體，再通過交叉以及變異等操作得到新的種群；

④重復②和③步驟，直到完成迭代次數。

2）基于遺傳算法的參數優化方法

由于遺傳算法無法直接處理控制模型中的待優化參數，因此需要對實際問題進行編碼，將待優化參數進行編碼處理。常用編碼方式有二進制編碼法與浮點數編碼法（實數編碼法）。相比于二進制編碼的方式，實數編碼法能夠有效地改善遺傳算法的計算復雜性，適用于求解精度要求較高的問題［13］，考慮到路徑跟蹤對于計算精度要求較高，所以選擇以實數編碼的方式產生初始種群。

遺傳算法基本不利用外部信息，僅通過適應值來確定目標個體是否適應當前環境，是否判定為優秀，并基于適應度值作為優勝劣汰的指標，因此，適應度函數選擇很大程度上影響了遺傳算法的綜合性能。

在遺傳算法的適應度函數設計上考慮橫向誤差的同時也要兼顧智能車的操縱穩定性，因此，建立以下性能指標：

式中：α和β均為系數，且滿足α＋β＝1；為跟蹤橫向誤差的平均值；為輸出轉交的標準差。

基于性能指標J，將其轉化為適應度函數用來評價個體的優劣程度。當J的取值越小時，則適應度值應當越大，此時算法的跟蹤效果最優，因此，適應度函數可以表示為

遺傳算法主要參數設置如表1所示。

表1 遺傳算法參數

1.2.3 自適應模糊控制系統設計

如上文所述，Stanley算法的控制效果受增益參數k的影響，在不同的道路與速度下，需要匹配不同的控制參數k才能達到最好的跟蹤效果。因此，針對軟目標車需要能夠以不同車速在不同曲率的道路下行駛的工作需求，本文中設計以道路曲率與軟目標車車速為輸入，增益參數k為輸出的模糊控制器，并通過遺傳算法獲得模糊控制參數表，以實現增益參數k的自適應調整。

將輸入量定義為5個子集表示｛左中，左小，零，右小，右中｝，對應到模糊語言集｛NM，NS，ZO，PS，PM｝（模糊子集對應含義分別為NM（negative middle）＝左中，NS（negative small）＝左小，ZO（zero）＝0，PS（positive small）＝右小，PM（positivemiddle）＝右中）。

為降低模糊控制器的整體復雜程度，選擇三角函數與梯形函數2種曲線形式的隸屬度函數作為輸入與輸出變量的隸屬度函數，得到各輸入與輸出的隸屬度函數，如圖7所示?；谶z傳算法優化尋找不同輸入下最優的增益參數，得到相應模糊控制的規則表如表2所示。

表2 模糊控制規則

圖7 模糊控制隸屬度函數

1.3 軟目標車縱向控制方法研究

1.3.1 PID控制器

PID控制器通過比較被控對象的實時信息與目標值的誤差的比例、積分和微分獲得控制量，實現對受控對象的控制。

PID控制器的控制效果會受到kp、ki、kd影響［14］，但傳統的人工經驗調整參數的方法，無法確保PID控制器速度跟隨的效果，且費時費力，基于此采用粒子群優化的方法優化PID參數。

1.3.2 粒子群優化算法

粒子群優化算法（particle swarm optimization，PSO），模擬鳥類捕食過程中的群體行為，最終找到群體最優捕食點。用一個粒子模擬鳥類個體，該粒子的當前位置即為對應優化問題的一個候選解，飛行過程即為該個體的搜尋過程，飛行速度會根據粒子的歷史最優位置與種群中的粒子最優位置進行動態調整。不斷迭代，更新速度和位置，得到最優解［15］。PSO優化流程如圖8所示。

圖8 PSO優化流程示意圖

a）初始化粒子群參數：設定粒子種群個數N＝30，PID控制器待優化參數為3個，因此解空間維度為三維，對粒子進行初始化，包括每個粒子的隨機速度和位置，并設置中止條件。

b）更新粒子的適應度：依據適應度函數更新粒子的當前適應度。

取ITAE指標作為PSO算法的適應度函數，即時間乘以系統誤差的絕對值作為適應度函數。

c）更新個體極值：根據計算所得適應度值，比較當前粒子與個體最優位置適應度，若更優，則更新當前個體最優位置。

d）更新全局極值：根據計算所得適應度值，比較當前粒子與全局最優位置適應度，若更優，則更新當前全局最優位置。

e）更新速度和位置：通過式（12）和式（13）更新每個粒子的速度與位置。

f）檢查是否滿足結束條件：重復b）—f）步驟，直到達成終止條件。

2 仿真結果和分析

為驗證本文中提出的軟目標車軌跡跟蹤方法的可行性、有效性和適應性，搭建Carsim-Matlab／Simulink聯合仿真模型，由Carsim提供車輛動力學模型，Matlab／Simulink搭建車輛橫縱向控制算法模型。設計了3種典型工況，分別在恒定參考車速下控制不同路段以驗證算法的有效性，設計有多曲率綜合工況，以驗證模糊控制算法的自適應性。

2.1 換道工況仿真分析

在換道的仿真實驗中，將期望車速設置為10 m／s的恒定車速。圖9（a）展示了模糊Stanley算法和傳統的Stanley算法在全局換道路徑下的追蹤效果，結果顯示：2種算法在車速為10 m／s時均能較好地完成車輛換道的路徑追蹤任務。但是如圖9（b）橫向誤差對比圖所示，傳統Stanley算法的最大橫向誤差絕對值約為0.17 m，而模糊Stanley算法的最大橫向誤差絕對值約為0.05 m，橫向偏離的程度與幅度均要小于傳統Stanley算法。在速度跟蹤上，如圖9（c）所示經過粒子群優化后的PID算法在收斂速度與穩定性上均要優于未進行優化的PID算法。

圖9 換道路徑仿真結果

綜上所述，模糊Stanley算法在跟蹤換道路徑時的性能要優于傳統的Stanley算法，經過粒子群優化后的PID算法也能夠更好地實現對速度的跟蹤。

2.2 S彎工況仿真分析

在S彎工況的仿真實驗中，分別考慮了5、10 m／s以及15 m／s車速下模糊Stanley控制算法的軌跡跟蹤效果，以驗證不同車速下在面對曲率方向突然變化時模糊Stanley的自適應性，并以10 m／s為例對比了模糊Stanley與傳統Stanley的橫向誤差與前輪轉角。圖10（a）展示了不同車速下模糊Stanley算法和車速為10 m／s的傳統Stanley算法在全局換道路徑下的追蹤效果，在曲率發生突變時的跟隨誤差會隨著車速的增加而變大，當車速為15 m／s時，此時的最大誤差約為0.56 m。

圖10 S型道路仿真結果

圖10（b）與圖9（c）則為車速為10 m／s時模糊Stanley與傳統Stanley的橫向誤差與前輪轉角。從圖中可以看出，相比于模糊Stanley算法，固定權重的Stanley算法在曲率發生突變時，受前輪轉角突變影響，其橫向偏差會出現一定程度的振蕩現象，且誤差收斂時間較長，而經過模糊調節的Stanley控制器可以快速收斂跟蹤誤差，無較大浮動的振蕩現象，最大的橫向偏差約為0.06 m，路徑跟蹤的精度與穩定性都比較好，能夠滿足精度要求。

2.3 U型彎工況仿真分析

選取U型彎工況來模擬真實駕駛環境中的掉頭動作，驗證控制算法在較大曲率軌跡下跟隨目標參考軌跡的能力。將目標車速設置為3 m／s，圖11（a）、（b）分別為期望路徑為U型彎時的全局路徑與橫向誤差。相較于Stanley控制器，在跟蹤大區率路徑時模糊Stanley控制器具有較好的路徑跟蹤性能。

圖11 U型彎路徑仿真結果

2.4 綜合工況仿真分析

設計包含有直線、換道、直角彎、S彎以及U型彎的多曲率工況，曲率如圖12（a）所示，以驗證算法的自適應性。

圖12 綜合工況仿真結果

圖12（b）為車速跟隨。經過粒子群優化后的PID控制器在車速發生變化時能夠更快地收斂到期望車速，超調量較小，控制過程平穩。

從圖12（c）與圖12（d）可以看出，2種控制器的控制誤差都在一定范圍內，其中模糊Stanley控制器的橫向誤差控制在較小的范圍內，最大誤差大約為0.1 m，整體控制效果要優于Stanley控制器，并且在曲率突變處模糊Stanley能夠較快地達到收斂。從圖12（f）航向角可以看出，模糊Stanley控制器的航向角曲線較為平滑，這也表明模糊Stanley控制器的穩定性要高于Stanley算法。

3 智能車場地測試用軟目標車實車試驗

3.1 軟目標車試驗平臺搭建

3.1.1 軟目標車硬件部分

所開發的軟目標車由目標假車與目標假車搭載平臺兩部分組成，如圖13所示。為避免在車輛測試中損傷被試車輛，目標假車采用泡沫板材質及雷達反射材料制成，在與被試車輛發生碰撞時，目標假車就會被推開或散開，防止或減輕對被試車輛造成的損壞，且在視覺和雷達等傳感器識別特性上與實車一致［16－17］。

圖13 開發的軟目標車

假車搭載平臺通過裝載假車目標物按照規定的行駛條件（速度、路徑、減速度、加速度等）運動，模擬道路實車目標，構建復雜的道路測試場景，滿足汽車的ADAS功能和自動駕駛功能測試。

為保證最低的雷達反射，同時避免被試車輛與目標假車搭載平臺的金屬結構發生碰撞，目標假車搭載平臺的設計高度較低，且目標假車搭載平臺的外部框架的裙部采用傾斜設計，以減輕被試車輛通過時的沖擊，當被試車輛碾壓到目標假車搭載平臺上后，搭載平臺內置的懸架系統會受到壓力作用而縮進平臺內部，保證在一定的測試范圍內不會對被試車輛造成破壞。

圖14所示為假車搭載平臺的整體結構，包括慣性導航系統、驅動系統、轉向系統、剎車總成、動力電池、無線通訊系統以及控制系統等。假車搭載平臺的驅動輪被串聯在同一根轉軸上，形成三輪結構，避免了轉速差對轉向系統的干擾，降低方向控制的難度，且由2個高性能伺服電機通過鏈傳動帶動車輪轉動，為軟目標車提供動力；該平臺車轉向輪由一套高精度單軸驅動器帶動，并通過其連桿機構實現車輪轉向的控制；在制動方面，設計時為每個輪子都設計有獨立的剎車機構，確保測試時能夠提供足夠的制動力矩。

圖14 目標假車搭載平臺整體結構示意圖

3.1.2 軟目標車系統軟件開發

要想實現軟目標車的路徑跟蹤效果，還需要在軟目標車的上位機、下位機（即工控機）上開發軟件，以實現系統的各部分協調運行。

上位機控制操作平臺主要實現期望路徑設置與下發、與下位機之間的數據交互，以及能夠在上位機中的控制操縱平臺中實時顯示并存儲下位機傳輸的數據，以便對實驗過程中出現的異常狀態進行檢測。

下位機控制操作軟件平臺主要結構如圖15所示，該軟件平臺能夠以多線程多任務的方式通過串口、CAN通信以及Wi－Fi通信的方式與定位導航定位模塊，底層控制模塊以及上位機之間進行通信。下位機平臺通過慣導數據得到精確的車輛實時狀態，并將結合車輛的實時狀態與上位機獲得的期望路徑數據輸出前輪轉角、加速度等控制信號，之后通過CAN通信實現對轉向推桿、剎車電機與驅動器電機的控制。

圖15 軟目標車下位機部分主要結構示意圖

3.2 試驗結果分析

考慮仿真環境與真實環境的差異性，進行實車試驗來驗證所設計的路徑跟蹤控制器在實際應用中的控制性能，實車試驗分為直線路徑跟蹤試驗與曲線路徑跟蹤試驗，受場地限制且基于安全性考慮，直線試驗車速為36、43.2、54 km／h，而曲線試驗的最大車速則不超過30 km／h。

3.2.1 直線路徑跟蹤試驗

期望路徑為RT Range所建立的本地坐標系下從點（0，0）到點（150，0）的直線路徑，圖16（a）即為直線路徑實車跟隨結果，圖16（b）為跟蹤誤差定量分析示意圖。車速為36、43.2 km／h時的實際路徑與期望軌跡一致，其中車速為36 km／h時的平均誤差大約為0.024 m，跟蹤誤差極值為0.044 m；車速為43.2 km／h時的平均誤差大約為0.029 m，跟蹤誤差極值為0.054 m。車速為54 km／h時，在開始階段車輛和目標路徑存在偏差，經過一段距離的行駛后誤差逐漸減小，平均誤差大約為0.041 m，跟蹤誤差極值為0.123 m，整體跟蹤效果較差，但總體偏差仍在有效范圍內。

圖16 直線路徑實車跟蹤結果和誤差分析

3.2.2 曲線路徑跟蹤試驗

曲線路徑實車跟蹤結果如圖17所示，選擇的期望曲線路徑有5次多項式生成的換道曲線與S型類sin曲線。由圖17（a）換道曲線實車跟蹤結果與圖17（e）S型曲線實車跟蹤結果可知，軟目標車在進行曲線路徑跟蹤時，整體跟蹤效果良好，但在曲率較大或曲率突變處跟蹤誤差較大。

圖17 曲線路徑實車跟蹤結果和誤差分析

如圖17（b）換道曲線實車跟蹤誤差與圖17（c）跟蹤誤差定量分析示意圖所示，當車速為10、20、30 km／h時的換道曲線跟蹤誤差平均值分別為0.035、0.040、0.068 m，跟蹤誤差極值分別為0.07、0.10、0.21 m，極值點均出現在換道結束后的曲率突變處，但隨后誤差便逐漸消除；如圖17（e）S型曲線實車跟蹤誤差與圖17（f）跟蹤誤差定量分析示意圖所示，當軟目標車車速為10、20、30 km／h時的跟蹤誤差平均值分別為0.060、0.069、0.104 m，跟蹤誤差極值分別為0.083、0.110、0.233 m，極值點出現在曲率最大處。2種曲線路徑跟蹤誤差極值均在路徑跟蹤的接受范圍內，且隨后便能迅速消除跟蹤誤差。

由以上試驗結果可知，本文中所提的路徑跟蹤控制方法具有一定的可行性和有效性，能夠滿足軟目標車場地測試需求。

4 結論

針對智能汽車封閉場地測試需求，開發了一種用于智能汽車場地測試的軟目標車，采用橫縱向綜合控制策略解決軟目標車的路徑跟蹤精度問題。通過粒子群優化縱向PID控制器調整速度誤差，控制軟目標車驅動和制動；在橫向控制方面，提出一種改進的模糊Stanley路徑跟蹤控制算法，基于遺傳算法得到Stanley控制最優參數及最優知識庫，利用模糊控制算法實現Stanley控制參數自適應控制的效果。仿真結果與實車試驗結果表明：所提出的方法有效地提高了軟目標車按照預設的GPS軌跡行駛的跟蹤精度，滿足軟目標車場地測試需求。由于場地原因及時間限制等因素，本研究仍存在一些需要改進的地方，如在構造增益參數的適應度函數時，僅考慮了跟蹤誤差與轉向角，在未來的工作中可考慮構建多維度的適應度函數，同時考慮縱橫向控制系統之間的耦合，對橫縱向綜合控制展開研究，并在條件允許的情況下進行更高車速的軟目標車實車試驗。