機械手、機械手控制裝置以及機械手系統(tǒng)的制作方法
【專利摘要】本發(fā)明提供能夠容易且可靠地抑制振動的機械手、機械手控制裝置以及機械手系統(tǒng)。機械手（1）具備：基臺（11）；第一臂部（12），其按照能夠以第一轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于基臺（11）；第二臂部（13），其按照能夠以第二轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于第一臂部（12）；第三臂部（14），其按照能夠以第三轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于第二臂部（13）；第一角速度傳感器，其設置于第一臂部（12），并且角速度的檢測軸與上述第一轉(zhuǎn)動軸平行；第二角速度傳感器，其設置于第二臂部（13），并且角速度的檢測軸與上述第二轉(zhuǎn)動軸平行；以及第三角速度傳感器，其設置于第三臂部（14），并且角速度的檢測軸與上述第三轉(zhuǎn)動軸平行。
【專利說明】機械手、機械手控制裝置以及機械手系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機械手、機械手控制裝置以及機械手系統(tǒng)。

【背景技術(shù)】
[0002] 在專利文獻1所記載的機械手中，在前端部即在最前端側(cè)的第六連桿上設置六軸 傳感器，并基于該六軸傳感器的檢測結(jié)果來針對各連桿分別求出繞目標軸的角速度的振動 分量，從而進行抑制振動的控制，其中，上述六軸傳感器對相互正交的X軸、Y軸、Z軸的各個 方向的加速度、繞X軸、Y軸、Z軸的各軸的加速度進行檢測。此外，連桿的角速度的振動分 量被稱為"扭轉(zhuǎn)角速度"或"振動角速度"等。
[0003] 專利文獻1 :日本特開2011-136395號公報
[0004] 在專利文獻1所記載的機械手中，由于六軸傳感器的姿勢因機械手的動作而改 變，所以需要根據(jù)該六軸傳感器的檢測結(jié)果來進行被稱為雅可比轉(zhuǎn)換的坐標軸轉(zhuǎn)換等，求 出各連桿的角速度的振動分量。并且，需要配合每時每刻變化的馬達的轉(zhuǎn)動角度來進行計 算。
[0005] 因此，需要復雜且龐大的運算處理，所以需要具有性能高、昂貴的CPU (Central Processing Unit :中央處理器）等的控制裝置，因而存在成本增加的問題。
[0006] 另外，由于需要復雜且龐大的運算處理，所以容易產(chǎn)生運算誤差，從而存在因該運 算誤差而不能充分地抑制振動的問題。


【發(fā)明內(nèi)容】

[0007] 本發(fā)明的目的在于提供能夠容易且可靠地抑制振動的機械手、機械手控制裝置以 及機械手系統(tǒng)。
[0008] 這種目的能夠通過下述的本發(fā)明而實現(xiàn)。
[0009] 本發(fā)明的機械手的特征在于，具備：基臺；第一臂部，其按照能夠以第一轉(zhuǎn)動軸為 轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于上述基臺；第二臂部，其按照能夠以第二轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn) 動的方式連結(jié)于上述第一臂部，該第二轉(zhuǎn)動軸是與上述第一轉(zhuǎn)動軸正交的軸或與正交于上 述第一轉(zhuǎn)動軸的軸平行的軸；第三臂部，其按照能夠以第三轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式 連結(jié)于上述第二臂部，該第三轉(zhuǎn)動軸是與上述第二轉(zhuǎn)動軸平行的軸；第一角速度傳感器，其 設置于上述第一臂部，并且角速度的檢測軸與上述第一轉(zhuǎn)動軸平行；第二角速度傳感器，其 設置于上述第二臂部，并且角速度的檢測軸與上述第二轉(zhuǎn)動軸平行；以及第三角速度傳感 器，其設置于上述第三臂部，并且角速度的檢測軸與上述第三轉(zhuǎn)動軸平行。
[0010] 由此，能夠容易且可靠地抑制振動。
[0011] 即，首先，能夠利用第一角速度傳感器來檢測第一臂部的角速度。另外，能夠利用 第二角速度傳感器來檢測第二臂部的角速度。另外，能夠利用第三角速度傳感器來檢測第 三臂部的角速度。而且，能夠基于上述檢測結(jié)果來抑制振動。
[0012] 另外，即便機械手的姿勢發(fā)生變化，第一角速度傳感器的角速度的檢測軸也是一 定的。因此，不需要對由第一角速度傳感器檢測出的第一臂部的角速度進行基于第一角速 度傳感器朝向的修正。
[0013] 另外，由于第二轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或與正交于第一轉(zhuǎn)動軸的軸平行，所以 即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，第二角速度傳感器的角速度的檢測軸也 是一定的。因此，不需要對由第二角速度傳感器檢測出的第二臂部的角速度進行基于第二 角速度傳感器朝向的修正。
[0014] 另外，由于第三轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或者與正交于第一轉(zhuǎn)動軸的軸平行，并 且與第二轉(zhuǎn)動軸平行，所以即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，另外即便第二 臂部轉(zhuǎn)動，第三角速度傳感器的角速度的檢測軸也是一定的。因此，不需要對由第三角速度 傳感器檢測出的第三臂部的角速度進行基于第三角速度傳感器朝向的修正。
[0015] 由此，不需要復雜且龐大的運算，由此，難以產(chǎn)生運算誤差，從而能夠可靠地抑制 振動，另外還能夠提高機械手控制中的響應速度。
[0016] 另外，能夠通過將第一角速度傳感器、第二角速度傳感器、第三角速度傳感器通用 化來降低成本。
[0017] 在本發(fā)明的機械手中，優(yōu)選為具備：第一角速度傳感器單兀，其具有第一殼體、上 述第一角速度傳感器以及電路部，上述第一角速度傳感器以及上述電路部設置于上述第一 殼體內(nèi)，上述電路部對從上述第一角速度傳感器輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送；第 二角速度傳感器單元，其具有第二殼體、上述第二角速度傳感器以及電路部，上述第二角速 度傳感器以及上述電路部設置于上述第二殼體內(nèi)，上述電路部對從上述第二角速度傳感器 輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送；以及第三角速度傳感器單元，其具有第三殼體、上述 第三角速度傳感器以及電路部，上述第三角速度傳感器以及上述電路部設置于上述第三殼 體內(nèi)，上述電路部對從上述第三角速度傳感器輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送，上述第 一角速度傳感器單元設置于上述第一臂部，上述第二角速度傳感器單元設置于上述第二臂 部，上述第三角速度傳感器單元設置于上述第三臂部。
[0018] 由此，與另外設置電路部的情況相比，能夠簡化結(jié)構(gòu)。
[0019] 在本發(fā)明的機械手中，優(yōu)選為上述第一殼體、上述第二殼體以及上述第三殼體的 外形分別是長方體，上述第一角速度傳感器的上述角速度的檢測軸與上述第一殼體的上述 長方體的最大面的法線一致，上述第二角速度傳感器的上述角速度的檢測軸與上述第二殼 體的上述長方體的最大面的法線一致，上述第三角速度傳感器的上述角速度的檢測軸與上 述第三殼體的上述長方體的最大面的法線一致。
[0020] 由此，能夠容易且可靠地對第一角速度傳感器的角速度的檢測軸、第二角速度傳 感器的角速度的檢測軸以及第三角速度傳感器的檢測軸的方向分別進行識別，從而能夠容 易地將第一角速度傳感器、第二角速度傳感器以及第三角速度傳感器調(diào)整為適當?shù)淖藙荨?br> [0021] 在本發(fā)明的機械手中，優(yōu)選為上述第一角速度傳感器、上述第二角速度傳感器以 及上述第三角速度傳感器分別具有陀螺元件，在將相互正交的兩軸設定為第一軸以及第二 軸時，上述陀螺元件具備：基部；一對檢測用振動臂，它們從上述基部與上述第一軸平行且 向相互相反的方向延伸突出；一對連結(jié)臂，它們從上述基部與上述第二軸平行且向相互相 反的方向延伸突出；以及一對驅(qū)動用振動臂，它們從各上述連結(jié)臂部的前端部或中部與上 述第一軸平行且向相互相反的方向延伸突出。
[0022] 由此，成為小型且檢測精度高的第一角速度傳感器、第二角速度傳感器以及第三 角速度傳感器。
[0023] 在本發(fā)明的機械手中，上述第一殼體在上述第一殼體的角部具有安裝于上述第一 臂部的安裝部，上述第二殼體在上述第二殼體的角部具有安裝于上述第二臂部的安裝部， 上述第三殼體在上述第三殼體的角部具有安裝于上述第三臂部的安裝部。
[0024] 由此，能夠?qū)⒌谝唤撬俣葌鞲衅鲉卧煽康匕惭b于第一臂部，能夠?qū)⒌诙撬俣?傳感器單元可靠地安裝于第二臂部，能夠?qū)⒌谌撬俣葌鞲衅鲉卧煽康匕惭b于第三臂 部。
[0025] 在本發(fā)明的機械手中，優(yōu)選為具備具有導電性并將上述第一殼體的上述安裝部固 定于上述第一臂部的固定部件，通過上述固定部件將上述第一角速度傳感器單元的上述電 路部接地連接于上述第一臂部，具備具有導電性并將上述第二殼體的上述安裝部固定于上 述第二臂部的固定部件，通過上述固定部件將上述第二角速度傳感器單元的上述電路部接 地于上述第二臂部，具備具有導電性并將上述第三殼體的上述安裝部固定于上述第三臂部 的固定部件，通過上述固定部件將上述第三角速度傳感器單元的上述電路部接地于上述第 三臂部。
[0026] 由此，能夠減少部件件數(shù)，從而能夠簡化結(jié)構(gòu)。
[0027] 在本發(fā)明的機械手中，優(yōu)選為上述第一臂部具有框體和與上述框體一體形成的臂 部側(cè)安裝部，上述第一角速度傳感器單元直接安裝于上述臂部側(cè)安裝部。
[0028] 由此，第一角速度傳感器單元能夠可靠地與第一臂部一體地轉(zhuǎn)動。
[0029] 在本發(fā)明的機械手中，優(yōu)選為上述第二臂部具有框體和與上述框體一體形成的臂 部側(cè)安裝部，上述第二角速度傳感器單元直接安裝于上述臂部側(cè)安裝部。
[0030] 由此，第二角速度傳感器單元能夠可靠地與第二臂部一體地轉(zhuǎn)動。
[0031] 在本發(fā)明的機械手中，優(yōu)選為上述第三臂部具有框體和與上述框體一體形成的臂 部側(cè)安裝部，上述第三角速度傳感器單元直接安裝于上述臂部側(cè)安裝部。
[0032] 由此，第三角速度傳感器單元能夠可靠地與第三臂部一體地轉(zhuǎn)動。
[0033] 在本發(fā)明的機械手中，優(yōu)選為具有電纜，該電纜配設于上述第一臂部，對該機械手 供給電力，上述第一角速度傳感器配置于上述第一臂部的與上述電纜相反一側(cè)的端部。 [0034] 由此，能夠防止第一角速度傳感器受到由電纜產(chǎn)生的噪聲的影響的情況，另外能 夠防止第一角速度傳感器側(cè)的電路、配線因電纜而短路的情況。
[0035] 在本發(fā)明的機械手中，優(yōu)選為具有電纜，該電纜配設于上述第二臂部，對該機械手 供給電力，上述第二角速度傳感器配置于上述第二臂部的與上述電纜相反一側(cè)的端部。
[0036] 由此，能夠防止第二角速度傳感器受到由電纜產(chǎn)生的噪聲的影響的情況，另外能 夠防止第二角速度傳感器側(cè)的電路、配線因電纜而短路的情況。
[0037] 在本發(fā)明的機械手中，優(yōu)選為具有電纜，該電纜配設于上述第三臂部，對該機械手 供給電力，上述第三角速度傳感器配置于上述第三臂部的與上述電纜相反一側(cè)的端部。
[0038] 由此，能夠防止第三角速度傳感器受到由電纜產(chǎn)生的噪聲的影響的情況，另外能 夠防止第三角速度傳感器側(cè)的電路、配線因電纜而短路的情況。
[0039] 在本發(fā)明的機械手中，優(yōu)選為具備：第四臂部，其按照能夠以第四轉(zhuǎn)動軸為轉(zhuǎn)動中 心轉(zhuǎn)動的方式連結(jié)于上述第三臂部，該第四轉(zhuǎn)動軸是與上述第三轉(zhuǎn)動軸正交的軸或與正交 于上述第三轉(zhuǎn)動軸的軸平行的軸；第五臂部，其按照能夠以第五轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的 方式連結(jié)于上述第四臂部，該第五轉(zhuǎn)動軸是與上述第四轉(zhuǎn)動軸正交的軸或與正交于上述第 四轉(zhuǎn)動軸的軸平行的軸；以及第六臂部，其按照能夠以第六轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式 連結(jié)于上述第五臂部，該第六轉(zhuǎn)動軸是與上述第五轉(zhuǎn)動軸正交的軸或與正交于上述第五轉(zhuǎn) 動軸的軸平行的軸。
[0040] 由此，能夠容易地進行復雜的動作。
[0041] 在本發(fā)明的機械手中，優(yōu)選為上述第一轉(zhuǎn)動軸與上述基臺的設置面的法線一致。
[0042] 由此，能夠容易地進行機械手的控制。
[0043] 本發(fā)明的機械手控制裝置的特征在于，該機械手控制裝置控制機械手的動作，該 機械手具備：基臺；第一臂部，其按照能夠以第一轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于上 述基臺；第二臂部，其按照能夠以第二轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于上述第一臂部， 該第二轉(zhuǎn)動軸是與上述第一轉(zhuǎn)動軸正交的軸或與正交于上述第一轉(zhuǎn)動軸的軸平行的軸；以 及第三臂部，其按照能夠以第三轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于上述第二臂部，該第 三轉(zhuǎn)動軸是與上述第二轉(zhuǎn)動軸平行的軸，上述機械手控制裝置具備：接收部，其對從設置于 上述第一臂部且角速度的檢測軸與上述第一轉(zhuǎn)動軸平行的第一角速度傳感器輸出的第一 信號、從設置于上述第二臂部且角速度的檢測軸與上述第二轉(zhuǎn)動軸平行的第二角速度傳感 器輸出的第二信號以及從設置于上述第三臂部且角速度的檢測軸與上述第三轉(zhuǎn)動軸平行 的第三角速度傳感器輸出的第三信號進行接收；運算部，其基于由上述接收部接收到的上 述第一信號、上述第二信號以及上述第三信號來求出上述第一臂部的角速度的振動分量、 上述第二臂部的角速度的振動分量以及上述第三臂部的角速度的振動分量；以及控制部， 其基于由上述運算部求出的上述第一臂部的角速度的振動分量、上述第二臂部的振動分量 以及上述第三臂部的角速度的振動分量來控制上述機械手的動作。
[0044] 由此，能夠容易且可靠地抑制振動。
[0045] 即，首先能夠利用運算部基于由第一角速度傳感器檢測出的第一臂部的角速度來 求出第一臂部的角速度的振動分量。另外，能夠利用運算部來求出由第二角速度傳感器檢 測出的第二臂部的角速度的振動分量。另外，能夠利用運算部來求出由第三角速度傳感器 檢測出的第三臂部的角速度的振動分量。而且，能夠基于第一臂部的角速度的振動分量、第 二臂部的角速度的振動分量以及第三臂部的角速度的振動分量來抑制振動。
[0046] 另外，即便機械手的姿勢變化，第一角速度傳感器的角速度的檢測軸也是一定的。 因此，不需要對由第一角速度傳感器檢測出的第一臂部的角速度進行基于第一角速度傳感 器朝向的修正。
[0047] 另外，由于第二轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或與正交于第一轉(zhuǎn)動軸的軸平行，所以 即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，第二角速度傳感器的角速度的檢測軸也 是一定的。因此，不需要對由第二角速度傳感器檢測出的第二臂部的角速度進行基于第二 角速度傳感器朝向的修正。
[0048] 另外，由于第三轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或者與正交于第一轉(zhuǎn)動軸的軸平行，并 且與第二轉(zhuǎn)動軸平行，所以即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，另外即便第二 臂部轉(zhuǎn)動，第三角速度傳感器的角速度的檢測軸也是一定的。因此，不需要對由第三角速度 傳感器檢測出的第三臂部的角速度進行基于第三角速度傳感器朝向的修正。
[0049] 由此，不需要復雜且龐大的運算，由此難以產(chǎn)生運算誤差，從而能夠可靠地抑制振 動，另外還能夠提1?機械手控制中的響應速度。
[0050] 本發(fā)明的機械手系統(tǒng)的特征在于，具備：本發(fā)明的機械手；機械手控制裝置，其控 制上述機械手的動作。
[0051] 由此，能夠容易且可靠地抑制振動。
[0052] S卩，首先能夠利用第一角速度傳感器來檢測第一臂部的角速度。另外，能夠利用第 二角速度傳感器來檢測第二臂部的角速度。另外，能夠利用第三角速度傳感器來檢測第三 臂部的角速度。而且，能夠基于上述檢測結(jié)果來抑制振動。
[0053] 另外，即便機械手的姿勢發(fā)生變化，第一角速度傳感器的角速度的檢測軸也是一 定的。因此，不需要對由第一角速度傳感器檢測出的第一臂部的角速度進行基于第一角速 度傳感器朝向的修正。
[0054] 另外，由于第二轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或與正交于第一轉(zhuǎn)動軸的軸平行，所以 即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，第二角速度傳感器的角速度的檢測軸也 是一定的。因此，不需要對由第二角速度傳感器檢測出的第二臂部的角速度進行基于第二 角速度傳感器朝向的修正。
[0055] 另外，由于第三轉(zhuǎn)動軸與第一轉(zhuǎn)動軸正交或者與正交于第一轉(zhuǎn)動軸的軸平行，并 且與第二轉(zhuǎn)動軸平行，所以即便機械手的姿勢變化，例如即便第一臂部轉(zhuǎn)動，另外即便第二 臂部轉(zhuǎn)動，第三角速度傳感器的角速度的檢測軸也是一定的。因此，不需要對由第三角速度 傳感器檢測出的第三臂部的角速度進行基于第三角速度傳感器朝向的修正。
[0056] 由此，不需要復雜且龐大的運算，由此難以產(chǎn)生運算誤差，從而能夠可靠地抑制振 動，另外還能夠提1?機械手控制中的響應速度。
[0057] 另外，能夠通過將第一角速度傳感器、第二角速度傳感器、第三角速度傳感器通用 化來降低成本。

【專利附圖】

【附圖說明】
[0058] 圖1是從正面?zhèn)扔^察本發(fā)明的機械手的實施方式的立體圖。
[0059] 圖2是從背面?zhèn)扔^察圖1示出的機械手的立體圖。
[0060] 圖3是圖1示出的機械手的簡圖。
[0061] 圖4是具有圖1示出的機械手的機械手系統(tǒng)的主要部分的框圖。
[0062] 圖5是圖1示出的機械手的主視圖。
[0063] 圖6是表示圖1示出的機械手的第一臂部的第一角速度傳感器附近的圖。
[0064] 圖7是表示圖1示出的機械手的第二臂部的第二角速度傳感器附近的圖。
[0065] 圖8是表示圖1示出的機械手的第三臂部的第三角速度傳感器附近的圖。
[0066] 圖9是圖1示出的機械手的第一角速度傳感器單元的剖視圖。
[0067] 圖10是角速度傳感器所具有的陀螺元件的俯視圖。
[0068] 圖11是表示圖10示出的陀螺元件的動作的圖。
[0069] 圖12是圖1示出的機械手的主要部分的框圖。
[0070] 圖13是圖1示出的機械手的主要部分的框圖。
[0071] 圖14是圖1示出的機械手的主要部分的框圖。
[0072] 圖15是圖1示出的機械手的主要部分的框圖。
[0073] 圖16是圖1示出的機械手的主要部分的框圖。
[0074] 圖17是圖1示出的機械手的變形例的主要部分的框圖。
[0075] 圖18是圖1示出的機械手的變形例的主要部分的框圖。
[0076] 圖19是圖1示出的機械手的變形例的主要部分的框圖。

【具體實施方式】
[0077] 以下，根據(jù)附圖示出的優(yōu)選的實施方式詳細地對本發(fā)明的機械手、機械手控制裝 置以及機械手系統(tǒng)進行說明。
[0078] 圖1是從正面?zhèn)扔^察本發(fā)明的機械手的實施方式的立體圖。圖2是從背面?zhèn)扔^察 圖1示出的機械手的立體圖。圖3是圖1示出的機械手的簡圖。圖4是具有圖1示出的機 械手的機械手系統(tǒng)的主要部分的框圖。圖5是圖1示出的機械手的主視圖。圖6是表示圖 1示出的機械手的第一臂部的第一角速度傳感器附近的圖。圖7是表示圖1示出的機械手 的第二臂部的第二角速度傳感器附近的圖。圖8是表示圖1示出的機械手的第三臂部的第 三角速度傳感器附近的圖。圖9是圖1示出的機械手的第一角速度傳感器單元的剖視圖。 圖10是角速度傳感器所具有的陀螺元件的俯視圖。圖11是表示圖10示出的陀螺元件的 動作的圖。圖12?圖16分別是圖1示出的機械手的主要部分的框圖。圖17?圖19分別 是圖1示出的機械手的變形例的主要部分的框圖。
[0079] 此外，以下，為了便于說明，將圖1?圖3、圖5?圖8中的上側(cè)稱為"上"或"上 方"、下側(cè)稱為"下"或"下方"。另外，將圖1?圖3、圖5?圖8中的基臺側(cè)稱為"基端"、其 相反一側(cè)稱為"前端"。另外，在圖9中，對應第一角速度傳感器單元并以括弧書寫來標注第 二角速度傳感器單元以及第三角速度傳感器單元的各部分的附圖標記，并省略第二角速度 傳感器單元以及第三角速度傳感器單元的圖示。
[0080] 圖1?圖4示出的機械手系統(tǒng)(工業(yè)用機械手系統(tǒng)）10例如能夠使用在制造手表 那樣的精密設備等的制造工序中，具有機械手(工業(yè)用機械手）1和控制該機械手1的動作 的機械手控制裝置(控制機構(gòu)）20 (參照圖4)。機械手1與機械手控制裝置20電連接。另 夕卜，機械手控制裝置20例如由內(nèi)置有CPU (Central Processing Unit:中央處理器）的個 人計算機（PC)等構(gòu)成。此外，稍后對機械手控制裝置20進行詳述。
[0081] 機械手1具備基臺11 ;4個臂部(連桿）12、13、14、15 ;腕(連桿）16 ;以及6個驅(qū)動 源401、402、403、404、405、406。該機械手1是將基臺11;臂部12、13、14、15;以及腕16從 基端側(cè)向前端側(cè)按照該順序連結(jié)的垂直多關(guān)節(jié)（6軸)機械手(機械手主體)。在垂直多關(guān)節(jié) 機械手中，還能夠?qū)⒒_11、臂部12?15以及腕16統(tǒng)稱為"臂部"，能夠分別將臂部12稱 為"第一臂部"、將臂部13稱為"第二臂部"、將臂部14稱為"第三臂部"、將臂部15稱為"第 四臂部"、將腕16稱為"第五臂部、第六臂部"。此外，在本實施方式中，腕16具有第五臂部 和第六臂部。能夠在腕16安裝終端執(zhí)行器等。
[0082] 臂部12?15、腕16被分別支承為能夠相對于基臺11獨立地移位。對于該臂部 12?15、腕16的長度分別不特別限定，但在圖示的結(jié)構(gòu)中，將第一臂部12、第二臂部13、第 四臂部15的長度設定為比第三臂部14以及腕16長。
[0083] 基臺11與第一臂部12經(jīng)由關(guān)節(jié)(接頭）171而連結(jié)。而且，第一臂部12能夠相對 于基臺11以與堅直方向平行的第一轉(zhuǎn)動軸01為轉(zhuǎn)動中心繞該第一轉(zhuǎn)動軸01轉(zhuǎn)動。第一 轉(zhuǎn)動軸01與基臺11的設置面即地面101的上表面的法線一致。繞該第一轉(zhuǎn)動軸01的轉(zhuǎn) 動是通過具有馬達401M的第一驅(qū)動源401的驅(qū)動而進行的。另外，第一驅(qū)動源401通過馬 達401M和電纜(未圖示）而被驅(qū)動，該馬達401M經(jīng)由電連接的馬達驅(qū)動器301而被機械手 控制裝置20控制(參照圖4)。此外，第一驅(qū)動源401可以構(gòu)成為通過與馬達401M -起設置 的減速器(未圖示）來傳遞來自馬達401M的驅(qū)動力，另外也可以省略減速器，但在本實施方 式中，第一驅(qū)動源401具有減速器。
[0084] 第一臂部12與第二臂部13經(jīng)由關(guān)節(jié)(接頭）172而連結(jié)。而且，第二臂部13能夠 相對于第一臂部12以與水平方向平行的第二轉(zhuǎn)動軸02為軸心轉(zhuǎn)動。第二轉(zhuǎn)動軸02與第一 轉(zhuǎn)動軸01正交。繞該第二轉(zhuǎn)動軸02的轉(zhuǎn)動是通過具有馬達402M的第二驅(qū)動源402的驅(qū) 動而進行的。另外，第二驅(qū)動源402通過馬達402M和電纜(未圖示)而被驅(qū)動，該馬達402M 經(jīng)由電連接的馬達驅(qū)動器302而被機械手控制裝置20控制(參照圖4)。此外，第二驅(qū)動源 402可以構(gòu)成為通過與馬達402M-起設置的減速器45 (參照圖5)來傳遞來自馬達402M 的驅(qū)動力，另外也可以省略減速器，但在本實施方式中，第二驅(qū)動源402具有減速器45。另 夕卜，第二轉(zhuǎn)動軸02也可以與正交于第一轉(zhuǎn)動軸01的軸平行。
[0085] 第二臂部13與第三臂部14經(jīng)由關(guān)節(jié)(接頭）173而連結(jié)。而且，第三臂部14能夠 相對于第二臂部13以與水平方向平行的第三轉(zhuǎn)動軸03為轉(zhuǎn)動中心繞該第三轉(zhuǎn)動軸03轉(zhuǎn) 動。第三轉(zhuǎn)動軸03與第二轉(zhuǎn)動軸02平行。繞該第三轉(zhuǎn)動軸03的轉(zhuǎn)動是通過第三驅(qū)動源 403的驅(qū)動而進行的。另外，第三驅(qū)動源403通過馬達403M和電纜(未圖示）而被驅(qū)動，該 馬達403M經(jīng)由電連接的馬達驅(qū)動器303而被機械手控制裝置20控制(參照圖4)。此外， 第三驅(qū)動源403可以構(gòu)成為通過與馬達403M -起設置的減速器(未圖示）來傳遞來自馬達 403M的驅(qū)動力，另外也可以省略減速器，但在本實施方式中，第三驅(qū)動源403具有減速器。
[0086] 第三臂部14與第四臂部15經(jīng)由關(guān)節(jié)(接頭）174而連結(jié)。而且，第四臂部15能夠 相對于第三臂部14 (基臺11)以與第三臂部14的中心軸方向平行的第四轉(zhuǎn)動軸04為轉(zhuǎn)動 中心繞該第四轉(zhuǎn)動軸04轉(zhuǎn)動。第四轉(zhuǎn)動軸04與第三轉(zhuǎn)動軸03正交。繞該第四轉(zhuǎn)動軸04 的轉(zhuǎn)動是通過第四驅(qū)動源404的驅(qū)動而進行的。另外，第四驅(qū)動源404通過馬達404M和電 纜(未圖示）而被驅(qū)動，該馬達404M經(jīng)由電連接的馬達驅(qū)動器304而被機械手控制裝置20 控制(參照圖4)。此外，第四驅(qū)動源404可以構(gòu)成為通過與馬達404M-起設置的減速器(未 圖示）來傳遞來自馬達404M的驅(qū)動力，另外也可以省略減速器，但在在本實施方式中，第四 驅(qū)動源404具有減速器。第四轉(zhuǎn)動軸04也可以與正交于第三轉(zhuǎn)動軸03的軸平行。
[0087] 第四臂部15與第五腕16經(jīng)由關(guān)節(jié)(接頭）175而連結(jié)。而且，腕16能夠相對于第 四臂部15以與水平方向平行的第五轉(zhuǎn)動軸05為轉(zhuǎn)動中心繞該第五轉(zhuǎn)動軸05轉(zhuǎn)動。第五 回動軸05與第四轉(zhuǎn)動軸04正交。繞該第五轉(zhuǎn)動軸05的轉(zhuǎn)動是通過第五驅(qū)動源405的驅(qū) 動而進行的。另外，第五驅(qū)動源405通過馬達405M和電纜(未圖示)而被驅(qū)動，該馬達405M 經(jīng)由電連接的馬達驅(qū)動器305而被機械手控制裝置20控制(參照圖4)。此外，第五驅(qū)動源 405可以構(gòu)成為通過與馬達405M -起設置的減速器(未圖示)來傳遞來自馬達405M的驅(qū)動 力，另外也可以省略減速器，但在本實施方式中，第五驅(qū)動源405具有減速器。
[0088] 另外，腕16也能夠經(jīng)由關(guān)節(jié)(接頭）176而以與第五轉(zhuǎn)動軸05垂直的第六轉(zhuǎn)動軸 06為轉(zhuǎn)動中心繞該第六轉(zhuǎn)動軸06轉(zhuǎn)動。第六轉(zhuǎn)動軸06與第五轉(zhuǎn)動軸05正交。繞該第六 轉(zhuǎn)動軸06的轉(zhuǎn)動是通過第六驅(qū)動源406的驅(qū)動而進行的。另外，第六驅(qū)動源406通過馬達 和電纜(未圖示)而被驅(qū)動，該馬達406M經(jīng)由電連接的馬達驅(qū)動器306而被機械手控制裝置 20控制(參照圖4)。此外，第六驅(qū)動源406可以構(gòu)成為通過與馬達406M -起設置的減速器 (未圖示)來傳遞來自馬達406M的驅(qū)動力，另外也可以省略減速器，但在本實施方式中，第六 驅(qū)動源406具有減速器。另外，第五轉(zhuǎn)動軸05也可以與正交于第四轉(zhuǎn)動軸04的軸平行，另 外第六轉(zhuǎn)動軸06也可以與正交于第五回動軸05的軸平行。
[0089] 另外，如圖6所示，在第一臂部12設置有第一角速度傳感器31，即設置有具有第一 角速度傳感器31的第一角速度傳感器單元71。利用該第一角速度傳感器31對第一臂部 12繞第一轉(zhuǎn)動軸01的角速度進行檢測。
[0090] 另外，如圖7所示，在第二臂部13設置有第二角速度傳感器32,即設置有具有第二 角速度傳感器32的第二角速度傳感器單元72。利用該第二角速度傳感器32對第二臂部 13繞第二轉(zhuǎn)動軸02的角速度進行檢測。
[0091] 另外，如圖8所示，在第三臂部14設置有第三角速度傳感器33,即設置有具有第三 角速度傳感器33的第三角速度傳感器單元73。利用該第三角速度傳感器33對第三臂部 14繞第三轉(zhuǎn)動軸03的角速度進行檢測。此外，由于第三轉(zhuǎn)動軸03與第二轉(zhuǎn)動軸02平行， 所以第三角速度傳感器33不是對第三臂部14繞第三轉(zhuǎn)動軸03的角速度，而是對包括第二 臂部13的轉(zhuǎn)動量在內(nèi)的第三臂部14的角速度，即對第三臂部14繞第二轉(zhuǎn)動軸02的角速 度進行檢測。
[0092] 在此，在該機械手1中，通過抑制第一臂部12、第二臂部13以及第三臂部14的振 動來抑制機械手1整體的振動。具體而言，在第一臂部12、第二臂部13以及第三臂部14的 全部設置角速度傳感器，基于第一角速度傳感器31、第二角速度傳感器32以及第三角速度 傳感器33的檢測結(jié)果來控制驅(qū)動源401、402、403的動作。如此，與在第一臂部12、第二臂 部13以及第三臂部14的至少一個臂部未設置角速度傳感器的結(jié)構(gòu)相比，通過在各臂部12、 13、14分別設置角速度傳感器能夠更可靠地抑制機械手1的振動。此外，通過使角速度傳感 器31、32、33具有相同結(jié)構(gòu)能夠?qū)⑸鲜鼋撬俣葌鞲衅?1、32、33通用化，從而能夠降低成本。
[0093] 對驅(qū)動源401?406而言，在各個馬達或減速器設置有第一位置傳感器411、第二 位置傳感器412、第三位置傳感器413、第四位置傳感器414、第五位置傳感器415以及第六 位置傳感器416。作為上述位置傳感器分別不特別限定，例如可以使用編碼器、旋轉(zhuǎn)編碼器、 解析器、電位器等。利用上述位置傳感器411?416分別對驅(qū)動源401?406的馬達或減 速器的軸部的轉(zhuǎn)動角度進行檢測。作為該驅(qū)動源401?406的馬達不分別特別限定，例如 優(yōu)選為使用AC伺服馬達、DC伺服馬達等伺服馬達。另外，上述各電纜可以分別穿過機械手 1〇
[0094] 如圖4所示，機械手1與機械手控制裝置20電連接。即，驅(qū)動源401?406、位置 傳感器411?416、角速度傳感器31、32、33分別與機械手控制裝置20電連接。
[0095] 而且，機械手控制裝置20能夠使臂部12?15、腕16分別獨立地動作，S卩，能夠經(jīng) 由馬達驅(qū)動器301?306來分別獨立地控制驅(qū)動源401?406。在該情況下，機械手控制 裝置20利用位置傳感器411?416、第一角速度傳感器31、第二角速度傳感器32以及第三 角速度傳感器33來進行檢測，基于該檢測結(jié)果來分別控制驅(qū)動源401?406的驅(qū)動，例如 分別控制角速度、轉(zhuǎn)動角度等。該控制程序預先存儲于內(nèi)置于機械手控制裝置20的記錄介 質(zhì)。
[0096] 如圖1、圖2所示，當機械手1是垂直多關(guān)節(jié)機械手時，基臺11位于該垂直多關(guān)節(jié) 機械手的最下方，是固定于設置空間的地面101的部分。作為該固定方法不特別限定，例如 在圖1、圖2示出的本實施方式中，使用利用多個螺栓111的固定方法。此外，作為基臺11 的設置空間處的固定位置，除地面之外，也可以是設置空間的壁、天花板。
[0097] 基臺11具有中空的基臺主體(殼體)112。基臺主體112能夠劃分為呈圓筒狀的圓 筒狀部113和一體地形成于該圓筒狀部113的外周部的呈箱狀的箱狀部114。而且，在這種 基臺主體112,例如，收納有馬達401M、馬達驅(qū)動器301?306。
[0098] 臂部12?15分別具有中空的臂部主體(框體)2、驅(qū)動機構(gòu)3以及密封機構(gòu)4。此 夕卜，以下，為便于說明，將第一臂部12所具有的臂部主體2、驅(qū)動機構(gòu)3以及密封機構(gòu)4分 別稱為"臂部主體2a"、"驅(qū)動機構(gòu)3a"以及"密封機構(gòu)4a"，將第二臂部13所具有的臂部主 體2、驅(qū)動機構(gòu)3以及密封機構(gòu)4分別稱為"臂部主體2b"、"驅(qū)動機構(gòu)3b"以及"密封機構(gòu) 4b"，將第三臂部14所具有的臂部主體2、驅(qū)動機構(gòu)3以及密封機構(gòu)4分別稱為"臂部主體 2c"、"驅(qū)動機構(gòu)3c"以及"密封機構(gòu)4c"，將第四臂部15所具有的臂部主體2、驅(qū)動機構(gòu)3以 及密封機構(gòu)4分別稱為"臂部主體2d"、"驅(qū)動機構(gòu)3d"以及"密封機構(gòu)4d"。
[0099] 另外，關(guān)節(jié)171?176分別具有轉(zhuǎn)動支承機構(gòu)（未圖示)。該轉(zhuǎn)動支承機構(gòu)是將相 互連結(jié)的兩個臂部中的一方支承為能夠相對于另一方轉(zhuǎn)動的機構(gòu)、是將相互連結(jié)的基臺11 與第一臂部12中的一方支承為能夠相對于另一方轉(zhuǎn)動的機構(gòu)、是將相互連結(jié)的第四臂部 15與第五腕16中的一方支承為能夠相對于另一方轉(zhuǎn)動的機構(gòu)。在以相互連結(jié)的第四臂部 15與腕16為一個例子的情況下，轉(zhuǎn)動支承機構(gòu)能夠使腕16相對于第四臂部15轉(zhuǎn)動。另外， 各轉(zhuǎn)動支承機構(gòu)分別具有減速器(未圖示)，上述減速器以規(guī)定的減速比對對應的馬達的轉(zhuǎn) 動速度進行減速，并將其驅(qū)動力傳遞至對應的臂部、腕16的腕主體161以及支承環(huán)162。此 夕卜，如上所述，在本實施方式中，包括該減速器與馬達作為驅(qū)動源。
[0100] 第一臂部12以相對于水平方向傾斜的姿勢連結(jié)于基臺11的上端部(前端部)。在 該第一臂部12中，驅(qū)動機構(gòu)3a具有馬達402M，并收納于臂部主體2a內(nèi)。另外，臂部主體2a 內(nèi)被密封機構(gòu)4a氣密密封。臂部主體2a具有前端側(cè)的1對舌片部241a、241b和基端側(cè)的 根部251。舌片部241a與舌片部241b分尚并相互對置。另外，舌片部241a、241b相對于根 部251傾斜，由此，第一臂部12相對于水平方向傾斜。而且，在舌片部241a與舌片部241b 之間配置有第二臂部13的基端部。
[0101] 另外，第一臂部12中第一角速度傳感器31的設置位置不特別限定，但在本實施方 式中，如圖6所示，第一角速度傳感器31即第一角速度傳感器單元71設置于第一臂部12的 臂部主體2a的根部251的內(nèi)部與電纜85相反一側(cè)的端部。電纜85是向機械手1的各馬 達401M?406M供給電力的電纜。由此，能夠防止第一角速度傳感器31受到從電纜85發(fā) 出的噪聲的影響的情況，另外，還能夠防止第一角速度傳感器單元71的后述的電路部713、 配線、第一角速度傳感器31由于電纜85而短路的情況。
[0102] 在此，針對驅(qū)動機構(gòu)3以及減速器，以設置于第一臂部12的臂部主體2a內(nèi)并使第 二臂部13轉(zhuǎn)動的驅(qū)動機構(gòu)3為代表進行說明。
[0103] 如圖5所示，驅(qū)動機構(gòu)3具有：第一帶輪91，其連結(jié)于馬達402M的軸部；第二帶輪 92,其與第一帶輪91配置為分離；以及帶(正時帶)93,其架設于第一帶輪91與第二帶輪92。 而且，第二帶輪92與第二臂部13的軸部利用減速器45連結(jié)。
[0104] 作為減速器45,不特別限定，例如能夠舉例由多個齒輪構(gòu)成的減速器、被稱為諧波 驅(qū)動器("諧波驅(qū)動器"是注冊商標）的減速器等。
[0105] 作為機械手1的臂部12?15、腕16的振動的主要原因，例如，能夠舉例減速器45 的扭轉(zhuǎn)、撓曲、帶93的伸縮、臂部12?15以及腕16的撓曲等。
[0106] 第二臂部13連結(jié)于第一臂部12的前端部。在該第二臂部13中，驅(qū)動機構(gòu)3b具 有馬達403M，并收納于臂部主體2b內(nèi)。另外，臂部主體2a內(nèi)被密封機構(gòu)4b氣密密封。臂 部主體2b具有前端側(cè)的1對舌片部242a、242b和基端側(cè)的根部252。舌片部242a與舌片 部242b分離并相互對置。而且，在舌片部242a與舌片部242b之間配置有第三臂部14的 基端部。
[0107] 另外，第二臂部13的第二角速度傳感器32的設置位置不特別限定，但在本實施方 式中，設置于第二臂部13的臂部主體2c的內(nèi)部與電纜85相反一側(cè)的端部。換言之，如圖 6所示，第二角速度傳感器單元72配置于與電纜85通過的舌片部242b不同的舌片部242a 內(nèi)。由此，能夠防止第二角速度傳感器32受到由電纜85產(chǎn)生的噪聲的影響的情況，另外， 能夠防止第二角速度傳感器單元72的電路部723、配線、第二角速度傳感器32因電纜85而 短路的情況。
[0108] 第三臂部14連結(jié)于第二臂部13的前端部。在該第三臂部14中，驅(qū)動機構(gòu)3c具 有馬達404M，并收納于臂部主體2c內(nèi)。另外，臂部主體2c內(nèi)被密封機構(gòu)4c氣密密封。此 夕卜，臂部主體2c由相當于上述臂部主體2a的根部251、上述臂部主體2b的根部252的部件 構(gòu)成。
[0109] 另外，第三臂部14中第三角速度傳感器33的設置位置不特別限定，但在本實施方 式中，如圖8所示，第三角速度傳感器33,即第三角速度傳感器單元73設置于第三臂部14 的臂部主體2c的內(nèi)部與電纜85相反一側(cè)的端部。由此，能夠防止第三角速度傳感器33受 到由電纜85產(chǎn)生的噪聲的影響的情況，另外，能夠防止第三角速度傳感器單元73的電路部 723、配線、第三角速度傳感器33因電纜85而短路的情況。
[0110] 第四臂部15以與第三臂部14的中心軸方向平行的方式連結(jié)于第三臂部14的前 端部。在該臂部15中，驅(qū)動機構(gòu)3d具有馬達405M、406M，并收納于臂部主體2d內(nèi)。另外， 臂部主體2d內(nèi)被密封機構(gòu)4d氣密密封。臂部主體2d具有前端側(cè)的1對舌片部244a、244b 和基端側(cè)的根部254。舌片部244a與舌片部244b分離并相互對置。而且，在舌片部244a 與舌片部244b之間配置有腕16的支承環(huán)162。
[0111] 在第四臂部15的前端部(與基臺11相反一側(cè)的端部)連結(jié)有腕16。在該腕16,在 其前端部（與第四臂部15相反一側(cè)的端部)，作為功能部(終端執(zhí)行器)，例如可拆裝地安裝 有把持手表等精密設備的機械手(未圖示)。此外，作為機械手，不特別限定，例如能夠舉例 具有多個指部（finger)的結(jié)構(gòu)的機械手。而且，該機械手1保持用機械手把持精密設備不 變，并通過控制臂部12?15、腕16等的動作來輸送精密設備。
[0112] 腕16具有呈圓筒狀的腕主體(第六臂部）161和呈環(huán)狀的支承環(huán)(第五臂部）162， 其中，該支承環(huán)(第五臂部）162與腕主體161獨立構(gòu)成，并設置于該腕主體161的基端部。
[0113] 腕主體161的前端面163是平坦的面，成為安裝機械手的安裝面。另外，腕主體 161經(jīng)由關(guān)節(jié)176連結(jié)于第四臂部15的驅(qū)動機構(gòu)3d，并被該驅(qū)動機構(gòu)3d的馬達406M驅(qū)動 而繞第六轉(zhuǎn)動軸06轉(zhuǎn)動。
[0114] 支承環(huán)162經(jīng)由關(guān)節(jié)175連結(jié)于第四臂部15的驅(qū)動機構(gòu)3d，并被該驅(qū)動機構(gòu)3d 的馬達405M驅(qū)動而隨腕主體161 -起繞第五轉(zhuǎn)動軸05轉(zhuǎn)動。
[0115] 作為臂部主體2的構(gòu)成材料，不特別限定，例如可以使用各種金屬材料，在上述金 屬材料中特別優(yōu)選鋁或鋁合金。在臂部主體2是使用金屬模而成型的鑄件的情況下，通過 對于該臂部主體2的構(gòu)成材料使用鋁或鋁合金，能夠容易地進行金屬模成型。
[0116] 另外，作為基臺11的基臺主體112、腕16的腕主體161以及支承環(huán)162的構(gòu)成材 料，分別不特別限定，例如能夠舉例與上述臂部主體2的構(gòu)成材料相同的材料等。此外，腕 16的腕主體161的構(gòu)成材料優(yōu)選為使用不銹鋼。
[0117] 另外，作為密封機構(gòu)4的構(gòu)成材料，不特別限定，例如能夠使用各種樹脂材料、各 種金屬材料。此外，作為密封機構(gòu)4的構(gòu)成材料，能夠通過使用樹脂材料來實現(xiàn)輕型化。
[0118] 接下來，對第一角速度傳感器單元71、第二角速度傳感器單元72以及第三角速度 傳感器單元73進行說明。
[0119] 如圖9所示，第一角速度傳感器單元71具有：第一殼體711 ;電路基板712,其設置 于第一殼體711內(nèi)，并具有配線；以及第一角速度傳感器31和電路部713,它們電連接于電 路基板712上。在本實施方式中，第一殼體711由密封件構(gòu)成，第一角速度傳感器31、電路 部713以及電路基板712整體被該密封件密封。
[0120] 同樣，第二角速度傳感器單元72具有：第二殼體721 ;電路基板722,其設置于第 二殼體721內(nèi)，并具有配線；以及第二角速度傳感器32和電路部723,它們電連接于電路基 板722上。在本實施方式中，第二殼體721由密封件構(gòu)成，第二角速度傳感器32、電路部723 以及電路基板722整體被該密封件密封。
[0121] 同樣，第三角速度傳感器單元73具有：第三殼體731 ;電路基板732,其設置于第 三殼體731內(nèi)，并具有配線；第三角速度傳感器33以及電路部733,它們電連接于電路基板 732上。在本實施方式中，第三殼體731由密封件構(gòu)成，第三角速度傳感器33、電路部733 以及電路基板732整體被該密封件密封。
[0122] 如此，通過封裝第一角速度傳感器31和電路部713、第二角速度傳感器32和電路 部723以及第三角速度傳感器33和電路部733能夠簡化結(jié)構(gòu)。
[0123] 此外，由于第一角速度傳感器單元71、第二角速度傳感器單元72以及第三角速度 傳感器單元73結(jié)構(gòu)相互相同，以下，以第一角速度傳感器單元71為代表進行說明，并省略 第二角速度傳感器單元72以及第三角速度傳感器單元73的說明。
[0124] 第一角速度傳感器31具有陀螺元件34,該陀螺元件34具備一個檢測軸。作為陀 螺元件34的結(jié)構(gòu)不特別限定，但例如，能夠使用如下陀螺元件。此外，如圖10以及圖11所 示，以下，將相互正交的軸設為X軸(第二軸)、Y軸(第一軸）以及Z軸。
[0125] 如圖10所示，陀螺元件34具有：水晶基板、未圖示的檢測用電極以及未圖示的驅(qū) 動用電極，其中，水晶基板具有：基部341 ;1對檢測用振動臂342a、342b，它們從基部341的 兩側(cè)沿Y軸方向且向相互相反一側(cè)延伸；1對連結(jié)臂343a、343b，它們從基部341的兩側(cè)沿 X軸方向且向相互相反一側(cè)延伸；1對驅(qū)動用振動臂344a、344b，它們從連結(jié)臂343a的前端 部的兩側(cè)沿Y軸方向且向相互相反一側(cè)延伸；以及1對驅(qū)動用振動臂344c、344d，它們從連 結(jié)臂343b的前端部的兩側(cè)沿Y軸方向且向相互相反側(cè)延伸，檢測用電極設置于各檢測用振 動臂342a、342b，驅(qū)動用電極設置于各驅(qū)動用振動臂344a、344b、344c、344d。
[0126] 這種陀螺元件34以如下的方式動作。此外，在圖11 (a)、ll (b)中，為便于表現(xiàn) 振動方式而用線表示各振動臂。
[0127] 首先，如圖11 (a)所示，在未對陀螺元件34施加角速度的狀態(tài)下，通過對驅(qū)動用 電極施加電壓來使各驅(qū)動用振動臂344a、344b、344c、344d向箭頭E示出的方向彎曲振動。 該彎曲振動按照規(guī)定的頻率重復實線所示的振動姿態(tài)和雙點劃線所示的振動姿態(tài)。此時， 驅(qū)動用振動臂344a、344b和驅(qū)動用振動臂344c、344d進行相對于通過重心G的Y軸線對稱 的振動。
[0128] 如圖11 (b)所示，在進行該振動的狀態(tài)下，若對陀螺元件34施加繞Z軸(檢測軸） 的角速度ω，則對驅(qū)動用振動臂344a、344b、344c、344d以及連結(jié)臂343a、343b作用箭頭B 方向的科里奧利力，從而對上述它們激發(fā)新的振動。另外，與此同時，對檢測用振動臂342a、 342b激發(fā)與箭頭B的振動呼應的箭頭C方向的振動。而且，從檢測用電極輸出與由該檢測 用振動臂342a、342b的振動產(chǎn)生的檢測用振動臂342a、342b的變形對應的信號（電壓)。
[0129] 以上，對陀螺元件34簡單地進行了說明。
[0130] 另外，電路部713具有：AD轉(zhuǎn)換部，其AD轉(zhuǎn)換從第一角速度傳感器31輸出的信號， 艮P、將模擬信號轉(zhuǎn)換為數(shù)字信號；以及發(fā)送部，其將上述轉(zhuǎn)換出的信號發(fā)送至機械手控制裝 置20。
[0131] 另外，第一殼體711的外形是長方體。而且，第一角速度傳感器31的檢測軸與第 一殼體711的長方體的最大面的法線一致。由此，能夠容易且可靠地對第一角速度傳感器 31的檢測軸以及第二角速度傳感器32的檢測軸的方向進行識別，從而能夠容易地將第一 角速度傳感器31以及第二角速度傳感器32調(diào)整為適當?shù)淖藙荨Ｌ貏e是，如上所述，陀螺元 件34呈平的形狀，由于以其板面的法線為檢測軸，所以易于使第一角速度傳感器31的檢測 軸與第一殼體711的長方體的最大面的法線一致。而且，第一角速度傳感器單元71以第一 角速度傳感器31的檢測軸31a與第一轉(zhuǎn)動軸01平行的方式設置。另外，第二角速度傳感 器單元72以第二角速度傳感器32的檢測軸32a與第二轉(zhuǎn)動軸02平行的方式設置。另外， 第三角速度傳感器單元73以第三角速度傳感器33的檢測軸33a與第三轉(zhuǎn)動軸03平行的 方式設置。
[0132] 另外，如圖6以及圖9所示，第一殼體711在其4個角部具有安裝于第一臂部12 的安裝部7111。在各安裝部7111分別形成有供陽螺紋件(固定部件）81插入的孔7112。
[0133] 另一方面，第一臂部12具有3個臂部側(cè)安裝部121，它們與臂部主體2a-體形成， 并安裝有第一角速度傳感器單元71(第一殼體711)。各臂部側(cè)安裝部121分別由在臂部主 體2a突出形成的支柱構(gòu)成。另外，各臂部側(cè)安裝部121分別配置于與第一殼體711的安裝 部7111對應的位置。另外，在各臂部側(cè)安裝部121的前端部分別形成有供陽螺紋件81旋 合的陰螺紋部122。
[0134] 此外，與上述臂部主體2a-體形成的臂部側(cè)安裝部121中的"一體"不是指分別 形成部件將它們接合的情況，而是指例如通過壓鑄等同時形成臂部主體2a與臂部側(cè)安裝 部121的情況。后述的與臂部主體2b、2c-體形成的臂部側(cè)安裝部131U41中的"一體"也 同樣。
[0135] 在將第一角速度傳感器單元71安裝(設置)于第一臂部12時，將三個陽螺紋件81 分別插入第一殼體711的孔7112,使之旋合于第一臂部12的臂部側(cè)安裝部121的前端部的 陰螺紋部122。由此，利用各陽螺紋件81將第一殼體711的3個安裝部7111分別固定于第 一臂部12所對應的臂部側(cè)安裝部121。即，將第一角速度傳感器單元71安裝于第一臂部 12的臂部側(cè)安裝部121。在該情況下，在臂部側(cè)安裝部121與第一角速度傳感器單元71之 間未夾設任何部件，即，第一角速度傳感器單元71直接安裝于臂部側(cè)安裝部121。由此，能 夠?qū)⒌谝唤撬俣葌鞲衅鲉卧?1可靠地安裝于第一臂部12,另外第一角速度傳感器單元71 能夠可靠地與第一臂部12 -體地轉(zhuǎn)動。
[0136] 此外，第一角速度傳感器單元71直接安裝于臂部側(cè)安裝部121中的"直接"不是 指將第一角速度傳感器單元71安裝于其它基板等中間體，將該中間體安裝于臂部側(cè)安裝 部121的情況。即，是指在臂部側(cè)安裝部121與第一角速度傳感器單元71之間除粘合劑等 之外未設有任何部件的情況。后述的第二角速度傳感器單元72、第三角速度傳感器33直接 安裝于臂部側(cè)安裝部131U41中的"直接"也同樣。
[0137] 另外，陽螺紋件81具有導電性，例如由各種金屬材料形成。該陽螺紋件81在插入 第一殼體711的孔7112旋合于臂部側(cè)安裝部121的前端部的陰螺紋部122時，電連接于電 路基板712的配線，該電路基板712電連接于電路部713的接地用端子，另外，陽螺紋件81 的前端部電連接于臂部側(cè)安裝部121。由此，電路部713的接地用端子經(jīng)由配線以及陽螺紋 件81電連接于第一臂部12的臂部主體2a，從而接地。由此，能夠減少接地所需要的部件件 數(shù)，從而能夠簡化結(jié)構(gòu)。
[0138] 另外，如圖7以及圖9所示，第二殼體721在其4個角部具有安裝于第二臂部13 的安裝部7211。在各安裝部7211分別形成有供陽螺紋件81插入的孔7212。
[0139] 另外，如圖7所示，第二臂部13具有臂部側(cè)安裝部131，臂部側(cè)安裝部131與臂部 主體2b-體形成并安裝有第二角速度傳感器單元72 (第二殼體721)。臂部側(cè)安裝部131 與臂部主體2b-體形成。另外，在臂部側(cè)安裝部131的各角部分別形成有供陽螺紋件81 旋合的陰螺紋部。
[0140] 在將第二角速度傳感器單元72安裝于第二臂部13時，將4個陽螺紋件81分別插 入第二殼體721的孔7212,使之旋合于第二臂部13的臂部側(cè)安裝部131的前端部的陰螺 紋部。由此，利用各陽螺紋件81將第二殼體721的4個安裝部7211分別固定于第二臂部 13的臂部側(cè)安裝部131。即，將第二角速度傳感器單元72安裝于第二臂部13的臂部側(cè)安 裝部131。在該情況下，在臂部側(cè)安裝部131與第二角速度傳感器單元72之間未夾設任何 部件，即，第二角速度傳感器單元72直接安裝于臂部側(cè)安裝部131。由此，能夠?qū)⒌诙撬?度傳感器單元72可靠地安裝于第二臂部13,另外第二角速度傳感器單元72能夠可靠地與 第二臂部13 -體地轉(zhuǎn)動。
[0141] 另外，陽螺紋件81在插入第二殼體721的孔7212旋合于臂部側(cè)安裝部131的陰 螺紋部時，電連接于電路基板722的配線，該電路基板722電連接于電路部723的接地用端 子，另外，陽螺紋件81的前端部電連接于臂部側(cè)安裝部131。由此，電路部723的接地用端 子經(jīng)由配線以及陽螺紋件81電連接于第二臂部13的臂部主體2b，從而接地。由此，能夠減 少接地所需要的部件件數(shù)，從而能夠簡化結(jié)構(gòu)。
[0142] 另外，如圖8以及圖9所示，第三殼體731在其4個角部具有安裝于第三臂部14 的安裝部7311。在各安裝部7311分別形成有供陽螺紋件81插入的孔7212。
[0143] 另外，如圖8所示，第三臂部14具有臂部側(cè)安裝部141，臂部側(cè)安裝部141與臂部 主體2c -體形成并安裝有第三角速度傳感器單元73 (第三殼體731)。臂部側(cè)安裝部141 呈與第三殼體731對應的形狀。即，臂部側(cè)安裝部141呈板狀，俯視時的形狀為四邊形，在 本實施方式中呈長方形。另外，在臂部側(cè)安裝部141的各角部分別形成有供陽螺紋件81旋 合的陰螺紋部。
[0144] 在將第三角速度傳感器單元73安裝于第三臂部14時，將4個陽螺紋件81分別插 入第三殼體731的孔7312旋合于第三臂部14的臂部側(cè)安裝部141的前端部的陰螺紋部。 由此，利用各陽螺紋件81將第三殼體731的4個安裝部7311分別固定于第三臂部14的臂 部側(cè)安裝部141。即，將第三角速度傳感器單元73安裝于第三臂部14的臂部側(cè)安裝部141。 在該情況下，在臂部側(cè)安裝部141與第三角速度傳感器單元73之間未夾設任何部件，即第 三角速度傳感器單元73直接安裝于臂部側(cè)安裝部141。由此，能夠?qū)⒌谌撬俣葌鞲衅鲉?元73可靠地安裝于第三臂部14,另外第三角速度傳感器單元73能夠可靠地與第三臂部14 一體地轉(zhuǎn)動。
[0145] 另外，陽螺紋件81在插入第三殼體731的孔7312旋合于臂部側(cè)安裝部141的陰 螺紋部時，電連接于電路基板732的配線，該電路基板732電連接于電路部733的接地用端 子，另外陽螺紋件81的前端部電連接于臂部側(cè)安裝部141。由此，電路部733的接地用端子 經(jīng)由配線以及陽螺紋件81電連接于第三臂部14的臂部主體2c，從而接地。由此，能夠減少 接地所需要的部件件數(shù)，從而能夠簡化結(jié)構(gòu)。
[0146] 接下來，參照圖4、圖12?圖16,對機械手控制裝置20的結(jié)構(gòu)進行說明。
[0147] 機械手控制裝置20具有：接收部，其對從第一角速度傳感器31輸出的第一信號、 從第二角速度傳感器32輸出的第二信號、從第三角速度傳感器33輸出的第三信號以及從 位置傳感器411?416輸出的各信號分別進行接收；運算部，其基于由該接收部接收的第一 信號、第二信號以及第三信號來求出第一臂部12的角速度的振動分量、第二臂部13的角速 度的振動分量以及第三臂部14的角速度的振動分量；以及控制部，其基于由該運算部求出 的第一臂部12的角速度的振動分量、第二臂部13的角速度的振動分量以及第三臂部14的 角速度的振動分量來控制機械手1的動作。
[0148] 具體而言，如圖4、圖12?圖17所示，機械手控制裝置20具有：上述接收部；第一 驅(qū)動源控制部201，其控制第一驅(qū)動源401的動作；第二驅(qū)動源控制部202,其控制第二驅(qū) 動源402的動作；第三驅(qū)動源控制部203,其控制第三驅(qū)動源403的動作；第四驅(qū)動源控制 部204,其控制第四驅(qū)動源404的動作；第五驅(qū)動源控制部205,其控制第五驅(qū)動源405的動 作；以及第六驅(qū)動源控制部206,其控制第六驅(qū)動源406的動作。
[0149] 此外，上述運算部由第一驅(qū)動源控制部201的后述的角速度計算部561、減法器 571、第二驅(qū)動源控制部202的后述的角速度計算部562、第三驅(qū)動源控制部203的后述的角 速度計算部563以及加減法器623構(gòu)成。
[0150] 如圖12所示，第一驅(qū)動源控制部201具有減法器511、位置控制部521、減法器 531、 角速度控制部541、轉(zhuǎn)動角度計算部551、角速度計算部561、減法器571、轉(zhuǎn)換部581、修 正值計算部591以及加法器601。
[0151] 如圖13所示，第二驅(qū)動源控制部202具有減法器512、位置控制部522、減法器 532、 角速度控制部542、轉(zhuǎn)動角度計算部552以及角速度計算部562。
[0152] 如圖13所示，第三驅(qū)動源控制部203具有減法器513、位置控制部523、減法器 533、 角速度控制部543、轉(zhuǎn)動角度計算部553、角速度計算部563、加減法器623、轉(zhuǎn)換部583、 修正值計算部593以及加法器603。
[0153] 如圖14所示，第四驅(qū)動源控制部204具有減法器514、位置控制部524、減法器 534、 角速度控制部544、轉(zhuǎn)動角度計算部554以及角速度計算部564。
[0154] 如圖15所示，第五驅(qū)動源控制部205具有減法器515、位置控制部525、減法器 535、 角速度控制部545、轉(zhuǎn)動角度計算部555以及角速度計算部565。
[0155] 如圖16所示，第六驅(qū)動源控制部206具有減法器516、位置控制部526、減法器 536、 角速度控制部546、轉(zhuǎn)動角度計算部556以及角速度計算部566。
[0156] 在此，機械手控制裝置20基于機械手1進行的處理的內(nèi)容來對腕16的目標位置 進行運算，生成用于使腕16移動至該目標位置的軌道。而且，為使腕16沿上述生成的軌道 移動，機械手控制裝置20按規(guī)定的控制周期測定各驅(qū)動源401?406的轉(zhuǎn)動角度，并將基 于該測定結(jié)果運算出的值分別作為各驅(qū)動源401?406的位置指令Pc而輸出至驅(qū)動源控 制部201?206 (參照圖12?圖16)。此外，上述以及以下，雖然記作"輸入、輸出值"但這 是"輸入、輸出與該值對應的信號"的意思。
[0157] 如圖12所示，對第一驅(qū)動源控制部201，除第一驅(qū)動源401的位置指令Pc之外，還 從第一位置傳感器411、角速度傳感器31分別輸入檢測信號。第一驅(qū)動源控制部201以使 根據(jù)第一位置傳感器411的檢測信號計算的第一驅(qū)動源的轉(zhuǎn)動角度(位置反饋值Pfb)成為 位置指令Pc，并且后述的角速度反饋值ωΛ成為后述的角速度指令coc的方式通過使用了 各檢測信號的反饋控制來驅(qū)動第一驅(qū)動源401。
[0158] S卩，對第一驅(qū)動源控制部201的減法器511輸入位置指令Pc，另外從轉(zhuǎn)動角度計算 部551輸入后述的位置反饋值Pfb。在轉(zhuǎn)動角度計算部551中，計數(shù)從第一位置傳感器411 輸入的脈沖數(shù)，并將與該計數(shù)值對應的第一驅(qū)動源401的轉(zhuǎn)動角度作為位置反饋值Pfb輸 出至減法器511。減法器511將上述位置指令Pc與位置反饋值Pfb之間的偏差(從第一驅(qū) 動源401的轉(zhuǎn)動角度的目標值減去位置反饋值Pfb得到的值）輸出至位置控制部521。
[0159] 位置控制部521通過進行使用從減法器511輸入的偏差和預先設定的系數(shù)即比例 增益等的規(guī)定的運算處理來對與該偏差對應的第一驅(qū)動源401的角速度的目標值進行運 算。位置控制部521將表示該第一驅(qū)動源401的角速度的目標值(指令值)的信號作為角速 度指令《c輸出至減法器531。此外，在此在本實施方式中，作為反饋控制而進行比例控制 (P控制)，但并不局限于此。
[0160] 對減法器531輸入角速度指令ω〇，另外輸入后述的角速度反饋值cofb。減法器 531將上述角速度指令ω c與角速度反饋值ω fb之間的偏差(從第一驅(qū)動源401的角速度 的目標值減去角速度反饋值《fb得到的值）輸出至角速度控制部541。
[0161] 角速度控制部541通過使用從減法器531輸入的偏差和預先設定的系數(shù)即比例增 益、積分增益等，并進行包括積分在內(nèi)的規(guī)定的運算處理，來生成與該偏差對應的第一驅(qū)動 源401的驅(qū)動信號(驅(qū)動電流)，并將其經(jīng)由馬達驅(qū)動器301供給至馬達401Μ。此外，在此在 本實施方式中，作為反饋控制而進行ΡΙ控制，但并不局限于此。
[0162] 如此，為使位置反饋值Pfb盡可能地與位置指令Pc相等，并且使角速度反饋值 ?fb盡可能地與角速度指令coc相等，進行反饋控制，控制第一驅(qū)動源401的驅(qū)動電流。
[0163] 接下來，對第一驅(qū)動源控制部201中的角速度反饋值cofb進行說明。
[0164] 在角速度計算部561中，基于從第一位置傳感器411輸入的脈沖信號的頻率來計 算第一驅(qū)動源401的角速度ωπι?，將該角速度comi輸出至加法器601。
[0165] 另外，在角速度計算部561中，基于從第一位置傳感器411輸入的脈沖信號的頻率 來計算第一臂部12繞第一轉(zhuǎn)動軸01的角速度ωΑΙπι，將該角速度ωΑΙπι輸出至減法器571。 此外，角速度《Aim是將角速度comi除以第一驅(qū)動源401的馬達401Μ與第一臂部12之間 即關(guān)節(jié)171的減速比得到的值。
[0166] 另外，利用第一角速度傳感器31來檢測第一臂部12繞第一轉(zhuǎn)動軸01的角速度。 而且，將該第一角速度傳感器31的檢測信號即由角速度傳感器31檢測出的第一臂部12繞 第一轉(zhuǎn)動軸01的角速度ωΑΙ輸出至減法器571。
[0167] 對減法器571輸入角速度ωΑΙ以及角速度ω Aim，減法器571將從該角速度ωΑΙ 減去角速度《Aim得到的值coAls (= ωΑΙ - ωΑΙπι)輸出至轉(zhuǎn)換部581。該值coAls相 當于第一臂部12繞第一轉(zhuǎn)動軸01的角速度的振動分量(振動角速度)。以下，將《Als稱 為振動角速度。在本實施方式中，進行將該振動角速度《Als (具體而言，是基于振動角速 度《Als生成的值即馬達401M的角速度comls)放大后述的增益Ka倍并返回第一驅(qū)動源 401的輸入側(cè)的反饋控制。具體而言，為使振動角速度《Als盡可能地為0,對第一驅(qū)動源 401進行反饋控制。由此，能夠抑制機械手1的振動。此外，在該反饋控制中，控制第一驅(qū)動 源401的角速度。
[0168] 轉(zhuǎn)換部581將振動角速度coAls轉(zhuǎn)換為第一驅(qū)動源401的角速度comls，然后將該 角速度〇mls輸出至修正值計算部591。該轉(zhuǎn)換能夠通過對振動角速度coAls乘以第一驅(qū) 動源401的馬達401M與第一臂部12之間即關(guān)節(jié)171的減速比來實現(xiàn)。
[0169] 修正值計算部591對角速度comls乘以預先設定的系數(shù)即增益(反饋增益）Ka來 求出修正值Ka· comls，并將該修正值Ka· comls輸出至加法器601。
[0170] 對加法器601輸入角速度ωπι?，另外輸入修正值Ka· comls。加法器601將角速 度ωπι?與修正值Ka*c〇mls的加法值作為角速度反饋值cofb輸出至減法器531。此外，以 下的動作如上述那樣。
[0171] 如圖13所示，對第二驅(qū)動源控制部202除第二驅(qū)動源402的位置指令Pc之外，還 從第二位置傳感器412輸入檢測信號。第二驅(qū)動源控制部202以使根據(jù)第二位置傳感器 412的檢測信號計算的第二驅(qū)動源402的轉(zhuǎn)動角度(位置反饋值Pfb)成為位置指令Pc，并 且使后述的角速度反饋值《fb成為后述的角速度指令coc的方式通過使用了各檢測信號 的反饋控制來驅(qū)動第二驅(qū)動源402。
[0172] S卩，對第二驅(qū)動源控制部202的減法器512輸入位置指令Pc，另外從轉(zhuǎn)動角度計算 部552輸入后述的位置反饋值Pfb。在轉(zhuǎn)動角度計算部552中，計數(shù)從第二位置傳感器412 輸入的脈沖數(shù)，并將與該計數(shù)值對應的第二驅(qū)動源402的轉(zhuǎn)動角度作為位置反饋值Pfb輸 出至減法器512。減法器512將上述位置指令Pc與位置反饋值Pfb之間的偏差(從第二驅(qū) 動源402的轉(zhuǎn)動角度的目標值減去位置反饋值Pfb得到的值）輸出至位置控制部522。
[0173] 位置控制部522通過進行使用從減法器512輸入的偏差和預先設定的系數(shù)即比例 增益等的規(guī)定的運算處理來對與該偏差對應的第二驅(qū)動源402的角速度的目標值進行運 算。位置控制部522將表示該第二驅(qū)動源402的角速度的目標值(指令值)的信號作為角速 度指令《C輸出至減法器532。此外，在此在本實施方式中，作為反饋控制而進行比例控制 (P控制)，但并不局限于此。
[0174] 另外，在角速度計算部562,基于從第二位置傳感器412輸入的脈沖信號的頻率來 計算第二驅(qū)動源402的角速度，并將該角速度作為角速度反饋值ωΛ輸出至減法器532。
[0175] 對減法器532輸入角速度指令coc，另外輸入角速度反饋值cofb。減法器532將 上述角速度指令ω c與角速度反饋值ω fb之間的偏差(從第二驅(qū)動源402的角速度的目標 值減去角速度反饋值《fb得到的值）輸出至角速度控制部542。
[0176] 角速度控制部542通過使用從減法器532輸入的偏差和預先設定的系數(shù)即比例增 益、積分增益等，進行包括積分在內(nèi)的規(guī)定的運算處理，來生成與該偏差對應的第二驅(qū)動源 402的驅(qū)動信號(驅(qū)動電流)，并將其經(jīng)由馬達驅(qū)動器302供給至馬達402M。此外，在此在本 實施方式中，作為反饋控制而進行PI控制，但并不局限于此。
[0177] 如此，以使位置反饋值Pfb盡可能地與位置指令Pc相等，并且使角速度反饋值 ω fb盡可能地與角速度指令ω c相等的方式，進行反饋控制，控制第二驅(qū)動源402的驅(qū)動電 流。
[0178] 如圖13所示，對第三驅(qū)動源控制部203除第三驅(qū)動源403的位置指令Pc之外，還 從第三位置傳感器413、第三角速度傳感器33分別輸入檢測信號。另外，對第三驅(qū)動源控制 部203從第二驅(qū)動源控制部202輸入第二臂部13繞第二轉(zhuǎn)動軸02的角速度ωΑ〗!!!'。
[0179] 第三驅(qū)動源控制部203以使根據(jù)第三位置傳感器413的檢測信號計算的第三驅(qū)動 源403的轉(zhuǎn)動角度(位置反饋值Pfb)成為位置指令Pc，并且使后述的角速度反饋值ω fb 成為后述的角速度指令的方式通過使用了各檢測信號的反饋控制來驅(qū)動第三驅(qū)動源 403。
[0180] 即，對第三驅(qū)動源控制部203的減法器513輸入位置指令Pc，另外從轉(zhuǎn)動角度計算 部553輸入后述的位置反饋值Pfb。在轉(zhuǎn)動角度計算部553中，計數(shù)從第三位置傳感器413 輸入的脈沖數(shù)，并將與該計數(shù)值對應的第三驅(qū)動源403的轉(zhuǎn)動角度作為位置反饋值Pfb輸 出至減法器513。減法器513將上述位置指令Pc與位置反饋值Pfb之間的偏差(從第三驅(qū) 動源403的轉(zhuǎn)動角度的目標值減去位置反饋值Pfb得到的值）輸出至位置控制部523。
[0181] 位置控制部523通過進行使用從減法器513輸入的偏差和預先設定的系數(shù)即比例 增益等的規(guī)定的運算處理來對與該偏差對應的第三驅(qū)動源403的角速度的目標值進行運 算。位置控制部523將表示該第三驅(qū)動源403的角速度的目標值(指令值)的信號作為角速 度指令《c輸出至減法器533。此外，在此在本實施方式中，作為反饋控制而進行比例控制 (P控制)，但并不局限于此。
[0182] 對減法器533輸入角速度指令ω〇，另外輸入后述的角速度反饋值cofb。減法器 533將上述角速度指令ω c與角速度反饋值ω fb之間的偏差(從第三驅(qū)動源403的角速度 的目標值減去角速度反饋值ω fb得到的值）輸出至角速度控制部543。
[0183] 角速度控制部543通過使用從減法器533輸入的偏差和預先設定的系數(shù)即比例增 益、積分增益等，進行包括積分在內(nèi)的規(guī)定的運算處理，來生成與該偏差對應的第三驅(qū)動源 403的驅(qū)動信號（驅(qū)動電流)，并將其經(jīng)由達驅(qū)動器303供給至馬達403M。此外，在此，在本 實施方式中，作為反饋控制而進行PI控制，但并不局限于此。
[0184] 如此，以使位置反饋值Pfb盡可能地與位置指令Pc相等，并且使角速度反饋值 ω fb盡可能地與角速度指令ω c相等的方式，進行反饋控制，控制第三驅(qū)動源403的驅(qū)動電 流。此外，第三轉(zhuǎn)動軸03由于與第一轉(zhuǎn)動軸01正交，所以不會受到第一臂部12的動作、振 動的影響，從而能夠相對于第三驅(qū)動源403獨立地控制第三驅(qū)動源403的動作。
[0185] 接下來，對第三驅(qū)動源控制部203中的角速度反饋值cofb進行說明。
[0186] 在角速度計算部563中，基于從第三位置傳感器413輸入的脈沖信號的頻率來計 算第三驅(qū)動源403的角速度ωπι3,將該角速度c〇 m3輸出至加法器603。
[0187] 另外，在角速度計算部563中，基于從第三位置傳感器413輸入的脈沖信號的頻率 來計算第三臂部14繞第三轉(zhuǎn)動軸03的角速度ωΑ3πι，將該角速度ωΑ3πι分別輸出至加減法 器623以及減法器633。此外，角速度ωΑ3πι是將角速度ω m3除以第三驅(qū)動源403的馬達 403Μ與第三臂部14之間即關(guān)節(jié)173的減速比得到的值。
[0188] 另外，利用第二角速度傳感器32來檢測第二臂部13繞第二轉(zhuǎn)動軸02的角速度。 而且，將該第二角速度傳感器32的檢測信號即由第二角速度傳感器32檢測出的第二臂部 13繞第二轉(zhuǎn)動軸02的角速度ωΑ2輸出至減法器633。此外，由于第二轉(zhuǎn)動軸02與第一轉(zhuǎn) 動軸01正交，所以能夠容易且可靠地求出第二臂部13繞第二轉(zhuǎn)動軸02的角速度，而不會 受到第一臂部12的動作、振動的影響。
[0189] 對減法器633輸入角速度ωΑ3πκ角速度ωΑ2,減法器633將從角速度ωΑ2減去 角速度ωΑ3πι得到的值ωΑ2' (= ω Α2 - ω A3m)輸出至加法器643。對加法器643輸入 角速度ωΑ2'、角速度ωΑ2πι，加法器643將把角速度ωΑ2'、ωΑ2πι相加得到的值c〇A2m' (=ωΑ2< + ωΑ2πι)輸出至加減法器623。
[0190] 另外，利用第三角速度傳感器33來檢測第三臂部14繞第三轉(zhuǎn)動軸03的角速度。 而且，將該第三角速度傳感器33的檢測信號，即由第三角速度傳感器33檢測出的第三臂部 14繞第三轉(zhuǎn)動軸03的角速度ω A3輸出至加減法器623。此外，由于第三轉(zhuǎn)動軸03與第一 轉(zhuǎn)動軸01正交，所以能夠容易且可靠地求出第三臂部14繞第三轉(zhuǎn)動軸03的角速度，而不 會受到第一臂部12的動作、振動的影響。
[0191] 對加減法器623輸入角速度ωΑ3、角速度ωΑ〗!!!'以及角速度ωΑ3πι，加減法器 623將從角速度ωΑ3減去角速度ωΑ2πι'以及角速度ωΑ3πι得到的值c〇A2s (= ωΑ3 - ωΑ2π^ - ωΑ3πι)輸出至轉(zhuǎn)換部583。該值c〇A3s相當于第三臂部14繞第三轉(zhuǎn)動軸03的 角速度的振動分量(振動角速度)。以下，將《A3s稱為振動角速度。在本實施方式中，進行 將該振動角速度《A3s (具體而言，是基于振動角速度c〇A3s生成的值即馬達403M的角速 度《m3s)放大后述的增益Ka倍并返回第三驅(qū)動源403的輸入側(cè)的反饋控制。具體而言， 為使振動角速度《A3s盡可能地為0,對第三驅(qū)動源403進行反饋控制。由此，能夠抑制機 械手1的振動。此外，在該反饋控制中，控制第三驅(qū)動源403的角速度。
[0192] 轉(zhuǎn)換部583將振動角速度c〇A3s轉(zhuǎn)換為第三驅(qū)動源403的角速度c〇m3s，并將該角 速度《 m3s輸出至修正值計算部593。該轉(zhuǎn)換能夠通過對振動角速度c〇A3s乘以第三驅(qū)動 源403的馬達403M與第三臂部14之間即關(guān)節(jié)173的減速比來實現(xiàn)。
[0193] 修正值計算部593對角速度c〇m3s乘以預先設定的系數(shù)即增益(反饋增益）Ka來 求出修正值Ka· c〇m3s，并將該修正值Ka· c〇m3s輸出至加法器603。此外，該第三驅(qū)動源 控制部203的增益Ka與第一驅(qū)動源控制部201的增益Ka可以相同，另外也可以不同。
[0194] 對加法器603輸入角速度ωπι3,另外輸入修正值Ka · c〇m3s。加法器603將角速 度ωπι3與修正值Ka*c〇m3s的加法值作為角速度反饋值cofb輸出至減法器533。此外，以 下的動作如上述那樣。
[0195] 此外，對于驅(qū)動源控制部204?206,由于分別與上述第二驅(qū)動源控制部202相同， 所以省略其說明。
[0196] 如上說明，在該機械手1以及機械手系統(tǒng)10中，能夠利用第一角速度傳感器31來 檢測第一臂部12的角速度，能夠利用第二角速度傳感器32來檢測第二臂部13的角速度， 能夠利用第三角速度傳感器33來檢測第三臂部14的角速度。而且，能夠基于上述的檢測 結(jié)果來抑制振動。
[0197] 另外，即便機械手1的姿勢變化，第一角速度傳感器31的檢測軸也是一定的。因 此，不需要對由第一角速度傳感器31檢測出的第一臂部12的角速度進行基于第一角速度 傳感器31朝向的修正。
[0198] 另外，第二轉(zhuǎn)動軸02由于與第一轉(zhuǎn)動軸01正交或與正交于第一轉(zhuǎn)動軸01的軸平 行，所以即便機械手1的姿勢變化，例如即便第一臂部12轉(zhuǎn)動，第二角速度傳感器32的檢 測軸也是一定的。因此，不需要對由第二角速度傳感器32檢測出的第二臂部13的角速度 進行基于第二角速度傳感器32朝向的修正。
[0199] 另外，第三轉(zhuǎn)動軸03由于與第一轉(zhuǎn)動軸01正交或與正交于第一轉(zhuǎn)動軸01的軸平 行、與第二轉(zhuǎn)動軸02平行，所以即便機械手1的姿勢變化，例如即便第一臂部12轉(zhuǎn)動，另外 即便第二臂部13轉(zhuǎn)動，第三角速度傳感器33的檢測軸也是一定的。因此，不需要對由第三 角速度傳感器33檢測出的第三臂部14的角速度進行基于第三角速度傳感器33朝向的修 正。
[0200] 由此，不需要復雜且龐大的運算，由此難以產(chǎn)生運算誤差，從而能夠可靠地抑制振 動，另外還能夠提1?機械手1控制中的響應速度。
[0201] 另外，與在第一臂部12、第二臂部13以及第三臂部14的至少一個臂部未配置角速 度傳感器的情況比較，能夠更可靠地抑制機械手1的振動。另外，通過將3個角速度傳感器 31、32、33通用化能夠降低成本。
[0202] 以上，對本實施方式的機械手1進行了說明。
[0203] 上述，對第三角速度傳感器33構(gòu)成為檢測包括第二臂部13的轉(zhuǎn)動量在內(nèi)的第三 臂部14繞第二轉(zhuǎn)動軸的角速度，即第二臂部13與第三臂部14互相干涉的情況下的機械手 1的控制方法進行了說明，但在第三角速度傳感器33能夠不包括第二臂部13的轉(zhuǎn)動量在內(nèi) 而實際只檢測第三臂部14的角速度的情況下，機械手1的控制方法可以是如下的方法。
[0204] 如圖17所示，對第一驅(qū)動源控制部201除第一驅(qū)動源401的位置指令Pc之外，還 從第一位置傳感器411、第一角速度傳感器31分別輸入檢測信號。第一驅(qū)動源控制部201 以使根據(jù)第一位置傳感器411的檢測信號計算的第一驅(qū)動源的轉(zhuǎn)動角度(位置反饋值Pfb) 成為位置指令Pc，并且使角速度反饋值ω fb成為角速度指令ω c的方式通過使用了各檢測 信號的反饋控制來驅(qū)動第一驅(qū)動源401。
[0205] 如圖18所示，對第二驅(qū)動源控制部202除第二驅(qū)動源402的位置指令Pc之外，還 從第二位置傳感器412、第二角速度傳感器分別輸入檢測信號。第二驅(qū)動源控制部202以使 根據(jù)第二位置傳感器412的檢測信號計算的第二驅(qū)動源的轉(zhuǎn)動角度(位置反饋值Pfb)成為 位置指令Pc，并且使角速度反饋值ω fb成為角速度指令ω c的方式通過使用了各檢測信號 的反饋控制來驅(qū)動第二驅(qū)動源402。
[0206] 如圖19所示，對第三驅(qū)動源控制部203除第三驅(qū)動源403的位置指令Pc之外，還 從第三位置傳感器413、第三角速度傳感器33分別輸入檢測信號。第三驅(qū)動源控制部203 以使根據(jù)第三位置傳感器413的檢測信號計算的第三驅(qū)動源的轉(zhuǎn)動角度(位置反饋值Pfb) 成為位置指令Pc，并且使角速度反饋值ω fb成為角速度指令ω c的方式通過使用了各檢測 信號的反饋控制來驅(qū)動第三驅(qū)動源403。
[0207] 由于上述驅(qū)動源401、402、403的驅(qū)動的控制與圖12示出的第一驅(qū)動源401的控 制相同，所以省略其說明。
[0208] 根據(jù)這種控制方法，例如由于能夠在各臂部12、13、14獨立地設定增益(反饋增益） Ka，所以能夠在各臂部12、13、14使增益Ka最佳化。因此，能夠更可靠地抑制機械手1的振 動。
[0209] 以上，基于圖示的實施方式對本發(fā)明的機械手、機械手控制裝置以及機械手系統(tǒng) 進行了說明，但本發(fā)明并不局限于此，各部分的結(jié)構(gòu)可以替換為具有相同的功能的任意結(jié) 構(gòu)。另外，也可以對本發(fā)明附加其它的任意的構(gòu)成件。
[0210] 此外，作為各驅(qū)動源的馬達，分別除了上述伺服馬達之外，例如能夠舉例步進電機 等。另外，在作為馬達而使用步進電機的情況下，作為位置傳感器，例如也可以使用通過計 數(shù)向步進電機輸入的驅(qū)動脈沖的個數(shù)來檢測馬達的轉(zhuǎn)動角度的位置傳感器。
[0211] 另外，各位置傳感器、各角速度傳感器的方式分別不特別限定，例如能夠舉例光學 式、磁式、電磁式、電式等。
[0212] 另外，在上述實施方式中，基于第二角速度傳感器的檢測結(jié)果來控制使第二臂部 轉(zhuǎn)動的第二驅(qū)動源的動作，但并不局限于此，例如，可以基于第二角速度傳感器的檢測結(jié)果 來控制使第三臂部轉(zhuǎn)動的第三驅(qū)動源的動作。
[0213] 另外，在上述實施方式中，機械手的轉(zhuǎn)動軸的個數(shù)是6個，但在本發(fā)明中并不局限 于此，機械手的轉(zhuǎn)動軸的個數(shù)也可以是3個、4個、5個或7個以上。
[0214] S卩，在上述實施方式中，由于腕具有兩個臂部，所以機械手的臂部的個數(shù)是6個， 但在本發(fā)明中，并不局限于此，機械手的臂部的個數(shù)也可以是3個、4個、5個或7個以上。
[0215] 另外，在上述實施方式中，機械手是具有一個將多個臂部可轉(zhuǎn)動地連結(jié)而構(gòu)成的 臂部連結(jié)體的單臂部機械手，但在本發(fā)明中，并不局限于此，例如，可以是具有兩個將多個 臂部可轉(zhuǎn)動地連結(jié)而構(gòu)成的臂部連結(jié)體的雙臂部機械手等，也可以是具有多個上述臂部連 結(jié)體的機械手。
[0216] 附圖標記說明：
[0217] 1…機械手(工業(yè)用機械手）；10…機械手系統(tǒng)；11···基臺；12、13、14、15?臂部(連 桿）；16…腕(連桿）；161…腕主體；162…支承環(huán)；163…前端面；171、172、173、174、175、 176…關(guān)節(jié)（接頭） ；2、2&、215、2(3、2士..臂部主體；3、3&、313、3(3、3士..驅(qū)動機構(gòu) ；31、32、33 - 角 速度傳感器；34…陀螺元件；341…基部；342a、342b…檢測用振動臂部；343a、343b…連結(jié) 臂部；344 &、34牝、344(：、344士..驅(qū)動用振動臂部；4、4&、413、4(3、4士..密封機構(gòu) ；2(>..機械手 控制裝置；201、202、203、204、205、206?驅(qū)動源控制部；301、302、303、304、305、306?馬達 驅(qū)動器；401、402、403、404、405、4(^..驅(qū)動源；401]\1、4021、4031、404]\1、4051、4061^..馬達 ； 411、412、413、414、415、416 -位置傳感器；511、512、513、514、515、516 -減法器；521、522、 523、524、525、526?位置控制部；531、532、533、534、535、536?減法器；541、542、543、544、 545、546…角速度控制部；551、552、553、554、555、556%轉(zhuǎn)動角度計算部；561、562、563、 564、565、566…角速度計算部；571、572、573…減法器；581、582、583…轉(zhuǎn)換部；591、592、 593…修正值計算部；601、602、603…加法器；623…加減法器；633…減法器；643…加法器； 45…減速機；71、72、73?角速度傳感器單元 ；711、721、731?殼體；7111、7211、7311"-安 裝部；712、722、732 …電路基板；713、723、733 …電路部；7112、7212、7312*"孔；81*"陽螺 紋件；85···電纜；91、92…帶輪；93…帶；101…地面；111…螺栓；112…基臺主體；113…圓 筒狀部；114…箱狀部；121、131、141…臂部側(cè)安裝部；122…陰螺紋件；241a、241b、242a、 24213、244&、2441^..舌片部 ；251、252、25七..根部介..重心；01、02、03、04、05、(^..轉(zhuǎn)動軸。
【權(quán)利要求】
1. 一種機械手，其特征在于，具備： 基臺； 第一臂部，其按照能夠以第一轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述基臺； 第二臂部，其按照能夠以第二轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第一臂部，該 第二轉(zhuǎn)動軸是與所述第一轉(zhuǎn)動軸正交的軸或與正交于所述第一轉(zhuǎn)動軸的軸平行的軸； 第三臂部，其按照能夠以第三轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第二臂部，該 第三轉(zhuǎn)動軸是與所述第二轉(zhuǎn)動軸平行的軸； 第一角速度傳感器，其設置于所述第一臂部，并且角速度的檢測軸與所述第一轉(zhuǎn)動軸 平行； 第二角速度傳感器，其設置于所述第二臂部，并且角速度的檢測軸與所述第二轉(zhuǎn)動軸 平行；以及 第三角速度傳感器，其設置于所述第三臂部，并且角速度的檢測軸與所述第三轉(zhuǎn)動軸 平行。
2. 根據(jù)權(quán)利要求1所述的機械手，其特征在于，具備： 第一角速度傳感器單元，其具有第一殼體、所述第一角速度傳感器以及電路部，所述第 一角速度傳感器以及所述電路部設置于所述第一殼體內(nèi)，所述電路部對從所述第一角速度 傳感器輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送； 第二角速度傳感器單元，其具有第二殼體、所述第二角速度傳感器以及電路部，所述第 二角速度傳感器以及所述電路部設置于所述第二殼體內(nèi)，所述電路部對從所述第二角速度 傳感器輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送；以及 第三角速度傳感器單元，其具有第三殼體、所述第三角速度傳感器以及電路部，所述第 三角速度傳感器以及所述電路部設置于所述第三殼體內(nèi)，所述電路部對從所述第三角速度 傳感器輸出的信號進行AD轉(zhuǎn)換并進行發(fā)送， 所述第一角速度傳感器單元設置于所述第一臂部，所述第二角速度傳感器單元設置于 所述第二臂部，所述第三角速度傳感器單元設置于所述第三臂部。
3. 根據(jù)權(quán)利要求2所述的機械手，其特征在于， 所述第一殼體、所述第二殼體以及所述第三殼體的外形分別是長方體， 所述第一角速度傳感器的所述角速度的檢測軸與所述第一殼體的所述長方體的最大 面的法線一致， 所述第二角速度傳感器的所述角速度的檢測軸與所述第二殼體的所述長方體的最大 面的法線一致， 所述第三角速度傳感器的所述角速度的檢測軸與所述第三殼體的所述長方體的最大 面的法線一致。
4. 根據(jù)權(quán)利要求2或3所述的機械手，其特征在于， 所述第一角速度傳感器、所述第二角速度傳感器以及所述第三角速度傳感器分別具有 陀螺元件， 在將相互正交的兩軸設定為第一軸以及第二軸時， 所述陀螺元件具備：基部； 一對檢測用振動臂，它們從所述基部與所述第一軸平行且向相互相反的方向延伸突 出； 一對連結(jié)臂，它們從所述基部與所述第二軸平行且向相互相反的方向延伸突出；以及 一對驅(qū)動用振動臂，它們從各所述連結(jié)臂的前端部或中部與所述第一軸平行且向相互 相反的方向延伸突出。
5. 根據(jù)權(quán)利要求2?4中任一項所述的機械手，其特征在于， 所述第一殼體在所述第一殼體的角部具有安裝于所述第一臂部的安裝部， 所述第二殼體在所述第二殼體的角部具有安裝于所述第二臂部的安裝部， 所述第三殼體在所述第三殼體的角部具有安裝于所述第三臂部的安裝部。
6. 根據(jù)權(quán)利要求2?5中任一項所述的機械手，其特征在于， 具備具有導電性并將所述第一殼體的所述安裝部固定于所述第一臂部的第一固定部 件，通過所述第一固定部件將所述第一角速度傳感器單元的所述電路部接地連接于所述第 一臂部， 具備具有導電性并將所述第二殼體的所述安裝部固定于所述第二臂部的第二固定部 件，通過所述第二固定部件將所述第二角速度傳感器單元的所述電路部接地連接于所述第 二臂部， 具備具有導電性并將所述第三殼體的所述安裝部固定于所述第三臂部的第三固定部 件，通過所述第三固定部件將所述第三角速度傳感器單元的所述電路部接地連接于所述第 三臂部。
7. 根據(jù)權(quán)利要求2?6中任一項所述的機械手，其特征在于， 所述第一臂部具有框體和與所述框體一體形成的臂部側(cè)安裝部， 所述第一角速度傳感器單元直接安裝于所述臂部側(cè)安裝部。
8. 根據(jù)權(quán)利要求2?7中任一項所述的機械手，其特征在于， 所述第二臂部具有框體和與所述框體一體形成的臂部側(cè)安裝部， 所述第二角速度傳感器單元直接安裝于所述臂部側(cè)安裝部。
9. 根據(jù)權(quán)利要求2?8中任一項所述的機械手，其特征在于， 所述第三臂部具有框體和與所述框體一體形成的臂部側(cè)安裝部， 所述第三角速度傳感器單元直接安裝于所述臂部側(cè)安裝部。
10. 根據(jù)權(quán)利要求1?9中任一項所述的機械手，其特征在于， 具有電纜，所述電纜配設于所述第一臂部，對該機械手供給電力， 所述第一角速度傳感器配置于所述第一臂部的與所述電纜相反一側(cè)的端部。
11. 根據(jù)權(quán)利要求1?10中任一項所述的機械手，其特征在于， 具有電纜，所述電纜配設于所述第二臂部，對該機械手供給電力， 所述第二角速度傳感器配置于所述第二臂部的與所述電纜相反一側(cè)的端部。
12. 根據(jù)權(quán)利要求1?11中任一項所述的機械手，其特征在于， 具有電纜，所述電纜配設于所述第三臂部，對該機械手供給電力， 所述第三角速度傳感器配置于所述第三臂部的與所述電纜相反一側(cè)的端部。
13. 根據(jù)權(quán)利要求1?12中任一項所述的機械手，其特征在于，具備： 第四臂部，其按照能夠以第四轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第三臂部，該 第四轉(zhuǎn)動軸是與所述第三轉(zhuǎn)動軸正交的軸或與正交于所述第三轉(zhuǎn)動軸的軸平行的軸； 第五臂部，其按照能夠以第五轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第四臂部，該 第五轉(zhuǎn)動軸是與所述第四轉(zhuǎn)動軸正交的軸或與正交于所述第四轉(zhuǎn)動軸的軸平行的軸；以及 第六臂部，其按照能夠以第六轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第五臂部，該 第六轉(zhuǎn)動軸是與所述第五轉(zhuǎn)動軸正交的軸或與正交于所述第五轉(zhuǎn)動軸的軸平行的軸。
14. 根據(jù)權(quán)利要求1?13中任一項所述的機械手，其特征在于， 所述第一轉(zhuǎn)動軸與所述基臺的設置面的法線一致。
15. -種機械手控制裝置，其特征在于， 該機械手控制裝置控制機械手的動作，該機械手具備：基臺；第一臂部，其按照能夠以 第一轉(zhuǎn)動軸為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述基臺；第二臂部，其按照能夠以第二轉(zhuǎn)動軸 為轉(zhuǎn)動中心轉(zhuǎn)動的方式連結(jié)于所述第一臂部，該第二轉(zhuǎn)動軸是與所述第一轉(zhuǎn)動軸正交的軸 或與正交于所述第一轉(zhuǎn)動軸的軸平行的軸；以及第三臂部，其按照能夠以第三轉(zhuǎn)動軸為轉(zhuǎn) 動中心而轉(zhuǎn)動的方式連結(jié)于所述第二臂部，該第三轉(zhuǎn)動軸是與所述第二轉(zhuǎn)動軸平行的軸， 所述機械手控制裝置具備： 接收部，其對從設置于所述第一臂部且角速度的檢測軸與所述第一轉(zhuǎn)動軸平行的第一 角速度傳感器輸出的第一信號、從設置于所述第二臂部且角速度的檢測軸與所述第二轉(zhuǎn)動 軸平行的第二角速度傳感器輸出的第二信號以及從設置于所述第三臂部且角速度的檢測 軸與所述第三轉(zhuǎn)動軸平行的第三角速度傳感器輸出的第三信號進行接收； 運算部，其基于由所述接收部接收到的所述第一信號、所述第二信號以及所述第三信 號來求出所述第一臂部的角速度的振動分量、所述第二臂部的角速度的振動分量以及所述 第三臂部的角速度的振動分量；以及 控制部，其基于由所述運算部求出的所述第一臂部的角速度的振動分量、所述第二臂 部的振動分量以及所述第三臂部的角速度的振動分量來控制所述機械手的動作。
16. -種機械手系統(tǒng)，其特征在于，具備： 權(quán)利要求1?14中任一項所述的機械手；以及 機械手控制裝置，其控制所述機械手的動作。
【文檔編號】B25J9/18GK104097200SQ201410089889
【公開日】2014年10月15日 申請日期:2014年3月12日 優(yōu)先權(quán)日:2013年4月10日
【發(fā)明者】仁宇昭雄 申請人:精工愛普生株式會社