TW201641233A - 多關節臂機構之動作控制裝置及機械手臂裝置 - Google Patents

Abstract

本發明係於機械臂之移動控制中消除或減少位置誤差。機器人裝置之動作控制裝置(100)具備：當前位置．姿勢計算部(104)，其自與複數個關節部(J1-J6)分別對應之當前之複數個關節變量計算關注點之當前位置；軌道計算部(105)，其計算自當前位置至最終目標位置之軌道；指令值計算部(106)，其基於軌道上之每隔單位時間之複數個目標位置各者至下一目標位置之距離及單位時間，計算移動速度，將計算之移動速度藉由與機械臂機構(200)之構造相應之雅可比逆矩陣變換成複數個關節部(J1-J6)之複數個關節角速度，且自複數個關節角速度及單位時間計算與下一目標位置對應之複數個關節變量作為複數個指令值；以及驅動器控制部(103)，其將複數個指令值分別輸出至複數個馬達驅動器(201、203、205、207、209、211)。

Description

多關節臂機構之動作控制裝置及機械手臂裝置

本發明之實施形態係關於一種多關節臂機構之動作控制裝置及機器人裝置。

近年來，機器人與使用者位於同一空間之環境不斷變多。認為今後看護用機器人、產業用機器人與作業者一起行進作業之狀況會不斷擴大。此種機器人大多具備垂直多關節臂機構。垂直多關節臂機構針對位置要求3自由度(x、y、z)、針對姿勢要求3自由度(α、β、γ)，通常係利用被稱為根部3軸之旋轉關節部J1、J2、J3及被稱為腕部3軸之旋轉關節部J4、J5、J6而實現。例如，對關節部J1、J4、J6應用扭轉關節部，對關節部J2、J3、J5應用彎曲關節部。於臂前端裝備有手等末端效應器(末端執行器)。

上述末端之位置、姿勢之動作控制有各種方式，作為其一例為，計算當前位置至最終目標位置之軌道，求出此軌道上每隔10ms等單位時間(控制周期)之各個目標位 置。

自目標位置使用利用有齊次變換矩陣之逆運動学求出複數個關節部各者之關節角度時，於實時控制中較佳為代數或幾何學的解法，可應用上述解法之臂機構有限，近年來通常係使用此類限制較少之雅可比逆矩陣之方法。

該方法中，自目標位置藉由雅可比逆矩陣計算複數個關節部各者之關節角速度，將該關節角速度作為指令值輸出至馬達驅動器。馬達驅動器將各關節部之速度作為目標值而驅動致動器(馬達)。

於該方式中，存在因移動開始時或伴隨速度變化之慣性誤差等而實際之末端位置偏離計算上之位置的狀況，為了修正此誤差大多係採用反饋控制。

但，反饋控制為修正動作後追加，其位置之誤差修正係添加於下一指令值，故而無法完全消除位置誤差，且亦會產生移動速度不穩定之事態。

本發明之目的係於機械臂之移動控制中消除或減少位置誤差。

本實施形態之多關節臂機構之動作控制裝置向與構成多關節臂機構之複數個關節部之致動器分別對應之複數個 馬達驅動器分別輸出複數個指令值。於指令值之計算處理中，首先藉由當前位置計算部自與複數個關節部分別對應之當前之複數個關節變量計算多關節臂機構上之關注點之當前位置。藉由軌道計算部計算自當前位置至最終目標位置之關注點之軌道。於指令值計算部中，基於軌道上之每隔單位時間之複數個目標位置各者至下一目標位置之距離及單位時間，計算關注點之移動速度，將計算之移動速度藉由與多關節臂機構之構造相應之雅可比逆矩陣變換成與複數個關節部分別對應之複數個關節角速度，且自計算之複數個關節角速度及單位時間，計算與下一目標位置對應之複數個關節變量作為複數個指令值。自指令值輸出部將複數個指令值輸出至複數個馬達驅動器。

100‧‧‧動作控制裝置

200‧‧‧機械臂機構

300‧‧‧操作部

101‧‧‧系統控制部

102‧‧‧操作部I/F

103‧‧‧驅動器控制部

104‧‧‧當前位置．姿勢計算部

105‧‧‧軌道計算部

106‧‧‧指令值計算部

109‧‧‧控制/資料匯流排

201、203、205、207、209、211‧‧‧馬達驅動器

202、204、206、208、210、212‧‧‧編碼器

圖1係本實施形態之機器人裝置之機械臂機構之外觀立體圖。

圖2係表示圖1之機械臂機構之內部構造之立體圖。

圖3係藉由圖符號表現來表示圖1之機械臂機構之圖。

圖4係本實施形態之機器人裝置之方塊圖。

圖5係圖4之動作控制裝置之動作控制之流程圖。

圖6係圖5之步驟S14之流程圖。

圖7係為了比較速度指令之情形而說明圖5之動作控制(位置指令)之效果所表示之第1關節部之關節角度之 時間變化的圖。

以下，一面參照圖式一面說明本實施形態之機器人裝置。本實施形態之機器人裝置具備動作控制裝置。動作控制裝置可作為對裝備於機器人裝置之多關節臂機構之各關節之馬達驅動器行進控制的單獨之裝置，而組入具備多關節臂機構之機器人裝置。於本實施形態中，係說明具有複數個關節部之一為直動伸縮關節之多關節臂機構之機器人裝置。於以下之說明中，對具有大致相同功能及構成之構成要素附加相同符號，且僅於必要情形時行進重複說明。

圖1係本實施形態之機器人裝置之機械臂機構200之外觀立體圖。機械臂機構200具有大致圓筒形狀之基部1、連接於基部1之臂部2及安裝於臂部2之前端之腕部4。於腕部4設有未圖示之轉接器。例如，轉接器係設於後述之第6旋轉軸RA6之旋轉部。設於腕部4之轉接器係安裝於與用途相應之機器人手。

機械臂機構200具有複數個、此處為6個關節部J1、J2、J3、J4、J5、J6。複數個關節部J1、J2、J3、J4、J5、J6係自基部1依序配設。一般而言，第1、第2、第3關節部J1、J2、J3被稱為根部3軸，第4、第5、第6關節部J4、J5、J6被稱為改變機器人手3之姿勢之腕部3軸。腕部4具有第4、第5、第6關節部J4、J5、J6。構成根部3軸之關節部J1、J2、J3之至少一者為直動伸縮 關節。此處，第3關節部J3係構成為直動伸縮關節部、尤其係伸縮距離相對較長之關節部。臂部2係表現直動伸縮關節部J3(第3關節部J3)之伸縮部分。

第1關節部J1係以例如被基座面垂直支持之第1旋轉軸RA1為中心之扭轉關節。第2關節部J2係以與第1旋轉軸RA1垂直配置之第2旋轉軸RA2為中心的彎曲關節。第3關節部J3係以與第2旋轉軸RA2垂直配置之第3軸(移動軸)RA3為中心讓臂部2直線伸縮之關節。

第4關節部J4係以與第4旋轉軸RA4為中心之扭轉關節。第4旋轉軸RA4係於後述之第7關節部J7不旋轉時、即臂部2全體為直線形狀時，與第3移動軸RA3大致一致。第5關節部J5係以與第4旋轉軸RA4正交之第5旋轉軸RA5為中心之彎曲關節。第6關節部J6係以與第4旋轉軸RA4正交且與第5旋轉軸RA5垂直配置之第6旋轉軸RA6為中心的彎曲關節。

形成基部1之臂支持體(第1支持體)11a具有以第1關節部J1之第1旋轉軸RA1為中心形成之圓筒形狀之中空構造。第1關節部J1係安裝於未圖示之固定台。當第1關節部J1旋轉時，臂部2係與第1支持體11a之軸旋轉一併左右迴旋。另，第1支持體11a亦可固定於接地面。於此情形時，係設為與第1支持體11a分開而臂部2獨立迴旋之構造。於第1支持體11a之上部連接有第2支持部11b。

第2支持部11b具有與第1支持部11a連續之中空構 造。第2支持部11b之一端係安裝於第1關節部J1之旋轉部。第2支持部11b之另一端開放，第3支持部11c係於第2關節部J2之第2旋轉軸RA2轉動自如地嵌入。第3支持部11c具有與第1支持部11a及第2支持部連通之鱗片狀之外裝形成的中空構造。第3支持部11c係伴隨第2關節部J2之彎曲旋轉而其後部被收容至第2支持部11b、或自第2支持部11b送出。構成機械臂機構200之直動伸縮關節部J3(第3關節部J3)之臂部2之後部係藉由其收縮而被收納於第1支持部11a與第2支持部11b連續之中空構造之內部。

第3支持部11c係於其後端下部以第2旋轉軸RA2為中心轉動自如地嵌入至第2支持部11b之開放端下部。藉此，構成作為以第2旋轉軸RA2為中心之彎曲關節部之第2關節部J2。當第2關節部J2轉動時，臂部2以第2旋轉軸RA2為中心朝垂直方向轉動、即起伏動作。

第4關節部J4係具有與沿著臂部2之伸縮方向之臂中心軸、即第3關節部J3之第3移動軸RA3典型地相接之第4旋轉軸RA4的扭轉關節。當第4關節部J4旋轉時，腕部4及安裝於腕部4之機器人手係以第4旋轉軸RA4為中心旋轉。第5關節部J5係具有與第4關節部J4之第4旋轉軸RA4正交之第5旋轉軸RA5之彎曲關節部。當第5關節部J5旋轉時，自第5關節部J5遍及前端而與機器人手一併上下(以第5旋轉軸RA5為中心朝垂直方向)轉動。第6關節部J6係具有與第4關節部J4之 第4旋轉軸RA4正交且與第5關節部J5之第5旋轉軸RA5垂直之第6旋轉軸RA6的彎曲關節。當第6關節部J6旋轉時，機器人手左右迴旋。

如上述般安裝於腕部4之轉接器之機器人手係藉由第1、第2、第3關節部J1、J2、J3而移動至任意位置，且藉由第4、第5、第6關節部J4、J5、J6而配置成任意姿勢。尤其係，第3關節部J3之臂部2之伸縮距離之長度可使機器人手到達至基部1之近接位置至遠隔位置之廣範圍之對象。第3關節部J3之特徵為藉由構成其之直動伸縮機構而實現之直線的伸縮動作及其伸縮距離之長度。

(內部構造之說明)

圖2係表示圖1之機械臂機構200之內部構造之立體圖。直動伸縮機構具有臂部2及射出部30。臂部2具有第1連結鏈節排21及第2連結鏈節排22。第1連結鏈節排21包含複數個第1連結鏈節23。第1連結鏈節23係大致平板形地構成。前後之第1連結鏈節23係於相互之端部部位藉由銷而彎曲自如地呈排狀連結。第1連結鏈節排21可朝內側或外側自如地彎曲。

第2連結鏈節排22包含複數個第2連結鏈節24。第2連結鏈節24構成為橫截面字形狀之短槽狀體。前後之第2連結鏈節24係於相互之底面端部部位藉由銷而彎曲自如地呈排狀連結。第2連結鏈節排22可朝內側彎曲。第2連結鏈節24之剖面為字形狀，故而第2連結 鏈節排22係相鄰之第2連結鏈節24之側板彼此碰撞，不會向外側彎曲。另，將第1、第2連結鏈節23、24之面朝第2旋轉軸RA2之面設為內表面，將其相反側之面設為外表面。第1連結鏈節排21中最前端之第1連結鏈節23、與第2連結鏈節排22中最前端之第2連結鏈節24係藉由結合鏈節27而連接。例如，結合鏈節27具有將第2連結鏈節24與第1連結鏈節23合成後之形狀。

射出部30係由複數個上部輥31及複數個下部輥32支持於角筒形狀之框架35而成。例如，複數個上部輥31係隔開與第1連結鏈節23之長度大致等價之間隔而沿著臂中心軸排列。同樣地，複數個下部輥32係隔開與第2連結鏈節24之長度大致等價之間隔而沿著臂中心軸排列。於射出部30之後方，導輥40與驅動齒輪50係隔著第1連結鏈節排21而對向設置。驅動齒輪50係經由未圖示之減速器而連接於馬達55。於第1連結鏈節23之內表面沿著連結方向而形成有線性齒輪。當複數個第1連結鏈節23呈直線狀整齊排列時相互之線性齒輪係直線狀連接，而構成較長之線性齒輪。驅動齒輪50嚙合於直線狀之線性齒輪。直線狀連接之線性齒輪係與驅動齒輪50一併構成齒輪齒條機構。

(接合構造之說明)

當臂伸長時，馬達55驅動，驅動齒輪50順向旋轉，此時第1連結鏈節排21藉由導輥40而成為與臂中心軸平 行之姿勢，於上部輥31與下部輥32之間被引導。隨著第1連結鏈節排21之移動，第2連結鏈節排22係藉由配置於射出部30後方之未圖示之導軌，而於射出部30之上部輥31與下部輥32之間被引導。於上部輥31與下部輥32之間被引導之第1、第2連結鏈節排21、22相互擠壓。藉此，利用第1、第2連結鏈節排21、22構成柱狀體。射出部30係將第1、第2連結鏈節排21、22接合而構成柱狀體，且上下左右地支持此柱狀體。利用第1、第2連結鏈節排21、22之接合而成之柱狀體係藉由射出部30牢固保持，藉此保持第1、第2連結鏈節排21、22之接合狀態。當維持第1、第2連結鏈節排21、22之接合狀態時，第1、第2連結鏈節排21、22之彎曲係相互約束。藉此，第1、第2連結鏈節排21、22構成具備一定剛性之柱狀體。所謂柱狀體係指於第2連結鏈節排22接合第1連結鏈節排21而成之柱狀之棒體。該柱狀體係由第2連結鏈節24與第1連結鏈節23一併整體構成為各種剖面形狀之筒狀體。所謂筒狀體係定義為上下左右被頂板、底板及兩側板包圍、前端部與後端部開放之形狀。利用第1、第2連結鏈節排21、22之接合而成之柱狀體係結合鏈節27成為始端，沿著第3移動軸RA3直線地自第3支持部11c之開口向外送出。

當臂收縮時，馬達55驅動，驅動齒輪50逆向旋轉，此時與驅動齒輪50卡合之第1連結鏈節排21被拉回至第1支持體11a內。隨著第1連結鏈節排之移動，柱狀體被 拉回至第3支持體11c內。被拉回之柱狀體係於射出部30後方分離。例如，構成柱狀體之第1連結鏈節排21被導輥40與驅動齒輪50夾持，構成柱狀體之第2連結鏈節排22藉由重力而被拉向下方，藉此第2連結鏈節排22與第1連結鏈節排21相互遠離。遠離之第1、第2連結鏈節排21、22分別恢復成可彎曲之狀態，單獨地彎曲，並被收納於第1支持體11a之內部。

(座標系之定義)

圖3係藉由圖符號表現來表示圖1之機械臂機構200之圖。於機械臂機構200中，構成根部3軸之第1關節部J1、第2關節部J2及第3關節部J3實現3個位置自由度。又，構成腕部3軸之第4關節部J4、第5關節部J5及第6關節部J6實現3個姿勢自由度。

第1關節部J1係配設於第1支持部11a與第2支持部11b之間，構成為以旋轉軸RA1為中心之扭轉關節。旋轉軸RA1係與設置有第1關節部J1之固定部之基座之基準面BP垂直而配置。與旋轉軸RA1平行地規定Z軸。規定以Z軸為中心之正交3軸之機器人座標系Σb(Xb、Yb、Zb)。

第2關節部J2係構成為以旋轉軸RA2為中心之彎曲關節。第2關節部J2之旋轉軸RA2係與空間座標系上之Xb軸平行地設置。第2關節部J2之旋轉軸RA2係設為與第1關節部J1之旋轉軸RA1垂直之朝向。進而，第2關 節部J2係相對於第1關節部J1而於第1旋轉軸RA1之方向(Zb軸方向)及與第1旋轉軸RA1垂直之Yb軸方向此2方向上偏置。以第2關節部J2相對於第1關節部J1而於上述2方向偏置之方式，將第2支持體11b安裝於第1支持體11a。於第1關節部J1連接第2關節部J2之假想臂桿部分(鏈路部分)具有由前端彎曲成直角之2個鈎形狀體組合而成之曲柄形狀。該假想臂桿部分係由具有中空構造之第1、第2支持體11a、11b構成。

第3關節部J3係構成為以移動軸RA3為中心之直動伸縮關節。第3關節部J3之移動軸RA3係設為與第2關節部J2之旋轉軸RA2垂直之朝向。於第2關節部J2之旋轉角為零度、即臂部2之起伏角為零度且臂部2為水平之基準姿勢下，第3關節部J3之移動軸RA3係設為與第2關節部J2之旋轉軸RA2以及第1關節部J1之旋轉軸RA1垂直之方向。於空間座標系上，第3關節部J3之移動軸RA3係設為平行於與Xb軸及Zb軸垂直之Yb軸。進而，第3關節部J3係相對於第2關節部J2而於其旋轉軸RA2之方向(Yb軸方向)、與移動軸RA3正交之Zb軸之方向此2方向上偏置。以第3關節部J3相對於第2關節部J2於上述2方向偏置之方式，將第3支持體11c安裝於第2支持體11b。於第2關節部J2連接第3關節部J3之假想臂桿部分(鏈路部分)具有前端垂直彎曲之鈎形狀體。該假想臂桿部分係由第2、第3支持體11b、11c構成。

第4關節部J4係構成為以旋轉軸RA4為中心之扭轉 關節。第4關節部J4之旋轉軸RA4係以與第3關節部J3之移動軸RA3大致一致之方式配置。

第5關節部J5係構成為以旋轉軸RA5為中心之彎曲關節。第5關節部J5之旋轉軸RA5係以與第3關節部J3之移動軸RA3及第4關節部J4之旋轉軸RA4大致正交之方式配置。

第6關節部J6係構成為以旋轉軸RA6為中心之扭轉關節。第6關節部J6之旋轉軸RA6係以與第4關節部J4之旋轉軸RA4及第5關節部J5之旋轉軸RA5大致正交之方式配置。第6關節部J6係為了使作為末端效應器之機器人手左右迴旋而設置。另，第6關節部J6亦可構成為其旋轉軸RA6與第4關節部J4之旋轉軸RA4及第5關節部J5之旋轉軸RA5大致正交之彎曲關節。

如此，將複數個關節部J1-J6之根部3軸之一之彎曲關節部換成直動伸縮關節部，相對於第1關節部J1使第2關節部J2於2方向偏置，相對於第2關節部J2使第3關節部J3於2方向偏置，藉此本實施形態之機器人裝置之機械臂機構200於構造上消除臨界點姿勢。

(方塊構成)

圖4係本實施形態之機器人裝置之方塊圖。另，於本實施形態之機械臂機構200之關節部J1、J2、J3、J4、J5、J6，作為致動器而設有例如步進馬達。於該等步進馬達，分別電性連接有馬達驅動器201、203、205、207、 209、211。又，該等步進馬達201、203、205、207、209、211之驅動軸上，分別連接有旋轉編碼器202、204、206、208、210、212，用於藉由計數器對每隔一定旋轉角輸出之脈衝行進加減算以測定角位置。

動作控制裝置100係為了藉由關節部J1-J6之動作控制而控制末端等之關注點之移動，且具有系統控制部101、操作部介面(I/F)102、驅動器控制部103、當前位置．姿勢計算部104、軌道計算部105、及指令值計算部106。

系統控制部101具有CPU(Central Processing Unit)及半導體記憶體等，統括地控制動作控制裝置100。於系統控制部101經由控制/資料匯流排109而連接有各部。

於動作控制裝置100經由操作部介面102而連接有操作部300。操作部300係作為用於供使用者輸入安裝於腕部4之機器人手(末端效應器)之移動或姿勢變更之輸入介面而發揮功能。於動作控制裝置100中，例如關注機器人手之把持中心點，將此點(末端基準點)作為控制點(末端座標系Σh之原點)而執行移動．姿勢變更等計算處理。於動作控制裝置100經由操作部300而輸入有末端之最終目標位置等。操作部300具有例如開關、滑鼠、鍵盤、軌跡球及觸控面板等輸入元件。

驅動器控制部103統括地控制馬達驅動器201、203、205、207、209、211。驅動器控制部103係將與由後述之指令值計算部106對關節部J1-J6之每一個計算 之指令值相應的控制信號，發送至馬達驅動器201、203、205、207、209、211各者。

當前位置．姿勢計算部104基於關節部J1、J2、J3、J4、J5、J6各者之關節變量，依照根據臂構造之鏈路參數規定之齊次變換矩陣，藉由順向運動学計算自機器人座標系觀察之末端基準點之位置及末端姿勢。另，所謂關節變量對於關節部J1、J2、J4、J5、J6而言係自基準位置之正負旋轉角度，若為關節部J3則係自最收縮狀態之拉伸距離(直動變位)。

如圖3所示，機器人座標系Σb係將第1關節部J1之第1旋轉軸RA1上之任意位置設為原點之座標系。於機器人座標系Σb中，規定正交3軸(Xb、Yb、Zb)。Zb軸係與第1旋轉軸RA1平行之軸。Xb軸與Yb軸係相互正交且與Zb軸正交之軸。末端座標系Σh係以安裝於腕部4之機器人手之任意位置(末端基準點)為原點之座標系。例如，當機器人手為2指手時，末端基準點之位置係規定為2指尖間中央位置。於末端座標系Σh中，規定正交3軸(Xh、Yh、Zh)。Zh軸係與第6旋轉軸RA6平行之軸。Xh軸與Yh軸係相互正交且與Zh軸正交之軸。例如，Xh軸係與機器人手之前後方向平行之軸。所謂末端姿勢係指繞末端座標系Σh相對於機器人座標系Σb之正交3軸各之旋轉角(繞Xh軸之旋轉角(擺動角)α、繞Yh軸之旋轉角(俯仰角)β、繞Zh軸之旋轉角(側傾角)γ。

當前位置．姿勢計算部104基於編碼器202、204、206、208、210、212之編碼器脈衝，計算機械臂機構200之關節變量向量^-θ。關節變量向量^-θ係由旋轉關節部J1、J2、J4、J5、J6之關節角度θ_J1、θ_J2、θ_J4、θ_J5、θ_J6與直動伸縮關節部J3之伸縮長L_J3賦予之關節變量之組(θ_J1、θ_J2、L_J3、θ_J4、θ_J5、θ_J6)。當前位置．姿勢計算部104藉由齊次變換矩陣K(參數(θ_J1、θ_J2、L_J3、θ_J4、θ_J5、θ_J6))，計算自機器人座標系Σb觀察之末端基準點之位置(x、y、z)及末端姿勢(α、β、γ)。齊次變換矩陣K係定義末端座標系Σh與機器人座標系Σb之關係的矩陣式。齊次變換矩陣K係由構成機械臂機構200之鏈路間之關係(鏈路長與鏈路之扭轉角)及關節部之軸間之關係(鏈路間距離與鏈路之間角度)決定。例如，當前位置．姿勢計算部104藉由將當前之機械臂機構200之關節變量向量^-θ₀(θ_0-J1、θ_0-J2、L_0-J3、θ_0-J4、θ_0-J5、θ_0-J6)代入齊次變換矩陣K，而計算自機器人座標系Σb觀察之末端基準點之當前位置x₀(x₀、y₀、z₀)及末端姿勢φ₀(α₀、β₀、γ₀)。

軌道計算部105係基於末端基準點(以下僅稱為末端)之當前位置．姿勢與末端之最終目標位置．姿勢，計算每隔單位時間(控制周期，例如10ms)之末端之目標位置。藉由賦予目標位置，成為由目標位置規定之位置與姿勢，可移動末端。末端之當前位置．姿勢係利用當前位置．姿勢計算部104之計算處理而獲得。末端之最終目標 位置．姿勢例如係經由操作部300而由使用者輸入。

軌道計算部105係藉由向預先設置之以末端之當前位置x₀、當前姿勢φ₀、末端之最終目標位置X₁、最終目標姿勢Φ₁為參數之多項式中代入各參數，而計算末端之軌道(以下稱為末端軌道)，並計算此末端軌道上每隔單位時間△t之目標位置。作為軌道計算方法係採用任意之方法。單位時間△t作為控制周期而為固定值，由使用者設定為例如10ms。軌道計算部105基於將末端自當前位置移動至最終目標位置所需之目標時間寬度T及單位時間△t，決定目標位置之數m(=T/△t)，計算每隔單位時間△t之末端軌道上之末端之目標位置(x₁、x₂．．．x_m(m=t1/△t))。例如，目標位置x₁係賦予末端基準點之位置x₁及末端姿勢φ₁之兩方之參數。目標時間T例如可由使用者直接輸入，亦可根據使用者操作操作部300之期間而決定，還可以根據末端之當前位置x₀與末端之最終目標位置X₁之距離而自動決定。例如，末端軌道作為將末端之當前位置x₀與末端之最終目標位置X₁連結之直線軌道而計算。

指令值計算部106計算末端之當前位置與末端之最終目標位置之間之複數個目標位置相關之複數個關節變量向量。另，所謂關節變量向量係指關節部J1-J6之6個關節變量、即包含旋轉關節部J1、J2、J4-J6之旋轉角及直動伸縮關節部J3之臂伸縮長。關於指令值計算部106之計算處理於下文敘述。圖5係圖4之動作控制裝置100之 動作控制之流程圖。

(步驟S11)計算當前之關節變量向量^-θ₀

依照系統控制部101之控制，藉由當前位置．姿勢計算部104，基於自編碼器202、204、206、208、210、212輸入之編碼器脈衝，計算當前之關節變量向量^-θ₀(θ_0-J1、θ_0-J2、L_0-J3、θ_0-J4、θ_0-J5、θ_0-J6)。

(步驟S12)計算末端之當前位置x₀

依照系統控制部101之控制，藉由當前位置．姿勢計算部104，基於當前之關節變量向量^-θ₀，計算機器人座標系Σb上之末端之當前位置x₀(x₀、y₀、z₀)與末端姿勢φ₀(α₀、β₀、γ₀)。

(步驟S13)計算末端之目標位置(x₁、x₂、．．．x_m(=X₁))

依照系統控制部101之控制，藉由軌道計算部105，計算自末端之當前位置x₀．末端姿勢φ₀至末端之最終目標位置X₁．末端姿勢Φ₁之末端軌道，並計算此末端軌道上每隔單位時間△t之末端之目標位置(x₁、x₂、．．．x_m(=X₁))。

(步驟S14)計算目標之關節變量向量(^-θ₁、^-θ₂、．．．^-θ_m)

依照系統控制部101之控制，藉由指令值計算部106，計算與末端之目標位置(x₁、x₂、．．．x_m(=X₁))分別對應之關節變量向量(^-θ₁、^-θ₂、．．．^-θ_m)。關於指令值計算部106之計算處理順序之詳細將於下文敘述。

(步驟S15)輸出目標之關節變量向量(^-θ₁、^-θ₂、．．．^-θ_m)

自驅動器控制部103向馬達驅動器201、203、205、207、209、211，以特定之控制周期△t(例如，10ms)依序輸出目標之關節變量向量(^-θ₁、^-θ₂、．．．^-θ_m)作為指令值。

其次，參照圖6對步驟S14之指令值計算部106之計算處理行進說明。圖6係圖5之步驟S14之流程圖。

(步驟S141)初始化(n=0)

此處為了便於說明，使用變量n。首先，將該變量n初始化為零值。

(步驟S142)計算雅可比逆矩陣J^-1(^-θ_n)

本實施形態之臂機構於其構造上不存在臨界點，因此始終存在雅可比逆矩陣。雅可比逆矩陣係將末端速度(末端位置．姿勢之微小變化)變換成關節角速度(關節角度．伸縮長之微小變化)之矩陣。雅可比逆矩陣係藉由表 現末端基準點之位置．末端姿勢之向量之關節變量之編微分而賦予。指令值計算部106根據於步驟S11中由當前位置．姿勢計算部104計算之當前之關節變量向量^-θ_n(θ_n-J1、θ_n-J2、L_n-J3、θ_n-J4、θ_n-J5、θ_n-J6)，計算臂構造之鏈路參數之雅可比逆矩陣J^-1(^-θ_n)。

(步驟S143)計算末端速度^-x_n+1

基於當前之末端位置(當前之目標位置)x_n、下一末端位置(單位時間△t後之下一目標位置)x_n+1、單位時間△t，計算末端速度^．x_n+1。

(步驟S144)計算關節角速度^-．θ_n+1

藉由雅可比逆矩陣J^-1(^-θ_n)將步驟S143中計算之末端速度^-x_n+1變換成關節角速度^-．θ_n+1。

(步驟S145)計算目標之關節變量向量^-θ_n+1

基於重複次數前之步驟S145中計算之(變量n=0時步驟S11中計算之)關節變量向量^-θ_n、步驟S144中計算之關節角速度^-．θ_n+1、單位時間△t，計算下一關節變量向量^-θ_n+1。藉由向關節角速度^-．θ_n+1乘以單位時間△t，計算單位時間△t之間之各關節之變位量。藉由在正要移動前之關節變量向量^-θ_n架上關節之變位量，而計算單位時間△t經過後之關節變量向量^-θ_n+1。

(步驟S146)處理繼續之判定

當變量n為重複次數(m-1)時，結束指令值計算部106之計算處理。另一方面，當變量(n)並非重複次數(m-1)時，將處理移行至步驟S147。

(步驟S147)變量之提升n←n+1

將變量n提升為(n+1)，使處理返回至步驟S142。

藉由以上說明之指令值計算部106之計算處理，計算與末端之目標位置(x₁、x₂、．．．x_m)分別對應之關節變量向量(^-θ₁、^-θ₂、．．．^-θ_m)。

其次，參照圖7對本實施形態之機器人裝置之動作控制裝置100之動作控制之效果行進說明。本實施形態之機器人裝置之動作控制裝置100基於末端之當前位置及末端之下一目標位置，計算與末端之目標位置對應之關節變量(關節角度、拉伸距離)θ_J1、θ_J2、L_J3、θ_J4、θ_J5、θ_J6，並將此關節變量θ_J1、θ_J2、L_J3、θ_J4、θ_J5、θ_J6分別作為指令值輸入至馬達驅動器201、203、205、207、209、211。馬達驅動器201、203、205、207、209、211將各關節部之關節角度、拉伸距離作為目標值而驅動致動器(步進馬達)。

圖7係為了比較速度指令之情形而說明圖5之動作控制(位置指令)之效果所示之第1關節部J1之關節角度之時間變化的圖。於圖7中，作為一例，關於第1關節部J1之關節角度θ_J1，將相對於馬達驅動器201以位置(關 節角度)為指令值時之時間變化，與將速度(關節角速度)為指令值時之時間變化一併表示。第1關節部J1之馬達驅動器201係自驅動器控制部103而於時刻t₀、t₁、t₂、．．．．t_m-1分別作為指令值被提供關節角度θ_1-J1、θ_2-J1、θ_{3-J1、．．．}θ_m-J1。如圖7所示，馬達驅動器201於自時刻t₀經過單位時間△t後之時刻t₁，將使第1關節部J1之關節角度到達關節角度θ_1-J1所需之脈衝供給至步進馬達。藉此，第1關節部J1之關節角度係於自時刻t₀經過單位時間△t後之時刻t₁，到達關節角度θ_1-J1。此處重要的是，即便因某種原因，第1關節部J1之關節角度於自時刻t₀經過單位時間△t後之時刻t₁未旋轉至關節角度θ_1-J1，計算上之預定的關節角度θ_1-J1與實際之關節角度之間之誤差在後面亦不會累積。即，馬達驅動器201即便於自時刻t₀經過單位時間△t後之時刻t₁未到達關節角度θ_1-J1，於自下一時刻t₁經過單位時間△t後之時刻t₂，亦會將使第1關節部J1之關節角度到達關節角度θ_2-J1所需之脈衝供給至步進馬達。藉此，第1關節部J1之關節角度即便於時刻t₁未旋轉至預定之關節角度θ_1-J1，亦可自時刻t₁之實際之關節角度於經過單位時間△t後之時刻t₂到達關節角度θ_1-J1。

另一方面，向馬達驅動器201輸入關節角速度作為指令值時，移動開始時或伴隨速度變化之慣性誤差累積，第1關節部J1之實際之關節角度有時偏離計算上預定的關節角度。另一方面，藉由如本實施形態之機器人裝置之動作 控制裝置100般，向馬達驅動器201輸入關節角度作為指令值，馬達驅動器201以於單位時間△t內移動至指令第1關節部J1之關節角度之關節角度的方式驅動步進馬達。藉此，即便於第1關節部J1之動作控制途中產生位置誤差時，此位置誤差亦不會累積。因此，本實施形態之機器人裝置於機械臂之移動控制中可消除或減少位置誤差。

即，於本實施形態中，計算目標位置間之末端速度，藉由雅可比逆矩陣將其變換成關節角速度(先前係將該關節角速度作為指令值)，自上述關節角速度求出關節變量向量，並將關節變量向量作為指令值供給至馬達驅動器，藉此於馬達驅動器中，係以於單位時間內移動至指令的關節變量向量之方式驅動馬達，因此與將關節角速度設為指令值之情形不同，原理上不會產生位置誤差、或可使位置誤差非常低，而無需反饋控制。此外，由於不產生位置誤差，故而亦可一面依序推進目標位置一面不斷的計算目標位置之關節變量向量，即亦可事先計算所有目標位置之關節變量向量。又，雖可藉由使用齊次變換矩陣之逆運動学求出關節變量向量，但此時為非線性演算所以計算處理非常複雜，且有時無法解析性地求出唯一解，但雅可比逆矩陣為線性演算且計算處理非常簡單，不會產生此種問題。本實施形態之直動伸縮臂機構與組合通常之旋轉關節而成之臂構造不同，不存在冗長性，所以並無產生無數解之狀況，因此，可於雅可比逆矩陣中簡單地處理。

雖對本發明之若干實施形態行進了說明，但該等實施 形態係作為示例而提示者，並不意圖限定發明之範圍。該等實施形態能以其他各種形態實施，且於不脫離發明主旨之範圍內，可行進各種省略、置換、變更。該等實施形態及其變形包含於發明之範圍及主旨，且同樣包含於申請專利範圍所記載之發明及其均等範圍內。

100‧‧‧動作控制裝置

200‧‧‧機械臂機構

300‧‧‧操作部

101‧‧‧系統控制部

102‧‧‧操作部I/F

103‧‧‧驅動器控制部

104‧‧‧當前位置．姿勢計算部

105‧‧‧軌道計算部

106‧‧‧指令值計算部

109‧‧‧控制/資料匯流排

201、203、205、207、209、211‧‧‧馬達驅動器

202、204、206、208、210、212‧‧‧編碼器

Claims (8)

1. 一種多關節臂機構之動作控制裝置，其係向與構成多關節臂機構之複數個關節部之致動器分別對應之複數個馬達驅動器分別輸出複數個指令值者，且具備：當前位置計算部，其自與上述複數個關節部分別對應之當前之複數個關節變量計算上述多關節臂機構上之關注點之當前位置；軌道計算部，其計算上述當前位置至最終目標位置之上述關注點之軌道；指令值計算部，其基於上述軌道上之每單位時間之複數個目標位置各者至下一目標位置之距離及上述單位時間，計算上述關注點之移動速度，將上述計算之移動速度藉由與上述多關節臂機構之構造相應之雅可比逆矩陣而變換成與上述複數個關節部分別對應之複數個關節角速度，且自上述計算之複數個關節角速度及上述單位時間，計算與上述下一目標位置對應之複數個關節變量作為上述複數個指令值；以及指令值輸出部，其將上述複數個指令值分別輸出至上述複數個馬達驅動器。
2. 如申請專利範圍1之多關節臂機構之動作控制裝置，其中上述指令值計算部係一面自上述當前位置朝上述最終目標位置依序聯繫一面針對上述複數個目標位置各者計算上述複數個關節變量。
3. 如申請專利範圍1之多關節臂機構之動作控制裝 置，其中上述指令值輸出部係每經過上述單位時間便將與上述下一目標位置相關之上述複數個指令值依序輸出至上述馬達驅動器。
4. 如申請專利範圍3之多關節臂機構之動作控制裝置，其中上述指令值輸出部係與藉由上述指令值計算部計算與上述複數個目標位置之全體相關之上述複數個關節變量之計算處理並行地，依序將上述複數個指令值輸出至上述馬達驅動器。
5. 如申請專利範圍1之多關節臂機構之動作控制裝置，其中上述複數個關節部係旋轉關節。
6. 如申請專利範圍1之多關節臂機構之動作控制裝置，其中於上述複數個關節部並無肘關節部，上述複數個關節部之至少一者為直動伸縮關節部。
7. 如申請專利範圍6之多關節臂機構之動作控制裝置，其中上述直動伸縮關節部具有：複數個第1連結鏈節，其等具有可彎曲地連結之剖面字形狀；複數個第2連結鏈節，其等具有可彎曲地連結之平板形狀，上述複數個第2連結鏈節中之前端之第2連結鏈節係與上述複數個第1連結鏈節中之前端之第1連結鏈節連接，上述第1連結鏈節於其上部接合上述第2連結鏈節，藉此約束彎曲而構成柱狀體，且藉由上述第1、第2連結鏈節之分離而解除上述柱狀體；以及射出部，其將上述第1連結鏈節接合於上述第2連結 鏈節構成上述柱狀體，且支持上述柱狀體。
8. 一種機器人裝置，其具有：多關節臂機構，其具備複數個關節部；及動作控制裝置，其向與構成上述多關節臂機構之複數個關節部之致動器分別對應之複數個馬達驅動器分別輸出複數個指令值；且上述動作控制裝置具有：當前位置計算部，其自與上述複數個關節部分別對應之當前之複數個關節變量，計算上述多關節臂機構上之關注點之當前位置；軌道計算部，其計算自上述當前位置至最終目標位置之上述關注點之軌道；指令值計算部，其基於上述軌道上之每隔單位時間之複數個目標位置各者至下一目標位置之距離及上述單位時間，計算上述關注點之移動速度，將上述計算之移動速度藉由與上述多關節臂機構之構造相應之雅可比逆矩陣，變換成與上述複數個關節部分別對應之複數個關節角速度，且自上述計算之複數個關節角速度及上述單位時間，計算與上述下一目標位置對應之複數個關節變量作為上述複數個指令值；以及指令值輸出部，其將上述複數個指令值分別輸出至上述複數個馬達驅動器。