基于多安全度模型的改進RRT路徑規劃方法*

朱旻華 任明武

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

近年來，隨著智能化信息處理技術的進步，自動駕駛技術得到了顯著的發展，該技術能最大限度地減輕駕駛員負擔、減少交通事故從而提升道路交通的安全性。路徑規劃的主要任務是根據全局地圖數據信息規劃出一條從起點至終點、無碰撞的路徑，是無人駕駛系統中的關鍵技術，具有重要的研究價值［1］。

傳統的路徑規劃算法可分為以下幾類：基于圖形學的算法提供了一些環境建模的方法，如可視圖法、柵格圖法、Voronoi圖法、切線圖法［2～5］等；Dijkstra、A*、D*、人工勢場法［6～9］等前向圖搜索算法在建模基礎上提供了路徑搜索方法；通過對仿生學的研究提出的算法主要有蟻群算法、遺傳算法、模擬退火算法［10～12］等。前向圖搜索算法和智能仿生算法雖然都能收斂到全局最優或較優路徑，但隨著機器人自由度和規劃空間維度的增加，會導致算法復雜度激增，造成維度災難。隨機采樣類的規劃算法很好的解決了這個問題，主要有概率路標算法［13］、快速擴展隨機樹［14］等方法。

快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）由 Steven M.LaValle于 1998年首次提出，是一種單查詢隨機搜索方法。樹中的子節點根據當前環境信息隨機擴展生成，避免了對環境空間的建模，可以有效地擴大問題規模。由于RRT算法在擴展時的隨機性，不可避免地導致了規劃路徑的隨機性和不穩定性，無法高效穩定地完成規劃任務。針對基本RRT的這些缺陷，文獻［15］提出了目標偏向的Goal-biasing算法，在擴展子節點時以一定的概率以目標為導向向著終點方向擴展；文獻［16］提出了雙向搜索樹Bi-direction RRT算法，同時從起點和終點進行雙向搜索，加快了收斂速度；文獻［17］提出了漸進最優的RRT*算法，采用代價函數來選取擴展節點鄰域中代價最小的節點；文獻［18］對于三種不同運動模型進行分析，提出了非完整約束下的RRT方法。

由于RRT算法中擴展子節點時的隨機性強，算法在擴展節點時缺少引導、擴展效率低。路徑的隨機性使得規劃路徑不穩定，且缺乏對自主車安全度的考慮。

2 基本RRT算法

RRT算法將狀態空間中的初始點作為根節點，通過隨機采樣子節點的方式增量構造隨機擴展樹，當隨機樹中的子節點包含目標區域的點或目標點時停止擴展，就可以在隨機樹中找到一條從根節點到目標點的路徑。其擴展方式如圖1所示。

圖1 基本RRT擴展示意圖

以初始狀態點qinit為根節點，在狀態空間內隨機生成一個采樣點qrand，qnear為離該隨機點最近的子節點，在qnear和qrand的連線上以一定的步長step擴展一個新的子節點qnew，若qnew與障礙物未發生碰撞，則將qnew加入隨機擴展樹中，反之則舍棄該點重新選擇qnew。重復上述步驟直至qnew到達qgoal或附近的目標區域，則成功找到一條從qinit到qgoal的規劃路徑。RRT算法偽代碼如下。

其中，函數Random_State（）根據均勻分布的概率獲取狀態空間中的隨機采樣點qrand；Nearest_Neighbor（qrand，Tree）尋找隨機樹中離隨機采樣點 qrand最近的 qnear點；New_State（qrand，qnear）對新節點進行碰撞檢測，若無碰撞則將新節點加入隨機樹，直到新節點與目標點足夠接近，表示搜索成功，返回隨機樹。

3 基于勢場引導的RRT算法

3.1 人工勢場法

Khatib最早于1986年提出了一種虛擬力法，即人工勢場法［19］。該方法的主要思想是將機器人所處的周圍環境模擬成一個勢場，將機器人在空間環境中的運動視作機器人在該虛擬力場中的運動：目標點的引力場（attraction）對機器人有吸引性，引導物體向目標點方向運動；障礙物的斥力場（repulsion）對機器人有排斥性，避免物體與之發生碰撞。自主車在虛擬力場中的受力為目標點的引力和障礙物的斥力作用在自主車上的合力，合力牽引著自主車在狀態空間中沿著勢場下降的方向做無碰運動。

假設自主車在狀態空間中的位置為q，該點的引力勢能和斥力勢能分別表示為和，則自主車在該點的勢能為

其中，引力勢能函數和斥力勢能函數為

在實際規劃問題中，每個障礙物可能由于其危險程度的高低［20］、輻射范圍大小的區別，對自主車規劃路線也會造成不同的影響。如若增加對自主車安全度的考慮，路徑規劃問題的維度增大，可能會導致算法效率的大幅下降。

3.2 基于人工勢場引導的概率分布模型

對于RRT算法和人工勢場法的研究發現，RRT算法節點擴展時的隨機性導致的盲目性使得規劃出的路徑節點狀態突變較大，路徑點可能距離障礙物很近，不易得到安全可靠的最優解且算法不穩定，但正是由于其隨機性強，規劃路徑不會出現陷入局部極小值無法逃脫的問題；人工勢場法相對穩定，規劃出的路徑相對平滑且安全，但由于局部極小值的存在可能使得目標不可達，導致規劃失敗。兩種路徑規劃算法在一定程度上可以優勢互補。所以，本文在基本RRT算法的基礎上引入了人工勢場引導的概率分布模型，提出了一種安全高效的改進算法。

原RRT算法的隨機性主要由Random_State（）函數體現，而Random_State（）函數采用的是整個狀態空間內的均勻分布，即狀態空間內的點被選中作為下一個擴展點的概率是一樣的。根據人工勢場的思想，自主車離障礙物越近，受到的斥力越大，自主車向著障礙物方向運動的概率越小，離障礙物越近的點被擴展為下一個隨機點的概率越小；同理，由于目標點的引力作用，自主車有向著目標點運動的趨勢，距離目標點越近的點被擴展的概率越大。即將Random_State（）函數中均勻分布的概率模型改為人工勢場引導的概率分布模型，使隨機樹在遠離障礙物的安全區域有較大概率生成下一個隨機點，而在靠近障礙物的危險區域有較小或沒有概率生成下一個隨機點，從而改善原RRT方法缺乏安全度的不足。

該改進方法適用于較大地圖任務空間中的全局規劃。將任務空間離散化為柵格地圖，若每個柵格表示20m*20m的空間，對于空間中的每個柵格，其8方向鄰域的柵格在無障礙的前提下對于自主車都是可達的。將RRT算法中的子節點擴展步長限制在柵格的8鄰域中，通過計算8鄰域柵格在人工勢場下的引力權重和斥力權重，原RRT中隨機擴展點的生成則轉化為當前柵格點8鄰域間的輪盤賭問題。

在實際情況中，每個障礙物都有不同的危險程度，例如戰場環境里的爆炸物比道邊危險度高很多，與障礙物距離越遠的柵格點的危險程度越低，安全度越高。假設一個障礙物點的危險度為3，則該障礙物輻射范圍內的柵格點的危險度如圖2所示。對于在多個障礙物輻射范圍內的柵格點，取最高的危險度作為該點的危險度。

圖2 危險度為3的障礙物周圍柵格的危險度示意圖

同理，由于目標點對自主車引力的影響，每個地圖中柵格都存在一個“趨向度”，距離目標點越近的節點趨向度越高。對于自主車所在柵格位置的8鄰域中，根據與目標點間的距離由小到大排序，距離最小的鄰域柵格的趨向度為8，距離最大的柵格趨向度為1。

若每個鄰域柵格的初始權重為weight0，則柵格權重的計算公式為

其中，katt和krep分別為引力系數和斥力系數。則每個鄰域柵格被擴展為隨機樹下一個子節點的概率為

該改進算法的流程圖如圖3所示。

圖3 本文改進算法的流程圖

3.3 路徑優化的后處理方法

由于每次擴展的新節點是從起點柵格依次根據啟發信息擴展鄰域柵格得到的，所以路徑搜索過程中可能會多次到達同一個柵格點，形成自相交的路徑。規劃路徑是一個包含了若干有序路徑點的集合，若從起點出發后，多次到達了同一個坐標點，則認為該規劃路徑中存在著冗余環，首次到達該點和最后一次到達該點間經過的柵格點都是多余的，需要從路徑中刪除。

去除了路徑中的冗余之后，路徑中不再有重復環路。但路徑中的折點較多，且很多折點是不必要的。在該優化路徑基礎上繼續對路徑進行剪枝優化，去除路徑中不必要的折點，使路徑更光滑。

通過兩次對路徑的優化，可以得到一個較優路徑。

4 仿真實驗結果分析

基礎RRT算法和本文的改進RRT方法的仿真實驗結果圖分別如圖4（a）、（b）所示。地圖大小為1024*768，將該地圖映射到柵格地圖中，柵格地圖的大小為60*50。起點用實心圓的點表示，終點用空心圓的點表示，灰色的線體現了路徑的搜索過程，黑色的線條即為算法最終搜索到的一條路徑。在基于勢場引導的改進RRT結果圖中，虛線是去除重復環之后的優化路徑，黑色的線是剪枝優化后的最終路徑。

圖4 RRT算法和本文改進算法的實驗結果圖

在該地圖上對兩種算法分別進行了100次仿真實驗，實驗結果如表1所示。

表1 RRT和本文改進算法的仿真結果

通過表1中數據可以看出，改進方法的效率更高，且通過對規劃路徑的優化，使得規劃路徑較短。由于改進方法中有人工勢場的啟發信息，降低了RRT算法中隨機性帶來的盲目性，算法運行結果相對穩定。

改進算法中的多安全度模型主要體現在障礙物危險度上，障礙物的危險度越高，算法在附近嘗試擴展節點的概率越低。圖4中的（a）為障礙物危險度為2的局部地圖，（b）為障礙物危險度為7的局部地圖，可以看出算法在圖（b）中障礙物周圍嘗試擴展的次數遠小于圖（a）。

圖5 在不同危險度的障礙物周圍的路徑搜索結果

5 結語

RRT算法不需要對狀態空間進行預處理而能快速搜索空間狀態點，有效避免了Dijkstra、A*等算法中復雜度高的問題。但RRT擴展節點時的盲目性使得算法效率較低，且在搜索路徑時沒有考慮到設備的安全性，規劃路徑可能處于障礙物周圍的危險區域。本文引入人工勢場，提出了一種基于多安全度模型的改進RRT算法，該算法能夠模擬人工勢場法中引力斥力對自主車的影響，使路徑盡可能遠離危險障礙，保證了規劃路徑的安全性，且啟發信息的加入使得算法能夠更快地收斂到一個解決方案。在此基礎上對路徑進行優化，優化后的路徑更接近于最優解，更符合實際情況。