TWI742635B - 教導位置與姿態的觸發與補償方法 - Google Patents

Abstract

本發明提供一種教導位置與姿態的觸發與補償方法。於姿態維持模式下，自動化機械系統每隔一週期計算並執行新的姿態維持命令以使其機械設備維持在第一姿態，並於新的姿態命令與第一姿態的參考命令具有力矩差異時，判定機械設備接受外力，決定外力的方向並切換至動力維持模式。於動力維持模式下，自動化機械系統依據補償力及各馬達的當前動力計算動力維持命令並經由馬達驅動器執行動力維持命令以減輕朝外力的方向調整機械設備為第二姿態的阻力。本發明可供用戶以便捷且省力的方式來對機械設備的位置姿態進行教導。

Description

教導位置與姿態的觸發與補償方法

本發明係與有關機械設備的姿態教導，特別有關於教導位置與姿態的觸發與補償方法。

於現有技術中，當要對機械設備(如多軸機構或機械手臂)的位置與姿態進行教導時，通常是由用戶操作具有多個操作按鈕的教導盒，前述多個操作按鈕分別用來控制機械設備的多個馬達來轉動，以使機械設備到達用戶期望的位置與姿態。

上述教導技術中，用戶必須將期望的位置與姿態自空間座標系(即XYZ軸座標系)轉換至機械設備的運動座標系(由多個馬達位置組成多維度座標系)才能進行操作，進而順利使機械設備到達用戶期望的位置與姿態。上述教導技術不僅費時亦不夠直覺。

為解決上述問題，目前另有一種位置與姿態教導技術被提出，前述位置與姿態教導技術是由用戶先操作機械設備進入力矩模式，再以人力調整機械設備的姿態，藉以直覺地使機械設備到達期望的位置與姿態。

然而，用戶每次要進行位置與姿態教導時，都必須先手動操作機械設備切換至力矩模式，因而相當不便。

此外，於力矩模式下，機械設備的各馬達是輸出固定的力矩以於未受額外的外力的情況下維持固定姿態，當用戶以人力調整機械設備的姿態時，必須克服機械設備的摩擦力與前述力矩，才能改變機械設備的姿態，而相當費力。

是以，現有姿態教導技術存在上述問題，而亟待更有效的方案被提出。

本發明之主要目的，係在於提供一種教導位置與姿態的觸發與補償方法，可自動偵測用戶是否開始執行姿態教導。

為達上述目的，本發明係提供一種教導位置與姿態的觸發與補償方法，用於包含控制器、馬達驅動器及機械設備的自動化機械系統，機械設備包括用以變換位置與姿態的多個馬達，教導位置與姿態的觸發與補償方法包括以下步驟：於位置與姿態維持模式下，控制器每隔一週期計算並執行新的位置與姿態維持命令以使機械設備維持在第一位置與姿態，其中位置與姿態維持命令包括各馬達的馬達控制命令；於新的該姿態維持命令的輸出力矩與該第一姿態所對應的一參考姿態命令的輸出力矩之間具有差異時，判定該機械設備接受一外力，決定該外力的方向並切換至一動力維持模式；於動力維持模式下，決定機械設備的各馬達的當前動力；依據外力的方向決定補償力；及，依據補償力及各馬達的當前動力計算動力維持命令並經由馬達驅動器執行動力維持命令 以減輕朝外力方向調整機械設備為第二姿態的阻力，其中動力維持命令包括各馬達的馬達控制命令。

本發明可供用戶以便捷且省力的方式來對機械設備的姿態進行教導。

1:機械設備

11:馬達

12:傳動機構

2:馬達驅動器

3:控制器

31:機構重力補償模組

32:摩擦力補償模組

33:安全模組

40:教導狀態

400:姿態維持模式

401:動力維持模式

41:工作狀態

410:姿態維持模式

D1:轉向資訊

F1、F2、F2’、F3:力

M1、M2:力矩

P1:位置資訊

V1:轉速資訊

S10-S18:第一觸發與補償步驟

S20-S22:姿態維持步驟

S30-S32:補償步驟

S400-S410:摩擦力補償步驟

S500-S515:第二觸發與補償步驟

圖1為本發明一實施態樣的自動化機械系統的架構圖。

圖2為本發明一實施態樣的教導狀態與工作狀態的示意圖。

圖3為本發明一實施態樣的計算補償力的示意圖。

圖4A為本發明一實施態樣的姿態教導的第一示意圖。

圖4B為本發明一實施態樣的姿態教導的第二示意圖。

圖4C為本發明一實施態樣的姿態教導的第三示意圖。

圖5為本發明的第一實施例的教導位置與姿態的觸發與補償方法的流程圖。

圖6為本發明的第二實施例的教導位置與姿態的觸發與補償方法的部分流程圖。

圖7為本發明的第三實施例的教導位置與姿態的觸發與補償方法的部分流程圖。

圖8為本發明的第四實施例的教導位置與姿態的觸發與補償方法的部分流程圖。

圖9為本發明的第五實施例的教導位置與姿態的觸發與補償方法的第一部分流程圖。

圖10為本發明的第五實施例的教導位置與姿態的觸發與補償方法的第二部分流程圖。

茲就本發明之一較佳實施例，配合圖式，詳細說明如後。

首請參閱圖1，為本發明一實施態樣的自動化機械系統的架構圖。本發明揭露一種教導位置與姿態的觸發與補償方法(下面簡稱為該方法)，該方法主要運用於如圖1及圖2所示的自動化機械系統。

如圖1所示，自動化機械系統包括機械設備1、馬達驅動器2及控制器3。

機械設備1(如機械手臂或其他多軸移動裝置)包括用以變換位置與姿態的多個馬達11及分別與該多個馬達11連接的多個傳動機構12(如機械肢體)。各馬達11分別連接馬達驅動器2，以接受馬達驅動器2的控制而轉動，並帶動對應的各傳動機構12進行運動。

馬達驅動器2用以對各馬達11進行控制，並可依據各馬達11的狀態取得各軸馬達11的位置資訊並回饋至控制器3。

控制器3用以計算各馬達的馬達控制命令，並經由馬達驅動器2來執行馬達控制命令。

於一實施例中，控制器3係依據各馬達11的位置資訊判斷其轉動方向及轉動速度，藉以計算進行移動或維持姿態所需的力矩(或者自設置於各馬達11的力矩感測器讀取力矩值)，並輸出至馬達驅動器2。馬達驅動器2依據力矩值對各馬達11進行對應的控制以移動機械設備1或使其維持姿態。

於一實施例中，於機械設備1接受用戶的姿態教導時，控制器3可適時經由馬達驅動器2來控制各馬達11提供補償力，以降低用戶扳動機械設備1所遭受的阻力。

具體而言，控制器3包括用以補償重力的機構重力補償模組31、用以補償摩擦力的摩擦力補償模組32及用以避免補償過度的安全模組33，其詳細運作方式容後詳述。

請一併參閱圖2，圖2為本發明一實施態樣的教導狀態與工作狀態的示意圖。本發明的自動化機械系統可於兩種狀態間進行運作，分別為教導狀態40與工作狀態41。

當自動化機械系統處於教導狀態40下，用戶可對機械設備1的姿態進行調整，並且，自動化機械系統可對調整後的姿態進行記錄。教導狀態40包括兩種模式，姿態維持模式400與動力維持模式401。

具體而言，當自動化機械系統處於姿態維持模式400下，是持續控制機械設備1擺出指定姿態，即各馬達11會持續保持在相同位置或維持相同的移動速度，並且，當機械設備1遭受外力時，各馬達會受控制來增加或減少輸出力矩，以使機械設備1維持在指定姿態或維持相同的移動速度。

當自動化機械系統處於動力維持模式401下，是持續控制各馬達11輸出相同的動力(如相同力矩)，即未遭受外力時，機械設備1可以維持對應的姿態；遭受足以克服阻力(如摩擦力或輸出動力)的外力時，機械設備1會朝往外力的方向移動來擺出新的姿態。藉此，用戶可調整機械設備1的姿態。

更進一步地，本發明的自動化機械系統是被設定來於進入教導狀態40直接切換至姿態維持模式400以等待用戶開始進行姿態教導。當偵測到外力介入時(如用戶意圖移動機械設備1)，自動切換至動力維持模式401以使用戶可對機械設備1的姿態進行調整。

更進一步地，於動力維持模式401下，自動化機械系統可於偵測到用戶停止變換機械設備1的姿態或是用戶手動執行模式切換操作時，切換回姿態維持模式400以維持指定姿態(如調整後的姿態)或指定的移動速度。

當自動化機械系統處於工作狀態41下(如經由自動偵測或用戶手動操作)，自動化機械系統可進入姿態維持模式400，並依據用戶設定的控制指令或排程來控制機械設備1擺出指定姿態，連續擺出不同姿態(如教導狀態下所學習的姿態)或以指定速度變換位置及/或姿態，藉以達成自動化控制(如自動組裝、自動檢測或自動拾取等等)。

教導狀態40的姿態維持模式400與工作狀態41的姿態維持模式410可為相同或相似的運作方式，但不加以限定。

於一實施例中，自動化機械系統在教導狀態40的姿態維持模式400下移動機械設備1的速度(即姿態變換速度)是小於在工作狀態41的姿態維持模式400下移動機械設備1的速度。

具體而言，教導狀態40通常是由用戶協助，當進行姿態變化時，為避免以過快的姿態變換速度誤碰撞用戶或周圍物件所帶來的損害，而刻意降低姿態維持模式400中的姿態變換速度。工作狀態41通常是機械設備1獨立運作，周圍沒有用戶或無關物件時，故姿態維持模式410可採用較快的姿態變換速度進行運作，以提升自動化處理的效率。

於一實施例中，控制器3可包括儲存模組(圖未標示)，前述儲存模組可包括非暫態電腦可讀取媒體，並儲存有電腦程式(如機械設備1的控制程式)，電腦程式記錄有電腦可讀取的程式碼。控制器3可執行電腦程式來經由馬達驅動器2控制各馬達11來實現本發明各實施例的該方法的各步驟。

請一併參閱圖5，為本發明的第一實施例的教導位置與姿態的觸發與補償方法的流程圖。該方法主要包括以下步驟。

步驟S10：控制器3進入姿態維持模式400。

步驟S11：於姿態維持模式400下，控制器3每隔一週期(如0.1毫秒、0.5毫秒、1秒或5秒)計算新的姿態維持命令，並執行新的姿態維持命令。

於一實施例中，姿態維持命令包括各馬達11的馬達控制命令。控制器3每隔一週期取得當前姿態(可包括當前位置)，並比較當前姿態是否與所指定的姿態(第一姿態)的姿態(可包括位置，如移動完成後的位置，可為空間座標系的位置或機器人座標系的位置)相符。若姿態相符，則控制器3可繼續輸出相同參數的姿態維持命令，即使各馬達11維持當前的動力輸出(如發出相同的馬達控制命令)；若姿態(或位置)不符，則控制器3可計算當前姿態與指定姿態之間的差異及/或當前位置與指定位置之間的差異，並依據此差異計算新的姿態維持命令(如重新計算各馬達11的移動方向、移動距離及/或輸出力矩做為新的馬達控制命令，而使該機械設備1於各馬達11移動後可擺出指定姿態)及/或移動至指定位置，將新的姿態維持命令發送至馬達驅動器2，以對各馬達11逐一進行控制調整，而使機械設備的當前姿態修正為指定姿態。藉此，機械設備1可適應所施加的外力，而持續維持在指定姿態(第一姿態)。

換句話說，前述新的姿態維持命令是用以控制各馬達11改變轉動方式(如改變輸出力矩)以抵消機械設備1所受外力。

步驟S12：控制器3對新的姿態維持命令(如最新一個週期執行的姿態維持命令)與用以擺出第一姿態的參考姿態命令進行比較(如對姿態維持命令所提供的輸出力矩與參考姿態命令所提供的輸出力矩進行比較)，以偵測姿態維持命令是否發生變化(如輸出力矩是否改變)，即判斷機械設備1是否受到外力(即用戶為了進行姿態教導而對機械設備1施力)，而使控制器3為了使機械設備1維持在指定姿態而產生用以抵消外力的新的姿態維持命令。

前述參考姿態命令是用以控制各馬達11的轉動方式以使機械設備1於未受外力下擺出指定的姿態(如第一姿態)。

步驟S13：控制器3判斷新的姿態維持命令與參考姿態命令之間是否具有差異(如新的姿態維持命令所提供的輸出力矩與參考姿態命令所提供的輸出力矩之間具有差異)。

若控制器3判斷新的姿態維持命令與參考姿態命令之間具有差異，則可決定外力的方向，並執行步驟S14。若控制器3判斷新的姿態維持命令與參考姿態命令之間沒有差異，則表示機械設備1未受到外力，並跳至步驟S11執行。

於一實施例中，各姿態維持命令是分別用以提供一組輸出力矩。控制器3是於新的姿態維持命令所對應的新的輸出力矩與參考姿態命令的輸出力矩之間的力矩差異符合預設的力矩臨界值時，判定新的姿態維持命令與參考姿態命令之間具有差異，並依據力矩變化決定外力的方向。

藉此，本發明可經由姿態命令的變化有效偵測用戶是否意圖進行姿態教導，且不需額外設置感測裝置。

步驟S14：於判定用戶意圖進行姿態教導時，控制器3切換至動力維持模式。

步驟S15：控制器3決定機械設備1的各馬達的當前動力，如當前的力矩、轉速及/或轉動方向。

步驟S16：控制器3依據外力的方向決定補償力。於一實施例中，補償力的方向是與外力相同，即補償力是用來抵銷移動機械設備1的阻力，但不以此為限。

步驟S17：控制器3依據補償力及各馬達的當前動力計算一組動力維持命令，並經由馬達驅動器2執行所計算的動力維持命令。前述動力維持命令 包括各馬達11的馬達控制命令，當各馬達11依據對應的馬達控制命令運作時，可減輕朝外力的方向調整機械設備為另一姿態(第二姿態，即用戶所期望的姿態)的阻力。

舉例來說，補償力的大小可等於(或接近)機械設備1的摩擦力(如最大靜摩擦力或動摩擦力)的大小，但方向相反(即與外力相同方向)。由於補償了摩擦力，用戶僅需所施加少量外力(如小於摩擦力的外力)，即可朝期望方向推動機械設備1。

於另一例子中，補償力的大小是略大於機械設備1的摩擦力的大小，並與外力相同方向，而使得機械設備1於未施下外力的情況下會緩慢朝先前施加的外力方向移動。藉此，用戶可更為輕鬆地使機械設備1朝期望方向移動。值得一提的是，於此狀況下，當機械設備1移動至用戶期望的位置時(即擺出用戶期望的姿態)，用戶僅需稍微施加阻力，即可使機械設備1停止移動，而維持用戶期望的姿態。

步驟S18：機械設備1接受用戶的姿態教導操作，而擺出用戶期望的姿態，接著，控制器3對最後的姿態(第二姿態)進行記錄，(如記錄各馬達11的位置)。

本發明可供用戶以便捷方式來觸發姿態教導，並以省力方式來對機械設備的姿態進行教導，且不需額外設置感測器。

續請一併參閱圖4A至圖4C，圖4A為本發明一實施態樣的姿態教導的第一示意圖，圖4B為本發明一實施態樣的姿態教導的第二示意圖，圖4C為本發明一實施態樣的姿態教導的第三示意圖。圖4A至圖4C用以示例性說明本發明如何觸發姿態教導，以及如何於姿態教導過程中進行力補償。

如圖4A所示，自動化機械系統於進入教導狀態40後切換至姿態維持模式400，並維持第一姿態(如執行第一姿態的姿態參考命令)且為靜止狀態。 當用戶欲執行姿態教導時，可直接對機械設備1的傳動機構12施加外力F1以改變機械設備1的第一姿態。於接受外力後，控制器3會依據外力F1計算新的姿態維持命令，並經由馬達驅動器2控制多個馬達11實現新的姿態維持命令(即增加了力矩M1來抵消外力F1)，以維持第一姿態。

接著，如圖4B所示，當控制器3偵測到第一姿態的姿態參考命令與新的姿態維持命令具有差異(即增加了力矩M1)時，判定用戶有姿態教導的意圖，切換至動力維持模式401，並根據力矩M1計算出外力F1的方向。

於動力維持模式401下，控制器3依據外力F1的方向決定補償力矩M2(如設定為最大靜摩擦力)並產生對應的動力維持命令。於經由馬達驅動器2執行動力維持命令後，由於馬達11會朝外力F1的方向提供補償力矩M2，用戶可省力地移動傳動機構12來使機械設備1擺出期望的第二姿態(如圖4C所示)。

藉此，本發明可供用戶便捷地觸發姿態教導，且省力地進行姿態教導。

續請一併參閱圖5及圖6，圖6為本發明的第二實施例的教導位置與姿態的觸發與補償方法的部分流程圖。於本實施例中，各姿態是以機械設備11擺出此姿態時各馬達11的當時位置來做成記錄，即前述當前姿態包括各馬達11的當前位置，前述第一姿態包括各馬達11的第一位置，前述第二姿態包括各馬達11的第2位置。

相較於圖5所示的該方法，本實施例的該方法的步驟S11更包括以下步驟。

步驟S20：於姿態維持模式400下，控制器3每隔一週期取得機械設備的當前姿態。

步驟S21：控制器3依據當前姿態與第一姿態之間的差異計算新的姿態維持命令。

於一實施例中，控制器3可計算各馬達11的當前位置與第一位置之間的差異，並依據所算出的位置差異產生可使各馬達11從當前位置移回至第一位置的各馬達控制命令，以做為新的姿態維持命令。

步驟S22：控制器3依據新的姿態維持命令的各馬達控制命令經由馬達驅動器2控制各馬達11轉動以使機械設備成為第一姿態。

於一實施例中，前述各馬達控制命令包括轉動速度命令與轉動方向命令。

藉此，經由不斷重複步驟S20-S22，本發明可是機械設備1於姿態維持模式下適應外力來維持指定的第一姿態。並且，因適應外力所改變的姿態維持命令的輸出可用來偵測是否用戶意圖(即偵測用戶是否開始姿態教導)。

續請一併參閱圖5及圖7，圖7為本發明的第三實施例的教導位置與姿態的觸發與補償方法的部分流程圖。相較於圖5的該方法，本實施例的該方法的步驟S16包括以下步驟。

步驟S30：控制器3計算摩擦力補償力，前述摩擦力補償力是用來抵消或減輕機械設備1朝外力的方向進行教導移動所生的摩擦力。

於一實施例中，若機械設備1為移動中狀態，摩擦力補償力的量值可接近或等於動摩擦力的量值，但方向相反；若機械設備1為靜止狀態，摩擦力補償力的量值可接近或等於最大靜摩擦力的量值，但方向相反。

步驟S31：控制器3依據各馬達的當前動力計算重力補償力，前述重力補償力是用來抵消或減輕機械設備的重力。

具體而言，由於傳動機構12具有重量，於姿態教導過程中會因重力而垮掉或改變姿態，而導致用戶必須克服傳動機構12的重力才能移動機械設備1。故本發明針對機械設備1的重力進行補償，可進一步節省用戶進行姿態教導所需力量大小。

步驟S32：控制器3依據前述摩擦力補償力及前述重力補償力決定最終的補償力。

於一實施例中，前述摩擦力補償力與前述重力補償力為力矩，控制器3計算前述摩擦力補償力與前述重力補償力的合力矩，作為最終的補償力。

續請一併參閱圖3，為本發明一實施態樣的計算補償力的示意圖。圖3用以示例性說明本發明如何計算補償力。

如圖所示，馬達驅動器2於每個通訊週期取得所受外力的力矩F1，回饋力矩F1至機構重力補償模組31及摩擦力補償模組32。

具體而言，控制器3可由各馬達11的位置資訊P1、轉向資訊D1及/或轉速資訊V1來計算力矩F1，將位置資訊P1回饋至機構重力補償模組31，並將轉向資訊D1與轉速資訊V1回饋至摩擦力矩補償模組32。

機構重力補償模組31依據每個通訊週期的位置資訊P1可計算各馬達11為負擔機械設備1於當前姿態下的重力所需的重力補償力矩F3。摩擦力矩補償模組32可在每個通訊週期依據轉向資訊D1及轉速資訊V1計算各馬達11為消除上述摩擦力所需的摩擦力補償力矩F2。

並且，為了避免摩擦力補償力矩F2過大而使機械設備1移動過快，摩擦力補償力矩F2會被回饋至安全模組33以進行調整並產生調整後的摩擦力補償力矩F2’。

接著，控制器3綜合重力補償力矩F1及摩擦力補償力矩F2’，計算並輸出最終的控制力矩C1至馬達驅動器2，並且馬達驅動器2依據控制力矩C1對各馬達11進行控制。由於控制力矩C1中包含了重力補償力矩F3，因此當各馬達11依據控制力矩C1轉動時，可提供足夠的支撐力而令機械設備1在該馬達驅動器2的動力維持模式下維持穩定，不會因為支撐力不足而垮掉。再者，控制力矩C1中同時包含了調整後的摩擦力補償力矩F2’，當使用者直接施力於機械設備1 上且各馬達11依據控制力矩C1轉動時，各馬達11可藉由調整後的摩擦力補償力矩F2’消除產生的摩擦力，而使得使用者可輕易地且安全地推動、拉動各傳動機構12，進而以人力直接進行位置教導。

續請一併參閱圖5、圖7及圖8，圖8為本發明的第四實施例的教導位置與姿態的觸發與補償方法的部分流程圖。相較於圖5及圖7所示的該方法，圖8的實施例示對如何計算摩擦力補償力(即步驟S30)做更詳細的說明。本實施例的該方法是經由以下步驟來計算摩擦力補償力。

步驟S400：控制器3判斷是否各馬達11受到外力而欲轉動。

若在姿態維持模式下任一馬達11未接受外力(例如此馬達11的輸出力矩未發生變化)，則控制器3執行步驟S401：判定用戶沒有直接施力於傳動機構12上(即未開始進行姿態教導)或者雖施加外力，但外力沒有作用於馬達11，故此馬達11不需進行摩擦力補償，並設定馬達的摩擦力補償力為零。

若任一馬達11接受外力，控制器3切換至動力維持模式，並執行步驟S402：判斷各傳動機構12的移動速度是否零(即傳動機構12已克服最大靜摩擦力而開始移動)。

若任一傳動機構12的移動速度為零，則控制器3判定馬達11未轉動，或是雖轉動，但提供動力未超過傳動機構12的最大靜摩擦力，而使傳動機構12維持靜止，並執行步驟S403：設定用以移動此傳動機構12的馬達的摩擦力補償力為起始摩擦力(如接近或等於最大靜摩擦力)。

若任一傳動機構12的移動速度大於零，則控制器3判定馬達11所提供動力曾經超過傳動機構12的最大靜摩擦力，而使傳動機構12開始移動，並執行步驟S404：設定用以移動此傳動機構12的馬達的摩擦力補償力為動摩擦力。前述動摩擦力小於前述起始摩擦力

於一實施例中，控制器3可依據各馬達11的當前轉動速度所對應的動摩擦係數與微動摩擦力計算前述動摩擦力。以力矩為例，可如下述式(一)計算：動摩擦力矩=微動摩擦力矩+動摩擦係數＊目前轉動速度………式(一)。

接著，控制器3執行步驟S405：判斷是否任一傳動機構12的移動速度大於安全速度。

若傳動機構12的移動速度不大於安全速度，則控制器3不須執行安全功能，即執行步驟S406：直接輸出所計算的摩擦力補償力。

若任一傳動機構12的移動速度大於安全速度，則控制器3執行安全功能，即步驟S407：控制器3調降摩擦力補償力，以降低傳動機構12的移動速度。

藉此，本發明可依據機械設備1的當前移動狀態來調整摩擦力補償力，而提升用戶進行姿態教導時的用戶體驗。

續請一併參閱圖9及圖10，圖9為本發明的第五實施例的教導位置與姿態的觸發與補償方法的第一部分流程圖，圖10為本發明的第五實施例的教導位置與姿態的觸發與補償方法的第二部分流程圖。本實施例的該方法包括以下步驟。

步驟S500：自動化機械系統接受操作(如經由人機介面接受進入教導狀態操作)，並受控制器3控制來切換至教導狀態40。

接著，控制器3執行步驟S501-S508以於姿態維持模式下偵測用戶是否開始執行姿態教導，並於偵測到用戶的開始意圖時自動切換至動力維持模 式來提供補償力並接受姿態教導。圖9的步驟S501-S508是與圖5的步驟S10-S17相同或相似，其執行細節不再重複敘述。

接著，控制器3執行步驟S509：於動力維持模式下偵測到機械設備1是否停止變換位置與姿態，如經由各馬達11的當前位置是否繼續發生變化來偵測用戶是否完成位置與姿態教導。

若機械設備1未停止變換位置與姿態，則控制器3再次執行步驟S509以持續偵測。

若機械設備1停止變換位置與姿態，則控制器3執行步驟S510：記錄當前姿態(第二姿態)，如記錄各馬達11的當前位置，並作為第二姿態的位置。

接著，控制器3執行步驟S511：控制器3判斷是否離開教導狀態40，如用戶操作人機介面來控制自動化機械系統離開教導狀態40，或是自動化機械系統於完成姿態教導後自動離開教導狀態40。

若未離開教導狀態40，則控制器3控制自動化機械系統套至步驟S501，即切換回姿態維持模式400以對新的教導意圖進行偵測。

若離開教導狀態40，則控制器3執行步驟S512：控制自動化機械系統切換至工作狀態，並進入姿態維持模式410。

於一實施例中，用戶可操作人機介面來控制自動化機械系統進入工作狀態41，或是自動化機械系統於完成姿態教導後自動進入工作狀態41。

接著，控制器3執行步驟S513：控制器3判斷是否收到任一工作控制命令。

於一實施例中，前述工作控制命令可包括各馬達11的馬達控制命令(如位置命令、轉向命令及/或轉速命令)。

接著，控制器3執行步驟S514：執行工作控制命令以控制各馬達11轉動至工作控制命令所指定的位置，並擺出工作控制命令所對應的姿態。

於一實施例中，工作控制命令的其中之一可以是要求機械設備1擺出第二姿態(即透過前述姿態教導所學習的姿態)的命令，而於步驟S514中控制器3是控制各馬達11轉動至於教導狀態40所記錄的第二姿態的位置。

步驟S515：控制器3判斷是否結束工作，如用戶可操作人機介面來控制自動化機械系統離開工作狀態41，或是自動化機械系統於完成工作後自動離開工作狀態41。

若判定結束工作，則控制器3結束該方法的執行。若判定未結束工作，則控制器3再次執行步驟S513。

藉此，本發明可將教導狀態40下所學習的姿態用於工作狀態41。

以上所述僅為本發明之較佳具體實例，非因此即侷限本發明之專利範圍，故舉凡運用本發明內容所為之等效變化，均同理皆包含於本發明之範圍內，合予陳明。

S10-S18:第一觸發與補償步驟

Claims (9)

1. 一種教導位置與姿態的觸發與補償方法，用於包含一控制器、一馬達驅動器及一機械設備的一自動化機械系統，該機械設備包括用以變換位置與姿態的多個馬達，該教導位置與姿態的觸發與補償方法包括以下步驟：a)於一姿態維持模式下，該控制器每隔一週期計算並執行新的姿態維持命令以使該機械設備維持在一第一姿態，其中該姿態維持命令包括各該馬達的一馬達控制命令，其中該多個姿態維持命令是分別用以提供多個輸出力矩；b)計算新的該姿態維持命令所提供的該輸出力矩及該第一姿態所對應的一參考姿態命令所提供的該輸出力矩之間的一力矩差異，於該力矩差異大於一力矩臨界值時，判定該機械設備接受一外力，依據該力矩差異決定該外力的方向並切換至一動力維持模式；c)於該動力維持模式下，決定該機械設備的各該馬達的當前動力；d)依據該外力的方向決定一補償力；及e)依據該補償力及各該馬達的該當前動力計算一動力維持命令並經由該馬達驅動器執行該動力維持命令以減輕朝該外力方向調整該機械設備為一第二姿態的阻力，其中該動力維持命令包括各該馬達的該馬達控制命令。
2. 如請求項1所述的教導位置與姿態的觸發與補償方法，其中該步驟a)包括以下步驟：a1)於該姿態維持模式下，該控制器每隔該週期取得該機械設備的一當前姿態；a2)依據該當前姿態與該第一姿態之間的一差異計算新的該姿態維持命令；及 a3)依據新的該姿態維持命令的各該馬達控制命令控制各該馬達轉動以使該機械設備為該第一姿態，其中各該馬達控制命令包括一轉動速度命令與一轉動方向命令。
3. 如請求項2所述的教導位置與姿態的觸發與補償方法，其中該當前姿態包括各該馬達的當前位置，該第一姿態包括各該馬達的一第一位置；該步驟a2)是計算各該馬達的該當前位置與該第一位置之間的該差異，並依據該差異產生使各該馬達從該當前位置移回至該第一位置的各該馬達控制命令，以做為新的該姿態維持命令。
4. 如請求項1所述的教導位置與姿態的觸發與補償方法，其中於該姿態維持模式下，新的該姿態維持命令是用以控制各該馬達改變轉動方式以抵消該機械設備所受該外力，該參考姿態命令是用以控制各該馬達轉動方式以使該機械設備於未受外力下擺出該第一姿態。
5. 如請求項1所述的教導位置與姿態的觸發與補償方法，其中該步驟d)包括一步驟d1)計算為了抵消或減輕該機械設備朝該外力的方向進行教導移動所生的摩擦力所需的一摩擦力補償力。
6. 如請求項6所述的教導位置與姿態的觸發與補償方法，其中該多個馬達分別用以帶動多個傳動機構移動以變換該機械設備的姿態，該步驟c)是取得各該傳動機構的一移動速度；該步驟d1)包括以下步驟：d11)於該馬達未受外力時，設定該馬達的該摩擦力補償力為零；d12)於任一該傳動機構的該移動速度為零時，設定對應的該馬達的該摩擦力補償力為一起始摩擦力；d13)於任一該傳動機構的該移動速度大於零時，設定對應的該馬達的該摩擦力補償力為一動摩擦力；及 d14)於任一該傳動機構的該移動速度大於一安全速度時，調降該摩擦力補償力。
7. 如請求項5所述的教導位置與姿態的觸發與補償方法，其中該步驟d)包括以下步驟：d2)依據各該馬達的該當前動力計算為了抵消或減輕該機械設備的重力所需的一重力補償力；及d3)依據該摩擦力補償力及該重力補償力決定最終的該補償力。
8. 如請求項1所述的教導位置與姿態的觸發與補償方法，更包括以下步驟：f)於該動力維持模式下偵測到該機械設備停止變換位置與姿態時，記錄各該馬達的一當前位置，並作為該第二姿態的位置；及g)切換至該姿態維持模式。
9. 如請求項8所述的教導位置與姿態的觸發與補償方法，更包括以下步驟：h)於該自動化機械系統被操作來切換至一教導狀態時，重複執行該步驟a)至該步驟g)；及i)於該自動化機械系統被操作來切換至一工作狀態且收到要求該機械設備擺出該第二姿態的一工作控制命令時，控制各該馬達轉動至於該教導狀態所記錄的該第二姿態的位置。

Images