專(zhuān)利名稱：機(jī)械手位置檢測(cè)裝置以及機(jī)械手系統(tǒng)的制作方法
機(jī)械手位置檢測(cè)裝置以及機(jī)械手系統(tǒng)技術(shù)區(qū)域本發(fā)明涉及一種檢測(cè)機(jī)械手位置的機(jī)械手位置檢測(cè)裝置以及具備該機(jī)械手位置檢測(cè)裝置的機(jī)械手系統(tǒng)。
背景技術(shù)：
以往，作為對(duì)把持工件的機(jī)械手的目標(biāo)位置進(jìn)行修正的方法，已知有一種例如像專(zhuān)利文獻(xiàn)I那樣的技術(shù)。在專(zhuān)利文獻(xiàn)I中，為了將機(jī)械手所把持的工件和設(shè)置于該機(jī)械手的基準(zhǔn)標(biāo)記配置到對(duì)工件進(jìn)行拍攝的攝像裝置的攝像范圍內(nèi)，使攝像裝置以及機(jī)械手分別移動(dòng)至規(guī)定的位置。而且，通過(guò)對(duì)由攝像裝置拍攝到的圖像進(jìn)行圖像處理，取得工件相對(duì)基準(zhǔn)標(biāo)記的中心坐標(biāo)和傾斜度等，基于該取得的中心坐標(biāo)等算出與機(jī)械手的目標(biāo)位置相關(guān)的位置修正量。 專(zhuān)利文獻(xiàn)I :日本專(zhuān)利第4539685號(hào)公報(bào)不過(guò)，在專(zhuān)利文獻(xiàn)I的方法中，雖然能夠準(zhǔn)確算出與機(jī)械手的目標(biāo)位置相關(guān)的位置修正量，但在對(duì)用于算出位置修正量的圖像進(jìn)行拍攝時(shí)需要停止機(jī)械手。雖說(shuō)是為了算出位置修正量，但若將機(jī)械手停止在規(guī)定的位置，則與其停止的時(shí)間量對(duì)應(yīng)地裝置的生產(chǎn)效率降低。為了抑制這種生產(chǎn)效率的降低，希望不停止機(jī)械手地對(duì)移動(dòng)中的工件進(jìn)行拍攝，基于該拍攝時(shí)的機(jī)械手的位置和拍攝后的工件的圖像取得機(jī)械手的位置修正量。另一方面，表示機(jī)械手的位置的來(lái)自編碼器的數(shù)據(jù)通常通過(guò)串行通信被輸入到機(jī)械手控制器。因此，與該數(shù)據(jù)被輸出的定時(shí)相比，在滯后了與串行通信的通信周期以及數(shù)據(jù)量等對(duì)應(yīng)的、編碼器和機(jī)械手控制器之間的通信時(shí)間的定時(shí)，被輸入到機(jī)械手控制器。此時(shí)，若像上述那樣機(jī)械手不斷地移動(dòng)，則基于編碼器算出的位置和從圖像得到的位置在時(shí)間軸上相互不同。

發(fā)明內(nèi)容
本發(fā)明是鑒于上述的實(shí)際狀況而完成的發(fā)明，其目的在于，提供一種針對(duì)從移動(dòng)中的機(jī)械手檢測(cè)到的該機(jī)械手的位置，可提高其精度的機(jī)械手位置檢測(cè)裝置以及機(jī)械手系統(tǒng)。本發(fā)明的機(jī)械手位置檢測(cè)裝置具有位置數(shù)據(jù)輸入部，其被從編碼器輸入表示機(jī)械手的位置的位置數(shù)據(jù)；和位置算出部，其取得上述被輸入的位置數(shù)據(jù)，算出機(jī)械手的位置；上述位置算出部接受用于檢測(cè)上述機(jī)械手的位置的指令，使用在上述指令之前的第I定時(shí)取得的第I位置數(shù)據(jù)、在上述指令之后的第2定時(shí)取得的第2位置數(shù)據(jù)、和上述編碼器與上述位置數(shù)據(jù)輸入部之間的通信所需的通信時(shí)間，通過(guò)上述第I位置數(shù)據(jù)和上述第2位置數(shù)據(jù)的線性插值，算出與被輸入上述指令的定時(shí)相比滯后上述通信時(shí)間的定時(shí)的位置，并將該算出結(jié)果作為被輸入上述指令時(shí)的上述機(jī)械手位置而輸出。根據(jù)該機(jī)械手位置檢測(cè)裝置，由于基于與指令被輸入的定時(shí)相比滯后了編碼器和位置數(shù)據(jù)輸入部之間的通信所需要的通信時(shí)間的定時(shí)，利用線性插值輸出機(jī)械手的位置，所以線性插值所使用的各數(shù)據(jù)的定時(shí)成為大致相等的時(shí)間軸上的定時(shí)。因此，通過(guò)使用第I位置數(shù)據(jù)和第2位置數(shù)據(jù)的線性插值而算出的機(jī)械手的位置，成為以通信時(shí)間修正機(jī)械手移動(dòng)的移動(dòng)量后的位置。因此，能夠更高精度地檢測(cè)移動(dòng)中的機(jī)械手的位置。在該機(jī)械手位置檢測(cè)裝置中，上述位置算出部具備指令檢測(cè)部，該指令檢測(cè)部能夠以比取得從一個(gè)編碼器輸入給上述位置輸入部的上述位置數(shù)據(jù)的規(guī)定周期T短的周期Ts檢測(cè)該指令。根據(jù)該機(jī)械手位置檢測(cè)裝置，與被從一個(gè)編碼器輸入位置數(shù)據(jù)的定時(shí)相比，可檢測(cè)指令的定時(shí)變多。結(jié)果，能夠檢測(cè)出與被輸入指令的定時(shí)更接近的時(shí)期的機(jī)械手的位置。在該機(jī)械手位置檢測(cè)裝置中，上述位置算出部按照每個(gè)周期T/n，取得來(lái)自相互 不同的編碼器的位置數(shù)據(jù)，按照每個(gè)上述編碼器輸出被輸入上述指令時(shí)的上述機(jī)械手的位置，所述周期T/n是將上述規(guī)定的周期T以I以上的整數(shù)n進(jìn)行n等分而得到的周期。根據(jù)該機(jī)械手位置檢測(cè)裝置，由于按照相互不同的編碼器算出機(jī)械手的位置，所以能夠以更高的精度檢測(cè)指令被輸入時(shí)的機(jī)械手的位置。在該機(jī)械手位置檢測(cè)裝置中，上述位置算出部對(duì)取得的上述位置數(shù)據(jù)是否正常進(jìn)行判斷，并且在判斷為檢測(cè)到上述指令時(shí)的前一上述位置數(shù)據(jù)不正常的情況下，將在該位置數(shù)據(jù)之前取得的前一位置數(shù)據(jù)作為上述第I位置數(shù)據(jù)使用。由于從編碼器輸入的位置數(shù)據(jù)是電信號(hào)，所以存在例如受到噪音等的影響的問(wèn)題。若使用這樣的受到影響的位置數(shù)據(jù)算出機(jī)械手的位置，則由位置算出部算出的機(jī)械手的位置的可靠性降低。在這一點(diǎn)上，根據(jù)上述結(jié)構(gòu)的機(jī)械手位置檢測(cè)裝置，由于位置算出部對(duì)取得的位置數(shù)據(jù)是否正常進(jìn)行判斷，并且在判斷為檢測(cè)到指令時(shí)的前一位置數(shù)據(jù)不正常的情況下，將在該位置數(shù)據(jù)之前取得的前一位置數(shù)據(jù)作為第I位置數(shù)據(jù)使用，所以，能夠使用正常的位置數(shù)據(jù)來(lái)算出機(jī)械手的位置。在該機(jī)械手位置檢測(cè)裝置中，上述位置算出部在判斷為緊接著檢測(cè)到上述指令后取得的上述位置數(shù)據(jù)不正常的情況下，將在緊接著該位置數(shù)據(jù)之后取得的位置數(shù)據(jù)作為上述第2位置數(shù)據(jù)使用。通常，第I定時(shí)和第2定時(shí)間隔越長(zhǎng)，越會(huì)失去相互之間的位置的線性。在該機(jī)械手位置檢測(cè)裝置中，由于使用這樣的線性插值，所以，第I定時(shí)和第2定時(shí)在時(shí)間上間隔越長(zhǎng)，由位置算出部算出的機(jī)械手的位置和實(shí)際的機(jī)械手的位置越容易產(chǎn)生大的誤差。在這一點(diǎn)上，根據(jù)上述結(jié)構(gòu)，例如當(dāng)將輸入位置數(shù)據(jù)的定時(shí)、即檢測(cè)到指令后的定時(shí)作為基準(zhǔn)定時(shí)時(shí)，即便是第I定時(shí)和第2定時(shí)在時(shí)間上間隔最長(zhǎng)的情況，第I位置數(shù)據(jù)也是在比基準(zhǔn)定時(shí)提前2個(gè)定時(shí)輸入的位置數(shù)據(jù)。第2位置數(shù)據(jù)是在比基準(zhǔn)定時(shí)滯后2個(gè)定時(shí)輸入的位置數(shù)據(jù)。結(jié)果，與基于在該第I定時(shí)之前取得的位置數(shù)據(jù)和在該第2定時(shí)之后取得的位置數(shù)據(jù)算出機(jī)械手的位置的情況相比，能夠提高算出的機(jī)械手的位置精度。本發(fā)明的機(jī)械手系統(tǒng)具備搬運(yùn)工件的機(jī)械手；編碼器，其輸出表示上述機(jī)械手位置的位置數(shù)據(jù)；指令輸出部，其輸出用于檢測(cè)上述機(jī)械手的位置的指令；和機(jī)械手位置檢測(cè)部，其被輸入上述位置數(shù)據(jù)和上述指令，基于上述位置數(shù)據(jù)檢測(cè)被輸入上述指令時(shí)的上述機(jī)械手的位置；上述機(jī)械手位置檢測(cè)部是上述的機(jī)械手位置檢測(cè)裝置。根據(jù)本發(fā)明的機(jī)械手系統(tǒng)，即便在機(jī)械手移動(dòng)中指令輸出部輸出了指令，也能夠高精度地檢測(cè)該機(jī)械手的位置。
該機(jī)械手系統(tǒng)還具備照相機(jī)，其通過(guò)在被輸入來(lái)自上述指令輸出部的指令的定時(shí)，對(duì)預(yù)定的拍攝范圍進(jìn)行拍攝，來(lái)取得包含向目標(biāo)位置移動(dòng)過(guò)程中的上述機(jī)械手所搬運(yùn)的工件的圖像；工件位置取得部，其基于上述照相機(jī)取得的圖像，取得上述工件的位置；以及目標(biāo)位置修正部，其基于上述工件位置取得部取得的上述工件的位置、和上述機(jī)械手位置檢測(cè)部檢測(cè)到的上述機(jī)械手的位置，對(duì)上述目標(biāo)位置進(jìn)行修正。根據(jù)該機(jī)械手系統(tǒng)，能夠基于被輸入來(lái)自指令輸出部的指令的定時(shí)的機(jī)械手的位置與工件的位置的相對(duì)位置，對(duì)機(jī)械手的目標(biāo)位置進(jìn)行修正。如上所述，由機(jī)械手位置檢測(cè)部檢測(cè)的機(jī)械手的位置是使機(jī)械手移動(dòng)并對(duì)其進(jìn)行檢測(cè)的位置，并且其精度提高。即，無(wú)需為了修正目標(biāo)位置而停止機(jī)械手，并且，針對(duì)機(jī)械手的目標(biāo)位置的修正量的精度也能夠提高。結(jié)果，能夠在抑制機(jī)械手系統(tǒng)的生產(chǎn)效率降低的同時(shí)，將工件搬運(yùn)到應(yīng)該搬運(yùn)的位置。圖I是表示本發(fā)明的一個(gè)實(shí)施方式所涉及的機(jī)械手系統(tǒng)的概要結(jié)構(gòu)的立體圖。圖2是表示機(jī)械手的正面結(jié)構(gòu)的主視圖。
圖3是表示機(jī)械手系統(tǒng)的系統(tǒng)結(jié)構(gòu)的功能框圖。圖4是表示取得各編碼器的位置數(shù)據(jù)的定時(shí)的時(shí)序圖。圖5是表示數(shù)據(jù)處理部的存儲(chǔ)部所存儲(chǔ)的數(shù)據(jù)的圖。圖6是使機(jī)械手的位置與時(shí)間對(duì)應(yīng)的圖，是用于概念性說(shuō)明算出機(jī)械手的位置的計(jì)算過(guò)程的說(shuō)明圖。圖7是表示算出機(jī)械手位置的處理步驟的流程圖。圖8是表示算出機(jī)械手位置的處理步驟的流程圖。
具體實(shí)施例方式下面，參照?qǐng)DI 圖8，對(duì)本發(fā)明所涉及的機(jī)械手位置檢測(cè)裝置以及機(jī)械手系統(tǒng)的一個(gè)實(shí)施方式進(jìn)行說(shuō)明。首先，參照?qǐng)DI及圖2對(duì)本發(fā)明所涉及的機(jī)械手系統(tǒng)的概要結(jié)構(gòu)進(jìn)行說(shuō)明。如圖I所示，機(jī)械手系統(tǒng)10中在以規(guī)定的搬運(yùn)速度沿搬運(yùn)方向Cv搬運(yùn)工件W的傳送帶11的側(cè)方，配置有對(duì)被傳送帶11搬運(yùn)的工件W進(jìn)行選擇性地把持、對(duì)其進(jìn)行規(guī)定作業(yè)的水平多關(guān)節(jié)型機(jī)械手12。在搬運(yùn)方向Cv的機(jī)械手12的上游側(cè)，設(shè)置有從上方對(duì)在傳送帶11上被搬運(yùn)的工件W進(jìn)行拍攝的照相機(jī)13。在該機(jī)械手系統(tǒng)10中，基于由照相機(jī)13拍攝到的圖像來(lái)選擇成為作業(yè)對(duì)象的工件W，并基于該圖像和測(cè)定傳送帶11的搬運(yùn)量的編碼器14，對(duì)被選擇為作業(yè)對(duì)象的工作W的位置進(jìn)行把握。被選擇為作業(yè)對(duì)象的工件W在通過(guò)機(jī)械手12而上升到規(guī)定高度的狀態(tài)下，被搬運(yùn)到設(shè)置于該機(jī)械手12的可動(dòng)范圍內(nèi)的作業(yè)臺(tái)15上的規(guī)定位置。如圖2所示，機(jī)械手12中在設(shè)置于地面等的基臺(tái)21的上端部設(shè)置有以沿垂直方向的控制軸Cl為中心相對(duì)基臺(tái)21可轉(zhuǎn)動(dòng)的基臺(tái)軸22。通過(guò)設(shè)置于基臺(tái)21內(nèi)的第I馬達(dá)23正反旋轉(zhuǎn)，基臺(tái)軸22以控制軸Cl為中心轉(zhuǎn)動(dòng)。在基臺(tái)軸22上連結(jié)固定有沿水平方向延伸的第I水平臂24的基端部。在第I水平臂24的前端部上連結(jié)固定有支承軸25。支承軸25支承第2水平臂26，該第2水平臂26以沿垂直方向的控制軸C2為中心相對(duì)支承軸25可轉(zhuǎn)動(dòng)。在第2水平臂26的基端部配設(shè)有第2馬達(dá)27。第2水平臂26借助因第2馬達(dá)27正反旋轉(zhuǎn)而使該第2馬達(dá)27從支承軸25受到的反作用力，以控制軸C2為中心相對(duì)第I水平臂24在水平方向轉(zhuǎn)動(dòng)。
在第2水平臂26的前端部設(shè)置有貫通第2水平臂26的上下旋轉(zhuǎn)軸29。上下旋轉(zhuǎn)軸29被支承為可相對(duì)第2水平臂26旋轉(zhuǎn)，且可在上下方向移動(dòng)。而且，上下旋轉(zhuǎn)軸29通過(guò)第2水平臂26所具備的升降馬達(dá)30正反旋轉(zhuǎn)，沿垂直方向的控制軸C3升降。上下旋轉(zhuǎn)軸29通過(guò)第2水平臂26所具備的旋轉(zhuǎn)馬達(dá)31正反旋轉(zhuǎn)，以沿垂直方向的自身的控制軸C4為中心正反旋轉(zhuǎn)。能夠在上下旋轉(zhuǎn)軸29的作業(yè)部32上安裝工具，例如把持被搬運(yùn)物的手、加工被加工物的手等。另外，在向作業(yè)臺(tái)15搬運(yùn)的工件W的搬運(yùn)路徑上，設(shè)置有檢測(cè)工件W通過(guò)了檢測(cè)線FL的檢測(cè)傳感器35。檢測(cè)傳感器35具備沿水平方向射出由指向性、收斂性高的激光構(gòu)成的檢測(cè)光LB的射出部36和接受由射出部36射出的檢測(cè)光LB的受光部37。射出部36及受光部37被設(shè)置成隔著機(jī)械手12的可動(dòng)范圍，且檢測(cè)光LB暫時(shí)被從傳送帶11向作業(yè)臺(tái)15搬運(yùn)的工件W遮擋。在檢測(cè)傳感器35的下側(cè)設(shè)置有與檢測(cè)傳感器35檢測(cè)工件W同步地從下方對(duì)該工件W進(jìn)行拍攝的照相機(jī)39。在該機(jī)械手系統(tǒng)10中，基于由該照相機(jī)39拍攝到的圖像和此時(shí)機(jī)械手12的位置，把握工件W相對(duì)機(jī)械手12的相對(duì)位置，來(lái)對(duì)機(jī)械手12的目標(biāo)位置進(jìn)行修正，以使工件W被搬運(yùn)至預(yù)先設(shè)定的位置。下面，參照?qǐng)D3 圖8對(duì)上述結(jié)構(gòu)的機(jī)械手系統(tǒng)10的系統(tǒng)結(jié)構(gòu)進(jìn)行說(shuō)明。如圖3所示，上述的機(jī)械手系統(tǒng)10通過(guò)控制器50被統(tǒng)一控制?？刂破?0是驅(qū)動(dòng)控制機(jī)械手12的控制裝置，對(duì)機(jī)械手12向目標(biāo)位置移動(dòng)的定位動(dòng)作進(jìn)行控制，或?qū)C(jī)械手12追隨在傳送帶11上移動(dòng)的工件W的追隨動(dòng)作進(jìn)行控制。在控制器50中預(yù)先設(shè)定有表示機(jī)械手12的結(jié)構(gòu)、動(dòng)作特性以及可動(dòng)范圍內(nèi)的區(qū)域即能夠追隨傳送帶11上的工件W的追隨區(qū)域等的機(jī)械手?jǐn)?shù)據(jù)。而且，設(shè)定有表示傳送帶11和機(jī)械手12的相對(duì)位置、傳送帶11和照相機(jī)39的相對(duì)位置及機(jī)械手12和照相機(jī)39的相對(duì)位置等的與各種位置相關(guān)的數(shù)據(jù)；以及表示工件種類(lèi)和形狀的工件數(shù)據(jù)等。并且，這些機(jī)械手?jǐn)?shù)據(jù)、與各種位置相關(guān)的數(shù)據(jù)以及工件數(shù)據(jù)被用于機(jī)械手12的驅(qū)動(dòng)控制?？刂破?0具備存儲(chǔ)用戶程序的用戶程序存儲(chǔ)部51、和執(zhí)行存儲(chǔ)在該用戶程序存儲(chǔ)部51的用戶程序的用戶程序執(zhí)行部52。而且具備收納由照相機(jī)13識(shí)別出的工件W的初始位置的工件位置存儲(chǔ)部53、和基于傳送帶11的移動(dòng)量算出該工件位置存儲(chǔ)部53的工件W的當(dāng)前位置的工件位置更新部54。并且，具備生成作業(yè)部32的軌道(各控制軸Cl C4的關(guān)節(jié)角度)，并將其變換成馬達(dá)指令值(脈沖)，輸出給后述的馬達(dá)控制部57的指令值生成部55。另外，還具備根據(jù)追隨區(qū)域和工件W的位置關(guān)系等檢測(cè)成為作業(yè)對(duì)象的工件W的作業(yè)對(duì)象檢測(cè)部56、和根據(jù)來(lái)自指令值生成部55的馬達(dá)指令值對(duì)各控制軸Cl C4的馬達(dá)23、27、30、31進(jìn)行控制的馬達(dá)控制部57。用戶程序存儲(chǔ)部51的用戶程序是執(zhí)行為了使機(jī)械手12的作業(yè)部32移動(dòng)來(lái)追隨作業(yè)對(duì)象的工件W所必要的各種處理的程序。而且，是執(zhí)行為了使作業(yè)部32所把持的工件W移動(dòng)到作業(yè)臺(tái)15的規(guī)定位置所必要的各種處理的程序。用戶程序執(zhí)行部52執(zhí)行用戶程序。用戶程序執(zhí)行部52根據(jù)作業(yè)對(duì)象檢測(cè)部56的檢測(cè)結(jié)果，將被檢測(cè)為作業(yè)對(duì)象的工件W作為作業(yè)對(duì)象進(jìn)行處理。并且，用戶程序執(zhí)行部52算出為了使機(jī)械手12追隨被選擇的工件W所必要的信息，而且算出為了使機(jī)械手12移動(dòng)到目標(biāo)位置所必要的信息，并將這些算出的數(shù)據(jù)傳送給指令值生成部55。另外，從后述的工件位置取得部58向作為目標(biāo)位置修正部的用戶程序執(zhí)行部52輸入搬運(yùn)中的工件W的位置，并且由位置算出部60算出的移動(dòng)中的機(jī)械手12的位置被輸入給用戶程序執(zhí)行部52。用戶程序執(zhí)行部52基于從工件位置取得部58輸入的工件W的位置和從位置算出部60輸入的機(jī)械手12的位置，對(duì)機(jī)械手12的目標(biāo)位置進(jìn)行修正，并且算出為了移動(dòng)到該修正后的目標(biāo)位置所必要的信息，并將該算出的數(shù)據(jù)傳送給指令值生成部55。工件位置更新部54根據(jù)照相機(jī)13拍攝到的傳送帶11上的工件W的拍攝圖像，求出工件W的初始位置，并將該初始位置的信息保存到工件位置存儲(chǔ)部53。另一方面，工件位置更新部54基于由從編碼器14輸入的傳送帶脈沖的積算而得到的傳送帶脈沖數(shù)，算出通過(guò)傳送帶11傳送的工件W的搬運(yùn)量。然后，求出拍攝工件W時(shí)(初始位置)的傳送帶脈沖數(shù)和當(dāng)前的傳送帶脈沖數(shù)之差，并利用該脈沖數(shù)的差與預(yù)先設(shè)定的單位傳送帶脈沖的搬運(yùn) 量的乘積，求出相對(duì)工件W的初始位置的搬運(yùn)量。工件位置更新部54例如通過(guò)對(duì)工件位置存儲(chǔ)部53中存儲(chǔ)的拍攝時(shí)算出的工件W的初始位置(X軸方向的坐標(biāo))加上如上所述算出的工件W的搬運(yùn)量等，能夠逐次計(jì)算在傳送帶11上搬運(yùn)的工件W的當(dāng)前位置。作業(yè)對(duì)象檢測(cè)部56以規(guī)定的執(zhí)行周期對(duì)工件W的當(dāng)前位置和追隨區(qū)域進(jìn)行比較。然后，當(dāng)工件W的當(dāng)前位置到達(dá)(進(jìn)入)追隨區(qū)域內(nèi)時(shí)，將該工件W檢測(cè)為作業(yè)對(duì)象，并且，當(dāng)該工件W的當(dāng)前位置不包含在追隨區(qū)域內(nèi)時(shí)，不將該工件W檢測(cè)為作業(yè)對(duì)象。然后，將檢測(cè)到的結(jié)果傳送給用戶程序執(zhí)行部52和指令值生成部55。指令值生成部55基于被輸入的各種數(shù)據(jù)，生成用于使機(jī)械手12移動(dòng)至目標(biāo)位置的軌道，并且計(jì)算用于使機(jī)械手12追隨該軌道的各控制軸Cl C4的關(guān)節(jié)角度。指令值生成部55將用于使機(jī)械手12沿著軌道的各控制軸Cl C4的關(guān)節(jié)角度作為目標(biāo)關(guān)節(jié)角度輸出給馬達(dá)控制部57。在本實(shí)施方式中，馬達(dá)控制部57以控制周期T控制I個(gè)馬達(dá)，并且以不同的定時(shí)控制各個(gè)馬達(dá)。本實(shí)施方式的馬達(dá)控制部57以控制周期T除以控制軸數(shù)n(n為I以上的整數(shù)，在本實(shí)施方式中n = 4)得到的周期T/n，對(duì)各馬達(dá)23、27、30、31依次進(jìn)行控制，并且以周期T/n依次輸出由指令值生成部55輸出的與各馬達(dá)的目標(biāo)關(guān)節(jié)角度相應(yīng)的輸出。即，控制器50以周期T/n按照第I馬達(dá)23、第2馬達(dá)27、升降馬達(dá)30、旋轉(zhuǎn)馬達(dá)31的順序切換成為控制對(duì)象的馬達(dá)。并且，控制器50具有工件位置取得部58，其取得由檢測(cè)傳感器35檢測(cè)出工件W時(shí)的該工件W的位置；以及位置算出部60，其算出該檢測(cè)時(shí)的機(jī)械手12的位置。工件位置取得部58通過(guò)對(duì)從照相機(jī)39輸入的圖像數(shù)據(jù)進(jìn)行圖像處理來(lái)取得工件W的位置數(shù)據(jù)，該照相機(jī)39從下方拍攝到達(dá)了檢測(cè)線FL的工件W。工件位置取得部58通過(guò)上述圖像處理，求出拍攝范圍中的工件W的中心位置的坐標(biāo)值，并輸出給用戶程序執(zhí)行部52。照相機(jī)39與作為指令輸出部的觸發(fā)信號(hào)生成部59電連接，若從該觸發(fā)信號(hào)生成部59被輸入作為指令的觸發(fā)信號(hào)，則在其攝像范圍進(jìn)行拍攝，并將其拍攝到的圖像的圖像數(shù)據(jù)輸出給工件位置取得部58。若由檢測(cè)傳感器35檢測(cè)到工件W，則觸發(fā)信號(hào)生成部59生成作為指令的觸發(fā)信號(hào)。觸發(fā)信號(hào)生成部59將其生成的觸發(fā)信號(hào)同時(shí)輸出給照相機(jī)39和后述的觸發(fā)信號(hào)輸入部61。下面，對(duì)作為機(jī)械手位置檢測(cè)裝置以及機(jī)械手位置檢測(cè)部的位置算出部60進(jìn)行詳細(xì)說(shuō)明。如圖3所示，位置算出部60具有作為指令檢測(cè)部的觸發(fā)信號(hào)輸入部61、位置數(shù)據(jù)輸入部62和數(shù)據(jù)處理部63。觸發(fā)信號(hào)輸入部61被輸入觸發(fā)信號(hào)生成部59所輸出的觸發(fā)信號(hào)。觸發(fā)信號(hào)輸入部61以觸發(fā)信號(hào)的檢測(cè)周期檢測(cè)觸發(fā)信號(hào)的狀態(tài)，在數(shù)據(jù)處理部63中，以馬達(dá)的控制周期T除以控制軸數(shù)n (在本實(shí)施方式中n = 4)而得到的檢測(cè)周期Ts ( = T/n)檢測(cè)觸發(fā)信號(hào)輸 入部61所檢測(cè)到的觸發(fā)信號(hào)的狀態(tài)。位置數(shù)據(jù)輸入部62與基于各馬達(dá)23、27、30、31的絕對(duì)位置來(lái)檢測(cè)各控制軸Cl、C2、C3、C4的關(guān)節(jié)角度的第I編碼器64、第2編碼器65、第3編碼器66、第4編碼器67連接。各編碼器64 67每隔馬達(dá)的控制周期T的一半時(shí)間(=T/2)、即編碼器和位置數(shù)據(jù)輸入部62之間的通信所需的通信時(shí)間Td，檢測(cè)所對(duì)應(yīng)的關(guān)節(jié)角度，并將其檢測(cè)到的表示關(guān)節(jié)角度的位置數(shù)據(jù)輸出給位置數(shù)據(jù)輸入部62。另外，位置數(shù)據(jù)輸入部62具有判定被輸入的位置數(shù)據(jù)是否正常的判定部68。而且，位置數(shù)據(jù)輸入部62使從各編碼器64 67輸入的位置數(shù)據(jù)與判定部68針對(duì)該位置數(shù)據(jù)的判定結(jié)果相對(duì)應(yīng)。數(shù)據(jù)處理部63取得被輸入給位置數(shù)據(jù)輸入部62的位置數(shù)據(jù)和與該位置數(shù)據(jù)對(duì)應(yīng)的判定結(jié)果。其中，如果數(shù)據(jù)處理部63從位置數(shù)據(jù)輸入部62同時(shí)取得與各編碼器對(duì)應(yīng)的位置數(shù)據(jù)，則需要將數(shù)據(jù)處理部63構(gòu)成為與各位置數(shù)據(jù)對(duì)應(yīng)，存在數(shù)據(jù)處理部63的結(jié)構(gòu)變復(fù)雜的問(wèn)題。鑒于此，數(shù)據(jù)處理部63中內(nèi)置有每隔周期T/n便加一的計(jì)數(shù)器，即表示與成為控制對(duì)象的控制軸對(duì)應(yīng)的計(jì)數(shù)值的計(jì)數(shù)器。而且，如圖4所示，數(shù)據(jù)處理部63每隔控制周期T便依次取得來(lái)自各編碼器64 67的位置數(shù)據(jù)，以使該計(jì)數(shù)器的計(jì)數(shù)值所表示的控制軸、和與被輸入到位置數(shù)據(jù)輸入部62的位置數(shù)據(jù)對(duì)應(yīng)的控制軸匹配。其中，來(lái)自編碼器的位置數(shù)據(jù)通過(guò)通信時(shí)間Td的串行通信，被輸入給位置數(shù)據(jù)輸入部62。因此，數(shù)據(jù)處理部63所取得的位置數(shù)據(jù)是表示在比取得該位置數(shù)據(jù)的定時(shí)提前通信時(shí)間Td的定時(shí)，由編碼器檢測(cè)到的位置的位置數(shù)據(jù)。如圖5所示，數(shù)據(jù)處理部63具有存儲(chǔ)部70，該存儲(chǔ)部70中預(yù)先設(shè)置有存儲(chǔ)與控制軸Cl的關(guān)節(jié)角度、控制軸C2的關(guān)節(jié)角度、控制軸C3的關(guān)節(jié)角度、控制軸C4的關(guān)節(jié)角度相關(guān)的各種數(shù)據(jù)的區(qū)域。在本實(shí)施方式中，將來(lái)自編碼器的位置數(shù)據(jù)被輸入給位置數(shù)據(jù)輸入部62的定時(shí)中、檢測(cè)到觸發(fā)信號(hào)的下一定時(shí)作為該編碼器的基準(zhǔn)定時(shí)。存儲(chǔ)部70中按照控制軸Cl C4每一個(gè)設(shè)置有對(duì)比上述基準(zhǔn)定時(shí)靠前取得的位置數(shù)據(jù)、即第I位置進(jìn)行存儲(chǔ)的區(qū)域。第I位置73a是被從第I編碼器64輸入的位置數(shù)據(jù)。第I位置73b是被從第2編碼器65輸入的位置數(shù)據(jù)。第I位置73c是被從第3編碼器66輸入的位置數(shù)據(jù)。第I位置73d是從被第4編碼器67輸入的位置數(shù)據(jù)。而且，存儲(chǔ)器70中按照控制軸Cl C4每一個(gè)設(shè)置有對(duì)比上述基準(zhǔn)定時(shí)靠后取得的位置數(shù)據(jù)、即第2位置進(jìn)行存儲(chǔ)的區(qū)域。第2位置74a是被從第I編碼器64輸入的位置數(shù)據(jù)。第2位置74b是被從第2編碼器65輸入的位置數(shù)據(jù)。第2位置74c是被從第3編碼器66輸入的位置數(shù)據(jù)。第2位置74d是被從第4編碼器輸入的位置數(shù)據(jù)。另外，在存儲(chǔ)部70中按照控制軸Cl C4每一個(gè)設(shè)置有存儲(chǔ)對(duì)于表示第I位置的位置數(shù)據(jù)，表示該位置數(shù)據(jù)是否正常的第I標(biāo)志的區(qū)域。第I標(biāo)志75a與被從第I編碼器64輸入的位置數(shù)據(jù)對(duì)應(yīng)。第I標(biāo)志75b與被從第2編碼器65輸入的位置數(shù)據(jù)對(duì)應(yīng)。第I標(biāo)志75c與被從第3編碼器66輸入的位置數(shù)據(jù)對(duì)應(yīng)。第I標(biāo)志75d與被從第4編碼器67輸入的位置數(shù)據(jù)對(duì)應(yīng)。另外，在存儲(chǔ)部70中按照控制軸Cl C4每一個(gè)設(shè)置有存儲(chǔ)對(duì)于表示第2位置的位置數(shù)據(jù)，表示該位置數(shù)據(jù)是否正常的第2標(biāo)志的區(qū)域。第2標(biāo)志76a與被從第I編碼器64輸入的位置數(shù)據(jù)對(duì)應(yīng)。第2標(biāo)志76b與被從第2編碼器65輸入的位置數(shù)據(jù)對(duì)應(yīng)。第2標(biāo)志76c與和第3編碼器66相關(guān)的位置數(shù)據(jù)對(duì)應(yīng)。第2標(biāo)志76d與被從第4編碼器67輸入的位置數(shù)據(jù)對(duì)應(yīng)。 而且，數(shù)據(jù)處理部63每當(dāng)取得位置數(shù)據(jù)時(shí)，便判斷該位置數(shù)據(jù)是否正確。此時(shí)，若在比上述基準(zhǔn)定時(shí)靠前的定時(shí)判斷為位置數(shù)據(jù)正常，則數(shù)據(jù)處理部63將該位置數(shù)據(jù)更新為第I位置，且將第I標(biāo)志設(shè)定為“O”。另一方面，若在比上述基準(zhǔn)定時(shí)靠前的定時(shí)判斷為位置數(shù)據(jù)異常，則數(shù)據(jù)處理部63不更新第I位置，將第I標(biāo)志設(shè)定為“I”。其中，數(shù)據(jù)處理部63針對(duì)控制軸Cl C4依次執(zhí)行上述的第I位置的更新以及第I標(biāo)志的設(shè)定，直到檢測(cè)到觸發(fā)信號(hào)為止。然后，若檢測(cè)到觸發(fā)信號(hào)，則數(shù)據(jù)處理部63將該檢測(cè)時(shí)的各第I位置以及第I標(biāo)志作為第I定時(shí)數(shù)據(jù)77保存到存儲(chǔ)部70的規(guī)定區(qū)域。另外，若在上述基準(zhǔn)定時(shí)之后的定時(shí)判斷為位置數(shù)據(jù)正常，則數(shù)據(jù)處理部63將該位置數(shù)據(jù)作為第2位置進(jìn)行更新，并將第2標(biāo)志設(shè)定為“O”。另一方面，若在上述基準(zhǔn)定時(shí)之后的定時(shí)判斷為位置數(shù)據(jù)異常，則數(shù)據(jù)處理部63不將該位置數(shù)據(jù)作為第2位置進(jìn)行更新，將第2標(biāo)志設(shè)定為“I”。然后，數(shù)據(jù)處理部63取得上述之后定時(shí)的下一定時(shí)的位置數(shù)據(jù)，無(wú)論該位置數(shù)據(jù)是否正常，都將該位置數(shù)據(jù)作為第2位置進(jìn)行更新。而且，若所有的第2位置全部被更新，則數(shù)據(jù)處理部63將各第2位置以及第2標(biāo)志作為第2定時(shí)數(shù)據(jù)78保存到存儲(chǔ)部70的規(guī)定區(qū)域。另外，在存儲(chǔ)部70中設(shè)置有對(duì)表示是否檢測(cè)到觸發(fā)信號(hào)的檢測(cè)標(biāo)志79進(jìn)行存儲(chǔ)的區(qū)域。若檢測(cè)到觸發(fā)信號(hào)，則數(shù)據(jù)處理部63將檢測(cè)標(biāo)志79設(shè)定為“ I ”，并且，若將算出的機(jī)械手12的位置輸出給用戶程序執(zhí)行部52，則將檢測(cè)標(biāo)志79設(shè)定為“O”。另外，如圖5所示，在存儲(chǔ)部70中設(shè)置有對(duì)表示檢測(cè)出觸發(fā)信號(hào)時(shí)作為控制對(duì)象的控制軸的數(shù)據(jù)、即控制軸數(shù)據(jù)80進(jìn)行存儲(chǔ)的區(qū)域。數(shù)據(jù)處理部63基于內(nèi)置的計(jì)數(shù)器的計(jì)數(shù)值，將表示檢測(cè)出觸發(fā)信號(hào)時(shí)的控制軸的數(shù)據(jù)保存為控制軸數(shù)據(jù)80。然后，每當(dāng)檢測(cè)到觸發(fā)信號(hào)時(shí)，數(shù)據(jù)處理部63便基于上述第I定時(shí)數(shù)據(jù)77、第2定時(shí)數(shù)據(jù)78、控制軸數(shù)據(jù)80，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的各控制軸Cl C4的關(guān)節(jié)角度，來(lái)檢測(cè)機(jī)械手12的位置。下面，參照?qǐng)D6對(duì)位置算出部60計(jì)算機(jī)械手12的位置的算出方法進(jìn)行說(shuō)明。其中，在圖6中將第I定時(shí)的關(guān)節(jié)角度、即第I位置表示為Pa，將第2定時(shí)的關(guān)節(jié)角度、即第2位置表示為Pb，將檢測(cè)出觸發(fā)信號(hào)時(shí)的關(guān)節(jié)角度表示為P (k)。而且，本實(shí)施方式中的通信時(shí)間Td為馬達(dá)的控制周期T的一半的時(shí)間(T/2)。若第2定時(shí)數(shù)據(jù)78被保存，則數(shù)據(jù)處理部63按照控制軸Cl C4每一個(gè)基于第I標(biāo)志與第2標(biāo)志的組合，從根據(jù)該組合而預(yù)先設(shè)定的4個(gè)算式中選擇I個(gè)算式。然后，通過(guò)將第I位置、第2位置和基于控制軸數(shù)據(jù)80的常數(shù)k代入到該選擇的算式，求出檢測(cè)出觸發(fā)信號(hào)時(shí)的各控制軸Cl C4的關(guān)節(jié)角度。這里所說(shuō)的常數(shù)k是基于成為算出對(duì)象的控制軸與檢測(cè)出觸發(fā)信號(hào)時(shí)成為控制對(duì)象的控制軸之間的關(guān)系，按每個(gè)控制軸決定的常數(shù)，以周期T/n為基準(zhǔn)單位，是以“T/nXk”表示檢測(cè)到觸發(fā)信號(hào)的定時(shí)至基準(zhǔn)定時(shí)為止的期間的常數(shù)。例如，在檢測(cè)出觸發(fā)信號(hào)時(shí)的控制對(duì)象為控制軸Cl (第I馬達(dá)23)的情況下，如果算出對(duì)象是控制軸C2(第2馬達(dá)27)，則由于比與第2編碼器65相關(guān)的基準(zhǔn)定時(shí)提前“周期T/n”檢測(cè)到觸發(fā)信號(hào)，所以常數(shù)k為“I”。另外，如果算出對(duì)象是控制軸C3(升降馬達(dá)30)，則由于比第3編碼器66的基準(zhǔn)定時(shí)提前“2X周期T/n”檢測(cè)到觸發(fā)信號(hào)，所以常數(shù)k為“2”。另外，如果算出對(duì)象是控制軸C4(旋轉(zhuǎn)馬達(dá)31)，則由于比基準(zhǔn)定時(shí)提前“3X周期T/n”檢測(cè)到觸發(fā)信號(hào)，所以常數(shù)k為“3”。另外，如果算出對(duì)象是控制軸Cl (第I馬達(dá)23)，則由于比基準(zhǔn)定時(shí)提前控制周期T檢測(cè)到觸發(fā)信號(hào)，所以常數(shù)k為“4”。在成為算出對(duì)象的控制軸的第I標(biāo)志以及第2標(biāo)志均為“0”的情況下，即在第I定時(shí)與基準(zhǔn)定時(shí)間隔了“控制周期T”、基準(zhǔn)定時(shí)與第2定時(shí)間隔了“控制周期T”的情況下， 數(shù)據(jù)處理部63利用以下所示的式(I)，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的該控制軸的關(guān)節(jié)角度。式I
P Zj _ PnrTP⑷= Pa +-x(Td + Tk)
T^Tn
r n D Vb-Pa m 2k, Z1 \=Pa +-x(3--) ... (I)
4n另外，在成為算出對(duì)象的控制軸的第I標(biāo)志為“0”、第2標(biāo)志為“I”的情況下，即在第I定時(shí)與基準(zhǔn)定時(shí)間隔了“控制周期T”、基準(zhǔn)定時(shí)與第2定時(shí)間隔了“2X控制周期T”的情況下，數(shù)據(jù)處理部63利用以下所示的式(2)，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的該控制軸的關(guān)節(jié)角度。式2
pA _ pnrTP(々) = Pa +-x(Td + T — —k)
V ；T + 2Tn
r n D Vb-Pa 2k、 /rt\=PaH--x (3--) (2)
6n另外，在成為算出對(duì)象的控制軸的第I標(biāo)志為“I”、第2標(biāo)志為“0”的情況下，即在第I定時(shí)與基準(zhǔn)定時(shí)間隔了“2X控制周期T”、基準(zhǔn)定時(shí)與第2定時(shí)間隔了“控制周期T”的情況下，數(shù)據(jù)處理部63利用以下所示的式(3)，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的該控制軸的關(guān)節(jié)角度。式3
pA _ pnrTP(々)= Pa +-X (Td + 2T--k)
v ;2T + Tn
r n ^ Vb-Pa .r 2k, /0\=PaH--x (5--) ... (3)
6n另外，在成為算出對(duì)象的控制軸的第I標(biāo)志和第2標(biāo)志均為“I”的情況下，即在第I定時(shí)與基準(zhǔn)定時(shí)間隔了 “2X控制周期T”、基準(zhǔn)定時(shí)與第2定時(shí)間隔了 “2X控制周期T”的情況下，數(shù)據(jù)處理部63利用以下所示的式(4)，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的該控制軸的關(guān)節(jié)角度。式4
權(quán)利要求
1.一種機(jī)械手位置檢測(cè)裝置，其特征在于，具備 位置數(shù)據(jù)取得部，其取得表示實(shí)際的機(jī)械手的位置的位置數(shù)據(jù)； 位置數(shù)據(jù)輸入部，其被輸入從上述位置數(shù)據(jù)取得部輸出的上述位置數(shù)據(jù)；和位置算出部，其通過(guò)線性插值來(lái)算出計(jì)算上的機(jī)械手的位置，該線性插值使用了在不同的定時(shí)被輸入給上述位置數(shù)據(jù)輸入部的第I以及第2位置數(shù)據(jù)。
2.根據(jù)權(quán)利要求I所述的機(jī)械手位置檢測(cè)裝置，其特征在于， 上述位置數(shù)據(jù)取得部是設(shè)置于上述機(jī)械手的編碼器， 上述位置算出部接受用于檢測(cè)上述機(jī)械手的位置的指令，算出上述計(jì)算上的機(jī)械手的位置， 上述第I位置數(shù)據(jù)是在上述指令之前的定時(shí)取得的數(shù)據(jù)， 上述第2位置數(shù)據(jù)是在上述指令之后的定時(shí)取得的數(shù)據(jù)。
3.根據(jù)權(quán)利要求2所述的機(jī)械手位置檢測(cè)裝置，其特征在于， 上述位置算出部具備指令檢測(cè)部，該指令檢測(cè)部能夠以比取得從I個(gè)編碼器輸入給上述位置數(shù)據(jù)輸入部的上述位置數(shù)據(jù)的規(guī)定周期T短的周期Ts，檢測(cè)上述指令。
4.根據(jù)權(quán)利要求3所述的機(jī)械手位置檢測(cè)裝置，其特征在于， 上述位置算出部按照每個(gè)周期T/n，取得來(lái)自相互不同的編碼器的位置數(shù)據(jù)，并按照每一個(gè)上述編碼器輸出被輸入上述指令時(shí)的上述機(jī)械手的位置，所述周期T/n是將上述規(guī)定周期T以I以上的整數(shù)n進(jìn)行n等分而得到的周期。
5.根據(jù)權(quán)利要求I 4中任意一項(xiàng)所述的機(jī)械手位置檢測(cè)裝置，其特征在于， 上述位置算出部對(duì)所取得的上述位置數(shù)據(jù)是否正常進(jìn)行判斷， 在判斷為檢測(cè)到上述指令時(shí)的前一上述位置數(shù)據(jù)不正常的情況下，將在該位置數(shù)據(jù)之前取得的前一位置數(shù)據(jù)作為上述第I位置數(shù)據(jù)使用。
6.根據(jù)權(quán)利要求5所述的機(jī)械手位置檢測(cè)裝置，其特征在于， 上述位置算出部在判斷為緊接著檢測(cè)到上述指令后取得的上述位置數(shù)據(jù)不正常的情況下，將緊接著該位置數(shù)據(jù)后取得的位置數(shù)據(jù)作為上述第2位置數(shù)據(jù)使用。
7.一種機(jī)械手系統(tǒng)，其特征在于，具備 搬運(yùn)工件的機(jī)械手； 機(jī)械手控制器，其具備輸出用于檢測(cè)上述機(jī)械手的位置的指令的指令輸出部，使上述機(jī)械手動(dòng)作； 還具備權(quán)利要求I 6中任意一項(xiàng)所述的機(jī)械手位置檢測(cè)裝置。
8.根據(jù)權(quán)利要求7所述的機(jī)械手系統(tǒng)，其特征在于，還具備 照相機(jī)，其通過(guò)在被輸入來(lái)自上述指令輸出部的上述指令的定時(shí)，對(duì)預(yù)定的拍攝范圍進(jìn)行拍攝，來(lái)取得包含向目標(biāo)位置移動(dòng)過(guò)程中的上述機(jī)械手所搬運(yùn)的工件的圖像； 工件位置取得部，其基于上述照相機(jī)所取得的圖像，取得上述工件的位置；和目標(biāo)位置修正部，其基于上述工件位置取得部取得的上述工件的位置、和由上述機(jī)械手位置檢測(cè)裝置算出的上述計(jì)算上的機(jī)械手的位置，對(duì)上述目標(biāo)位置進(jìn)行修正。
全文摘要
本發(fā)明涉及一種檢測(cè)機(jī)械手位置的機(jī)械手位置檢測(cè)裝置以及具備該機(jī)械手位置檢測(cè)裝置的機(jī)械手系統(tǒng)。位置算出部具備觸發(fā)信號(hào)輸入部，被輸入觸發(fā)信號(hào)；位置數(shù)據(jù)輸入部，延遲通信時(shí)間地被輸入從編碼器輸出的位置數(shù)據(jù)；數(shù)據(jù)處理部，取得被輸入的位置數(shù)據(jù)，算出檢測(cè)出觸發(fā)信號(hào)時(shí)的機(jī)械手的位置。數(shù)據(jù)處理部使用檢測(cè)觸發(fā)信號(hào)前所取得的第1位置數(shù)據(jù)和其定時(shí)、以及檢測(cè)到觸發(fā)信號(hào)后的第2位置數(shù)據(jù)和其定時(shí)，對(duì)機(jī)械手的位置進(jìn)行線性插值，算出與被輸入觸發(fā)信號(hào)的定時(shí)相比滯后通信時(shí)間的位置，作為檢測(cè)出觸發(fā)信號(hào)時(shí)的機(jī)械手的位置。
文檔編號(hào)B25J9/16GK102653099SQ20121004144
公開(kāi)日2012年9月5日 申請(qǐng)日期2012年2月21日 優(yōu)先權(quán)日2011年3月4日
發(fā)明者淺田篤 申請(qǐng)人:精工愛(ài)普生株式會(huì)社