專利名稱：預(yù)測(cè)機(jī)器人的著眼部位與周邊物的干涉的方法及裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及在機(jī)器人的卡爪內(nèi)裝載的工具或者傳感器等著眼部位伴隨機(jī)器人的動(dòng)作移動(dòng)的情況下，預(yù)測(cè)在其移動(dòng)路徑上的著眼部位與在機(jī)器人的周圍固定設(shè)置的周邊設(shè)備或者柵欄等周邊物的干涉的方法及裝置。
背景技術(shù)：
在機(jī)器人使用視覺傳感器取出工件的撿拾作業(yè)的情況下，測(cè)量不能通過(guò)視覺傳感器定位的工件的位置姿勢(shì)，根據(jù)其測(cè)量結(jié)果，生成機(jī)器人向能夠握持工件的位置姿勢(shì)的移動(dòng)路徑。因?yàn)楫?dāng)工件的位置姿勢(shì)變化時(shí)機(jī)器人的移動(dòng)路徑也變化，所以在機(jī)器人的卡爪中 裝載的工具或者傳感器等著眼部位、與在機(jī)器人的周圍固定設(shè)置的周邊設(shè)備或者柵欄等周邊物在移動(dòng)路徑上是否干涉也在變化。在多個(gè)工件散裝的情況下，對(duì)于通過(guò)視覺傳感器測(cè)量的各個(gè)工件生成機(jī)器人的移動(dòng)路徑。理想的是對(duì)于生成的多個(gè)移動(dòng)路徑進(jìn)行干涉預(yù)測(cè)，使機(jī)器人握持與判斷為不干涉的移動(dòng)路徑對(duì)應(yīng)的工件。這里，作為預(yù)測(cè)干涉的現(xiàn)有技術(shù)，有進(jìn)行移動(dòng)路徑的移動(dòng)結(jié)束位置處的機(jī)器人的著眼部位與周邊物的干涉的確認(rèn)的方法。例如，在日本特開2000-326280號(hào)公報(bào)中，公開了標(biāo)量型機(jī)器人的干涉檢查方法，這里，把關(guān)于機(jī)器人的目標(biāo)位置的指令變換為各關(guān)節(jié)軸的行程值，進(jìn)行關(guān)于各關(guān)節(jié)軸的行程限制的檢查，同時(shí)把關(guān)于機(jī)器人的目標(biāo)位置的指令變換為正交坐標(biāo)空間上的位置，在正交坐標(biāo)空間上進(jìn)行關(guān)于上述干涉區(qū)域的檢查，在確認(rèn)目標(biāo)位置滿足上述行程限制以及關(guān)于上述干涉區(qū)域的限制的情況下，給予各關(guān)節(jié)軸動(dòng)作指令。另外，在日本特開2002-331480號(hào)公報(bào)中公開了用于避免機(jī)器人與其他物體的干涉的干涉避免裝置，在其
段中，記載了在機(jī)器人控制裝置6的存儲(chǔ)器中預(yù)先設(shè)定存儲(chǔ)工具3的形狀信息以及收納箱4的形狀信息和位置姿勢(shì)信息、還有示教時(shí)的工件10的位置姿勢(shì)Wn、工具的位置姿勢(shì)Tn，根據(jù)用上述I式求出的工具的位置姿勢(shì)Ta的工具形狀信息、和收納箱4的形狀信息和位置姿勢(shì)信息，能夠判斷工具3和收納箱4是否干涉。作為預(yù)測(cè)干涉的其他的現(xiàn)有技術(shù)，在日本特開2002-273675號(hào)公報(bào)中公開了示教作業(yè)自動(dòng)化了的機(jī)器人的控制裝置以及控制系統(tǒng)，這里，模擬機(jī)器人對(duì)于預(yù)定作業(yè)的動(dòng)作，檢查有無(wú)與作業(yè)路徑中的部件的干涉，在判定為機(jī)器人臂或者在其尖端安裝的工具與部件干涉的情況下，如果干涉區(qū)間長(zhǎng)度不超過(guò)預(yù)定值，則在干涉位置分?jǐn)鄼C(jī)器人的作業(yè)路徑，在該分?jǐn)嗟淖鳂I(yè)路徑內(nèi)使機(jī)器人動(dòng)作。另外，作為其他的現(xiàn)有技術(shù)，在日本特開平9-27046號(hào)公報(bào)中，公開了一種干涉檢查方法生成非凸多面體的凸包，進(jìn)行該凸包與其他物體間的干涉檢查，在干涉開始時(shí)，解除凸包，進(jìn)行上述非凸多面體與其他物體間的干涉檢查，該方法具有下述步驟把構(gòu)成非凸多面體的頂點(diǎn)分割為第一軸(例如X軸)的坐標(biāo)值的差為零或者微小量e以內(nèi)的多個(gè)頂點(diǎn)組的步驟；針對(duì)各頂點(diǎn)組在第二 -第三軸平面(例如YZ平面)上生成二維凸包的步驟；合并鄰接的二維凸包生成三維凸包，以后依次合并鄰接的三維凸包生成目的的非凸多面體的凸包的步驟。進(jìn)而作為其他的現(xiàn)有技術(shù)，在日本特開2004-280635號(hào)公報(bào)中公開了一種模擬多個(gè)機(jī)器人的動(dòng)作的模擬裝置，該模 擬裝置具有軌跡信息記錄單元，用于記錄表示每一臺(tái)機(jī)器人模型的一定時(shí)間動(dòng)作的軌跡的形狀的多個(gè)多面體的坐標(biāo)值和模擬時(shí)刻；和干涉檢測(cè)單元，用于分別在數(shù)據(jù)表(spread sheet)內(nèi)輸入表示上述多個(gè)機(jī)器人的軌跡的形狀的多個(gè)多面體的三維坐標(biāo)值和時(shí)刻，通過(guò)計(jì)算檢測(cè)在多個(gè)表間的相同時(shí)刻空間干涉的多面體。在日本特開2000-326280號(hào)公報(bào)或者日本特開2002-331480號(hào)公報(bào)中記載的技術(shù)，即使能夠確認(rèn)移動(dòng)結(jié)束位置處的干涉，而確認(rèn)、預(yù)測(cè)移動(dòng)路徑上的干涉也困難。例如，在日本特開2002-331480號(hào)公報(bào)的圖2中，表示在取出位于收納箱4中的工件7時(shí)的工具3的移動(dòng)路徑，雖然在移動(dòng)結(jié)束時(shí)刻T4時(shí)工具3和收納箱4不干涉，但是在移動(dòng)中的時(shí)刻T2和T3之間工具3和收納箱4有時(shí)干涉。另外，日本特開2002-273675號(hào)公報(bào)中記載的技術(shù)，為以高的精度預(yù)測(cè)生成的移動(dòng)路徑上的干涉，對(duì)于移動(dòng)路徑上的全部插補(bǔ)位置，逐次檢查機(jī)器人的著眼部位與周邊物是否干涉。但是在該技術(shù)中，計(jì)算時(shí)間過(guò)長(zhǎng)，認(rèn)為不適合在機(jī)器人動(dòng)作中實(shí)時(shí)地預(yù)測(cè)干涉。日本特開平9-27046號(hào)公報(bào)中記載的技術(shù)，是在預(yù)測(cè)干涉時(shí)利用所謂的凸包的技術(shù)，但是這是預(yù)測(cè)處于靜止?fàn)顟B(tài)的多面體之間干涉的技術(shù)，不能應(yīng)用于移動(dòng)的多面體。另外日本特開2004-280635號(hào)公報(bào)記載的技術(shù),在多個(gè)機(jī)器人或者加工機(jī)械、夾具同時(shí)動(dòng)作時(shí)，計(jì)算覆蓋各自的動(dòng)作區(qū)域的多面體，把該機(jī)器人之間等的干涉判定置換為該多面體之間的干涉判定。但是，因?yàn)榕c動(dòng)作區(qū)域?qū)?yīng)的多面體不限于凸多面體，所以有時(shí)干涉判定計(jì)算復(fù)雜，花費(fèi)時(shí)間，不適合在機(jī)器人動(dòng)作中實(shí)時(shí)進(jìn)行干涉判定。
發(fā)明內(nèi)容
因此本發(fā)明的目的是提供一種干涉預(yù)測(cè)方法以及干涉預(yù)測(cè)裝置，用于在機(jī)器人的卡爪中裝載的工具或者傳感器等著眼部位伴隨機(jī)器人的動(dòng)作移動(dòng)的情況下，以實(shí)用的精度以及計(jì)算速度預(yù)測(cè)移動(dòng)路徑上的著眼部位和在機(jī)器人的周圍固定設(shè)置的周邊設(shè)備或者柵欄等的周邊物的干涉。為實(shí)現(xiàn)上述目的，本發(fā)明的一種實(shí)施方式，提供一種干涉預(yù)測(cè)方法，該方法存儲(chǔ)周邊物占有區(qū)域和著眼部位近似多面體，周邊物占有區(qū)域是用多面體近似在空間固定的周邊物的形狀得到的周邊物近似多面體占有上述空間的區(qū)域，著眼部位近似多面體是用多面體近似機(jī)器人的著眼部位得到的形狀；伴隨上述機(jī)器人的動(dòng)作計(jì)算多個(gè)著眼部位占有區(qū)域，它們是上述著眼部位近似多面體在多個(gè)時(shí)刻的各個(gè)時(shí)刻占有上述空間的區(qū)域；根據(jù)上述多個(gè)著眼部位占有區(qū)域和上述周邊物占有區(qū)域，預(yù)測(cè)上述機(jī)器人的著眼部位和上述周邊物有無(wú)干涉，該干涉預(yù)測(cè)方法計(jì)算在按照上述多個(gè)時(shí)刻的順序排列上述多個(gè)著眼部位占有區(qū)域時(shí)相鄰的著眼部位占有區(qū)域之間的組合的全部凸包；計(jì)算上述周邊物占有區(qū)域來(lái)作為凸多面體的和；判定全部上述凸包和上述凸多面體有無(wú)共有區(qū)域來(lái)作為上述機(jī)器人的著眼部位和上述周邊物有無(wú)干涉。本發(fā)明的另一種實(shí)施方式，提供一種干涉預(yù)測(cè)裝置，其具有存儲(chǔ)部，用于存儲(chǔ)周邊物占有區(qū)域和著眼部位近似多面體，周邊物占有區(qū)域是用多面體近似在空間固定的周邊物的形狀得到的周邊物近似多面體占有上述空間的區(qū)域，著眼部位近似多面體是用多面體近似機(jī)器人的著眼部位得到的形狀；著眼部位占有區(qū)域計(jì)算部，用于伴隨上述機(jī)器人的動(dòng)作計(jì)算多個(gè)著眼部位占有區(qū)域，它們是上述著眼部位近似多面體在多個(gè)時(shí)刻的各個(gè)時(shí)刻占有上述空間的區(qū)域；以及干涉預(yù)測(cè)部，用于根據(jù)上述多個(gè)著眼部位占有區(qū)域和上述周邊物占有區(qū)域，預(yù)測(cè)上述機(jī)器人的著眼部位和上述周邊物有無(wú)干涉，上述干涉預(yù)測(cè)部具有凸包計(jì)算部，用于計(jì)算在按照上述多個(gè)時(shí)刻的順序排列上述多個(gè)著眼部位占有區(qū)域時(shí)相鄰的著眼部位占有區(qū)域之間的組合的全部凸包；以及周邊物占有區(qū)域計(jì)算部，用于計(jì)算上述周邊物占有區(qū)域來(lái)作為凸多面體的和，上述干涉預(yù)測(cè)部，判定全部上述凸包和上述凸多面體有無(wú)共有區(qū)域來(lái)作為上述機(jī)器人的著眼部位和上述周邊物有無(wú)干涉。
通過(guò)下面參照附圖對(duì)于以下的合適的實(shí)施方式的說(shuō)明，能夠更加明了本發(fā)明的上述以及其他的目的、特征以及優(yōu)點(diǎn)。圖I是表示本發(fā)明的干涉預(yù)測(cè)裝置的一個(gè)實(shí)施方式的概要結(jié)構(gòu)的圖，圖2是表示以多面體近似工具以及收納箱的例子的圖，圖3是表示給作為凸多面體的例子的正四面體分配索引的例子的圖，
圖4是表示根據(jù)本發(fā)明的干涉預(yù)測(cè)裝置或者干涉預(yù)測(cè)方法的干涉預(yù)測(cè)過(guò)程的流程圖，圖5是表示分割數(shù)為4的情況下的各時(shí)間點(diǎn)的工具的位置姿勢(shì)、收納箱、和工件的位置關(guān)系的圖，圖6是說(shuō)明求出各時(shí)間點(diǎn)的多面體的頂點(diǎn)的直線度的例子的圖，圖7是說(shuō)明根據(jù)多面體區(qū)域求出凸包的例子的圖，圖8是表示把多邊形分割為多個(gè)三角形的例子的圖，圖9是表示凸包和表示周邊物的多面體的位置關(guān)系的圖。
具體實(shí)施例方式圖I是表示本發(fā)明的干涉預(yù)測(cè)裝置的一個(gè)實(shí)施方式的概要圖。此外，本發(fā)明中的所謂“機(jī)器人的著眼部位”指機(jī)器人臂的本體或者附屬物中在動(dòng)作中有與周邊設(shè)備干涉可能的部位。在本實(shí)施方式中，通過(guò)在機(jī)器人控制裝置6內(nèi)設(shè)置用于預(yù)測(cè)干涉的軟件，構(gòu)成干涉預(yù)測(cè)裝置。在機(jī)器人5的手腕尖端上安裝用于取出工件7的工具3、用于對(duì)工件7進(jìn)行攝影的攝像機(jī)2、以及向工件7投射激光的激光投射器I。通過(guò)攝像機(jī)2以及激光投射器1，構(gòu)成檢測(cè)工件7的位置以及姿勢(shì)(以下稱位置姿勢(shì))的三維視覺傳感器。在本實(shí)施方式中工件7在收納箱4內(nèi)收納。在該收納箱4內(nèi)以散裝的狀態(tài)收納多個(gè)工件7，因此各個(gè)工件7的位置姿勢(shì)成不規(guī)則狀態(tài)。機(jī)器人5、激光投射器I、攝像機(jī)2以及工具3歷來(lái)熟知，故而省略詳細(xì)的說(shuō)明。機(jī)器人控制裝置6，在具有處理器、ROM、EAM、非易失性RAM、帶有顯示裝置的輸入單元、輸入輸出接口、攝像機(jī)接口、驅(qū)動(dòng)控制機(jī)器人各關(guān)節(jié)軸的伺服電動(dòng)機(jī)等的伺服控制器等這點(diǎn)上，與現(xiàn)有的機(jī)器人控制裝置同樣即可。圖示例子的機(jī)器人5的關(guān)節(jié)軸數(shù)為6，當(dāng)命令工具3的位置姿勢(shì)時(shí)，機(jī)器人控制裝置6反推各關(guān)節(jié)軸的旋轉(zhuǎn)角，控制機(jī)器人5各軸的動(dòng)作，使工具3成為該位置姿勢(shì)。另外，在輸入輸出接口上連接激光投射器I以及工具3，在攝像機(jī)接口上連接攝像機(jī)2。假定在機(jī)器人控制裝置6的存儲(chǔ)器中預(yù)先設(shè)定、存儲(chǔ)工具3對(duì)于機(jī)器人5的手腕尖端的位置姿勢(shì)、工具3的形狀、收納箱4的位置姿勢(shì)、收納箱4的形狀、示教時(shí)的工件7的位置姿勢(shì)、對(duì)于該位置姿勢(shì)工具握持工件7時(shí)的位置姿勢(shì)。此時(shí)，關(guān)于存儲(chǔ)的工具3以及收納箱4的形狀，假定如圖2所示，工具3用組合兩個(gè)長(zhǎng)方體的多面體近似的區(qū)域D1表示，收納箱4用從長(zhǎng)方體的上側(cè)挖去一個(gè)稍小的長(zhǎng)方體的多面體近似的區(qū)域D2表示。雖然區(qū)域Dp D2都可以表現(xiàn)為多個(gè)長(zhǎng)方體區(qū)域的和，但是為使干涉預(yù)測(cè)的計(jì)算簡(jiǎn)單，決定近似的區(qū)域用凸多面體的和來(lái)表示。具體來(lái)說(shuō)，給各個(gè)多面體區(qū)域具有的頂點(diǎn)、各個(gè)邊分配索引，存儲(chǔ)各頂點(diǎn)的空間坐標(biāo)值、位于各邊的兩端的頂點(diǎn)的索引、位于各面的末端處的邊的索弓I。例如。在近似的區(qū)域是圖3所示那樣的各邊的長(zhǎng)度是3的正四面體(凸多面體)的情況下，給各頂點(diǎn)以及各邊分配索引，存儲(chǔ)某坐標(biāo)系中的頂點(diǎn)V1、頂點(diǎn)V2、頂點(diǎn)V3、頂點(diǎn) V4各自的坐標(biāo)值(0. 000,0. 000,2. 449)、(I. 732,0. 000,0. 000)、(-0. 886，I. 500,0. 000)、(-0. 886，-I. 500,0. 000)、邊E1、邊E2、邊E3、邊E4、邊E5、邊E6各自的兩端的頂點(diǎn)的索引(V1, V2)、(V1, V4)、(V1, V3)、(V2, V4)、(V3, V4)、(V2, V3)、位于面 S1、面 S2、面 S3J S4 各自的末端的邊的頂點(diǎn)索引(E1, E2, E4)、(E2, E3, E5)、(E1, E3, E6)、(E4, E5, E6)。圖4是表示在使用機(jī)器人5取出工件7的作業(yè)中判斷是否發(fā)生工具3和收納箱4的干涉的處理過(guò)程的流程圖。在處理過(guò)程中，首先根據(jù)來(lái)自機(jī)器人控制裝置6的指令，在把機(jī)器人5移動(dòng)到能夠?qū)κ占{箱4內(nèi)的工件7進(jìn)行攝影的位置區(qū)域后，使用激光投射器I以及攝像機(jī)2，測(cè)量收納箱4內(nèi)的工件7的位置姿勢(shì)(步驟101)。接著，合并該測(cè)量出的工件7的位置姿勢(shì)、預(yù)先存儲(chǔ)的示教時(shí)的工件7的位置姿勢(shì)、預(yù)先存儲(chǔ)的握持工件時(shí)的工具的位置姿勢(shì)，計(jì)算能夠用工具3握持所測(cè)量的工件7從收納箱中取出的工具3的位置姿勢(shì)。然后，關(guān)于工具3，把測(cè)量工件7時(shí)的位置姿勢(shì)作為移動(dòng)開始時(shí)的位置姿勢(shì)，把能夠用工具3握持所測(cè)量的工件7從收納箱中取出的工具3的位置姿勢(shì)作為移動(dòng)結(jié)束時(shí)的位置姿勢(shì)，生成工具3的移動(dòng)路徑(步驟102)。移動(dòng)路徑的生成方法有多種，但是例如有使機(jī)器人各關(guān)節(jié)軸的旋轉(zhuǎn)速度始終成為恒定的移動(dòng)路徑的生成方
法。亦即是在假定把移動(dòng)開始時(shí)的機(jī)器人5的各關(guān)節(jié)軸k(k= 1、2.....6)的旋轉(zhuǎn)角的值
作為9 k、把移動(dòng)結(jié)束時(shí)的各關(guān)節(jié)軸k的旋轉(zhuǎn)角的值作為Cpk、把移動(dòng)開始時(shí)刻作為0(零)、把移動(dòng)結(jié)束時(shí)刻作為T的情況下，生成使時(shí)刻t (0 < t < T)時(shí)的各關(guān)節(jié)軸k的旋轉(zhuǎn)角成為0 k+AkXt那樣的移動(dòng)路徑的方法。式中Ak= ((pk- 0k) / T。但是這樣的移動(dòng)路徑的生成方法是一例，這里敘述的干涉預(yù)測(cè)的應(yīng)用，不依賴于移動(dòng)路徑的生成方法。接著n等分從移動(dòng)開始到移動(dòng)結(jié)束的移動(dòng)時(shí)間，設(shè)得到的n+1個(gè)時(shí)刻(時(shí)間點(diǎn))為
T0^T1.....Tn(步驟103)。Ttl以及Tn分別是移動(dòng)開始時(shí)刻和移動(dòng)結(jié)束時(shí)刻。分割數(shù)n考慮
干涉預(yù)測(cè)的粗細(xì)或處理時(shí)間，可以預(yù)先給定，也可以根據(jù)移動(dòng)時(shí)間以及移動(dòng)路徑長(zhǎng)度逐次決定。分割數(shù)越大越能提高干涉預(yù)測(cè)的精度，但是處理時(shí)間大體與分割數(shù)成比例變大。因?yàn)楸仨殞?shí)時(shí)進(jìn)行干涉預(yù)測(cè)的處理，所以處理時(shí)間盡可能短為好。圖5是表示分割數(shù)n = 4的情況下的各時(shí)間點(diǎn)的工具3的位置姿勢(shì)、收納箱4、和工件7的位置關(guān)系的圖。這里設(shè)工具3的移動(dòng)時(shí)間為2. 0秒，移動(dòng)路徑長(zhǎng)為400_。在把移動(dòng)時(shí)間4等分的情況下,從移動(dòng)開始時(shí)生成0. 0秒、0. 5秒、I. 0秒、I. 5秒、2. 0秒5個(gè)時(shí)間點(diǎn)，鄰接的時(shí)間點(diǎn)間的機(jī)器人的移動(dòng)路徑長(zhǎng)度為100mm。
接著把變量j設(shè)定為2 (j — 2)，進(jìn)行工具的凸包D’工的計(jì)算(步驟104)。首先，對(duì)于工具3占有的區(qū)域D1，計(jì)算該凸包的頂點(diǎn)。所謂凸包是包含連接給予的集合的任意兩點(diǎn)的線段(在集合的包含關(guān)系的意義上)的最小的集合，換言之，指包含全部點(diǎn)的集合體的最小的凸形狀、即通過(guò)用直線連接點(diǎn)的集合體中最外側(cè)的點(diǎn)形成的多面體。因此如果區(qū)域D1是多面體，則凸包也成為多面體。凸包的頂點(diǎn)，例如可以使用稱為QuickHull (“TheQuickhull Algorithm for Convex Hulls，，ACM Translations on Mathematical Software,Vol. 22，No. 4，December 1996，Pages 469-483)的算法根據(jù)多面體區(qū)域01的各頂點(diǎn)來(lái)求出。接著在步驟105，求出時(shí)間點(diǎn)Tj的直線度Ljtj具體說(shuō)，對(duì)于在步驟104求得的凸包D’ ,的各頂點(diǎn)V，如圖6所示，對(duì)于各頂點(diǎn)V把時(shí)間點(diǎn)Tj-PTpTV1處的其頂點(diǎn)的位置分別設(shè)為Pj-i、Pj、Pj+1，把從點(diǎn)Pj引向線段Ph1Pp1的垂線的垂足設(shè)為Hj，通過(guò)以下的式(I)計(jì)算頂點(diǎn)V在時(shí)間點(diǎn)Tj時(shí)的直線度Lv，jt) p~5~
I — rMrW
_5] (I)進(jìn)而把全部頂點(diǎn)在時(shí)間點(diǎn)L時(shí)的直線度Lq中的最小值作為工具3在時(shí)間點(diǎn)Tj時(shí)的直線度Lj (步驟105)。直線度Lv,」可以取范圍0 I的值，但是值越接近1，折線Ph1PjPp1越接近直線。然后，對(duì)于在機(jī)器人控制裝置中預(yù)先存儲(chǔ)的接近0的正值e，判定工具3的直線度Lj和(I- e )的大小關(guān)系(步驟106)，在Lj比(I- e )大的情況下，通過(guò)使Tj — TJ+1、
TJ+1 — Tj+2.....Tlri — Tn、n — n-1，從時(shí)間點(diǎn)的列中刪除時(shí)間點(diǎn)Tj (步驟107)。由此，能夠
幾乎不降低干涉預(yù)測(cè)的精度地減少一個(gè)在后面的步驟中計(jì)算的凸包的個(gè)數(shù)，減少計(jì)算花費(fèi)的時(shí)間。關(guān)于幾乎不降低干涉預(yù)測(cè)的精度的理由后面進(jìn)行說(shuō)明。在步驟108中，判定j和(n-1)的大小關(guān)系，在j < n-1時(shí)使j — j+1 (步驟109)，返回步驟105，在j < n-1成立之前進(jìn)行各時(shí)間點(diǎn)的直線度Lj的值的檢查。接著使j — I (步驟110)，在各時(shí)間點(diǎn)T」(j = 1、2.....n)把工具3占有的區(qū)域
設(shè)為Duj,計(jì)算鄰接的時(shí)間點(diǎn)TjfTj的區(qū)域的凸包Cj (步驟111)。區(qū)域Dm是在各時(shí)間點(diǎn)與L的工具3的移動(dòng)量對(duì)應(yīng)用三維空間對(duì)區(qū)域D1進(jìn)行剛體變換的區(qū)域。因?yàn)閰^(qū)域口工是多面體，所以區(qū)域Dy也是多面體，的凸包&也成為凸多面體。詳細(xì)說(shuō)明步驟111。首先，根據(jù)多面體D1, j_p D1,」的各頂點(diǎn)通過(guò)上述的QuickHull等求出凸包Cj的各頂點(diǎn)。接著，對(duì)于凸包Cj的全部?jī)身旤c(diǎn)的組合(VnV2),通過(guò)以下的步驟進(jìn)行線段V1V2是否是凸包Cj的邊，求出凸包Cj的各邊。向與空間上的線段V1V2垂直的平面n正投影凸包G的全部頂點(diǎn)。此時(shí)，頂點(diǎn)VpV2在平面n上的同一點(diǎn)P投影。在平面n上，如果正投影的全部點(diǎn)的凸包的頂點(diǎn)或邊上有點(diǎn)P，則可以判斷線段V1V2是凸包q的邊。平面上的凸包的頂點(diǎn)同樣可以通過(guò)QuickHull計(jì)算。另外，在平面上，如下那樣判斷連接凸包的各頂點(diǎn)PpP2的線段P1P2是否是該凸包的邊。向與線段P1P2垂直的直線、上正投影該凸包的全部頂點(diǎn)，如果在直線、上在作為這些點(diǎn)的凸包的線段的兩端的某個(gè)處有正投影線段P1P2的而得點(diǎn)，則可以判斷線段P1P2是邊。另外，凸包Cj的各面，對(duì)于用凸包Cj的三邊的組合組成三角形的全部的組合，在與包含該三角形的平面垂直的直線A上正投影凸包Cj的全部頂點(diǎn)，如果在直線A上在作為這些點(diǎn)的凸包的線段的兩端的某個(gè)處有正投影該三角形而得的點(diǎn)，則可以判斷該三角形是凸包Cj的面。圖7表示區(qū)域D1^ Dlj和根據(jù)它們計(jì)算的凸包&的例子。當(dāng)設(shè)區(qū)域D1^ Dlj是與用實(shí)線表示的各時(shí)刻的工具3實(shí)質(zhì)相當(dāng)?shù)膮^(qū)域時(shí)，凸包&成為在區(qū)域D1,爿、D1, j上加上用虛線表示的區(qū)域而得的區(qū)域。接著檢查凸包&和多面體區(qū)域D2有無(wú)共有區(qū)域(步驟112)。因?yàn)槎嗝骟w區(qū)域D2用多個(gè)凸多面體區(qū)域Dy的和表示，所以如果在凸包&和凸多面體Dy的組合中有持有共有區(qū)域的組合，則可以說(shuō)在凸包&和多面體區(qū)域D2中有共有區(qū)域。因?yàn)橥拱?amp;和凸多面體區(qū)域Dy的任何一面都是多邊形，所以如圖8那樣能夠進(jìn)行向多個(gè)三角形的分割(在圖8中把六邊形分割為四個(gè)三角形I IV)。于是，凸包&和凸多面體區(qū)域Dy的任何一個(gè)都成為僅用三角形覆蓋的閉區(qū)域，通過(guò)覆蓋凸包Cj的三角形和覆蓋凸多面體區(qū)域D2,」的三角形的邊(線段)交叉的組合是否在一個(gè)以上；覆蓋凸包Cj的三角形的邊(線段)和覆蓋凸多面體區(qū)域Dy的三角形交叉的組合是否在一個(gè)以上；或者凸包&和凸多面體區(qū)域D2,」的某個(gè)的區(qū)域已經(jīng)完全包含一方的區(qū)域這三個(gè)條件的至少一個(gè)是否成立，能夠進(jìn)行共有區(qū)域的有無(wú)的判斷。 如下那樣判斷線段和三角形的交叉。關(guān)于空間上的線段和三角形，把線段的兩端的點(diǎn)VA2的空間上的坐標(biāo)值分別設(shè)為a p a 2，把三角形的頂點(diǎn)VB2、B3的空間上的坐標(biāo)值設(shè)為用三維矢量表示a p a 2、P p P 2、P 3。首先計(jì)算3X3行列式(a「a 2，
P2-P3)是否是正則矩陣，在是正則矩陣的情況下判斷為線段A1A2和二角形B1B2B3不平行，在不是正則矩陣的情況下判斷為線段A1A2和三角形B1B2B3平行。在線段A1A2和三角形B1B2B3不平行的情況下，當(dāng)設(shè)包含線段A1A2的空間上的直線和包含三角形B1B2B3的空間上的平面的交點(diǎn)的空間上的坐標(biāo)值為X時(shí)，可以唯一表示為X = S1 a Js2 a 2, x = ^ :+t2 3 2+t3 3 3式中，Sps2、tp t2、t3 是滿足S^S2 = I, ti+^+tg = I的實(shí)數(shù)值。在所計(jì)算的Sp S2、t2、t3五個(gè)值全部在0以上而且在I以下的情況下，可以判斷為線段A1A2和三角形B1B2B3交叉。另一方面，在線段A1A2和三角形B1B2B3平行的情況下，計(jì)算點(diǎn)A1和平面B1B2B3的距離，如果該距離是0，則線段A1A2和三角形B1B2B3在同一平面上。在線段A1A2和三角形B1B2B3在同一平面上的情況下，如果2X 2行列式(a「a 2，^1-P2)是正則矩陣，則因?yàn)榫€段A1A2和三角形B1B2B3的邊B1B2不平行，所以當(dāng)設(shè)包含線段A1A2的空間上的直線與包含邊B1B2的空間上的直線的交點(diǎn)的坐標(biāo)值為y時(shí)，可以唯一表示為y = S1 a ^s2 a 2, y = ^ :+t2 3 2式中，SpSyVt2是滿足S^S2 = I, t!+t2 = I的實(shí)數(shù)值。在所計(jì)算的SpS2U1U2四個(gè)值全部在0以上而且在I以下的情況下，因?yàn)榫€段A1A2和邊B1B2交叉，所以可以判斷為線段A1A2和三角形B1B2B3交叉。在線段A1A2和三角形B1B2B3位于同一平面上的情況下，對(duì)于邊B2B3和邊B3B1也和邊B1B2同樣，檢查與線段A1A2的交叉，判斷線段A1A2和三角形B1B2B3的交叉。如果在任何一個(gè)交叉的判斷中都得不到線段A1A2和三角形B1B2B3交叉的判斷，則判斷為線段A1A2和三角形B1B2B3不交叉。凸包Cj是否完全包含凸多面體區(qū)域D2,」，可以通過(guò)關(guān)于凸多面體區(qū)域D2, j的全部頂點(diǎn)連接凸包&的重心與其頂點(diǎn)的線段與凸包&的表面是否相交來(lái)判斷。凸多面體區(qū)域D2, j是否完全包含凸包Cp也可以同樣判斷。上述那樣的有無(wú)共有區(qū)域的判斷結(jié)果，在凸包&和多面體區(qū)域D2有共有區(qū)域的情況下，判斷為在移動(dòng)路徑中工具3和收納箱4干涉(步驟113)，結(jié)束處理，在沒有共有區(qū)域的情況下，檢查是否j < n (步驟114)。在步驟114中，如果j < n，則前進(jìn)到步驟109，設(shè)定j為(j+1) (j — j+1)，返回步驟105。另一方面，如果在步驟114中j Sn，則判斷為在移動(dòng)路徑中工具3和收納箱4不干涉(步驟115)，結(jié)束處理。圖9是放大圖5表示的時(shí)間點(diǎn)T2和時(shí)間點(diǎn)T3之間的部分的圖，在T2、T3的各處工具3占有的兩個(gè)區(qū)域的凸包C3和多面體區(qū)域D2有共有區(qū)域。因此，在圖5的移動(dòng)路徑的情 況下，干涉預(yù)測(cè)裝置預(yù)測(cè)、判斷工具3和收納箱4干涉。凸包&和區(qū)域D2的共有區(qū)域的有無(wú)的判斷的計(jì)算需要的時(shí)間、根據(jù)兩個(gè)區(qū)域D1,j-pDy計(jì)算凸包&需要的時(shí)間分別需要I 2毫秒(ms)。在圖5的移動(dòng)路徑的情況下，因?yàn)闄C(jī)器人的移動(dòng)時(shí)間的分割數(shù)為4，所以干涉預(yù)測(cè)需要的時(shí)間最大為(2+2) X4= 16ms左右，不成為妨礙機(jī)器人動(dòng)作程度的時(shí)間。前面，敘述了在直線度h的值接近I、表示工具3占有的區(qū)域D1的凸包的各頂點(diǎn)的移動(dòng)的折線Pp1P^V1接近直線的情況下，即使在步驟107從時(shí)間點(diǎn)的列中刪除時(shí)間點(diǎn)Tj，干涉預(yù)測(cè)的精度也不降低，其依賴以下的理由。在折線Ph1PjPp1接近直線的情況下，區(qū)域D1, & Duj的凸包和區(qū)域D^Dm1的凸包的和與區(qū)域D1,爿、D1^1的凸包大體一致。因此，在通過(guò)凸包和多面體區(qū)域D2有無(wú)共有區(qū)域來(lái)判斷干涉的情況下，即使從時(shí)間點(diǎn)的列中刪除時(shí)間點(diǎn)L節(jié)省區(qū)域Dy的計(jì)算，干涉預(yù)測(cè)的精度也幾乎不降低。如上所述，在本發(fā)明中，通過(guò)在移動(dòng)路徑上的幾個(gè)插補(bǔ)位置求著眼部位占有的區(qū)域，求相鄰的插補(bǔ)位置處的區(qū)域彼此的全部凸包，用凸多面體的和表示周邊物，檢查全部這些凸包和全部凸多面體的干涉，由此預(yù)測(cè)移動(dòng)路徑上的著眼部位和周邊物的干涉。根據(jù)本發(fā)明，因?yàn)樵趧?dòng)作緊跟前以實(shí)用的精度以及計(jì)算速度自動(dòng)預(yù)測(cè)干涉，所以在取出散裝的工件等的用途中，能夠一邊避免干涉一邊使機(jī)器人動(dòng)作。另外在本申請(qǐng)發(fā)明中，求機(jī)器人的著眼部位的相鄰的插補(bǔ)位置處的區(qū)域彼此的凸包(凸多面體的范疇)，用凸多面體的和表示周邊物，把干涉判定置換為凸多面體彼此的干涉判定，謀求計(jì)算的簡(jiǎn)單化、高速化，實(shí)現(xiàn)機(jī)器人動(dòng)作中的實(shí)時(shí)的干涉判定。
權(quán)利要求
1.一種干涉預(yù)測(cè)方法， 該干涉預(yù)測(cè)方法存儲(chǔ)周邊物占有區(qū)域(D2)和著眼部位近似多面體(D1),周邊物占有區(qū)域(D2)是用多面體近似在空間固定的周邊物(4)的形狀得到的周邊物近似多面體占有上述空間的區(qū)域，著眼部位近似多面體(D1)是用多面體近似機(jī)器人(5)的著眼部位得到的形狀； 伴隨上述機(jī)器人(5)的動(dòng)作計(jì)算多個(gè)著眼部位占有區(qū)域(D1, P，它們是上述著眼部位近似多面體(D1)在多個(gè)時(shí)刻的各時(shí)刻占有上述空間的區(qū)域；以及 根據(jù)上述多個(gè)著眼部位占有區(qū)域(Dy)和上述周邊物占有區(qū)域(D2)，預(yù)測(cè)上述機(jī)器人(5)的著眼部位和上述周邊物(4)有無(wú)干涉， 該干涉預(yù)測(cè)方法的特征在于， 計(jì)算在按照上述多個(gè)時(shí)刻的順序排列上述多個(gè)著眼部位占有區(qū)域(Duj)時(shí)相鄰的著眼部位占有區(qū)域之間的組合的全部凸包(Cj)； 計(jì)算上述周邊物占有區(qū)域(D2)來(lái)作為凸多面體的和； 判定全部上述凸包(Cj)和上述凸多面體有無(wú)共有區(qū)域來(lái)作為上述機(jī)器人(5)的著眼部位和上述周邊物(4)有無(wú)干涉。
2.一種干涉預(yù)測(cè)裝置￠)，具有 存儲(chǔ)部，用于存儲(chǔ)周邊物占有區(qū)域(D2)和著眼部位近似多面體(D1),周邊物占有區(qū)域(D2)是用多面體近似在空間固定的周邊物(4)的形狀得到的周邊物近似多面體占有上述空間的區(qū)域，著眼部位近似多面體(D1)是用多面體近似機(jī)器人(5)的著眼部位得到的形狀；著眼部位占有區(qū)域計(jì)算部，用于伴隨上述機(jī)器人(5)的動(dòng)作計(jì)算多個(gè)著眼部位占有區(qū)域(Duj),它們是上述著眼部位近似多面體(D1)在多個(gè)時(shí)刻的各時(shí)刻占有上述空間的區(qū)域；以及 干涉預(yù)測(cè)部，用于根據(jù)上述多個(gè)著眼部位占有區(qū)域(Dm)和上述周邊物占有區(qū)域(D2),預(yù)測(cè)上述機(jī)器人(5)的著眼部位和上述周邊物(4)有無(wú)干涉， 該干涉預(yù)測(cè)裝置的特征在于， 上述干涉預(yù)測(cè)部具有 凸包計(jì)算部，用于計(jì)算在按照上述多個(gè)時(shí)刻的順序排列上述多個(gè)著眼部位占有區(qū)域(Dlj)時(shí)相鄰的著眼部位占有區(qū)域之間的組合的全部凸包(CP ;以及 周邊物占有區(qū)域計(jì)算部，用于計(jì)算上述周邊物占有區(qū)域(D2)來(lái)作為凸多面體的和， 上述干涉預(yù)測(cè)部，判定全部上述凸包(Cj)和上述凸多面體有無(wú)共有區(qū)域來(lái)作為上述機(jī)器人(5)的著眼部位和上述周邊物(4)有無(wú)干涉。
全文摘要
提供預(yù)定機(jī)器人的著眼部位和周邊物的干涉的方法以及裝置。在機(jī)器人的卡爪內(nèi)裝載的工具或者傳感器等著眼部位伴隨機(jī)器人的動(dòng)作移動(dòng)的情況下，以實(shí)用的精度以及計(jì)算速度預(yù)測(cè)在移動(dòng)路徑上的著眼部位與在機(jī)器人的周圍固定設(shè)置的周邊物的干涉。把在各時(shí)間點(diǎn)Tj工具占有的區(qū)域作為D1，j，計(jì)算鄰接的時(shí)間點(diǎn)Tj-1、Tj的區(qū)域D1，j-1、D1，j的凸包Cj。檢查凸包Cj和多面體區(qū)域D2有無(wú)共有區(qū)域。在凸包Cj和多面體區(qū)域D2有共有區(qū)域的情況下，判斷移動(dòng)路徑中工具和收納箱干涉，結(jié)束處理，在沒有共有區(qū)域的情況下，檢查是否j＜n，如果j≥n，則判斷移動(dòng)路徑中工具和收納箱不干涉，結(jié)束處理。
文檔編號(hào)B25J9/16GK102744727SQ20121011347
公開日2012年10月24日 申請(qǐng)日期2012年4月17日 優(yōu)先權(quán)日2011年4月18日
發(fā)明者小洼恭平, 組谷英俊 申請(qǐng)人:發(fā)那科株式會(huì)社