專(zhuān)利名稱(chēng)：機(jī)械手裝置以及機(jī)械手裝置的控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)械手裝置以及機(jī)械手裝置的控制方法。
背景技術(shù)：
作為IC分類(lèi)機(jī)和組裝裝置的一部分被經(jīng)常使用的具有多關(guān)節(jié)構(gòu)造的機(jī)械手裝置，逐漸被經(jīng)常用于各種操作現(xiàn)場(chǎng)中。因此，怎樣能夠讓機(jī)械手裝置更迅速且更準(zhǔn)確地將手臂移動(dòng)到規(guī)定位置，已成為重要的性能標(biāo)準(zhǔn)和品質(zhì)。通常為了使機(jī)械手裝置的手臂高速且準(zhǔn)確地移動(dòng)，需要減少施加于手臂的慣性力，且不增大對(duì)驅(qū)動(dòng)手臂的致動(dòng)器施加的負(fù)荷。作為減少施加到手臂的慣性力的方法之一， 手臂本身的輕型化作為簡(jiǎn)便且有效的方法被使用。但是，通過(guò)手臂的輕型化，會(huì)導(dǎo)致手臂的剛性降低，從而會(huì)使因在手臂停止時(shí)產(chǎn)生的手臂的撓曲而產(chǎn)生振動(dòng)的情況增加，即使進(jìn)行控制以使得手臂前端部停止在目標(biāo)位置，也會(huì)產(chǎn)生手臂振動(dòng)的振幅量的位置偏移，振動(dòng)衰減的時(shí)間變成到開(kāi)始下一動(dòng)作為止的待機(jī)時(shí)間，從而成為高速動(dòng)作的障礙。針對(duì)該問(wèn)題，公開(kāi)有如下方法在手臂前端設(shè)置加速度傳感器，以加速度信號(hào)為基礎(chǔ)使手臂工作，并抑制振動(dòng)的方法(例如，專(zhuān)利文獻(xiàn)1)，在手臂前端以及手臂上設(shè)置角速度傳感器，以角速度信號(hào)為基礎(chǔ)控制手臂動(dòng)作的方法(例如，專(zhuān)利文獻(xiàn)2)，和基于設(shè)置在手臂前端的慣性傳感器的信號(hào)來(lái)驅(qū)動(dòng)驅(qū)動(dòng)體的方法(例如，專(zhuān)利文獻(xiàn)幻等。但是，在這些現(xiàn)有文獻(xiàn)中，在作為手臂控制的基準(zhǔn)的加速度傳感器或角速度傳感器等慣性傳感器本身發(fā)生了故障的情況下，即使所得到的數(shù)據(jù)信號(hào)不好，也進(jìn)行基于該不良信號(hào)的控制，會(huì)導(dǎo)致因機(jī)械手裝置的失控等帶來(lái)的危險(xiǎn)的發(fā)生。作為檢測(cè)該傳感器自身的故障的技術(shù)，公開(kāi)有如下技術(shù)預(yù)先設(shè)定針對(duì)傳感器的檢測(cè)值的閾值，在閾值和檢測(cè)值之差超過(guò)了判斷值時(shí)判斷為傳感器發(fā)生了故障(專(zhuān)利文獻(xiàn)4、5)。專(zhuān)利文獻(xiàn)1日本特開(kāi)平1-173116號(hào)公報(bào)專(zhuān)利文獻(xiàn)2日本特開(kāi)2005-242794號(hào)公報(bào)專(zhuān)利文獻(xiàn)3日本特開(kāi)平7-9374號(hào)公報(bào)專(zhuān)利文獻(xiàn)4日本特開(kāi)2009-8412號(hào)公報(bào)專(zhuān)利文獻(xiàn)5日本特開(kāi)2009-184035號(hào)公報(bào)但是，就上述專(zhuān)利文獻(xiàn)所公開(kāi)的技術(shù)來(lái)說(shuō)，由于以使用的傳感器的正常工作時(shí)的檢測(cè)值作為基準(zhǔn)來(lái)預(yù)先設(shè)定閾值，因此在實(shí)際的機(jī)械手裝置的各種動(dòng)作狀態(tài)下，有時(shí)即使傳感器發(fā)生故障，在不超過(guò)閾值的情況下也無(wú)法進(jìn)行正確的故障判斷。

發(fā)明內(nèi)容
因此，提供一種在實(shí)際工作時(shí)與作為基準(zhǔn)的傳感器檢測(cè)數(shù)據(jù)進(jìn)行比較，根據(jù)該檢測(cè)數(shù)據(jù)的差值來(lái)進(jìn)行可靠的故障判斷的機(jī)械手裝置以及機(jī)械手裝置的控制方法。本發(fā)明為了至少解決上述的課題之一，能夠作為下述的方式或者應(yīng)用例被實(shí)現(xiàn)。〔應(yīng)用例1〕本應(yīng)用例的機(jī)械手裝置，其特征在于，具備手臂連結(jié)裝置，其具有致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的；基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；慣性傳感器，其安裝在上述手臂上；第一計(jì)算部，其根據(jù)來(lái)自上述角度傳感器的上述致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述致動(dòng)器的角速度以及角加速度；第二計(jì)算部，其根據(jù)由上述慣性傳感器檢測(cè)到的輸出，計(jì)算上述手臂的角速度或者角加速度；比較部，其對(duì)由上述第一計(jì)算部計(jì)算出的上述致動(dòng)器的角速度或者角加速度、和由上述第二計(jì)算部計(jì)算出的上述手臂的角速度或者角加速度進(jìn)行比較；故障判斷部，當(dāng)在上述比較部中，上述致動(dòng)器的角速度或者角加速度與上述手臂的角速度或者角加速度之差的絕對(duì)值大于閾值Ω時(shí)，該故障判斷部判斷為上述慣性傳感器發(fā)生了故障，并輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。〔應(yīng)用例2〕在上述的應(yīng)用例中，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的 120%的負(fù)荷、且以最大負(fù)荷時(shí)的允許加速度的120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述第一計(jì)算部計(jì)算的角速度為、由上述第二計(jì)算部計(jì)算的角速度為時(shí)，相對(duì)于cos和
之差的絕對(duì)值的最大值，上述閾值Ω是。Ω = 2Χ (| s- L|)max根據(jù)上述的應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是以動(dòng)作狀態(tài)下的致動(dòng)器的動(dòng)作數(shù)據(jù)作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于使用了固定值的閾值，例如，也發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，成為作為基準(zhǔn)的致動(dòng)器的基準(zhǔn)的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障。〔應(yīng)用例3〕本應(yīng)用例的機(jī)械手裝置，其特征在于，具備手臂連結(jié)裝置，其具有致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的；基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；至少 2個(gè)慣性傳感器，被安裝在上述手臂上；計(jì)算部，其存儲(chǔ)上述慣性傳感器檢測(cè)出的多個(gè)輸出值，并計(jì)算平均值；比較部，其對(duì)由上述計(jì)算部計(jì)算出的上述輸出值的平均值進(jìn)行比較；故障判斷部，當(dāng)在上述比較部中，2個(gè)上述輸出值的平均值之差的絕對(duì)值中存在大于閾值S的上述差時(shí)，該故障判斷部判斷為上述慣性傳感器中的任意一個(gè)以上發(fā)生了故障，并輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)?！矐?yīng)用例4〕在上述的應(yīng)用例中，其特征在于，在上述慣性傳感器的白噪聲的標(biāo)準(zhǔn)偏差是σ時(shí)，上述閾值S為S = 6 σ根據(jù)上述應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是求出動(dòng)作狀態(tài)下的具備的2個(gè)以上的慣性傳感器的檢測(cè)值之差，如果是正常則以白噪聲的標(biāo)準(zhǔn)偏差作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于使用了固定值的閾值，例如發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，與基準(zhǔn)值進(jìn)行比較的慣性傳感器的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障。〔應(yīng)用例5〕本應(yīng)用例的機(jī)械手裝置，其特征在于，具備基體，其具備基體致動(dòng)器、 將上述基體致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述基體致動(dòng)器的旋轉(zhuǎn)角度的基體角度傳感器、和手臂驅(qū)動(dòng)裝置，該手臂驅(qū)動(dòng)裝置具有手臂連結(jié)裝置，與上述轉(zhuǎn)矩傳遞機(jī)構(gòu)連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述手臂連結(jié)裝置；手臂，其與上述基體的上述手臂連結(jié)裝置連結(jié)，具有手臂致動(dòng)器、將上述手臂致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述手臂致動(dòng)器的旋轉(zhuǎn)角度的手臂角度傳感器、和工件保持裝置驅(qū)動(dòng)裝置，該工件保持裝置驅(qū)動(dòng)裝置具有工件保持裝置，與上述手臂連結(jié)裝置連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述工件保持裝置；2個(gè)以上慣性傳感器，其安裝在上述工件保持裝置上，至少包含加速度傳感器；第一計(jì)算部，其根據(jù)來(lái)自上述基體角度傳感器的上述基體致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述手臂驅(qū)動(dòng)裝置的加速度，并根據(jù)來(lái)自上述手臂角度傳感器的上述手臂致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述工件保持裝置驅(qū)動(dòng)裝置的加速度；第二計(jì)算部，其根據(jù)由上述慣性傳感器檢測(cè)到的輸出，計(jì)算上述工件保持裝置的加速度；比較部，其對(duì)由上述第一計(jì)算部計(jì)算出的上述加速度和由上述第二計(jì)算部計(jì)算出的上述加速度進(jìn)行比較；故障判斷部， 其在由上述第一計(jì)算部計(jì)算出的上述加速度和由上述第二計(jì)算部計(jì)算出的上述加速度之差的絕對(duì)值大于閾值A(chǔ)的情況下，判斷為上述慣性傳感器發(fā)生了故障，并輸出使上述基體致動(dòng)器以及上述手臂致動(dòng)器的動(dòng)作停止的信號(hào)?！矐?yīng)用例6〕在上述的應(yīng)用例中，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的 120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述第一計(jì)算部計(jì)算的加速度為α s、由上述第二計(jì)算部計(jì)算的加速度為c^時(shí)，相對(duì)于α 3和 q之差的絕對(duì)值的最大值，上述閾值A(chǔ)為。A = 2 X (I α s- α LI) max根據(jù)上述應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是將動(dòng)作狀態(tài)下的致動(dòng)器的動(dòng)作數(shù)據(jù)作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于使用了固定值的閾值，例如發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，成為作為基準(zhǔn)的致動(dòng)器的基準(zhǔn)的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障?！矐?yīng)用例7〕本應(yīng)用例的機(jī)械手裝置的控制方法，其特征在于，該機(jī)械手裝置具備 手臂連結(jié)裝置，其具有致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的；基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；和安裝在上述手臂上的慣性傳感器，該機(jī)械手裝置的控制方法包括計(jì)算步驟，根據(jù)來(lái)自上述角度傳感器的上述致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述致動(dòng)器的角速度以及角加速度，并根據(jù)由上述慣性傳感器檢測(cè)到的輸出來(lái)計(jì)算上述手臂的角速度或者角加速度；比較步驟，對(duì)通過(guò)上述計(jì)算步驟而計(jì)算出的上述致動(dòng)器的角速度或者角加速度、與上述手臂的角速度或者角加速度進(jìn)行比較；判斷步驟，當(dāng)在上述比較步驟中上述致動(dòng)器的角速度或者角加速度與上述手臂的角速度或者角加速度之差的絕對(duì)值大于閾值Ω時(shí)，判斷為上述慣性傳感器發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為發(fā)生了故障時(shí)，輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。〔應(yīng)用例8〕在上述的應(yīng)用例中，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的 120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)在上述計(jì)算步驟中計(jì)算的上述致動(dòng)器的角速度為、上述手臂的角速度為時(shí)，相對(duì)于0^和
之差的絕對(duì)值的最大值，上述閾值Ω為Ω = 2Χ (I s- L|)max根據(jù)上述的應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是將動(dòng)作狀態(tài)下的致動(dòng)器的動(dòng)作數(shù)據(jù)作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于利用了固定值的閾值，例如也發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，成為作為基準(zhǔn)的致動(dòng)器的基準(zhǔn)的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障，能夠可靠地使機(jī)械手裝置的動(dòng)作停止，能夠保障安全的操作。〔應(yīng)用例9〕本應(yīng)用例的機(jī)械手裝置的控制方法，其特征在于，該機(jī)械手裝置具備 手臂連結(jié)裝置，其具有致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的； 基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；和至少2個(gè)慣性傳感器，其安裝在上述手臂上，該機(jī)械手裝置的控制方法包括計(jì)算步驟，存儲(chǔ)上述慣性傳感器檢測(cè)到的多個(gè)輸出值，并計(jì)算平均值；比較步驟， 對(duì)由上述計(jì)算步驟計(jì)算出的上述輸出值的平均值進(jìn)行比較；判斷步驟，當(dāng)在上述比較步驟中，2個(gè)上述輸出值的平均值之差的絕對(duì)值中存在大于閾值S的上述差時(shí)，判斷為上述慣性傳感器中的任意一個(gè)以上發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為故障時(shí)， 輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。〔應(yīng)用例10〕上述的應(yīng)用例中，其特征在于，在上述慣性傳感器的白噪聲的標(biāo)準(zhǔn)偏差是σ時(shí)，上述閾值S為S = 6 σ根據(jù)上述的應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是求出動(dòng)作狀態(tài)下的具有2個(gè)以上的慣性傳感器的檢測(cè)值之差，如果是正常則以白噪聲的標(biāo)準(zhǔn)偏差作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于利用了固定值的閾值，例如也發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，與基準(zhǔn)值進(jìn)行比較的慣性傳感器的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障，能夠可靠地使機(jī)械手裝置的動(dòng)作停止， 能夠保障安全的操作?！矐?yīng)用例11〕本應(yīng)用例的機(jī)械手裝置的控制方法，其特征在于，該機(jī)械手裝置具備基體，其具備基體致動(dòng)器、將上述基體致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述基體致動(dòng)器的旋轉(zhuǎn)角度的基體角度傳感器、和手臂驅(qū)動(dòng)裝置，該手臂驅(qū)動(dòng)裝置具有手臂連結(jié)裝置，與上述轉(zhuǎn)矩傳遞機(jī)構(gòu)連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述手臂連結(jié)裝置；手臂，其與上述基體的上述手臂連結(jié)裝置連結(jié)，具有手臂致動(dòng)器、將上述手臂致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述手臂致動(dòng)器的旋轉(zhuǎn)角度的手臂角度傳感器、和工件保持裝置驅(qū)動(dòng)裝置，該工件保持裝置驅(qū)動(dòng)裝置具有工件保持裝置，與上述手臂連結(jié)裝置連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述工件保持裝置，該機(jī)械手裝置的控制方法包括計(jì)算步驟，根據(jù)來(lái)自上述基體角度傳感器的上述基體致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)計(jì)算上述手臂驅(qū)動(dòng)裝置的加速度，根據(jù)來(lái)自上述手臂角度傳感器的上述手臂致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)計(jì)算上述工件保持裝置驅(qū)動(dòng)裝置的加速度，并根據(jù)由安裝在上述工件保持裝置上、并至少具有加速度傳感器的一個(gè)以上慣性傳感器檢測(cè)到的輸出，計(jì)算上述工件保持裝置的速度以及加速度； 比較步驟，對(duì)由上述計(jì)算步驟計(jì)算出的上述手臂驅(qū)動(dòng)裝置、上述工件保持裝置驅(qū)動(dòng)裝置的加速度、和上述工件保持裝置的加速度進(jìn)行比較；判斷步驟，當(dāng)在上述比較步驟中，根據(jù)上述角度傳感器的輸出值計(jì)算出的上述手臂驅(qū)動(dòng)裝置、上述工件保持裝置驅(qū)動(dòng)裝置的加速度與根據(jù)上述慣性傳感器的輸出值計(jì)算出的上述工件保持裝置的加速度之差的絕對(duì)值大于閾值A(chǔ)時(shí)，判斷為上述慣性傳感器發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為發(fā)生了故障時(shí)，輸出使上述基體致動(dòng)器以及上述手臂致動(dòng)器的動(dòng)作停止的信號(hào)?！矐?yīng)用例12〕在上述的應(yīng)用例中，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述計(jì)算步驟計(jì)算的上述手臂驅(qū)動(dòng)裝置或上述工件保持裝置驅(qū)動(dòng)裝置的加速度為α s、上述工件保持裝置的加速度為時(shí)，相對(duì)于％和C^之差的絕對(duì)值的最大值，上述閾值A(chǔ) 為A = 2 X (I α s- α LI) max根據(jù)上述的應(yīng)用例，構(gòu)成為不將決定閾值的基準(zhǔn)值設(shè)為固定值而是將動(dòng)作狀態(tài)下的致動(dòng)器的動(dòng)作數(shù)據(jù)作為基準(zhǔn)值來(lái)檢測(cè)慣性傳感器的故障。根據(jù)該構(gòu)成，對(duì)于利用了固定值的閾值，例如也發(fā)生過(guò)沒(méi)有檢測(cè)到動(dòng)作速度或加速度較小的情況下等的故障的情況，而基于動(dòng)作中的依次信息，成為作為基準(zhǔn)的致動(dòng)器的基準(zhǔn)的動(dòng)作數(shù)據(jù)也被依次改寫(xiě)，由此例如即使是微小動(dòng)作，也能夠在各種動(dòng)作模式下可靠地檢測(cè)慣性傳感器的故障，能夠可靠地使機(jī)械手裝置的動(dòng)作停止，能夠保障安全的操作。


圖1的(a)是第一實(shí)施方式涉及的機(jī)械手裝置的概略俯視圖，(b)是其概略剖視圖。圖2是第一實(shí)施方式涉及的機(jī)械手裝置的故障檢測(cè)的框圖。圖3是第一實(shí)施方式涉及的機(jī)械手裝置的故障檢測(cè)的流程圖。圖4的(a)是第二實(shí)施方式涉及的機(jī)械手裝置的概略俯視圖，(b)是其概略剖視圖。圖5是第二實(shí)施方式涉及的機(jī)械手裝置的故障檢測(cè)的框圖。圖6是表示傳感器的白噪聲的一個(gè)例子的圖表。圖7是第二實(shí)施方式涉及的機(jī)械手裝置的故障檢測(cè)的流程圖。圖8是第三實(shí)施方式涉及的機(jī)械手裝置的概略俯視圖。圖9是第三實(shí)施方式涉及的機(jī)械手裝置的概略立體圖。
圖10是第三實(shí)施方式涉及的機(jī)械手裝置的故障檢測(cè)的框圖。圖中符號(hào)說(shuō)明11. · ·第一手臂；12. · ·第二手臂；13. · ·第三手臂；40. · ·基體；51,52,53. · ·致動(dòng)器，81、82、83...角度傳感器；91、92、93...慣性傳感器；1100...第一計(jì)算部；1200...第二計(jì)算部；2000...比較部；3000...判斷部，4000...控制部
具體實(shí)施例方式下面參照

本發(fā)明涉及的實(shí)施方式。(第一實(shí)施方式)針對(duì)本發(fā)明涉及的第一實(shí)施方式進(jìn)行說(shuō)明。圖1表示了第一實(shí)施方式涉及的機(jī)械手裝置，其中，(a)是概略俯視圖，(b)是概略剖視圖。本實(shí)施方式的機(jī)械手裝置是在水平方向上能夠旋轉(zhuǎn)地連結(jié)有3個(gè)手臂的、所謂的3軸水平多關(guān)節(jié)機(jī)械手100(以下，稱(chēng)為機(jī)械手裝置100)。機(jī)械手裝置100具備手臂體10，該手臂體10構(gòu)成為將第一手臂11和第二手臂12 通過(guò)第一手臂連結(jié)裝置21可旋轉(zhuǎn)地串聯(lián)連結(jié)，并將第二手臂12和第三手臂13通過(guò)第二手臂連結(jié)裝置22可旋轉(zhuǎn)地串聯(lián)連結(jié)。手臂體10進(jìn)一步通過(guò)基體連結(jié)裝置30可旋轉(zhuǎn)地與固定在底座的基體40連結(jié)，從而構(gòu)成機(jī)械手裝置100。第一手臂連結(jié)裝置21具備致動(dòng)器51和將致動(dòng)器51的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞裝置61，第二手臂連結(jié)裝置22也同樣地具備致動(dòng)器52和轉(zhuǎn)矩傳遞裝置 62。另外，基體連結(jié)裝置30具備致動(dòng)器53和將致動(dòng)器53的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞裝置63。在作為手臂體10的與基體40相反的端部的第三手臂13的端部具有保持加工用工具或被加工物的工件保持裝置70。在第一手臂連結(jié)裝置21所具備的致動(dòng)器51上具備檢測(cè)旋轉(zhuǎn)角度的角度傳感器 81，同樣地在第二手臂連結(jié)裝置的致動(dòng)器52上具備角度傳感器82。另外，在基體連結(jié)裝置 30的致動(dòng)器53上也具備角度傳感器83。而且，在第一手臂11上具有慣性傳感器91，在第二手臂12上具有慣性傳感器92，在第三手臂13上具有慣性傳感器93。慣性傳感器91、92、 93至少包括角速度傳感器，慣性傳感器91能夠檢測(cè)第一手臂11的慣性傳感器安裝位置處的角速度，慣性傳感器92能夠檢測(cè)第二手臂12的慣性傳感器安裝位置處的角速度，慣性傳感器93能夠檢測(cè)第三手臂13的慣性傳感器安裝位置處的角速度。圖2是本實(shí)施方式涉及的框圖。由角度傳感器81、82、83檢測(cè)到的致動(dòng)器51、52、 53的旋轉(zhuǎn)角度數(shù)據(jù)被在第一計(jì)算部1100中進(jìn)行旋轉(zhuǎn)角度換算，將該旋轉(zhuǎn)角度以時(shí)間進(jìn)行1 次微分，來(lái)計(jì)算角速度。在第二計(jì)算部1200中，根據(jù)第一手臂11、第二手臂12、第三手臂13所具備的慣性傳感器91、92、93檢測(cè)到的數(shù)據(jù)來(lái)計(jì)算第一手臂11的角速度《al、第二手臂的角速度coa2、 和第三手臂的角速度"a3。比較部2000對(duì)在第一計(jì)算部1100以及第二計(jì)算部1200中計(jì)算出的角速度進(jìn)行
比較。角速度的比較方法是進(jìn)行求出下述差值的計(jì)算，即求出各致動(dòng)器的角速度、和從與該致動(dòng)器連結(jié)的手臂所具備的慣性傳感器的角速度分量中去除了具備該致動(dòng)器的連結(jié)裝置的角速度分量而得的實(shí)質(zhì)角速度之差。
作為例子，利用第二手臂12進(jìn)行說(shuō)明。將根據(jù)連結(jié)第二手臂12與第一手臂11的第一手臂連結(jié)裝置21的角度傳感器81的檢測(cè)數(shù)據(jù)而求出的角度設(shè)為θ 1，將轉(zhuǎn)矩傳遞裝置 61的減速比設(shè)為Ni，根據(jù)第一手臂11所具備的慣性傳感器91的檢測(cè)值而求出的角速度為 al，根據(jù)第二手臂12所具備的慣性傳感器92的檢測(cè)值而求出的角速度為，由此在比較部中作為比較結(jié)果求出絕對(duì)值，即下式。(ω32-ω3 )-(1 θ 1/dtXl/Nl) (式 1-1)在此t表示時(shí)間。當(dāng)微分值(1Θ Ι/dt在第一計(jì)算部1100中被計(jì)算，并將計(jì)算出的第一手臂連結(jié)裝置21的致動(dòng)器51的角速度設(shè)為ω 1時(shí)，式1能夠改寫(xiě)成下式。I (COa2-COal)-CO IX1/Ν1) I (式 1—2)根據(jù)在比較部2000求出的比較結(jié)果、例如對(duì)于第二手臂12是根據(jù)(式1_1)的值，而在判斷部3000中對(duì)作為對(duì)象的慣性傳感器是正常還是故障進(jìn)行判斷。在判斷部3000 中，預(yù)先將用于進(jìn)行故障判斷的閾值(判斷值)儲(chǔ)存到未圖示的存儲(chǔ)裝置中，根據(jù)相對(duì)于閾值比較結(jié)果的大小來(lái)進(jìn)行正常、故障的判斷。對(duì)于除上述例示的第二手臂12以外的手臂也同樣，對(duì)于第一手臂11、第三手臂13 來(lái)說(shuō)，作為與(式1-1)相當(dāng)?shù)谋容^結(jié)果的絕對(duì)值如下述那樣。在第一手臂11的情況下，比較結(jié)果如下式。ω3 -ω3Χ1/Ν3 (式 1—3)是根據(jù)第一手臂11所具備的慣性傳感器91的檢測(cè)值而求出的角速度，ω3是根據(jù)基體連結(jié)裝置30的致動(dòng)器53所具備的角度傳感器83的檢測(cè)值而計(jì)算出的角速度，Ν3 是基體連結(jié)裝置30的轉(zhuǎn)矩傳遞裝置63的減速比。在第三手臂13的情況下，比較結(jié)果如下式。((Oa3-COa2)-ω 2Χ1/Ν2) (式 1-4) a3是根據(jù)第三手臂13所具備的慣性傳感器93的檢測(cè)值而求出的角速度，《a2是根據(jù)第二手臂12所具備的慣性傳感器92的檢測(cè)值而求出的角速度，ω 2是根據(jù)第二手臂連結(jié)裝置22的致動(dòng)器52所具備的角度傳感器82的檢測(cè)值而計(jì)算出的角速度，Ν2是第二手臂連結(jié)裝置22的轉(zhuǎn)矩傳遞裝置62的減速比。在此對(duì)閾值進(jìn)行說(shuō)明。閾值是以根據(jù)作為基準(zhǔn)的第一手臂連結(jié)裝置21、第二手臂連結(jié)裝置22、基體連結(jié)裝置30所具備的致動(dòng)器51、52、53的角度傳感器81、82、83檢測(cè)到的角度數(shù)據(jù)而計(jì)算出的角速度為基準(zhǔn)而設(shè)定的。將慣性傳感器91的故障判斷的閾值設(shè)為Ω 1，慣性傳感器92的故障判斷的閾值設(shè)為Ω 2，慣性傳感器93的故障判斷的閾值設(shè)為Ω3。例如，對(duì)決定第一手臂11所具備的慣性傳感器91的故障判斷的閾值Ω 1的情況進(jìn)行說(shuō)明。使機(jī)械手裝置100持有最大負(fù)荷的120%的負(fù)荷，并以第一手臂11在最大負(fù)荷時(shí)的允許加速度的120%使第一手臂11動(dòng)作。相對(duì)于根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T時(shí)間后的慣性傳感器91的輸出值而求出的角速度ω alA與根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T時(shí)間后的基體連結(jié)裝置30的致動(dòng)器53所具備的角度傳感器83的檢測(cè)值而求出的角速度ωω1Α之差的絕對(duì)值的最大值，將閾值Ω 1設(shè)為下式。Ω1 = 2Χ|ω3 Α-ωπ1Α|ω3χ (式 1—5)同樣地，使機(jī)械手裝置100持有最大負(fù)荷的120%的負(fù)荷，并以第二手臂12在最大負(fù)荷時(shí)的允許加速度的120%使第二手臂12動(dòng)作。相對(duì)于第二手臂12的角速度、與根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T小時(shí)后的第一手臂連結(jié)裝置21的致動(dòng)器51所具備的角度傳感器81的檢測(cè)值而求出的角速度ωω2Α之差的絕對(duì)值的最大值，能夠得到閾值Ω2、即下式(式1-6)， 其中，上述第二手臂12的角速度基于根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T時(shí)間后的慣性傳感器92的輸出值而求出的角速度《a2A和、根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T小時(shí)后的慣性傳感器91的輸出值而求出的角速度《alA之間的差值。Ω 2 = 2 X I ω a2A- ω alA- ω m2A | max (式 1—6)另外，使機(jī)械手裝置100持有最大負(fù)荷的120%的負(fù)荷，并以第三手臂13在最大負(fù)荷時(shí)的允許加速度的120%來(lái)使第三手臂13動(dòng)作。相對(duì)于第三手臂13的角速度、與根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T小時(shí)后的第2手臂連結(jié)裝置22的致動(dòng)器52所具備的角度傳感器82的檢測(cè)值而求出的角速度ωω3Α之差的絕對(duì)值的最大值，能夠得到閾值Ω3、即下式(式1-7)， 其中，上述第三手臂13的角速度基于根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T小時(shí)后的慣性傳感器93的輸出值而求出的角速度《a3A和、根據(jù)從動(dòng)作開(kāi)始起經(jīng)過(guò)T小時(shí)后的慣性傳感器92的輸出值而求出的角速度《a2A之間的差值。Ω 3 = 2 X I ω a3A- ω a2A- ω m3A | max (式 1—7)由這樣求出的閾值Ω 1、Ω 2、Ω 3與上述的(式1_1)、(式1_2)、(式1_3)進(jìn)行比較，在滿(mǎn)足下述的條件時(shí)，將慣性傳感器判斷為發(fā)生了故障。慣性傳感器91故障判斷如下I ω -ω3Χ1/Ν3) I > Ω 1 (式 1—8)慣性傳感器91、92故障判斷如下I (ω -ω3 )-ω IX 1/Ν1) I > Ω2 (式 1—9)慣性傳感器92、93故障判斷如下I (Coa3-COa2)-CO 2Χ1/Ν2) I > Ω3 (式 1—10)接下來(lái)，對(duì)第一實(shí)施方式涉及的控制方法進(jìn)行說(shuō)明。圖3是說(shuō)明第一實(shí)施方式涉及的制御方法的流程圖。首先，在計(jì)算步驟(Slll，S112)中，執(zhí)行取得角度傳感器、慣性傳感器的檢測(cè)值并求出角速度的計(jì)算。在第一計(jì)算部1100的計(jì)算步驟(Slll)中，取得手臂連結(jié)裝置21、22 以及基體連結(jié)裝置30所具備的致動(dòng)器51、52、53中具備的角度傳感器81、82、83的檢測(cè)數(shù)據(jù)。將所取得的檢測(cè)數(shù)據(jù)轉(zhuǎn)換為角度數(shù)據(jù)，并將轉(zhuǎn)換后的角度數(shù)據(jù)以時(shí)間進(jìn)行微分來(lái)計(jì)算角速度。在第二計(jì)算部1200的計(jì)算步驟(SlU)中，取得第一手臂連結(jié)裝置21以及第二手臂連結(jié)裝置22所具備的慣性傳感器91、92的檢測(cè)數(shù)據(jù)和，工件保持裝置70所具備的慣性傳感器93的檢測(cè)數(shù)據(jù)。將所取得的檢測(cè)數(shù)據(jù)轉(zhuǎn)換為角速度。接下來(lái)，轉(zhuǎn)移至對(duì)在計(jì)算步驟Sill、S112中計(jì)算出的致動(dòng)器的角速度和手臂的角速度進(jìn)行比較的比較步驟(S120)。在比較步驟(S120)中，將上述的(式1-2)、(式1-3)、 (式1-4)的計(jì)算結(jié)果向接下來(lái)的判斷步驟(S130)輸出。在判斷步驟(S130)中，判斷從比較步驟(S120)輸入的計(jì)算結(jié)果是否大于預(yù)先儲(chǔ)存在未圖示的存儲(chǔ)裝置中的閾值Ω1、Ω2、Ω3。在判斷結(jié)果為“是”的情況下，即在來(lái)自比較步驟(S120)的計(jì)算結(jié)果大于閾值的情況下，判斷為相應(yīng)的慣性傳感器發(fā)生了故障。
以第一手臂11為例進(jìn)行說(shuō)明。對(duì)于第一手臂11，以基體連結(jié)裝置30所具備的角度傳感器83的檢測(cè)值為基準(zhǔn)，執(zhí)行第一手臂11所具備的慣性傳感器91的故障判斷。將在計(jì)算步驟(S111、S112)中根據(jù)角度傳感器83的檢測(cè)值計(jì)算出的第一手臂11的角速度《a3/ N3和、由慣性傳感器91得到的第一手臂11的角速度ω 1在比較步驟(S120)中通過(guò)(式 1-8)而得到的比較結(jié)果進(jìn)行輸出，在判斷步驟(S130)中判斷其是否是大于閾值Ω1的值。 即，在判斷結(jié)果為下式的情況下，判斷為慣性傳感器91發(fā)生了故障。I ω -ω3Χ1/Ν3) I > Ω 1 (式 1—8)同樣地，對(duì)于第二手臂12以及第三手臂13，當(dāng)判斷結(jié)果在第二手臂12的情況下成為下式時(shí)，判斷為慣性傳感器91或慣性傳感器92的其中一方、或雙方發(fā)生了故障。(ω -ω3 )-ω IX 1/Ν1) I > Ω2 (式 1-9)當(dāng)判斷結(jié)果在第三手臂的情況下成為下式時(shí)，判斷為慣性傳感器92或慣性傳感器93的其中一方、或雙方發(fā)生了故障。I (ω a3- ω a2) - ω 2 X 1/N2) | > Ω 3 (式 1—10)在判斷步驟(S130)中，在判斷為沒(méi)有發(fā)生故障(否)的情況下，再次返回計(jì)算步驟(S111、S112)，并反復(fù)進(jìn)行慣性傳感器的故障判斷。當(dāng)在判斷步驟(S130)中判斷為發(fā)生了故障(是)時(shí)，轉(zhuǎn)移至停止指示步驟 (S140)。在停止指示步驟(S140)中，向控制部4000發(fā)送出使致動(dòng)器51、52、53的動(dòng)作停止的指令，由控制部4000向致動(dòng)器51、52、53發(fā)送停止信號(hào)，動(dòng)作從而停止。即、機(jī)械手裝置 100的動(dòng)作被停止。然后，將被判斷為故障的慣性傳感器從機(jī)械手裝置卸下，實(shí)施必要的修理或者傳感器更換。由于如上述那樣慣性傳感器的故障的判斷基準(zhǔn)是根據(jù)各連結(jié)裝置所具備的致動(dòng)器的角度傳感器的檢測(cè)數(shù)據(jù)而得到的角速度，所以判斷基準(zhǔn)本身也是與手臂體10的動(dòng)作連動(dòng)地發(fā)生變動(dòng)的值，由于以更實(shí)際的動(dòng)作為基準(zhǔn)進(jìn)行故障判斷，因此能夠進(jìn)行準(zhǔn)確的慣性傳感器的故障判斷。由此，能夠可靠地避免因機(jī)械手裝置的失控帶來(lái)的危險(xiǎn)，保證因早期的正?；玫降姆€(wěn)定的機(jī)械手裝置動(dòng)作，并維持較高的產(chǎn)品品質(zhì)。(第二實(shí)施方式)第二實(shí)施方式相對(duì)于第一實(shí)施方式的不同之處在于慣性傳感器的配置和成為判斷基準(zhǔn)的檢測(cè)數(shù)據(jù)。因此，對(duì)與第一實(shí)施方式共同的構(gòu)成標(biāo)注相同的附圖標(biāo)記來(lái)進(jìn)行說(shuō)明。圖4表示了第二實(shí)施方式涉及的機(jī)械手裝置，其中，(a)是概略俯視圖，(b)是概略剖視圖。本實(shí)施方式的機(jī)械手裝置是，在水平方向上能夠旋轉(zhuǎn)地連結(jié)有3個(gè)手臂的、所謂的 3軸水平多關(guān)節(jié)機(jī)械手200(以下，稱(chēng)為機(jī)械手裝置200)。第二實(shí)施方式涉及的機(jī)械手裝置200相對(duì)于第一實(shí)施方式涉及的機(jī)械手裝置100 的不同之處在于在各手臂上具備2個(gè)慣性傳感器。在第一手臂11上具有慣性傳感器91a 和91b，在第二手臂12上具有慣性傳感器9 和92b，在第三手臂13上具有慣性傳感器93a 和93b。這些慣性傳感器被配置成相互接近，并且使用標(biāo)準(zhǔn)性能相同的慣性傳感器。圖5是第二實(shí)施方式涉及的框圖。在計(jì)算部1300中，從手臂體10所具備的慣性傳感器91a、91b、92a、92b、93a、93b中，按tl tn的每個(gè)時(shí)間取得檢測(cè)數(shù)據(jù)。即，慣性傳感器91a中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為α lal、α la2、α la3、 、α lan
慣性傳感器91b中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為α lbl、α lb2、α lb3、 、α lbn慣性傳感器92a中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為a2al、a2a2、a2a3、 、a2an慣性傳感器92b中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為a2bl> a2b2> a2b3>~> α 2bn慣性傳感器93a中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為α 3al、α 3a2、α 3a3、 、α 3an慣性傳感器93b中的tl tn時(shí)間的η個(gè)檢測(cè)數(shù)據(jù)生成為α 3bl> α 3b2> α 3b3>~> α 3bn計(jì)算所取得的各慣性傳感器9la、9lb、92a、92b、93a、9 的各η個(gè)檢測(cè)數(shù)據(jù)的平均
值。即，慣性傳感器91a的檢測(cè)數(shù)據(jù)平均值α lam為α lam = ( α lal+ α la2+ α la3+ + α lan) /n慣性傳感器91b的檢測(cè)數(shù)據(jù)平均值α 為Q Ibm = ( Q Ibl+ Q Ib2+ 0 Ib3+ + α lbn) A 慣性傳感器92a的檢測(cè)數(shù)據(jù)平均值α 2am為 a2am = ( α 2al+ Q 2a2+ Q 2a3+ + α 2 J A 慣性傳感器92b的檢測(cè)數(shù)據(jù)平均值α 2tal為 a2bm = ( α 2bl+ Q 2b2+ Q 2b3+ + α 2 J A 慣性傳感器93a的檢測(cè)數(shù)據(jù)平均值α 3am為 Q 3am = ( α 3al+ Q 3a2+ Q 3a3+ + α 3 J A 慣性傳感器9 的檢測(cè)數(shù)據(jù)平均值α 3tal為Q 3bm = ( α 3bl+ Q 3b2+ Q 3b3+ + α 3bn) A此外，數(shù)據(jù)個(gè)數(shù)η優(yōu)選取100以上，根據(jù)進(jìn)行計(jì)算的CPU的性能等條件適當(dāng)?shù)貨Q定數(shù)據(jù)個(gè)數(shù)η即可。在比較部2100中，根據(jù)所得到的各檢測(cè)數(shù)據(jù)平均值，求出接近的慣性傳感器的檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值。例如，在第一手臂11中慣性傳感器91a和91b是接近的慣性傳感器，求出其檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值α ld，即為下式。Ialam-CilbmI = Cild (式 2—1)同樣地，對(duì)于第二手臂12、第三手臂13，也求出接近的慣性傳感器的檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值a2d、Ci3d、即為下式。I a2am-a2bm| = a2d (式 2—2)O3am-Q3bJ = a3d (式 2-3)接下來(lái)，在判斷部3100中，根據(jù)在比較部2100中求出的、作為比較結(jié)果的各慣性傳感器的檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值ald、a2d、Ci3d，對(duì)作為對(duì)象的慣性傳感器是正常還是故障進(jìn)行判斷。在判斷部3100中，預(yù)先將用于進(jìn)行故障判斷的閾值儲(chǔ)存到未圖示的存儲(chǔ)裝置中，根據(jù)相對(duì)于閾值比較結(jié)果的大小來(lái)進(jìn)行正常、故障的判斷。接著對(duì)閾值進(jìn)行說(shuō)明。閾值是根據(jù)各慣性傳感器的特性如下述那樣決定的。艮口，利用在安裝到機(jī)械手裝置200之前的慣性傳感器91a、91b、92a、92b、93a、9;3b或與安裝的慣性傳感器91a、91b、9h、92b、93a、9;3b相同標(biāo)準(zhǔn)的未使用的慣性傳感器，測(cè)量各慣性傳感器的白噪聲(白色噪聲)。白噪聲例如如圖6所示那樣具有不規(guī)則地振動(dòng)的波形。求出所測(cè)量的各慣性傳感器的白噪聲的標(biāo)準(zhǔn)偏差。例如，對(duì)于針對(duì)第一手臂11所具備的慣性傳感器91a、91b的閾值，根據(jù)利用在安裝到機(jī)械手裝置200之前的慣性傳感器91a、91b，或與慣性傳感器91a、91b標(biāo)準(zhǔn)相同的未使用的慣性傳感器而測(cè)量出的白噪聲的值，求出標(biāo)準(zhǔn)偏差ο 1。以所求出的標(biāo)準(zhǔn)偏差ο 1為基準(zhǔn)，將第一手臂11所具備的慣性傳感器91a、91b的故障判斷的閾值Sl設(shè)定為下式。Sl = 6X σ 1同樣地，根據(jù)第二手臂12所具備的慣性傳感器92a、92b的白噪聲的標(biāo)準(zhǔn)偏差ο 2 設(shè)定閾值S2，根據(jù)第三手臂13所具備的慣性傳感器93a、9!3b的白噪聲的標(biāo)準(zhǔn)偏差σ 3設(shè)定閾值S3，閾值S2和閾值S3分別為下式。S2 = 6Χ σ 2S3 = 6Χ σ 3根據(jù)這樣設(shè)定的閾值Si、S2、S3和在比較部2100得到的利用(式2_1)(式2_2) (式2- 求出的、各慣性傳感器的檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值α ld、α 2d、α 3d的大小來(lái)進(jìn)行故障判斷。即，在α ld > Sl的情況下，判斷為慣性傳感器91a、91b的其中一方或雙方發(fā)生了故障，在a2d > S2的情況下，判斷為慣性傳感器92a、92b的其中一方或雙方發(fā)生了故障，在α 3d > S3的情況下，判斷為慣性傳感器93a、93b的其中一方或雙方發(fā)生了故障。接下來(lái)，對(duì)第二實(shí)施方式涉及的控制方法進(jìn)行說(shuō)明。圖7是說(shuō)明第二實(shí)施方式涉及的制御方法的流程圖。首先，在計(jì)算步驟(S210)中，取得來(lái)自慣性傳感器的檢測(cè)數(shù)據(jù)，并計(jì)算檢測(cè)數(shù)據(jù)的平均值α lam、α lbm、α 2am、α 2bm、α 3am、α 3bm。接下來(lái)，轉(zhuǎn)移至比較步驟(S220)。在比較步驟(S220)中，根據(jù)在計(jì)算步驟(S210) 計(jì)算出的檢測(cè)數(shù)據(jù)的平均值α lam、α lbm、α 2am、α 2bm、α 3am、α 3bm，計(jì)算接近的慣性傳感器91a 和91b的檢測(cè)數(shù)據(jù)的平均值Cilam和Ciltal之差的絕對(duì)值Cild(式2-1)作為比較值。同樣地， 將接近的慣性傳感器9 和92b的檢測(cè)數(shù)據(jù)的平均值α 2am和α 2bm之差的絕對(duì)值α 2d(式 2-2)、接近的慣性傳感器93a和9 的檢測(cè)數(shù)據(jù)的平均值α 3am和α 3bm之差的絕對(duì)值α 3d(式 2-3)作為比較值進(jìn)行計(jì)算。接下來(lái)，轉(zhuǎn)移至判斷步驟(S230)。在判斷步驟(S230)中，對(duì)來(lái)自比較步驟(S220) 的計(jì)算結(jié)果是否大于預(yù)先儲(chǔ)存在未圖示的存儲(chǔ)裝置中的閾值Si、S2、S3進(jìn)行判斷。在判斷結(jié)果為“是”的情況下，即在來(lái)自比較步驟(S220)的計(jì)算結(jié)果大于閾值的情況下，判斷為相應(yīng)的慣性傳感器發(fā)生了故障。以第一手臂11為例進(jìn)行說(shuō)明。在第一手臂11中，慣性傳感器91a、91b成為判斷的對(duì)象，在檢測(cè)數(shù)據(jù)平均值之差的絕對(duì)值α ld相對(duì)于閾值Sl成為下式的情況下，判斷為慣性傳感器91a、91b的其中一方或雙方發(fā)生了故障。α ld > Sl同樣地，在第二手臂12中，在成為下式的情況下，判斷為慣性傳感器92a、92b中的其中一方或雙方發(fā)生了故障，α 2d > S2
在第三手臂13中，在成為下式的情況下，判斷為慣性傳感器93a、93b中的其中一方或雙方發(fā)生了故障。α 3d > S3在判斷步驟(S230)中，在判斷為沒(méi)有發(fā)生故障、即“否”的情況下，再次返回計(jì)算步驟(S210)，反復(fù)進(jìn)行慣性傳感器的故障判斷。在判斷步驟(S230)中，當(dāng)判斷為發(fā)生了故障(是)時(shí)，轉(zhuǎn)移至停止指示步驟 (SMO)。在停止指示步驟(SMO)中，向控制部4000發(fā)送出使致動(dòng)器51、52、53的動(dòng)作停止的指令，由控制部4000向致動(dòng)器51、52、53發(fā)送停止信號(hào)，動(dòng)作從而停止。即機(jī)械手裝置 200的動(dòng)作被停止。然后，將判斷為故障的慣性傳感器從機(jī)械手裝置卸下，實(shí)施必要的修理或者傳感器更換。如上述那樣，成為慣性傳感器的故障的判斷基準(zhǔn)的是，在手臂上至少具備2個(gè)以上的多個(gè)慣性傳感器，對(duì)相同的手臂上所具備的慣性傳感器的輸出進(jìn)行比較，通過(guò)將傳感器具有的白噪聲的標(biāo)準(zhǔn)偏差作為判斷基準(zhǔn)，能夠進(jìn)行準(zhǔn)確的慣性傳感器的故障判斷。由此， 能夠可靠地避免因機(jī)械手裝置的失控帶來(lái)的危險(xiǎn)，保證因早期的正?；玫降姆€(wěn)定的機(jī)械手裝置動(dòng)作，并維持較高的產(chǎn)品品質(zhì)。(第三實(shí)施方式)在第一實(shí)施方式以及第二實(shí)施方式中，通過(guò)所謂的水平多關(guān)節(jié)機(jī)械手裝置進(jìn)行了說(shuō)明，而在第三實(shí)施方式中，說(shuō)明直動(dòng)式機(jī)械手裝置中的傳感器的故障判斷。圖8是第三實(shí)施方式涉及的機(jī)械手裝置1000的概略俯視圖。另外，圖9是從圖8 中的箭頭P方向觀察的剖視圖外觀的概略圖。圖8中為了明確機(jī)械手裝置1000的構(gòu)成，而省略了覆蓋主要部分的框體等的圖示。機(jī)械手裝置1000具備固定于操作臺(tái)或者地板等的基體300、可移動(dòng)地被固定在基體300上的手臂400、和可移動(dòng)地固定在手臂400上且具備保持加工用工具或被加工物的工件保持部520的工件保持裝置500。基體300具備基體致動(dòng)器320、將基體致動(dòng)器320的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的基體轉(zhuǎn)矩傳遞裝置310、與基體轉(zhuǎn)矩傳遞裝置310的旋轉(zhuǎn)軸連結(jié)的手臂驅(qū)動(dòng)帶340、將手臂驅(qū)動(dòng)帶340可拉動(dòng)旋轉(zhuǎn)地固定的基體滑輪350。另外，在基體致動(dòng)器320上具有檢測(cè)旋轉(zhuǎn)角度的角度傳感器330。手臂400具備手臂致動(dòng)器420、將手臂致動(dòng)器420的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的手臂轉(zhuǎn)矩傳遞裝置410、與手臂轉(zhuǎn)矩傳遞裝置410的旋轉(zhuǎn)軸連結(jié)的工件保持裝置驅(qū)動(dòng)帶440、將工件保持裝置驅(qū)動(dòng)帶440可拉動(dòng)旋轉(zhuǎn)地固定的手臂滑輪450。另外，在手臂致動(dòng)器420上具備檢測(cè)旋轉(zhuǎn)角度的角度傳感器430?？梢苿?dòng)地固定在手臂400上的工件保持裝置500中，具備至少具有2個(gè)加速度傳感器530a、530b的慣性傳感器530，通過(guò)未圖示的驅(qū)動(dòng)裝置來(lái)驅(qū)動(dòng)工件保持部520。另外，手臂400上具備固定在基體300所具備的手臂驅(qū)動(dòng)帶340上的手臂固定裝置460，通過(guò)手臂驅(qū)動(dòng)帶340的動(dòng)作而使手臂400動(dòng)作。另外，工件保持裝置500上具備固定在手臂400所具備的工件保持裝置驅(qū)動(dòng)帶440上的工件保持裝置固定裝置510，通過(guò)工件保持裝置驅(qū)動(dòng)帶440的動(dòng)作而使工件保持裝置500動(dòng)作。這樣構(gòu)成的機(jī)械手裝置1000能夠使手臂400以及工件保持裝置500在圖8以及圖9的箭頭Q、R所示的方向上移動(dòng)，并在規(guī)定的位置進(jìn)行操作。
圖10是第三實(shí)施方式涉及的框圖。在第一計(jì)算部1400中，將通過(guò)角度傳感器330、 430檢測(cè)到的基體致動(dòng)器320、手臂致動(dòng)器420的檢測(cè)數(shù)據(jù)換算為基體致動(dòng)器320的旋轉(zhuǎn)角度θ a、手臂致動(dòng)器的旋轉(zhuǎn)角度eb。對(duì)所得到的旋轉(zhuǎn)角度ea、Θ b以時(shí)間進(jìn)行2次微分，來(lái)計(jì)算下式的旋轉(zhuǎn)角加速度。d2 θ Jdt2 = ω/ (式 3-1)d2 6b/dt2 = ω,' (式 3-2)在將把基體致動(dòng)器320的旋轉(zhuǎn)角度經(jīng)由基體轉(zhuǎn)矩傳遞裝置310而轉(zhuǎn)換為手臂驅(qū)動(dòng)帶340的移動(dòng)長(zhǎng)度的轉(zhuǎn)換系數(shù)設(shè)為Kl的情況下，手臂驅(qū)動(dòng)帶340的移動(dòng)加速度通過(guò)下式被求出。= ω3' /Kl (式 3-3)同樣地，在將把手臂致動(dòng)器420的旋轉(zhuǎn)角度經(jīng)由手臂轉(zhuǎn)矩傳遞裝置410而轉(zhuǎn)換為工件保持裝置驅(qū)動(dòng)帶440的移動(dòng)長(zhǎng)度的轉(zhuǎn)換系數(shù)設(shè)為Κ2的情況下，工件保持裝置驅(qū)動(dòng)帶 440的移動(dòng)加速度^^通過(guò)下式求出。^b= ω/ /Κ2 (式 3-4)在第二計(jì)算部1500中，根據(jù)工件保持裝置500所具備的慣性傳感器530中包含的加速度傳感器530a、530b檢測(cè)出的數(shù)據(jù)來(lái)計(jì)算加速度。在此，配置成在加速度傳感器530a 例如檢測(cè)圖9所示的Q方向的加速度分量的情況下，另一個(gè)加速度傳感器530b檢測(cè)R方向的加速度。或者也可以利用3軸加速度傳感器、從而使用正交的2軸方向的檢測(cè)數(shù)據(jù)。得到根據(jù)加速度傳感器530a的Q方向的檢測(cè)數(shù)據(jù)換算出的加速度α a、和根據(jù)加速度傳感器530b的R方向的檢測(cè)數(shù)據(jù)而換算出的加速度α b。在比較部2200中，對(duì)在第一計(jì)算部1400中得到的驅(qū)動(dòng)帶的移動(dòng)加速度β a、^^和在第二計(jì)算部1500中得到的工件保持裝置500的加速度α a、α b進(jìn)行比較。比較部2200 中的比較是指計(jì)算加速度差的絕對(duì)值，圖8所示的Q方向?yàn)橄率溅?-β J而R方向?yàn)橄率溅?-β,在判斷部3200中，在從上述的比較部2200輸出的比較結(jié)果、即加速度差的絕對(duì)值大于閾值的情況下，判斷為成為對(duì)象的慣性傳感器發(fā)生了故障。在此，對(duì)閾值進(jìn)行說(shuō)明。首先，加速度傳感器530a的故障判斷的閾值A(chǔ)l如下述那樣被設(shè)定。使機(jī)械手裝置1000持有最大負(fù)荷的120%的負(fù)荷，并以工件保持裝置的向著圖8所示的Q方向的最大負(fù)荷時(shí)的允許加速度的120%的加速度，使工件保持裝置500在Q方向上動(dòng)作。求出根據(jù)從動(dòng)作開(kāi)始經(jīng)過(guò)T時(shí)間后的加速度傳感器530a的輸出值而求出的工件保持裝置500的加速度α at，和根據(jù)從動(dòng)作開(kāi)始經(jīng)過(guò)T時(shí)間后的使工件保持裝置500在Q方向上移動(dòng)的基體致動(dòng)器320所具備的角度傳感器330的旋轉(zhuǎn)角度而計(jì)算出的手臂驅(qū)動(dòng)帶340的移動(dòng)加速度 3at之差。該aat和β at之差的絕對(duì)值的最大值的2倍成為閾值A(chǔ)l。S卩，通過(guò)下式得到閾值A(chǔ)l。Al = 2X (| Qat-^atDmax (式 3-5)同樣地，對(duì)于加速度傳感器530b的故障判斷的閾值A(chǔ)2，使機(jī)械手裝置1000持有最大負(fù)荷的120%的負(fù)荷，以工件保持裝置的向著圖8所示的R方向的最大負(fù)荷時(shí)的允許加速度的120%的加速度，使工件保持裝置500在R方向上動(dòng)作。求出根據(jù)從動(dòng)作開(kāi)始經(jīng)過(guò)T 時(shí)間后的加速度傳感器530b的輸出值而求出的工件保持裝置500的加速度α bt、和根據(jù)從動(dòng)作開(kāi)始經(jīng)過(guò)T時(shí)間后的使工件保持裝置500在R方向上移動(dòng)的手臂致動(dòng)器420所具備的角度傳感器430的旋轉(zhuǎn)角度而計(jì)算出的工件保持裝置驅(qū)動(dòng)帶440的移動(dòng)加速度之差。 該abt和β bt之差的絕對(duì)值的最大值的2倍成為閾值A(chǔ)2。即，通過(guò)下式得到閾值A(chǔ)2。A2 = 2X (| Qbt-^btDmax (式 3-6)判斷部3200利用由(式3-5)、(式3-6)求出的閾值A(chǔ)1、A2和從比較部2200得到的比較結(jié)果來(lái)執(zhí)行故障判斷。對(duì)于加速度傳感器530a，在成為如下情況時(shí)，判斷為加速度傳感器530a發(fā)生了故障。I α a- β a > Al同樣地，對(duì)于加速度傳感器530b，在成為如下情況時(shí)，判斷為加速度傳感器530b 發(fā)生了故障。α,-β, I > A2在判斷部3200中，在判斷為加速度傳感器530a或530b發(fā)生了故障的情況下，向控制部4000發(fā)送使基體致動(dòng)器320或手臂致動(dòng)器420停止的指示，控制部4000使基體致動(dòng)器320或者手臂致動(dòng)器420停止。由此，能夠使機(jī)械手裝置1000安全地停止。接下來(lái)，對(duì)第三實(shí)施方式涉及的控制方法進(jìn)行說(shuō)明。表示第三實(shí)施方式涉及的控制方法的流程圖與第一實(shí)施方式相同，可以使用圖3所示的流程圖。因此，省略與第一實(shí)施方式相同的部分的詳細(xì)說(shuō)明，對(duì)與第一實(shí)施方式的不同點(diǎn)進(jìn)行說(shuō)明。在第三實(shí)施方式涉及的控制方法中，在第一計(jì)算步驟(Slll)中，對(duì)來(lái)自基體致動(dòng)器320所具備的角度傳感器330的旋轉(zhuǎn)角度數(shù)據(jù)進(jìn)行計(jì)算，計(jì)算手臂驅(qū)動(dòng)帶340的移動(dòng)加速度β a、和來(lái)自手臂致動(dòng)器420所具備的角度傳感器430的旋轉(zhuǎn)角度，計(jì)算工件保持裝置驅(qū)動(dòng)帶440的移動(dòng)加速度β b，并轉(zhuǎn)移至比較步驟(S120)。在第二計(jì)算步驟(SlU)中，根據(jù)工件保持裝置500所具備的慣性傳感器530中包含的加速度傳感器530a、530b檢測(cè)出的數(shù)據(jù)計(jì)算加速度α a、ab，并轉(zhuǎn)移至比較步驟 (S120)。接下來(lái)，轉(zhuǎn)移至比較步驟(S120)。在比較步驟(S120)中，使用在第一計(jì)算步驟 (Slll)以及第二計(jì)算步驟(S112)中得到的加速度值，將下述的值作為比較結(jié)果向判斷步驟(S130)輸出，并轉(zhuǎn)移至判斷步驟(S130)。α3-β Jα,-β,接下來(lái)，轉(zhuǎn)移至判斷步驟(S 130)。在判斷步驟(S 130)中，判斷從比較步驟(S 120) 輸入的比較結(jié)果是否大于預(yù)先儲(chǔ)存在未圖示的存儲(chǔ)裝置中的閾值A(chǔ)l，Α2。在判斷結(jié)果是 “是”的情況下，即在來(lái)自比較步驟(S120)的比較結(jié)果大于閾值的情況下，判斷為相應(yīng)的加速度傳感器發(fā)生了故障。SP，在I α a- β a > Al或者，I α,-β, >A2的情況下，判斷為加速度傳感器530a或530b發(fā)生了故障。
在判斷步驟(S130)中，在判斷為是故障判定、即是“是”的情況下，轉(zhuǎn)移至停止指示步驟(S140)。在停止指示步驟(S140)中，從控制部4000向基體致動(dòng)器320、手臂致動(dòng)器 420輸出停止動(dòng)作的指示，機(jī)械手裝置1000從而停止。如上述那樣，成為慣性傳感器的故障的判斷基準(zhǔn)是，根據(jù)基體以及手臂所具備的致動(dòng)器的角度傳感器的檢測(cè)數(shù)據(jù)而得到的驅(qū)動(dòng)帶的移動(dòng)加速度，所以判斷基準(zhǔn)本身也是與工件保持裝置500的動(dòng)作連動(dòng)地發(fā)生變動(dòng)的值，由于以更實(shí)際的動(dòng)作為基準(zhǔn)進(jìn)行故障判斷，因此能夠進(jìn)行準(zhǔn)確的加速度傳感器的故障判斷。由此，能夠可靠地避免因機(jī)械手裝置的失控而帶來(lái)的危險(xiǎn)，保證因早期的正?；玫降姆€(wěn)定的機(jī)械手裝置動(dòng)作，并維持較高的廣品品質(zhì)。
權(quán)利要求
1.一種機(jī)械手裝置，其特征在于，具備手臂連結(jié)裝置，其具有致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的； 基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器； 慣性傳感器，其安裝在上述手臂上；第一計(jì)算部，其根據(jù)來(lái)自上述角度傳感器的上述致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述致動(dòng)器的角速度以及角加速度；第二計(jì)算部，其根據(jù)由上述慣性傳感器檢測(cè)到的輸出，計(jì)算上述手臂的角速度或者角加速度；比較部，其對(duì)由上述第一計(jì)算部計(jì)算出的上述致動(dòng)器的角速度或者角加速度、和由上述第二計(jì)算部計(jì)算出的上述手臂的角速度或者角加速度進(jìn)行比較；故障判斷部，當(dāng)在上述比較部中，上述致動(dòng)器的角速度或者角加速度與上述手臂的角速度或者角加速度之差的絕對(duì)值大于閾值Ω時(shí)，該故障判斷部判斷為上述慣性傳感器發(fā)生了故障，并輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。
2.根據(jù)權(quán)利要求1所述的機(jī)械手裝置，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的120%的負(fù)荷、且以最大負(fù)荷時(shí)的允許加速度的 120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述第一計(jì)算部計(jì)算的角速度為Os、由上述第二計(jì)算部計(jì)算的角速度為時(shí)，相對(duì)于cos和之差的絕對(duì)值的最大值，上述閾值Ω是Ω =2Χ(|ω5-ω,|)π3χ0
3.一種機(jī)械手裝置，其特征在于，具備手臂連結(jié)裝置，其具有致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的； 基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；至少2個(gè)慣性傳感器，被安裝在上述手臂上； 計(jì)算部，其存儲(chǔ)上述慣性傳感器檢測(cè)出的多個(gè)輸出值，并計(jì)算平均值； 比較部，其對(duì)由上述計(jì)算部計(jì)算出的上述輸出值的平均值進(jìn)行比較； 故障判斷部，當(dāng)在上述比較部中，2個(gè)上述輸出值的平均值之差的絕對(duì)值中存在大于閾值S的上述差時(shí)，該故障判斷部判斷為上述慣性傳感器中的任意一個(gè)以上發(fā)生了故障，并輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。
4.根據(jù)權(quán)利要求3所述的機(jī)械手裝置，其特征在于，在上述慣性傳感器的白噪聲的標(biāo)準(zhǔn)偏差是σ時(shí)，上述閾值S為 S = 6 σ。
5.一種機(jī)械手裝置，其特征在于，具備基體，其具備基體致動(dòng)器、將上述基體致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述基體致動(dòng)器的旋轉(zhuǎn)角度的基體角度傳感器、和手臂驅(qū)動(dòng)裝置，該手臂驅(qū)動(dòng)裝置具有手臂連結(jié)裝置，與上述轉(zhuǎn)矩傳遞機(jī)構(gòu)連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述手臂連結(jié)裝置；手臂，其與上述基體的上述手臂連結(jié)裝置連結(jié)，具有手臂致動(dòng)器、將上述手臂致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述手臂致動(dòng)器的旋轉(zhuǎn)角度的手臂角度傳感器、和工件保持裝置驅(qū)動(dòng)裝置，該工件保持裝置驅(qū)動(dòng)裝置具有工件保持裝置，與上述手臂連結(jié)裝置連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述工件保持裝置；2個(gè)以上慣性傳感器，其安裝在上述工件保持裝置上，至少包含加速度傳感器； 第一計(jì)算部，其根據(jù)來(lái)自上述基體角度傳感器的上述基體致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述手臂驅(qū)動(dòng)裝置的加速度，并根據(jù)來(lái)自上述手臂角度傳感器的上述手臂致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述工件保持裝置驅(qū)動(dòng)裝置的加速度；第二計(jì)算部，其根據(jù)由上述慣性傳感器檢測(cè)到的輸出，計(jì)算上述工件保持裝置的加速度；比較部，其對(duì)由上述第一計(jì)算部計(jì)算出的上述加速度和由上述第二計(jì)算部計(jì)算出的上述加速度進(jìn)行比較；故障判斷部，其在由上述第一計(jì)算部計(jì)算出的上述加速度和由上述第二計(jì)算部計(jì)算出的上述加速度之差的絕對(duì)值大于閾值A(chǔ)的情況下，判斷為上述慣性傳感器發(fā)生了故障，并輸出使上述基體致動(dòng)器以及上述手臂致動(dòng)器的動(dòng)作停止的信號(hào)。
6.根據(jù)權(quán)利要求5所述的機(jī)械手裝置，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的 120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述第一計(jì)算部計(jì)算的加速度為Cis、由上述第二計(jì)算部計(jì)算的加速度為時(shí)，相對(duì)于％和C^之差的絕對(duì)值的最大值，上述閾值A(chǔ)為 A = 2X (I α s- α L| )max。
7.一種機(jī)械手裝置的控制方法，其特征在于， 該機(jī)械手裝置具備手臂連結(jié)裝置，其具有致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的； 基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、和檢測(cè)上述致動(dòng)器的旋轉(zhuǎn)角度的角度傳感器；和安裝在上述手臂上的慣性傳感器， 該機(jī)械手裝置的控制方法包括計(jì)算步驟，根據(jù)來(lái)自上述角度傳感器的上述致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)來(lái)計(jì)算上述致動(dòng)器的角速度以及角加速度，并根據(jù)由上述慣性傳感器檢測(cè)到的輸出來(lái)計(jì)算上述手臂的角速度或者角加速度；比較步驟，對(duì)通過(guò)上述計(jì)算步驟而計(jì)算出的上述致動(dòng)器的角速度或者角加速度、與上述手臂的角速度或者角加速度進(jìn)行比較；判斷步驟，當(dāng)在上述比較步驟中上述致動(dòng)器的角速度或者角加速度與上述手臂的角速度或者角加速度之差的絕對(duì)值大于閾值Ω時(shí)，判斷為上述慣性傳感器發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為發(fā)生了故障時(shí)，輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。
8.根據(jù)權(quán)利要求7所述的機(jī)械手裝置的控制方法，其特征在于，在承受上述機(jī)械手裝置的最大負(fù)荷的120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)在上述計(jì)算步驟中計(jì)算的上述致動(dòng)器的角速度為 、上述手臂的角速度為時(shí)，相對(duì)于0^和之差的絕對(duì)值的最大值，上述閾值Ω為Ω =2Χ(|ω5-ω,|)π3χ0
9.一種機(jī)械手裝置的控制方法，其特征在于， 該機(jī)械手裝置具備手臂連結(jié)裝置，其具有致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；手臂體，其是由多個(gè)手臂通過(guò)上述手臂連結(jié)裝置串聯(lián)且可旋轉(zhuǎn)地被連結(jié)而形成的； 基體，上述手臂體通過(guò)設(shè)置在上述手臂體的一個(gè)端部的基體連結(jié)裝置被可旋轉(zhuǎn)地連結(jié)在該基體上，該基體連結(jié)裝置具備上述致動(dòng)器、和將上述致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)；和至少2個(gè)慣性傳感器，其安裝在上述手臂上， 該機(jī)械手裝置的控制方法包括計(jì)算步驟，存儲(chǔ)上述慣性傳感器檢測(cè)到的多個(gè)輸出值，并計(jì)算平均值； 比較步驟，對(duì)由上述計(jì)算步驟計(jì)算出的上述輸出值的平均值進(jìn)行比較； 判斷步驟，當(dāng)在上述比較步驟中，2個(gè)上述輸出值的平均值之差的絕對(duì)值中存在大于閾值S的上述差時(shí)，判斷為上述慣性傳感器中的任意一個(gè)以上發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為故障時(shí)，輸出使上述致動(dòng)器的動(dòng)作停止的信號(hào)。
10.根據(jù)權(quán)利要求9所述的機(jī)械手裝置的控制方法，其特征在于， 在上述慣性傳感器的白噪聲的標(biāo)準(zhǔn)偏差是σ時(shí)，上述閾值S為 S = 6 σ。
11.一種機(jī)械手裝置的控制方法，其特征在于， 該機(jī)械手裝置具備基體，其具備基體致動(dòng)器、將上述基體致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述基體致動(dòng)器的旋轉(zhuǎn)角度的基體角度傳感器、和手臂驅(qū)動(dòng)裝置，該手臂驅(qū)動(dòng)裝置具有手臂連結(jié)裝置，與上述轉(zhuǎn)矩傳遞機(jī)構(gòu)連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述手臂連結(jié)裝置；手臂，其與上述基體的上述手臂連結(jié)裝置連結(jié)，具有手臂致動(dòng)器、將上述手臂致動(dòng)器的轉(zhuǎn)矩以規(guī)定的減速比進(jìn)行傳遞的轉(zhuǎn)矩傳遞機(jī)構(gòu)、檢測(cè)上述手臂致動(dòng)器的旋轉(zhuǎn)角度的手臂角度傳感器、和工件保持裝置驅(qū)動(dòng)裝置，該工件保持裝置驅(qū)動(dòng)裝置具有工件保持裝置，與上述手臂連結(jié)裝置連結(jié)，并直線(xiàn)往返驅(qū)動(dòng)上述工件保持裝置， 該機(jī)械手裝置的控制方法包括計(jì)算步驟，根據(jù)來(lái)自上述基體角度傳感器的上述基體致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)計(jì)算上述手臂驅(qū)動(dòng)裝置的加速度，根據(jù)來(lái)自上述手臂角度傳感器的上述手臂致動(dòng)器的旋轉(zhuǎn)角度數(shù)據(jù)計(jì)算上述工件保持裝置驅(qū)動(dòng)裝置的加速度，并根據(jù)由安裝在上述工件保持裝置上、且至少具有加速度傳感器的一個(gè)以上慣性傳感器檢測(cè)到的輸出，計(jì)算上述工件保持裝置的速度以及加速度；比較步驟，對(duì)由上述計(jì)算步驟計(jì)算出的上述手臂驅(qū)動(dòng)裝置、上述工件保持裝置驅(qū)動(dòng)裝置的加速度、和上述工件保持裝置的加速度進(jìn)行比較；判斷步驟，當(dāng)在上述比較步驟中，根據(jù)上述角度傳感器的輸出值計(jì)算出的上述手臂驅(qū)動(dòng)裝置、上述工件保持裝置驅(qū)動(dòng)裝置的加速度與根據(jù)上述慣性傳感器的輸出值計(jì)算出的上述工件保持裝置的加速度之差的絕對(duì)值大于閾值A(chǔ)時(shí)，判斷為上述慣性傳感器發(fā)生了故障；停止指示步驟，當(dāng)在上述判斷步驟中判斷為發(fā)生了故障時(shí)，輸出使上述基體致動(dòng)器以及上述手臂致動(dòng)器的動(dòng)作停止的信號(hào)。
12.根據(jù)權(quán)利要求11所述的機(jī)械手裝置的控制方法，其特征在于， 在承受上述機(jī)械手裝置的最大負(fù)荷的120%的負(fù)荷，并以最大負(fù)荷時(shí)的允許加速度的 120%的加速度進(jìn)行動(dòng)作的情況下，當(dāng)由上述計(jì)算步驟計(jì)算的上述手臂驅(qū)動(dòng)裝置或上述工件保持裝置驅(qū)動(dòng)裝置的加速度為α s、上述工件保持裝置的加速度為C^時(shí)，相對(duì)于％和 α L之差的絕對(duì)值的最大值，上述閾值A(chǔ)為 A = 2X (I α s- α L| )max。
全文摘要
本發(fā)明涉及機(jī)械手裝置以及機(jī)械手裝置的控制方法，該機(jī)械手裝置通過(guò)具有致動(dòng)器的連結(jié)裝置而將手臂可旋轉(zhuǎn)或可直線(xiàn)移動(dòng)地連結(jié)，該機(jī)械手裝置具備安裝在手臂上的慣性傳感器；第一計(jì)算部，根據(jù)來(lái)自致動(dòng)器所具備的角度傳感器的旋轉(zhuǎn)角度數(shù)據(jù)，計(jì)算致動(dòng)器的角速度以及角加速度；第二計(jì)算部，其根據(jù)由慣性傳感器檢測(cè)到的輸出，計(jì)算手臂的角速度或者角加速度；比較部，其對(duì)由第一計(jì)算部計(jì)算出的角速度或者角加速度和由第二計(jì)算部計(jì)算出的角速度或者角加速度進(jìn)行比較；故障判斷部，其在比較部中致動(dòng)器和手臂的角速度或者角加速度之差的絕對(duì)值大于閾值的情況下判斷為慣性傳感器發(fā)生了故障。
文檔編號(hào)G01C25/00GK102328312SQ20111019807
公開(kāi)日2012年1月25日 申請(qǐng)日期2011年7月11日 優(yōu)先權(quán)日2010年7月12日
發(fā)明者笹井重德 申請(qǐng)人:精工愛(ài)普生株式會(huì)社