一種結(jié)合規(guī)劃層的無(wú)人駕駛路徑跟蹤方法

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

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

局部路徑規(guī)劃[1-4]和路徑跟蹤技術(shù)[5-7]是無(wú)人駕駛車輛的核心技術(shù)。局部路徑規(guī)劃的目標(biāo)是在動(dòng)態(tài)環(huán)境下，尋找一條從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑，目前常用的方法有人工勢(shì)場(chǎng)法、柵格法、可視圖法以及其他各種智能化算法。其中人工勢(shì)場(chǎng)法以其數(shù)學(xué)計(jì)算簡(jiǎn)單明了而被廣泛應(yīng)用，當(dāng)前研究主要集中在解決如局部極小問(wèn)題、目標(biāo)不可達(dá)問(wèn)題和抖動(dòng)消除問(wèn)題等方面。

路徑跟蹤控制的目的是使車輛沿著期望路徑行駛，同時(shí)保證車輛的橫向穩(wěn)定性，其控制算法是路徑跟蹤控制的關(guān)鍵。當(dāng)前路徑跟蹤控制算法主要有模糊控制、滑模控制策略、單點(diǎn)或多點(diǎn)預(yù)瞄策略、模型預(yù)測(cè)控制、最優(yōu)控制等。

無(wú)人駕駛車輛在實(shí)際環(huán)境中進(jìn)行自主駕駛時(shí)，由于外部環(huán)境是動(dòng)態(tài)變化的，所以給定期望路徑下的跟蹤控制并不能保證無(wú)人駕駛車輛準(zhǔn)確地處理任何路徑跟蹤問(wèn)題。主要原因有：(1)由于環(huán)境未知，預(yù)先給定的參考路徑不一定能滿足安全行駛的要求；(2)當(dāng)所給定的期望路徑上出現(xiàn)障礙物時(shí)，無(wú)人駕駛車輛需要根據(jù)障礙物信息進(jìn)行重新規(guī)劃。

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

1 路徑跟蹤的預(yù)瞄模型

1.1 雙差分GPS定位

本文采用雙差分GPS對(duì)車輛進(jìn)行定位，其原理主要是根據(jù)基準(zhǔn)站已知精密坐標(biāo)，計(jì)算出基準(zhǔn)站到衛(wèi)星的距離改正數(shù)，并由基準(zhǔn)站實(shí)時(shí)地將這一改正數(shù)發(fā)送。車輛自身載有兩個(gè)移動(dòng)站，它們不但接收GPS信號(hào)，同時(shí)接收基準(zhǔn)站的改正數(shù)。通過(guò)這種方法提高了車輛自身的定位精度，為驗(yàn)證本文提出算法的有效性提供了可靠的硬件支持。

1.2 橫向預(yù)瞄誤差模型的建立

車輛在實(shí)際行駛過(guò)程中以當(dāng)前車輛的運(yùn)動(dòng)狀態(tài)為基礎(chǔ)，預(yù)測(cè)車輛行駛至前方某位置時(shí)車輛質(zhì)心與期望路徑道路中心線之間的橫向偏差，該偏差即為橫向預(yù)瞄誤差[8-9]。在車輛進(jìn)行路徑跟蹤時(shí)，通過(guò)橫向預(yù)瞄誤差與車輛的運(yùn)動(dòng)狀態(tài)得到汽車的前輪轉(zhuǎn)角，進(jìn)而根據(jù)前輪轉(zhuǎn)角實(shí)現(xiàn)對(duì)目標(biāo)路徑的跟蹤。橫向預(yù)瞄誤差模型如圖1所示。

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

從圖1中幾何關(guān)系可知：

(1)

(2)

(3)

由式(2)得

(4)

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

ep=e+Lsinψr

(5)

橫向預(yù)瞄誤差變化率可表示為

(6)

1.3 雙閉環(huán)PID控制路徑跟蹤

基于橫向預(yù)瞄誤差模型的路徑跟蹤控制系統(tǒng)相當(dāng)于一個(gè)駕駛員模型，駕駛員模型是導(dǎo)航技術(shù)的重要組成部分。駕駛員根據(jù)橫向預(yù)瞄誤差的正負(fù)和大小來(lái)轉(zhuǎn)動(dòng)方向盤調(diào)節(jié)前輪轉(zhuǎn)角，實(shí)現(xiàn)對(duì)車輛行駛路徑的調(diào)節(jié)。

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

圖2 雙閉環(huán)PID控制器Fig.2 Double closed loop PID controller

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

(7)

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

2 基于人工勢(shì)場(chǎng)法的局部路徑規(guī)劃

2.1 傳統(tǒng)人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法由虛擬力法[11-12]據(jù)電荷間相互作用的規(guī)律理論演變而來(lái)。其基本思想是，在被控對(duì)象運(yùn)動(dòng)環(huán)境中人為建立勢(shì)場(chǎng)，其中障礙物為斥力場(chǎng)，目標(biāo)點(diǎn)為引力場(chǎng)，分別對(duì)被控對(duì)象產(chǎn)生斥力與引力，斥力與引力形成的合力控制被控對(duì)象的運(yùn)動(dòng)方向。

在人工勢(shì)場(chǎng)法規(guī)劃過(guò)程中，規(guī)定引力的大小與車輛和目標(biāo)點(diǎn)之間的距離成正比，車輛相距目標(biāo)點(diǎn)越遠(yuǎn)引力作用越明顯。相反，斥力的大小與車輛和障礙物之間的距離成反比，車輛相距障礙物越近，斥力就越大。當(dāng)車輛距離障礙物在斥力作用范圍以外時(shí)，不考慮斥力對(duì)車輛的影響。其中目標(biāo)點(diǎn)對(duì)車輛引力勢(shì)能函數(shù)為

(8)

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

Fatt=kρ(Xr,Xg)

(9)

車輛所受到的障礙物的斥力處理方式與引力類似，假設(shè)Xo為障礙物的坐標(biāo)位置，m為斥力系數(shù)，假設(shè)障礙物斥力場(chǎng)的最大影響范圍是半徑為p的圓環(huán)，當(dāng)車輛距離障礙物大于p時(shí)，車輛所受到的斥力大小為0，因此斥力勢(shì)能函數(shù)為

(10)

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

(11)

所以，車輛在人工勢(shì)場(chǎng)下所受到的合力表示為

(12)

式中，n表示車輛在當(dāng)前位置所受到的障礙物斥力影響的個(gè)數(shù)。

2.2 改進(jìn)的人工勢(shì)場(chǎng)法

(13)

式中,n為任意常數(shù)。改進(jìn)后的斥力函數(shù)為

(14)

其中，

(15)

3 規(guī)劃層與控制層結(jié)合的設(shè)計(jì)

本文在原有的路徑跟蹤控制基礎(chǔ)上加入規(guī)劃層后，形成新的控制系統(tǒng)，結(jié)構(gòu)如圖3所示。在該控制系統(tǒng)中，當(dāng)環(huán)境感知系統(tǒng)探測(cè)到障礙物后，將障礙物的相對(duì)位置等信息傳遞給路徑規(guī)劃系統(tǒng)，由該系統(tǒng)根據(jù)需要選擇是否重新規(guī)劃路徑[15-16]，再通過(guò)路徑跟蹤控制系統(tǒng)計(jì)算出被控車輛到達(dá)下一時(shí)刻預(yù)測(cè)位置所需要轉(zhuǎn)動(dòng)的方向盤轉(zhuǎn)角。當(dāng)被控車輛到達(dá)下一時(shí)刻預(yù)測(cè)位置時(shí)，將當(dāng)前時(shí)刻車輛的狀態(tài)信息反饋給跟蹤控制系統(tǒng)以及環(huán)境感知系統(tǒng)，從而實(shí)現(xiàn)雙閉環(huán)的控制。

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

4 仿真及實(shí)驗(yàn)分析

4.1 仿真實(shí)驗(yàn)

對(duì)以上提出的算法進(jìn)行仿真實(shí)驗(yàn)，設(shè)定起始點(diǎn)、障礙物點(diǎn)以及目標(biāo)點(diǎn)的位置，得到的結(jié)果如圖4—圖7所示。

圖4 無(wú)規(guī)劃層的路徑跟蹤圖 Fig.4 Path tracking control without planning layer

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

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

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

圖4是在單個(gè)障礙物環(huán)境下沒(méi)有規(guī)劃層算法的仿真結(jié)果；圖5是在單個(gè)障礙物環(huán)境下具有規(guī)劃層算法的仿真結(jié)果。由圖4與圖5的對(duì)比可以看出，具有規(guī)劃層的路徑跟蹤算法可以在車輛遇到障礙物的時(shí)候，實(shí)時(shí)矯正與更新車輛的期望跟蹤路徑，提高了路徑跟蹤的安全性與可靠性。圖6和圖7是在不同障礙物個(gè)數(shù)的環(huán)境下具有規(guī)劃層算法的仿真結(jié)果，以此來(lái)驗(yàn)證當(dāng)環(huán)境由簡(jiǎn)單變向復(fù)雜時(shí)上文所提出算法的有效性。

4.2 路徑跟蹤靈敏度分析

為了驗(yàn)證該算法的有效性，本文提出路徑跟蹤靈敏度這一概念來(lái)表示當(dāng)仿真環(huán)境由簡(jiǎn)單變向復(fù)雜時(shí)路徑跟蹤誤差的變化情況，誤差變化大說(shuō)明車輛路徑跟蹤的靈敏度低，誤差變化小說(shuō)明車輛路徑跟蹤的靈敏度高。為了說(shuō)明誤差的變化情況，提出了平均偏差P，其計(jì)算公式如下：

(16)

從表1中可以看出，當(dāng)仿真環(huán)境中的障礙物個(gè)數(shù)從1個(gè)到3個(gè)時(shí)，平均偏差變化了2.05 cm，障礙物個(gè)數(shù)從3個(gè)變化到5個(gè)時(shí)平均偏差變化了1.28 cm，由此可見(jiàn)當(dāng)仿真環(huán)境由簡(jiǎn)單變向復(fù)雜時(shí)，平均偏差變化不大，路徑跟蹤靈敏度較高。

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

障礙物個(gè)數(shù)偏差累計(jì)量/m軌跡點(diǎn)數(shù)目/個(gè)平均偏差/m1576.524 81 1680.493 63628.744 31 2230.514 15740.821 41 4060.526 9

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

為了驗(yàn)證本文算法的有效性，利用改進(jìn)的人工勢(shì)場(chǎng)法結(jié)合預(yù)瞄模型的跟蹤控制算法進(jìn)行了實(shí)車試驗(yàn)。試驗(yàn)的車輛為改制的越野車，駕駛系統(tǒng)采用線控轉(zhuǎn)向系統(tǒng)，控制器采用Dspace公司的AUTObox，車輛的定位采用NovAtel公司的GPS定位系統(tǒng)，算法的實(shí)現(xiàn)環(huán)節(jié)是將Simulink自動(dòng)代碼生成的sdf文件與dspace上位機(jī)連接，再通過(guò)下位機(jī)AUTObox控制器來(lái)控制車輛的行為。

1)控制層與規(guī)劃層結(jié)合算法的試驗(yàn)

圖8所示為本文規(guī)劃層與控制層結(jié)合的算法在實(shí)車上試驗(yàn)的結(jié)果圖。圖中藍(lán)色線為期望軌跡線，黑色虛線為車輛遇到障礙物之后重新規(guī)劃的軌跡，紅色線為車輛跟蹤的軌跡。

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

圖9 跟蹤路徑中轉(zhuǎn)角對(duì)比圖 Fig.9 Corner comparison diagram in path tracking

2)試驗(yàn)方向盤實(shí)際轉(zhuǎn)角與目標(biāo)轉(zhuǎn)角結(jié)果圖

圖9是無(wú)人駕駛車輛在以上提出的算法中進(jìn)行路徑跟蹤的轉(zhuǎn)角對(duì)比圖。圖中紅色線代表目標(biāo)轉(zhuǎn)角，是通過(guò)預(yù)瞄模型計(jì)算出來(lái)的、不偏離期望路徑所需要轉(zhuǎn)動(dòng)的方向盤角度,藍(lán)色線代表實(shí)際轉(zhuǎn)角。由于實(shí)際執(zhí)行的過(guò)程中，方向盤會(huì)存在轉(zhuǎn)向的阻力與摩擦力，從而出現(xiàn)實(shí)際轉(zhuǎn)角不會(huì)完全與理想的目標(biāo)轉(zhuǎn)角吻合的情況。由圖9可知，轉(zhuǎn)向效果較好，路徑跟蹤效果較好。

5 結(jié)論

針對(duì)傳統(tǒng)路徑跟蹤方法中不能在未知環(huán)境下避開(kāi)障礙物的問(wèn)題，本文提出了規(guī)劃層與控制層結(jié)合的方法。規(guī)劃層采用改進(jìn)的人工勢(shì)場(chǎng)法，控制層采用了雙閉環(huán)的PID控制方法，并且根據(jù)模糊控制原理在線實(shí)時(shí)調(diào)節(jié)PID控制算法的3個(gè)參數(shù)，從而在車輛進(jìn)行路徑跟蹤過(guò)程中可以較好地躲避障礙物，提高了路徑跟蹤的安全性與魯棒性。研究結(jié)論如下：

1)通過(guò)仿真結(jié)果可以看出，當(dāng)障礙物個(gè)數(shù)從少變多時(shí)，路徑跟蹤的平均誤差有所增加，但不是線性增長(zhǎng)且增加幅度不大，由此看出雙閉環(huán)PID控制算法的靈敏度較高。

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

3)整體來(lái)看，本文算法中規(guī)劃層與控制層能形成較好的銜接，并且基本實(shí)現(xiàn)了在動(dòng)態(tài)環(huán)境中在線實(shí)時(shí)地調(diào)整參數(shù)重新規(guī)劃路徑， 提高了路徑跟蹤的安全性和魯棒性。