全局路徑規(guī)劃RRT算法原理

無人駕駛路徑規(guī)劃

眾所周知，無人駕駛大致可以分為三個(gè)方面的工作：感知，決策及控制。

路徑規(guī)劃是感知和控制之間的決策階段，主要目的是考慮到車輛動(dòng)力學(xué)、機(jī)動(dòng)能力以及相應(yīng)規(guī)則和道路邊界條件下，為車輛提供通往目的地的安全和無碰撞的路徑。

路徑規(guī)劃問題可以分為兩個(gè)方面：

（一）全局路徑規(guī)劃：全局路徑規(guī)劃算法屬于靜態(tài)規(guī)劃算法，根據(jù)已有的地圖信息（SLAM）為基礎(chǔ)進(jìn)行路徑規(guī)劃，尋找一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。

通常全局路徑規(guī)劃的實(shí)現(xiàn)包括Dijikstra算法，A*算法，RRT算法等經(jīng)典算法，也包括蟻群算法、遺傳算法等智能算法；

（二）局部路徑規(guī)劃：局部路徑規(guī)劃屬于動(dòng)態(tài)規(guī)劃算法，是無人駕駛汽車根據(jù)自身傳感器感知周圍環(huán)境，規(guī)劃處一條車輛安全行駛所需的路線，常應(yīng)用于超車，避障等情景。通常局部路徑規(guī)劃的實(shí)現(xiàn)包括動(dòng)態(tài)窗口算法（DWA），人工勢場算法，貝塞爾曲線算法等，也有學(xué)者提出神經(jīng)網(wǎng)絡(luò)等智能算法。

本系列就從無人駕駛路徑規(guī)劃的這兩方面進(jìn)行展開，對一些經(jīng)典的算法原理進(jìn)行介紹，并根據(jù)個(gè)人的一些理解和想法提出了一些改進(jìn)的意見，通過Matlab2019對算法進(jìn)行了仿真和驗(yàn)證。過程中如果有錯(cuò)誤的地方，歡迎在評(píng)論區(qū)留言討論，如有侵權(quán)請及時(shí)聯(lián)系。

那么廢話不多說，直接進(jìn)入第一部分的介紹，全局路徑規(guī)劃算法-RRT算法。

全局路徑規(guī)劃 - RRT算法原理

RRT算法，即快速隨機(jī)樹算法（Rapid Random Tree），是LaValle在1998年首次提出的一種高效的路徑規(guī)劃算法。RRT算法以初始的一個(gè)根節(jié)點(diǎn)，通過隨機(jī)采樣的方法在空間搜索，然后添加一個(gè)又一個(gè)的葉節(jié)點(diǎn)來不斷擴(kuò)展隨機(jī)樹。

當(dāng)目標(biāo)點(diǎn)進(jìn)入隨機(jī)樹里面后，隨機(jī)樹擴(kuò)展立即停止，此時(shí)能找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑。算法的計(jì)算過程如下：

step1：初始化隨機(jī)樹。將環(huán)境中起點(diǎn)作為隨機(jī)樹搜索的起點(diǎn)，此時(shí)樹中只包含一個(gè)節(jié)點(diǎn)即根節(jié)點(diǎn)；

stpe2：在環(huán)境中隨機(jī)采樣。在環(huán)境中隨機(jī)產(chǎn)生一個(gè)點(diǎn)，若該點(diǎn)不在障礙物范圍內(nèi)則計(jì)算隨機(jī)樹中所有節(jié)點(diǎn)到的歐式距離，并找到距離最近的節(jié)點(diǎn)，若在障礙物范圍內(nèi)則重新生成并重復(fù)該過程直至找到;

stpe3：生成新節(jié)點(diǎn)。在和連線方向，由指向固定生長距離生成一個(gè)新的節(jié)點(diǎn)，并判斷該節(jié)點(diǎn)是否在障礙物范圍內(nèi)，若不在障礙物范圍內(nèi)則將添加到隨機(jī)樹 中，否則的話返回step2重新對環(huán)境進(jìn)行隨機(jī)采樣；

step4：停止搜索。當(dāng)和目標(biāo)點(diǎn)之間的距離小于設(shè)定的閾值時(shí)，則代表隨機(jī)樹已經(jīng)到達(dá)了目標(biāo)點(diǎn)，將作為最后一個(gè)路徑節(jié)點(diǎn)加入到隨機(jī)樹中，算法結(jié)束并得到所規(guī)劃的路徑 。

RRT算法由于其隨機(jī)采樣及概率完備性的特點(diǎn)，使得其具有如下優(yōu)勢：

（1）不需要對環(huán)境具體建模，有很強(qiáng)空間搜索能力；

（2）路徑規(guī)劃速度快；

（3）可以很好解決復(fù)雜環(huán)境下的路徑規(guī)劃問題。

但同樣是因?yàn)殡S機(jī)性，RRT算法也存在很多不足的方面：

（1）隨機(jī)性強(qiáng)，搜索沒有目標(biāo)性，冗余點(diǎn)多，且每次規(guī)劃產(chǎn)生的路徑都不一樣，均不一是最優(yōu)路徑；

（2）可能出現(xiàn)計(jì)算復(fù)雜、所需的時(shí)間過長、易于陷入死區(qū)的問題；

（3）由于樹的擴(kuò)展是節(jié)點(diǎn)之間相連，使得最終生成的路徑不平滑；

（4）不適合動(dòng)態(tài)環(huán)境，當(dāng)環(huán)境中出現(xiàn)動(dòng)態(tài)障礙物時(shí)，RRT算法無法進(jìn)行有效的檢測；

（5）對于狹長地形，可能無法規(guī)劃出路徑。