本發(fā)明的實(shí)施方式涉及一種機(jī)械臂機(jī)構(gòu)及步進(jìn)馬達(dá)控制裝置。

背景技術(shù)：

步進(jìn)馬達(dá)作為例如伺服馬達(dá)而廣泛使用于機(jī)器人等。步進(jìn)馬達(dá)如果過(guò)負(fù)荷、或產(chǎn)生突然的速度變化則會(huì)失調(diào)。失調(diào)是指輸入至步進(jìn)馬達(dá)的脈沖信號(hào)與實(shí)際的馬達(dá)旋轉(zhuǎn)之間不同步的現(xiàn)象。如果產(chǎn)生失調(diào)，步進(jìn)馬達(dá)則不會(huì)旋轉(zhuǎn)至目標(biāo)位置。因此，首先需要檢測(cè)失調(diào)的產(chǎn)生。檢測(cè)失調(diào)的方法有如下方法：在步進(jìn)馬達(dá)設(shè)置編碼器，對(duì)步進(jìn)馬達(dá)進(jìn)行反饋控制。根據(jù)該方法，可基于編碼器的輸出，來(lái)指定步進(jìn)馬達(dá)的當(dāng)前位置，從而可檢測(cè)目標(biāo)位置與當(dāng)前位置之間的不一致作為失調(diào)。一般而言，如果檢測(cè)到步進(jìn)馬達(dá)的失調(diào)，立即進(jìn)行停止步進(jìn)馬達(dá)的控制。然而，有時(shí)在例如能立即去除失調(diào)的要因的情形時(shí)，即便產(chǎn)生失調(diào)也可不停止步進(jìn)馬達(dá)而直接繼續(xù)進(jìn)行控制。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種失調(diào)檢測(cè)后也可使步進(jìn)馬達(dá)適宜地進(jìn)行動(dòng)作的控制。

本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)具有機(jī)械臂、步進(jìn)馬達(dá)、馬達(dá)驅(qū)動(dòng)器、編碼器及失調(diào)檢測(cè)部。機(jī)械臂具有關(guān)節(jié)部。步進(jìn)馬達(dá)產(chǎn)生使關(guān)節(jié)部運(yùn)轉(zhuǎn)的動(dòng)力。馬達(dá)驅(qū)動(dòng)器依照目標(biāo)角度驅(qū)動(dòng)步進(jìn)馬達(dá)。編碼器在步進(jìn)馬達(dá)的驅(qū)動(dòng)軸每旋轉(zhuǎn)既定角度時(shí)輸出編碼器脈沖。失調(diào)檢測(cè)部基于根據(jù)編碼器脈沖特定的步進(jìn)馬達(dá)的當(dāng)前角度與目標(biāo)角度，檢測(cè)步進(jìn)馬達(dá)的失調(diào)。馬達(dá)驅(qū)動(dòng)器在從檢測(cè)到失調(diào)起經(jīng)過(guò)既定的推遲時(shí)間之前，失調(diào)未被恢復(fù)時(shí)，在推遲時(shí)間的經(jīng)過(guò)的時(shí)間點(diǎn)停止步進(jìn)馬達(dá)的驅(qū)動(dòng)。

附圖說(shuō)明

圖1是本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的外觀立體圖；

圖2是表示圖1的機(jī)械臂機(jī)構(gòu)的內(nèi)部構(gòu)造的立體圖；

圖3是從剖面方向觀察圖1的機(jī)械臂機(jī)構(gòu)的內(nèi)部構(gòu)造的圖；

圖4是表示本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的構(gòu)成的一個(gè)例子的框結(jié)構(gòu)圖；

圖5是用于說(shuō)明本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部的動(dòng)作的流程圖；

圖6是用于說(shuō)明本實(shí)施方式的變形例1所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部的動(dòng)作的流程圖；

圖7是用于說(shuō)明本實(shí)施方式的變形例2所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部的動(dòng)作的流程圖。

具體實(shí)施方式

以下，參照附圖，對(duì)本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)進(jìn)行說(shuō)明。在以下的說(shuō)明中，對(duì)具有大致相同功能及構(gòu)成的構(gòu)成要素標(biāo)注相同符號(hào)，且僅在必要的情形時(shí)進(jìn)行重復(fù)說(shuō)明。

圖1是本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的外觀立體圖。圖2、圖3是表示圖1的機(jī)械臂機(jī)構(gòu)的內(nèi)部構(gòu)造。機(jī)械臂機(jī)構(gòu)具有大致圓筒形狀的基部1及與基部1連接的臂部2。在臂部2的前端安裝有被稱為末端執(zhí)行器的末端效應(yīng)器3。圖1中圖示把可把持住對(duì)象物的手部作為末端效應(yīng)器3。末端效應(yīng)器3并不限定于手部，也可為其他工具、相機(jī)、顯示器。也可在臂部2的前端設(shè)置可更換為任意種類的末端效應(yīng)器3的轉(zhuǎn)接器。

臂部2具有多個(gè)、此處為6個(gè)關(guān)節(jié)部j1、j2、j3、j4、j5、j6。多個(gè)關(guān)節(jié)部j1、j2、j3、j4、j5、j6從基部1依序配設(shè)。一般而言，第1、第2、第3軸ra1、ra2、ra3被稱為根部3軸，第4、第5、第6軸ra4、ra5、ra6被稱為改變手部3的姿勢(shì)的腕部3軸。構(gòu)成根部3軸的關(guān)節(jié)部j1、j2、j3的至少一個(gè)為直動(dòng)關(guān)節(jié)。這里，第3關(guān)節(jié)部j3構(gòu)成為直動(dòng)關(guān)節(jié)、尤其是伸縮距離相對(duì)較長(zhǎng)的關(guān)節(jié)部。第1關(guān)節(jié)部j1是以例如被基座面垂直支撐的第1旋轉(zhuǎn)軸ra1為中心的扭轉(zhuǎn)關(guān)節(jié)。第2關(guān)節(jié)部j2是以與第1旋轉(zhuǎn)軸ra1垂直配置的第2旋轉(zhuǎn)軸ra2為中心的彎曲關(guān)節(jié)。第3關(guān)節(jié)部j3是以與第2旋轉(zhuǎn)軸ra2垂直配置的第3軸(移動(dòng)軸)ra3為中心而直線伸縮的關(guān)節(jié)。第4關(guān)節(jié)部j4是以與第3移動(dòng)軸ra3一致的第4旋轉(zhuǎn)軸ra4為中心的扭轉(zhuǎn)關(guān)節(jié)，第5關(guān)節(jié)部j5是以與第4旋轉(zhuǎn)軸ra4正交的第5旋轉(zhuǎn)軸ra5為中心的彎曲關(guān)節(jié)。第6關(guān)節(jié)部j6是以與第4旋轉(zhuǎn)軸ra4正交、且與第5旋轉(zhuǎn)軸ra5垂直配置的第6旋轉(zhuǎn)軸ra6為中心的彎曲關(guān)節(jié)。

通過(guò)第1關(guān)節(jié)部j1的扭動(dòng)旋轉(zhuǎn)，臂部2與手部3一起旋轉(zhuǎn)。如圖3所示，步進(jìn)馬達(dá)31a設(shè)在第1關(guān)節(jié)部j1的附近。當(dāng)步進(jìn)馬達(dá)31a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)31a的驅(qū)動(dòng)軸的動(dòng)力經(jīng)由未圖示的齒輪等而傳遞至第1關(guān)節(jié)部j1。由此，第1關(guān)節(jié)部j1旋轉(zhuǎn)，臂部2與手部3一起旋轉(zhuǎn)。

通過(guò)第2關(guān)節(jié)部j2的彎曲旋轉(zhuǎn)，臂部2與手部3一起以第2關(guān)節(jié)部j2的第2旋轉(zhuǎn)軸ra2為中心進(jìn)行起伏移動(dòng)。如圖3所示，步進(jìn)馬達(dá)32a設(shè)在第2關(guān)節(jié)部j2的附近。當(dāng)步進(jìn)馬達(dá)32a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)32a的驅(qū)動(dòng)軸的動(dòng)力例如直接傳遞至第2關(guān)節(jié)部j2。由此，臂部2與手部3一起以第2關(guān)節(jié)部j2的第2旋轉(zhuǎn)軸ra2為中心進(jìn)行起伏移動(dòng)。形成基部1的臂支撐體(第1支撐體)11a具有以第1關(guān)節(jié)部j1的第1旋轉(zhuǎn)軸ra1為中心形成的圓筒形狀的中空構(gòu)造。第1關(guān)節(jié)部j1安裝在未圖示的固定臺(tái)。當(dāng)?shù)?關(guān)節(jié)部j1旋轉(zhuǎn)時(shí)，第1支撐體11a與臂部2的旋轉(zhuǎn)一起進(jìn)行軸旋轉(zhuǎn)。另外，第1支撐體11a也可固定在接地層。在此情形時(shí)，設(shè)為與第1支撐體11a分開(kāi)而臂部2獨(dú)立旋轉(zhuǎn)的構(gòu)造。在第1支撐體11a的上部連接有第2支撐體11b。

第2支撐體11b具有與第1支撐體11a連續(xù)的中空構(gòu)造。第2支撐體11b的一端是安裝在第1關(guān)節(jié)部j1的旋轉(zhuǎn)部。第2支撐體11b的另一端開(kāi)放，第3支撐體11c在第2關(guān)節(jié)部j2的第2旋轉(zhuǎn)軸ra2上自由旋轉(zhuǎn)地嵌入。第3支撐體11c具有與第1支撐體11a及第2支撐體連通的鱗片狀的中空構(gòu)造。第3支撐體11c伴隨第2關(guān)節(jié)部j2的彎曲旋轉(zhuǎn)而其后部收納或送出至第2支撐體11b。構(gòu)成臂部2的直動(dòng)關(guān)節(jié)部的第3關(guān)節(jié)部j3的后部是通過(guò)其收縮而被收納在第1支撐體11a與第2支撐體11b連續(xù)的中空構(gòu)造的內(nèi)部。

第1關(guān)節(jié)部j1包含圓環(huán)形狀的固定部及旋轉(zhuǎn)部，固定部固定在基座。在旋轉(zhuǎn)部安裝有第1支撐體11a及第2支撐體11b。當(dāng)?shù)?關(guān)節(jié)部j1旋轉(zhuǎn)時(shí)，第1、第2、第3支撐體11a、11b、11c以第1旋轉(zhuǎn)軸ra1為中心而與臂部2及手部3一起旋轉(zhuǎn)。

第3支撐體11c是其后端下部以第2旋轉(zhuǎn)軸ra2為中心自由旋轉(zhuǎn)地嵌入至第2支撐體11b的開(kāi)放端下部。由此，構(gòu)成作為以第2旋轉(zhuǎn)軸ra2為中心的彎曲關(guān)節(jié)部的第2關(guān)節(jié)部j2。當(dāng)?shù)?關(guān)節(jié)部j2轉(zhuǎn)動(dòng)時(shí)，臂部2與手部3一起以第2關(guān)節(jié)部j2的第2旋轉(zhuǎn)軸ra2為中心朝垂直方向轉(zhuǎn)動(dòng)、即起伏動(dòng)作。第2關(guān)節(jié)部j2的第2旋轉(zhuǎn)軸ra2設(shè)為與作為扭轉(zhuǎn)關(guān)節(jié)部的第1關(guān)節(jié)部j1的第1旋轉(zhuǎn)軸ra1垂直。

如上所述，作為關(guān)節(jié)部的第3關(guān)節(jié)部j3構(gòu)成臂部2的主要構(gòu)成部分。在臂部2的前端設(shè)有所述手部3。通過(guò)第1至第6關(guān)節(jié)部j1-j6的轉(zhuǎn)動(dòng)、彎曲、伸縮，可將手部3的2指手16配置為任意的位置·姿勢(shì)。尤其是，第3關(guān)節(jié)部j3的直動(dòng)伸縮距離的長(zhǎng)度可實(shí)現(xiàn)手部3對(duì)基部1的附近位置至遠(yuǎn)距離位置的大范圍的對(duì)象的作用。

第3關(guān)節(jié)部j3的特征為通過(guò)構(gòu)成的直動(dòng)伸縮臂機(jī)構(gòu)而實(shí)現(xiàn)直動(dòng)伸縮距離的長(zhǎng)度。直動(dòng)伸縮距離的長(zhǎng)度通過(guò)圖2、圖3所示的構(gòu)造達(dá)成。直動(dòng)伸縮臂機(jī)構(gòu)具有第1連結(jié)鏈節(jié)排21及第2連結(jié)鏈節(jié)排20。在臂部2水平配置的基準(zhǔn)姿勢(shì)下，第1連結(jié)鏈節(jié)排21位于第2連結(jié)鏈節(jié)排20的下部，第2連結(jié)鏈節(jié)排20位于第1連結(jié)鏈節(jié)排21的上部。

第1連結(jié)鏈節(jié)排21具有相同的剖面コ字形狀，包含通過(guò)銷而在背面部位排狀連結(jié)的多個(gè)第1連結(jié)鏈節(jié)23。具備如下性質(zhì)：因利用第1連結(jié)鏈節(jié)23的剖面形狀及銷實(shí)現(xiàn)的鏈接位置，而第1鏈接鏈節(jié)排21可向其背面方向bd彎曲但無(wú)法反過(guò)來(lái)朝表面方向fd彎曲。第2連結(jié)鏈節(jié)排20具有寬度與第1連結(jié)鏈節(jié)23大致相等的大致平板形狀，包含以在背面方向及表面方向均可彎曲的狀態(tài)利用銷排狀連結(jié)的多個(gè)第2連結(jié)鏈節(jié)22。第1連結(jié)鏈節(jié)排21是通過(guò)結(jié)合鏈節(jié)26而與第2連結(jié)鏈節(jié)排20在前端部結(jié)合。結(jié)合鏈節(jié)26具有第1連結(jié)鏈節(jié)23與第2連結(jié)鏈節(jié)22成為一體的形狀。結(jié)合鏈節(jié)26為始端，第2連結(jié)鏈節(jié)排20與第1連結(jié)鏈節(jié)排21同時(shí)從第3支撐體11c送出時(shí)，第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20相互接合。第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20是通過(guò)結(jié)合鏈節(jié)26而在前端部結(jié)合，各自的后部被保持在第3支持體11c的內(nèi)部防止脫出，由此保持接合狀態(tài)。當(dāng)?shù)?連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20保持接合狀態(tài)時(shí)，第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20的彎曲被限制，由此由第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20構(gòu)成具備一定剛性的柱狀體。當(dāng)?shù)?連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20相互分離時(shí)，彎曲限制被解除，分別恢復(fù)成可彎曲的狀態(tài)。第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20在第3支撐體11c的開(kāi)口附近接合并被送出。第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20在第3支撐體11c的內(nèi)部分離，分別變成可彎曲的狀態(tài)。第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20單獨(dú)地彎曲，且在第1支撐持體11a的內(nèi)部單獨(dú)地收容。

如圖2所示，在第2連結(jié)鏈節(jié)22的內(nèi)側(cè)分別形成線性齒輪22a。相鄰的線性齒輪22a在第2連結(jié)鏈節(jié)排20成直線狀時(shí)連結(jié)，而構(gòu)成連續(xù)的線性齒輪。如圖3所示，第2連結(jié)鏈節(jié)22在第3支撐體11c內(nèi)被夾持在輥r1與驅(qū)動(dòng)齒輪24a之間。線性齒輪22a嚙合在驅(qū)動(dòng)齒輪24a。如圖3所示，步進(jìn)馬達(dá)33a設(shè)在第2關(guān)節(jié)部j2的附近。當(dāng)步進(jìn)馬達(dá)33a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)33a的驅(qū)動(dòng)軸的動(dòng)力傳遞至驅(qū)動(dòng)齒輪24a。驅(qū)動(dòng)齒輪24a通過(guò)步進(jìn)馬達(dá)33a而正向旋轉(zhuǎn)，由此第2連結(jié)鏈節(jié)排20與第1連結(jié)鏈節(jié)排21一起從第3支撐體11c被送出。這時(shí)，第1連結(jié)鏈節(jié)排21與第2連結(jié)鏈節(jié)排20被設(shè)在第3支持體11c的開(kāi)口附近的一對(duì)上下輥r2、r4夾持，相互擠壓而接合，在該狀態(tài)下沿著第3移動(dòng)軸ra3直線地被送出。驅(qū)動(dòng)齒輪24a通過(guò)步進(jìn)馬達(dá)33a而逆向旋轉(zhuǎn)，由此第2連結(jié)鏈節(jié)排20與第1連結(jié)鏈節(jié)排21在第3支撐體11c的內(nèi)部在上下輥r2、r4的后方解除接合狀態(tài)而相互分離。分離后的第2連結(jié)鏈節(jié)排20與第1連結(jié)鏈節(jié)排21分別恢復(fù)為可彎曲的狀態(tài)，被設(shè)在第2、第3支撐體11b、11c的內(nèi)部的導(dǎo)軌引導(dǎo)，朝沿著第1旋轉(zhuǎn)軸ra1的方向彎曲，并收納至第1支撐體11a的內(nèi)部。

如圖1所示，手部3裝配在臂部2的前端。手部3通過(guò)第1、第2、第3關(guān)節(jié)部j1、j2、j3而移動(dòng)至任意位置，通過(guò)第4、第5、第6關(guān)節(jié)部j4、j5、j6而配置成任意姿勢(shì)。手部3具有開(kāi)閉的2個(gè)指部16a、16b。第4關(guān)節(jié)部j4是具有與沿著臂部2的伸縮方向的臂部2的中心軸、即第3關(guān)節(jié)部j3的移動(dòng)軸ra3典型一致的第4旋轉(zhuǎn)軸ra4的扭轉(zhuǎn)關(guān)節(jié)。如果第4關(guān)節(jié)部j4旋轉(zhuǎn)，則手部3從第4關(guān)節(jié)部j4到前端以第4旋轉(zhuǎn)軸ra4為中心旋轉(zhuǎn)。如圖3所示，步進(jìn)馬達(dá)34a設(shè)在第4關(guān)節(jié)部j4的附近。當(dāng)步進(jìn)馬達(dá)34a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)34a的驅(qū)動(dòng)軸的動(dòng)力例如直接傳遞至第4關(guān)節(jié)部j4。由此，手部3從第4關(guān)節(jié)部j4朝前端以第4關(guān)節(jié)部j4的第4旋轉(zhuǎn)軸ra4為中心旋轉(zhuǎn)。

第5關(guān)節(jié)部j5是具有與第4關(guān)節(jié)部j4的移動(dòng)軸ra4正交的第5旋轉(zhuǎn)軸ra5的彎曲關(guān)節(jié)部。如果第5關(guān)節(jié)部旋轉(zhuǎn)，則與手部16從第5關(guān)節(jié)部j5朝前端一起上下轉(zhuǎn)動(dòng)。如圖3所示，步進(jìn)馬達(dá)35a設(shè)在第5關(guān)節(jié)部j5的附近。當(dāng)步進(jìn)馬達(dá)35a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)35a的驅(qū)動(dòng)軸的動(dòng)力是經(jīng)由未圖示的齒輪等而傳遞至第5關(guān)節(jié)部j5。由此，手部16從第5關(guān)節(jié)部j5朝前端而上下轉(zhuǎn)動(dòng)。

第6關(guān)節(jié)部j6是具有與第4關(guān)節(jié)部j4的第4旋轉(zhuǎn)軸ra4正交、與第5關(guān)節(jié)部j5的第5旋轉(zhuǎn)軸ra5垂直的第6旋轉(zhuǎn)軸ra6的彎曲關(guān)節(jié)。如果第6關(guān)節(jié)部j6旋轉(zhuǎn)則手部16左右旋轉(zhuǎn)。如圖3所示，步進(jìn)馬達(dá)36a設(shè)在第6關(guān)節(jié)部j6的附近。當(dāng)步進(jìn)馬達(dá)36a旋轉(zhuǎn)時(shí)，步進(jìn)馬達(dá)36a的驅(qū)動(dòng)軸的動(dòng)力是經(jīng)由未圖示的齒輪等而傳遞至第6關(guān)節(jié)部j6。由此，手部16左右旋轉(zhuǎn)。

圖4是表示本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的構(gòu)成的一個(gè)例子的框結(jié)構(gòu)圖。另外，本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)具有關(guān)節(jié)部j1～j6。在關(guān)節(jié)部j1～j6分別設(shè)有馬達(dá)驅(qū)動(dòng)部31～36。馬達(dá)驅(qū)動(dòng)部31～36分別具有步進(jìn)馬達(dá)31a～36a。這里，僅對(duì)馬達(dá)驅(qū)動(dòng)部31進(jìn)行說(shuō)明。其他馬達(dá)驅(qū)動(dòng)部32～36的構(gòu)成及動(dòng)作與馬達(dá)驅(qū)動(dòng)部31的構(gòu)成及動(dòng)作相同。這里，省略其他馬達(dá)驅(qū)動(dòng)部32～36的說(shuō)明。機(jī)械臂機(jī)構(gòu)具有系統(tǒng)控制部41、電源供給部42、馬達(dá)控制部43、輸入部44、設(shè)定部45、及馬達(dá)驅(qū)動(dòng)部31。馬達(dá)控制部43與馬達(dá)驅(qū)動(dòng)部31構(gòu)成步進(jìn)馬達(dá)控制裝置。步進(jìn)馬達(dá)控制裝置可作為獨(dú)立裝置而應(yīng)用于使用步進(jìn)馬達(dá)的其他裝置、機(jī)構(gòu)。

系統(tǒng)控制部41具有cpu(centralprocessingunit)及半導(dǎo)體內(nèi)存等。系統(tǒng)控制部41將經(jīng)由輸入部44輸入至機(jī)械臂機(jī)構(gòu)的指示信息暫時(shí)記憶在半導(dǎo)體內(nèi)存。系統(tǒng)控制部41基于輸入的指示信息統(tǒng)括地控制機(jī)械臂機(jī)構(gòu)的各部。

電源供給部42從搭載在機(jī)械臂機(jī)構(gòu)的電池(未圖示)或外部的商用電源，產(chǎn)生用于供給至馬達(dá)驅(qū)動(dòng)器31b的電力。

馬達(dá)控制部43統(tǒng)括地控制馬達(dá)驅(qū)動(dòng)器31b。具體而言，馬達(dá)控制部43將與后述設(shè)定部45設(shè)定的條件相應(yīng)的信號(hào)輸出至馬達(dá)驅(qū)動(dòng)器31b及失調(diào)檢測(cè)部31e中的至少一方。馬達(dá)控制部43將與后述驅(qū)動(dòng)條件設(shè)定部452設(shè)定的脈沖條件相關(guān)的脈沖控制信號(hào)輸出至馬達(dá)驅(qū)動(dòng)器31b。馬達(dá)控制部43將與后述驅(qū)動(dòng)條件設(shè)定部452設(shè)定的最大驅(qū)動(dòng)電流值相關(guān)的電流控制信號(hào)輸出至馬達(dá)驅(qū)動(dòng)器31b。另外，馬達(dá)控制部43將與后述推遲時(shí)間設(shè)定部451設(shè)定的推遲時(shí)間相關(guān)的數(shù)據(jù)輸出至馬達(dá)驅(qū)動(dòng)器31b。

輸入部44是作為供使用者向機(jī)械臂機(jī)構(gòu)輸入指示信息的輸入接口發(fā)揮功能。輸入部44連接有操作裝置(未圖示)。例如，操作裝置具備用于指定關(guān)節(jié)部的開(kāi)關(guān)、用于指定使關(guān)節(jié)部旋轉(zhuǎn)或直動(dòng)的動(dòng)作速度、動(dòng)作方向及動(dòng)作量的操縱手柄等。另外，操作裝置也可作為用于設(shè)定后述脈沖條件、驅(qū)動(dòng)電流值及推遲時(shí)間等的輸入設(shè)備而使用。通過(guò)用戶對(duì)操作裝置進(jìn)行操作，而輸入各關(guān)節(jié)部的移動(dòng)指示信息及各種條件的設(shè)定信息。機(jī)械臂機(jī)構(gòu)依照移動(dòng)指示信息而動(dòng)作。構(gòu)成操作裝置的這些輸入設(shè)備也可由其他裝置例如鼠標(biāo)、鍵盤(pán)、軌跡球及觸控面板等代替。

設(shè)定部45具有推遲時(shí)間設(shè)定部451及驅(qū)動(dòng)條件設(shè)定部452。

驅(qū)動(dòng)條件設(shè)定部452設(shè)定步進(jìn)馬達(dá)31a的驅(qū)動(dòng)條件。另外，驅(qū)動(dòng)條件設(shè)定部452可個(gè)別地設(shè)定多個(gè)步進(jìn)馬達(dá)31a～36a各自的驅(qū)動(dòng)條件。驅(qū)動(dòng)條件包含脈沖條件及最大容許電流值等。脈沖條件表示從馬達(dá)驅(qū)動(dòng)器31b輸入至步進(jìn)馬達(dá)31a的脈沖的條件。脈沖條件包含脈沖數(shù)及脈沖周期等。脈沖數(shù)與步進(jìn)馬達(dá)31a的旋轉(zhuǎn)角度相關(guān)聯(lián)。例如，驅(qū)動(dòng)條件設(shè)定部452基于經(jīng)由輸入部44輸入的關(guān)節(jié)部j1的旋轉(zhuǎn)量而設(shè)定對(duì)應(yīng)的步進(jìn)馬達(dá)31a的目標(biāo)角度。然后，基于設(shè)定的目標(biāo)角度及步進(jìn)馬達(dá)31a的基本步進(jìn)角度來(lái)決定脈沖數(shù)。脈沖周期與步進(jìn)馬達(dá)31a的旋轉(zhuǎn)速度相關(guān)聯(lián)。驅(qū)動(dòng)條件設(shè)定部452是設(shè)定用于使關(guān)節(jié)部j1以經(jīng)由輸入部44輸入的動(dòng)作速度旋轉(zhuǎn)的脈沖周期。最大容許電流值是馬達(dá)驅(qū)動(dòng)器31b用于驅(qū)動(dòng)步進(jìn)馬達(dá)31a的驅(qū)動(dòng)電流值所容許的最大值。驅(qū)動(dòng)條件設(shè)定部452使用與關(guān)節(jié)部j1相關(guān)的動(dòng)態(tài)模型數(shù)據(jù)，根據(jù)機(jī)械臂機(jī)構(gòu)的與關(guān)節(jié)部j1鏈接的臂的重心位置、質(zhì)量、臂長(zhǎng)、慣性張量、關(guān)節(jié)部j1的位置(角度)、速度、加速度等，計(jì)算關(guān)節(jié)部j1產(chǎn)生的扭矩。另外，動(dòng)態(tài)模型數(shù)據(jù)是指與馬達(dá)的動(dòng)態(tài)特性相關(guān)的模型的數(shù)據(jù)。然后，基于所計(jì)算的扭矩決定最大容許電流值。這樣，基于預(yù)先計(jì)算的扭矩決定供給至步進(jìn)馬達(dá)31a的驅(qū)動(dòng)電流值的最大值的方法，有助于抑制步進(jìn)馬達(dá)31a的消耗電力及發(fā)熱量。另外，驅(qū)動(dòng)條件設(shè)定部452也可設(shè)定用于產(chǎn)生經(jīng)由輸入部44輸入的扭矩的驅(qū)動(dòng)電流值。

推遲時(shí)間設(shè)定部451依照經(jīng)由輸入部44的使用者指示，設(shè)定推遲時(shí)間。推遲時(shí)間是指從檢測(cè)到步進(jìn)馬達(dá)31a的失調(diào)起，至馬達(dá)驅(qū)動(dòng)器31b推遲步進(jìn)馬達(dá)31a的驅(qū)動(dòng)的停止的時(shí)間。因此，只要未從使用者發(fā)出步進(jìn)馬達(dá)31a的停止指示，在檢測(cè)到失調(diào)起經(jīng)過(guò)推遲時(shí)間之前，推遲步進(jìn)馬達(dá)31a的停止。

馬達(dá)驅(qū)動(dòng)部31具有步進(jìn)馬達(dá)31a、馬達(dá)驅(qū)動(dòng)器31b、編碼器31c、編碼器脈沖計(jì)數(shù)器31d、及失調(diào)檢測(cè)部31e。馬達(dá)驅(qū)動(dòng)部31設(shè)在對(duì)應(yīng)的關(guān)節(jié)部j1的附近。

馬達(dá)驅(qū)動(dòng)器31b控制步進(jìn)馬達(dá)31a的驅(qū)動(dòng)及停止。具體而言，馬達(dá)驅(qū)動(dòng)器31b使用從電源供給部42供給的電力，產(chǎn)生用于驅(qū)動(dòng)步進(jìn)馬達(dá)31a的驅(qū)動(dòng)電流。此時(shí)，由馬達(dá)驅(qū)動(dòng)器31a產(chǎn)生的驅(qū)動(dòng)電流具有最大容許電流值以下的電流值。馬達(dá)驅(qū)動(dòng)器31a保持表示關(guān)節(jié)部j1的旋轉(zhuǎn)速度與最佳驅(qū)動(dòng)電流的關(guān)系的數(shù)據(jù)，并產(chǎn)生與關(guān)節(jié)部j1的旋轉(zhuǎn)速度相應(yīng)的驅(qū)動(dòng)電流。另外，也可根據(jù)使用者經(jīng)由輸入部44指示的扭矩而設(shè)定驅(qū)動(dòng)電流。

馬達(dá)驅(qū)動(dòng)器31b產(chǎn)生與來(lái)自馬達(dá)控制部43的脈沖控制信號(hào)相應(yīng)的脈沖(以下稱為步進(jìn)脈沖)。步進(jìn)馬達(dá)31a依照由馬達(dá)驅(qū)動(dòng)器31b產(chǎn)生的步進(jìn)脈沖而旋轉(zhuǎn)。通過(guò)步進(jìn)馬達(dá)31a旋轉(zhuǎn)，與步進(jìn)馬達(dá)31a直接連接或經(jīng)由皮帶及齒輪等間接連接的關(guān)節(jié)部j1被驅(qū)動(dòng)。

馬達(dá)驅(qū)動(dòng)器31b基于來(lái)自失調(diào)檢測(cè)部31e的失調(diào)判定信號(hào)，指定失調(diào)的產(chǎn)生及恢復(fù)的時(shí)序。馬達(dá)驅(qū)動(dòng)器31b在從失調(diào)檢測(cè)部31e檢測(cè)到步進(jìn)馬達(dá)31a的失調(diào)起經(jīng)過(guò)推遲時(shí)間之前，當(dāng)失調(diào)未被恢復(fù)時(shí)，在推遲時(shí)間的經(jīng)過(guò)的時(shí)間點(diǎn)停止步進(jìn)脈沖的產(chǎn)生與驅(qū)動(dòng)電流的一方或兩方。這里，以步進(jìn)脈沖的停止為例進(jìn)行說(shuō)明。由此，步進(jìn)馬達(dá)31a的驅(qū)動(dòng)停止。

馬達(dá)驅(qū)動(dòng)器31b基于脈沖控制信號(hào)指定每單位時(shí)間的目標(biāo)角度。與每單位時(shí)間的目標(biāo)角度相關(guān)的數(shù)據(jù)被輸出至失調(diào)檢測(cè)部31e。所謂這里的單位時(shí)間，相當(dāng)于例如馬達(dá)驅(qū)動(dòng)部31的控制周期。另外，馬達(dá)驅(qū)動(dòng)器31b的控制周期短于馬達(dá)控制部43的控制周期。這種情形時(shí)，馬達(dá)驅(qū)動(dòng)部31的各構(gòu)成要素在馬達(dá)驅(qū)動(dòng)器31b的控制下動(dòng)作，由此與馬達(dá)控制部43的控制下動(dòng)作的情形相比，可實(shí)現(xiàn)更高速的動(dòng)作。

編碼器31c是連接在步進(jìn)馬達(dá)31a的驅(qū)動(dòng)軸。編碼器31c在連接在步進(jìn)馬達(dá)31a的驅(qū)動(dòng)軸的圓片每旋轉(zhuǎn)既定角度時(shí)產(chǎn)生編碼器脈沖。例如，在圓片的圓周方向等間隔設(shè)有120個(gè)狹縫的情形時(shí)，編碼器脈沖是步進(jìn)馬達(dá)31a每旋轉(zhuǎn)3度便輸出編碼器脈沖。編碼器脈沖被輸出至編碼器脈沖計(jì)數(shù)器31d。

編碼器脈沖計(jì)數(shù)器31d對(duì)輸入的編碼器脈沖的脈沖數(shù)進(jìn)行計(jì)數(shù)。編碼器脈沖的計(jì)數(shù)信息被輸出至馬達(dá)驅(qū)動(dòng)器31b及失調(diào)檢測(cè)部31e。

失調(diào)檢測(cè)部31e檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生。具體而言，失調(diào)檢測(cè)部31e根據(jù)計(jì)數(shù)信息指定步進(jìn)馬達(dá)31a的當(dāng)前的旋轉(zhuǎn)角度(以下稱為當(dāng)前角度)。另外，失調(diào)檢測(cè)部31e基于來(lái)自馬達(dá)驅(qū)動(dòng)器31b的輸出，指定步進(jìn)馬達(dá)31a的目標(biāo)角度。失調(diào)檢測(cè)部31e在當(dāng)前角度與目標(biāo)角度不一致時(shí)，檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)。失調(diào)檢測(cè)部31e將通知失調(diào)的產(chǎn)生的有無(wú)的失調(diào)判定信號(hào)輸出至馬達(dá)驅(qū)動(dòng)器31b。

圖5是用于說(shuō)明本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部31的動(dòng)作的流程圖。每當(dāng)有各關(guān)節(jié)部的移動(dòng)指示時(shí)便將來(lái)自馬達(dá)控制部43的脈沖控制信號(hào)反復(fù)輸入至馬達(dá)驅(qū)動(dòng)器31b。在圖5所示的流程圖中，說(shuō)明輸入一個(gè)脈沖控制信號(hào)時(shí)的驅(qū)動(dòng)部31的動(dòng)作。

(步驟s11)目標(biāo)角度與最大容許電流值的輸入

從馬達(dá)控制部43將脈沖控制信號(hào)及電流控制信號(hào)輸入至馬達(dá)驅(qū)動(dòng)器31b。

(步驟s12)驅(qū)動(dòng)步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b，產(chǎn)生與脈沖控制信號(hào)相應(yīng)的步進(jìn)脈沖及與電流控制信號(hào)相應(yīng)的驅(qū)動(dòng)電流。通過(guò)產(chǎn)生的驅(qū)動(dòng)電流驅(qū)動(dòng)步進(jìn)馬達(dá)31a，通過(guò)步進(jìn)脈沖使步進(jìn)馬達(dá)31a旋轉(zhuǎn)。

(步驟s13)檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生

通過(guò)失調(diào)檢測(cè)部31e檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生。當(dāng)檢測(cè)到失調(diào)的產(chǎn)生時(shí)，處理轉(zhuǎn)向步驟s14。另一方面，在未檢測(cè)到失調(diào)的產(chǎn)生的情形時(shí)，處理返回至步驟s12。即，只要未檢測(cè)到失調(diào)，馬達(dá)驅(qū)動(dòng)器31b便會(huì)對(duì)步進(jìn)馬達(dá)31a進(jìn)行通常的控制。步進(jìn)馬達(dá)31a旋轉(zhuǎn)至其旋轉(zhuǎn)角度到達(dá)目標(biāo)角度為止。

(步驟s14)推遲時(shí)間內(nèi)是否從失調(diào)恢復(fù)的判定

通過(guò)馬達(dá)驅(qū)動(dòng)器31b測(cè)量從最初檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間。當(dāng)從檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間未達(dá)推遲時(shí)間t1時(shí)，處理返回至步驟s12。另一方面，當(dāng)檢測(cè)到失調(diào)起經(jīng)過(guò)推遲時(shí)間t1之前失調(diào)未恢復(fù)時(shí)，以已到達(dá)推遲時(shí)間t1為契機(jī)，處理轉(zhuǎn)向步驟s15。

(步驟s15)停止步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b停止步進(jìn)脈沖的產(chǎn)生。由此，停止步進(jìn)馬達(dá)31a的驅(qū)動(dòng)。

根據(jù)本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)，能獲得如下的效果。本實(shí)施方式所涉及的馬達(dá)驅(qū)動(dòng)器31b是在檢測(cè)到步進(jìn)馬達(dá)31a的失調(diào)后，并不立即停止步進(jìn)馬達(dá)31a的驅(qū)動(dòng)，而是在經(jīng)過(guò)推遲時(shí)間t1之前推遲步進(jìn)馬達(dá)31a的驅(qū)動(dòng)的停止。然后，在檢測(cè)到失調(diào)起經(jīng)過(guò)推遲時(shí)間t1之前失調(diào)已恢復(fù)的情形時(shí)，避免步進(jìn)馬達(dá)31a的驅(qū)動(dòng)的停止。因此，在可立即恢復(fù)失調(diào)的情形時(shí)，本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)可在其失調(diào)恢復(fù)前等待所設(shè)定的推遲時(shí)間。因此，本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)即使在失調(diào)檢測(cè)后也可減少不必要的步進(jìn)馬達(dá)31a的驅(qū)動(dòng)的停止。換言之，本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)可向使用者提供較高的操作性，由此使用者可高效地操作機(jī)械臂機(jī)構(gòu)。

(變形例1)

變形例1所涉及的機(jī)械臂機(jī)構(gòu)與本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)之間的差異為，在連接在變形例1所涉及的機(jī)械臂機(jī)構(gòu)的輸入部44的操作裝置裝備有重開(kāi)開(kāi)關(guān)。重開(kāi)開(kāi)關(guān)是用于不伴隨主電源的on/off而強(qiáng)制重開(kāi)暫時(shí)停止的步進(jìn)脈沖的產(chǎn)生及驅(qū)動(dòng)電流的供給的開(kāi)關(guān)。因此，重開(kāi)開(kāi)關(guān)與一般的伴隨主電源的on/off的重設(shè)按鈕或重啟按鈕不同。

圖6是用于說(shuō)明本實(shí)施方式的變形例1所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部31的動(dòng)作的流程圖。圖6是與圖5的說(shuō)明同樣地，說(shuō)明輸入一個(gè)脈沖控制信號(hào)時(shí)的驅(qū)動(dòng)部31的動(dòng)作。

(步驟s21)目標(biāo)角度與最大容許電流值的輸入

從馬達(dá)控制部43將脈沖控制信號(hào)及電流控制信號(hào)輸入至馬達(dá)驅(qū)動(dòng)器31b。

(步驟s22)驅(qū)動(dòng)步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b產(chǎn)生與脈沖控制信號(hào)相應(yīng)的步進(jìn)脈沖及與電流控制信號(hào)相應(yīng)的驅(qū)動(dòng)電流。通過(guò)產(chǎn)生的驅(qū)動(dòng)電流驅(qū)動(dòng)步進(jìn)馬達(dá)31a，通過(guò)步進(jìn)脈沖使步進(jìn)馬達(dá)31a旋轉(zhuǎn)。

(步驟s23)檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生

通過(guò)失調(diào)檢測(cè)部31e檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生。當(dāng)檢測(cè)到失調(diào)的產(chǎn)生時(shí)，處理轉(zhuǎn)向步驟s14。另一方面，在未檢測(cè)到失調(diào)的產(chǎn)生的情形時(shí)，處理返回至步驟s22。即，只要未檢測(cè)到失調(diào)，馬達(dá)驅(qū)動(dòng)器31b進(jìn)行通常的步進(jìn)馬達(dá)31a的控制。步進(jìn)馬達(dá)31a旋轉(zhuǎn)至其旋轉(zhuǎn)角度到達(dá)目標(biāo)角度為止。

(步驟s24)推遲時(shí)間內(nèi)是否從失調(diào)恢復(fù)的判定

通過(guò)馬達(dá)驅(qū)動(dòng)器31b測(cè)量從最初檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間。當(dāng)檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間未達(dá)推遲時(shí)間t1時(shí)，處理返回至步驟s22。另一方面，在從檢測(cè)到失調(diào)起經(jīng)過(guò)推遲時(shí)間t1之前，失調(diào)未恢復(fù)時(shí)，以到達(dá)推遲時(shí)間t1為契機(jī)，處理轉(zhuǎn)向步驟s25。

(步驟s25)停止步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b停止步進(jìn)脈沖的產(chǎn)生。由此，停止步進(jìn)馬達(dá)31a的驅(qū)動(dòng)。

(步驟s26)重開(kāi)開(kāi)關(guān)的操作的有無(wú)的判定

在步進(jìn)馬達(dá)31a的驅(qū)動(dòng)停止后重開(kāi)開(kāi)關(guān)被按壓的情形時(shí)，從步驟s21重新開(kāi)始處理。另外，無(wú)關(guān)于步進(jìn)馬達(dá)31a的驅(qū)動(dòng)停止后的經(jīng)過(guò)時(shí)間，只要主電源未off則重開(kāi)開(kāi)關(guān)始終有效。但為使重開(kāi)開(kāi)關(guān)暫時(shí)失效，也可設(shè)置切換重開(kāi)開(kāi)關(guān)的有效/失效的開(kāi)關(guān)等。由此，可防止為除去失調(diào)要因而操作者靠近機(jī)械臂機(jī)構(gòu)時(shí)，第三者誤按下重開(kāi)開(kāi)關(guān)，將操作者卷入機(jī)械臂機(jī)構(gòu)的動(dòng)作。

根據(jù)本實(shí)施方式的變形例1所涉及的機(jī)械臂機(jī)構(gòu)，能獲得如下的效果。變形例1所涉及的機(jī)械臂機(jī)構(gòu)具有重開(kāi)開(kāi)關(guān)。馬達(dá)驅(qū)動(dòng)器31b以重開(kāi)開(kāi)關(guān)被按下為契機(jī)，可使動(dòng)作途中停止的步進(jìn)馬達(dá)31a的驅(qū)動(dòng)從剛停止前的狀態(tài)強(qiáng)制重開(kāi)。使用者在步進(jìn)馬達(dá)31a停止后，進(jìn)行用于除去失調(diào)要因的作業(yè)，僅按下重開(kāi)開(kāi)關(guān)，便可從停止?fàn)顟B(tài)繼續(xù)機(jī)械臂機(jī)構(gòu)的操作。因此，本實(shí)施方式的變形例1所涉及的機(jī)械臂機(jī)構(gòu)可向使用者提供較高的機(jī)器人操作性。

(變形例2)

變形例2所涉及的機(jī)械臂機(jī)構(gòu)與本實(shí)施方式所涉及的機(jī)械臂機(jī)構(gòu)之間的差異為，變形例2所涉及的機(jī)械臂機(jī)構(gòu)在檢測(cè)到失調(diào)時(shí)可指定其要因，并根據(jù)失調(diào)要因變更步進(jìn)馬達(dá)31a的驅(qū)動(dòng)條件。

圖7是用于說(shuō)明本實(shí)施方式的變形例2所涉及的機(jī)械臂機(jī)構(gòu)的驅(qū)動(dòng)部31的動(dòng)作的流程圖。圖7是與圖5的說(shuō)明同樣地說(shuō)明輸入一個(gè)脈沖控制信號(hào)時(shí)的驅(qū)動(dòng)部31的動(dòng)作。

(步驟s31)目標(biāo)角度與最大容許電流值的輸入

從馬達(dá)控制部43將脈沖控制信號(hào)及電流控制信號(hào)輸入至馬達(dá)驅(qū)動(dòng)器31b。

(步驟s32)驅(qū)動(dòng)步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b，產(chǎn)生與脈沖控制信號(hào)相應(yīng)的步進(jìn)脈沖及與電流控制信號(hào)相應(yīng)的驅(qū)動(dòng)電流。通過(guò)產(chǎn)生的驅(qū)動(dòng)電流驅(qū)動(dòng)步進(jìn)馬達(dá)31a，通過(guò)步進(jìn)脈沖使步進(jìn)馬達(dá)31a旋轉(zhuǎn)。

(步驟s33)檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生

通過(guò)失調(diào)檢測(cè)部31e，檢測(cè)步進(jìn)馬達(dá)31a的失調(diào)的產(chǎn)生。當(dāng)檢測(cè)到失調(diào)的產(chǎn)生時(shí)，處理轉(zhuǎn)向步驟s34。另一方面，在未檢測(cè)到失調(diào)的產(chǎn)生的情形時(shí)，處理返回至步驟s32。即，只要未檢測(cè)到失調(diào)，馬達(dá)驅(qū)動(dòng)器31b便進(jìn)行通常的步進(jìn)馬達(dá)31a的控制，使步進(jìn)馬達(dá)31a旋轉(zhuǎn)至步進(jìn)馬達(dá)31a的旋轉(zhuǎn)角度到達(dá)目標(biāo)角度為止。

(步驟s34)指定失調(diào)要因

通過(guò)馬達(dá)驅(qū)動(dòng)器31b指定失調(diào)要因。失調(diào)要因有內(nèi)在要因與外在要因。內(nèi)在要因包含步進(jìn)馬達(dá)31a的扭矩不足導(dǎo)致馬達(dá)的旋轉(zhuǎn)無(wú)法追蹤步進(jìn)脈沖的現(xiàn)象。外在要因包含臂部接觸人或物而無(wú)法進(jìn)行預(yù)定動(dòng)作的現(xiàn)象。馬達(dá)驅(qū)動(dòng)器31b基于來(lái)自未圖示的接觸傳感器的輸出而指定失調(diào)要因。具體而言，馬達(dá)驅(qū)動(dòng)器31b在通過(guò)失調(diào)檢測(cè)部31e檢測(cè)到失調(diào)的時(shí)序的接觸傳感器的輸出的變化值超過(guò)閾值時(shí)，將該失調(diào)的要因指定為外在要因。另外，馬達(dá)驅(qū)動(dòng)器31b在通過(guò)失調(diào)檢測(cè)部31e檢測(cè)到失調(diào)的時(shí)序的接觸傳感器的輸出的變化值未達(dá)閾值時(shí)，將該失調(diào)的要因指定為內(nèi)在要因。

(步驟s35)根據(jù)失調(diào)要因變更驅(qū)動(dòng)條件

通過(guò)馬達(dá)驅(qū)動(dòng)器31b，根據(jù)失調(diào)要因變更步進(jìn)馬達(dá)31a的驅(qū)動(dòng)電流值。例如，在失調(diào)要因?yàn)橥庠谝虻那樾螘r(shí)，馬達(dá)驅(qū)動(dòng)器31b減小驅(qū)動(dòng)電流值。另一方面，在失調(diào)要因?yàn)閮?nèi)在要因的情形時(shí)，馬達(dá)驅(qū)動(dòng)器31b增大驅(qū)動(dòng)電流值。另外，在失調(diào)要因?yàn)橥庠谝虻那樾螘r(shí)，馬達(dá)驅(qū)動(dòng)器31b也可維持驅(qū)動(dòng)電流值。另外，馬達(dá)驅(qū)動(dòng)器31b也可在變更驅(qū)動(dòng)電流的同時(shí)也變更推遲時(shí)間t1。

(步驟s36)推遲時(shí)間內(nèi)是否從失調(diào)恢復(fù)的判定

通過(guò)馬達(dá)驅(qū)動(dòng)器31b測(cè)量從最初檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間。當(dāng)檢測(cè)到失調(diào)起的經(jīng)過(guò)時(shí)間未達(dá)推遲時(shí)間t1時(shí)，處理返回至步驟s32。另一方面，在從檢測(cè)到失調(diào)起經(jīng)過(guò)推遲時(shí)間t1之前，失調(diào)未恢復(fù)時(shí)，以到達(dá)推遲時(shí)間t1為契機(jī)，處理轉(zhuǎn)向步驟s37。另外，也存在通過(guò)馬達(dá)驅(qū)動(dòng)器31b變更推遲時(shí)間t1的情形。

(步驟s37)停止步進(jìn)馬達(dá)31a

通過(guò)馬達(dá)驅(qū)動(dòng)器31b，停止步進(jìn)脈沖的產(chǎn)生。由此，停止步進(jìn)馬達(dá)31a的驅(qū)動(dòng)。

根據(jù)本實(shí)施方式的變形例2所涉及的機(jī)械臂機(jī)構(gòu)，可獲得如下的效果。變形例2所涉及的機(jī)械臂機(jī)構(gòu)的馬達(dá)驅(qū)動(dòng)器31b可在檢測(cè)到步進(jìn)馬達(dá)31a的失調(diào)后，根據(jù)失調(diào)要因變更步進(jìn)馬達(dá)31a的驅(qū)動(dòng)條件。例如，因內(nèi)在要因所致的失調(diào)是由于步進(jìn)馬達(dá)31a的扭矩不足而產(chǎn)生。在這種情形時(shí)，馬達(dá)驅(qū)動(dòng)器31b進(jìn)行增大步進(jìn)馬達(dá)31a的驅(qū)動(dòng)電流值的控制。由此，步進(jìn)馬達(dá)31a的扭矩變大，有能夠恢復(fù)內(nèi)在要因所致的失調(diào)的可能性。另外，在失調(diào)要因?yàn)橥庠谝?與人或其他干渉物的碰撞等)的情形時(shí)，馬達(dá)驅(qū)動(dòng)器31b進(jìn)行減小驅(qū)動(dòng)電流值的控制。由此，步進(jìn)馬達(dá)31a的扭矩變小，可防止機(jī)械臂機(jī)構(gòu)的故障、人身傷害、物品破損等，從而確保安全。

另外，馬達(dá)驅(qū)動(dòng)器31b也可進(jìn)行在變更驅(qū)動(dòng)電流值的同時(shí)變更推遲時(shí)間的控制。例如，在失調(diào)要因?yàn)橥庠谝虻那樾螘r(shí)，馬達(dá)驅(qū)動(dòng)器31b通過(guò)控制推遲時(shí)間變短，而可縮短直至步進(jìn)馬達(dá)31a的停止的推遲時(shí)間，從而可防止機(jī)械臂機(jī)構(gòu)的故障、人身傷害、物品破損等。另一方面，通過(guò)控制推遲時(shí)間變長(zhǎng)，可延長(zhǎng)用于從步進(jìn)馬達(dá)31a的失調(diào)的恢復(fù)時(shí)間，從而可減少步進(jìn)馬達(dá)31a的不必要停止。即，在失調(diào)的檢測(cè)后馬達(dá)驅(qū)動(dòng)器31a縮短還是延長(zhǎng)推遲時(shí)間，是根據(jù)機(jī)械臂機(jī)構(gòu)的使用場(chǎng)景、用途等而不同。失調(diào)后變更的驅(qū)動(dòng)條件及其參數(shù)可依照使用者指示而適宜地變更。例如，使用者也可通過(guò)選擇機(jī)械臂機(jī)構(gòu)的使用用途及場(chǎng)景，而選擇性地決定要變更的驅(qū)動(dòng)條件等。

雖然對(duì)本發(fā)明的幾個(gè)實(shí)施方式進(jìn)行了說(shuō)明，但是這些實(shí)施方式只是作為例子提出的，并非用于限定本發(fā)明的范圍。對(duì)于這些新的實(shí)施方式，能夠以其他各種方式進(jìn)行實(shí)施，在不脫離本發(fā)明的要旨的范圍內(nèi)，能夠進(jìn)行各種省略、置換、變更。這些實(shí)施方式和其變形，包含于本發(fā)明的范圍和要旨的同時(shí)，也包含于權(quán)利要求書(shū)中記載的發(fā)明及其均等范圍內(nèi)。

符號(hào)說(shuō)明

1：基部

2：臂部

3：末端效應(yīng)器

j1、j2、j4、j5、j6：旋轉(zhuǎn)關(guān)節(jié)部

j3：直動(dòng)關(guān)節(jié)部

11a：第1支撐體

11b：第2支撐體

11c：第3支撐體

20：第2連結(jié)鏈節(jié)排

21：第1連結(jié)鏈節(jié)排

22：第2連結(jié)鏈節(jié)

23：第1連結(jié)鏈節(jié)

26：結(jié)合鏈節(jié)

31：馬達(dá)驅(qū)動(dòng)部

31a：步進(jìn)馬達(dá)

31b：馬達(dá)驅(qū)動(dòng)器

31c：編碼器

31d：編碼器脈沖計(jì)數(shù)器

31e：失調(diào)檢測(cè)部

41：系統(tǒng)控制部

42：電源供給部

43：馬達(dá)控制部

44：輸入部

45：設(shè)定部

451：推遲時(shí)間設(shè)定部

452：驅(qū)動(dòng)條件設(shè)定部