我的漂亮朋友,91国产视频在线

這篇文章想和讀者分享一下自動駕駛規(guī)劃算法中一類較常用的算法，基于采樣的規(guī)劃算法。之前的文章中說到，基于圖搜索的規(guī)劃算法總是能給出全局范圍內(nèi)的最優(yōu)解，而以此為代價的就是當(dāng)?shù)貓D過大，規(guī)劃的維度過高時，它的搜索效率就會變得很慢。在某些場景下，我們不一定需要規(guī)劃出的路徑是全局最優(yōu)解，只要是可行解就能滿足我們的需求，而我們的關(guān)注點主要放在求解效率上。這時，我們就可以為了效率犧牲最優(yōu)性，也就引出了這篇文章的內(nèi)容——基于采樣的路徑規(guī)劃算法。

PRM 算法

首先介紹的第一種就是經(jīng)典的PRM算法（Probabilistic Road Map）。PRM主要包含了兩個步驟，一是學(xué)習(xí)階段，二是查詢階段。在學(xué)習(xí)階段中，其會在地圖空間中進(jìn)行均勻的隨機采樣，并刪除采樣落在障礙物上的點，接著對相鄰的點進(jìn)行連接并做碰撞檢測，剔除不是collision-free的連線。而在查詢階段中，就是利用上一步構(gòu)建好的采樣節(jié)點及連續(xù)，運用圖搜索的方法（Dijkstra或者A*）來找出一條可行路徑。

考慮到上述的算法缺陷，有一種提升PRM的方法就是Lazy Collision-checking，即在學(xué)習(xí)的階段不考慮障礙物的碰撞（不進(jìn)行collision-checking的步驟），而在查詢階段，如果搜索出的路徑中含有碰撞的部分，則刪除該部分并只對這部分進(jìn)行相鄰節(jié)點的重新尋路。　

RRT算法

接下來，介紹第二種基于采樣的算法，也是最常用的一種——RRT(Rapidly-exploring Random Tree)算法。RRT其實代表了一系列基于隨機生長樹思想的算法，也是目前機器人領(lǐng)域運用最廣泛、優(yōu)化變種最多的一類算法，首先介紹一下最原始的RRT算法。

RRT的算法流程偽代碼如上圖所示，首先對地圖進(jìn)行隨機采樣，得到采樣點x_rand。再從已有的樹中找到距離x_rand最近的點x_near，從ｘ_near往x_rand的方向行駛距離StepSize得到x_new，并將x_near與x_new的連線添加進(jìn)一個臨時變量中。接著對這段路進(jìn)行碰撞檢測，如果無碰撞，則將x_new添加進(jìn)樹中。如此不停循環(huán)，直到x_new到達(dá)期望的終點時結(jié)束。

最終的運行效果如上圖所示，可以看出rrt算法雖然很快，但給出的解往往是曲折環(huán)繞的，控制模塊往往也無法跟蹤這樣的路徑。該算法的優(yōu)缺點如下：

RRT Family

上面說到RRT的速度是非常快的，但是針對Narrow Passage這種場景，傳統(tǒng)的RRT算法就會顯得比較笨拙，需要花費很長的時間才能找到可行路徑，如下圖所示。

為了解決這個問題，需要對RRT算法進(jìn)行一些改進(jìn)。
1. Bidirectional RRT第一種改進(jìn)思路就是不同于只從起點開始出發(fā)，而是從起點和終點同時出發(fā)，從而構(gòu)建出兩棵樹，當(dāng)兩棵樹相互連接至一起時，則說明已經(jīng)找到路徑，進(jìn)而進(jìn)行雙回溯得到路徑。這種思路可以很好地解決當(dāng)終點落在Narrow Passage內(nèi)這種場景，由于從狹窄通道內(nèi)直接出發(fā)尋點，采樣到駛出通道點的概率也就高了許多。
2. RRT*在RRT算法中，每次都會選擇距離最近的點作為新加入點的父節(jié)點，但顯然這種做法不一定是最合理的。而RRT*所進(jìn)行的改變主要為兩點：重選父節(jié)點和重連接。RRT*的偽代碼如下所示：

RRT*的思想是，它在采樣點加入進(jìn)樹結(jié)構(gòu)之后，以其為圓心再畫一個圓，在這個圓之內(nèi)，搜索是否有其余點與采樣點連接后，能夠使得起點到采樣點的距離更短。如果有的話，就對采樣點的父節(jié)點進(jìn)行更新，并重連采樣點與其新的父節(jié)點。RRT*的優(yōu)勢就在于，不同于RRT算法只找到一條可行路徑就結(jié)束，RRT*會不停的循環(huán)迭代，不停地更新當(dāng)前路徑，因此只要時間足夠長的話，RRT*是可以求得最短路徑的。下圖中就可以很明顯的看出RRT*求得的最終結(jié)果和RRT的最終結(jié)果，顯然RRT*搜索出的路徑是短許多的。

3. Kinodynaimic RRT* 上面講的這類RRT算法都是簡單的將兩點進(jìn)行連線，這樣導(dǎo)致最終生成的路徑會相當(dāng)不平滑，甚至相鄰的線段都是無法滿足機器人的動力學(xué)要求的。也就是說雖然規(guī)劃出了路徑，但是給到控制模塊卻沒有辦法執(zhí)行。因此，不同于直線連接，而是通過曲線、多項式等來連接x_near和x_new，從而使得生成的路徑能夠符合車輛動力學(xué)的相關(guān)約束。 4. Any-Time RRT* Any-Time RRT*如名稱所示，其會不停地更新路徑，即不停地以當(dāng)前位置作為規(guī)劃起始點，來重新規(guī)劃路徑。這種方式可以更好地應(yīng)對動態(tài)環(huán)境變化比較多的場景。

5. Informed RRT*前面說到RRT*在找到一條路徑之后，繼續(xù)不停地在全局空間中進(jìn)行重采樣從而更新路徑。但是在已經(jīng)找到一條路徑的情況下，再在全局范圍內(nèi)隨機撒點顯然不是一種高效的行為，而Informed RRT*的主要思想就是在生成一條初始路徑后，構(gòu)建出一個橢圓區(qū)域，然后在橢圓區(qū)域內(nèi)進(jìn)行隨機采樣并更新路徑，這樣就避免了在無效區(qū)域內(nèi)的浪費時間，從而極大提升了路徑優(yōu)化的效率。