TW201702030A - 機器人裝置及步進馬達控制裝置 - Google Patents

機器人裝置及步進馬達控制裝置 Download PDF

Info

Publication number
TW201702030A
TW201702030A TW105115564A TW105115564A TW201702030A TW 201702030 A TW201702030 A TW 201702030A TW 105115564 A TW105115564 A TW 105115564A TW 105115564 A TW105115564 A TW 105115564A TW 201702030 A TW201702030 A TW 201702030A
Authority
TW
Taiwan
Prior art keywords
track
offset
unit
command value
stepping motor
Prior art date
Application number
TW105115564A
Other languages
English (en)
Inventor
尹祐根
Original Assignee
生活機器人學股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 生活機器人學股份有限公司 filed Critical 生活機器人學股份有限公司
Publication of TW201702030A publication Critical patent/TW201702030A/zh

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/087—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices for sensing other physical parameters, e.g. electrical or chemical properties
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40512—Real time path planning, trajectory generation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40516—Replanning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40519—Motion, trajectory planning
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/19—Drive system for arm
    • Y10S901/23—Electric motor
    • Y10S901/24—Stepper motor

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)

Abstract

本發明係以步進馬達產生失調為前提而提供產生失調時之較佳應對。機器人裝置具備具有關節部之機械臂機構、產生使關節部作動之動力的步進馬達(310)、驅動步進馬達之馬達驅動器(210)、計算機械臂機構之關注點自當前位置移動至最終目標位置之軌道的軌道計算部(105)、向馬達驅動器輸出與軌道計算部所計算之軌道相應的指令值之指令值輸出部(103)、以及偵測步進馬達之失調之失調偵測部(216)。進而，機器人裝置具備系統控制部(101)。系統控制部於偵測到失調時，再次計算自因失調而移位之關注點之位置至最終目標位置之軌道，為了依照再次計算之軌道使關注點移動，而控制軌道計算部及指令值輸出部。

Description

機器人裝置及步進馬達控制裝置
本發明之實施形態係關於一種機器人裝置及步進馬達控制裝置。
步進馬達因旋轉角與脈衝信號之數成比例，故而基本上無反饋電路之必要性，便可進行開環控制，相較AC馬達、DC馬達而言有利，但反過來，若施加過負荷或脈衝頻率過高，則同步偏移而產生控制紊亂之所謂「失調」。若產生失調，則步進馬達停止。又，即便重新開始，要到達目標位置則必須返回至基準位置，由此不可避免地產生停工時間。失調現象於步進馬達中不可避免，因此先前一直嘗試有不引起步進馬達失調之各種努力，但大多數治標不治本。
本發明之目的係以步進馬達產生失調為前提，提供產 生失調時之較佳對策。
本實施形態之機器人裝置具備：機械臂機構，其具有關節部；步進馬達，其產生使關節部作動之動力；馬達驅動器，其驅動步進馬達；軌道計算部，其計算機械臂機構之關注點自當前位置移動至最終目標位置之軌道；指令值輸出部，其將與軌道計算部計算之軌道相應的指令值輸出至馬達驅動器；以及失調偵測部，其偵測步進馬達之失調。進而，機器人裝置具備系統控制部。系統控制部偵測到失調時，再次計算自因失調而移位之關注點之位置至最終目標位置之軌道，為了依照再次計算之軌道移動關注點，控制軌道計算部及指令值輸出部。
50‧‧‧操作部
100‧‧‧動作控制裝置
101‧‧‧系統控制部
102‧‧‧操作部I/F
103‧‧‧指令值輸出部
104‧‧‧當前位置．姿勢計算部
105‧‧‧軌道計算部
106‧‧‧指令值計算部
107‧‧‧動態計算部
108‧‧‧驅動電流決定部
109‧‧‧驅動器單元I/F
110‧‧‧控制/資料匯流排
210~260‧‧‧驅動器單元
310‧‧‧步進馬達
211‧‧‧控制部
212‧‧‧電源電路
213‧‧‧脈衝信號產生部
215‧‧‧編碼器
216‧‧‧失調判定部
217‧‧‧計數器
圖1係本實施形態之機器人裝置之機械臂機構之外觀立體圖。
圖2係表示圖1之機械臂機構之內部構造之側視圖。
圖3係藉由圖符號表現來表示圖1之機械臂機構之構成之圖。
圖4係表示本實施形態之機器人裝置之構成之方塊圖。
圖5係表示圖4之動作控制裝置之失調時之處理次序的流程圖。
圖6係表示圖5之失調後之複數個軌道之圖。
圖7係表示圖5之步驟S13之次序之流程圖。
以下，一面參照圖式一面對本實施形態之機器人裝置進行說明。本實施形態之機器人裝置具備動作控制裝置。動作控制裝置係作為單獨之裝置發揮功能，控制裝備於機器人裝置之多關節臂機構之各關節之馬達驅動器。又，動作控制裝置亦可組入具備多關節臂機構之機器人裝置。本實施形態之機器人裝置之動作控制裝置可應用於多種形態之多關節臂機構。此處，以複數個關節部之一為直動伸縮關節之多關節臂機構為一例進行說明。於以下之說明中，對具有大致相同功能及構成之構成要素附加相同符號，且僅於必要之情形時進行重複說明。
圖1係本實施形態之機械臂機構之外觀立體圖。機械臂機構具有大致圓筒形狀之基部10、連接於基部10之臂部2及安裝於臂部2之前端之腕部4。於腕部4設置有未圖示之轉接器。例如，轉接器設置於後述之第6旋轉軸RA6之旋轉部。於設置於腕部4之轉接器，安裝有與用途相應的機器人手。
機械臂機構具有複數個、此處為6個關節部J1、J2、J3、J4、J5、J6。複數個關節部J1、J2、J3、J4、J5、J6係自基部10依序配設。一般而言，第1、第2、第3關節部J1、J2、J3被稱為根部3軸，第4、第5、第6關節部 J4、J5、J6被稱為改變機器人手之姿勢之腕部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為中心的彎曲關節。
形成基部10之臂支持體(第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送出。構成機械臂機構之直動伸縮關節部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之伸縮距離之長度可使機器人手到達自基部10之近接位置至遠隔位置之廣範圍之對象。第3關節部J3之特徵為藉由構成其之直動伸縮機構而實現之直線伸縮動作及其伸縮距離之長度。
(內部構造之說明)圖2
圖2係表示圖1之機械臂機構之內部構造之立體圖。直動伸縮機構具有臂部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係經由未圖示之減速器而連接於步進馬達330。於第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
圖3係藉由圖符號表現來表示圖1之機械臂機構之圖。於機械臂機構中，藉由構成根部3軸之第1關節部J1、第2關節部J2及第3關節部J3，而實現3個位置自由度。又，藉由構成腕部3軸之第4關節部J4、第5關節部J5及第6關節部J6而實現3個姿勢自由度。
機器人座標系Σ b係以第1關節部J1之第1旋轉軸RA1上之任意位置為原點之座標系。於機器人座標系Σ b中，規定有正交3軸(Xb、Yb、Zb)。Zb軸係與第1旋轉軸RA1平行之軸。Xb軸與Yb軸係相互正交、且與Zb軸正交之軸。末端座標系Σ h係以安裝於腕部4之機器人手5之任意位置(末端基準點)為原點之座標系。例如，當機器人手5為2指手時，末端基準點(以下僅稱為末 端)之位置係規定於2指尖間中央位置。於末端座標系Σ h中，規定有正交3軸(Xh、Yh、Zh)。Zh軸係與第6旋轉軸RA6平行之軸。Xh軸與Yh軸係相互正交、且與Zh軸正交之軸。例如，Xh軸係與機器人手5之前後方向平行之軸。所謂末端姿勢係設為末端座標系Σ h之相對於機器人座標系Σ b之繞正交3軸各者之旋轉角(繞Xh軸之旋轉角(側傾角)α、繞Yh軸之旋轉角(俯仰角)β、繞Zh軸之旋轉角(擺動角)γ)。
第1關節部J1係配設於第1支持部11a與第2支持部11b之間，構成為以旋轉軸RA1為中心之扭轉關節。旋轉軸RA1係與設置有第1關節部J1之固定部之基座之基準面BP垂直地配置。
第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係為了使作為末端效應器之機器人手5左右迴旋而設置。另，第6關節部J6亦可構成其旋轉軸RA6與第4關節部J4之旋轉軸RA4及第5關節部J5之旋轉軸RA5大致正交的彎曲關節。
如此，將複數個關節部J1-J6之根部3軸中之一個彎曲關節部更換為直動伸縮關節部，使第2關節部J2於2方向上相對於第1關節部J1偏移，且使第3關節部J3於2方向上相對於第2關節部J2偏移，藉此本實施形態之機器人裝置之機械臂機構於構造上消除臨界點姿勢。
(區塊構成圖)圖4
圖4係表示本實施形態之機器人裝置之構成的方塊圖。於本實施形態之機器人裝置之機械臂機構之關節部J1、J2、J3、J4、J5、J6分別設置有步進馬達310、320、330、340、350、360作為致動器。於步進馬達310、320、330、340、350、360電性連接有驅動器單元210、220、230、240、250、260。典型而言，驅動器單元210、220、230、240、250、260係與控制對象之步進馬達310、320、330、340、350、360併設。該等驅動器單元210、220、230、240、250、260具有相同構成，依照來自動作控制裝置100之控制信號，對控制對象之步進馬達進行相同動作。此處，僅對驅動器單元210進行說明， 省略其它驅動器單元220、230、240、250、260之說明。
驅動器單元210控制步進馬達310之驅動及停止。驅動器單元210具有控制部211、電源電路212、脈衝信號產生部213、旋轉編碼器215、失調判定部216、及計數器217。控制部211依照自動作控制裝置100輸入之控制信號，統括地控制驅動器單元210。
具體而言，自動作控制裝置100向驅動器單元210內之控制部211輸入與步進馬達310之旋轉角之單位時間△t經過後之目標位置(區別於後述之最終目標位置)相關的位置指令信號。控制信號係作為△t後之關節變量(關節角度，J3中為伸縮距離)而提供。控制部211基於步進馬達310之旋轉角之當前位置及目標位置，決定脈衝數Np。具體而言，控制部211基於步進馬達310之旋轉角之當前位置及目標位置計算旋轉角度差，並以當前位置與目標位置之旋轉角度差除以步進馬達310之例如0.72度之步進角，藉此決定脈衝信號之脈衝數Np。又，控制部211基於所決定之脈衝數與單位時間△t，決定脈衝頻率fp。具體而言，控制部211於單位時間△t上乘以脈衝數Np，並計算其倒數，藉此決定脈衝頻率fp。控制部211對脈衝信號產生部213輸出與脈衝數相關之脈衝數指令信號及與脈衝頻率相關之脈衝頻率指令信號。脈衝信號產生部213對步進馬達310之各相，按由脈衝頻率指令信號指令之頻率、即該頻率之倒數之周期，輸出由脈衝數指令信號指令之脈衝數之脈衝信號。自脈衝信號產生部213輸出之脈衝 信號亦被取入後述之失調判定部216。
於控制部211，自動作控制裝置100輸入有表示步進馬達310之驅動電流值之驅動電流指令信號。控制部211對電源電路212輸出與驅動電流指令信號相應的控制信號。電源電路212使用自未圖示之外部之電源供給部供給之電力，產生具有指定之驅動電流值之驅動電流。將產生之驅動電流供給至步進馬達310。步進馬達310係藉由自電源電路212供給之驅動電流而驅動，且依照自脈衝信號產生部213輸出之脈衝信號而旋轉。
旋轉編碼器215連接於步進馬達310之驅動軸，每隔例如0.18度之一定之旋轉角而輸出脈衝信號(編碼器脈衝)。計數器217根據旋轉方向對自旋轉編碼器215輸出之編碼器脈衝之數進行加減算，藉此計算計數數。該計數數係於步進馬達310之驅動軸之基準位置(原點)重設。根據重設次數及計數數而求出關節部之關節角度(關節變量)。
失調判定部216比較與自脈衝信號產生部213輸出之脈衝信號相對之編碼器脈衝之計數數，藉此判定步進馬達310是否產生失調。失調判定部216同步於脈衝信號而重複進行編碼器脈衝之計數/重設。脈衝信號之一周期內旋轉之步進角固定。對應於步進角之編碼器脈衝之數固定。若編碼器脈衝之計數數與對應於步進角之數一致，則步進馬達310之旋轉與脈衝信號同步、即未產生失調。另一方面，若編碼器脈衝之計數數與對應於步進角之數不一致， 則步進馬達310之旋轉與脈衝信號不同步、即產生失調。當步進馬達310產生失調時，失調判定部216產生失調偵測信號。
動作控制裝置100具有系統控制部101、操作部介面102、指令值輸出部103、當前位置．姿勢計算部104、軌道計算部105、指令值計算部106、動態計算部107、驅動電流決定部108、及驅動器單元介面109。於動作控制裝置100，自驅動器單元210經由驅動器單元介面109而輸入有與計數器217計數之計數數(表示自基準位置之旋轉角)及重設次數(表示旋轉次數)相關之資料以及自失調判定部216輸出的失調偵測信號。本實施形態之機器人裝置之動作控制裝置100可作為独立裝置而應用於使用步進馬達之其它裝置、機構。
(系統控制部)
系統控制部101具有CPU(Central Processing Unit)及半導體記憶體等，統括地控制動作控制裝置100。於系統控制部101經由控制/資料匯流排110而連接有各部。
(操作部)
於動作控制裝置100經由操作部介面102而連接有操作部50。操作部50係作為讓使用者輸入腕部4或機器人手(末端效應器)之關注點之位置之變更、姿勢之變更及時間變更之輸入介面而發揮功能。動作控制裝置100係將 例如2指手之末端作為關注點(控制點)而執行移動．姿勢變更等之計算處理。例如，操作部50具備用於使機器人手移動之指定最終目標位置及移動時間之操縱桿等。例如，基於操縱桿之操作方向、操縱桿之傾斜角度、操縱桿之操作加速度，而輸入機器人手之最終目標位置及移動時間。另，構成操作部50之該等輸入裝置可由其它裝置、例如滑鼠、鍵盤、軌跡球及觸控面板等代替。
(指令值輸出部)
指令值輸出部103依照系統控制部101之控制，將由後述之指令值計算部106計算之關節部J1-J6各者之指令值(△t後之關節角度(關節變量))，分別輸出至驅動器單元210、220、230、240、250、260。具體而言，指令值輸出部103將與由後述之指令值計算部106計算之關節部J1-J6各者之關節變量相應的位置指令信號，輸出至驅動器單元210、220、230、240、250、260。又，指令值輸出部103將與由後述之驅動電流決定部108決定之驅動關節部J1-J6各者之驅動電流值相應的驅動電流指令信號，輸出至驅動器單元210、220、230、240、250、260。
(當前位置．姿勢計算部)
當前位置．姿勢計算部104基於關節部J1、J2、J3、J4、J5、J6各者之關節變量，依照根據臂構造之鏈路參數 規定之同次變換矩陣，利用順運動學計算自機器人座標系觀察之末端關注點之位置及姿勢。另，所謂關節變量，對於關節部J1、J2、J4、J5、J6而言係指自基準位置之正負之旋轉角度，若為關節部J3則係指自最收縮狀態之伸展距離(直動變位)。
當前位置．姿勢計算部104基於由驅動器單元210、220、230、240、250、260各者之計數器217計數之計數數，計算與關節部J1-J6相關之關節變量向量-θ。關節變量向量-θ係由旋轉關節部J1、J2、J4、J5、J6之關節角度θJ1、θJ2、θJ4、θJ5、θJ6及直動伸縮關節部J3之伸縮長LJ3而提供之關節變量之組(θJ1、θJ2、LJ3、θJ4、θJ5、θJ6)。例如，當前位置．姿勢計算部104保持於關節部J1之步進馬達310之啟動前為止之累積計數數，並將啟動中計數之計數數加至累積計數數，藉此更新累積計數數。當前位置．姿勢計算部104對累積計數數乘以與1計數對應之步進角度，藉此計算步進馬達310之自基準位置之旋轉角度，從而特定關節部J1之關節變量。當前位置．姿勢計算部104藉由相同之手法特定其它關節部J2-J6之關節變量。
當前位置．姿勢計算部104藉由同次變換矩陣K(參數(θJ1、θJ2、LJ3、θJ4、θJ5、θJ6))，計算自機器人座標系Σ b觀察之末端關注點之位置(x、y、z)及末端姿勢(α、β、γ)。同次變換矩陣K係對末端座標系Σ h與機器人座標系Σ b之關係進行定義之矩陣式。同次 變換矩陣K係由構成機械臂機構之鏈路間之關係(鏈路長與鏈路之扭轉角)及關節部之軸間之關係(鏈路間距離與鏈路之間角度)而決定。例如，當前位置．姿勢計算部104將當前之機械臂機構之關節變量向量-θ0(θ0-J1、θ0-J2、L0-J3、θ0-J4、θ0-J5、θ0-J6)代入同次變換矩陣K，而計算自機器人座標系Σ b觀察之末端之當前位置P0(x0、y0、z0)及末端姿勢φ0(α0、β0、γ0)。
(軌道計算部)
軌道計算部105基於末端之當前位置．姿勢及末端之最終目標位置．姿勢，計算連結其等之間之每隔單位時間△t(控制周期，例如10ms)之末端之目標位置之點列。末端之當前位置．姿勢係自當前位置．姿勢計算部104之計算處理而獲得。末端之最終目標位置．姿勢與移動時間係經由例如操作部50而由使用者輸入。軌道計算部105向事先預設之以末端之當前位置及當前姿勢、末端之最終目標位置及最終目標姿勢為參數的函數代入各參數，藉此計算末端之軌道(以下稱為末端軌道)，並計算此末端軌道上每隔單位時間△t之目標位置。作為軌道計算方法可採用任意之方法。單位時間△t係作為控制周期而為固定值，由使用者設定為例如10ms。軌道計算部105基於自末端之當前位置移動至最終目標位置所需之移動時間寬度T及單位時間△t，決定目標位置之數m(=T/△t)，並計算末端軌道上每隔單位時間△t之目標位置(p1、 p2．．．pm(m=T/△t))。此處，目標位置係提供末端之位置及姿勢之兩者之參數。
詳細說明於後文敘述，軌道計算部105於偵測到失調時，可基於系統控制部101之控制而再次計算5種軌道。第1-第3軌道係自因失調而自本來之軌道移位之手之關注點之位置(重新開始移動位置)經過不同路徑而到達最終目標位置的軌道。第4、第5軌道係自重新開始移動位置經過不同路徑而返回至移動開始時之位置的軌道。當偵測到失調時，自該等5個軌道中選擇一個軌道，並沿著此軌道進行移動控制。
另，於軌道計算部105，軌道計算方法為任意，且亦可事先預設複數個函數。例如，軌道計算部105依照系統控制部101之控制，變更用於末端軌道計算之函數。例如，軌道計算部105使用與通常之末端軌道之計算用函數不同的函數，再次計算失調偵測後之末端軌道。例如，失調偵測後再次計算之末端軌道係等價於將失調偵測後之位置P0'與最終目標位置P1連結之最短路徑。又，失調偵測後再次計算之末端軌道具有自失調偵測後之位置朝向失調偵測時之位置將末端拉回特定距離之軌道。
(指令值計算部)
指令值計算部106計算與所計算之軌道上以△t之周期排列之目標位置分別對應的複數個關節變量向量。另，所謂關節變量向量係指關節部J1-J6之6個關節變量、 即旋轉關節部J1、J2、J4-J6之旋轉角與直動伸縮關節部J3之臂伸縮長之6個變量。關於指令值計算部106之計算處理於後文進行敘述。
(動態計算部)
動態計算部107計算使末端自目標位置移動至△t後之下一目標位置所需之分別設置於關節部J1-J6之步進馬達310~360各者之扭矩(驅動扭矩)。又，動態計算部107計算為了使臂部2靜止而各關節部所需之扭矩(靜止扭矩)。各關節部所需之靜止扭矩係對根據較此關節部更前方之部分之重心質量、此關節部之旋轉軸至重心之距離等所計算的該關節部之負荷扭矩，逆向施加等價扭矩、即自重所致之負荷扭矩以實現均衡之扭矩。
步進馬達310~360各者之必要扭矩係使用與關節部J1-J6相關之動態模型資料，連結機械臂機構之關節部J1-J6之各鏈路之重心位置、重心質量、鏈路長、慣性張量、關節部之關節變量(旋轉角位置或直動變位)、關節角速度、關節角加速度等而計算。另，動態模型係指與馬達之動態特性相關之模型。
(驅動電流決定部)
驅動電流決定部108係為了產生動態計算部107所計算之扭矩而決定步進馬達310、320、330、340、350、360各者之驅動電流值。驅動電流決定部108保持將步進 馬達310、320、330、340、350、360各者之、扭矩大小與驅動電流值關聯而成之對應表之資料。驅動電流決定部108參照對應表，決定與動態計算部107所計算之關節部J1-J6各者之扭矩之大小分別對應的驅動電流值。
(失調時之應對次序)圖5、圖6
以下，參照圖5，對本實施形態之機器人裝置之動作控制裝置100之失調時之應對次序進行說明，並參照圖6進行補充說明。圖5係用於說明圖4之動作控制裝置100之失調時之處理次序的流程圖。圖6係表示第1-第5軌道(1)-(5)之圖。動作控制裝置100之各部係依照系統控制部101之控制而按照圖5之流程圖動作。
(步驟S11)最終目標位置等之輸入
於末端移動時首先經由操作部介面102，輸入末端關注點之最終目標位置。藉由步驟S11，如圖6所示，輸入最終目標位置P1。又，於失調產生後判定是否等待操作者之手動指示而重新開始移動(手動重新開始之接通/斷開)，於手動重新開始設定為斷開時進一步設定至失調產生後重新開始移動為止之待機時間tw。另，待機時間tw可設定為0以上。將待機時間tw設定為0小時，當然意味著允許立即重新開始移動。又，失調產生後返回至移動開始時之位置(移動開始位置)P0時，設定是否由操作者手動選擇返回路徑不同之軌道(4)、(5)之任一者。
(步驟S12)末端軌道之計算處理
自末端關注點之當前位置(移動開始位置)、至步驟S11中輸入之最終目標位置之末端軌道係藉由軌道計算部105計算。藉由軌道計算部105，於末端關注點之當前位置至最終目標位置為止之末端軌道上設定每隔單位時間△t之複數個目標位置。於圖6之例中，計算將當前位置P0與最終目標位置P1連結之末端軌道，並於此末端軌道上設定每隔單位時間△t之複數個目標位置(p1、p2、．．．pm-1、pm(=P1))。末端關注點之當前位置P0係自當前位置．姿勢計算部104之計算處理而獲得。具體而言，藉由當前位置．姿勢計算部104，基於由驅動器單元210、220、230、240、250、260各者之計數器217計數之計數數，計算當前之關節變量向量-θ0(θ0-J1、θ0-J2、L0-J3、θ0-J4、θ0-J5、θ0-J6)，並基於此此關節變量向量-θ0，利用順運動學計算末端之當前位置P0。
(步驟S13)關節部J1-J6之指令值(關節變量向量)之計算處理
與步驟S12所計算之複數個目標位置分別對應的各關節之角度、表示伸展距離之複數個關節變量向量係藉由指令值計算部106計算。藉由步驟S13，如圖6所示，計算與複數個目標位置(p1、p2、．．．pm-1、pm(=P1))分別對應的複數個關節變量向量(-θ1、-θ2、．．．-θm)。關於指令值計算部106之計算處理次序於後文詳細敘述。
(步驟S14)指令值之輸出處理
與步驟S13所計算之關節變量向量對應之位置指令信號、及與驅動電流決定部108決定之驅動電流值對應的驅動電流指令信號，係自指令值輸出部103以特定之控制周期△t(例如，10ms)依次輸出至驅動器單元210、220、230、240、250、260。藉由步驟S14，末端自當前位置P0每隔控制周期△t依次通過目標位置p1、p2、．．．pm-1，移動至最終目標位置P1。
(步驟S15)末端是否到達最終目標位置之判定處理
藉由系統控制部101判定末端是否到達最終目標位置。具體而言，藉由當前位置．姿勢計算部104，將每隔控制周期△t計算之末端之當前位置與圖6所示之最終目標位置P1進行比較。於末端之當前位置與最終目標位置一致時，判定末端到達最終目標位置。此時，動作控制裝置100之一系列控制完成。另一方面，於末端之當前位置與末端之最終目標位置不一致時，使處理移行至步驟S16。
(步驟S16)失調產生之有無之判定處理
藉由系統控制部101判定失調產生之有無。具體而言，自失調判定部216向動作控制裝置100輸入有失調偵測信號時，使處理移行至步驟S17。於圖6中，將失調偵測時點之末端之位置設為Pso，因某種外因、例如作業者觸碰機械臂機構之臂部2，而末端移動至位置P0'。另一方面，若未自失調判定部216輸出失調偵測信號，則於末 端之當前位置到達末端之最終目標位置之前，以特定之控制周期△t依次輸出位置指令信號及驅動電流指令信號。
(步驟S17)失調產生時之末端軌道之再次計算處理
於步驟S17中，系統控制部101基於因失調而移位之距離△d、即當初之軌道上之失調偵測時點之末端之位置Pso與失調產生後移位之末端位置P0'之距離(移位距離)△d、及失調偵測至失調消除而能夠重新開始移動為止之時間之經過時間(n．△t1)，決定失調偵測後重新開始末端移動時維持當初之最終目標位置P1作為到達位置、或者自當初之最終目標位置P1變更為移動開始時之位置P0。判定是否滿足失調所致之移位距離△d未達特定之閾值THd、且失調偵測至重新開始移動之經過時間(n．△t1)未達特定之閾值THt之條件。以下，將該經過時間(n．△t1)稱為失調所致之停工時間。另，失調偵測後，動作控制部100重複輸出用於控制驅動器單元210而重新開始移動之指令值，重試重新開始移動。將該重複周期(重試周期)設為△t1、將重複次數(重試次數)設為n。來自脈衝信號產生部213之脈衝信號數與編碼脈衝數匹配、即脈衝信號與旋轉同步之狀態係失調消除後之狀態，此狀態下可重新開始移動。重複進行重新開始移動之重試控制，直至達成該狀態為止。
於步驟S17中，於末端位置不會因失調而自本来之軌道大幅偏移(移位距離△d相對較短)、進而失調產生至消除為止不會經過長時間(停工時間n．△t1相對較短) 時，重新開始移動並指向當初之最終目標位置，另一方面，於末端位置因失調而自本来之軌道大幅偏移(移位距離△d相對較長)及/或失調產生至消除為止經過長時間(停工時間n．△t1相對較長)時，解除當初之最終目標位置，返回至移動開始位置，等待下一任務。
另，為了判定移位距離△d之長短而與移位距離△d比較之閾值THd、為了判定停工時間(n．△t1)之長短而與停工時間(n．△t1)比較之閾值THt係藉由系統控制部101而動態地變動。例如，當失調偵測時之位置Pso較最終目標位置P1而靠近移動開始位置P0時設定為相對較小之值，當失調偵測時之位置Pso較移動開始位置P0靠近最終目標位置P1時設定為相對較大之值。
於步驟S17中判定為“YES”時，即重新開始移動而指向當初之最終目標位置時，於步驟S18中，系統控制部101等待重新開始移動，直至輸入有操作者之重新開始移動指示。或者，於步驟S11中將操作者之重新開始移動之手動指示設定為斷開時，自失調偵測時點開始等待步驟S11中設定之待機時間tw之經過。該等係為了確保重新開始移動時之安全性而設定。
於步驟S18中藉由操作者輸入有重新開始移動指示時，或經過待機時間tw時，藉由步驟S19-S21依照選擇之軌道進行指令值輸出，重新開始移動。實際上，軌道再計算步驟S19及軌道選擇步驟S20係與步驟S18同時執行。於步驟S19中，圖6所示之3種軌道(1)-(3)係 藉由軌道計算部105計算。
第1軌道(1)係如下軌道：自因失調而移位之手之關注點之位置(重新開始移動時之位置)P0’，經過因失調而自移動開始位置P0至最終目標位置P1之當初之軌道偏移之路徑後，返回當初之軌道上之失調偵測位置Pso，並沿著當初之軌道到達最終目標位置。
第2軌道(2)係如下軌道：自因失調而移位之手之關注點之位置(重新開始移動時之位置)P0’，移動至失調偵測時之位置Pso與最終目標位置P1之間之位置，並沿著當初之軌道到達最終目標位置。第3軌道(3)係自因失調而移位之關注點之位置P0’至最終目標位置P1為止之直線的最短軌道。
其次，於步驟S20中，自該等3種軌道(1)-(3)中藉由軌道計算部105或系統控制部101選擇一個軌道。此處，自安全性之観點出發，分別將第1軌道(1)設定為第1位之優先順位，將第2軌道(2)設定為第2位之優先順位，將第3軌道(3)設定為第3位之優先順位。作為選擇條件，係自當初之移動開始位置P0至最終目標位置P1之移動中本来設定的任務時間，是否能夠維持在未達對末端關注點之移動速度規定之速度上限值之範圍內。若假定軌道長最、安全性最高之第1軌道(1)滿足該選擇條件，則選擇第1軌道(1)。若第1軌道(1)不滿足該選擇條件，假定軌道長第二長、安全性第二高之第2軌道(2)滿足該選擇條件，則選擇第2軌道(2)。當 第1、第2軌道(1)、(2)均不滿足該選擇條件，至最終目標位置P1之路徑最短之第3軌道(3)滿足該選擇條件時，選擇第3軌道(3)。另，第3軌道(3)不滿足該選擇條件時之動作係任意地設定，典型而言，可於此重新開始移動位置P0’停住，或者亦可選擇第4軌道(4)，亦返回至移動開始位置P0而等待下一任務。
於步驟21中，藉由指令值計算部106計算表示與步驟S20中選擇之軌道上之複數個目標位置分別對應的複數個關節變量向量的指令值，並以特定之控制周期△t之周期，依序與表示驅動電流決定部108決定之各位置之驅動電流值的指令信號，一併自指令值輸出部103輸出至驅動器單元210、220、230、240、250、260。
當步驟S17中判定為“NO”時，即重新開始移動沿著返回軌道(4)、(5)之任一者返回至原本的移動開始位置P0時，於步驟S22中，系統控制部101等待重新開始移動，直至輸入有操作者之重新開始移動指示，或者自失調偵測時點等待步驟S11中設定之待機時間tw之經過。
當步驟S22中操作者輸入有重新開始移動指示時，或經過待機時間tw時，於步驟S23中，將步驟S11中設定之「返回至移動開始位置P0之返回軌道(4)、(5)之手動選擇功能」設定為斷開時，於步驟S24中，藉由軌道計算部105計算自因失調而移位之手之關注點之位置(重新開始移動時之位置)P0’返回至原本的移動開始位置P0 之返回軌道(4)。
返回軌道(4)係如下軌道：自重新開始移動時之位置P0’，經過因失調而自移動開始位置P0至最終目標位置P1之當初之軌道偏移之路徑，返回至失調偵測位置Pso，沿著當初之軌道到達原本的移動開始位置P0。另一方面，第5軌道(5)係自因失調而移位之關注點之位置(重新開始移動位置)P0’至原本的移動開始位置P0之直線的最短返回軌道。返回軌道(4)係之前剛通過之路程，故而相較於新的第5軌道(5)，軌道上介置障礙物等之可能性較低，即安全性較高。因此，操作者未手動進行軌道選擇時，相比返回軌道(5)優先選擇返回軌道(4)。
於步驟25中，藉由指令值計算部106計算表示與步驟S24所計算之軌道(4)上之複數個目標位置分別對應的複數個關節變量向量的指令值，並以特定之控制周期△t之周期，依序與表示驅動電流決定部108決定之各位置之驅動電流值的指令信號，一併自指令值輸出部103輸出至驅動器單元210、220、230、240、250、260。
於步驟S23中，將步驟S11中設定之「返回至移動開始位置P0之返回軌道(4)、(5)之手動選擇功能」設定為接通時，於步驟S26中，藉由系統控制部101而於操作部50之顯示器上顯示畫面，用於提醒操作者選擇返回軌道(4)與第5軌道(5)之任一者。當操作者選擇返回軌道(4)時，移行至步驟S24。當操作者選擇返回軌道 (5)時，於步驟S28中藉由軌道計算部105計算自因失調而移位之手之關注點之位置(重新開始移動時之位置)P0’返回至原本的移動開始位置P0的直線且最短的返回軌道(5)，並移行至步驟S25。
上述步驟S14-S28重複進行，直至手5之末端關注點經由步驟S15而到達最終目標位置P1、或返回至移動開始位置P0。
如此，機械臂之致動器採用步進馬達因其失調而停止動作，故而難以採用，但如上述般當產生失調時可再次計算此時之位置至最終目標位置為止之軌道，並重新開始沿著此再次計算之軌道之末端移動，且於無法維持任務時間時，可重新開始返回至原本的移動開始位置之末端移動而等待下一任務指令。該等作用效果可促進對機械臂之致動器採用步進馬達。
採用本實施形態之失調時重新開始移動之控制，意味著將能夠活用作為步進馬達固有之特性的失調以提高安全性之新穎構思得以實際地實用化。先前當作業員接觸臂時，需要藉由加速度感測器等偵測此接觸而停止動作等之安全機構。但，當作業員接觸採用步進馬達之機械臂時，因其負荷產生失調而動作停止。因此，能夠確保與先前相同之安全性。
(指令值計算部之處理之說明)圖7
其次，參照圖7對步驟S21、S24之指令值計算部 106之指令值計算處理進行說明。
(步驟S131)初始化(n=0)
此處，為便於說明，使用變量n。首先，將該變量n初始化為零值。
(步驟S132)雅可比逆矩陣J-1(-θn)之計算
本實施形態之機器人裝置之機械臂機構其構造上不存在臨界點，因此始終存在雅可比逆矩陣。雅可比逆矩陣係將末端速度(末端位置．姿勢之微小變化)變換成關節角速度(關節角度．伸縮長之微小變化)的矩陣。雅可比逆矩陣係藉由對表示末端之位置．末端姿勢之向量之關節變量進行偏微分而提供。指令值計算部106藉由步驟S12中當前位置．姿勢計算部104計算之當前之關節變量向量-θn(θn-J1、θn-J2、Ln-J3、θn-J4、θn-J5、θn-J6)，利用臂構造之鏈路參數計算雅可比逆矩陣J-1(-θn)。
(步驟S133)末端速度-pn+1之計算
基於當前之末端位置(當前之目標位置)pn、下一末端位置(單位時間△t後之下一目標位置)pn+1、單位時間△t，計算末端速度．pn+1。
(步驟S134)關節角速度-．θn+1之計算
藉由雅可比逆矩陣J-1(-θn)，將於步驟S133所計算之末端速度-pn+1變換成關節角速度-．θn+1。
(步驟S135)目標之關節變量向量-θn+1之計算
基於重複次數前之步驟S135所計算之(變量n=0之情形時為步驟S11所計算之)關節變量向量-θn、步驟 S134所計算之關節角速度-．θn+1、單位時間△t，計算下一關節變量向量-θn+1。藉由於關節角速度-．θn+1乘以單位時間△t，計算單位時間△t之間之各關節之變位量。藉由在即將移動前之關節變量向量-θn加上關節之變位量，而計算經過單位時間△t後之關節變量向量-θn+1。
(步驟S136)處理繼續之判定
當變量n為重複次數(m-1)時，結束指令值計算部106之計算處理。另一方面，當變量(n)並非重複次數(m-1)之情形時，將處理移行至步驟S137。
(步驟S137)變量之提升n←n+1
將變量n提升為(n+1)，使處理返回至步驟S142。
藉由以上說明之指令值計算部106之計算處理，而計算與末端之目標位置(p1、p2、．．．pm)分別對應的關節變量向量(-θ1、-θ2、．．．-θm)。
(效果)
如以上所說明般，於本實施形態之機器人裝置中，於構成機械臂機構之關節部J1-J6分別具備步進馬達310~360。該等步進馬達310~360之動作係由動作控制裝置100控制。步進馬達係若對步進馬達施加大於步進馬達之扭矩之負荷便會產生失調，故而一般而言並不用於產業用機器人。但，本實施形態之機器人裝置於構造上並無臨界 點，因此臂不會於未預期之時序為了避免臨界點而進行較大之迴旋移動等，故而可作為例如與作業者協動而進行作業之機器人裝置使用。即，本實施形態之機器人裝置可接近作業者而配置。因此，藉由使用步進馬達作為關節部之致動器，即便於作業者接觸機器人裝置之臂之情形時，若對步進馬達施加步進馬達之必要扭矩以上之負荷，便會產生失調，故而臂不會以步進馬達之必要扭矩以上之較大力傷害作業者。因此，藉由對關節部之致動器使用步進馬達，與使用其他馬達、例如AC馬達等之情形時，可減小臂接觸作業者引起之作業者之受傷等風險。
如此，於本實施形態之機器人裝置之設想配置環境中，預先設想了作業者接觸臂而步進馬達失調。於此種情形時，有失調後之位置自預定之末端軌道上偏移之情形。但，本實施形態之機器人裝置之動作控制裝置100之軌道計算部105於步進馬達310~360產生失調時，可再次計算自失調偵測後之末端之當前位置至末端之最終目標位置為止之末端軌道。因此，即便於產生失調之情形時，亦可繼續進行末端之移動控制，使末端自失調偵測後之位置再次移動至最終目標位置。藉此，即便於產生失調之情形時，亦不會中斷末端之移動控制，且無需使末端暫時返回至基準位置，故而可縮短停工時間。因此，本實施形態之機器人裝置以步進馬達產生失調為前提可提供產生失調時之較佳應對。
本實施形態中，係以機器人裝置為例進行步進馬達控 制之說明，但亦可對其他作動對象、例如輸送機裝置之線移動之致動器採用步進馬達，亦可應用於此步進馬達控制。計算作為作動對象之線自當前狀態(當前速度)變化(速度變化)至目標狀態(目標速度)之過程，並依照此過程輸出指令值。此過程中步進馬達產生失調時，再次計算產生失調時點之線之速度至目標速度之過程，並依照再次計算之過程使線之速度階段性變化。此種應用亦包含於本實施形態之思想範圍內。
雖對本發明之若干實施形態進行了說明，但該等實施形態係作為示例而提示者，並不意圖限定發明之範圍。該等實施形態能以其它各種形態實施，且於不脫離發明主旨之範圍內，可進行各種省略、置換、變更。該等實施形態及其變形包含於發明之範圍及主旨，且同樣包含於申請專利範圍所記載之發明及其均等範圍內。
50‧‧‧操作部
100‧‧‧動作控制裝置
101‧‧‧系統控制部
102‧‧‧操作部I/F
103‧‧‧指令值輸出部
104‧‧‧當前位置．姿勢計算部
105‧‧‧軌道計算部
106‧‧‧指令值計算部
107‧‧‧動態計算部
108‧‧‧驅動電流決定部
109‧‧‧驅動器單元I/F
110‧‧‧控制/資料匯流排
210、220、260‧‧‧驅動器單元
310‧‧‧步進馬達
211‧‧‧控制部
212‧‧‧電源電路
213‧‧‧脈衝信號產生部
215‧‧‧編碼器
216‧‧‧失調判定部
217‧‧‧計數器

Claims (13)

  1. 一種機器人裝置，其特徵在於具備：機械臂機構，其具有關節部；步進馬達，其產生使上述關節部作動之動力；馬達驅動器，其驅動上述步進馬達；軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，再次計算自因上述失調而移位之上述關注點之位置至上述最終目標位置之軌道，並控制上述軌道計算部及上述指令值輸出部，使上述關注點依照上述再次計算之軌道移動。
  2. 如申請專利範圍1之機器人裝置，其中藉由上述軌道計算部，再次計算第1軌道及第2軌道，上述第1軌道係自因上述失調而移位之上述關注點之位置，返回至上述移動開始位置與上述最終目標位置之當初之軌道，並到達上述最終目標位置，上述第2軌道係自因上述失調而移位之上述關注點之位置至上述最終目標位置之直線軌道。
  3. 如申請專利範圍2之機器人裝置，其中上述系統控制部基於維持根據上述當初之軌道所預定之任務時間所必需的上述關注點之移動速度，而選擇上述第1、第2軌道之一者。
  4. 如申請專利範圍3之機器人裝置，其中上述系統控制部於上述第1軌道預定之上述關注點之移動速度未達特定之上限值時優先選擇上述第1軌道，於上述第1軌道預定之上述關注點之移動速度為特定之上限值以上時選擇上述第2軌道。
  5. 如申請專利範圍4之機器人裝置，其中上述系統控制部於自偵測到上述失調之時點經過特定之等待時間之後，為了依照上述第1軌道或上述第2軌道重新開始上述關注點之移動，而控制上述指令值輸出部。
  6. 如申請專利範圍4之機器人裝置，其中上述系統控制部於偵測到上述失調後等待使用者之重新開始移動指示，為了依照上述第1軌道或上述第2軌道重新開始上述關注點之移動，而控制上述指令值輸出部。
  7. 如申請專利範圍1之機器人裝置，其中藉由上述軌道計算部再次計算第1軌道、第2軌道及第3軌道，上述第1軌道係自因上述失調而移位之上述關注點之位置，經由因上述失調而自上述移動開始位置至上述最終目標位置之當初之軌道偏移的路徑，返回至上述當初之軌道，並到達上述最終目標位置，上述第2軌道係自因上述失調而移位之上述關注點之位置經過與因上述失調而自上述當初之軌道偏移之路徑不同的路徑，返回至上述當初之軌道，並到達上述最終目標位置，上述第3軌道係自因上述失調而移位之上述關注點之位置至上述最終目標位置之直線軌道。
  8. 一種機器人裝置，其特徵在於具備：機械臂機構，其具有關節部；步進馬達，其產生使上述關節部作動之動力；馬達驅動器，其驅動上述步進馬達；軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，計算自因上述失調而移位之上述關注點之位置返回至上述移動開始位置的軌道，為了依照上述計算之軌道使上述關注點返回至上述移動開始位置而控制上述軌道計算部及上述指令值輸出部。
  9. 一種機器人裝置，其特徵在於具備：機械臂機構，其具有關節部；步進馬達，其產生使上述關節部作動之動力；馬達驅動器，其驅動上述步進馬達；軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，當自上述軌道 起因上述失調而移位之上述關注點之移位距離為特定距離以下、且自偵測到上述失調之時點至上述失調恢復之時點的延遲時間為特定時間以下時，使上述關注點沿著自因上述失調而移位之上述關注點之位置至上述最終目標位置之軌道移動，另一方面，當上述移位距離並非上述特定距離以下、或上述延遲時間並非上述特定時間以下時，為了使上述關注點自因上述失調而移位之上述關注點之位置返回至上述移動開始位置，控制上述軌道計算部及上述指令值輸出部。
  10. 一種步進馬達控制裝置，其係對產生使裝備於機械臂機構之關節部作動之動力之步進馬達進行控制者，其特徵在於具備：軌道計算部，其計算上述機械臂機構之關注點自當前位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，再次計算自因上述失調而移位之上述關注點之位置至上述最終目標位置之軌道，為了依照上述再次計算之軌道使上述關注點移動，控制上述軌道計算部及上述指令值輸出部。
  11. 一種步進馬達控制裝置，其係對產生使裝備於機械臂機構之關節部作動之動力之步進馬達進行控制者，其特徵在於具備： 軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，計算自因上述失調而移位之上述關注點之位置返回至上述移動開始位置的軌道，為了依照上述計算之軌道使上述關注點返回至上述移動開始位置，控制上述軌道計算部及上述指令值輸出部。
  12. 一種步進馬達控制裝置，其係對產生使裝備於機械臂機構之關節部作動之動力的步進馬達進行控制者，其特徵在於具備：軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，計算自因上述失調而移位之上述關注點之位置返回至上述移動開始位置之軌道，為了依照上述計算之軌道使上述關注點返回至上述移動開始位置，控制上述軌道計算部及上述指令值輸出部。
  13. 一種步進馬達控制裝置，其係對產生使裝備於機 械臂機構之關節部作動之動力的步進馬達進行控制者，其特徵在於具備：軌道計算部，其計算上述機械臂機構之關注點自作為當前位置之移動開始位置移動至最終目標位置之軌道；指令值輸出部，其向上述馬達驅動器輸出與上述軌道計算部所計算之軌道相應的指令值；失調偵測部，其偵測上述步進馬達之失調；以及系統控制部，其於偵測到上述失調時，當自上述軌道起因上述失調而移位之上述關注點之移位距離為特定距離以下、且自偵測到上述失調之時點至上述失調恢復之時點之延遲時間為特定時間以下時，使上述關注點沿著自因上述失調而移位之上述關注點之位置至上述最終目標位置之軌道移動，另一方面，當上述移位距離並非上述特定距離以下、或上述延遲時間並非上述特定時間以下時，為了使上述關注點自因上述失調而移位的上述關注點之位置返回至上述移動開始位置，控制上述軌道計算部及上述指令值輸出部。
TW105115564A 2015-05-28 2016-05-19 機器人裝置及步進馬達控制裝置 TW201702030A (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2015108688A JP6659238B2 (ja) 2015-05-28 2015-05-28 ロボット装置及びステッピングモータ制御装置

Publications (1)

Publication Number Publication Date
TW201702030A true TW201702030A (zh) 2017-01-16

Family

ID=57393896

Family Applications (1)

Application Number Title Priority Date Filing Date
TW105115564A TW201702030A (zh) 2015-05-28 2016-05-19 機器人裝置及步進馬達控制裝置

Country Status (6)

Country Link
US (1) US20180079077A1 (zh)
EP (1) EP3305478A4 (zh)
JP (1) JP6659238B2 (zh)
CN (1) CN107614209A (zh)
TW (1) TW201702030A (zh)
WO (1) WO2016190140A1 (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6468804B2 (ja) * 2014-10-30 2019-02-13 ライフロボティクス株式会社 ロボットアーム機構
JP2018051635A (ja) * 2016-09-26 2018-04-05 セイコーエプソン株式会社 ロボット制御装置、ロボットおよびロボットシステム
JP7007839B2 (ja) * 2017-08-31 2022-01-25 川崎重工業株式会社 多関節ロボット
JP6846717B2 (ja) * 2018-02-14 2021-03-24 ファナック株式会社 ロボットのジョイントの異常検出装置及び異常検出方法
JP7310963B2 (ja) * 2019-03-22 2023-07-19 オムロン株式会社 ロボットハンド、ロボットハンド制御方法及びプログラム
CN109861599B (zh) * 2019-03-25 2021-11-12 北京美联泰科生物技术有限公司 电机控制方法
JP7393178B2 (ja) * 2019-10-09 2023-12-06 川崎重工業株式会社 制御方法、制御装置、ロボットシステム及びプログラム
US11919169B2 (en) * 2019-11-19 2024-03-05 Google Llc Optimization of motion paths of a robot using vision data
CN113315425B (zh) * 2021-07-28 2021-11-30 深圳市永达电子信息股份有限公司 步进电机定位方法、系统、设备及存储介质
TWI766807B (zh) * 2021-09-17 2022-06-01 財團法人精密機械研究發展中心 機械手臂的安全復位方法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63206197A (ja) * 1987-02-20 1988-08-25 Jeol Ltd ア−ム移動を司どるモ−タの制御機構
FR2671305B1 (fr) * 1991-01-09 1995-04-07 Commissariat Energie Atomique Procede de pilotage d'un bras de robot en definissant des trajets de substitution.
US5426722A (en) * 1993-09-09 1995-06-20 Stratasys, Inc. Method for optimizing the motion of a multi-axis robot
US6212968B1 (en) * 1998-07-29 2001-04-10 Janome Sewing Machine Co., Ltd, SCARA robot
JP2000047731A (ja) * 1998-07-29 2000-02-18 Janome Sewing Mach Co Ltd スカラロボット
US6278907B1 (en) * 1999-11-24 2001-08-21 Xerox Corporation Apparatus and method of distributing object handling
US6871115B2 (en) * 2002-10-11 2005-03-22 Taiwan Semiconductor Manufacturing Co., Ltd Method and apparatus for monitoring the operation of a wafer handling robot
WO2006102517A2 (en) * 2005-03-23 2006-09-28 Hurco Companies, Inc. Method of tolerance-based trajectory planning and control
US7706920B2 (en) * 2007-09-19 2010-04-27 Disney Enterprises, Inc. Smooth clipping of data streams
KR101732902B1 (ko) * 2010-12-27 2017-05-24 삼성전자주식회사 로봇의 경로 계획 장치 및 그 방법
US8810187B2 (en) * 2011-08-19 2014-08-19 Oriental Motor Boston Technology Group Incorporated Method and apparatus for misstep detection and recovery in a stepper motor
JP2013132696A (ja) * 2011-12-26 2013-07-08 Seiko Epson Corp アーム制御装置、および、アーム制御方法
JP6399591B2 (ja) * 2014-10-31 2018-10-03 ライフロボティクス株式会社 ロボットアーム機構及びステッピングモータ制御装置

Also Published As

Publication number Publication date
EP3305478A1 (en) 2018-04-11
US20180079077A1 (en) 2018-03-22
CN107614209A (zh) 2018-01-19
EP3305478A4 (en) 2019-01-23
WO2016190140A1 (ja) 2016-12-01
JP6659238B2 (ja) 2020-03-04
JP2016221606A (ja) 2016-12-28

Similar Documents

Publication Publication Date Title
TW201702030A (zh) 機器人裝置及步進馬達控制裝置
WO2016204099A1 (ja) ロボット装置及びモータ制御装置
JP6595206B2 (ja) 多関節アーム機構の動作制御装置及びロボット装置
CN106863266B (zh) 机器人、控制装置以及机器人系统
JP6399591B2 (ja) ロボットアーム機構及びステッピングモータ制御装置
TWI682839B (zh) 多關節機械手臂及多關節機械手臂的控制方法
TWI675728B (zh) 機器人及機器人系統
WO2016208431A1 (ja) ロボット装置
JPWO2017043583A1 (ja) ロボット装置
TW201644182A (zh) 機器人裝置及步進馬達控制裝置
JP2010110878A (ja) 多関節型ロボット装置およびその制御方法
WO2017022533A1 (ja) ロボット装置
JP6300693B2 (ja) ロボット
CN111225772A (zh) 垂直多关节机器人的示教数据创建系统
JP2017052072A (ja) ロボット装置
JP7178994B2 (ja) 把持システム
JP4970492B2 (ja) 多関節ロボット
JP6648491B2 (ja) 制御装置、ロボットおよびロボットシステム
JP6252272B2 (ja) 垂直多関節型ロボットの位置誤差抑制方法
JP6439375B2 (ja) ロボット及び制御装置
JP2015000438A (ja) ロボット、位置姿勢補正装置および位置姿勢補正方法
JP2016097475A (ja) ロボット及びロボットシステム
JP2012228760A (ja) ロボット