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

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

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

?

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

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

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

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

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

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

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

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

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

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

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

1.1 凸多面體表示

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

(1)

1.2 剛體表示

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

(2)

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

1.3 樹型結構物體表示

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

(3)

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

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

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

圖2 機械臂的樹型結構物體表示Fig.2 Tree object representation of the manipulator

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

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

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

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

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

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

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

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

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

1.4.3 樹型結構物體中的凸多面體坐標變換

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

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

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

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

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

2 機械臂的碰撞檢測算法

2.1 樹型結構物體的層次包圍盒

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

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

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

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

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

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

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

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

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

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

3.1 路徑編碼設計

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

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

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

3.2 路徑解碼設計

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

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

3.3 適應度函數(shù)設計

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

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

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

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

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

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

f3(θ)=collsionTimes

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

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

3.4 算法步驟

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

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

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

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

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

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

圖11 抓取物件避碰作業(yè)任務時仿真過程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ī)劃，保證了機械臂順利地避開復雜障礙并到達期望位姿。

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

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

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

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

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

3 結論

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

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

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

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

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

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

[2]王田苗, 陶永. 我國工業(yè)機器人技術現(xiàn)狀與產業(yè)化發(fā)展戰(zhàn)略[J]. 機械工程學報, 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]. 機械工程學報, 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)仿真學報, 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.

國家自然科學基金項目(61104037, 61304060); 中央高?；A業(yè)務費項目(HEUCF041307, HEUCFX41304); 國家科技合作專項項目(2013DFR10030).

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

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

10.11990/jheu.201505048

網絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20160914.1426.004.html

TP24

A

1006-7043(2016)10-1373-08

張智, 鄒盛濤, 董然，等. 復雜環(huán)境建模與機器人避障規(guī)劃研究[J]. 哈爾濱工程大學學報， 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.