徐聯(lián)杰， 劉檢華， 何永熹， 吳宏超， 劉佳順

(北京理工大學(xué)機(jī)械與車輛學(xué)院，北京 100081)

一種基于改進(jìn)快速搜索隨機(jī)樹(shù)算法的管路自動(dòng)布局方法

徐聯(lián)杰， 劉檢華， 何永熹， 吳宏超， 劉佳順

(北京理工大學(xué)機(jī)械與車輛學(xué)院，北京 100081)

針對(duì)非正交管路自動(dòng)布局問(wèn)題，提出一種基于障礙物碰撞信息的快速搜索隨機(jī)樹(shù)改進(jìn)算法。該算法主要采用基于碰撞信息的節(jié)點(diǎn)擴(kuò)展策略、快速繞障算法以及基于概率思想的節(jié)點(diǎn)擴(kuò)展策略3種方法進(jìn)行改進(jìn)，能夠在較短的時(shí)間內(nèi)搜索出一條沿結(jié)構(gòu)件表面從起點(diǎn)到終點(diǎn)的路徑，在此基礎(chǔ)上采用基于關(guān)鍵節(jié)點(diǎn)的路徑優(yōu)化策略，對(duì)求解得到的布局路徑進(jìn)行優(yōu)化后形成最終的管路布局結(jié)果。開(kāi)發(fā)了原型系統(tǒng)，通過(guò)實(shí)例驗(yàn)證了該算法的可行性。

管路；快速擴(kuò)展隨機(jī)樹(shù)；碰撞檢測(cè)；快速繞障；關(guān)鍵節(jié)點(diǎn)

管路是復(fù)雜機(jī)械產(chǎn)品重要組成部分，管路布局質(zhì)量可直接影響管路產(chǎn)品的可靠性。隨著CAD技術(shù)的發(fā)展，國(guó)內(nèi)外學(xué)者嘗試采用自動(dòng)布局技術(shù)提高管路布局設(shè)計(jì)效率和質(zhì)量。

管路自動(dòng)布局設(shè)計(jì)是在給定的幾何、拓?fù)?、技術(shù)和規(guī)則等約束中求解可行管路布置結(jié)果的過(guò)程。從幾何意義上看，是在限定的布置空間中，從指定起點(diǎn)開(kāi)始尋找出一條與其他物體不發(fā)生干涉，滿足各種約束條件且到指定終點(diǎn)的路徑，該問(wèn)題與三維空間的機(jī)器人路徑尋優(yōu)問(wèn)題類似，屬于NP-hard組合優(yōu)化問(wèn)題。

管路自動(dòng)布局問(wèn)題經(jīng)歷了從二維空間的簡(jiǎn)單避開(kāi)障礙物發(fā)展到三維空間內(nèi)的滿足多目標(biāo)、多約束[1]路徑求解的過(guò)程。國(guó)外方面，1975年Rourke[2]首次將迷宮算法用于管路自動(dòng)布局，并使用矩形框包圍障礙物的屏蔽技術(shù)解決碰撞干涉問(wèn)題，該算法基于波的連續(xù)擴(kuò)散原理找到一條最短路徑，但是求解所需的存儲(chǔ)空間大、操作速度慢。Ito[3-4]應(yīng)用遺傳算法在二維平面內(nèi)搜索管路的最優(yōu)路徑，取得了突破性的進(jìn)展，但其在處理遺傳算法的編碼方式和遺傳算子上不太理想。2002年P(guān)ark和Storch[5]利用單元生成法解決多約束目標(biāo)的管路敷設(shè)問(wèn)題，通過(guò)假定的敷設(shè)模式來(lái)產(chǎn)生管線的單元；當(dāng)空間約束繁雜時(shí)，建立基本路徑和改進(jìn)空間路徑并不容易。國(guó)內(nèi)方面，北京航空航天大學(xué)的樊江等[6-7]針對(duì)航空發(fā)動(dòng)機(jī)三維復(fù)雜管路系統(tǒng)的自動(dòng)布局問(wèn)題進(jìn)行了深入地研究，實(shí)現(xiàn)了管路敷設(shè)智能化，并進(jìn)行了很多有價(jià)值的探索。東北大學(xué)的王成恩等[8]針對(duì)航空發(fā)動(dòng)機(jī)中的復(fù)雜管路，基于計(jì)算幾何、微分幾何以及智能優(yōu)化的思想提出了一系列管路布局算法，并取得了較好的效果。大連理工大學(xué)的范小寧[9]采用基于蟻群算法和協(xié)作式互利共生類協(xié)同進(jìn)化算法對(duì)船舶空間多管路的并行敷設(shè)問(wèn)題進(jìn)行求解，并在算法的協(xié)作機(jī)制中采用了優(yōu)良個(gè)體構(gòu)造小環(huán)境的方法，避免了在管路數(shù)目較多時(shí)出現(xiàn)組合爆炸現(xiàn)象。上海交通大學(xué)的曲艷峰等[10-11]采用八叉樹(shù)模型進(jìn)行環(huán)境建模，并用蟻群算法進(jìn)行路徑搜索，以較高的效率實(shí)現(xiàn)了三維管路路徑規(guī)劃問(wèn)題求解。

雖然目前針對(duì)管路自動(dòng)布局問(wèn)題已經(jīng)有了一系列的研究成果，但是在路徑搜索算法中一般需要對(duì)搜索空間進(jìn)行柵格化建模，在柵格數(shù)量較多時(shí)會(huì)出現(xiàn)組合爆炸的問(wèn)題。快速搜索隨機(jī)樹(shù)(rapidly- exploring random tree，RRT)算法是一種增量的路徑規(guī)劃算法，其不需要進(jìn)行柵格化建模，且改進(jìn)具有很高的靈活性，因此在路徑搜索中具有明顯的優(yōu)勢(shì)。到目前為止，RRT算法已經(jīng)有很多經(jīng)典的改進(jìn)方法，其改進(jìn)主要集中在采樣策略、擴(kuò)展策略以及多樹(shù)擴(kuò)展 3個(gè)方面。其中，采樣策略改進(jìn)的經(jīng)典算法主要有 RRT-GoalBais[12]、RRT-GoalZoom、RRT-Local等；擴(kuò)展策略改進(jìn)的經(jīng)典算法主要有RRT-Con[13]、RRT-blossom[14]等；RRT算法還存在多樹(shù)擴(kuò)展[15]的改進(jìn)策略，主要有RRT-ExtExt、RRT-ExtCon、RRT-ConCon等改進(jìn)算法。此外，RRT算法的其他改進(jìn)也在實(shí)際中得到了使用，如機(jī)器人自動(dòng)導(dǎo)航[16]、無(wú)人機(jī)路徑規(guī)劃[17]、機(jī)械臂避障抓取運(yùn)動(dòng)規(guī)劃[18]等。雖然這些改進(jìn)算法在計(jì)算中具有較高的效率，并且有些已經(jīng)進(jìn)入了實(shí)際的運(yùn)用，但是由于管路自動(dòng)布局問(wèn)題的復(fù)雜性，RRT算法有時(shí)(如需要沿障礙物表面敷設(shè)的管路)不能直接用來(lái)進(jìn)行自動(dòng)布局，還需進(jìn)行進(jìn)一步地改進(jìn)。

本文根據(jù)RRT算法在路徑搜索中的特性，結(jié)合管路自動(dòng)布局中的約束問(wèn)題，在現(xiàn)有RRT算法雙樹(shù)擴(kuò)展的基礎(chǔ)上，對(duì)其進(jìn)行改進(jìn)，提出了一種管路自動(dòng)布局方法，開(kāi)發(fā)了管路自動(dòng)布局設(shè)計(jì)原型系統(tǒng)，并通過(guò)實(shí)例對(duì)算法進(jìn)行了測(cè)試與驗(yàn)證。

1　管路自動(dòng)布局求解問(wèn)題分析

1.1基于改進(jìn)RRT算法的管路自動(dòng)布局總體思路

基于改進(jìn) RRT算法的管路自動(dòng)布局方法的總體解決方案，如圖1所示。

圖1　管路自動(dòng)布局總體解決方案

在圖1的解決方案中，①獲取三維管路布局結(jié)構(gòu)模型，構(gòu)建三維管路自動(dòng)布局環(huán)境，在此基礎(chǔ)上開(kāi)展管路布局路徑求解；②對(duì)管路的約束信息進(jìn)行分析，并對(duì)部分工程約束進(jìn)行數(shù)學(xué)建模，轉(zhuǎn)化為管路路徑規(guī)劃過(guò)程中的約束條件，在RRT算法的初始化和改進(jìn)中集中體現(xiàn)，然后利用該算法在可行空間中找到一條初始路徑；③根據(jù)管路約束對(duì)利用RRT算法得到的初始路徑進(jìn)行優(yōu)化，得到最終的管路路徑；④根據(jù)路徑可以直接生成虛擬三維實(shí)體管路模型。此外，在進(jìn)行路徑搜索以及路徑優(yōu)化的過(guò)程中，需要調(diào)用碰撞模型進(jìn)行碰撞檢測(cè)以保證路徑始終處于可行空間中。

1.2管路自動(dòng)布局問(wèn)題描述

管路自動(dòng)布局問(wèn)題可以看作是一個(gè)剛性機(jī)器人在布局空間中進(jìn)行運(yùn)動(dòng)規(guī)劃，并找到一條從起點(diǎn)到終點(diǎn)的無(wú)碰路徑，其數(shù)學(xué)描述如圖2所示。

圖2　管路布局問(wèn)題描述

管路路徑問(wèn)題的工作空間用W表示，3W?R，該狀態(tài)空間不僅包含初始產(chǎn)品所有的機(jī)械零件、裝配夾具和卡具以及工作臺(tái)等，還包括已經(jīng)完成布局的管路及其附件等。其中，作運(yùn)動(dòng)規(guī)劃的剛性機(jī)器人用 A表示，規(guī)劃空間中的障礙物用 O表示，A、O?W。

一般來(lái)說(shuō)，管路的截面都為恒定的圓形，因此可將進(jìn)行路徑規(guī)劃的剛性機(jī)器人設(shè)定為一個(gè)球形剛體，其球心的運(yùn)動(dòng)軌跡即為管路的中心線，剛體運(yùn)動(dòng)掃掠的軌跡即為管路的初始路徑。在管路路徑求解計(jì)算過(guò)程中，球形剛體變換所產(chǎn)生的流形為三維流形，因此求解該路徑規(guī)劃問(wèn)題的狀態(tài)空間是三維的，用Cspace表示，Cspace=R3。Cfree是剛性機(jī)器人在狀態(tài)空間中所有可行解的集合，該空間稱為自由空間。在Cfree中的任意一個(gè)解用x表示，x∈Cfree。

在進(jìn)行管路路徑求解計(jì)算之前，先輸入一個(gè)起點(diǎn)xinit和一個(gè)終點(diǎn)xgoal(xinit，xgoal∈Cfree)，經(jīng)過(guò)計(jì)算得到一系列離散的路徑節(jié)點(diǎn)，其包含了球形剛體的位姿信息，對(duì)這些離散點(diǎn)進(jìn)行優(yōu)化處理即可得到最終的管路路徑。

1.3管路自動(dòng)布局中考慮的約束

在管路自動(dòng)布局過(guò)程中，通常應(yīng)考慮到工程中的敷管規(guī)則。本文主要考慮了沿結(jié)構(gòu)件表面布局的約束，其針對(duì)某些特定的管路，如較長(zhǎng)或振動(dòng)較強(qiáng)的管路，需要利用管卡等進(jìn)行固定以獲得較好的力學(xué)特性，為滿足這一約束對(duì)RRT算法進(jìn)行了相關(guān)的改進(jìn)。同時(shí)，管路與結(jié)構(gòu)件表面之間及管路之間均需預(yù)留一定的間隙，以滿足管路的可裝配性約束(柔性管可以貼壁敷設(shè)，不需考慮此約束)。此外，在優(yōu)先考慮這兩個(gè)約束的基礎(chǔ)上，還需考慮其他管路設(shè)計(jì)規(guī)則：

(1) 管路不能與障礙物(零部件、附件、管路)干涉，即路徑可行；

(2) 管路兩端為直管段，直管段的長(zhǎng)度不小于管徑的2.5倍，以滿足可裝配性約束；

(3) 管路應(yīng)平直且盡可能短，以減輕重量，減少流體損失；

(4) 管路彎頭數(shù)應(yīng)盡可能的少，以減少壓降；

(5) 管路彎曲角度應(yīng)不小于90°，以滿足可加工性約束。

2　基于障礙物碰撞信息的RRT算法改進(jìn)

本文提出的基于障礙物碰撞信息的 RRT改進(jìn)算法，針對(duì)管路需要沿著結(jié)構(gòu)件表面敷設(shè)的約束進(jìn)行相關(guān)的數(shù)據(jù)設(shè)定與算法改進(jìn)，使其在搜索空間中能較快地找到一條沿結(jié)構(gòu)件表面從起點(diǎn)到終點(diǎn)的路徑。算法主要針對(duì)節(jié)點(diǎn)的擴(kuò)展、繞開(kāi)障礙物以及隨機(jī)樹(shù)的收斂性3個(gè)方面進(jìn)行改進(jìn)，包括：基于碰撞信息的節(jié)點(diǎn)擴(kuò)展策略、快速繞障算法以及基于概率思想的節(jié)點(diǎn)擴(kuò)展策略。

2.1基本RRT算法

RRT算法首先由美國(guó)伊利諾伊大學(xué) LaValle[19]提出，是一種適用于在高維空間中搜索路徑的方法。該算法的路徑節(jié)點(diǎn)擴(kuò)展過(guò)程如圖 3所示，在RRT算法進(jìn)行路徑規(guī)劃之前需設(shè)置基本的相關(guān)參數(shù)，如路徑搜索的起點(diǎn)xinit、終點(diǎn) xgoal位置，路徑點(diǎn)擴(kuò)展步長(zhǎng)ρ、終止循環(huán)數(shù)K等。在設(shè)置好參數(shù)后，算法會(huì)先創(chuàng)建一個(gè)空的擴(kuò)展樹(shù)T，并將起點(diǎn)xinit放入T中作為T(mén)的根節(jié)點(diǎn)，并對(duì)T進(jìn)行如下循環(huán)擴(kuò)展：

(1) 通過(guò)均勻隨機(jī)采樣函數(shù)得到一個(gè)隨機(jī)點(diǎn)xrand；

(2) 在T的所有節(jié)點(diǎn)中找到距離xrand最近的節(jié)點(diǎn)xnear；

(3) 沿xnear指向xrand的方向，在與xnear的距離為步長(zhǎng)ρ的位置，得到一個(gè)新的路徑點(diǎn)xnew；

(4) 從xnear到xnew進(jìn)行干涉檢測(cè)，若發(fā)生干涉，則進(jìn)入下一次循環(huán)；

(5) 若從xnear到xnew不干涉，則將新得到的節(jié)點(diǎn)加入T中，并判斷該節(jié)點(diǎn)是否足夠接近終點(diǎn)，若足夠接近，則直接與終點(diǎn)相連，算法結(jié)束，否則繼續(xù)循環(huán)。

若以上循環(huán)執(zhí)行 K次之后仍然無(wú)法找到可行路徑，則說(shuō)明當(dāng)前參數(shù)設(shè)置不合適，待求解問(wèn)題無(wú)可行解或者陷入局部最優(yōu)解等情況，需要重新設(shè)置算法參數(shù)。

圖3　RRT算法的擴(kuò)展過(guò)程

RRT算法的5個(gè)主要特點(diǎn)：①能在有限時(shí)間內(nèi)返回一個(gè)可行解；②在算法中障礙物不需要顯示表達(dá)；③不需要對(duì)空間進(jìn)行預(yù)處理建模；④具有概率完備性，當(dāng)采樣基數(shù)無(wú)限大時(shí)，找到可行解的概率趨近 100%；⑤能夠有效解決高維空間和復(fù)雜約束的運(yùn)動(dòng)規(guī)劃問(wèn)題，求解速度快。

由于RRT算法具有以上的特點(diǎn)，在管路路徑規(guī)劃中具有很大的優(yōu)勢(shì)，然而由于該算法具有擴(kuò)展到未探索區(qū)域的趨勢(shì)，如果將其直接用于管路自動(dòng)布局，不僅計(jì)算效率低，且不能滿足管路布局中的工程約束，因此基于RRT良好的適應(yīng)性，對(duì)其采樣和擴(kuò)展等方面進(jìn)行改進(jìn)以應(yīng)用于管路自動(dòng)布局設(shè)計(jì)。

2.2算法初始化

為滿足管路的相關(guān)約束，在進(jìn)行路徑求解之前，需要對(duì)相關(guān)的數(shù)據(jù)進(jìn)行設(shè)定，主要包括球形剛體半徑的確定、算法起始點(diǎn)位置的確定等。

根據(jù)可裝配性約束的要求，管路的兩端設(shè)計(jì)為直管段，且直管段的長(zhǎng)度不小于管徑的2.5倍。針對(duì)該約束，在確定RRT算法的起始點(diǎn)和終止點(diǎn)的位置時(shí)，將選定管接口的圓心沿接口平面外法向方向移動(dòng)距離d(d的值為管路外徑的2.5倍)，將移動(dòng)后得到的位置點(diǎn)分別設(shè)為起點(diǎn)和終點(diǎn)。

此外，利用三角面片模型對(duì)規(guī)劃空間中所有的物體包括剛體小球與障礙進(jìn)行碰撞檢測(cè)。碰撞檢測(cè)模型在規(guī)劃算法中作為一個(gè)黑盒來(lái)使用，在每次碰撞檢測(cè)中，若檢測(cè)到碰撞，則返回本次碰撞相關(guān)的信息。

2.3基于碰撞信息的節(jié)點(diǎn)擴(kuò)展策略

文獻(xiàn)[20]提出了一種基于邊界約束的 RRT算法，該算法能夠利用邊界約束使隨機(jī)樹(shù)沿障礙物表面擴(kuò)展，采用傳感器檢測(cè)障礙物邊界，而在管路自動(dòng)布局中障礙物邊界需要調(diào)用大量的碰撞檢測(cè)來(lái)探測(cè)，因此并不適用。本文根據(jù)文獻(xiàn)[21]中OBRRT的節(jié)點(diǎn)擴(kuò)展的思想，在路徑搜索時(shí)利用剛性機(jī)器人與環(huán)境中的障礙物的碰撞信息構(gòu)造邊界約束，這些碰撞信息包括發(fā)生碰撞的碰撞點(diǎn)位置以及障礙物表面的外法向量，在擴(kuò)展過(guò)程中基于該信息確定擴(kuò)展方向，并且將障礙物表面的外法向量作為方向?qū)傩蕴砑拥疆?dāng)前節(jié)點(diǎn)中。其中，確定節(jié)點(diǎn)擴(kuò)展方向主要有獲取碰撞信息和計(jì)算擴(kuò)展方向兩部分。

(1) 獲取碰撞信息。碰撞信息的獲取主要是剛性機(jī)器人在待擴(kuò)展節(jié)點(diǎn)位置沿著某一向量進(jìn)行碰撞檢測(cè)，根據(jù)檢測(cè)到的碰撞獲得障礙物表面的碰撞位置點(diǎn)xc以及該點(diǎn)所在表面的外法向量n。在該過(guò)程中，需要解決的問(wèn)題是如何構(gòu)造碰撞檢測(cè)向量，使待擴(kuò)展節(jié)點(diǎn)在其附近能夠快速地檢測(cè)到碰撞。其中向量的長(zhǎng)度為定值l，其大小設(shè)為節(jié)點(diǎn)擴(kuò)展步長(zhǎng)ρ 的3倍(當(dāng)待擴(kuò)展節(jié)點(diǎn)為根節(jié)點(diǎn)時(shí)，l的大小為10 ρ)。

本文針對(duì)向量ξ的構(gòu)造，依次采用如圖4所示的3種方法：通過(guò)生成的隨機(jī)采樣點(diǎn)xrand構(gòu)造從xnear到xrand的向量，如圖4(a)所示；構(gòu)造從xnear到xgoal的向量ξ，如圖4(b)所示；利用父節(jié)點(diǎn)xpar的位置信息，構(gòu)造從xpar到xnear的向量，如圖4(c)所示。當(dāng)任意一步構(gòu)造的向量檢測(cè)到碰撞時(shí)，即可獲取碰撞信息。

圖4　基于節(jié)點(diǎn)信息構(gòu)造碰撞檢測(cè)方向向量ξ

若上述方法無(wú)法檢測(cè)到碰撞，則構(gòu)造一個(gè)循環(huán)，該循環(huán)通過(guò)隨機(jī)生成一個(gè)向量ξ進(jìn)行碰撞檢測(cè)，若檢測(cè)到碰撞時(shí)，即可獲取碰撞信息進(jìn)行下一步擴(kuò)展；若循環(huán)次數(shù)達(dá)到20，結(jié)束循環(huán)，當(dāng)前節(jié)點(diǎn)擴(kuò)展失敗。

(2) 計(jì)算擴(kuò)展方向。在隨機(jī)樹(shù)貼近障礙物表面的擴(kuò)展過(guò)程中，不僅要保證節(jié)點(diǎn)能夠快速有效地搜索到一條可行路徑，而且根據(jù)路徑節(jié)點(diǎn)生成管路走向還需要盡量與結(jié)構(gòu)件表面保持平齊，基于該約束，隨機(jī)樹(shù)中貼近障礙物表面擴(kuò)展的隨機(jī)樹(shù)既要能夠快速找到一條連通路徑，還要與障礙物之間保持穩(wěn)定的距離，因此確定擴(kuò)展方向是很有必要的。

在確定節(jié)點(diǎn)擴(kuò)展方向的過(guò)程中，主要分以下2種情況進(jìn)行處理：

夫妻離婚爭(zhēng)孩子，老婆理直氣壯說(shuō)：“孩子從我肚子里出來(lái)的，當(dāng)然歸我！”老公說(shuō)：“笑話！簡(jiǎn)直是胡說(shuō)八道！取款機(jī)里取出來(lái)的錢(qián)能歸取款機(jī)嗎？還不是誰(shuí)插卡歸誰(shuí)！”

①當(dāng)待擴(kuò)展節(jié)點(diǎn)xnear與發(fā)生碰撞的障礙物表面的距離等于d時(shí)，其中d為管路半徑的1.3倍，如圖5(a)所示，根據(jù)碰撞得到的障礙物表面法向量n，計(jì)算出垂直于n的向量ξn，取沿ξn方向，距離xnear長(zhǎng)度為擴(kuò)展步長(zhǎng)ρ的坐標(biāo)點(diǎn)為新節(jié)點(diǎn)xnew的位置，若從xnear到xnew之間無(wú)碰撞，則將xnew加入到隨機(jī)樹(shù)T中，擴(kuò)展成功，否則擴(kuò)展失敗；

②若擴(kuò)展節(jié)點(diǎn)xnear與障礙物表面的距離不等于d時(shí)，如圖5(b)所示，根據(jù)碰撞得到的碰撞點(diǎn)位置xc和障礙物表面外法向n，可以計(jì)算得到垂直于 n的向量ξn，并且在xnear到障礙物表面的垂線上計(jì)算出與障礙物距離為d的點(diǎn)xt，取沿ξn方向，距離xt長(zhǎng)度為擴(kuò)展步長(zhǎng)ρ的坐標(biāo)點(diǎn)為新節(jié)點(diǎn)xnew的位置，若從xnear到xnew之間無(wú)碰撞，則將xnew加入到隨機(jī)樹(shù)T中，擴(kuò)展成功，否則擴(kuò)展失敗。

圖5　xnear與障礙物表面的距離是否等于d時(shí)擴(kuò)展方向的計(jì)算

在進(jìn)行擴(kuò)展的過(guò)程中，根據(jù)得到的碰撞信息，可采用2種方法得到向量ξn，這2種方法能夠引導(dǎo)隨機(jī)樹(shù)的節(jié)點(diǎn)沿障礙物表面向隨機(jī)方向和終點(diǎn)方向擴(kuò)展，2種方向分別以0.5的概率生成，既能保證隨機(jī)樹(shù)快速向終點(diǎn)方向生長(zhǎng)，同時(shí)也具有沿障礙物表面探索其他可行路徑的能力。

隨機(jī)方向。如圖6(a)所示，根據(jù)得到的障礙物表面法向量n，可以得到垂直于n的一個(gè)平面，在該平面內(nèi)的任意一個(gè)向量都垂直于向量n，因此令該向量為ξn=(x,y,z)，則n·ξn=0，在區(qū)間(0,1)內(nèi)通過(guò)均勻隨機(jī)的方法得到x，y的值，進(jìn)而計(jì)算出z的大小，通過(guò)單位化即可得到一個(gè)隨機(jī)向量ξn。

終點(diǎn)方向。如圖6(b)所示，利用已有的xnear、xgoal的位置信息，構(gòu)造向量 ξgoal，則可以在經(jīng)過(guò)垂直于向量n的平面中計(jì)算出一個(gè)與ξgoal夾角最小的單位向量ξn，從而使隨機(jī)樹(shù)向終點(diǎn)方向擴(kuò)展。

圖6　向量ξn的確定

具體計(jì)算方法是根據(jù)ξn與向量n、ξgoal之間的幾何關(guān)系，構(gòu)造式(1)所示的方程組：

解式(1)，即可計(jì)算出向量ξn的值。沿障礙物表面擴(kuò)展的RRT算法偽代碼如圖7所示。

2.4快速繞障算法

在路徑搜索過(guò)程中，由于RRT算法采用基于終點(diǎn)信息擴(kuò)展的策略進(jìn)行節(jié)點(diǎn)的擴(kuò)展，因此當(dāng)該算法在沿著障礙物表面擴(kuò)展時(shí)需要繞開(kāi)其他障礙物，可能會(huì)產(chǎn)生搜索節(jié)點(diǎn)過(guò)多或搜索路徑沿另一障礙物表面擴(kuò)展的問(wèn)題，不僅影響算法的搜索效率，而且得到的路徑可能無(wú)法滿足管路敷設(shè)的約束要求。因此本文提出一種基于障礙物表面法向量的快速繞障算法，該方法能基于上一節(jié)點(diǎn)的碰撞信息進(jìn)行下一節(jié)點(diǎn)的定向擴(kuò)展，使節(jié)點(diǎn)能夠更加快速地繞過(guò)障礙物，具體來(lái)說(shuō)，當(dāng)沿障礙物擴(kuò)展的節(jié)點(diǎn)處于以下2種情況時(shí)：①節(jié)點(diǎn)在擴(kuò)展失敗時(shí)檢測(cè)到碰撞，計(jì)算得到其障礙物表面法向量 nc與待擴(kuò)展節(jié)點(diǎn)的方向?qū)傩韵蛄縩的夾角大于45°；②待擴(kuò)展節(jié)點(diǎn)在獲取碰撞信息中得到的碰撞向量與待擴(kuò)展節(jié)點(diǎn)的方向?qū)傩韵蛄縩的夾角大于45°。將向量nc作為當(dāng)前節(jié)點(diǎn)的第二個(gè)屬性向量添加到節(jié)點(diǎn)中，如圖8所示。

根據(jù)節(jié)點(diǎn)的兩個(gè)屬性向量確定擴(kuò)展方向，進(jìn)行循環(huán)擴(kuò)展過(guò)程如下：

圖7　沿障礙物表面擴(kuò)展的RRT算法偽代碼

圖8　具有兩個(gè)屬性向量的節(jié)點(diǎn)

(1) 當(dāng)前節(jié)點(diǎn)具有第二個(gè)屬性向量時(shí)，根據(jù) n 與nc的值，計(jì)算出方向向量ξe=±(n×nc)，如圖9(a)所示，該向量垂直于向量n和nc，ξe取與向量ξgoal夾角為銳角的值；

(2) 節(jié)點(diǎn) xnear沿 ξe方向擴(kuò)展一個(gè)步長(zhǎng) ρ得到xnew，此時(shí)若 xnear到 xnew方向不發(fā)生碰撞，則擴(kuò)展成功，否則退出循環(huán)；

(3) 在擴(kuò)展得到新的節(jié)點(diǎn) xnew后，如圖 9(b)所示，將節(jié)點(diǎn)xnew作為待擴(kuò)展節(jié)點(diǎn)，根據(jù)父節(jié)點(diǎn)的方向?qū)傩韵蛄?n和 nc的反方向進(jìn)行碰撞檢測(cè)，若 2個(gè)方向都發(fā)生碰撞，計(jì)算得到節(jié)點(diǎn)xnew的2個(gè)方向?qū)傩韵蛄縩和nc，則按照上述1、2循環(huán)進(jìn)行節(jié)點(diǎn)擴(kuò)展，在循環(huán)擴(kuò)展過(guò)程中，為避免節(jié)點(diǎn)的擴(kuò)展發(fā)生反向的情況，ξe的方向還應(yīng)該與父節(jié)點(diǎn)到待擴(kuò)展節(jié)點(diǎn)的方向向量的夾角成銳角，否則結(jié)束循環(huán)；

(4) 當(dāng)循環(huán)擴(kuò)展過(guò)程中節(jié)點(diǎn) xnew沒(méi)有得到方向向量nc時(shí)，如圖9(c)所示，則沿nc方向(或者沿–nc方向，取與ξgoal夾角為銳角的方向)擴(kuò)展一個(gè)臨時(shí)節(jié)點(diǎn)xnew2，若擴(kuò)展失敗，則退出循環(huán)；若擴(kuò)展成功，則該節(jié)點(diǎn)的屬性向量n與xnew的向量n相同，并在該節(jié)點(diǎn)通過(guò)隨機(jī)生成至多k個(gè)方向進(jìn)行碰撞檢測(cè)，若不存在 nc，則刪除節(jié)點(diǎn) xnew2并結(jié)束循環(huán)，若存在，則按照上述循環(huán)繼續(xù)進(jìn)行節(jié)點(diǎn)擴(kuò)展；

(5) 對(duì)于進(jìn)行快速繞障算法擴(kuò)展的節(jié)點(diǎn)，為避免在后續(xù)的節(jié)點(diǎn)擴(kuò)展中重復(fù)利用該方法，利用快速繞障算法擴(kuò)展的節(jié)點(diǎn)以及在這些節(jié)點(diǎn)一定范圍內(nèi)的其他節(jié)點(diǎn)不再調(diào)用該方法進(jìn)行擴(kuò)展，為保證節(jié)點(diǎn)的可擴(kuò)展性，該節(jié)點(diǎn)依然可以利用原來(lái)的方法進(jìn)行擴(kuò)展。

快速繞障算法的偽代碼如圖10所示。

圖9　基于障礙物表面法向量的快速繞障算法

2.5基于概率的節(jié)點(diǎn)擴(kuò)展策略

在管路路徑規(guī)劃中，雖然管路是沿著結(jié)構(gòu)件表面敷設(shè)，而在某些無(wú)法檢測(cè)到碰撞的情況時(shí)，如管路接口處于懸臂狀態(tài)、路徑需跨越窄縫等，隨機(jī)樹(shù)需要在自由空間中進(jìn)行擴(kuò)展。因此針對(duì)RRT算法在路徑搜索的過(guò)程中，既需要保證隨機(jī)樹(shù)在沿著結(jié)構(gòu)件表面向目標(biāo)點(diǎn)收斂的同時(shí)，還需要保留算法一定的發(fā)散性，并保證隨機(jī)樹(shù)能向自由空間擴(kuò)展，因此本文采用基于概率的思想進(jìn)行節(jié)點(diǎn)的擴(kuò)展，對(duì)每個(gè)節(jié)點(diǎn)賦予一個(gè)概率值，針對(duì)隨機(jī)樹(shù)中的待擴(kuò)展節(jié)點(diǎn)，通過(guò)在區(qū)間［0,1］內(nèi)生成的一個(gè)隨機(jī)數(shù)與當(dāng)前節(jié)點(diǎn)的概率值進(jìn)行比較，當(dāng)該值小于或等于當(dāng)前節(jié)點(diǎn)的擴(kuò)展概率時(shí)，對(duì)該節(jié)點(diǎn)進(jìn)行擴(kuò)展，否則不擴(kuò)展。

圖10　快速繞障算法偽代碼

本文針對(duì)2種情況對(duì)節(jié)點(diǎn)概率進(jìn)行賦值：

(1) 若某個(gè)節(jié)點(diǎn)在待擴(kuò)展時(shí)檢測(cè)到與障礙物發(fā)生碰撞，則認(rèn)為該節(jié)點(diǎn)位于障礙物表面附近，將其擴(kuò)展概率設(shè)為1；

(2) 若某個(gè)節(jié)點(diǎn)在待擴(kuò)展時(shí)未檢測(cè)到碰撞，則認(rèn)為該節(jié)點(diǎn)附近沒(méi)有障礙物，將其擴(kuò)展概率設(shè)為p(p∈[0,1])。

在管路自動(dòng)布局的過(guò)程中，p的大小根據(jù)實(shí)際情況確定，當(dāng)沿障礙物擴(kuò)展敷設(shè)的要求不嚴(yán)格時(shí)，p值較大，反之較小，一般設(shè)置為0.25左右?；诟怕仕枷氲墓?jié)點(diǎn)擴(kuò)展的流程圖如圖11所示。

圖11　基于概率的節(jié)點(diǎn)擴(kuò)展流程

3　基于關(guān)鍵節(jié)點(diǎn)的路徑優(yōu)化

在采用本文算法得到的初始路徑，是通過(guò)具有固定步長(zhǎng)的樹(shù)節(jié)點(diǎn)得到的，因此該路徑對(duì)與管路來(lái)說(shuō)具有較多的冗余節(jié)點(diǎn)，并不能直接用來(lái)生成管路模型，需要對(duì)其進(jìn)行優(yōu)化。在優(yōu)化過(guò)程中保證管路沿結(jié)構(gòu)件表面敷設(shè)的前提下，還需要保證管路長(zhǎng)度盡量短并且彎頭數(shù)應(yīng)盡可能的少，因此需采用一種基于關(guān)鍵節(jié)點(diǎn)的路徑優(yōu)化方法。

由于在路徑搜索過(guò)程中，隨機(jī)樹(shù)中大部分的節(jié)點(diǎn)都是根據(jù)障礙物表面外法向量進(jìn)行擴(kuò)展的，且節(jié)點(diǎn)中保存有相關(guān)的擴(kuò)展信息，因此在管路布局中路徑優(yōu)化的方法是利用路徑中的節(jié)點(diǎn)信息確定關(guān)鍵節(jié)點(diǎn)，并基于這些關(guān)鍵節(jié)點(diǎn)剔除其他的冗余節(jié)點(diǎn)。關(guān)鍵節(jié)點(diǎn)的判斷依據(jù)包括：

(1) 路徑的起點(diǎn)和終點(diǎn)；

(2) 從路徑起點(diǎn)開(kāi)始，第一個(gè)具有方向?qū)傩缘墓?jié)點(diǎn)；

(3) 快速繞障算法所生成路徑的起點(diǎn)、終點(diǎn)和拐點(diǎn)；

(4) 從具有屬性向量n的關(guān)鍵節(jié)點(diǎn)開(kāi)始向后搜索，當(dāng)出現(xiàn)屬性向量與該關(guān)鍵節(jié)點(diǎn)的屬性向量夾角大于 30°的節(jié)點(diǎn)時(shí)，說(shuō)明該路徑節(jié)點(diǎn)位置附近的障礙物表面很可能出現(xiàn)了較大凸凹現(xiàn)象，為保證路徑始終處于障礙物附近，將該節(jié)點(diǎn)設(shè)為路徑的關(guān)鍵節(jié)點(diǎn)，如圖12所示的節(jié)點(diǎn)xk2。

根據(jù)關(guān)鍵節(jié)點(diǎn)的定義，需確定、保留路徑中的關(guān)鍵節(jié)點(diǎn)，并剔除路徑中的冗余點(diǎn)，與此同時(shí)，引入插值碰撞檢測(cè)對(duì)路徑進(jìn)行優(yōu)化。其優(yōu)化過(guò)程為：從路徑起點(diǎn)開(kāi)始，將路徑中不屬于關(guān)鍵節(jié)點(diǎn)的路徑點(diǎn)剔除，保留關(guān)鍵節(jié)點(diǎn)，再對(duì)每個(gè)關(guān)鍵節(jié)點(diǎn)之間進(jìn)行插值碰撞檢測(cè)，若兩個(gè)節(jié)點(diǎn)之間發(fā)生碰撞，則在原路徑的基礎(chǔ)上對(duì)后一關(guān)鍵節(jié)點(diǎn)進(jìn)行回溯碰撞檢測(cè)，如圖13所示，節(jié)點(diǎn)1到10為原路徑中的節(jié)點(diǎn)，其中關(guān)鍵節(jié)點(diǎn)為1、10，由于兩個(gè)關(guān)鍵節(jié)點(diǎn)之間發(fā)生碰撞，因此先在節(jié)點(diǎn) 1、9之間進(jìn)行插值碰撞檢測(cè)，若仍然發(fā)生碰撞，繼續(xù)在 1、8之間進(jìn)行插值碰撞檢測(cè)，直到節(jié)點(diǎn)7未發(fā)生碰撞時(shí)，將該節(jié)點(diǎn)保存下來(lái)，然后以節(jié)點(diǎn)7為起點(diǎn)，繼續(xù)按照上述方法進(jìn)行優(yōu)化。

圖12　拐角位置關(guān)鍵節(jié)點(diǎn)的判斷

在管路布局中，為保證敷設(shè)管路的可加工性，還需要對(duì)路徑節(jié)點(diǎn)的彎曲角度進(jìn)行局部?jī)?yōu)化處理，如圖14所示：根據(jù)與控制點(diǎn)1相連的兩個(gè)控制點(diǎn)2、3得到兩個(gè)單位向量V12、V13，當(dāng)其夾角α小于90°時(shí)，則計(jì)算出α角平分線的單位向量V1，將控制點(diǎn)1沿V1方向以一定的步長(zhǎng)λ逐步平移，直到彎曲角度大于90°。

圖13　引入碰撞檢測(cè)的節(jié)點(diǎn)優(yōu)化過(guò)程

圖14　彎曲角度的調(diào)整

圖15為路徑節(jié)點(diǎn)優(yōu)化示意圖，圖15(a)所示路徑為根據(jù)改進(jìn)RRT算法得到的初始路徑點(diǎn)，xinit與xgoal分別為路徑的起點(diǎn)和終點(diǎn)，經(jīng)過(guò)優(yōu)化后得到的最終路徑如圖15(b)所示。

圖15　路徑節(jié)點(diǎn)優(yōu)化示意圖

4　實(shí)例驗(yàn)證

利用三維造型引擎ACIS和三維顯示交互工具包HOOPS，自主設(shè)計(jì)并開(kāi)發(fā)了管路自動(dòng)布局系統(tǒng)，該系統(tǒng)在Microsoft Visual Studio2005上利用C++語(yǔ)言開(kāi)發(fā)，開(kāi)發(fā)硬件為HP Z400圖形工作站，CPU為2.67 GHz Intel Xeon W3520，內(nèi)存4 G (3.48 G可用)，顯卡為 Nvidia Quadro 5000，操作系統(tǒng)為Windows 7專業(yè)版。

為了驗(yàn)證算法的計(jì)算效率與布局質(zhì)量，對(duì)提出改進(jìn)的RRT算法進(jìn)行了測(cè)試，測(cè)試結(jié)果與雙樹(shù)擴(kuò)展中3種經(jīng)典的算法RRT-ExtExt、RRT-ConCon以及RRT-ExtCon進(jìn)行了對(duì)比，測(cè)試結(jié)果如圖16所示；計(jì)算結(jié)果如表1所示。通過(guò)綜合對(duì)比可以看出，改進(jìn)的RRT算法在保證搜索成功率的同時(shí)，能夠在較短的時(shí)間內(nèi)搜索出符合管路約束的路徑。

為驗(yàn)證該算法沿結(jié)構(gòu)件表面敷設(shè)的效果，在如圖 17所示模型(管路接口已隱藏)中的凹形區(qū)域進(jìn)行實(shí)例測(cè)試，求解得到的雙樹(shù)節(jié)點(diǎn)如圖17(a)所示，其中紅色高亮節(jié)點(diǎn)為得到的初始路徑點(diǎn)，生成的實(shí)體管路模型如圖17(b)所示。

圖16　測(cè)試對(duì)比實(shí)例

圖17　管路沿結(jié)構(gòu)件表面布局效果

表1　算法測(cè)試結(jié)果

運(yùn)用提出的改進(jìn) RRT算法進(jìn)行管路自動(dòng)布局設(shè)計(jì)，在自主開(kāi)發(fā)的管路布局系統(tǒng)上對(duì)某產(chǎn)品進(jìn)行了驗(yàn)證，根據(jù)選定的接口位置信息自動(dòng)求解出管路路徑，并生成實(shí)體管路模型，如圖18所示。

圖18　某產(chǎn)品管路自動(dòng)布局結(jié)果

5　結(jié)論及展望

(1) 本文提出了一種基于障礙物碰撞檢測(cè)的RRT算法，其主要針對(duì)節(jié)點(diǎn)的擴(kuò)展、繞開(kāi)障礙物以及隨機(jī)樹(shù)的生長(zhǎng)趨勢(shì)3個(gè)方面進(jìn)行改進(jìn)，并采用基于關(guān)鍵節(jié)點(diǎn)的路徑優(yōu)化方法對(duì)求解到的路徑進(jìn)行了優(yōu)化，在較短的時(shí)間內(nèi)搜索出了符合限定約束條件下的管路路徑，有效地解決復(fù)雜結(jié)構(gòu)條件下的沿結(jié)構(gòu)件表面的管路自動(dòng)布局問(wèn)題。

(2) 在算法中利用快速繞障算法，避免了障礙物表面生成過(guò)多的冗余節(jié)點(diǎn)，提高了算法的搜索效率。

(3) 該算法能夠得到較好的布局方案，但是由于管路布局的復(fù)雜性，并沒(méi)有全面考慮工程約束，這些都需要在以后的工作中進(jìn)一步探索。

[1] Sandurkar S, Chen W. GAPRUS—genetic algorithms based pipe routing using tessellated objects [J]. Computers in Industry, 1999, 38(3): 209-223.

[2] Rourke P W. Development of a three-dimensional pipe routing algorithm [D]. Benthlehem: Lehigh University, 1975.

[3] Ito T. A genetic algorithm approach to piping route path planning [J]. Journal of Intelligent Manufacturing, 1999, 10(1): 103-114.

[4] Ito T. Piping layout wizard: basic concepts and its potential for pipe route planning [M]. Methodology and Tools in Knowledge-Based Systems. Springer Berlin Heidelberg, 1998: 438-447.

[5] Park J H, Storch R L. Pipe-routing algorithm development: case study of a ship engine room design [J]. Expert Systems with Applications, 2002, 23(3): 299-309.

[6] 樊江, 馬枚, 楊曉光. 航空發(fā)動(dòng)機(jī)外部管路自動(dòng)敷設(shè)研究[J]. 機(jī)械設(shè)計(jì), 2003, 20(7): 21-23.

[7] 樊江, 馬枚, 楊曉光. 基于協(xié)進(jìn)化的管路系統(tǒng)智能尋徑[J]. 航空動(dòng)力學(xué)報(bào), 2004, 19(5): 593-597.

[8] 王成恩, 柳強(qiáng), 白曉蘭, 等. 航空發(fā)動(dòng)機(jī)復(fù)雜約束空間內(nèi)管路敷設(shè)技術(shù)[J]. 計(jì)算機(jī)集成制造系統(tǒng), 2010, 16(11): 327-332.

[9] 范小寧. 船舶管路布局優(yōu)化方法及應(yīng)用研究[D]. 大連:大連理工大學(xué), 2006.

[10] 曲艷峰, 蔣丹. 基于八叉樹(shù)建模和ACA的三維管路路徑規(guī)劃[J]. 計(jì)算機(jī)工程, 2011, 37(23): 4-7.

[11] Qu Y F, Jiang D, Liu B. A multi-pipe path planning by modified ant colony optimization [J]. Computer Aided Drafting, Design and Manufacturing, 2011, 21(1): 1-7.

[12] LaValle S M, Kuffner J J. Randomized kinodynamic planning [J]. The International Journal of Robotics Research, 2001, 20(5): 378-400.

[13] Kuffner J J, LaValle S M. RRT-connect: an efficient approach to single-query path planning [C]//Robotics and Automation, 2000. Proceedings. ICRA’00. IEEE International Conference on. IEEE, 2000: 995-1001.

[14] Kalisiak M, van de Panne M. RRT-blossom: RRT with a local flood-fill behavior [C]//International Conference on Robotics & Automation. IEEE, 2006: 1237-1242.

[15] Zucker M, Kuffner J, Branicky M. Multipartite RRTs for rapid replanning in dynamic environments [C]//Robotics and Automation, 2007 IEEE International Conference on. IEEE, 2007: 1603-1609.

[16] 賈菁輝. 移動(dòng)機(jī)器人的路徑規(guī)劃與安全導(dǎo)航[D]. 大連:大連理工大學(xué), 2009.

[17] 劉偉, 鄭征, 蔡開(kāi)元, 等. 快速平滑收斂策略下基于QS-RRT的UAV運(yùn)動(dòng)規(guī)劃[J]. 中國(guó)科學(xué): 信息科學(xué), 2012, 42(11): 1403-1422.

[18] 楊宇盟, 聶斌, 方紅根, 等. 虛擬人手臂避障抓取運(yùn)動(dòng)規(guī)劃[J]. 計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào), 2014, 26(8): 1362-1373.

[19] LaValle S M. Rapidly-exploring random trees a new tool for path planning [R]. Technical Report No. 98-11. 1998.

[20] 呂偉新, 趙立軍, 王珂, 等. 基于邊界約束RRT的未知環(huán)境探索方法[J]. 華中科技大學(xué)學(xué)報(bào): 自然科學(xué)版, 2011, 39(S2): 366-369.

[21] Rodriguez S, Tang X, Lien J M, et al. An obstacle-based rapidly-exploring random tree [C]//Robotics and Automation, ICRA 2006. Proceedings 2006 IEEE International Conference on. IEEE, 2006: 895-900.

A Method for Pipe Auto Layout Based Improved RRT Algorithm

Xu Lianjie,Liu Jianhua,He Yongxi,Wu Hongchao,Liu Jiashun

(School of Mechanical Engineering, Beijing Institute of Technology, Beijing 100081, China)

An improved rapidly-exploring random tree algorithm is proposed based on collision information for the problem of non-orthogonal pipe automatic routing. This algorithm has three main improved methods: node expansion based collision information, fast bypassing obstacle algorithm and node expansion based on the thinking of node’s probability. It could search out a path to walk along the surface of structure parts in comparably short time. On the basis of the three methods, the optimization strategy based key nodes is used to optimize the obtained path and form the final result of pipe routing layout. A prototype system is developed and the feasibility of the algorithm by instance is verified.

pipe; rapidly-exploring random tree; collision detection; fast bypassing obstacle algorithm; key nodes

TP 391.9

10.11996/JG.j.2095-302X.2016010001

A

2095-302X(2016)01-0001-10

2015-09-24；定稿日期：2015-10-09

國(guó)家自然科學(xué)基金項(xiàng)目(51275047)；“十二五”國(guó)防基礎(chǔ)科研項(xiàng)目(A2220110008)

徐聯(lián)杰(1990–)，男，湖南常德人，碩士研究生。主要研究方向?yàn)楣苈纷詣?dòng)布局技術(shù)。E-mail：xulianjie1234@163.com

劉檢華(1977–)，男，江西萍鄉(xiāng)人，教授，博士，博士生導(dǎo)師。主要研究方向?yàn)閿?shù)字化裝配與檢測(cè)。E-mail：jeffliu@bit.edu.cn