王碩,段蓉凱,廖與禾

(1.西安交通大學現代設計及轉子軸承系統(tǒng)教育部重點實驗室,710049,西安;2.西安交通大學陜西省機械產品質量保障與診斷重點實驗室,710049,西安)

隨著社會和科學的不斷進步,機器人技術得到了迅速的發(fā)展,智能機器人逐漸在醫(yī)療衛(wèi)生、家庭服務、軍事航天、工業(yè)生產等領域得到廣泛的應用。與此同時,路徑規(guī)劃作為機器人技術中重要的研究領域,也開始迅速發(fā)展[1-4]。

路徑規(guī)劃的研究目的是在有障礙物的環(huán)境中,使機器人可通過算法自主規(guī)劃出無碰撞的、從起點到目標點的可行路徑,或滿足某些約束條件的最優(yōu)路徑[5]。常用的算法有圖搜索法[6]、柵格法[7]、人工勢場法[8]、遺傳算法[9]、蟻群算法[10]、RRT法[11]等。其中,RRT算法因具有泛用性好、適合移動機器人和多自由度工業(yè)機器人等特點而被廣泛使用,但由于采樣過程的隨機性,基本RRT算法存在樹的擴展隨機性大、冗余節(jié)點多、容易在目標點周圍發(fā)生振蕩、規(guī)劃的路徑較長等問題。

為解決基本RRT算法所存在的問題,國內外學者進行了大量有針對性的研究工作。例如,文獻[12]在基本RRT算法的基礎上提出了雙向隨機搜索(從起點和終點同時擴展兩顆樹)的RRT-Connect算法,減少了路徑規(guī)劃的時間;文獻[13]提出了引入貪婪策略的RRT-Connect算法,使樹盡可能地向目標點方向生長,搜索路徑更優(yōu);文獻[14]提出了動態(tài)調整父節(jié)點從而對路線進行優(yōu)化的RRT-star算法,縮短了規(guī)劃路徑的長度;文獻[15]提出了Quick-RRT-star算法,擴大了RRT-star算法動態(tài)調整父節(jié)點的范圍,對搜索路徑進行了優(yōu)化;文獻[16]以一定的概率將目標點作為采樣點,文獻[17]引入了權重系數,增加了樹向目標點方向的擴展分量,文獻[18]引入了引力系數,使樹在擴展時一直受到目標點方向的引力影響,這幾種方法都能使樹偏向目標點方向進行生長,從而減少了樹擴展時的隨機性。

這些研究在機器人路徑規(guī)劃方面進行了許多有益的探索,并取得了一定的進展。但目前對于樹擴展隨機性大的問題的改進研究大多只考慮了如何讓樹偏向目標點方向進行生長,而沒有考慮障礙物的影響,因此造成樹始終偏向目標點方向生長,無法根據障礙物信息靈活調整擴展方向并在障礙物附近產生大量無效節(jié)點的問題。此外,目前對于RRT算法容易在目標點附近發(fā)生振蕩問題的研究也還存在不足。為進一步解決這些問題,本文提出了一種改進的RRT算法。首先,通過引入動態(tài)權重系數以提高樹擴展時的靈活性;其次,針對擴展樹容易在目標點附近產生振蕩的問題,研究了擴展步長的自適應設置方法。最后,結合剪枝處理[19]和基于三次B樣條曲線[20]的平滑處理完成了規(guī)劃路徑的優(yōu)化。

1 RRT算法基本原理

基本RRT算法的擴展圖如圖1所示。

圖1 基本RRT算法擴展圖

該算法以起點為根節(jié)點,在狀態(tài)空間中進行隨機采樣,通過特定的約束條件進行隨機樹的擴展。當隨機樹上的點到達目標點設定的鄰域范圍內時,即可找到一條從起點到目標點的唯一路徑。

RRT算法的基本步驟如下。

(1)設在已知的空間環(huán)境中,有定義的起點xinit和目標點xgoal。

(2)在空間中隨機生成一個點xrand并遍歷已生成的擴展樹,找到距離xrand最近的節(jié)點xnearest,沿著xrand向xnearest方向擴展一定的距離e得到新節(jié)點xnew,連接xnearest和xnew得到直線L;檢測直線L是否和障礙物發(fā)生碰撞,如果未發(fā)生碰撞就將xnew加入樹中。

(3)重復步驟(2)直到新節(jié)點到達目標點xgoal的鄰域范圍內,連接xnew和xgoal,從xnew開始依次尋找并連接父節(jié)點,即完成起點到目標點的路徑規(guī)劃。

基本的RRT算法存在以下缺點[21-23]。

(1)樹擴展時隨機性較大:由于隨機采樣點是根據均勻函數產生的,導致新節(jié)點的生成缺乏偏向性。

(2)冗余節(jié)點較多:樹擴展時的隨機性使得RRT算法會在非目標方向的區(qū)域生成大量冗余節(jié)點,從而產生不合理的規(guī)劃路徑。

(3)樹容易在目標點周圍發(fā)生振蕩:為了提高樹擴展的效率,樹的擴展步長總是大于目標點鄰域,這導致樹擴展到目標點附近時容易發(fā)生振蕩現象。

(4)路徑平滑性不足:節(jié)點與節(jié)點之間通過直線連接,所規(guī)劃路徑的各段直線之間因此存在拐角,缺乏平滑性,難以滿足機器人運動的平穩(wěn)性要求。

2 改進RRT算法

基于上述對基本RRT算法和相關改進研究存在問題的分析,本文首先提出采用動態(tài)權重系數和自適應擴展步長的方式優(yōu)化路徑搜索過程,并在對初步規(guī)劃后的路徑進行剪枝處理以去除冗余節(jié)點后,利用平滑處理完成路徑的最終規(guī)劃,具體詳述如下。

2.1 動態(tài)權重系數

基本RRT算法在進行樹的擴展時沒有考慮目標點的引導信息,因此生成了大量遠離目標區(qū)域的無效節(jié)點,導致路徑規(guī)劃時具有較大的隨機性。文獻[17]提出了在目標點和隨機點兩個方向分別取固定權重系數p1和p2(p1和p2的最優(yōu)值由仿真實驗確定),再進行矢量合成確定擴展方向和擴展距離的方法。這種方法增加了樹擴展時向目標點的偏向性,能夠明顯地降低路徑規(guī)劃的時間,但是p1總是取大于p2的固定值導致遇到障礙物時容易產生大量無效節(jié)點,這就增加了路徑規(guī)劃所需的迭代次數。針對這一問題,本文引入了動態(tài)權重系數,使樹盡可能地在向目標點進行擴展的同時又能夠即時地避開障礙物。為了提高算法對障礙物的敏感性,本文將目標點方向和隨機點方向的權重系數取為p和1-p這兩個相互制約的參數。不考慮權重系數的變化,引入權重系數后樹的擴展示意圖如圖2所示。

圖2 引入權重系數后樹的擴展示意圖

新節(jié)點xnew的坐標由以下公式決定

(1)

式中:p代表權重系數,00.5時,新節(jié)點將偏向目標點方向;當p<0.5時,新節(jié)點將偏向隨機方向。

由于路徑搜索開始前障礙物的信息未知,路徑規(guī)劃過程中p的取值如可根據障礙物的情況自適應地動態(tài)調整,對于路徑規(guī)劃而言是更合理的方式。文獻[24]提出一種將人工勢場法與RRT算法相結合的方法。該方法與動態(tài)權重系數法效果相似,只是由于需要在生成新節(jié)點時不斷地更新引力和斥力這兩個參數,計算效率并不理想。本文在上述工作的基礎上,進一步提出一種p值動態(tài)調整的思路,即首先考慮目標點方向因素占主導的情況(p>0.5),使樹偏向目標點進行擴展。如果得到的xnew與xnearest之間的連線L與障礙物沒有發(fā)生碰撞,則將xnew加入擴展樹。否則,考慮隨機方向因素占主導的情況并調整p值使p<0.5,以便樹能快速避開障礙物。如果新的xnew與xnearest之間的連線L與障礙物沒有發(fā)生碰撞,則將xnew加入擴展樹。依此進入下一輪迭代。這種動態(tài)調整p值的方式讓樹能夠在偏向目標點擴展的同時,快速地避開障礙物。

2.2 自適應擴展步長

新節(jié)點xnew生成后會判斷該節(jié)點和目標點xgoal之間的距離是否小于目標點的鄰域λ,若滿足條件就將xnew和xgoal相連。一般情況下,為了確保樹擴展時的效率,擴展步長e總是大于λ,當xnearest和xgoal之間的距離為(λ,e)范圍內的值時,如果樹繼續(xù)以步長e進行擴展,容易造成新的節(jié)點xnew在目標點周圍振蕩,陷入局部死循環(huán),不僅增加路徑規(guī)劃的時間,而且使路徑在目標點附近較為曲折,增加了路徑的長度和路徑復雜度。為解決這一問題,本文中e將根據擴展樹與目標點之間的距離自適應地進行調整。記擴展步長的初始設定值為e1,當xnearest和xgoal之間的距離大于e1時,e=e1;當xnearest和xgoal之間距離為[λ,e]范圍內的值時,e=e2=‖xgoal-xnearest‖。固定擴展步長和自適應擴展步長的擴展示意圖如圖3所示(示意圖僅顯示擴展步長的對比,未考慮動態(tài)權重系數)。

(a)固定擴展步長擴展示意圖

2.3 路徑剪枝和平滑處理

引入動態(tài)權重系數后樹擴展的隨機性大大減小,但是由于生成的節(jié)點眾多,依然存在冗余節(jié)點。本文對初步路徑規(guī)劃得到的節(jié)點集合進行剪枝,從起點xinit開始盡量去除集合中不必要的節(jié)點。

剪枝處理的具體思路如下:對初步路徑規(guī)劃得到的節(jié)點集合,從起點xinit開始,依次遍歷后續(xù)節(jié)點xk,如果xinit和xk之間的連線和障礙物之間沒有發(fā)生碰撞,則將xinit和xk之間的所有節(jié)點都刪除;如果xinit和xk之間的連線和障礙物之間發(fā)生了碰撞,則從節(jié)點xk-1開始重復以上步驟,直到遍歷到目標點xgoal,最后將剩余的節(jié)點結合依次進行連接,得到新的路徑。這種剪枝方式極大地減少了路徑上的節(jié)點數量。

剪枝后路徑的拐點位置仍然是分段的,不平滑。機器人按照這樣的路徑行走,會在拐點處因為行進方向變化產生沖擊,影響機器人的運行平穩(wěn)性。因此需要進一步對路徑進行平滑處理。鑒于B樣條曲線精度較高,能夠較好地控制曲線的平滑度[25],且三次B樣條擬合曲線的曲率半徑較小,在拐點的過渡較為平滑,計算量也相對較少,本文選用三次B樣條曲線對剪枝后的路徑進行平滑處理。

K次B樣條曲線可表示為

(2)

其中,di為曲線控制的頂點,即樹的節(jié)點;Bi,K為K次B樣條曲線的基函數。K次分段曲線由節(jié)點向量U=[u0,u1,…,un+K+1]決定。三次B樣條曲線的基函數為

(3)

2.4 改進RRT算法流程

綜合以上幾種改進措施,改進的RRT算法流程圖如圖4所示。

圖4 改進的RRT算法流程圖

改進的RRT算法流程如下。

(1)在目標空間中,設定起點xinit和目標點xgoal,擴展步長e的初始值e1,目標點的鄰域λ。

(2)生成隨機采樣點xrand,遍歷擴展樹找到最近節(jié)點xnearest,檢測xnearest和xgoal之間的距離是否大于e1。

(3)如果步驟(2)條件滿足,則在e1的基礎上引入權重系數p1得到新節(jié)點xnew,否則將e的值設為新值e2,在e2的基礎上引入權重系數p1得到新節(jié)點xnew。

(4)檢測xnew和xnearest之間的連線L是否和障礙物發(fā)生碰撞,如果未發(fā)生碰撞,就將xnew加入擴展樹,否則在當前e取值的基礎上引入權重系數p2得到新節(jié)點xnew。

(5)檢測xnew點和xnearest之間的連線L是否和障礙物發(fā)生碰撞,如果未發(fā)生碰撞,就將xnew加入擴展樹,否則返回步驟(2)。

(6)檢測xinit和xgoal之間的距離是否小于λ,如果滿足條件,連接xnew和目標點,從xnew開始依次尋找并連接父節(jié)點得到起點到目標點的路徑。

(7)對生成的初始路徑進行剪枝處理和曲線擬合。

3 仿真驗證

3.1 初始路徑規(guī)劃仿真

為了對改進的RRT算法的效果進行驗證,在matlab中進行了二維空間環(huán)境下的仿真。設計的地圖包含較多障礙物,以驗證改進的算法能夠快速地避開障礙物。環(huán)境場景的大小為800 mm×800 mm,設定起點坐標(50 mm,50 mm),目標點坐標(750 mm,750 mm),初始擴展步長e1=50 mm,目標點鄰域λ=20 mm,分別使用RRT算法、RRT-star算法、固定權重系數的RRT算法(引入自適應擴展步長)和本文改進的RRT算法進行路徑規(guī)劃對比分析。為了確保樹的偏向性,固定權重系數的RRT算法的權重系數p取值為0.8,本文改進的RRT算法的權重系數p取值為0.8(目標點方向因素占主導時)和0.2(隨機方向因素占主導時)。黑色粗線代表規(guī)劃出的路徑,四種算法的規(guī)劃結果如圖5所示。

(a)基本RRT算法

表1為4種算法在較多障礙物環(huán)境下分別進行500次路徑規(guī)劃所得的平均數據。數據表明,在路徑規(guī)劃時間和路徑長度這兩個主要指標上,改進的RRT算法相較基本RRT算法和RRT-star算法都有明顯的優(yōu)勢。改進的RRT算法和基本RRT算法相比,路徑規(guī)劃時間減少54.08%,路徑長度減少19.56%;改進的RRT算法和RRT-star算法相比,路徑長度的減小雖然不甚明顯,但所需的路徑規(guī)劃時間大幅減少。改進的RRT算法相較使用固定權重系數的RRT算法,雖然路徑長度略微增加了5.21%,但是路徑規(guī)劃時間大幅減少了60.31%,算法的改進是有效的。

表1 4種算法的路徑規(guī)劃數據

3.2 自適應擴展步長仿真

為進一步檢驗動態(tài)權重系數和自適應擴展步長的有效性,繼續(xù)在保持仿真環(huán)境和其他參數不變的條件下,對僅引入自適應擴展步長的改進算法進行檢驗,仿真分析結果如圖6所示。可以看到,基本RRT算法在目標點xgoal附近發(fā)生了振蕩(圖6(a)),而引入自適應擴展步長的RRT算法會根據xnearest和目標點之間的距離動態(tài)地調整擴展步長,有效地減少了振蕩并迅速到達路徑目標點(圖6(b))。

(a)基本RRT算法

表2為基本RRT算法、僅引入自適應擴展步長的RRT算法和改進的RRT算法在較多障礙物環(huán)境下分別進行500次路徑規(guī)劃所得到的平均數據。數據表明,引入自適應擴展步長的RRT算法和基本RRT算法相比有明顯提升,改進是有效的。引入自適應擴展步長的RRT算法和基本RRT算法相比,路徑規(guī)劃時間減少34.74%,路徑長度減少2.08%。進一步的數據分析還表明,僅考慮自適應擴展步長的RRT算法在路徑規(guī)劃時間和路徑長度這兩個指標上與改進的RRT算法相比均仍有較大差距。因此,在路徑規(guī)劃中同時考慮動態(tài)權重系數和自適應擴展步長有其重要性。

表2 3種算法的路徑規(guī)劃數據

3.3 剪枝及平滑處理仿真

為了減少冗余節(jié)點并提高路徑的平滑性,將初步規(guī)劃后的路徑進行剪枝并采用三次B樣條進行平滑處理,matlab中的仿真結果如圖7所示。

(a)剪枝處理

從圖7中可以看出,剪枝后的路徑節(jié)點和拐角都大大減少,平滑處理后的路徑和原路徑相比具有較好的平滑性,能夠更好地滿足機器人運動的平穩(wěn)性要求。

4 結 論

針對基本RRT算法和相關改進研究存在的問題,本文針提出了一種改進的RRT算法,該算法通過引入動態(tài)權重系數和自適應擴展步長,減少了樹擴展的隨機性以及樹在目標點周圍的振蕩現象;并對初步規(guī)劃后的路徑進行剪枝和三次B樣條平滑處理。本算法在多處對基本RRT算法進行了改進,具有一定的參考價值。

根據仿真結果,與基本RRT算法相比,本算法的有效性和優(yōu)越性主要體現在以下幾個方面:

(1)與基本RRT算法相比,改進的RRT算法進行路徑規(guī)劃所需的時間大幅降低,在仿真環(huán)境下,路徑規(guī)劃時間減少54.8%;

(2)與基本RRT算法相比,改進的RRT算法規(guī)劃的路徑長度減少,在仿真環(huán)境下,路徑長度減少了19.56%;

(3)將改進的RRT算法初步規(guī)劃得到的路徑進行剪枝和平滑處理后,新的路徑具有較好的平滑性,能夠更好地滿足機器人運動的要求。