基于RRTConCon算法的船舶裝配拆卸高斯采樣路徑規(guī)劃

閆富玉 朱曉軍 彭 飛

海軍工程大學(xué)船舶與動力學(xué)院，湖北武漢 430033

1 引 言

維修性是艦船裝備的重要質(zhì)量特性，是艦船戰(zhàn)斗力的主要因素之一。艦船裝備設(shè)計方案確定后，維修性就已基本確定。因此在艦船設(shè)計初期，就必須考慮艦船裝備的維修可達(dá)性，即裝備裝配拆卸的可達(dá)性。裝配拆卸路徑規(guī)劃是用于檢驗裝備裝配拆卸可達(dá)性的一種有效方法，其基本任務(wù)是為裝備的可拆卸單元 （Local Replaceable Unit，LRU）尋找使其能從裝配體中順利進(jìn)出的路徑。目前，解決此類問題的一種重要方法是在虛擬的三維環(huán)境中利用智能路徑規(guī)劃技術(shù)尋求一條從起始點到目標(biāo)點的路徑，當(dāng)LRU沿此路徑裝配拆卸時不會與環(huán)境中其它物體發(fā)生碰撞［1］。這種自動路徑規(guī)劃技術(shù)對于復(fù)雜空間中的產(chǎn)品布置和裝配拆卸設(shè)計非常重要，如潛艇艙室、艦上狹窄通道等狹小空間。因此，在艦船裝備設(shè)計的初期充分考慮解決裝備維修的可達(dá)性將有利于提高艦船設(shè)計的質(zhì)量和維修性，提高艦船生命力。

2 裝配拆卸路徑規(guī)劃問題

裝配拆卸路徑規(guī)劃問題本質(zhì)上可歸結(jié)為機(jī)器人的運動規(guī)劃問題。在三維環(huán)境中，LRU可視為具有n個自由度的剛體機(jī)器人，而裝配體則可看作是靜態(tài)障礙物［2-3］。為便于計算，將LRU運動的物理空間轉(zhuǎn)化為數(shù)學(xué)上的構(gòu)型空間（C 空間）［4］，則C空間可以分為兩部分：障礙空間（Cobs）和自由空間（Cfree）。LRU運動的路徑對應(yīng)于自由空間中一條連接初始位姿點與目標(biāo)位姿點的曲線。由此，路徑規(guī)劃問題便轉(zhuǎn)化為求解C空間中一條曲線的數(shù)學(xué)問題。

路徑規(guī)劃算法大致可分為基于自由空間幾何構(gòu)造的規(guī)劃、前向圖搜索算法和近年興起的以解決高維姿態(tài)空間和復(fù)雜環(huán)境中路徑規(guī)劃為目的的基于隨機(jī)采樣的運動規(guī)劃。前兩種算法的計算復(fù)雜度與運動物體的自由度呈指數(shù)關(guān)系，對于船舶等復(fù)雜空間不適用。因此，1990年Barraquand和Latombe提出了基于隨機(jī)采樣的RPP規(guī)劃算法［5］。在此基礎(chǔ)上，逐步發(fā)展出了PRM算法［6］和 RRT 算法［7］，這兩種算法非常適合于解決高維空間中的路徑規(guī)劃問題［8］。此后，RRT算法又發(fā)展出一些改進(jìn)算法，如 RRTExtExt［9］、RRTConCon［10］等。

在現(xiàn)有的路徑規(guī)劃算法中，RRTConCon算法的規(guī)劃效率較高，但該算法是采用隨機(jī)采樣的方法選取位姿點，在解決船舶艙室內(nèi)狹窄通道的路徑規(guī)劃問題時效率不高。為解決這一問題，本文在RRTConCon算法的基礎(chǔ)上，采用高斯分布函數(shù)進(jìn)行分區(qū)采樣，即在大的開闊區(qū)域設(shè)置較少的采樣點，在復(fù)雜區(qū)域或狹窄通道設(shè)置較多的采樣點，然后進(jìn)行局部規(guī)劃，找出拆裝路徑，提出了一種基于高斯采樣的RRTGaussion算法，以提高規(guī)劃效率。

3 基于高斯采樣的RRTConCon算法

3.1 RRTConCon 算法

RRTConCon算法是RRT算法的一種改進(jìn)。RRT算法的核心思想是通過隨機(jī)采樣使搜索樹G（NG，EG）向C空間中的未探索區(qū)域逐步擴(kuò)展，以實現(xiàn)高維狀態(tài)空間中的快速路徑規(guī)劃。算法開始時，xinit形成樹的根節(jié)點。每一次循環(huán)中，利用Random_State函數(shù)從C空間X中選擇一個隨機(jī)點xrandom∈X，同時按照給定的距離度量函數(shù)ρ，利用Nearest_Neighbor函數(shù)從G中選擇最近的節(jié)點xnear∈NG。然后利用Control函數(shù)選擇最佳輸入點來對最近點xnear進(jìn)行擴(kuò)展。Control函數(shù)執(zhí)行時首先以xnear為基礎(chǔ)，選擇輸入u∈U，利用增量仿真函數(shù)產(chǎn)生一個新的位姿點xnew∈Xfree。如果xnear至xnew的路徑滿足全局約束，并且xnew和xrandom之間的距離在所有由U產(chǎn)生的狀態(tài)中最小，則將xnew加入到RRT搜索樹中。當(dāng)xnew和xgoal之間的距離滿足事先約定的最小距離時，便認(rèn)為已經(jīng)找到節(jié)點，搜索成功。

RRTConCon算法對RRT算法的改進(jìn)主要有兩個。一個是RRTConCon算法分別從初始位姿點和目標(biāo)位姿點各產(chǎn)生一棵樹，只要兩棵樹之間的距離滿足事先約定的最小距離，就認(rèn)為搜索成功。而RRT算法只是從初始位姿點產(chǎn)生一顆樹，因此RRTConCon算法更容易找到路徑。另一個改進(jìn)之處在于擴(kuò)展函數(shù)的不同，RRT算法的擴(kuò)展函數(shù)為Extend函數(shù)，而RRTConCon算法的擴(kuò)展函數(shù)為Connect函數(shù)。兩者的不同之處在于：當(dāng)xnear至xnew的路徑滿足全局約束，并且xnew和xrandom之間的距離在所有由U產(chǎn)生的狀態(tài)中最小時，Extend函數(shù)會將xnew加入到RRT搜索樹中，然后再繼續(xù)產(chǎn)生隨機(jī)點。而Connect函數(shù)不會將xnew加入到RRT搜索樹中，而是在xnew和xrandom之間繼續(xù)產(chǎn)生新的x′new，直到x′new不再滿足條件，便再產(chǎn)生下一個隨機(jī)點。與Extend函數(shù)相比，Connect函數(shù)的優(yōu)點在于它可以減少采樣點的個數(shù)，縮短采樣時間。但是在狹窄通道或者大的開闊區(qū)域，所需的采樣的數(shù)量是不同的，因此需要用到下面的高斯采樣。

3.2 高斯采樣

一個合理的采樣點的集合應(yīng)該這樣分布：在大的開放區(qū)域有較少的采樣點，而在靠近障礙物的復(fù)雜區(qū)域或者狹窄通道則有大量的采樣點，因此，一個采樣點加入樹中的概率由它附近的禁止點的數(shù)量決定。為了得到這樣的分布，這里用到了高斯密度函數(shù)。

定義一個n維的高斯密度函數(shù)：

式中，σ為高斯函數(shù)的規(guī)模（或者寬度）。為了在復(fù)雜區(qū)域或者狹窄通道設(shè)置較多的采樣點，定義障礙函數(shù)Obs（c）在與障礙物碰撞時為1，其它為0。并定義：

那么f（c；σ）即為C空間中任意一點被采樣的概率。顯然，采樣點附近障礙物越多，f（c；σ）取值便越大。為了避免禁止點，我們定義：

也就是說，在障礙物內(nèi)，g（c；σ）＝0，其它情況下 g（c；σ）＝f（c；σ）。 g（c；σ）就是我們想要的概率分布。

根據(jù)上述分布，設(shè)計一個基于高斯分布的算法以獲得采樣點的集合，算法的偽代碼如下：

loop

1.c1←產(chǎn)生一個隨機(jī)點

2.m←產(chǎn)生一個滿足正態(tài)分布的隨機(jī)數(shù)

3.d←將m變成一個距離（如果m為負(fù)值，將它變?yōu)檎担?/p>

4.c2←在距離c1為d的點中隨機(jī)選取一個點

5.如果 c1∈Cfree且 c2?Cfree那么

6.將 c1加到樹中

7.如果 c2∈Cfree且 c1?Cfree那么

8.將 c2加到樹中

9.否則

10.丟棄兩個位姿點

根據(jù)高斯分布，如果產(chǎn)生了一個禁止位姿點和一個自由位姿點，則只增加一個自由位姿點到樹中。這種采樣方法稱為高斯采樣，因為它是根據(jù)分布g（c；σ）產(chǎn)生的一個采樣集。

將高斯采樣與RRTConCon算法結(jié)合便產(chǎn)生了一種新的算法——RRTGaussion算法。該算法和RRTConCon算法一樣，在概率上是完備的，也就是說，如果在通道中存在一條路徑，當(dāng)采樣點足夠多時，找到可行路徑的概率為1。但RRTGaussion算法同其它基于采樣的路徑規(guī)劃算法一樣，都不能保證其搜索結(jié)果為最優(yōu)。

有一個參數(shù)發(fā)揮了重要作用：高斯分布的方差σ2，其對應(yīng)于高斯采樣的規(guī)模σ。根據(jù)高斯分布，選擇的方差越小，靠近障礙物的位姿點就越多。反之，選擇的方差越大，均勻分散在自由空間的位姿點就越多。

在三維環(huán)境中，當(dāng)LRU帶有旋轉(zhuǎn)和平移的自由度時，選擇的方差應(yīng)該使大部分位姿點落在復(fù)雜區(qū)域或者狹窄通道內(nèi)，但是允許LRU繞其旋轉(zhuǎn)軸旋轉(zhuǎn)，而不與障礙物發(fā)生碰撞。另外，旋轉(zhuǎn)自由度的處理方式要與平移自由度不同。根據(jù)距離d，改變第2個采樣點的平移自由度，而旋轉(zhuǎn)自由度則可任取。

4 算法實例與討論

4.1 Flange模型裝配拆卸仿真分析

Flange模型由GE公司的研究與發(fā)展中心提供，是用于檢驗維修性設(shè)計的標(biāo)準(zhǔn)模型。它是將圖1b中的LRU模型移入圖1a的裝配拆卸環(huán)境模型中（圖1c），屬狹窄通道的路徑規(guī)劃問題。下面將用Flange模型來比較RRTConCon算法與RRTGaussion算法的優(yōu)劣。

Flange模型經(jīng)處理后轉(zhuǎn)變?yōu)槎噙呅文Ｐ汀Ｑb配拆卸環(huán)境模型含990個多邊形，LRU模型含5 306個多邊形，仿真程序采用C＋＋語言實現(xiàn)，圖形的渲染采用OpenGL函數(shù)庫，碰撞檢測函數(shù)庫采用PQP算法庫。所有的調(diào)試都在一個4核、8 GB內(nèi)存的HPXW8600工作站上進(jìn)行。

為避免隨機(jī)性，將程序運行100次，取其平均值，得到如表1、表2所示的仿真結(jié)果。

表1 Flange模型RRTConCon算法仿真結(jié)果Tab.1 RRTConCon simulation results of Flange model

表2 RRTGaussion算法仿真結(jié)果Tab.2 RRTGaussion simulation restuls of Flange model

從表1和表2的仿真結(jié)果可知：

1）RRTConCon算法和RRTGaussion算法是概率完備的，但是可以看到都有失敗。這是因為在算法中，設(shè)定如果在5 000個采樣點之內(nèi)找不到路徑，即認(rèn)為規(guī)劃失敗。在失敗次數(shù)方面，RRTGaussion算法比RRTConCon算法要少，這說明RRTGaussion算法比RRTConCon算法更容易找到路徑。

2）在采樣點的數(shù)量上，RRTGaussion算法要比RRTConCon算法少一些。這是因為采用高斯采樣后使采樣點的分布更加合理，所以RRTGaussion算法只用較少的采樣點就可以規(guī)劃出路徑。碰撞檢測次數(shù)的變化規(guī)律與采樣點數(shù)量的變化規(guī)律是一致的。

3）在規(guī)劃時間上，總體而言，RRTGaussion算法要比RRTConCon算法少15%左右。這是因為需要的采樣點減少了，碰撞檢測次數(shù)也減少了，相應(yīng)地，規(guī)劃時間也就減少了。

綜合來說，RRTGaussion算法提高了Flange模型路徑規(guī)劃的效率，是一種比較有效的算法。

4.2 船舶機(jī)艙齒輪泵裝配拆卸仿真分析

下面選取某型船機(jī)艙的一部分作為裝配拆卸環(huán)境模型，某型齒輪泵作為LRU模型（圖2），對RRTGaussion和RRTConCon算法的效率進(jìn)行仿真對比分析。其裝配拆卸路徑圖如圖3所示。

船舶機(jī)艙模型和齒輪泵模型都是采用法國達(dá)索公司的Catia軟件構(gòu)造，經(jīng)過處理后轉(zhuǎn)變?yōu)槎噙呅文Ｐ?。機(jī)艙含多邊形203 024個，齒輪泵含多邊形2 536個，采用的仿真程序、函數(shù)庫等與Flange模型一致。所有的調(diào)試都在一個4核、8 GB內(nèi)存的HPXW6600工作站上進(jìn)行。

同F(xiàn)lange模型測試一樣，為避免隨機(jī)性，將程序運行50次，取其平均值，得到了如表3和表4所示的結(jié)果。

表3 船舶機(jī)艙環(huán)境下RRTConCon算法仿真結(jié)果Tab.3 RRTConCon simulation results for ship engine room

表4 船舶機(jī)艙環(huán)境下RRTGaussion算法仿真結(jié)果Tab.4 RRTGaussion simulation results for ship engine room

由表3和表4的仿真結(jié)果可知：

1）在這個模型中，設(shè)定如果在1 000個采樣點之內(nèi)找不到路徑，即認(rèn)為規(guī)劃失敗。比較表3和表4可以看出，在算法失敗次數(shù)方面，RRTConCon算法要比RRTGaussion算法少。

2）在采樣點數(shù)量上的變化規(guī)律和碰撞檢測次數(shù)的變化規(guī)律與Flange模型分析結(jié)果類似。

3）在規(guī)劃時間上，總體而言，RRTGaussion算法要比RRTConCon算法少10%左右。

5 結(jié) 論

實驗結(jié)果表明，高斯采樣改進(jìn)了基于隨機(jī)采樣的RRTConCon算法，提高了規(guī)劃效率，但仍有幾個問題需要解決：

1）在產(chǎn)生隨機(jī)點的過程中，丟棄了很多采樣點，實際上這些采樣點本身就包含了C空間的一些信息。因此，如果在高斯采樣的同時將這些點回收利用，將能進(jìn)一步提高算法的效率。

2）RRTGaussion算法在檢驗?zāi)Ｐ汀狥lange模型中使得失敗次數(shù)減少，但在船舶機(jī)艙測試環(huán)境中又使得失敗次數(shù)增加，該問題還有待于進(jìn)一步的研究。

［1］CHANG H，LI T-Y.Assembly maintainability study with motion planning ［C］／／Proceeding of IEEE International Conference on Robotics and Automation.Nagoya： IEEE Computer Society Press，1995：1012-1017.

［2］GARBER M，LIN，M.Constraint-based motion planning for virtual prototyping ［C］／／Proceeding of ACM Symposium on Solid Model and Application，Saarbrucken，Germany，2002.

［3］ZHANG L，HUANG X，KIM Y.D-Plan：Efficient collision-free path computation for part removal and sisassembly［J］.Journal of Computer-Aided Design and Applications，2008：774-786.

［4］LOZANO-PEREZ T，WESLEY M A.An algorithm for planning collision-free paths among polyhedral obstacles［J］.Communication of the ACM，1979，22（10）：560-570.

［5］BARRAQUAND J，LATOMBE J C.A monte-carlo algorithm for path planning with many degrees of freedom［C］//Proceeding of IEEE International Conference on Robotics and Automation，Cincinnati：IEEE Computer Society Press，1990：1712-1717.

［6］KAVRAKI L，LATOMBE J.Randomized preprocessing of configuration space for fast path planning ［C］//Proceeding of IEEE International Conference on Robotics and Automation，SanDiego： IEEE Computer Society Press，1994：2138-2139.

［7］LAVALLE S M.Rapidly-exploring random trees： a new tool for path planning ［R］.Technical Report TR98-11，Computer Science Department，Iowa State University，1998.

［8］劉華軍，楊靜宇，陸建峰.移動機(jī)器人運動規(guī)劃研究綜述［J］.中國工程科學(xué)，2006，8（1）：85-94.

［9］LAVALLE S M，KUFFNER J J.Rapidly-exploring random trees： progress and prospects ［M］.Algorithmic and Computational Robotics：New Directions.DONALD BR，LYNCH K M，RUS D，Eds.Massachu setls：Wellesley，2001：293-308.

［10］唐華斌，孫增圻.結(jié)合啟發(fā)式函數(shù)的隨機(jī)運動規(guī)劃方法［J］.清華大學(xué)學(xué)報（自然科學(xué)版），2006，46（4）：580-583.