一種結合規劃層的無人駕駛路徑跟蹤方法

沈 鵬，曹 凱，劉秉政，楊 旭，奉 柳

(山東理工大學 交通與車輛工程學院，山東 淄博 255049)

局部路徑規劃[1-4]和路徑跟蹤技術[5-7]是無人駕駛車輛的核心技術。局部路徑規劃的目標是在動態環境下，尋找一條從當前點到目標點的無碰撞路徑，目前常用的方法有人工勢場法、柵格法、可視圖法以及其他各種智能化算法。其中人工勢場法以其數學計算簡單明了而被廣泛應用，當前研究主要集中在解決如局部極小問題、目標不可達問題和抖動消除問題等方面。

路徑跟蹤控制的目的是使車輛沿著期望路徑行駛，同時保證車輛的橫向穩定性，其控制算法是路徑跟蹤控制的關鍵。當前路徑跟蹤控制算法主要有模糊控制、滑模控制策略、單點或多點預瞄策略、模型預測控制、最優控制等。

無人駕駛車輛在實際環境中進行自主駕駛時，由于外部環境是動態變化的，所以給定期望路徑下的跟蹤控制并不能保證無人駕駛車輛準確地處理任何路徑跟蹤問題。主要原因有：(1)由于環境未知，預先給定的參考路徑不一定能滿足安全行駛的要求；(2)當所給定的期望路徑上出現障礙物時，無人駕駛車輛需要根據障礙物信息進行重新規劃。

為了解決以上問題，本文提出雙閉環PID的控制方法(路徑閉環，預瞄偏差閉環)進行路徑跟蹤，同時結合視覺傳感器的信息來實時更新和矯正期望路徑，并且提出路徑跟蹤靈敏度概念來分析路徑跟蹤的誤差變化情況，以實現在跟蹤期望路徑的同時可以較好地避開障礙物。

1 路徑跟蹤的預瞄模型

1.1 雙差分GPS定位

本文采用雙差分GPS對車輛進行定位，其原理主要是根據基準站已知精密坐標，計算出基準站到衛星的距離改正數，并由基準站實時地將這一改正數發送。車輛自身載有兩個移動站，它們不但接收GPS信號，同時接收基準站的改正數。通過這種方法提高了車輛自身的定位精度，為驗證本文提出算法的有效性提供了可靠的硬件支持。

1.2 橫向預瞄誤差模型的建立

車輛在實際行駛過程中以當前車輛的運動狀態為基礎，預測車輛行駛至前方某位置時車輛質心與期望路徑道路中心線之間的橫向偏差，該偏差即為橫向預瞄誤差[8-9]。在車輛進行路徑跟蹤時，通過橫向預瞄誤差與車輛的運動狀態得到汽車的前輪轉角，進而根據前輪轉角實現對目標路徑的跟蹤。橫向預瞄誤差模型如圖1所示。

圖1 橫向預瞄誤差模型Fig.1 Lateral preview error model

從圖1中幾何關系可知：

(1)

(2)

(3)

由式(2)得

(4)

將式(1)和式(4)帶入式(3)，則橫向預瞄誤差模型可表示為

ep=e+Lsinψr

(5)

橫向預瞄誤差變化率可表示為

(6)

1.3 雙閉環PID控制路徑跟蹤

基于橫向預瞄誤差模型的路徑跟蹤控制系統相當于一個駕駛員模型，駕駛員模型是導航技術的重要組成部分。駕駛員根據橫向預瞄誤差的正負和大小來轉動方向盤調節前輪轉角，實現對車輛行駛路徑的調節。

為了較好地控制路徑跟蹤，這里采用了雙閉環PID控制器(如圖2所示)。PID控制器主要對偏差量進行比例、積分以及微分控制。首先采用路徑閉環控制方法，在期望路徑的基礎上，根據視覺傳感器得知的環境信息實時更新和規劃新的路徑，并且進行跟蹤，通過這種方法實現期望路徑與實時矯正路徑之間的閉環控制；其次采用橫向預瞄偏差的閉環控制方法，結合每一時刻差分GPS對車輛的定位信息(包含二維坐標以及航向角)，實時更新當前時刻的橫向預瞄偏差，在車輛到達下一時刻位置時，重復此過程，從而實現了橫向預瞄偏差的閉環控制。使用PID控制器進行控制具有算法簡單、實現方便、魯棒性好、參數調整容易、可靠性高等優點。

圖2 雙閉環PID控制器Fig.2 Double closed loop PID controller

本文PID控制參數的調整采用了參數模糊自整定的方法，通過該方法找出PID參數與橫向預瞄誤差以及其變化率之間的模糊關系。以車輛當前狀態為基準，車載計算機通過感知層接收到的有關車輛位置坐標、航向角信息實時預測出下一時刻所到達的位置，計算出橫向預瞄誤差et以及橫向預瞄誤差的變化率ec，在車輛運行過程中通過不斷檢測et和ec，再根據模糊控制原理[10]的3個參數進行在線修改，以滿足不同的et和ec對控制參數的不同要求，從而可以實時地計算出方向盤轉角，達到較好的路徑跟蹤效果。PID控制律如下：

(7)

式中：KP,Ki,Kd分別為PID控制器的3個參數；e(t)為橫向預瞄誤差；δ(t)是方向盤轉角。

2 基于人工勢場法的局部路徑規劃

2.1 傳統人工勢場法

人工勢場法由虛擬力法[11-12]據電荷間相互作用的規律理論演變而來。其基本思想是，在被控對象運動環境中人為建立勢場，其中障礙物為斥力場，目標點為引力場，分別對被控對象產生斥力與引力，斥力與引力形成的合力控制被控對象的運動方向。

在人工勢場法規劃過程中，規定引力的大小與車輛和目標點之間的距離成正比，車輛相距目標點越遠引力作用越明顯。相反，斥力的大小與車輛和障礙物之間的距離成反比，車輛相距障礙物越近，斥力就越大。當車輛距離障礙物在斥力作用范圍以外時，不考慮斥力對車輛的影響。其中目標點對車輛引力勢能函數為

(8)

式中：k為引力系數；Xr表示車輛的位置坐標；Xg表示目標點的位置坐標；ρ(Xr,Xg)表示車輛與目標點之間的歐幾里得距離。根據勢能函數與引力函數的關系得到引力函數為

Fatt=kρ(Xr,Xg)

(9)

車輛所受到的障礙物的斥力處理方式與引力類似，假設Xo為障礙物的坐標位置，m為斥力系數，假設障礙物斥力場的最大影響范圍是半徑為p的圓環，當車輛距離障礙物大于p時，車輛所受到的斥力大小為0，因此斥力勢能函數為

(10)

式中,ρ(Xr,Xo)表示車輛與障礙物之間的歐幾里得距離。車輛所受到的斥力為斥力場的負梯度方向，斥力函數表示為

(11)

所以，車輛在人工勢場下所受到的合力表示為

(12)

式中，n表示車輛在當前位置所受到的障礙物斥力影響的個數。

2.2 改進的人工勢場法

(13)

式中,n為任意常數。改進后的斥力函數為

(14)

其中，

(15)

3 規劃層與控制層結合的設計

本文在原有的路徑跟蹤控制基礎上加入規劃層后，形成新的控制系統，結構如圖3所示。在該控制系統中，當環境感知系統探測到障礙物后，將障礙物的相對位置等信息傳遞給路徑規劃系統，由該系統根據需要選擇是否重新規劃路徑[15-16]，再通過路徑跟蹤控制系統計算出被控車輛到達下一時刻預測位置所需要轉動的方向盤轉角。當被控車輛到達下一時刻預測位置時，將當前時刻車輛的狀態信息反饋給跟蹤控制系統以及環境感知系統，從而實現雙閉環的控制。

圖3 融入規劃層的路徑跟蹤控制圖Fig.3 The path tracking control combined with planning layer

4 仿真及實驗分析

4.1 仿真實驗

對以上提出的算法進行仿真實驗，設定起始點、障礙物點以及目標點的位置，得到的結果如圖4—圖7所示。

圖4 無規劃層的路徑跟蹤圖 Fig.4 Path tracking control without planning layer

圖5 單個障礙物下的路徑跟蹤圖Fig.5 Path tracking control with one obstacle

圖6 3個障礙物下的路徑跟蹤圖Fig.6 Path tracking control with three obstacles

圖7 5個障礙物下的路徑跟蹤圖Fig.7 Path tracking control with five obstacles

圖4是在單個障礙物環境下沒有規劃層算法的仿真結果；圖5是在單個障礙物環境下具有規劃層算法的仿真結果。由圖4與圖5的對比可以看出，具有規劃層的路徑跟蹤算法可以在車輛遇到障礙物的時候，實時矯正與更新車輛的期望跟蹤路徑，提高了路徑跟蹤的安全性與可靠性。圖6和圖7是在不同障礙物個數的環境下具有規劃層算法的仿真結果，以此來驗證當環境由簡單變向復雜時上文所提出算法的有效性。

4.2 路徑跟蹤靈敏度分析

為了驗證該算法的有效性，本文提出路徑跟蹤靈敏度這一概念來表示當仿真環境由簡單變向復雜時路徑跟蹤誤差的變化情況，誤差變化大說明車輛路徑跟蹤的靈敏度低，誤差變化小說明車輛路徑跟蹤的靈敏度高。為了說明誤差的變化情況，提出了平均偏差P，其計算公式如下：

(16)

從表1中可以看出，當仿真環境中的障礙物個數從1個到3個時，平均偏差變化了2.05 cm，障礙物個數從3個變化到5個時平均偏差變化了1.28 cm，由此可見當仿真環境由簡單變向復雜時，平均偏差變化不大，路徑跟蹤靈敏度較高。

表1 不同環境下軌跡跟蹤的平均偏差
Tab.1 Mean deviation of path tracking in different environments

障礙物個數偏差累計量/m軌跡點數目/個平均偏差/m1576.524 81 1680.493 63628.744 31 2230.514 15740.821 41 4060.526 9

4.3 實車試驗

為了驗證本文算法的有效性，利用改進的人工勢場法結合預瞄模型的跟蹤控制算法進行了實車試驗。試驗的車輛為改制的越野車，駕駛系統采用線控轉向系統，控制器采用Dspace公司的AUTObox，車輛的定位采用NovAtel公司的GPS定位系統，算法的實現環節是將Simulink自動代碼生成的sdf文件與dspace上位機連接，再通過下位機AUTObox控制器來控制車輛的行為。

1)控制層與規劃層結合算法的試驗

圖8所示為本文規劃層與控制層結合的算法在實車上試驗的結果圖。圖中藍色線為期望軌跡線，黑色虛線為車輛遇到障礙物之后重新規劃的軌跡，紅色線為車輛跟蹤的軌跡。

圖8 控制層與規劃層結合的跟蹤結果圖 Fig.8 Path tracking result diagram with control layer and planning layer combined

圖9 跟蹤路徑中轉角對比圖 Fig.9 Corner comparison diagram in path tracking

2)試驗方向盤實際轉角與目標轉角結果圖

圖9是無人駕駛車輛在以上提出的算法中進行路徑跟蹤的轉角對比圖。圖中紅色線代表目標轉角，是通過預瞄模型計算出來的、不偏離期望路徑所需要轉動的方向盤角度,藍色線代表實際轉角。由于實際執行的過程中，方向盤會存在轉向的阻力與摩擦力，從而出現實際轉角不會完全與理想的目標轉角吻合的情況。由圖9可知，轉向效果較好，路徑跟蹤效果較好。

5 結論

針對傳統路徑跟蹤方法中不能在未知環境下避開障礙物的問題，本文提出了規劃層與控制層結合的方法。規劃層采用改進的人工勢場法，控制層采用了雙閉環的PID控制方法，并且根據模糊控制原理在線實時調節PID控制算法的3個參數，從而在車輛進行路徑跟蹤過程中可以較好地躲避障礙物，提高了路徑跟蹤的安全性與魯棒性。研究結論如下：

1)通過仿真結果可以看出，當障礙物個數從少變多時，路徑跟蹤的平均誤差有所增加，但不是線性增長且增加幅度不大，由此看出雙閉環PID控制算法的靈敏度較高。

2)結合實車試驗可以看出，在實際環境中，雖然部分路徑的跟蹤效果不是特別理想，但是排除客觀存在的原因如路面情況以及方向盤的摩擦阻力，整體來看本文提出的算法可以較為完整地實現預期效果。

3)整體來看，本文算法中規劃層與控制層能形成較好的銜接，并且基本實現了在動態環境中在線實時地調整參數重新規劃路徑， 提高了路徑跟蹤的安全性和魯棒性。