全局路徑規劃RRT算法原理

無人駕駛路徑規劃

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

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

路徑規劃問題可以分為兩個方面：

（一）全局路徑規劃：全局路徑規劃算法屬于靜態規劃算法，根據已有的地圖信息（SLAM）為基礎進行路徑規劃，尋找一條從起點到目標點的最優路徑。

通常全局路徑規劃的實現包括Dijikstra算法，A*算法，RRT算法等經典算法，也包括蟻群算法、遺傳算法等智能算法；

（二）局部路徑規劃：局部路徑規劃屬于動態規劃算法，是無人駕駛汽車根據自身傳感器感知周圍環境，規劃處一條車輛安全行駛所需的路線，常應用于超車，避障等情景。通常局部路徑規劃的實現包括動態窗口算法（DWA），人工勢場算法，貝塞爾曲線算法等，也有學者提出神經網絡等智能算法。

本系列就從無人駕駛路徑規劃的這兩方面進行展開，對一些經典的算法原理進行介紹，并根據個人的一些理解和想法提出了一些改進的意見，通過Matlab2019對算法進行了仿真和驗證。過程中如果有錯誤的地方，歡迎在評論區留言討論，如有侵權請及時聯系。

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

全局路徑規劃 - RRT算法原理

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

當目標點進入隨機樹里面后，隨機樹擴展立即停止，此時能找到一條從起始點到目標點的路徑。算法的計算過程如下：

step1：初始化隨機樹。將環境中起點作為隨機樹搜索的起點，此時樹中只包含一個節點即根節點；

stpe2：在環境中隨機采樣。在環境中隨機產生一個點，若該點不在障礙物范圍內則計算隨機樹中所有節點到的歐式距離，并找到距離最近的節點，若在障礙物范圍內則重新生成并重復該過程直至找到;

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

step4：停止搜索。當和目標點之間的距離小于設定的閾值時，則代表隨機樹已經到達了目標點，將作為最后一個路徑節點加入到隨機樹中，算法結束并得到所規劃的路徑 。

RRT算法由于其隨機采樣及概率完備性的特點，使得其具有如下優勢：

（1）不需要對環境具體建模，有很強空間搜索能力；

（2）路徑規劃速度快；

（3）可以很好解決復雜環境下的路徑規劃問題。

但同樣是因為隨機性，RRT算法也存在很多不足的方面：

（1）隨機性強，搜索沒有目標性，冗余點多，且每次規劃產生的路徑都不一樣，均不一是最優路徑；

（2）可能出現計算復雜、所需的時間過長、易于陷入死區的問題；

（3）由于樹的擴展是節點之間相連，使得最終生成的路徑不平滑；

（4）不適合動態環境，當環境中出現動態障礙物時，RRT算法無法進行有效的檢測；

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