基于優化模型車輛路徑跟蹤控制研究

陳 思，韓 愈，唐 超，林 業

(天津科技大學機械工程學院，天津 300222)

自動駕駛是集人工智能、物聯網、大數據為一體的新技術，是汽車未來發展的主要趨勢之一．路徑跟蹤是實現自動駕駛的基礎，也是實現自動駕駛的核心技術[1]．

路徑跟蹤就是通過控制車輛的轉向使其沿著定義好的路徑行駛，近年來它一直是自主地面車輛研究的一個活躍領域．從控制理論的角度出發，路徑跟蹤的任務可以理解為設計橫向控制器不斷地生成車輛轉向指令來補償跟蹤偏差，從而使車輛可以安全平穩地行駛[2]．

目前，實現路徑跟蹤的主流方法可以分為兩類，基于幾何關系設計的幾何跟蹤法和基于模型控制的方法．幾何跟蹤法包括Pure Pursuit和Stanley方法等，基于模型控制的方法有MPC、LQR方法等．近年來，國內外學者都在路徑跟蹤領域進行了大量研究．李紅志等[3]提出一種預瞄時間自適應算法，根據不同的預瞄時間，預測將來一段時間內車輛運行情況，并根據優化函數選取合適的預瞄時間使得車輛穩定通過期望路徑．明廷友[4]提出一種改進的預瞄跟隨算法，確保計算得到的預瞄點落在期望的路徑上，避免曲率過大而脫離路徑．丁海濤等[5]建立一種形成人-車-路閉環控制的駕駛員模型，引入了橫向加速度反饋，并設計一種預覽點搜索算法，提高了對路徑的包容性．對于路徑跟蹤問題，盡管模型控制的方法解決這種多約束問題非常有效，同時也展現出了強大的控制性能[6-7]，但算法復雜度較高，優化求解比較費時，在很多情況下幾何跟蹤的方法更容易被接受．經過不斷地改進，幾何跟蹤的方法已經能夠用于路面車輛開發．斯坦福大學為開發的無人車“Stanley”[8]設計了一種新的控制模型(Stanley模型)，成功進行了實車開發并拿下比賽冠軍；彭之川等[9]對Pure Pursuit模型進行改進，在客車上實現了路徑跟蹤．

幾何跟蹤的方法的優點在于控制模型簡單易實現、計算速度快，本文也從模型的幾何關系控制原理出發，對比了如今比較流行的控制模型，并提出了優化建議．

1 幾何跟蹤方法分析

幾何跟蹤方法就是在建立的車輛道路模型基礎上運用幾何關系建立前輪轉角與車輛位姿和路徑點位置的函數關系，給出車輛到達下一個目標點需要的轉角控制量．車輛-道路幾何模型如圖1所示，在大地坐標系下車輛位置參考點為o點，行駛速度為v，下一個目標點為g點，通過車輛-道路幾何關系確定此時車輛需要向右轉動φ角度，向目標點行進．

圖1 車輛-道路幾何模型Fig. 1 Vehicle-road geometric model

1.1 Pure Pursuit模型分析

Pure Pursuit模型是一種應用非常廣泛的控制模型，最早由Wallace等[10]在1985年提出，該模型對各種路況都有很好的適應性． Pure Pursuit模型建立不需要考慮車輛動力學，也不要求路徑的連續性和光滑性．通過調整參數，該模型可以滿足多種場景的需要．因此，許多自主地面車輛都采用Pure Pursuit模型控制作為主要的橫向控制．

在Pure Pursuit模型中，以后軸為控制中心，不考慮行駛阻力，將車輪假設成剛性的，簡化阿克曼轉向模型，即在轉向時，假設前軸左右輪的轉角是相等的，把四輪車輛模型簡化成二輪自行車模型，如圖2所示．

圖2 自行車模型Fig. 2 Bicycle model

自行車模型可以將前輪轉角與后軸行駛的路徑曲率之間的關系進行簡單描述，如式(1)所示．

經證實，在中低速場景下，式(1)能夠較為準確地描述車輛運動．有了簡化的阿克曼轉向模型，Pure Pursuit模型便可建立，如圖3所示．

圖3 Pure Pursuit模型Fig. 3 Model of Pure Pursuit

該算法的核心思想是以車輛后軸中心為控制點，在期望路徑上以距離ld向前確定一個預瞄點，假定車輛能夠沿著某條平滑的路徑安全的到達確定的點，然后根據車輛當前位姿、轉彎半徑以及預瞄距離來確定所需要的前輪轉角，根據圖3所示的幾何模型，建立式(2)所示關系．

式中：ld表示預瞄距離；R表示轉向半徑；α表示車輛當前位置與目標點之間的偏角．

進一步可得式(3)

由式(1)、式(3)可得前輪轉角和預瞄距離之間的關系

式(4)中的角度α可以通過幾何關系得到

式中：gx、yg表示目標點的橫縱向坐標；x、y表示車輛當前的橫縱向坐標．

由式(4)可知，預瞄距離會直接影響前輪轉角控制量，也就是說轉角的大小一直是根據目標點的位置所決定的．以往的研究表明，較短的預瞄距離可以帶來較小的跟蹤誤差，但車輛的穩定性較差；較長的預瞄距離可以帶來更平滑的跟蹤效果，但由于模型對彎道過于敏感，在曲率較大的路況下會出現車輛超前轉彎，不能很好地貼合道路行駛的情況，對于路面車輛來說，嚴重偏離路徑將會造成不可挽回的危險，如圖4所示．

圖4 車輛駛離彎道Fig. 4 Vehicle pulling away from the bend

預瞄距離的選擇往往需要根據不同的道路場景進行大量的實驗得出，穩定性和跟蹤性能之間的折中選擇是一個非常困難的工作，并且在調整控制參數時對路徑的幾何特征是非常依賴的[8]．

不同預瞄距離下的跟蹤效果如圖5所示．

圖5 不同預瞄距離下的跟蹤效果Fig. 5 Tracking results at different preview distances

圖5展示了在不同預瞄距離下，車輛實際行駛路徑與期望路徑之間的關系，可見不同的預瞄距離對控制效果有非常明顯的影響．

1.2 Stanley模型分析

Stanley控制模型也叫前饋控制模型，如圖6所示．在大地坐標系下，建立Stanley模型，該模型的核心思想是以車輛前軸中心為控制點，計算當前車輛與道路之間的偏差，包括車頭朝向與道路方向的航向偏差以及車輛與道路的橫向距離偏差，根據總偏差給出回到路徑所需的前輪轉角控制量．

圖6 Stanley模型Fig. 6 Model of Stanley

據此，建立關系式(6)

式中：δ為轉角控制量；heθ和leθ分別為消除航向偏差和橫向偏差的轉角量；dl為預瞄距離；e為橫向距離偏差．

車輛的航向偏差很容易修正，Stanley控制模型的難點在于消除橫向偏差．假如車輛經過時間t達到目標點，可以得到橫向距離偏差的關系式(7)

式(7)兩邊同時對t求導，可以得到橫向距離偏差的變化率

當橫向距離偏差很小時，可根據等價聯動關系，得到式(9)

由式(7)—式(9)可得

式(10)對t積分，可得到橫向距離偏差關于t的響應函數

Stanley模型的更優之處在于計算了車輛的橫向偏差，并通過反饋的方式對這一偏差進行補償，提高了跟蹤的精度．但該模型對路徑的平滑性要求較高，從式(11)可以看出，橫向距離偏差以指數衰減，在速度過快或者預瞄距離較短的情況下容易出現控制超調，具體表現為在一些路況車輛會沖出道路和左右晃動，從圖7仿真結果中可以看出，在彎道處由于超調使車輛偏離期望路徑．

圖7 路徑偏離Fig. 7 Path deviation

由上述可知，基于幾何跟蹤的方式實現路徑跟蹤，不同的道路場景、控制模型以及不同的控制參數都會很大程度上影響跟蹤效果．針對不同的工況，都需進行大量的實驗得到較佳的控制參數，這不僅費時費力，而且不具有適用性．以往的實驗表明，目前還沒有一種單一的控制模型能夠適應所有的路徑條件．Pure Pursuit模型對道路的要求較低，工況適應較強，但跟蹤精度受前向預瞄的影響較大，實際中需進行大量的實驗來獲取最優的預瞄參數．而Stanley模型，考慮了車輛的橫向偏差，并通過前饋的方式對轉角進行控制修正，提高了跟蹤精度，但跟蹤誤差的快速收斂和控制超調之間的矛盾無法完全調和．

因此，本文建立了一種優化模型，同樣基于幾何關系的控制原理建立模型，對比上文討論的兩種模型，對以下方面做出改進．

(1)考慮車輛動力學約束．車輛動力學能夠在高速和大曲率的場景下如實反映力與運動之間的關系，實現對行駛車輛的穩定控制，動力學模型具有強非線性時變的特征[11-12]，因此能夠更加準確地反映車輛的運動關系.

(2)能夠自適應預瞄．對于幾何跟蹤法，預瞄距離的存在會給模型本身控制精度帶來一定的影響，而自適應策略會很大程度上提高模型在各種工況下的魯棒性[13]．

(3)形成車輛-道路閉環控制．不管什么模型都不能完全地映射出真實情況，但是偏差是可以測量的，而模型的偏差預測和誤差補償是影響控制精度的關鍵因素[14]．

2 優化模型的建立

2.1 車輛動力學模型

要對車輛進行精確化控制，需要建立能夠反映真實車輛且簡單直觀的數字化模型．為了準確描述車輛運動，需要建立更高階、更穩定的控制關系，因此建立車輛動力學模型來更好地約束車輛運動，所建立的動力學模型如圖8所示．

圖8 動力學模型Fig. 8 Dynamics model

圖8中，XOY為大地坐標系，xoy為車輛坐標系.根據車輛在y方向的受力關系，有

式中：m為車輛的質量；ay為車輛在y方向的加速度；fF和rF分別為車輛前軸和后軸的側偏力；δ為前輪轉角．

車輛在y方向的加速度由前向和橫擺運動產生加速度分量組成，得如下關系

根據式(12)、式(13)可以得到

由力矩平衡可得：

式中：Iz為轉動慣量；lf和lr分別為前軸和后軸到質心的距離．

對車輪進行受力分析，可得

式(16)和式(17)中：αf和αr分別為前后輪側偏角．

側偏力可以表示為

式(18)和式(19)中：kf和kr分別為前后輪側偏剛度；fθ、rθ分別為前后輪實際方向角．

根據運動關系有

式(20)和(21)中：vfy為前輪沿y方向的速度；vyr為后輪沿y方向的速度．

式(22)和(23)中：y˙為車輛速度沿y方向的分量．根據式(16)—式(23)，可得

綜上可得動力學約束關系為

2.2 預瞄控制系統

由上述討論可知，對于幾何跟蹤模型，預瞄距離的選取會直接影響跟蹤精度．改進的優化模型設計了一種自適應預瞄策略，該策略會先確定一個預瞄區間，然后從預瞄區間內的路徑點中選擇最優目標點．

首先確定預瞄區間的起始點．一般來說，車速較快時，選擇較遠的路徑點為目標點；而車速較慢時，選擇離車輛較近的路徑點更為合適．在彎道時，希望保留更多的路徑信息，如圖9所示，若求得當前車輛所在位置的道路曲率，則有

圖9 預瞄起始點Fig. 9 Starting point of preview

式中：v為車速，為已知量；c為增益系數；k為該路徑點處的曲率，確定方式見圖10．

圖10 求解曲率Fig. 10 Solving curvature

如圖10所示，設A、C為與路徑點B相鄰的前后路徑點，外圓為過A、B、C三點組成的三角形外接圓，通過式(28)可得到點B處的曲率k．

式(28)中的sinθ可通過三角形余弦定理求得，這樣便可求得每一路徑點的曲率．

在確定了預瞄起始點之后，便要確定預瞄終點，就可以知道當前車輛的預瞄區間．如圖11所示，在大地坐標系下可以確定路徑點和車輛的坐標，1g點為預瞄起始點，若以前軸中心為控制點，行駛到路徑點1g需要的前輪轉角控制量為f1δ，以后軸中心為控制點，則需要的前輪轉角控制量為r1δ，假設r1δ小于f1δ，得到轉角區間(r1δ，f1δ)．

圖11 預瞄終止點Fig. 11 Terminal point of preview

下面給出計算流程：

其中：of(xf,yf)、or(xr,yr)分別表示前后輪位置，gi(xi,yi)表示路徑點坐標，g (xmax, ymax)就是計算得到的預瞄終止點．

在上一步計算得到預瞄區間之后，只需在預瞄區間內找到最佳預瞄點即可，預瞄區間內的點并不是都適合作為車輛下一個預瞄點，預瞄點的選擇需要綜合考慮車輛在路徑跟隨中的平順性和預瞄橫向偏差，評價標準為

其中：J為損失函數，θf為航向偏差，為預瞄橫向偏差(如圖12，假設車輛欲到達路徑點g，用直線表示車輛行駛到g點的軌跡為g點之前預瞄區間所有路徑點到R的距離之和)，sd表示車輛與當前參考路徑點之間的距離，p、q為控制參數．

圖12 選擇最佳預瞄點Fig. 12 Selecting the best preview point

計算流程如下：

其中：o(xo,yo,θo)表示車輛的位置和航向角；gi(xi,yi,τi) 表示路徑點位置和切線方向；g(xopt,yopt)就是損失代價最小對應的路徑點．

2.3 橫向控制系統

得到最佳的預瞄點之后，通過建立幾何關系便可以得到車輛行駛至目標點所需要的轉角控制量．

式中：fθ為車輛航向偏差補償角；eθ為消除橫向偏差的補償角；e為橫向距離偏差；sd為車輛到目標點的距離．

考慮到車輪的側偏現象，最終的轉角δreal還應考慮轉向的不足．

式中：K表示轉向梯度；ay為橫向加速度；ga為重力加速度．

在本文所設計的控制器中，為使跟蹤效果更好，依然對轉角進行反饋補償，但為使車輛更加平穩，式(30)中的參數K不僅僅表示轉向梯度，同時也受橫向距離偏差的影響．駕駛員控制車輛可以認為是一個不斷反復進行的“信息感知—軌跡決策—操作校正”的過程[15]，因此該控制器由兩部分組成，一是通過車輛-道路模型計算出所需的轉角控制量，二是通過橫向偏差計算補償角，通過反饋對轉角進行增益控制，控制流程如圖13所示．

圖13 控制流程Fig. 13 Control process

3 結果分析

為了呈現改進之后的模型效果，在Simulink中搭建優化模型，并與Prescan聯合進行仿真測試，在每次實驗中，實驗車速和預瞄距離(預瞄距離僅針對Pure Pursuit 和Stanley模型)都設為固定值，車輛仿真參數見表1．

表1 車輛仿真參數Tab. 1 Vehicle parameter

3.1 仿真實驗

本次仿真實驗分別在預瞄距離為4、6、8m的情況下對比分析優化模型的跟蹤效果和橫向偏差(預瞄距離僅針對Pure Pursuit模型和Stanley模型)，結果如圖14、15所示．

圖14 不同ld下的跟蹤效果Fig. 14 Tracking results of different ld

圖15 不同ld下的橫向偏差Fig. 15 Lateral errors of different ld

從跟蹤結果圖和橫向偏差圖可以看出，調整預瞄距離可以明顯改善Pure Pursuit和Stanley模型的跟蹤效果，在預瞄距離為4m時，Pure Pursuit 模型的最大橫向偏差為0.8m左右，而預瞄距離為8m時的最大橫向偏差接近1.6m．Stanley模型同樣受到預瞄距離的影響，并且橫向偏差曲線呈現小幅二次波形，這是之前所述的控制超調使車輛產生晃動所致．在某些時刻，前兩種模型的跟蹤效果比優化的模型要好，但自適應預瞄使得優化模型更加靈活，對于大曲率路段能夠減少精度的損失．

同時在預瞄距離為6m時，分析了3種模型的航向角變化情況，結果如圖16所示．從實驗結果可以看出，即便采用了前饋控制，優化模型航向角變化非常平穩，而Stanley模型在轉角處出現了轉向過度的情況，在直線階段也出現了微小的晃動(圖16(b))，Pure Pursuit模型在彎道部分變化要延后一些，這是少許轉向不足所致．總的來說，相較于前兩種模型，優化模型的跟蹤精度和控制平穩性都得到了提升．

圖16 3種模型的航向角變化圖Fig. 16 Changes of heading angle of three models

3.2 實車實驗

實車平臺如圖17所示．

圖17 實車實驗平臺Fig. 17 Vehicle experimental platform

實車實驗所用的車輛為本實驗團隊基于大眾速騰燃油車研發，通過外接的駕駛機器人系統對車輛進行控制，達到無人化的目的．駕駛機器人基本硬件設施為：1個激光雷達、1個前視主動安全攝像頭、6個毫米波雷達(前后保險杠處分別裝載3個)、1個車載工控機、1個組合導航系統．駕駛機器人結構包括1個轉向控制器、1個油門控制、1個剎車控制器和1個擋位控制器．

本次實驗所用到的慣導系統是華測公司推出的CGI-610型高精度MEMS組合導航接收機．本次實驗對車輛采用RTK技術和慣導系統精確定位，基準站安裝在空曠地帶的高處，流動站安裝在車輛后軸中心垂直上方的備胎處，定向天線位于車頂前方，定位天線位于車頂后方．通過安裝在慣導上的4G網絡進行數據通信，實驗路段采集于一段校園路段，為了安全考慮，實驗車速較低，在7m/s車速下完成實驗．3.2.1 慣導標定與數據提取

首先進行慣導標定，標定參數見表2．

表2 慣導標定參數Tab. 2 INS configuration parameters

在標定好之后，通過CAN接口輸出慣導信息，本次實驗提取定位經度、定位緯度以及姿態角3個信號頻道的數據，提取的GPS信息如圖18所示．

圖18 GPS信息Fig. 18 Information of GPS

3.2.2 實車結果分析

本次實車實驗結果采用事后分析方式，通過兩次慣導輸出數據進行對比．實車實驗結果如圖19所示.由圖19可知，優化模型對實車同樣有很不錯的控制效果，并且車輛全程行駛平順，通過自適應預瞄和反饋修正使車輛控制更加平穩，同時也保證了跟蹤精度．但在彎道處出現了少許的轉向不足，如圖19(e)中，彎道處最大橫向偏差達到1.6m左右，當然其中包含著轉向機構的控制誤差和定位誤差，應當進一步修正．

圖19 實車實驗結果Fig. 19 Experimental results of actual vehicle

4 結 語

在無人駕駛路徑跟蹤領域，基于幾何關系的幾何跟蹤法是一種可靠性很高的方法，如今已經成功實現產品化．但由于模型無法完全反映真實場景，智能算法也無法代替駕駛員操控，導致在極限工況下，最后的跟蹤效果都不盡人意，但通過不斷地改進優化，控制模型可以滿足更多的場景需要．

現在知名度較高的幾何跟蹤模型包括Pure Pursuit模型和Stanley模型，這兩種控制模型已被應用到了各種場景，但由于模型的先天不足，且預瞄距離對轉角控制影響較大，導致在極限工況下跟蹤效果并不好，甚至還可能出現駛離道路的現象．

本文優化的模型提高了模型控制的穩定性，且沒有增加模型的復雜度，對于改進路面車輛同樣有促進作用；但優化模型的局限或者說當前的幾何跟蹤模型都沒有考慮車輛和道路的幾何形狀，也沒有將橫向控制和縱向控制進行耦合，這也是未來路徑跟蹤需要考慮的問題．據以往的研究表明，在車速過快或蜿蜒曲折道路的工況下，幾何式的控制效果明顯不如在中低速和平整道路工況下，為解決這個問題，一個可行的方法就是結合模型控制的方法，在不同的工況下采用不同的控制方法，提高車輛路徑跟蹤的穩定性．