基于雙向RRT算法的管線路徑規劃及建模仿真

王素琴，王 飛，袁建平，陳曉龍，陳顯龍

(1.華北電力大學 控制與計算機工程學院，北京 102206；2.北京恒華偉業科技股份有限公司，北京 100011)

管線的種類和數量日益增多，大到保障城市正常運行的各類工程管線，小到設備內部連接各種元器件的線纜。管線敷設環境越來越復雜，如何更加快速、合理地進行管線的路徑規劃至關重要。

管線路徑規劃問題是指在給定的空間內，從指定起點到終點，設計一條不與其他物體發生干涉，同時又滿足各種約束條件的路徑。路徑規劃問題的求解方法很多，如遺傳算法[1-2]、蟻群算法[3]、A*算法[4-6]、RRT算法[7]。PARK et al[8]針對航空航天電纜設計和生產提出了基于Agent的二維電纜路線設計。ITO[9]采用遺傳算法來進行管線布局設計中路徑的交互式規劃。權建洲等[10]提出基于改進A*算法的電子制造裝備布線路徑搜索方法，引入評估函數優化搜索。吳保勝等[11]提出了基于重力規則的蟻群算法路徑搜索策略。上述算法一般需要對搜索空間進行柵格化建模；當搜索空間較大時，會占用大量存儲空間，影響計算效率。另外這些算法往往容易陷入局部最優解。

快速擴展隨機樹算法(RRT)是一種樹形數據存儲結構和算法，通過不斷遞增的方法建立，用于快速遍歷空間的未探索區域。RRT算法在進行路徑規劃時不需要對環境進行具體建模，靈活性高，對場景有很好的適應性。劉瀟等[12]提出了基于障礙物與目標吸引的改進快速擴展隨機樹算法用于線纜自動布線。徐聯杰等[13]提出了基于障礙物碰撞信息的快速搜索隨機樹改進算法。RRT算法雖然能夠在較短的時間內搜索出一條或多條路徑，但是無法進行修改，所求路徑為折線段，無法滿足部分曲線路徑彎曲半徑的要求，交互性較差，不滿足工程的實際需要。

考慮到彎曲變形、規劃環境對管線路徑規劃的影響，本文使用雙向RRT算法規劃出一條路徑后，引入Catmull-Rom樣條曲線對路徑關鍵點進行曲線擬合，添加了管線路徑彎曲半徑的約束，同時在虛擬環境中實現三維管線路徑規劃的模擬仿真及調整。此算法進一步提高了RRT算法在管線路徑規劃中的適應性和穩定性。

1 管線路徑規劃問題分析

管線按可彎曲程度分為可彎曲管線和不易彎曲管線。通過某些加工措施易將其彎曲的管線為可彎曲管線，如電信電纜、電力電纜等；通過加工措施不易將其彎曲的管線或強行彎曲損壞的管線為不易彎曲管線，如給水管道、電信管道等。管線路徑規劃問題也可以看作碰撞球的干涉問題[11]，即在三維環境中，以路徑離散點為球心，球直徑略大于管線直徑(管線需要與障礙物保留一定的間隙，因此碰撞球的直徑需要大于管線直徑，大小可按照工藝要求設定，一般設為管線半徑的1.2倍)，進行碰撞干涉檢測。如果該碰撞球沿著路徑進行搜索時不與環境中的障礙物發生干涉，那么這條路徑是可行的。

對部分可彎曲管線進行路徑規劃設計時，還需要考慮最小彎曲半徑的約束。由于這類管線絕緣及其構造特性要求，任何敷設方式及其全部路徑均應滿足最小彎曲半徑要求。以電力電纜為例，單芯電纜的最小彎曲半徑不小于電纜外徑的15倍，多芯電纜的最小彎曲半徑不小于電纜外徑的10倍。

2 管線路徑規劃方法

2.1 基本RRT算法

RRT算法是一種常見的用于機器人路徑規劃的方法，它本質上是一種隨機生成的數據結構——樹，該算法自LAVALLE[14]提出以來已經得到了極大的發展，到現在依然有改進的RRT不斷地被提出。

在RRT算法進行路徑規劃前，需要設置一些參數，包括路徑搜索的起點qinit，終點qgoal，搜索步長l，最大搜索次數N等。然后創建擴展隨機樹T，以qinit為隨機樹的根節點。然后根據圖1所示的節點擴展方法進行循環擴展，其中l表示搜索步長，具體步驟如下。

1) 采用隨機采樣的方法得到一個隨機采樣點qrand.

2) 找到距離采樣點qrand最近的樹節點qnear，進行節點擴展得到待定節點qnew.

3) 如果從qnear到qnew發生碰撞干涉，舍棄qnew進入下一次循環。

4) 如果qnear到qnew不發生碰撞干涉，將qnew加入擴展樹，作為qnear的子節點。判斷qnew是否足夠接近qgoal，若足夠接近，則直接與終點相連，循環終止，否則進入下一循環。

圖1 節點擴展Fig.1 Node expansion

2.2 節點擴展策略優化

基本RRT算法在節點擴展時，由于qrand點的生成是隨機的，導致了隨機樹擴展過于分散，且搜索時間過長，甚至在設定的有限次循環中無法到達目標點。針對這一問題，可以在節點擴展時，根據隨機概率決定qrand是目標點還是隨機點，增加qrand為qgoal的概率，當qrand為目標點時，節點擴展方向為目標方向，加快隨機樹到達目標點的速度。引入路徑緩存[15]的方法，在隨機樹搜索路徑時以一定概率選擇之前成功規劃路徑上的節點，使隨機樹的搜索更高效。

2.3 雙向擴展隨機樹

雙向快速擴展隨機樹(BI-RRT)是在擴展策略上對基本RRT算法進行改進[16]，從起點和終點各自創建一棵擴展隨機樹。在每次循環過程中，先擴展其中一棵隨機樹，然后嘗試將另一棵隨機樹擴展到當前新擴展的節點，使兩棵擴展隨機樹朝向對方擴展；兩棵隨機樹交替擴展，直至兩棵隨機樹節點相遇(距離小于預設值)。基本RRT和雙向RRT算法在二維路徑規劃上的效果對比如圖2所示。

圖2 RRT和BI-RRT比較Fig.2 Comparison of RRT and BI-RRT

雖然雙向RRT算法在增加新節點時要判斷是否可以將兩棵樹連接起來，增加了資源的消耗，但是提高了隨機樹擴展的效率和精度。

2.4 路徑平滑處理

經過BI-RRT算法規劃出來的原始路徑含有許多冗余節點，不能直接作為管線路徑，需要采用貪心算法來過濾這些冗余節點，從而獲得更優化的路徑規劃結果。如圖3所示，從起點qinit開始，依次尋找能夠無碰撞連接終點qgoal的頂點，記錄此點q'(1)，再從起點qinit開始，依次尋找能夠無碰撞連接q'的頂點，記錄此點q'(2)，直到起點qinit和q'(x)能夠無碰撞連接，將所有的q'(x)連接就得到了平滑后的路徑。

圖3 路徑平滑處理Fig.3 Path smoothing

2.5 基于關鍵點的曲線擬合

經過平滑處理的路徑是由離散點形成的折線段組成，通常不滿足可彎曲管線敷設要求，因此還需要進行折線段的光滑擬合。本文采用Catmull-Rom曲線對路徑離散點進行擬合。Catmull-Rom曲線是由4個控制點P0，P1，P2，P3定義的插值樣條曲線，繪制從P1到P2的曲線，如圖4所示。

Catmull-Rom曲線的構造公式見式(1).

圖4 Catmull-Rom曲線Fig.4 Catmull-Rom curve

(1)

式中，P(t)是指擬合出來的單段三次Catmull-Rom曲線，變量t的取值范圍是[0,1].當t從0到1線性變化時，曲線就從P1點(t=0)移動到P2點(t=1)，生成P1和P2之間的曲線段。將式(1)轉換為多項式形式，如式(2)所示。

(2)

式中：

在Catmull-Rom曲線中，首尾控制點P0和P3只是作為曲線擬合的輔助點，算法只會生成P1到P2之間的曲線段。如果需要曲線生成P0到P3的曲線段，則需要構造新的輔助點，例如可以使用[2P0-P1,P0,P1,P2]來繪制P0和P1之間的曲線，使用[P1,P2,P3,2P3-P2]來繪制P2和P3之間的曲線。

2.6 基于關鍵點的管線路徑調整

采用Catmull-Rom曲線對規劃路徑進行曲線擬合后，可能會與其他物體發生干涉或不滿足某些約束條件，這時可以通過自動或手動調整關鍵點的位置進行管線路徑的適應性調整。

對于Catmull-Rom曲線，改變某個關鍵點的位置對該點相鄰的曲線段有直接影響，對其他曲線段的影響很小。如圖5所示，當曲線上點P3移動到P'3的位置時，只有P2，P3間和P3，P4間的曲線段發生明顯變化。這一特點正是可彎曲管線路徑規劃設計所需要的，在不影響已完成的管線段的情況下，改變需要調整的管線段，保證路徑規劃的連續性。

2.7 最小彎曲半徑約束

對可彎曲管線進行路徑規劃設計時，必須考慮最小彎曲半徑的約束條件，對不滿足該約束的管線段進行優化調整。管線路徑的曲率半徑計算方法[17]如公式(3)所示。

圖5 調整路徑關鍵點Fig.5 Adjust path key points

式中：r為管線的彎曲半徑；P'和P"分別為P(t)關于自變量t的一階導數和二階導數。將曲線P(t)展開得到x，y，z三個方向關于自變量t的函數x(t)，y(t)，z(t)；x'(t)，x"(t)，y'(t)，y"(t)，z'(t)，z"(t)分別是分量x(t)，y(t)，z(t)關于自變量t的一階導數和二階導數。

計算管線路徑上各個點的曲率半徑，判斷是否滿足最小彎曲半徑的要求。對不滿足彎曲半徑約束的路段，可以通過移動關鍵點位置或者增加、減少關鍵點的方式進行調整。

3 建模仿真

為了更加形象、直觀地展示管線路徑規劃結果，在Unity場景下引入動態建模方法，結合路徑中心線與管線單元模型生成實體管線模型。

3.1 管線建模

本文采用將管線路徑和管線單元模型結合的建模方法，根據改進RRT算法和平滑處理后獲得管線路徑關鍵點；通過Catmull-Rom獲得管線走線路徑后，將管線單元模型順次拼接得到完整的管線三維實體模型。管線單元模型如圖6所示。

圖6 管線單元模型Fig.6 Pipeline unit model

首先獲取單元模型的三角面頂點信息，計算模型長度。三角面頂點信息包括頂點坐標、法線向量、紋理坐標和切線向量。然后根據管線路徑的長度和方向，依次計算所有分段的三角面信息。根據單元模型三角面頂點信息和管線路徑走向，將所有分段順次拼接，同時去除位置相同的頂點，實現各分段的無縫鏈接，得到網格狀三維管線模型。圖7表示將單元模型按照管線路徑方向順次拼接、去除位置相同的頂點，虛線段之間為一個單元模型。

圖7 單元模型合成管線模型Fig.7 Unit model synthesis pipeline model

3.2 實驗仿真

本文所述RRT算法實驗平臺為Python3引入的matplotlib環境和Unity物理引擎。

3.2.1 基本RRT算法

圖8(a)和圖8(b)是用基本RRT算法在障礙物位置和數量一樣、起點和終點相同的情況下，實驗兩次搜索出來的兩條路徑。在這種情況下，兩條路徑差別很大，存在路徑搜索分散、耗時長、誤差大等問題。

圖8 基本RRT實驗結果Fig.8 Results of basic RRT

3.2.2 節點擴展優化

在基本RRT算法的基礎上，引入路徑緩存的思想，增加隨機節點為目標節點的概率。圖9所示為連續6次規劃得到的路徑，6條路徑在走向上大體一致，且路徑更加平緩。

圖9 節點擴展優化實驗結果Fig.9 Results of node expansion optimization

3.2.3 路徑平滑處理

基于RRT算法得到的初始路徑包含大量的冗余節點。采用貪心算法刪除這些節點，能夠有效縮短管線路徑長度，同時使路徑更平滑，如圖10所示。其中，折線為平滑后的路徑。

圖10 路徑平滑處理實驗結果Fig.10 Result of path smoothing experiment

3.2.4 雙向快速擴展隨機樹

在單棵隨機樹的基礎上，增加一棵從終點開始生長的擴展隨機樹。圖11所示為三維環境下，基本RRT和雙向RRT的對比實驗結果。可以發現，在相同實驗環境下，雙向RRT在節點擴展和搜索效率上比基本RRT更優。

3.2.5 基于關鍵點的曲線擬合

圖11 基本RRT和雙向RRT管線路徑規劃實驗Fig.11 Basic RRT and Bi-RRT pipeline path planning experiments

經過平滑處理后的管線路徑，雖然減少了關鍵點的數量，縮短了路徑長度，但是在關鍵點處的折線路徑可能無法滿足某些管線的敷設要求，因此需要使用樣條曲線擬合。本文采用Catmull-Rom曲線對關鍵點進行曲線擬合，擬合效果如圖12所示。其中圖12(a)和圖12(b)為二維實驗圖，圖(c)和圖(d)為三維實驗圖，折線為平滑處理后的路徑，曲線為擬合處理后的路徑。

圖12 管線路徑曲線擬合實驗結果Fig.12 Pipeline path curve fitting experiment

3.2.6 仿真實驗對比分析

本文在Python環境下，采用同樣的地圖場景和A*算法、遺傳算法進行對比實驗，驗證所提出算法的優越性和適應性。

圖13表示本文方法在同一場景下迭代60次得到的路徑長度。其中縱坐標表示路徑長度，橫坐標表示迭代次數。從圖中可以看出，在引入路徑緩存的情況下，本文算法能夠快速收斂并穩定到一個較優的長度。

圖13 進行60次迭代得到的路徑長度Fig.13 Path length obtained from 60 iterations

在實驗中，如果程序結束能夠找到一條從始至終的路徑，表示這次規劃是成功的。表1所示實驗結果為本文方法與A*算法、遺傳算法在同一場景下進行50次實驗得到的結果。可以看出，各算法均有很高的成功率，相比于A*算法和遺傳算法，本文方法能夠在更短的搜索時間內找到一條長度更短的路徑。

表1 相同實驗環境下三種算法實驗結果Table 1 Results of three algorithess under the same experimental environment

3.2.7 Unity場景實驗

在Unity平臺上進行虛擬環境下的管線路徑仿真模擬。圖14表示經過搜索算法和關鍵點優化后得到的曲線路徑。

圖14 管線路徑規劃與仿真建模Fig.14 Pipeline path planning and simulation modeling

4 結束語

本文在雙向快速擴展隨機樹的基礎上，引入路徑緩存的方法，使擴展樹的生長更具有方向性、高效性。采用貪心算法對管線初始路徑進行平滑處理。利用Catmull-Rom曲線進行路徑關鍵點的曲線擬合和調整，同時添加彎曲半徑約束，使得管線路徑更加合理、有效。

根據上述方法，本文設計實現了在Unity虛擬環境中進行管線路徑規劃，在規劃路徑的同時，進行三維管線實體仿真建模，實現管線的局部調整，使系統更加靈活。

本文方法適用于結構相對規則的管線、軌道類的路徑規劃設計和模擬仿真，在動態環境下的路徑搜索和仿真還需要進一步研究。