復(fù)雜環(huán)境建模與機器人避障規(guī)劃研究

張智, 鄒盛濤, 董然, 張樂樂, 郭文縣

(哈爾濱工程大學(xué) 自動化學(xué)院，黑龍江 哈爾濱 150001)

?

復(fù)雜環(huán)境建模與機器人避障規(guī)劃研究

張智, 鄒盛濤, 董然, 張樂樂, 郭文縣

(哈爾濱工程大學(xué) 自動化學(xué)院，黑龍江 哈爾濱 150001)

針對復(fù)雜環(huán)境下的機械臂作業(yè)問題，提出了一種基于空間凸多面體集模型的避碰路徑規(guī)劃方法。首先，給出一種利用空間凸多面體模型的復(fù)雜物體建模方法，該方法具有一套樹型遞歸結(jié)構(gòu)框架，能精確逼近任意形狀的物體，并支持子物體之間的自由度連接，從而能恰當表示如多關(guān)節(jié)機械臂和電腦桌等帶有運動自由度(關(guān)節(jié)轉(zhuǎn)動、柜門開合、抽屜拉出等)的物體；然后，設(shè)計了一種基于直線掃掠體序列的碰撞檢測方法，它能快速、準確地進行連續(xù)運動的機械臂與空間物體間的碰撞檢測；最后，采用遺傳算法實現(xiàn)了機械臂的避碰路徑規(guī)劃。在桌面上擺放多個復(fù)雜形狀物體后，機械臂順利完成了避障作業(yè)規(guī)劃。仿真結(jié)果驗證了算法的有效性，表明了其實用價值。

機械臂;凸多面體;碰撞檢測;遺傳算法;避碰路徑規(guī)劃

機器人是當代科技發(fā)展的重要成果，代表了機電一體化和自動化技術(shù)的最高成就，其中工業(yè)機器人(機械臂)已經(jīng)發(fā)展為現(xiàn)代高端制造業(yè)的支撐技術(shù)，在材料焊接、噴涂、物品搬運、物料切割等多個行業(yè)得到了廣泛的應(yīng)用[1-2]。

當機器人應(yīng)用于社會化的環(huán)境時，其面臨的環(huán)境理解及作業(yè)規(guī)劃問題要遠比工業(yè)機器人復(fù)雜[3-4]，真實環(huán)境下機器人和環(huán)境物體往往形狀復(fù)雜并且位姿隨時間動態(tài)變化，沒有固定規(guī)律，而且場景中物體數(shù)量極多，物體的擺放相對自由。因此，如何對復(fù)雜環(huán)境物體進行建模，并針對性地設(shè)計機器人避障作業(yè)規(guī)劃算法，就成為了機器人社會化過程的一個關(guān)鍵問題。Hasan[5]將機械臂用軸線段表示，而將障礙物用圓柱包圍盒和橢球包圍盒表示，在避碰規(guī)劃方面利用人工神經(jīng)網(wǎng)絡(luò)算法實現(xiàn)避碰，但是該方法需要對神經(jīng)網(wǎng)絡(luò)進行大量訓(xùn)練，難以適應(yīng)動態(tài)環(huán)境；祁若龍等[6]將機械臂的關(guān)節(jié)簡化為軸線段，將障礙物簡化為球體，通過增加球體的數(shù)量來提高模型的精度，在避碰規(guī)劃方法將路徑分段描述，避開一個障礙需要增加一個中間點，將未知的中間點利用遺傳算法優(yōu)化實現(xiàn)避碰。賈慶軒等[7]將環(huán)境障礙物利用球體包圍盒進行近似建模，通過障礙物與機械臂的碰撞條件建立自由空間，進而采用A*算法在自由空間中對避碰路徑進行搜索。

總的來說，國內(nèi)外的相關(guān)研究主要集中在機械臂避碰路徑規(guī)劃方法的研究：遺傳算法、神經(jīng)網(wǎng)絡(luò)法、A*算法等，而對于環(huán)境建模主要采用簡單規(guī)則物體對形狀進行近似，但是這種方法極大浪費了機器人避碰規(guī)劃的可用空間。

本文算法力求適用于任意復(fù)雜環(huán)境，為了準確的對復(fù)雜環(huán)境物體進行建模，本文提出一種基于樹型凸多面體集的物體描述方法，該方法將多個凸多面體組合近似為單個剛體，然后利用多個剛體組合成單個子物體，再利用多個子物體遞歸組合成為樹型結(jié)構(gòu)物體，該方法支持各級之間(物體與子物體、子物體(或剛體)與凸多面體)的相對自由度連接。為了有效實現(xiàn)機械臂連續(xù)運動情況下的避碰運動規(guī)劃，提出對機械臂的連續(xù)路徑進行采樣，并在相鄰采樣點之間構(gòu)造近似軌跡包圍盒，利用遺傳算法建立了機械臂避碰規(guī)劃問題的適應(yīng)度函數(shù)，采用關(guān)節(jié)空間內(nèi)若干關(guān)鍵點對避碰路徑進行編碼，最后通過仿真和實驗對本文提出的復(fù)雜環(huán)境建模方法以及避碰方法進行了驗證。

1 復(fù)雜環(huán)境物體建模技術(shù)

環(huán)境建模應(yīng)該滿足以下兩點：一是環(huán)境應(yīng)該由形狀緊密的包圍盒構(gòu)建；二是需要采用一種有效的數(shù)據(jù)結(jié)構(gòu)對其進行管理，以便描述物體間的邏輯關(guān)系，并且支持自由度變換，方便物體的坐標變換。本節(jié)將提出一種基于樹型凸多面體集的物體描述方法，并將采用這種方法描述的物體稱為樹型結(jié)構(gòu)物體。樹型結(jié)構(gòu)物體分為物體、子物體和凸多面體三級，下面分別從這三個層次依次介紹。

1.1 凸多面體表示

將凸多面體表示為點、面、棱3種元素的集合：

(1)

1.2 剛體表示

剛體用來表示機械臂或其他物體的子部件，內(nèi)部不帶有任何相對運動自由度，如機械臂的底座或某個關(guān)節(jié)、桌子的抽屜、桌體框架等，每個剛體由于形狀復(fù)雜，難以用一個凸多面體精確逼近，因此采用凸多面體集合表示：

(2)

圖1 剛體的凸多面體集表示Fig.1 Polytopes-representation of rigid body

1.3 樹型結(jié)構(gòu)物體表示

物體的表示采用樹型結(jié)構(gòu)，即每個物體可由若干個子物體構(gòu)成，每個子物體又可包含若干子物體，依次遞歸表示，直到葉子節(jié)點，而各葉子節(jié)點均為剛體，物體表示如下：

(3)

1.4 機械臂工作環(huán)境物體的樹型結(jié)構(gòu)表示

基于1.3節(jié)給出的物體表示模型，本節(jié)將舉例給出機械臂及電腦桌的表示方法。

1.4.1 機械臂的凸多面體集表示

圖2 機械臂的樹型結(jié)構(gòu)物體表示Fig.2 Tree object representation of the manipulator

同樣，W1～W6子物體所包含的Dof(Γi,ηi)分別用來描述各關(guān)節(jié)的轉(zhuǎn)動關(guān)系，由表1中的D-H參數(shù)確定，例如W1，取Γ1=3，表示繞z軸旋轉(zhuǎn)(見1.3節(jié)定義)，η1動態(tài)跟隨關(guān)節(jié)1轉(zhuǎn)角θ1變化。

在完成各層子物體和剛體的分解后，還需按照1.2節(jié)的描述，將每一級剛體(即每個關(guān)節(jié))形狀分解為若干個凸多面體的組合，分解完成后，即實現(xiàn)了對Reinovo型機械臂的空間凸多面體集描述，最終結(jié)果如圖3所示。

表1 機械臂的D-H參數(shù)表

圖3 機械臂的凸多面體集表示Fig.3 Polytopes-representation of the manipulator

1.4.2 電腦桌的凸多面體集表示

采用1.1～1.3節(jié)描述的樹型物體表示方法，對作業(yè)環(huán)境物體中典型的物體電腦桌進行描述，子物體結(jié)構(gòu)圖如圖4所示。圖中的左柜門系統(tǒng)W3、左抽屜系統(tǒng)W4、右抽屜系統(tǒng)W5是相對于上級物體具有運動自由度的，分別代表了柜門的旋轉(zhuǎn)和抽屜的拉出。

圖4 桌子的樹型物體表示Fig.4 Tree object representation of the desk

將桌子的子剛體G0～G6中各子剛體繼續(xù)分解為若干個凸多面體(分解過程略)后，即可得到桌子的凸多面體集表示，最終結(jié)果如圖5所示。

圖5 桌子的凸多面體集表示Fig.5 Polytopes-representation of the desk

1.4.3 樹型結(jié)構(gòu)物體中的凸多面體坐標變換

前面各節(jié)給出了基于空間凸多面體集的物體形狀描述，以及基于樹型結(jié)構(gòu)的多自由度物體表示方法，為了實現(xiàn)任意物體間的碰撞檢測，在此需給出坐標變換方法，將某樹型物體中所有子剛體中的所有子凸多面體變換至世界系表示。

算法1 物體的子凸多面體坐標變換

3)在完成當前首節(jié)點所有子物體遍歷后，算法結(jié)束。

算法1構(gòu)成了遞歸調(diào)用模式，算法啟動后會自動遍歷樹型結(jié)構(gòu)的物體，并對葉子節(jié)點的子剛體，自動遍歷其下屬的子凸多面體，直到完成所有凸多面體的變換。

需說明的是，建模過程樹型結(jié)構(gòu)的組織，凸多面體劃分等過程目前是人工完成的，也可將本文算法與凸多面體自動分割等算法(本文不討論)相結(jié)合，來進一步提高建模效率。

2 機械臂的碰撞檢測算法

2.1 樹型結(jié)構(gòu)物體的層次包圍盒

為了提高碰撞檢測的執(zhí)行效率，快速排除不可能發(fā)生碰撞的物體，一種有效的方法就是層次包圍體法[8]。第1章介紹樹狀結(jié)構(gòu)物體的凸多面體坐標變化，然后利用變化后的凸多面體分別構(gòu)造包含

物體層、剛體層和凸多面體層的層次包圍盒。本文采用自底向上的方法構(gòu)造層次包圍盒。

1)底層包圍盒：本文利用樹型結(jié)構(gòu)物體的描述方法對機械臂工作場景中所有的物體進行描述，由于樹狀結(jié)構(gòu)物體均由凸多面體集構(gòu)成，因此底層凸多面體即是底層包圍盒。

2)中層包圍盒：包圍體層次樹的中層對應(yīng)樹型結(jié)構(gòu)物體中的剛體，對于機械臂而言，中層剛體代表關(guān)節(jié)，對于桌子而言，中層剛體代表抽屜、柜門等部分。剛體由多個凸多面體組成(如關(guān)節(jié)由多個凸多面體近似)，依次遍歷剛體中的每一個凸多面體，將得到實時的世界坐標系下凸多面體頂點數(shù)據(jù)，采用AABB構(gòu)造中層包圍盒。

3)頂層包圍盒：包圍體層次樹的頂層對應(yīng)樹型結(jié)構(gòu)物體中的物體，對于機械臂而言，頂層物體對應(yīng)機械臂本身，對于桌子而言，頂層物體對應(yīng)桌子，遍歷物體中剛體，并遍歷每一個剛體的AABB包圍盒，可得到所有AABB在世界坐標系下的頂點坐標，采用AABB包圍盒構(gòu)造頂層包圍盒。

2.2 機械臂連續(xù)運動碰撞檢測方法

針對機械臂離散時間碰撞檢測容易導(dǎo)致檢測遺漏等不精確問題，本文的解決方法提出對機械臂的連續(xù)路徑進行采樣，并在相鄰采樣點之間構(gòu)造近似軌跡包圍盒[9]，其中樹型結(jié)構(gòu)物體中單個凸多面體的軌跡包圍盒構(gòu)造示意圖如圖6所示。

圖6 凸多面體的軌跡包圍盒Fig.6 Path swept volumes of the polytopes

圖7 機械臂的碰撞檢測算法Fig.7 Collision detection of the manipulator

3 機械臂遺傳避碰路徑規(guī)劃

3.1 路徑編碼設(shè)計

由于關(guān)節(jié)空間不存在機械臂奇異性問題，因此，本文機械臂路徑編碼將在關(guān)節(jié)空間進行，具體方法是在關(guān)節(jié)空間中選取N組關(guān)節(jié)角度序列作為機械臂連續(xù)路徑運動的中間點，每個路徑點將由六個關(guān)節(jié)轉(zhuǎn)角構(gòu)成的六維矢量表示，最終在遺傳算法中采用的染色體的編碼方式為

如圖8，染色體編碼中每一組角度矢量，均對應(yīng)著一種機械臂空間位姿(中間點)，將若干個空間位姿點通過插值算法連接，便可形成空間連續(xù)運動路徑，當搜索過程不斷調(diào)整中間點的位置時，即可得到有效的避碰路徑。中間點個數(shù)N應(yīng)選取適當，N過大會導(dǎo)致搜索空間太大，算法收斂速度變慢，N太小將無法找到可行解，一般應(yīng)根據(jù)規(guī)劃任務(wù)的復(fù)雜性及規(guī)劃實時性要求折中選取，本文取N=3，插值方法為關(guān)節(jié)空間線性插值，由于每個中間點均在6維空間變化，這樣編碼后仍能保證機械臂具有較強的避碰規(guī)劃能力。

圖8 機械臂路徑點示意圖Fig.8 Path points of the manipulator

3.2 路徑解碼設(shè)計

路徑解碼就是根據(jù)中間點還原一條連續(xù)運動路徑，并且保證路徑的唯一。路徑設(shè)定的開始點、結(jié)束點以及N個中間點構(gòu)成N+2組六維位姿矢量，我們需要依次連接相鄰的兩個矢量構(gòu)成子路徑片段，最終連接各子路徑還原整個路徑。針對任意給定的兩個位姿，本文將推導(dǎo)出路徑片段的還原方法。

式中wj.max為每個關(guān)節(jié)允許轉(zhuǎn)動的最大角速度。

3.3 適應(yīng)度函數(shù)設(shè)計

適應(yīng)度函數(shù)綜合考慮機械臂運動過程的時間、能量、碰撞、限位等約束，對機械臂定義適應(yīng)度函數(shù)表示為

適應(yīng)度函數(shù)是充分考慮機械臂運動過程中的各種指標后加權(quán)得到的，wi(i=1,2,3,4)是各項指標對應(yīng)的加權(quán)系數(shù)。

f1(θ)是機械臂沿路徑θ(t)運動的時間，對于N個中間路徑點的機械臂，其運動時間為

f2(θ)表征了機械臂沿路徑θ(t)由起點經(jīng)過中間點到終點所消耗的能量，對于N個中間路徑點的六自由度機械臂，其能量[6]為

式中：αj是機械臂各關(guān)節(jié)變化的權(quán)重，反映了機械臂各關(guān)節(jié)運動代價的比例關(guān)系。

f3(θ)表示機械臂沿路徑θ(t)運動時與自身或環(huán)境障礙發(fā)生碰撞程度。具體碰撞檢測時需對機械臂連續(xù)路徑進行離散采樣，對每個采樣時刻對應(yīng)的關(guān)節(jié)狀態(tài)分別作碰撞檢測，并以發(fā)生碰撞的采樣時刻數(shù)量collsionTimes來表示碰撞程度：

f3(θ)=collsionTimes

f4(θ)代表機械臂沿路徑θ(t)運動時關(guān)節(jié)角位移限制，對于N個中間路徑點的六自由度機械臂，其代價為

式中：θj.min、θj.max代表關(guān)節(jié)最小和最大限位，fabs(θj.m-θj.max)

3.4 算法步驟

綜合以上算法，總結(jié)機械臂遺傳避碰路徑規(guī)劃算法的流程圖如圖9所示。

圖9 機械臂路徑規(guī)劃方法流程圖Fig.9 The path planning procedure of the manipulator

4 機械臂的避碰路徑規(guī)劃仿真

為了考察本文算法在實際生活中的應(yīng)用情況，利用VC++開發(fā)機械臂避碰作業(yè)仿真系統(tǒng)，模擬實物機械臂工作情況，本文仿真實驗均在IntelXeonCPU2.4GHz, 2GB內(nèi)存計算機上完成。為了驗證本文相關(guān)算法，本文設(shè)計了機械臂抓取物件時的避碰實驗，任務(wù)描述如圖10所示。

進化過程中，根據(jù)本文的適應(yīng)度函數(shù)對機械臂的避碰路徑進行評價，在遺傳算子的作用下種群逐漸趨于穩(wěn)定，算法迭代30次，適應(yīng)度函數(shù)的收斂曲線如圖12所示。

圖10 機械臂避障任務(wù)描述Fig.10 Task description of the manipulator

圖11 抓取物件避碰作業(yè)任務(wù)時仿真過程Fig.11 The path planning results of the simulating manipulator

圖12 最優(yōu)解收斂曲線Fig.12 The convergent curve of optimal solution

從圖11和圖12可以看出，利用本文基于遺傳算法的機械臂避碰路徑規(guī)劃方法順利地實現(xiàn)了機械臂的避碰路徑規(guī)劃，保證了機械臂順利地避開復(fù)雜障礙并到達期望位姿。

本文將仿真系統(tǒng)生成的角度序列(一般在70組左右)發(fā)送到實物機械臂，實現(xiàn)了實物機械臂抓取物件時的避碰路徑規(guī)劃，實物機械臂避碰過程如圖13所示。

圖13 避碰作業(yè)任務(wù)時實物運行過程Fig.13 The path planning results of the real manipulator

為驗證算法的統(tǒng)計性能，在仿真系統(tǒng)中進行多次規(guī)劃仿真試驗，試驗分6組，任務(wù)設(shè)定均與前文相同，但每組對應(yīng)不同的物體擺放位置，對每組各進行20次仿真，各組仿真的成功率、迭代次數(shù)及平均時間如表2所示。

表2 機械臂的數(shù)據(jù)統(tǒng)計

通過實驗觀測可知，實物機械臂依據(jù)算法規(guī)劃的軌跡，成功地完成了抓取物件的避碰作業(yè)任務(wù)。需說明的是，目前實物場景中物體的位置為人工測量并輸入到仿真系統(tǒng)，如要提高系統(tǒng)自主規(guī)劃能力，需將本文方法與視覺目標識別和定位等技術(shù)進一步結(jié)合方可。另外，本文對所述方法進行連續(xù)20次運行，實驗過程中仿真機械臂全部避開障礙，穩(wěn)定可靠且計算效率較高。

3 結(jié)論

本文針對機械臂在復(fù)雜環(huán)境中的作業(yè)問題，從環(huán)境建模、碰撞檢測和避碰規(guī)劃方法等方面進行了深入、系統(tǒng)地研究，得到如下結(jié)論：

1)基于樹型凸多面體集的物體建模方法，是一種適用性較強并且頗具研究潛力的空間環(huán)境建模方法；

2)基于該方法，構(gòu)造了樹型結(jié)構(gòu)物體的層次包圍盒模型，實現(xiàn)了機械臂連續(xù)運動情況下的碰撞檢測算法，有效地實現(xiàn)了在放有各類復(fù)雜形狀物體的桌面上的機械臂作業(yè)路徑規(guī)劃；

3)隨著后續(xù)研究工作深入，本文環(huán)境建模方法將在機械臂的復(fù)雜環(huán)境作業(yè)方面發(fā)揮更大的作用。

[1]蔡自興. 機器人學(xué)基礎(chǔ)[M]. 北京: 機械工業(yè)出版社, 2009: 163-166.

CAI Zixing. Fundamentals of Robotics[M]. Beijing: China Machine Press, 2009: 163-166.

[2]王田苗, 陶永. 我國工業(yè)機器人技術(shù)現(xiàn)狀與產(chǎn)業(yè)化發(fā)展戰(zhàn)略[J]. 機械工程學(xué)報, 2014, 50(9): 1-13. WANG Tianmiao, TAO Yong. Research status and industrialization development strategy of Chinese industrial robot[J]. Journal of mechanical engineering, 2014, 50(9): 1-13.

[3]SEKMEN A, CHALLA P. Assessment of adaptive human-robot interactions[J]. Knowledge-based systems, 2013, 42: 49-59.

[5]HASAN A T, ISMAIL N, HAMOUDA A M S, et al. Artificial neural network-based kinematics jacobian solution for serial manipulator passing through singular configurations[J]. Advances in engineering software, 2010, 41(2): 359-367.

[6]祁若龍, 周維佳, 王鐵軍. 一種基于遺傳算法的空間機械臂避障軌跡規(guī)劃方法[J]. 機器人, 2014, 36(3): 263-270. QI Ruolong, ZHOU Weijia, WANG Tiejun. An obatacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm[J]. Robot, 2014, 36(3): 263-270.

[7]賈慶軒, 陳鋼, 孫漢旭, 等. 基于A*算法的空間機械臂避障路徑規(guī)劃[J]. 機械工程學(xué)報, 2010, 46(13): 109-115.

JIA Qingxuan, CHEN Gang, SUN Hanxu, et al. Path planning for space manipulator to avoid obstacle based on A*algorithm[J]. Journal of mechanical engineering, 2010, 46(13): 109-115.

[8]馬登武, 葉文, 李瑛. 基于包圍盒的碰撞檢測算法綜述[J]. 系統(tǒng)仿真學(xué)報, 2006, 18(4): 1058-1061, 1064.

MA Dengwu, YE Wen, LI Ying. Survey of box-based algorithms for collision detection[J]. Journal of system simulation, 2006, 18(4): 1058-1061, 1064.

[9]ERICSON C. Real-time Collision Detection[M]. Singapore: CRC Press, 2004: 36-48.

[10]GILBERT E G, JOHNSON D W, KEERTHI S S. A fast procedure for computing the distance between complex objects in three-dimensional space[J]. IEEE journal on robotics and automation, 1988, 4(2): 193-203.

[11]張智, 鄒盛濤, 李佳桐, 等. 六自由度機械手三維可視化仿真研究[J]. 計算機仿真, 2015, 32(2): 374-377, 382.

ZHANG Zhi, ZOU Shengtao, LI Jiatong, et al. Three-dimensional visual simulation research on six degrees of freedom manipulator[J]. Computer simulation, 2015, 32(2): 374-377, 382.

Research on complex environmental modeling and robot operation planning

ZHANG Zhi, ZOU Shengtao, DONG Ran, ZHANG Lele, GUO Wenxian

(College of Automation, Harbin Engineering University, Harbin 150001, China)

For the operation of a manipulator in a complex environment, a collision avoidance path planning method on the basis of spatial convex polytope model was developed. First, a complex object modeling method utilizing spatial polytope model was proposed. The approach was characterized by a framework with a tree structure. It can precisely approximate the outline of an arbitrary object and support DOF linking among the affiliated bodies. Therefore, a moving objective with certain DOF (joint rotation, opening/closing of cabinet door, pull-out of drawer, etc.), such as a multi-joint robot arm and a computer table, can always be represented appropriately. Then, a collision detection method featuring a sequence of linear-swept volumes was designed. It permits efficient and accurate collision detections between the continuously moving manipulator and the environmental objectives involved. Finally, a collision avoidance path planning was realized by using the genetic algorithm. In simulations, the operational planning was implemented successfully for the case where some tasks are required to be finished on a desktop with multiple complex-shaped objects settled. Experimental results demonstrate the effectiveness and utility value of the proposed method.

manipulator; epolytopes; collision detection; genetic algorithm; collision avoidance path planning

2015-05-15.

日期：2016-09-14.

國家自然科學(xué)基金項目(61104037, 61304060); 中央高校基礎(chǔ)業(yè)務(wù)費項目(HEUCF041307, HEUCFX41304); 國家科技合作專項項目(2013DFR10030).

張智(1981-), 男, 副教授.

張智, E-mail： neverbadzz@163. com.

10.11990/jheu.201505048

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20160914.1426.004.html

TP24

A

1006-7043(2016)10-1373-08

張智, 鄒盛濤, 董然，等. 復(fù)雜環(huán)境建模與機器人避障規(guī)劃研究[J]. 哈爾濱工程大學(xué)學(xué)報， 2016, 37(10): 1373-1380.

ZHANG Zhi, ZOU Shengtao, DONG Ran, et al. Research on complex environmental modeling and robot operation planning[J]. Journal of Harbin Engineering University, 2016, 37(10): 1373-1380.