基于蒙特卡洛法的機(jī)器人無碰撞工作空間求解*

付　兵，胡飛飛，陳　琳，潘海鴻

(1.湖北汽車工業(yè)學(xué)院 機(jī)械工程學(xué)院 汽車動力傳動與電子控制湖北省重點(diǎn)實驗室，湖北 十堰　442002；2. 廣西大學(xué) 機(jī)械工程學(xué)院 廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實驗室，南寧　530004)

?

基于蒙特卡洛法的機(jī)器人無碰撞工作空間求解*

付兵1，胡飛飛1，陳琳2，潘海鴻2

(1.湖北汽車工業(yè)學(xué)院 機(jī)械工程學(xué)院 汽車動力傳動與電子控制湖北省重點(diǎn)實驗室，湖北 十堰442002；2. 廣西大學(xué) 機(jī)械工程學(xué)院 廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實驗室，南寧530004)

摘要：為研究末端執(zhí)行器對機(jī)器人實際工作空間的影響，以及機(jī)器人關(guān)節(jié)之間、末端執(zhí)行器與機(jī)器人關(guān)節(jié)之間的碰撞對機(jī)器人工作空間的影響，文章以6自由度工業(yè)機(jī)器人為研究對象，研究獲取帶末端執(zhí)行器的機(jī)器人無碰撞工作空間。提出采用蒙特卡洛法隨機(jī)生成大量機(jī)器人關(guān)節(jié)轉(zhuǎn)角值，并用這些關(guān)節(jié)轉(zhuǎn)角值更新機(jī)器人虛擬樣機(jī)的模型位置姿態(tài)，然后采取混合包圍體層次樹碰撞檢測算法判斷機(jī)器人自身關(guān)節(jié)及末端執(zhí)行器與機(jī)器人關(guān)節(jié)之間是否發(fā)生碰撞來分析機(jī)器人的無碰撞工作空間，獲取工作空間中的碰撞點(diǎn)，得到機(jī)器人的無碰撞工作空間。結(jié)果表明本文提出的方法能夠有效獲取帶末端執(zhí)行器的機(jī)器人無碰撞工作空間。

關(guān)鍵詞：工業(yè)機(jī)器人；蒙特卡洛法；工作空間；碰撞檢測

0引言

工業(yè)機(jī)器人廣泛應(yīng)用在裝配制造業(yè)，尤其在汽車制造行業(yè)中，具有廣闊的應(yīng)用前景。機(jī)器人工作空間是機(jī)器人末端在空間中所達(dá)到的位置集合，工作空間的大小代表機(jī)器人的活動范圍，是機(jī)器人的主要性能指標(biāo)之一，在機(jī)器人結(jié)構(gòu)設(shè)計[1]、 運(yùn)動學(xué)分析、 軌跡規(guī)劃及機(jī)器人工作站[2]設(shè)計中具有重要意義[3-4]。

目前，數(shù)值法[5]是獲取機(jī)器人工作空間最為常用的求解方法，其特點(diǎn)是：不受機(jī)器人關(guān)節(jié)數(shù)的限制，是以極值理論和優(yōu)化方法為基礎(chǔ)的，簡單且適用于工程問題。Yang X等[6]考慮機(jī)器人工作空間中的障礙物對機(jī)器人工作空間的影響，提出采用凸超二次曲面包絡(luò)障礙物和機(jī)器人末端執(zhí)行器，采用內(nèi)點(diǎn)算法計算機(jī)器人與障礙物之間的距離來判斷是否發(fā)生碰撞從而繪制機(jī)器人無碰撞工作空間。田海波等[7]分別采用蒙特卡洛法和定步距角步長法分析機(jī)械臂的工作空間，給出機(jī)械臂末端的工作空間點(diǎn)云圖，證實在分析機(jī)械臂的工作空間中，蒙特卡洛法優(yōu)于定步距角步長法。劉志忠等[8]在機(jī)器人工作空間研究分析中，基于蒙特卡洛法獲取機(jī)器人工作空間的邊界點(diǎn)，然后在每個邊界點(diǎn)對應(yīng)的關(guān)節(jié)變量值的足夠小鄰域內(nèi)生成一定數(shù)量的新的隨機(jī)關(guān)節(jié)變量值，從而得到一些新的工作空間隨機(jī)點(diǎn)，進(jìn)而獲取更加準(zhǔn)確的邊界點(diǎn)。李保豐等[9]采用蒙特卡洛法分析三自由度空間機(jī)器人的工作空間，得到機(jī)器人的工作空間。趙燕江等[10]提出基于Matlab的機(jī)器人工作空間求解方法仿真法，利用SimMechanics工具箱，根據(jù)機(jī)器人的結(jié)構(gòu)進(jìn)行建模，利用仿真的形式跟蹤手臂末端軌跡，并記錄其末端點(diǎn)到達(dá)的位置坐標(biāo)，繪制和計算出機(jī)器人工作空間。Abdel-Malek K等[11]在研究串聯(lián)型機(jī)械臂的邊界曲面時候采用二次曲面獲取機(jī)器人工作空間，并分析空間中的內(nèi)、外表面。張鵬程[3]提出采用包絡(luò)法獲取工作空間的包絡(luò)面方程。

從現(xiàn)有關(guān)于機(jī)器人工作空間的分析研究中，有學(xué)者研究機(jī)器人工作空間中的奇異點(diǎn)[12]，有研究除去障礙物的機(jī)器人無碰撞工作空間[6]，但目前掌握的文獻(xiàn)研究中并沒有考慮到：①安裝末端執(zhí)行器對機(jī)器人工作空間的影響；②機(jī)器人運(yùn)動中其關(guān)節(jié)之間以及與末端執(zhí)行器之間的碰撞問題對機(jī)器人工作空間的影響，忽略這兩點(diǎn)將導(dǎo)致：①不能獲得機(jī)器人真正的工作空間；②不能確保機(jī)器人自身安全。本文采用蒙特卡洛法和混合包體層次樹碰撞檢測算法，分析和獲取機(jī)器人無碰撞的工作空間，這對機(jī)器人空間布局優(yōu)化、末端執(zhí)行器的設(shè)計和選用以及確保機(jī)器人安全工作等均有重要意義。

1機(jī)器人工作空間計算

1.1機(jī)器人正運(yùn)動學(xué)

(1)

(2)

1.2蒙特卡洛法與正運(yùn)動學(xué)

機(jī)器人末端參考點(diǎn)可以在機(jī)器人坐標(biāo)系中以機(jī)器人關(guān)節(jié)轉(zhuǎn)角值為變量來表示：

P=(px,py,pz)=fkine(θ1,θ2,…,θN)

(3)

于是，機(jī)器人的工作空間可表示為：

(4)

蒙特卡洛法隨機(jī)生成機(jī)器人關(guān)節(jié)轉(zhuǎn)角值公式如公式(5)所示：

θni=θni,min+(θni,max-θni,min)×tni,tni∈[0,1]

(5)

注：i為機(jī)器人關(guān)節(jié)序號，i=1,2,3,4. . . ；n為隨機(jī)生成的關(guān)節(jié)變量組數(shù)。

2機(jī)器人無碰撞工作空間計算

在機(jī)器人各個關(guān)節(jié)的運(yùn)動范圍中取值時，存在兩種碰撞可能：①靠近末端的機(jī)器人關(guān)節(jié)在運(yùn)動中與靠近底座的關(guān)節(jié)發(fā)生碰撞；②機(jī)器人末端執(zhí)行器在運(yùn)動中與機(jī)器人關(guān)節(jié)發(fā)生碰撞。這兩點(diǎn)對機(jī)器人在工作空間的運(yùn)動安全均有影響，實際的機(jī)器人安全工作空間應(yīng)是在排除碰撞點(diǎn)之后工作區(qū)間。

為尋找機(jī)器人工作空間中的碰撞點(diǎn)，本文借鑒虛擬仿真技術(shù)。首先，在三維軟件(如Pro/E)建立機(jī)器人1：1的三維模型，并將存儲格式轉(zhuǎn)換為STL格式(由三角面片組成)；然后編程讀取模型文件中三角面片頂點(diǎn)信息，用讀取的模型三角面片頂點(diǎn)信息在基于OpenGL建立的虛擬環(huán)境中重繪機(jī)器人模型以建立機(jī)器人虛擬樣機(jī)；接著，用機(jī)器人關(guān)節(jié)轉(zhuǎn)角值來計算機(jī)器人模型當(dāng)前的位置姿態(tài)，并基于混合包圍體層次樹碰撞檢測算法[13]檢測該關(guān)節(jié)轉(zhuǎn)角下模型之間是否發(fā)生碰撞，將發(fā)生碰撞的機(jī)器人關(guān)節(jié)轉(zhuǎn)角值存儲起來；最后，通過機(jī)器人正運(yùn)動學(xué)方程將碰撞的關(guān)節(jié)轉(zhuǎn)角值映射到機(jī)器人坐標(biāo)系中得到取機(jī)器人工作空間中的碰撞點(diǎn)坐標(biāo)。

總體方案流程圖如下圖1所示，初始化中采用文獻(xiàn)[13]的方法建立單個機(jī)器人混合包圍體層次樹。首先，計算機(jī)器人各個關(guān)節(jié)模型以及末端執(zhí)行器模型的LSS包圍體，在LSS包圍體的基礎(chǔ)上構(gòu)建AABB包圍體作為頂層包圍體；然后，在各個關(guān)節(jié)模型及末端執(zhí)行器上建立OBB包圍體層次樹作為中間層，其葉子節(jié)點(diǎn)OBB包圍體中只包含一個三角面片，并由三角面片組成底層；最后，建立非相鄰關(guān)節(jié)模型碰撞檢測對的混合包圍體層次遍歷樹用于檢測機(jī)器人關(guān)節(jié)之間的碰撞，建立末端執(zhí)行器與機(jī)器人關(guān)節(jié)碰撞檢測對的混合包圍體層次遍歷樹用于檢測末端執(zhí)行器與機(jī)器人關(guān)節(jié)之間的碰撞。

圖1　機(jī)器人無碰撞工作空間算法流程圖

初始化完成之后采用方程式(5)隨機(jī)生成機(jī)器人關(guān)節(jié)轉(zhuǎn)角值序列，以該值計算機(jī)器人虛擬樣機(jī)中各個關(guān)節(jié)模型當(dāng)前的位置姿態(tài)(即三維設(shè)計時模型的參考坐標(biāo)系位置姿態(tài))。如圖1中所示，首先根據(jù)更新后的模型位置姿態(tài)來更新混合包圍體層次樹中包圍體信息；然后在建立的碰撞檢測對的混合包圍體層次遍歷樹中進(jìn)行包圍體相交測試，若父節(jié)點(diǎn)包圍體之間相交則對子節(jié)點(diǎn)包圍體進(jìn)行相交測試，直到相交檢測到混合包圍體層次樹底層中三角面片是否相交，若相交則認(rèn)為該碰撞檢測對對應(yīng)的模型間發(fā)生碰撞。如果發(fā)生碰撞則將轉(zhuǎn)角值序列存儲起來，通過正運(yùn)動學(xué)公式(2)計算得參考點(diǎn)位置坐標(biāo)值存儲到碰撞區(qū)域中；不碰撞則通過正運(yùn)動學(xué)公式(2)計算得的參考點(diǎn)位置坐標(biāo)值存儲到機(jī)器人工作空間中。重復(fù)上述過程直到存儲目標(biāo)坐標(biāo)點(diǎn)數(shù)達(dá)到設(shè)定點(diǎn)數(shù)。對得到的位置坐標(biāo)點(diǎn)進(jìn)行繪制得到機(jī)器人碰撞區(qū)域云圖和機(jī)器人工作空間云圖，機(jī)器人工作空間集減去碰撞區(qū)域后得到的剩余空間可表示為機(jī)器人實際無碰撞工作空間。

3實驗及結(jié)果分析

工業(yè)機(jī)器人選用松下TA1400機(jī)器人，繪制其三維模型并進(jìn)行簡化，然后基于OpenGL建立機(jī)器人TA1400的虛擬樣機(jī)如圖2所示。采用D-H參數(shù)坐標(biāo)法對其進(jìn)行運(yùn)動學(xué)建模，建模得到機(jī)器人D-H參數(shù)如表1所示。

圖2　TA1400型機(jī)器人D-H參數(shù)坐標(biāo)系

Jointθiαiaidi轉(zhuǎn)動范圍1θ1-90°1500±170°2θ20°5600-180°-+50°3θ3-90°1300-120-+90°4θ490°0600-180°-+180°5θ5-90°00-90°-+150°6θ60°00-180°-+180°

末端執(zhí)行器參考坐標(biāo)系在關(guān)節(jié)六坐標(biāo)系中的位置姿態(tài)矩陣為:

在基于OpenGL開發(fā)的多機(jī)器人仿真環(huán)境中，采用上述方法求得表示機(jī)器人工作空間和碰撞區(qū)域的隨機(jī)點(diǎn)坐標(biāo)，并進(jìn)行繪制得到表示機(jī)器人工作空間的圖3、圖4，其中圖3為機(jī)器人D-H坐標(biāo)系中關(guān)節(jié)六坐標(biāo)系原點(diǎn)P點(diǎn)的云圖，圖4為機(jī)器人末端執(zhí)行器參考坐標(biāo)系原點(diǎn)T點(diǎn)的云圖；以及表示機(jī)器人碰撞區(qū)域云圖5(末端執(zhí)行器與機(jī)器人關(guān)節(jié)之間碰撞)、6(機(jī)器人關(guān)節(jié)之間碰撞以及末端執(zhí)行器與機(jī)器人關(guān)節(jié)之間的碰撞)，圖5和圖6均為碰撞下T點(diǎn)的云圖。圖3～圖6中四個機(jī)器人相對位置是一樣的。

由圖3、圖4可知，安裝末端執(zhí)行器后機(jī)器人的工作空間得到擴(kuò)大，與不安裝末端執(zhí)行器的工作空間形狀、體積相比均發(fā)生了變化。對應(yīng)圖3、圖4中的區(qū)域①和②可得原先不能到達(dá)的部分點(diǎn)現(xiàn)在可以到達(dá)。

由圖5得出機(jī)器人安裝末端執(zhí)行器后，在其關(guān)節(jié)轉(zhuǎn)角范圍內(nèi)存在焊槍與機(jī)器人關(guān)節(jié)之間的碰撞區(qū)域。由圖5和圖6云圖形狀不同且圖6中的云圖體積大于圖5中的云圖體積，可得在機(jī)器人轉(zhuǎn)角范圍內(nèi)取值時存在機(jī)器人自身關(guān)節(jié)之間的碰撞區(qū)域。

由圖3、圖4與圖5、圖6對應(yīng)比較，可以看出機(jī)器人的碰撞區(qū)域與不碰撞工作空間部分重合。機(jī)器人末端工具坐標(biāo)系存在位置和姿態(tài)，本文通過正運(yùn)動學(xué)映射出的點(diǎn)只是工具坐標(biāo)系的位置，而同一位置不同姿態(tài)下，機(jī)器人關(guān)節(jié)組合不同，從而導(dǎo)致相同位置點(diǎn)存在碰撞和不碰撞情形。另外，還可以看出碰撞區(qū)域體積小于機(jī)器人工作空間體積，那么機(jī)器人工作空間集中減去碰撞區(qū)域集則可為機(jī)器人無碰撞工作空間，該區(qū)域內(nèi)不會發(fā)生碰撞。

圖3　機(jī)器人工作空間(P點(diǎn)，50萬點(diǎn))

圖4　機(jī)器人工作空間(T點(diǎn)，50萬點(diǎn))

圖5　機(jī)器人關(guān)節(jié)與焊槍的碰撞區(qū)域(T點(diǎn)，50萬點(diǎn))

圖6　機(jī)器人關(guān)節(jié)之間以及與焊槍的碰撞區(qū)域(T點(diǎn)，50萬點(diǎn))

4結(jié)論

本文基于蒙特卡洛法和混合包圍體碰撞檢測算法，采用虛擬現(xiàn)實技術(shù)建立機(jī)器人碰撞檢測模型得到了安裝末端執(zhí)行器后的機(jī)器人工作空間中碰撞區(qū)域以及無碰撞工作空間，這對機(jī)器人結(jié)構(gòu)設(shè)計和末端執(zhí)行器設(shè)計、選用以及確保機(jī)器人工作安全有重要指導(dǎo)意義，最終可以得到結(jié)論如下：

(1)帶末端執(zhí)行器的與不帶的機(jī)器人工作空間差別較大，末端執(zhí)行器的選用可以擴(kuò)大機(jī)器人的工作范圍。

(2)在機(jī)器人關(guān)節(jié)轉(zhuǎn)角范圍內(nèi)，機(jī)器人自身關(guān)節(jié)之間及末端執(zhí)行器之間確實存在碰撞區(qū)域且與工作空間存在重合部分。基于蒙特卡洛法和混合包圍體層次樹碰撞檢測算法可以有效獲取該碰撞區(qū)域。機(jī)器人工作空間區(qū)域減去碰撞區(qū)域所得區(qū)域即為機(jī)器人無碰撞工作空間。

(3)機(jī)器人自身關(guān)節(jié)之間的碰撞區(qū)域大于機(jī)器人關(guān)節(jié)與末端執(zhí)行器之間的碰撞區(qū)域，在機(jī)器人設(shè)計中僅僅限定各個關(guān)節(jié)的有效轉(zhuǎn)角范圍，而不設(shè)定各個關(guān)節(jié)轉(zhuǎn)角范圍取值之間的相互約束關(guān)系，不能確保機(jī)器人運(yùn)動安全。

[參考文獻(xiàn)]

[1] Panda S, Mishra D, Biswal B B. Revolute manipulator workspace optimization: A comparative study[J]. Applied Soft Computing. 2013, 13(2): 899-910.

[2] 王召莉, 陳衛(wèi)東. 基于關(guān)節(jié)空間評估的機(jī)器人作業(yè)單元布局優(yōu)化[J]. 上海交通大學(xué)學(xué)報，2009，43(11):1762-1766.

[3] 張鵬程，張鐵. 基于包絡(luò)法六自由度工業(yè)機(jī)器人工作空間的分析[J].機(jī)械設(shè)計與制造，2010(10):164-166.

[4] Jin Y, Chen I, Yang G. Workspace evaluation of manipulators through finite-partition of SE(3)[J]. Robotics and Computer-Integrated Manufacturing. 2011, 27(4): 850-859.

[5] Cao Y, Lu K, Li X, et al. Accurate numerical methods for computing 2d and 3d robot workspace[J]. International Journal of Advanced Robotic Systems: INTECH, 2011, 6: 1-13.

[6] Yang X, Wang H, Zhang C, et al. A method for mapping the boundaries of collision-free reachable workspaces[J]. Mechanism and Machine Theory，2010, 45(7): 1024-1033.

[7] 田海波，馬宏偉. 井下排爆機(jī)器人機(jī)械臂運(yùn)動學(xué)及工作空間的分析[J]. 礦山機(jī)械，2012，40(9): 105-108.

[8] 劉志忠，柳洪義，羅忠，等. 機(jī)器人工作空間求解的蒙特卡洛法改進(jìn)[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2013,44(1): 230-235.

[9] 李保豐，孫漢旭，賈慶軒，等. 基于蒙特卡洛法的空間機(jī)器人工作空間計算[J]. 航天器工程，2011,20(4): 79-85.

[10] 趙燕江，張永德，姜金剛，等. 基于Matlab的機(jī)器人工作空間求解方法[J]. 機(jī)械科學(xué)與技術(shù)，2009,28(12): 1657-1661.

[11] Abdel-Malek K, Yeh H, Othman S. Interior and exterior boundaries to the workspace of mechanical manipulators[J]. Robotics and Computer Integrated Manufacturing, 2000, 16(5): 365-376.

[12] Yang J, Abdel-Malek K, Zhang Y. On the workspace boundary determination of serial manipulators with non-unilateral constraints[J]. Robotics and Computer-Integrated Manufacturing. 2008, 24(1): 60-76.

[13] 陳琳, 付兵, 潘海鴻,等. 一種適用于多機(jī)器人的動態(tài)包圍體層次樹碰撞檢測算法[J]. 組合機(jī)床與自動化加工技術(shù)，2014(7):73-76.

(編輯趙蓉)

The Solution of Robot Manipulator’s Collision Free Workspace Based on Monte Carlo Method

FU Bin1, HU Fei-fei1, CHEN Lin2, PAN Hai-hong2

(1.Key Laboratory of Automotive Power Train and Electronics，School of Mechanical Engineering, Hubei University of Automotive Technology, Shiyan Hubei 442002,China;2.Guangxi Key Laboratory of Manufacturing System & Advanced Manufacturing Technology,School of Mechanical Engineering, Guangxi University, Nanning 530004,China)

Abstract：To study the end effector’s influence on actual workspace of the robot manipulator, so at the same time the factors such like the collision between the robot joints, and the collision between end effector and robot joints were take into account to analyze the influence of the robot workspace. This paper makes a research on the collision-free workspace for the robot manipulator with an end effector by using a 6 DOF industrial robot as the object. This paper proposes a method by using the monte carlo method to generate a lot of robot joint angles values randomly, and use those robot joint angles to update the robot virtual prototype’s position and attitude respectively, then use mixed bounding volume hierarchy tree collision detection algorithm to detect the collision among the robot's own joints and the collision between the end effector and robot joints to analyze robot collision-free workspace. In the end, it accesses the collision point in the robot manipulator’s workspace and gets collision-free workspace of the robot manipulator. The results show that the proposed method can effectively get collision-free workspace for the robot manipulator with an end effector.

Key words：industrial robot; monte carlo method; workspace；collision detection

中圖分類號：TH166;TG659

文獻(xiàn)標(biāo)識碼：A

作者簡介：付兵(1987—)，男，湖北廣水人，湖北汽車工業(yè)學(xué)院助理工程師，碩士，研究方向為機(jī)器人技術(shù)、多機(jī)器人協(xié)同控制，(E-mail)fubing123456789@126.com；潘海鴻(1966—)，男，南寧人，廣西大學(xué)教授，博導(dǎo)，博士，研究方向為機(jī)器人技術(shù)，多機(jī)器人協(xié)調(diào)控制技術(shù)，數(shù)控，(E-mail)hustphh@163.com。

*基金項目：國家自然科學(xué)基金(51065005)；汽車動力傳動與電子控制湖北省重點(diǎn)實驗室開放基金(ZDK1201408)

收稿日期：2015-09-11;修回日期：2015-10-14

文章編號：1001-2265(2016)02-0016-04

DOI:10.13462/j.cnki.mmtamt.2016.02.005