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

朱旻華 任明武

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

1 引言

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

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

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

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

2 基本RRT算法

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

圖1 基本RRT擴展示意圖

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

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

3 基于勢場引導的RRT算法

3.1 人工勢場法

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

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

其中，引力勢能函數(shù)和斥力勢能函數(shù)為

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

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

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

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

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

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

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

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

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

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

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

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

3.3 路徑優(yōu)化的后處理方法

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

去除了路徑中的冗余之后，路徑中不再有重復環(huán)路。但路徑中的折點較多，且很多折點是不必要的。在該優(yōu)化路徑基礎(chǔ)上繼續(xù)對路徑進行剪枝優(yōu)化，去除路徑中不必要的折點，使路徑更光滑。

通過兩次對路徑的優(yōu)化，可以得到一個較優(yōu)路徑。

4 仿真實驗結(jié)果分析

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

圖4 RRT算法和本文改進算法的實驗結(jié)果圖

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

表1 RRT和本文改進算法的仿真結(jié)果

通過表1中數(shù)據(jù)可以看出，改進方法的效率更高，且通過對規(guī)劃路徑的優(yōu)化，使得規(guī)劃路徑較短。由于改進方法中有人工勢場的啟發(fā)信息，降低了RRT算法中隨機性帶來的盲目性，算法運行結(jié)果相對穩(wěn)定。

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

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

5 結(jié)語

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