本發(fā)明屬于自動(dòng)駕駛，具體涉及一種自動(dòng)駕駛路徑規(guī)劃與跟蹤控制方法。

背景技術(shù)：

1、汽車自動(dòng)駕駛系統(tǒng)(motor?vehicle?auto?driving?system)，又稱自動(dòng)駕駛汽車(autonomous?vehicles；self-piloting?automobile)也稱無(wú)人駕駛汽車、電腦駕駛汽車、或輪式移動(dòng)機(jī)器人，是一種通過(guò)車載電腦系統(tǒng)實(shí)現(xiàn)無(wú)人駕駛的智能汽車系統(tǒng)。目前，為保證自動(dòng)駕駛技術(shù)的安全性，自動(dòng)駕駛車輛上通常配有各種各樣的傳感器，如攝像頭、毫米波雷達(dá)、慣性測(cè)量單元等。然而，自動(dòng)駕駛車輛由于天氣、障礙物、光線等原因不可避免地存在視野盲區(qū)，單憑自動(dòng)駕駛汽車(autonomous?vehicles,簡(jiǎn)稱avs)上有限的傳感器無(wú)法滿足自動(dòng)駕駛技術(shù)對(duì)動(dòng)態(tài)環(huán)境進(jìn)行實(shí)時(shí)觀察的要求。視野盲區(qū)對(duì)自動(dòng)駕駛車輛路徑規(guī)劃的可靠性帶來(lái)極大的挑戰(zhàn)。車路協(xié)同技術(shù)利用路側(cè)單元以及附近車輛的傳感器，為自動(dòng)駕駛汽車提供了其盲區(qū)的傳感信息。在基于車路協(xié)同的自動(dòng)駕駛系統(tǒng)中，路側(cè)單元(roadside?units,簡(jiǎn)稱rsus)、av相互協(xié)作，以延伸自動(dòng)駕駛汽車的視線。協(xié)同感知信息經(jīng)計(jì)算處理，其結(jié)果通過(guò)車間(vehicle?to?vehicle,簡(jiǎn)稱v2v)通信與車路(vehicle?toinstructure，簡(jiǎn)稱v2i)通信返回至tav，為tav的路徑規(guī)劃提供了更全面、更完整的信息，使得自動(dòng)駕駛的安全性大大提升。

2、在自動(dòng)駕駛車輛行駛過(guò)程中，對(duì)于包括路口在內(nèi)的道路場(chǎng)景，除了通過(guò)多車協(xié)同控制對(duì)車輛進(jìn)行協(xié)同規(guī)劃，單車路徑規(guī)劃也是自動(dòng)駕駛車輛的重要模塊。單車路徑規(guī)劃需在環(huán)境地圖上，根據(jù)給定的車輛起始點(diǎn)和目標(biāo)點(diǎn)，用規(guī)劃算法搜索出安全、無(wú)碰撞的路徑；然而在現(xiàn)有技術(shù)中，并未設(shè)計(jì)出有能夠進(jìn)行單車路徑規(guī)劃和跟蹤控制的方法，繼而使得單車路徑規(guī)劃和跟蹤控制的實(shí)現(xiàn)變成了本領(lǐng)域一個(gè)亟待解決的問(wèn)題。

3、因此，本發(fā)明提出了一種自動(dòng)駕駛路徑規(guī)劃與跟蹤控制方法，以解決上述現(xiàn)有技術(shù)存在的問(wèn)題。

技術(shù)實(shí)現(xiàn)思路

1、(一)解決的技術(shù)問(wèn)題

2、針對(duì)現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種自動(dòng)駕駛路徑規(guī)劃與跟蹤控制方法，具備能夠?yàn)檐囕v規(guī)劃出符合運(yùn)動(dòng)學(xué)規(guī)律的循跡路徑，根據(jù)規(guī)劃模塊輸出的期望路徑，在組合導(dǎo)航系統(tǒng)的配合下，使得車輛按照設(shè)定的速度及路徑實(shí)現(xiàn)自主行駛等優(yōu)點(diǎn)，解決了現(xiàn)有技術(shù)無(wú)法實(shí)現(xiàn)單車路徑規(guī)劃和跟蹤控制等問(wèn)題。

3、(二)技術(shù)方案

4、為實(shí)現(xiàn)上述能夠?yàn)檐囕v規(guī)劃出符合運(yùn)動(dòng)學(xué)規(guī)律的循跡路徑、使車輛按照設(shè)定的速度及路徑實(shí)現(xiàn)自主行駛的目的，本發(fā)明提供如下技術(shù)方案：

5、一種自動(dòng)駕駛路徑規(guī)劃與跟蹤控制方法，包括

6、步驟1：在車輛行駛過(guò)程中，根據(jù)定位模塊輸出的當(dāng)前位置和規(guī)劃的目標(biāo)點(diǎn)，在全局地圖中基于dijkstra算法產(chǎn)生全局路徑，車輛基于全局路徑和全局地圖產(chǎn)生循跡路徑；

7、步驟2：對(duì)步驟1所獲基于全局路徑給出的循跡路徑，采用多傳感器融合方法實(shí)時(shí)構(gòu)建局部代價(jià)地圖，并在局部代價(jià)地圖中進(jìn)行基于dwa算法的局部路徑規(guī)劃，產(chǎn)生車輛實(shí)時(shí)避障路徑；

8、步驟3：根據(jù)車輛視覺(jué)系統(tǒng)獲取的信息，產(chǎn)生車道保持及換道路徑，并對(duì)循跡路徑、避障路徑和車道保持及換道路徑進(jìn)行評(píng)估選擇，決策出最優(yōu)局部路徑；

9、步驟4：對(duì)步驟3所獲最優(yōu)局部路徑，在組合導(dǎo)航系統(tǒng)的配合下，利用路徑跟蹤橫縱向控制系統(tǒng)采用閉環(huán)控制算法對(duì)車輛進(jìn)行橫/縱向控制，使得車輛按照設(shè)定速度及路徑進(jìn)行自主行駛。

10、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，基于dijkstra算法產(chǎn)生全局路徑，車輛基于全局路徑和全局地圖產(chǎn)生循跡路徑的過(guò)程包括

11、步驟1.1：在全局地圖中，初始化起始點(diǎn)的距離為0，其他點(diǎn)的距離為∞；

12、步驟1.2：創(chuàng)建集合open，存放當(dāng)前正在處理的節(jié)點(diǎn)，并將起始點(diǎn)加入open；

13、步驟1.3：創(chuàng)建一個(gè)集合q，存放所有未處理的點(diǎn)；

14、步驟1.4：當(dāng)q集不為空時(shí)，重復(fù)以下循環(huán)：查找open中距離最小點(diǎn)，更新這個(gè)點(diǎn)的周圍點(diǎn)的距離值，并將他們加入open，將當(dāng)前點(diǎn)從open移除；結(jié)束后獲得通往目標(biāo)點(diǎn)的最短距離以及對(duì)應(yīng)的路徑序列。

15、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，局部代價(jià)地圖構(gòu)建過(guò)程包括

16、(1)已知車輛位姿序列和觀測(cè)序列，求解局部環(huán)境地圖，設(shè)每個(gè)柵格獨(dú)立同分布，可把問(wèn)題轉(zhuǎn)換為求解每個(gè)柵格的占有概率；

17、(2)通過(guò)設(shè)置柵格占有概率的閾值，分別區(qū)分占有柵格、未知柵格和空閑柵格；

18、(3)每獲得一幀傳感器數(shù)據(jù)，都會(huì)實(shí)時(shí)更新對(duì)應(yīng)柵格的狀態(tài)，對(duì)障礙物區(qū)域柵格以及障礙物周圍范圍的柵格區(qū)域賦予權(quán)值，得到代價(jià)地圖，最后利用地圖融合的方法將代價(jià)地圖和包含障礙信息的全局地圖融合。

19、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，基于dwa算法進(jìn)行局部路徑規(guī)劃的過(guò)程為從由線速度與角速度組成的速度空間(v，ω)中采樣多組速度組合，并通過(guò)車輛運(yùn)動(dòng)學(xué)模型預(yù)測(cè)車輛的運(yùn)動(dòng)，得到車輛在規(guī)定時(shí)間內(nèi)的多組運(yùn)動(dòng)軌跡，再通過(guò)評(píng)價(jià)函數(shù)對(duì)這些軌跡進(jìn)行評(píng)估，選擇最優(yōu)軌跡，生成實(shí)時(shí)避障路徑。

20、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，在對(duì)循跡路徑、避障路徑和車道保持及換道路徑進(jìn)行評(píng)估選擇時(shí)，使用的路徑代價(jià)函數(shù)為

21、ctotal＝kcobs+cs

22、其中，ctotal為軌跡的總體代價(jià)，k為比例系數(shù)，cobs為安全性代價(jià)，cs為軌跡的平滑性代價(jià)；

23、cs＝δβ∫v2ds

24、其中：δβ為航向角變化量，v為車輛當(dāng)前速度，s為軌跡。

25、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，最優(yōu)局部路徑的決策過(guò)程包括

26、首先利用路徑代價(jià)函數(shù)對(duì)避障路徑、循跡路徑和車道保持及換道路徑分別進(jìn)行代價(jià)評(píng)估，選擇代價(jià)最小路徑作為最優(yōu)路徑。

27、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，路徑跟蹤橫縱向控制系統(tǒng)使用pid控制作為分別對(duì)車輛進(jìn)行橫/縱向控制的控制器，pid控制器的微分方程為：

28、u(t)＝kpe(t)+ki∫e(t)dt+kde(t)

29、其中，kp，ki，kd分別為比例系數(shù)、積分系數(shù)、微分系數(shù)，e(t)為反饋?zhàn)饔孟碌恼`差信號(hào)，u(t)為經(jīng)過(guò)比例、積分、微分環(huán)節(jié)的疊加后得到控制信號(hào)；

30、對(duì)pid微分方程進(jìn)行離散化，得到pid控制算法的計(jì)算公式為：

31、

32、其中，k是一個(gè)數(shù)量，表示某一個(gè)采樣時(shí)間點(diǎn)，k-1就表示k的前一個(gè)，i就是0～k之間的一個(gè)變量，t為控制器采樣時(shí)間，ti＝tkp/ki,td＝kd/kp。

33、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，路徑跟蹤橫縱向控制系統(tǒng)包括

34、基于道路曲率的縱向期望速度決策及速度跟蹤pid縱向速度跟蹤控制模型，用于進(jìn)行自動(dòng)駕駛車輛的縱向速度決策及縱向速度跟蹤；

35、基于動(dòng)態(tài)預(yù)瞄的路徑跟蹤反饋控制模型，用于進(jìn)行自動(dòng)駕駛車輛的橫向路徑跟蹤。

36、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，利用基于道路曲率的縱向期望速度決策及速度跟蹤pid縱向速度跟蹤控制模型進(jìn)行自動(dòng)駕駛車輛的縱向速度決策及縱向速度跟蹤的過(guò)程包括

37、使用基于道路曲率的車輛縱向期望速度決策，通過(guò)pid控制器對(duì)車輛縱向期望速度進(jìn)行跟蹤控制，當(dāng)pid控制器獲取縱向期望速度和實(shí)際車速后，獲得速度偏差，并應(yīng)用pid控制算法計(jì)算油門踏板及剎車踏板的控制量大小，實(shí)現(xiàn)自動(dòng)駕駛車輛的縱向速度決策和縱向速度跟蹤。

38、根據(jù)本公開(kāi)實(shí)施例的一種具體實(shí)現(xiàn)方式，利用基于動(dòng)態(tài)預(yù)瞄的路徑跟蹤反饋控制模型進(jìn)行自動(dòng)駕駛車輛的橫向路徑跟蹤的過(guò)程包括

39、首先，通過(guò)利用基于期望路徑曲率連續(xù)變化的預(yù)瞄距離確定預(yù)瞄距離；隨后，通過(guò)在高車速時(shí)計(jì)算預(yù)瞄航向角偏差，低車速時(shí)結(jié)合橫向距離偏差與預(yù)瞄航向角偏差來(lái)計(jì)算出最終反饋誤差量，并通過(guò)pid控制算法進(jìn)行反饋誤差控制，輸出最終期望方向盤轉(zhuǎn)角，實(shí)現(xiàn)自動(dòng)駕駛車輛的橫向路徑跟蹤。

40、(三)有益效果

41、與現(xiàn)有技術(shù)相比，本發(fā)明提供了一種自動(dòng)駕駛路徑規(guī)劃與跟蹤控制方法，具備以下有益效果：

42、1.本方法采用從全局到局部的聚焦方式，結(jié)合多傳感器融合和基于dijkstra算法產(chǎn)生的全局路徑構(gòu)建局部地圖，進(jìn)而基于dwa的局部路徑規(guī)劃生成實(shí)時(shí)避障路徑，能夠準(zhǔn)確計(jì)算出由起點(diǎn)到終點(diǎn)的最優(yōu)路徑解；

43、2.本方法根據(jù)視覺(jué)系統(tǒng)獲取的車道線等信息，產(chǎn)生車道保持及換道路徑；并基于局部代價(jià)地圖對(duì)三種路徑進(jìn)行評(píng)估選擇，決策出最優(yōu)局部路徑，提高了規(guī)劃的準(zhǔn)確性，同時(shí)增強(qiáng)了車輛對(duì)動(dòng)態(tài)場(chǎng)景的適應(yīng)性，便于及時(shí)避障；

44、3.本方法在對(duì)車輛進(jìn)行跟蹤控制時(shí)，由于車輛在行駛過(guò)程中，在曲率較大路徑處若車速過(guò)快，則無(wú)法取得較好的路徑跟蹤效果，因此將橫向控制與縱向控制相結(jié)合，在保證穩(wěn)定性的同時(shí)使得自動(dòng)駕駛車輛軌跡跟蹤效果得到了有效提升。