侵權(quán)投訴
訂閱
糾錯(cuò)
加入自媒體

自動(dòng)駕駛規(guī)劃算法解析

本文來(lái)源:智車科技

/ 導(dǎo)讀 /

自動(dòng)駕駛汽車從A點(diǎn)行駛到B點(diǎn),需要軌跡規(guī)劃算法來(lái)進(jìn)行全局規(guī)劃,而具體都有哪些算法呢?這篇文章想和大家分享一下一類最常用的軌跡規(guī)劃算法,基于圖搜索的規(guī)劃算法。

在開(kāi)始介紹圖搜索算法之前,先簡(jiǎn)單介紹一下自動(dòng)駕駛中的規(guī)劃問(wèn)題:規(guī)劃模塊處于自動(dòng)駕駛軟件框架中的中間位置,其接收感知、定位、地圖發(fā)來(lái)的上游信息,輸出一條安全、平穩(wěn)、舒適的軌跡給到控制模塊,因此起到了一個(gè)承上啟下的作用,可以說(shuō)是影響自動(dòng)駕駛中舒適性及安全性最重要的一環(huán)。而傳統(tǒng)意義上的規(guī)劃問(wèn)題可以分為兩個(gè)步驟:

前端負(fù)責(zé)粗粒度的路徑查找,搜索出一條可行路徑;后端負(fù)責(zé)細(xì)粒度的軌跡生成,生成出一條控制模塊可以很好執(zhí)行的平滑軌跡。而這篇文章想要探討的,就是前端路徑搜索中一種最常用的方法,基于圖搜索的路徑規(guī)劃算法。

圖搜索基礎(chǔ)

圖是數(shù)據(jù)結(jié)構(gòu)中非常重要的一個(gè)概念,包含了節(jié)點(diǎn)和邊。在自動(dòng)駕駛中,通?梢詫⒌貓D存儲(chǔ)為柵格地圖,每一格就代表了圖的節(jié)點(diǎn),格與格之間的連線就代表了邊。

上圖展示了一種無(wú)向圖,即節(jié)點(diǎn)之間的連線是沒(méi)有指向的。而在實(shí)際場(chǎng)景中,往往每條邊(道路)不僅僅需要考慮距離信息,還需要考慮方向信息、路口擁堵情況、車流量等等,因此自動(dòng)駕駛中往往構(gòu)建的為有向圖、權(quán)重圖等等。除此之外,合理地對(duì)自動(dòng)駕駛場(chǎng)景下的地圖進(jìn)行分割也是保證規(guī)劃效果的一個(gè)很重要的基礎(chǔ),不能分割太密集導(dǎo)致規(guī)劃搜索的效率太低,也不能太粗略從而導(dǎo)致某些場(chǎng)景下明明存在可行解卻無(wú)法搜索到。 構(gòu)建完圖之后,具體的規(guī)劃過(guò)程其實(shí)就是一個(gè)搜索的過(guò)程,即如何在給定起點(diǎn)及終點(diǎn)的條件下快速搜索出一條滿足期望的最優(yōu)路徑。在代碼實(shí)現(xiàn)上,整個(gè)過(guò)程需要維護(hù)一個(gè)容器(container),具體的操作分為三個(gè)步驟:移除、擴(kuò)展、塞入,以此不停循環(huán),直至搜索到終點(diǎn)。下面介紹幾種最常用的搜索算法。

        搜索算法DFS & BFS

了解了圖搜索的基礎(chǔ)之后,接下來(lái)介紹兩種最基礎(chǔ)的搜索算法:深度優(yōu)先搜索(DFS)和廣度優(yōu)先搜索(BFS)。深度優(yōu)先顧名思義,從起點(diǎn)開(kāi)始,按照某個(gè)順序一條路走下去,直至不能再繼續(xù)為止,然后回到上一節(jié)點(diǎn),再換另一條路走下去;而廣度優(yōu)先則是每一步都擴(kuò)展同一層的所有可能節(jié)點(diǎn),一層一層擴(kuò)展下去,直到某一層搜索到終點(diǎn)為止。可以看到深度優(yōu)先搜索的過(guò)程是一條路走到底后,最后訪問(wèn)的節(jié)點(diǎn)最先拿來(lái)處理,整個(gè)過(guò)程可以用棧(stack)來(lái)表示,符合“后進(jìn)先出”的原則;而廣度優(yōu)先搜索的過(guò)程是一層中先訪問(wèn)的節(jié)點(diǎn)拿來(lái)處理,可以用隊(duì)列(queue)來(lái)表示,符合“先進(jìn)先出”的原則。

那對(duì)于搜索算法來(lái)說(shuō),哪一種算法好一些呢?可以看下下面這張圖,相同的場(chǎng)景下,BFS可以給出一條最短路徑,而DFS雖然速度很快,但隨機(jī)性很大,無(wú)法給出一條最優(yōu)路徑,這一缺陷使得我們不得不拋棄DFS,目前的主流基于圖搜索的規(guī)劃算法,原理其實(shí)都是基于BFS延伸出來(lái)的。

但是BFS其實(shí)也有一個(gè)很嚴(yán)重的問(wèn)題,就是其遍歷的無(wú)效節(jié)點(diǎn)過(guò)多,從而導(dǎo)致搜索效率太慢,上面左圖中的深灰色格點(diǎn)就展示了在搜索過(guò)程中,所需要訪問(wèn)的節(jié)點(diǎn),可以看出大多數(shù)的訪問(wèn)其實(shí)都是無(wú)用的,不能給最終的搜索提供任何幫助。針對(duì)這一缺陷,就引入了Heuristic Search(啟發(fā)式搜索),即加入終點(diǎn)信息,從而使得搜索的目標(biāo)更明確,避免過(guò)多的無(wú)效搜索。而基于這一改進(jìn)提出的算法就是GBFS(Greedy Breath-First Search)。

BFS和DPS是根據(jù)先入或者后入的順序來(lái)選擇要處理的節(jié)點(diǎn),之中不考慮任何終點(diǎn)相關(guān)的信息,而GBFS則是將與終點(diǎn)的距離考慮進(jìn)來(lái),構(gòu)造一個(gè)規(guī)則來(lái)挑選依次要訪問(wèn)的節(jié)點(diǎn)。與終點(diǎn)的距離有多種形式,最常用的三種為Euclidean Distance、Manhattan Distance以及Diagonal Distance。

舉個(gè)例子,在實(shí)現(xiàn)BFS算法時(shí),上圖中起點(diǎn)周圍的8個(gè)鄰居節(jié)點(diǎn)會(huì)一起存儲(chǔ)進(jìn)容器中,由于右上角的節(jié)點(diǎn)距離終點(diǎn)更近,因此再?gòu)棾鰰r(shí)首先彈出該節(jié)點(diǎn),基于該節(jié)點(diǎn)再進(jìn)行擴(kuò)展,從而加快了搜索效率。從下圖中可以看出,算法過(guò)程中所訪問(wèn)的節(jié)點(diǎn)減少了很多,搜索的目標(biāo)性更加明確,從而極大提升了搜索效率。

Dijkstra算法和A*算法

有了上面的基礎(chǔ),理解路徑規(guī)劃中的Dijkstra和A*算法就很容易了。Dijkstra算法其實(shí)BFS的進(jìn)階版,其可以用于處理帶權(quán)重邊的地圖,因此更適合在實(shí)際場(chǎng)景中使用。在該算法中,通常采用優(yōu)先隊(duì)列(priority queue)來(lái)作為訪問(wèn)容器,這是由于優(yōu)先隊(duì)列(<key, value>這種形式)可以根據(jù)設(shè)定的key值自動(dòng)進(jìn)行排序,在Dijkstra中key值可以設(shè)定為和起點(diǎn)的距離,由于沒(méi)考慮和終點(diǎn)的距離信息,因此還不能顯示出優(yōu)先隊(duì)列的優(yōu)勢(shì),但之后的A*算法里可以看出利用這種結(jié)構(gòu)的方便性。Dijkstra算法的偽代碼如下圖所示:

A*算法和Dijkstra算法的唯一區(qū)別就在于優(yōu)先隊(duì)列中排序的依據(jù)不同,即key值不同。不同于Dijkstra,A*在存儲(chǔ)節(jié)點(diǎn)時(shí),還會(huì)考慮和終點(diǎn)的距離(可以類比GBFS之于BFS),其key值計(jì)算可以表示為:

其中即為Heuristic Function,有了這個(gè)指向信息,A*算法可以更快地找到終點(diǎn),避免了許多的無(wú)效搜索。其偽代碼如下圖所示:

這里我們可以看出優(yōu)先隊(duì)列的優(yōu)勢(shì)了,我們只需要每次計(jì)算的值并將其存儲(chǔ)進(jìn)優(yōu)先隊(duì)列,它就會(huì)自動(dòng)根據(jù)其值進(jìn)行排序,因此每次就可以取出容器的頂部值即為的最小值。在同一場(chǎng)景下,它們的實(shí)際效果如下圖所示,可以看出由于A*避免了許多無(wú)效節(jié)點(diǎn)的訪問(wèn),效率提升很多。 而這又引出了另一個(gè)問(wèn)題,Dijkstra由于無(wú)差別的搜索可以保證最短路徑,A*帶有強(qiáng)指向型的搜索方式,能保證結(jié)果最優(yōu)嗎?這其實(shí)取決于A*的啟發(fā)函數(shù)設(shè)定,為了保證最優(yōu)性,需要保證啟發(fā)函數(shù)是admissible的,即啟發(fā)函數(shù)的值需要小于等于實(shí)際上該點(diǎn)到終點(diǎn)的距離。

如果啟發(fā)式函數(shù)是admissible的,那么A*的最終搜索結(jié)果就是最優(yōu)的。其實(shí)這也很好理解,因?yàn)槿绻麊l(fā)函數(shù)的選擇實(shí)際上大于到終點(diǎn)的實(shí)際距離,那么依據(jù)該規(guī)則進(jìn)行的排序搜索,必然會(huì)漏掉距離最短的那條路。因此如果我們需要A*給出最短路徑的話,我們可以將啟發(fā)函數(shù)設(shè)定為歐式距離或者對(duì)角距離,而不是曼哈頓距離。
以上就是基于圖搜索的常用路徑規(guī)劃算法介紹,歡迎大家交流指正~

- End -

聲明: 本文由入駐維科號(hào)的作者撰寫,觀點(diǎn)僅代表作者本人,不代表OFweek立場(chǎng)。如有侵權(quán)或其他問(wèn)題,請(qǐng)聯(lián)系舉報(bào)。

發(fā)表評(píng)論

0條評(píng)論,0人參與

請(qǐng)輸入評(píng)論內(nèi)容...

請(qǐng)輸入評(píng)論/評(píng)論長(zhǎng)度6~500個(gè)字

您提交的評(píng)論過(guò)于頻繁,請(qǐng)輸入驗(yàn)證碼繼續(xù)

  • 看不清,點(diǎn)擊換一張  刷新

暫無(wú)評(píng)論

暫無(wú)評(píng)論

文章糾錯(cuò)
x
*文字標(biāo)題:
*糾錯(cuò)內(nèi)容:
聯(lián)系郵箱:
*驗(yàn) 證 碼:

粵公網(wǎng)安備 44030502002758號(hào)