王稷堯，袁鋒偉

（南華大學機械工程學院，衡陽 421001）

0 引言

隨著智能機器人不斷發(fā)展，路徑規(guī)劃問題研究越來越深入。路徑規(guī)劃指機器人在當前環(huán)境中按照一定的標準，搜索出一條從起始狀態(tài)點到目標狀態(tài)點，能夠繞開障礙物的最優(yōu)或次優(yōu)路徑[1]。根據(jù)其原理不同，路徑規(guī)劃算法有人工勢場法[2]、神經(jīng)網(wǎng)絡法[3]、遺傳算法[4]等。其中，以RRT算法為代表的采樣規(guī)劃算法，通過一個初始點作為根節(jié)點，采用隨機采樣的方式增加樹結構的分支，當分支節(jié)點包含目標節(jié)點或進入目標區(qū)域，即可快速生成一條由樹節(jié)點生成的從初始點到目標點的路徑，具有概率完備性，在多障礙物的復雜場景應用較多[5]。

北京工業(yè)大學的阮曉鋼等[6]，提出了一種基于信息增益與RRT思想相結合的機器人環(huán)境探索策略，增強機器人對未知環(huán)境的探索降，低傳統(tǒng)RRT算法固有的盲目性。昆明理工大學的胡兵等[1]，在可變步長的隨機樹生長過程中，引入雙向生長策略，利用雙向生長特性，提高路徑搜索效率，解決了最優(yōu)路徑與低效率間的矛盾。林依凡等[7]提出了一種無碰撞檢測RRT*運動規(guī)劃方法，增強了在復雜環(huán)境中的路徑規(guī)劃能力。四川大學的仲健寧等[8]提出一種基于多種啟發(fā)式策略和強化節(jié)點機制改進的高效RRT*路徑規(guī)劃算法，該算法提出強化節(jié)點機制，擴大節(jié)點蘊涵的信息，提高節(jié)點利用率。廣東工業(yè)大學的何兆楚等[9]利用人工勢場法進行局部路徑規(guī)劃，當陷入局部極小值時，使用改進的快速擴展隨機算法自適應地選擇臨時目標點，使搜索過程跳出局部極小值點；當機械臂逃離局部極小值點時，切換回人工勢場法進行規(guī)劃。

基于以上的研究分析，根據(jù)RRT算法隨機性、無序性的特點，對RRT算法進行選點方式、算法優(yōu)化、碰撞檢測等方面的改進，提出一種新的路徑規(guī)劃算法。本研究以傳統(tǒng)RRT算法與勢場法相結合，對RRT算法加入勢場法、新節(jié)點生成策略、狹窄空間算法等，對RRT算法進行進一步優(yōu)化。由于勢場的影響，檢測到的障礙物會產(chǎn)生斥力場，因此節(jié)點生成時函數(shù)值過大，會直接將其剔除，因此改進后的RRT算法也不再需要重復進行碰撞檢測，縮短了整體的運算時間。

1 問題分析

1.1 RRT算法原理

經(jīng)典RRT算法是從狀態(tài)空間一初始點出發(fā)，通過隨機采樣擴展增加新節(jié)點，生成一個隨機擴展樹，當隨機樹中的新節(jié)點包含目標點或進入目標區(qū)域時，初始點到目標點間至少形成一條以隨機樹新節(jié)點組成的路徑[10]。

以二維空間中的RRT算法為例，假設在二維狀態(tài)空間C中，引入初始點xinit和目標點xgoal，使得xinit根據(jù)快速拓展隨機樹T進行隨機樹的逐步擴建，最終到達xgoal，其過程如圖1所示。

圖1 節(jié)點生成策略

狀態(tài)空間C中，障礙物所在的區(qū)域為障礙區(qū)Xob(XobX)，其他可通行的區(qū)域為通行區(qū)Xpa(XpaX)。在工作過程中，RRT算法會從初始點xinit開始逐步通過隨機采樣擴展結點，首先它會選定任意一個父節(jié)點，之后根據(jù)一個隨機的方向和規(guī)定的步長生成一個最近的臨時節(jié)點xrand，根據(jù)xrand所在位置判定其在Xob(XobX)還是Xpa(XpaX)。若其在障礙區(qū)，則該xrand無效：若其在通行區(qū)，則可生成一新的父節(jié)點xnew。這一過程會一直重復，直到xnew進入目標點xgoal的鄰域范圍Xgoal(XgoalX)，表明已成功找到目標點xgoal，進行逆追隨找到連接xinit與xgoal的最短路徑，完成路徑規(guī)劃。其偽代碼如表1所示。

表1 RRT algorithm

隨機樹T生長過程中，函數(shù)RANDOM_STATE（）會從空間中選取一點；根據(jù)函數(shù)NEAREST_COMPARE（），會在空間中拓展出一點；經(jīng)過函數(shù)STEER（），SELECT_INPUT（），NEW_STATE（）判定后得到隨機樹上的新節(jié)點；之后collisionchecking（）函數(shù)會對其進行碰撞檢測，若滿足條件則生成新節(jié)點；最后進行鄰域檢測findnear_neighbor（），若滿足則路徑規(guī)劃成功；經(jīng)過路徑比較函數(shù)MINPATH（）輸出最優(yōu)路線。

1.2 實際問題分析

經(jīng)過實驗測試，傳統(tǒng)的RRT算法依然存在著很多問題。

（1）規(guī)劃路徑成功率較低，在測試實驗中，設定最大節(jié)點生成個數(shù)為1 000個，在圖2實驗中，成功率為70%；在圖3實驗中，成功率僅僅為50%。若想增高成功率，可以設定更多的節(jié)點數(shù)和減小節(jié)點步長，但無疑會加大工作量，延長整體的工作時間。

圖2 路徑規(guī)劃1

圖3 路徑規(guī)劃2

（2）所規(guī)劃路徑質(zhì)量較低，在兩種地圖中，RRT算法雖然都成功完成了路徑規(guī)劃任務，但所得到的路徑是非常不理想的，與最優(yōu)路徑之間還存在著一定的距離，同時所規(guī)劃路徑存在過大的轉角，在實際工作過程中，會產(chǎn)生一定程度的震蕩，降低整個工作過程的穩(wěn)定性。

（3）在狹窄空間中，RRT算法容易陷入無解情況，如圖3所示，由于算法生成策略的隨機性，在狹窄的通路中，當父節(jié)點離障礙物較近時，新生成的節(jié)點很大概率落入障礙區(qū)中，導致選點失敗陷入死循環(huán)。

針對于以上的問題，本文提出了一種RRT結合人工勢場軌跡規(guī)劃的算法。改進RANDOM_STATE（）選點函數(shù)，加入引力場和斥力場綜合作用影響和狹窄概率算法，使得生成受到勢場和概率算法的綜合影響。由于勢場的加入，障礙物生成的斥力場會影響到隨機點生成，使得隨機點不會生成在障礙區(qū)中，因此改進后的算法不再需要進行碰撞檢測函數(shù)collisionchecking（），提高了工作效率。引力場與斥力場的相互作用，使得整個空間中的勢場連續(xù)性變化，加之勢場法中對于機器人轉角因素的考慮，減小了路徑規(guī)劃中的過大轉角，增強了工作過程的平穩(wěn)性。在狹窄空間中，狹窄概率算法的加入使得路徑搜索的成功率增大，在面臨復雜地形時整個算法的表現(xiàn)更加出色。

2 算法的改進

2.1 勢場法的引入

針對傳統(tǒng)RRT算法隨機性過大、規(guī)劃路徑質(zhì)量較低的問題，本文提出一種基于傳統(tǒng)RANDOM_STATE（）選點函數(shù)的增益求解函數(shù)。

在傳統(tǒng)的勢場法中，針對于機器人中的路徑規(guī)劃問題，經(jīng)常使用如下的引力函數(shù)和斥力函數(shù)：

式中：Ua(Pn)為機器人在位置Pn時所受到的引力；Ka為引力系數(shù)；Pg為目標點所在位置。

式中：Ur為斥力函數(shù)；Kr為斥力系數(shù)；ln為機器人各個連桿與障礙物之間的距離；l0為整個斥力場的作用范圍。

根據(jù)機器人各個連桿與障礙物之間的距離，就可以很輕松的知道機器人與障礙物是否發(fā)生了碰撞，因此在優(yōu)化選點函數(shù)時，可以將兩者的距離差作為條件編入函數(shù)，以此省略了之后的collisionchecking（）碰撞檢測函數(shù)。

對于任意時刻，機器人受到的總勢能為引力勢能與斥力勢能的總和，即：

2.2 選點函數(shù)的改進

在RRT算法中，傳統(tǒng)的選點函數(shù)如圖4所示，以xnew父節(jié)點為中心，初始設定的步長為半徑，得到子節(jié)點的出生圓。之后根據(jù)隨機方向rand，得到一個新的子節(jié)點xrand，再進行碰撞檢測，若滿足工作要求則子節(jié)點生成成功。

圖4 傳統(tǒng)原理

從其工作原理中可看出，方向的確定是完全隨機的，而且每次生成新的子節(jié)點之后都要進行碰撞檢測，雖然拓展性較好，但針對于機器人的復雜工作環(huán)境來看，其工作效率很低，選點方式也并不科學。

結合勢場法的思想，如圖5所示，根據(jù)實際工作環(huán)境設定工作區(qū)域，將其分為若干個區(qū)域，計算每個區(qū)域中的綜合增益，根據(jù)增益得到每個區(qū)域方向的生成概率，完成最終的子節(jié)點生成。綜合增益可表示為：

圖5 改進原理

式中：k0為初始增益系數(shù)；X0為初始增益，其計算已在經(jīng)典算法中得出；ku為勢場增益系數(shù)；Xui為勢場增益。k0、ku可以根據(jù)實際的工作過程進行取值。Xui可表示為：

式中：k?為條件系數(shù)，若步長圓內(nèi)存在障礙物，則取值為0，否則為1；Di為各個區(qū)域范圍；U(θ,r)為總勢能函數(shù)，其中θ、r可根據(jù)實際工作過程進行取值。

根據(jù)每部分的增益結果，可得到各區(qū)域隨機方向生成的概率為：

改進后的選點函數(shù)由于勢場的引導作用，省去了很多無效點的生成和碰撞檢測，使得路徑規(guī)劃效率更高，節(jié)約了時間。新生成父、子節(jié)點之間的轉角不會過大，使得整個路徑更光滑，也更接近理想路徑。

2.3 加入狹窄概率函數(shù)

針對于RRT算法在狹窄環(huán)境中易陷入無解的情況，有針對性的加入一種狹窄概率函數(shù)，其偽代碼如表2所示。

表2 狹義算法

隨機選點函數(shù)COLLISIONCHECKING_NEW在工作過程中，狹窄概率函數(shù)會同時使用監(jiān)測函數(shù)MONITOR監(jiān)測新生成的xnew節(jié)點，當xnew與D中已經(jīng)生成的節(jié)點之間的距離和位置條件，滿足judge函數(shù)時，則會將工作區(qū)域個數(shù)進行更細劃分，以此找到可能存在的狹窄通路。

RRT算法方向選擇的隨機性和勢場法容易陷入局部死循環(huán)的特性，都決定了其對于狹窄路徑規(guī)劃存在著弊端。狹窄概率算法很好地解決了狹窄通路難以尋找的問題，使得算法的整體魯棒性增強，優(yōu)化了路徑規(guī)劃過程。

3 實驗驗證

3.1 實驗一

將RRT算法與RRT結合人工勢場軌跡規(guī)劃的算法進行仿真實驗，驗證算法的實際工作情況。如圖6~7所示，本次實驗采用Matlab R2016b數(shù)學仿真軟件進行，設置多組地圖進行測試。實驗一的部分參數(shù)如表3所示。

表3 實驗一參數(shù)

圖6 傳統(tǒng)算法

圖7 改進算法

實驗過程中，黑色圖形為隨機設置的障礙物，紅色圓點為初始點xinit，綠色圓點為目標點xgoal，隨機樹生成的節(jié)點為藍色圓點，每步之間的距離用藍色線段相連，其中找到的最優(yōu)路徑用紅色路線標出。

實驗數(shù)據(jù)對比如表4所示。實驗中的部分數(shù)據(jù)如表中所示，通過實驗數(shù)據(jù)可明顯看出，改進后的算法成功率有所提高，在200次實驗中僅有6次沒有成功找到目標點，成功率提高了13%。同時，所產(chǎn)生的節(jié)點個數(shù)大大減少，改進后算法生成的節(jié)點數(shù)減少了大約86.22%，因此整體工作效率也得到了很大提高，平均工作時間減少了大約67.6%，同時改進后的算法所找到的路徑質(zhì)量較高，接近理想路徑的數(shù)量更多。

表4 算法比較

綜上所述，改進后的算法在普通環(huán)境中能更好地實現(xiàn)路徑規(guī)劃，無論是從成功率、節(jié)點數(shù)還是工作時間上都得到了一定改善，接下來仍要繼續(xù)測試在一些特殊條件中算法的表現(xiàn)如何。

3.2 實驗二

在本組實驗中，針對于算法的狹窄空間路徑規(guī)劃能力進行測試，如圖8~9所示。采用Matlab R2016b數(shù)學仿真軟件進行，建立了整體地形較為狹窄的空間，設置多個復雜、易誘導路徑，模擬實際的工作過程。部分實驗參數(shù)如表5所示。

表5 實驗二參數(shù)

圖8 傳統(tǒng)算法

在對算法進行了200次模擬后，結果如表6所示。普通RRT算法成功率僅為47%，且所用平均節(jié)點數(shù)約為909個，實驗時間較長。而改進后的算法成功率為63.5%，增加了16.5%，平均節(jié)點數(shù)減少了大約13.3%，平均時間減少了大約22.1%。

圖9 改進算法

表6 實驗二結果

因為設置地圖環(huán)境較為復雜，且設定步數(shù)也僅為1 000步，所以兩種算法成功率均較低，但是也可明顯看出改進后的算法對于路徑的規(guī)劃表現(xiàn)更佳，從成功率和規(guī)劃效率上有了一定的優(yōu)化，尤其在地圖的狹窄部分，由于選點函數(shù)和狹窄概率函數(shù)的影響，使得選點判斷更合理，所選擇路徑更優(yōu)。

4 結束語

設計一種新的選點函數(shù)，通過區(qū)域化增益的方式，隨機生成新的子節(jié)點，通過節(jié)點選擇判定后產(chǎn)生，可以有效減少無效點，提高選點效率。

針對特殊狹窄空間，彈性地改變選點判定范圍，更有利于尋找正確路徑，提高整體路徑規(guī)劃效率。

本文研究提出的RRT結合人工勢場軌跡規(guī)劃的算法，經(jīng)過模擬實驗測試，相對于傳統(tǒng)的RRT算法，改進后的算法擁有更好的路徑規(guī)劃能力，能有效改善路徑規(guī)劃中路徑的選擇問題。