融合動態窗與改進RRT的全位置機器人路徑規劃算法研究

張海波，嚴小珊，畢齊林，唐惠玲

(1.廣州航海學院土木與工程管理學院，廣東廣州510725；2.廣東工業大學物理與光電工程學院，廣東廣州 510006)

0 前言

傳統的輪式、腿式、履帶式等移動機器人難以在狹小空間移動，全位置式移動機器人(簡稱MW-WH-OIR)有助于改善這種情況，因此具有較廣泛的應用前景及重要的研究意義。

在“十三五”、“工業4.0計劃”等政策的推動下，我國巡檢業加快了向規模化、自動化、智能化方向轉型的步伐，工業移動機器人也迎來了快速發展的戰略機遇期[1-2]。目前，國內的巡檢業(如變電站巡檢，軌道交通巡檢，城市應急安防巡檢業)還存在大量人工巡檢、多固定點監控的方法。此類工作量大、枯燥，有時候還存在一定危險，很多時候靠人眼并不能及時發現缺陷，效率低、周期長，存在極大的盲區，易造成潛在的安全隱患。此外，多固定點監視范圍有限、成本高、實用性差，不能滿足現代需求。

MW-WH-OIR能夠在任意方向移動，靈活性好，搭載攝像頭便可以大大擴展檢測范圍，可以廣泛應用于智能巡檢領域。國內智能巡檢市場對工業移動機器人的需求正在不斷增加，然而在移動機器人的相關技術研究中，導航技術是最核心的技術，路徑規劃則是導航技術的一項重要內容。

工業移動機器人成為當代工業重要利器的同時，常常要考慮其運動規劃問題，高效的路徑規劃有利于提高工作效率。如何在復雜的動態障礙物環境下，規劃一條實時的從起始點到目標點無碰撞的可行路徑是MW-WH-OIR技術研發的關鍵。

MW-WH-OIR的路徑規劃是指在具有障礙物的復雜環境下，按照規劃路徑的某個測度(如時間最短、路徑最優、能量消耗最低等)，在任務區域內尋找一條從起始點到目標點與障礙物無碰撞的可行路徑[3]。20世紀70年代至今，國內外學者對此進行了大量的研究，同時還提出了許多適用于不同應用場景下的路徑規劃算法并不斷進行改進、優化。2007年趙曉等人[4]利用A*算法實現了移動機器人的無碰撞路徑規劃。王淼弛[5]針對A*算法存在的次優路徑問題，改進了A*算法的啟發搜索策略，優化了路徑長度，同時提高了路徑平滑度；王奇志[6]針對傳統人工勢場法出現的零勢能域現象，采用限定范圍的方法改進了人工勢場法，實現了多障礙物情況下機器人快速、實時、避障的路徑規劃。2010年SHI等[7]為解決傳統人工勢場法用于移動機器人運動規劃時，出現目標點距離障礙物太近而無法到達目標點以及存在局部極小點的陷阱問題，提出了縮小障礙物影響范圍、障礙物影響范圍分層的改進方法，使得算法適用于移動機器人在各種復雜環境下的路徑規劃。李寶霞[8]針對模糊系統的隸屬度函數確定過程繁瑣問題，將遺傳算法結合到模糊系統中，對模糊算法進行改進，生成了模糊遺傳算法。

以上常用的路徑規劃算法在簡單的環境下能快速地找到最優路徑，但其沒有考慮非完整性約束問題，也不適合解決高維空間多自由度機器人復雜約束下的運動規劃問題，可能導致實際路徑不可行，不能滿足移動機器人運動規劃的實時性。

為此，基于隨機采樣的運動規劃算法，YANG等提出了一種基于采樣的快速隨機擴展樹方法(Rapidly-exploring Random Trees，RRT)算法[9]。由于RRT算法的快速搜索隨機性，增量式搜索方式能夠把非完整性約束或非完整性動力學約束考慮到運動規劃中，因此非常適合解決高維空間多自由度機器人在復雜環境和變化場景中的運動規劃問題。樊曉平和李雙艷[10]針對復雜的動態環境，將RRT算法與滾動規劃結合，同時在RRT算法中引入選擇性參數BIAS，實現了向目標點快速收斂的實時在線路徑規劃。喬慧芬等[11]為實現動態不確定環境下實時路徑規劃，結合人工勢場法下規劃的初始路徑和RRT算法指導下的局部路徑，有效實現了移動機器人的無碰撞實時路徑規劃。

本文作者基于MW-WH-OIR的工作環境，提出一種改進的快速擴展隨機樹算法，使MW-WH-OIR能夠在一個復雜的動態環境下進行在線實時路徑規劃。

實際情況下，MW-WH-OIR更多時候需要對動態環境變化實時做出動作，并不斷尋找最優可行路徑。ZHUANG等[12]基于極坐標空間，實現了以機器人期望運動方向角為指標的動態不確定環境下移動機器人的在線實時路徑規劃。曾碧和楊宜民[13]利用模糊描述對環境進行建模,將改進的蟻群算法與機器人滾動在線路徑規化方法相結合，實現了動態環境下在線實時規劃問題。以上算法都未考慮移動機構的非完整約束條件。快速搜索隨機樹可在環境中的從起始點開始快速隨機擴展搜索樹，直到尋找到目標點，其結構簡單、搜索能力強且考慮了執行機構的非完整約束條件，還易于與其他算法結合生成一個新的可行算法。在國內，RRT算法應用于已知的環境下的研究已經很成熟，但應用于動態環境下的實時路徑規劃的研究還較少。

在實際工作中，MW-WH-OIR多處于一個復雜的動態環境，傳統的RRT算法及大多數改進后的RRT算法生成的路徑往往不能被采用。因此，如何使MW-WH-OIR在一個復雜的動態環境下進行高效的在線實時避障及路徑規劃是文中需要解決的問題。

1 傳統RRT算法

1.1 傳統RRT算法基本流程

傳統RRT算法相較其他路徑規劃算法更加具備快速搜索的能力，能夠很好地適應動態變化的環境，也因其快速擴展的隨機性，使得該算法具有很強的避障功能。圖1所示為傳統RRT算法的流程。

圖1 傳統RRT算法基本流程

1.2 傳統RRT算法的基本原理

傳統RRT算法不斷地將樹枝從起始節點Xinit(根節點)隨機擴展到目標節點Xgoal，其過程中合理避開障礙物，遍歷生成隨機樹T，最后選取一條最優的可行路徑[16]。傳統的RRT算法：

Algorithm1：傳統RRT

1：TU←Xinit；

2：while(n←1 to Nmax)do

3：Xrand← Sample_node(Xfree)；

4：Xnear←Nearclose(Xrand，TU)；

6：Xnew←Steer(Xnear，Xrand)；

7：if Check_path(Xnearest，Xnew)= = true then；

8：Add_node(Xnew)；

9：end if

10：end while

11：return TU；

傳統RRT算法的基本步驟如下：

步驟1：初始化隨機樹T后將給定起始點Xinit添加到隨機樹T作為根節點；

步驟2：在Wfree空間中隨機獲取一個生長樹節點Xrand；

步驟3：用Nearclose()函數計算樹T中與Xrand距離最近的擴展節點Xnear；

步驟4：用Steer()函數在Xrand與Xnear方向上生長新節點Xnew；

步驟5：判斷Xnew生長區域是否存在障礙物，若是，則跳轉進行步驟3；若否，則在隨機樹T上添加一個新節點Xnewi(i∈N，N為自然數)

步驟6：判斷是否滿足Xrandi=Xgoal或者Xnewi∈Xgoal的條件，若不滿足條件，則跳轉進行步驟4；若滿足條件，則從目標節點Xgoal開始依次找到父親節點直到起始點Xinit為止，之后返回一條可行的規劃路徑，結束程序。

2 基于改進RRT算法的MW-WH-OIR實時路徑規劃方法理論分析

2.1 定義滾動窗口

MW-WH-OIR在進行管道、軌道、安防巡檢作業時，常處于一個復雜動態障礙物環境下，為此首要考慮路徑規劃的實時性。全局路徑規劃方法運算時間太長，不適合動態復雜環境下的路徑規劃，為此文中采用局部路徑規劃方法。為滿足路徑規劃的實時性，將傳感器檢測范圍視為一個可進行自適應調節大小的窗口(文中視檢測范圍是一個半圓)，如圖2所示，當H2大于檢測范圍時，對整個路徑進行多階段的規劃。

圖2 滾動窗口示意

2.2 無障礙物時的MW-WH-OIR實時路徑規劃

通過GPS導航系統確定起始點Xinit與目標點Xgoal位置，若起始節點與目標節點之間，不存在障礙物時，起始節點Xinit與目標節點Xgoal之間的連線距離就是MW-WH-OIR所要行走的最短可行路徑，如圖3所示。

圖3 無障礙物處理模型

2.3 有障礙物時的MW-WH-OIR實時路徑規劃

動態障礙物的位置會隨著時間發生變化，故建模是以時間為軸建立方程。障礙物任意時刻的位置狀態用(x，y，t)表示。

2.3.1 面向MW-WH-OIR的勻速障礙物預測模型

如圖4所示，假設障礙物以速度v(t)=b勻速移動，在t時刻于點A位置處(xi，yi，t)被滑動窗口檢測到，而下一時刻t+Δt時，處于點B(Δt取值極小)，當Δt取極小值時，AB之間的連線可視為一條直線，直線AB與X軸之間的夾角為θ，則對于障礙物從t→t+Δt過程中，它的位置坐標變化情況如下：

圖4 勻速障礙物預測模型

(1)

通過公式(1)可以快速計算出任意時刻障礙物的移動位置。

2.3.2 面向MW-WH-OIR的變速障礙物預測模型

MW-WH-OIR在二維平面工作，由于傳感器可以實時獲取局部動態障礙物的位置信息，文中通過傳感器在前期獲取的動態障礙物位置信息，結合自回歸模型預測下一時刻動態障礙物的運動位置，進行局部規劃，避開障礙物。

如圖5所示，假設傳感器的滑動窗口檢測到的動態障礙物為obsq(q∈N)，并記錄了該障礙物上一時刻和當前時刻的位置信息，記為sq(t-1)、sq(t)(t∈N)，由此建立預測下一時刻位置sq(t+1)的自回歸模型：

圖5 存在變速的障礙物預測模型

sq(t)=λ1sq(t-1)+e(t)

(2)

相應的推導n階自回歸模型：

(3)

其中：e(t)為位置預測誤差；λ為位置的回歸系數。

同理對動態障礙物的加速度aq進行n階自回歸建模：

(4)

其中：p(t)為加速度預測誤差；β為加速度的回歸系數。

速度與位置的關系式：

vq(t)=sq(t)-sq(t-1)

(5)

加速度與速度關系式：

aq(t)=vq(t)-vq(t-1)

(6)

結合上式，易得加速度與位置的關系式：

aq(t)=sq(t)-2sq(t-1)+sq(t-2)

(7)

加速度回歸系數β通過最小二乘法擬合，由公式(4)可引入加速度的殘差平方和函數：

(8)

(9)

通過對E(β)進行微分求最值，可以得到：

(10)

當加速度的回歸系數βq與時間相關時，在殘差平方和函數取值最小的函數中引入一個指數權重因子μ(0<μ<1),μ越趨近于1，表明加速度變化得越快。

(11)

求上式的解：

(12)

當能擬合估算回歸系數βq，便可以計算下一時刻動態障礙物的預測位置，即：

(13)

3 面向MW-WH-OIR的改進RRT算法

3.1 MW-WH-OIR運動學模型

Mecanum Wheel能實現全方位移動，1973年由瑞士人LION發明[14]。它有3個自由度，即繞輥子軸轉動、繞輪子軸轉動、繞輥子與地面的接觸點轉動。MW-WH-OIR的移動機構主要采用4個Mecanum Wheel，可以實現任意方向的自由移動，其轉彎半徑近似為零，非常適合代替作業人員在工作空間狹小或對機器人的機動性要求較高的場合，如大型管道內側巡檢、變電站巡檢、城市應急安防巡檢等，因而在工業上應用越來越廣泛[15]。本文作者針對MW-WH-OIR的路徑規劃分析時，可以不考慮其轉彎半徑限制的問題。以下對MW-WH-OIR運動學模型進行分析。

如圖6所示，以O為原點建立坐標系，Y軸為MW-WH-OIR的正前方，X軸為MW-WH-OIR正右方。vn(n=1，2，3，4)為4個Mecanum Wheel上對應與地面接觸的轉動輥子繞軸的自身線速度；r為Mecanum Wheel的半徑，則：vn=r×ωn(n=1，2，3，4)；ωn(n=1，2，3，4)為對應4個Mecanum Wheel的自身角速度；vx、vy分別為MW-WH-OIR在X、Y方向的移動速度；ω為繞中心點O垂直軸轉動的角速度；α為Mecanum Wheel輪轂軸線和輥子軸線的夾角；2L2、2L1為機器人車身的長和寬。

圖6 MW-WH-OIR運動學分析示意

若vx、vy已知，求4個輪子的線速度vn，則根據MW-WH-OIR運動學分析得計算公式如下：

v1=vy-vxtanα-(L2tanα+L1)ω

(14)

v2=vy+vxtanα+(L2tanα+L1)ω

(15)

v3=vy+vxtanα-(L2tanα+L1)ω

(16)

v4=vy-vxtanα+(L2tanα+L1)ω

(17)

反之，若4個輪子的線速度vn已知，即可求解MW-WH-OIR在X、Y方向上的移動速度(vx，vy)及相應的角速度ω，公式如下：

(18)

vy=0.25(v1+v2+v3+v4)

(19)

(20)

結合公式(14)—(17)，可分析MW-WH-OIR在各個方向上的移動情況，4種典型MW-WH-OIR的移動情況和4個Mecanum Wheel轉向關系如圖7所示。

圖7 4種典型MW-WH-OIR移動情況

3.2 改進RRT算法

改進的RRT算法針對上述討論的傳統RRT算法存在的不足之處進行改進和創新，同時將其應用于更加復雜的動態環境下，擴展了算法的應用范圍，其中考慮了MW-WH-OIR車身約束，從而更加符合MW-WH-OIR的實際工作情況。以下為改進RRT算法的偽代碼：

Algorithm2：改進RRT

1：構建：仿真環境地圖；

2：定義環境信息：機器人運動狀態點Xinit、目標點Xgoal、子目標點Zgoal、步長Stepsize、安全閾值范圍Q；

3：檢測：判斷起始點、目標點是否在仿真地圖的障礙物上，若是，則退出，否則繼續；

4：TU←Xinit；

5：while N>Nmaxdo；

6：Update(Zgoal)

7：obsn←Sample_obs;(n=1,2)；

8： Filter(Xnearest，Xnew)

9：Index_node = Index_tree(Xrand)；

10：while Index_node> 1 do

11：Update_node(Xrand)；

12：Select(Xnew，Xnear)

13：end while；

14：end while；

15：return T；

Function：Update(Zgoal，Xgoal)

1：while Xgoal==Zgoaldo

2：if Checkpath1(Zgoal)

3：Zgoal← Get(Zgoal)；

4：else if (a1Q))

5：Zgoal← Get(A1)；

6：else

7：Zgoal←Get(A2)

8：end if

9：if Xgoal?S

10：Xgoal=Zgoal

11：end if

12：end while

Function：Filter(Xnearest，Xnew)

1：Xrand← Sample_node；

2：Xnearest← Nearclose(Xrand，TU)；

3：Index_node = Index_tree(Xrand)；

4：for Xnearest∈Xneardo

5：if Xnear≠ ?；

6： Total C = Cost(Xnearest)+Cost(Xnew，)；

7：end if；

8：if Total C1==min(total C)；

9：Xrand←Xnearest；

10：end if；

11：end for；

Function：Select(Xnew，Xnear)

1：if Checkpath2(Xnear，Xnew)&distance(Xnew>Q)；

2：Xnew← New_node(Xnewest，Xrand)；

3：else;

4：delete(Xnew)；

5：end if;

為彌補傳統RRT算法的不足，本文作者提出的Algorithm2在Algorithm1的基礎上添加了3個函數，下面對Algorithm2中的3個重要函數進行簡單的介紹。

3.2.1 建立最小安全距離Q模型

由于傳統RRT算法中并沒有考慮移動機器人車身尺寸D的約束，故在規劃路徑過程中存在生長節點靠近障礙物的現象。當生長節點已經與障礙物粘在一起，MW-WH-OIR沿著該路徑行走時必定會跟障礙物發生碰撞，損壞車身。為避免MW-WH-OIR與障礙物發生碰撞，設置最小安全距離Q(Algorithm2：第22～26行，其中Q>D)，如圖8所示，障礙物邊緣擴展的黃色區域即是安全閾值范圍Q，令生長節點禁止擴展到安全閾值范圍內，這樣便使得MW-WH-OIR在移動過程中與障礙物始終保持一定的安全距離。以下為安全閾值Q算法：

圖8 設置安全閾值

Algorithm3：Select(Xnew，Xnear)

1：if Checkpath(Xnear，Xnew)&distance(Xnew>Q)；

2：Xnew← New_node(Xnewest，Xrand)；

3：else;

4：delete(Xnew)；

5：end if;

在檢測生長節點Xnew是否與障礙物碰撞的同時判斷與障礙物的距離是否大于Q值，若是，則添加新的生長節點，否則刪除Xnew，重新尋找符合條件的Xnew。

3.2.2 子目標點的選取及更新算法

圖9 子目標節點的選取及更新

Algorithm4：Update(Zgoal，Xgoal)

1：while Xgoal==Zgoaldo

2：if Checkpath1(Zgoal)

3：Zgoal← Get(Zgoal)；

4：elseif (a1Q))

5：Zgoal← Get(A1)；

6：else

7：Zgoal←Get(A2)

8：end if

9：if Xgoal?S

10：Xgoal=Zgoal

11：end if

11：end while

3.2.3 過濾無用節點

在基本RRT樹枝生長過程中，會生成許多不必要的樹枝，需要大量的迭代次數才能找到目標點，而且生成的路徑曲折冗長、不平滑，耗費大量的搜索時間，搜索效率極低[17]。

為解決以上問題，本文作者在傳統RRT算法的基礎上添加了一個過濾不必要節點的過程(Algorithm5：第5～9行)，僅保留一個最優生長結點，如圖10所示。品紅色的生長節點明顯偏離Zgoal，不是最優生長節點故將它過濾。這樣就降低了迭代次數，大大提高搜索效率，減少冗余節點，路徑的平滑度也有所提升。

圖10 過濾無用節點

Algorithm5：Filter(Xnearest，Xnew)：

1：Xrand← Sample_node；

2：Xnearest← Nearclose(Xrand，TU)；

3：Index_node = Index_tree(Xrand)；

4：for Xnearest∈Xneardo

5：if Xnear≠ ?；

6： Total C = Cost(Xnearest)+Cost(Xnew，)；

7：end if；

8：if Total C1==min(total C)；

9：Xrand←Xnearest；

10：end if；

11：end for；

當Xnear不為空集時，尋找Xnear集合中與Xnew形成最小長度代價的Xnearest，最后賦值給Xrand進行路徑規劃。

4 仿真實驗

通過建立上述模型和RRT實時路徑規劃算法，在MATLAB2018a下進行了仿真實驗。實驗用計算機配置如下：處理器為Intel-Core-i5-7200，Xeon(R)，主頻為3.10 GHz，內存為16 GB，操作系統為64位Windows10。實驗設置地圖大小為500×500(無量綱)，如圖11所示。此次仿真實驗中設置傳統RRT算法在靜態障礙物環境下的生長步長為25，隨機生長節點概率閾值為20；起始點Xinit=(5,499)，子目標點Zgoal=(150,300)，目標點Xgoal=(499,5)。

圖11 傳統RRT算法路徑規劃仿真實驗

利用改進的RRT算法將MW-WH-OIR的工作環境建模定位于500×500的地圖區域，如圖12中左上角為機器人第一次局部檢測范圍內路徑規劃的全過程，圓形的障礙物為可視范圍內的隨機動態障礙物，其余為靜態障礙物，其中動態障礙物的起始點是任意設定的。

圖12 基于改進RRT算法的實時路徑規劃仿真實驗

4.1 傳統RRT算法MW-WH-OIR路徑規劃仿真

傳統RRT算法在500×500的復雜環境下(環境中的障礙物都為靜態)的規劃路徑如圖11所示，節點表示MW-WH-OIR經過的路徑。由圖11可以明顯看出：隨機樹的生長區域幾乎覆蓋了整個工作環境，整個過程需要經過大量的迭代才能找到目標點Xgoal，極大地降低了搜索效率。圖11(d)中的最終規劃路徑存在大量的冗余點，路徑較為曲折，甚至在一些空曠區域內也存在許多彎曲的路徑，偏離最優解。此外，還存在生長節點貼近障礙物的現象，如圖11(d)紅色標注)，在實際駕駛過程很容易與障礙物發生碰撞。

4.2 改進RRT算法MW-WH-OIR實時路徑規劃仿真

對比圖11、圖12的仿真結果可知:相較于傳統RRT算法，改進的RRT算法規劃路徑并沒有出現大量的冗余點、曲折點，樹枝生長過程中能夠順利地避開障礙物，始終與障礙物保持一定的安全距離，最終朝向子目標點生長。此外，由圖12可知，本文作者提出的算法解決了路徑曲折、樹枝生長無方向性、搜索效率低的問題，進而大幅度提高了規劃路徑平滑度，同時避免了MW-WH-OIR與障礙物發生碰撞的現象，滿足移動機器人的實際移動需求。

4.3 實驗結果對比分析

為了更好地驗證算法性能，分別對3種算法在文中設定的障礙物環境下進行500次仿真實驗，得到每種算法規劃所需的最小、最大以及平均時間的實驗數據，如表1所示。

表1 復雜障礙物環境下算法路徑規劃性能數據

從表1可看出：不管是在簡單靜態障礙物威脅還是復雜動態障礙物威脅環境下，文中算法的規劃時間均是最少的，與傳統RRT算法相比較，它將搜索效率提高了43%。

5 結論

針對傳統RRT算法應用于MW-WH-OIR生成的路徑存在路徑曲折、搜索效率低、樹枝生長缺乏目標引導性甚至路徑不可行的問題，首先，添加了刪除無用節點策略，減少冗余點，提高探索效率；其次，提出了子目標點的選取原則，在能夠保證順利避障的前提下，使得生長樹優先朝向目標點生長；最后，設置合適的安全閾值范圍，使移動機器人與障礙物能夠保持一定的安全距離。最終提高了樹枝生長效率，生成的路徑較為平滑，能夠更好地應用于MW-WH-OIR的實際工作中。