TWI727851B - 於弧形路徑中控制端效器追蹤物件的方法 - Google Patents

Abstract

本發明提出一種於弧形路徑中控制端效器追蹤物件的方法，運用於具有輸送帶及端效器的自動化作業系統。該方法是規畫直線軌跡與姿態插補軌跡，計算移動速度曲線、位置補償量、旋轉速度曲線與姿態補償量，依據上述資料產生並執行控制命令以控制端效器朝物件移動並旋轉端效器的端效器姿態。並且，端效器於追上物件時，是與物件具有相同的移動速度與對應的姿態變化。本發明可有效控制端效器於弧形路徑中追隨物件以進行作業。

Description

於弧形路徑中控制端效器追蹤物件的方法

本發明係與追蹤物件位置與姿態的方法有關，特別有關於在弧形路徑中控制端效器追蹤物件的方法。

現有的自動作業系統包括輸送帶、機械設備的端效器與控制器。輸送帶是由馬達所驅動並用以輸送物件。端效器(end-effector)用以對輸送帶上物件進行抓取、放置、加工或檢測等自動化作業。控制器可依據馬達轉動速度與方向來預測輸送帶上的物件於各時間點或各週期的預測位置，並控制端效器前往至預測位置來對物件進行自動化作業。

然而，端效器通常是由靜止開始動作，且端效器的移動速度與物件的移動速度不一定相同，這使得當端效器抵達預測位置時，可能因兩者速度不同步而造成抓取、放置、加工或檢測等自動化作業失敗(如物件未抵達預測位置或已離開預測位置)。

此外，現有技術由於是依據輸送帶速度預測物件於指定時間的預測位置，並控制端效器前往預測位置來進行自動作業，當輸送帶速度改變，物件便不會於指定時間抵達預測位置，而使端效器錯過物件。

此外，輸送帶通常有直線路徑與弧形路徑(即彎角)兩種形態，由於物件於弧形路徑的實際移動距離與位移不同，控制端效器追上物件所需之軌跡規劃演算法過於複雜而不易實現，這導致現有的自動作業系統通常是在直線路徑設置端效器來進行自動化作業，而浪費了弧形路徑的空間。

此外，當物件於弧形路徑被輸送時，物件的姿態會不斷改變，這使得端效器無法準確地對物件執行自動化作業。

是以，現有自動作業系統的追蹤物件方法存在上述問題，而亟待更有效的方案被提出。

本發明之主要目的，係在於提供一種方法，可使端效器於弧形路徑中隨物件移動並改變姿態。

為達上述目的，本發明係提供一種於弧形路徑中控制端效器追蹤物件的方法，運用於具有一輸送帶及一端效器的一自動化作業系統，該輸送帶用以輸送一物件並具有一弧形路徑，該方法包括以下步驟：a)取得進入該弧形路徑的該物件的一物件進入姿態及一物件進入位置；b)取得一物件移動速度及一物件旋轉速度；c)規畫自一端效器起始位置到該物件進入位置的一直線軌跡；d)依據該物件移動速度計算用以使該端效器追上該物件的一移動速度曲線及用於該直線軌跡的一位置補償量；e)依據該物件進入姿態及一端效器起始姿態規畫一姿態插補軌跡；f)依據該物件旋轉速度計算用以使該端效器旋轉至與該物件的姿態對應的一旋轉速度曲線及用於該姿態插補軌跡的一姿態補償量； g)依據該直線軌跡、該移動速度曲線、該位置補償量、該姿態插補軌跡及該姿態補償量產生一控制命令；及h)執行該控制命令來控制該端效器朝該物件移動並旋轉姿態，其中於該端效器追上該物件後，該端效器與該物件的移動速度相同，且該端效器與該物件的姿態對應變化。

於一實施例中，該自動化作業系統更包括一觸發裝置，該方法於該步驟a)之前更包括一步驟i)於通過該觸發裝置偵測到該物件進入該弧形路徑時，執行該步驟a)至該步驟h)。

於一實施例中，該物件的姿態是以一座標系表示，步驟b)是計算該輸送帶的該弧形路徑的線速度作為該座標系的該物件移動速度，依據該物件移動速度計算一物件移動角度，並依據該物件移動角度計算該座標系的該物件旋轉速度。

於一實施例中，該方法更包括一步驟j)於該端效器追上該物件後，控制該端效器對該物件執行一自動化作業。

於一實施例中，該自動化作業包括對該物件執行抓取、放置、加工或檢測的至少其中之一操作。

於一實施例中，該方法更包括一步驟k)於偵測到該物件移動速度改變時，再次執行該步驟a)至該步驟h)。

於一實施例中，該步驟d)包括以下步驟：d1)依據該物件移動速度計算該移動速度曲線，其中該移動速度曲線是用以指示該端效器的一端效器移動速度的變化；及d2)基於該移動速度曲線計算該端效器於各週期的一弧形移動距離及一弧形移動角度，依據各該弧形移動距離及各該弧形移動角度執行一弧形插補以獲 得各該週期的一弧形補償位置，並依據該物件進入位置及各該週期的該弧形補償位置計算各該週期的該位置補償量。

於一實施例中，該步驟g)包括一步驟g1)依據該端效器於該直線軌跡的各該週期的直線偏移量及各該週期的該位置補償量計算該端效器於各該週期的一端效器位置，並依據各該週期的該端效器位置計算該端效器於各該週期的一移動控制命令。

於一實施例中，該步驟f)包括f1)依據該物件旋轉速度計算該旋轉速度曲線；及f2)依據該旋轉速度曲線計算各週期的一物件旋轉角度，依據該物件進入姿態與各該週期的該物件旋轉角度執行一四元素插補以獲得該物件於各該週期的一四元素旋轉座標作為各該週期的該姿態補償量。

於一實施例中，該步驟g)包括一步驟g2)依據各該週期的該四元素旋轉座標執行一姿態融合以獲得該物件於各該週期的一旋轉矩陣，並依據各該旋轉矩陣產生各該週期的一旋轉控制命令。

本發明可有效於弧形路徑中追蹤物件的軌跡與姿態，而使端效器可於弧形路徑中對追隨物件以進行作業。

10:控制器

11:端效器

12:輸送帶

130-132、140-142:位置

2:自動化作業系統

20:控制器

21:觸發裝置

22:編碼器

23:輸送帶

24:端效器

30:終端機

31:網路

40:直線軌跡規劃模組

41:移動速度同步模組

410:移動速度曲線模組

411:弧形插補模組

42:姿態軌跡規劃模組

43:旋轉速度同步模組

430:旋轉速度曲線模組

431:姿態插補模組

501-505:弧形軌跡

511-514:物件位移量

511’-513’:位置補償量

520:直線軌跡

521-523:彌補的直線偏移量

600-604:物件弧形位置

601’-603':弧形補償位置

610-614:端效器位置

70:輸送帶

71-73:機械設備

θ 1-θ 4:角度

A1、A2:位移

C1-C3:命令

R1-R3:區域

t0-t3:時間點

Vo:物件移動速度

Ve:端效器移動速度

X、Y、Z:軸

o:座標原點

S10-S17:第一追蹤步驟

S200-S214:第二追蹤步驟

圖1為於直線路徑中執行自動化作業的示意圖。

圖2為於弧形路徑中執行自動化作業的示意圖。

圖3為本發明一實施態樣的自動化作業系統的架構圖。

圖4為本發明一實施態樣的控制器的架構圖。

圖5為本發明一實施態樣的移動速度曲線的示意圖。

圖6A為本發明一實施態樣的自動化作業系統的移動軌跡的第一示意圖。

圖6B為本發明一實施態樣的自動化作業系統的移動軌跡的第二示意圖。

圖6C為本發明一實施態樣的自動化作業系統的移動軌跡的第三示意圖。

圖6D為本發明一實施態樣的自動化作業系統的移動軌跡的第四示意圖。

圖6E為本發明一實施態樣的自動化作業系統的移動軌跡的第五示意圖。

圖7為本發明一實施態樣的自動化作業系統的姿態軌跡的示意圖。

圖8為本發明一實施態樣的自動化作業系統的連續弧形路徑的示意圖。

圖9為本發明第一實施例的於弧形路徑中控制端效器追蹤物件的方法的流程圖。

圖10為本發明第二實施例的於弧形路徑中控制端效器追蹤物件的方法的流程圖。

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

首請參閱圖1及圖2，圖1為於直線路徑中執行自動化作業的示意圖，圖2為於弧形路徑中執行自動化作業的示意圖。圖1及圖2是用以更為清楚地 說明本發明所要解決的主要問題。自動化作業系統包括端效器11、輸送帶12及控制器10。

如圖1所示，於直線路徑中，同一物件於位置130、位置131及位置132的姿態(以XYZ軸的朝向表示)都是相同的，即物件於直線路徑中並不會改變姿態，這使得端效器11只需依據物件當前姿態作為自身的目標姿態即可順利夾取物件。

如圖2所示，於弧形路徑中，同一物件於位置140、位置141及位置142的姿態都不同，即物件於弧形路徑中會不斷改變姿態，這使得端效器11於弧形路徑中無法只以物件當前姿態作為目標姿態對物件進行作業。

並且，於弧形路徑中，物件的實際移動距離(即位置140與位置141之間的弧形軌跡的長度)是大於位移(即位置140與位置141之間的直線軌跡的長度)，這使得一般直線路徑(實際移動距離等於位移)中所採用的端效器控制方式不適用於弧形路徑。

請參閱圖3，為本發明一實施態樣的自動化作業系統的架構圖。本發明提供一種於弧形路徑中控制端效器追蹤物件的方法(以下簡稱該方法)，該方法主要運用於一種自動化作業系統2。

自動化作業系統2可包括觸發裝置21、編碼器22、輸送帶23、端效器24及控制器20。編碼器22電性連接輸送帶23，控制器20電性連接編碼器22及端效器24。觸發裝置21電性連接編碼器22及控制器20。

於本發明中，輸送帶23具有弧形路徑(如圖6A至圖8所示弧形路徑)，本發明主要是揭露如何於弧形路徑中控制端效器追蹤物件。

編碼器22連接馬達(圖未標示)或輸送帶23，並記錄馬達或輸送帶23的各項編碼器資訊(例如馬達的轉角度數或速度，或者輸送帶23的位置或速度)。輸送帶23藉由前述馬達的轉動而運作以輸送一或多個物件。端效器24設置 於機械設備上(如機器手臂或機器人，圖未標示)，並受控制來朝向輸送帶23上的物件移動，以對物件進行取、放或其他加工程序。

於一實施例中，編碼器22於被觸發(如收到觸發裝置21的觸發訊號或經由計時器定時觸發)時將編碼器資訊(如物件被觸發時的輸送帶位置)傳輸至控制器20，控制器20可依據被觸發時的編碼器資訊與當前的編碼器資訊計算輸送帶23上的各物件目前的(輸送帶)位置及/或速度。

觸發裝置21(如攝影機、紅外線感測器或超聲波感測器等等)主要是於偵測到待處理的物件出現在輸送帶23作業區域時發出觸發訊號，藉以令控制器52自編碼器22讀取編碼器資訊。藉此，控制器20可在觸發裝置21被觸發時獲得物件的當前位置。

於一實施例中，自動化作業系統2可經由網路31連接外部的終端機30。終端機30可提供控制介面(圖未標示)，控制介面提供自動化作業系統2的管理者於線上對輸送帶23的速度進行調整。

請一併參閱圖4，為本發明一實施態樣的控制器的架構圖。控制器20可包括用以控制端效器24追上物件的直線軌跡規劃模組40及移動速度同步模組41，以及用以調整端效器24的姿態至適合對物件進行作業的姿態軌跡規劃模組42及旋轉速度同步模組43。

圖4所示的各模組可以硬體方式(如電路板、積體電路或SoC)或軟體方式(如韌體或應用程式等電腦程式)來加以實現，不加以限定。當以軟體方式實現時，控制器20可包括非暫態電腦可讀取媒體，非暫態電腦可讀取媒體儲存有前述電腦程式，當控制器20執行電腦程式後，可實現前述各模組的功能。

請一併參閱圖3、圖4及圖9，圖9為本發明第一實施例的於弧形路徑中控制端效器追蹤物件的方法的流程圖。本實施例的該方法包括以下步驟。

步驟S10：控制器20判斷物件是否進入弧形路徑。

於一實施例中，觸發裝置21是被設置於(或朝向)弧形路徑的入口，並可於偵測到物件進入弧形路徑時(即物件抵達觸發位置)對外發送觸發訊號。

若控制器20判斷沒有物件進入弧形路徑，則再次執行步驟S10以持續偵測。

若控制器20判斷任一物件進入弧形路徑，則執行步驟S11：控制器20取得進入弧形路徑的物件的姿態與位置。

步驟S11：控制器20取得進入弧形路徑的物件所在的物件進入位置與所擺出的物件進入姿態。

於一實施例中，控制器20於收到觸發訊號時可將物件進入位置設定為觸發位置，如觸發裝置21所在位置或所感測的位置。

於一實施例中，觸發裝置21為攝影機，控制器20於攝影機所拍攝的影像畫面中辨識物件姿態。

於一實施例中，控制器20可依據物件進入位置與輸送帶23的速度(即物件移動速度)持續計算物件的物件當前位置，如每隔一週期計算一次物件當前位置，前述週期可為50毫秒、500毫秒、1秒、5秒、10秒等等，不加以限定。

於一實施例中，控制器20可依據物件進入姿態與物件移動速度持續計算物件於輸送帶23中的物件當前姿態，如每隔一週期計算一次物件當前姿態。

於一實施例中，如圖2所示，各物件可被設定一組座標系，並具有座標系原點o(x,y,z)，此座標系於空間座標系(如輸送帶座標系或現實空間座標系)中的朝向與位置即可做為物件的姿態與位置。前述空間座標系可為笛卡爾空間座標系(Cartesian coordinates)。

步驟S12：控制器20取得物件移動速度，並可進一步取得物件旋轉速度。

於一實施例中，控制器20可計算輸送帶23於弧形路徑的線速度作為座標系的移動速度。並且，控制器20可依據前述線速度計算物件的自體的旋轉速度，如依據物件移動速度計算單位時間(如過去一週期)內的物件移動角度，並依據物件移動角度計算物件的物件旋轉速度。

步驟S13：控制器20通過直線軌跡規畫模組40依據物件進入位置及端效器24的端效器位置規畫直線軌跡。

於一實施例中，直線軌跡規劃模組40可取得物件進入位置及端效器起始位置，再依據物件進入位置與端效器起始位置規劃直線軌跡。並且，直線軌跡規劃模組40依據直線軌跡週期性地輸出點位座標(x,y,z)，以控制端效器24朝物件移動。

步驟S14：控制器20通過移動速度同步模組41依據物件的移動速度計算移動速度曲線及位置補償量。

於一實施例中，移動速度同步模組41取得物件移動速度及端效器移動能力(如速度上限或加速度)，再依據兩者規劃一組速度曲線。並且，移動速度同步模組41可依據移動速度曲線週期性地輸出偏移量(offset，如後述的落後偏移量)以作為加速或減速的參考。

前述移動速度曲線是用以指示端效器24於不同時間點或週期的端效器移動速度。並且，當端效器24依據移動速度曲線進行移動時，將可追上物件(即彌補完所有的落後偏移量)，並可於追上物件後維持與物件相同的移動速度。

請一併參閱圖5，為本發明一實施態樣的移動速度曲線的示意圖。圖5用以對移動速度曲線進行示例性說明。

於圖5的例子中，輸送帶23的移動速度(物件移動速度)固定為Vo。於追蹤程序下(即端效器24未追上物件)，控制器20是控制端效器24由靜止開始加速。於t1時間點(如第一週期結束)時端效器移動速度Ve(t1)與物件移動速度Vo相同，於t2時間點(如第二週期結束)時端效器移動速度Ve(t2)達到最高值，並開始逐漸減速，而使得於t3時間點(如第三週期結束)時端效器移動速度Ve(t1)與物件移動速度Vo相同。

並且，於t3時間點時，端效器24已彌補完於t1時間點之前的落後偏移量，即區域A1的面積(即端效器24於t1-t3時間點之間比物件多增加的位移量)會等於區域A2的面積(即物件於t1時間點之前比端效器24多增加的位移量)。

接著，於已同步程序下(即端效器24已追上物件)，端效器24維持與物件相同的移動速度(如維持相同的位移量)，以持續跟隨物件並對物件進行自動化作業。

請一併參閱圖6A至圖6E，圖6A為本發明一實施態樣的自動化作業系統的移動軌跡的第一示意圖，圖6B為本發明一實施態樣的自動化作業系統的移動軌跡的第二示意圖，圖6C為本發明一實施態樣的自動化作業系統的移動軌跡的第三示意圖，圖6D為本發明一實施態樣的自動化作業系統的移動軌跡的第四示意圖，圖6E為本發明一實施態樣的自動化作業系統的移動軌跡的第五示意圖。於本發明中，移動速度同步模組41可對速度曲線的位移執行弧形插補以計算對應的補償量。並且，於本例子中，是假設端效器24花費三週期追上物件。並且，於本例子中，端效器24對於物件的落後偏移量包括直線偏移量與弧形偏移量。

如圖6A所示，當物件抵達弧形路徑的物件進入位置600時會觸發設置於此的觸發裝置21，而使得控制器20規劃自端效器24起始位置610到物件進 入位置600的直線軌跡520(直線軌跡520的長度即為端效器24落後物件的直線偏移量)與端效器24的移動速度曲線。

並且，控制器20基於移動速度曲線開始控制端效器24沿直線軌跡520朝物件進入位置600移動。

接著，如圖6B所示，於第一週期中，物件從物件進入位置600沿弧形路徑(弧形軌跡501)移動至物件弧形位置601(即物件位移量511)。控制器20依據端效器24於第一週期在直線軌跡520上所能彌補的直線偏移量521(可基於端效器24的移動速度曲線經計算獲得)與第一週期的位置補償量511’(即弧形偏移量，表示端效器24於第一週期在弧形路徑上可以彌補的偏移量，於本例子中為空間向量)計算端效器24於第一週期的移動目的地(即端效器位置611，可為空間座標)。

更進一步地，移動速度同步模組41可依據移動速度曲線410計算出第一週期的弧形移動距離，依據此移動距離與弧形路徑的半徑計算弧形移動角度θ 1。再依據物件進入位置600(空間座標，如笛卡爾空間座標)、弧形路徑的半徑與弧形移動角度θ 1執行弧形插補模組411來計算出弧形補償位置601’的座標(空間座標)。依據弧形補償位置601’的座標與物件進入位置600之間的位移量計算出第一週期的位置補償量511’。

接著，如圖6C所示，於第二週期中，物件從物件弧形位置601沿弧形路徑(弧形軌跡502)移動至物件弧形位置602(即物件位移量512)。移動速度同步模組41依據端效器24於第二週期在直線軌跡520上所能彌補的直線偏移量522與第二週期的位置補償量512’(即端效器24於第二週期在弧形路徑上可以彌補的弧形偏移量)計算端效器24於第二週期的移動目的地(即端效器位置612)。

更進一步地，移動速度同步模組41可依據移動速度曲線410模組計算出弧形路徑累積的移動距離，依據此移動距離與弧形路徑的半徑計算弧形 移動角度(即物件進入位置600與弧形補償位置602’之間的移動角度θ 2)，依據物件進入位置600、弧形路徑的半徑與移動角度θ 2執行弧形插補模組411來計算出弧形補償位置602’的座標，並依據弧形補償位置602’的座標與物件進入位置600之間的位移量計算第二週期的位置補償量512’。

值得一提的是，由於移動速度曲線410的最高移動速度大於物件移動速度(才能從靜止狀態追上持續移動的物件)，於第二週期中，弧形路徑補償位置的移動速度可加速至大於物件於弧形路徑的移動速度，而開始彌補落後的弧形偏移量。

接著，如圖6D所示，於第三週期中，物件從物件弧形位置602沿弧形路徑(弧形軌跡503)移動至物件弧形位置603(即物件位移量513)。控制器20依據端效器24於第三週期在直線軌跡520上所能彌補的直線偏移量523與第三週期的位置補償量513’(即端效器24於第三週期在弧形路徑上可以彌補的弧形偏移量)計算端效器24於第三週期的移動目的地(即端效器位置613)。於本例子中，第三週期結束時，端效器24彌補完所有落後偏移量而追上物件，如端效器位置613與物件弧形位置603相同。

更進一步地，移動速度同步模組41可依據移動速度曲線410計算出弧形路徑累積的移動距離，由於第三週期結束已彌補完所有落後偏移量，故弧形路徑累積的移動距離等於物件弧形軌跡501、502、503的長度和。依據此移動距離與弧形路徑的半徑計算弧形移動角度(即物件進入位置600與弧形補償位置603’之間的移動角度θ 3)，依據物件進入位置600、弧形路徑的半徑與移動角度θ 3執行弧形插補411來計算出弧形補償位置603’的座標，並依據弧形補償位置603’的座標與物件進入位置600之間的位移量計算第三週期的位置補償量513’。

接著，如圖6E所示，於第四週期中(即端效器24追上物件後)，物件從物件弧形位置603沿弧形路徑(弧形軌跡504)移動至物件弧形位置604。由於直線軌跡520已全部彌補完畢，移動速度同步模組41可直接將物件位移量514設定為第四週期的位置補償量514(即端效器24於第四週期以後在弧形路徑上可以彌補的弧形偏移量等同於物件的位移量)，計算端效器24於第四週期的移動目的地(即端效器位置614)，以此類推。

更進一步地，移動速度同步模組41可依據物件進入位置600與物件移動速度計算出物件進入弧形路徑後的累積的移動距離(即弧形軌跡501、502、503、504的長度和)，依據此移動距離與弧形路徑的半徑計算物件進入弧形路徑後的物件移動角度(即物件進入位置600與物件弧形位置603之間的物件移動角度θ 4)，依據物件進入位置600、弧形路徑的半徑與物件移動角度θ 4執行弧形插補來計算出物件弧形位置604的座標。並且，由於端效器24已追上物件且兩者的速度已達成同步，移動速度同步模組41可將物件弧形位置604的座標與物件進入位置600之間的物件位移量514直接設定為第四週期的位置補償量514。

此外，於本例子中，第四週期開始，端效器24可以與物件相同的速度持續追蹤物件，並開始對物件自動化處理。並且，當端效器24完成自動化作業後，端效器24可自動移回至端效器起始位置610，以等待下一物件的抵達，而物件繼續沿著弧形軌跡505直到離開弧形路徑。

藉此，本發明可持續修正端效器24的軌跡來確保端效器24追上物件並於追上物件後持續追蹤物件以進行自動化處理。

復請參閱圖9，接著執行步驟S15：控制器20取得物件進入弧形路徑時端效器24的端效器初始姿態，並通過姿態軌跡規劃模組42依據物件進入姿態及端效器起始姿態規畫一組姿態插補軌跡。前述姿態插補軌跡是用來使端效器24自端效器起始姿態轉動至成為可對物件進行作業的姿態。

步驟S16：控制器20通過旋轉速度同步模組43取得物件旋轉速度，並依據物件旋轉速度計算旋轉速度曲線與姿態補償量。

具體而言，由於物件在弧形路徑中移動時，姿態會不斷地改變，對此，本發明進一步提供姿態補償量，並使端效器24於跟隨物件的同時，依據此組姿態補償量連續改變姿態，以持續保持可對物件進行作業的姿態。

於一實施例中，旋轉速度同步模組43取得物件旋轉速度及端效器旋轉能力(如旋轉角速度上限、角加速度或可旋轉角度)，再依據兩者規劃一組旋轉速度曲線。並且，旋轉速度同步模組43可依據旋轉速度曲線週期性地輸出偏移量以作為加速或減速旋轉的參考。

前述旋轉速度曲線可用以指示端效器24於不同時間點的端效器旋轉速度。並且，當端效器24依據旋轉速度曲線進行旋轉時，將可於追上物件後與物件保持相對應的姿態，而可對物件進行作業。

請一併參閱圖6A-6C與圖7，圖7為本發明一實施態樣的自動化作業系統的姿態軌跡的示意圖。

於物件抵達物件進入位置600時，姿態軌跡規劃模組42依據物件於物件進入位置600的物件進入姿態及端效器24於端效器起始位置610的端效器起始姿態規畫一組姿態插補軌跡，即自端效器起始位置610的端效器起始姿態改變為可對物件進入位置600的物件進行作業的端效器姿態。

於第一週期中，物件自物件進入位置600的姿態改變為物件弧形位置601的姿態。旋轉速度同步模組43可(依據物件於物件進入位置600的姿態與物件旋轉角度)計算姿態補償量，如執行四元素插補來獲得物件的於物件弧形位置601的四元素旋轉座標。接著，控制器20可依據姿態補償量控制端效器24改變姿態(如於X-Y平面水平轉動)來保持可對物件弧形位置601的物件進行作業的姿態。

於第二週期中，物件自物件弧形位置601的姿態改變為物件弧形位置602的姿態。旋轉速度同步模組43可(依據物件於物件弧形位置601的姿態與第二週期的物件旋轉角度)計算姿態補償量，控制器20可依據姿態補償量控制端效器24改變姿態，如改變為可對物件弧形位置602的物件進行作業的姿態。

於第三週期中，物件自物件弧形位置602的姿態改變為物件弧形位置603的姿態。旋轉速度同步模組43可(依據物件於物件弧形位置602的姿態與第三週期的物件旋轉角度)計算姿態補償量，控制器20可依據姿態補償量控制端效器24改變姿態，如改變為可對物件弧形位置603的物件進行作業的姿態，以此類推。

藉此，本發明可使端效器的姿態對應物件當前姿態旋轉改變，而可以於端效器追上物件後，順利對物件進行自動化作業。

更進一步地，當物件移動至物件弧形位置604且端效器24移動至端效器位置614時，端效器24完成自動化作業，並可自動移回至端效器起始位置610來擺出端效器起始姿態，以等待下一物件的抵達。

於一實施例中，前述姿態補償量可為物件當前姿態，並用以對姿態插補軌跡的物件進入姿態進行修正以獲得可於追上物件後持續對物件進行作業的姿態變換軌跡，即修正後的姿態插補軌跡是表示各週期的端效器24與物件當前姿態的姿態差異。

並且，旋轉速度同步模組43可週期性地計算姿態補償量以持續修正姿態插補軌跡與端效器當前姿態。

復請參閱圖9，接著執行步驟S17：控制器20依據直線軌跡、移動速度曲線、位置補償量、姿態插補軌跡、旋轉速度曲線及姿態補償量產生控制命令。並且，控制器20依據所產生的控制命令控制端效器24朝物件移動並旋轉端效器24的端效器姿態。

並且，於端效器24追上物件後，端效器24的移動速度是與物件的移動速度相同而可持續跟隨物件，且端效器24的姿態是對應物件姿態進行變化而可持續對物件進行作業。

請參閱圖4，於一實施例中，控制器20將端效器位置(如端效器起始位置或端效器當前位置)與物件位置(如物件進入位置或物件當前位置)輸入至直線軌跡規劃模組40以獲得「直線軌跡」的輸出(步驟S13)；將物件移動速度與端效器移動能力輸入至移動速度同步模組41以獲得「移動速度曲線」及「位置補償量」的輸出(步驟S14)；將端效器姿態(如端效器起始姿態或端效器當前姿態)與物件姿態(如物件進入姿態或物件當前姿態)輸入至姿態軌跡規劃模組42以獲得「姿態插補軌跡」的輸出(步驟S15)；將物件旋轉速度(可由物件的移動線速度計算獲得)與端效器旋轉能力輸入至旋轉速度同步模組43以獲得「旋轉速度曲線」與「姿態補償量」的輸出(步驟S16)。接著，控制器20可依據直線軌跡、移動速度曲線及位置補償量計算用以控制端效器24移動的一組移動控制命令C1，並依據姿態插補軌跡、旋轉速度曲線及姿態補償量計算用以控制端效器24旋轉的一組旋轉控制命令C2。最後，控制器20將移動控制命令C1與旋轉控制命令C2整合為控制命令C3，並經由執行控制命令C3來控制端效器24。

藉此，本發明可有效於弧形路徑中追蹤物件的軌跡與姿態，而使端效器可於弧形路徑中對追隨物件以進行作業。

請一併參閱圖4及圖10，圖10為本發明第二實施例的於弧形路徑中控制端效器追蹤物件的方法的流程圖。於本實施例中，當偵測到物件速度(如輸送帶的速度)改變時，會依據最新的參數(如物件當前位置及/或物件移動速度)重新計算一次控制命令並執行，藉以提升作業的準確性。

並且，如圖4所示，於本實施例中移動速度曲線模組41包括移動速度曲線模組410與弧形插補模組411。旋轉速度同步模組43包括旋轉速度曲線模組430及姿態插補模組431。本實施例的該方法包括以下步驟。

步驟S200：控制器20判斷物件是否進入弧形路徑。

若控制器20判斷沒有物件進入弧形路徑，則再次執行步驟S200以持續偵測。

若控制器20判斷任一物件進入弧形路徑，則執行步驟S201：控制器20取得進入弧形路徑的物件所在的物件進入位置與所擺出的物件進入姿態。

步驟S202：控制器20取得進入弧形路徑的物件當前的物件移動速度與物件旋轉速度。前述物件旋轉速度可由物件移動速度計算獲得。

接著，控制器20可依序或同時執行步驟S203、步驟S204、步驟S207及步驟S208。

步驟S203：控制器20通過直線軌跡規劃模組40規劃從端效器起始位置至物件進入位置的直線軌跡。

步驟S204：控制器20通過移動速度曲線模組410依據物件的移動速度計算端效器24的移動速度曲線。前述移動速度曲線的最高速度是大於物件的移動速度，而使端效器24可於落後的情況下追上物件。

步驟S205：控制器20基於移動速度曲線計算端效器24於各週期的弧形移動距離，依據各週期的弧形移動距離與弧形路徑的半徑計算各週期的弧形移動角度，再依據物件進入位置、弧形路徑的半徑與各週期的弧形移動角度執行弧形插補來計算出各週期的弧形補償位置，並依據將物件進入位置與各週期的弧形補償位置之間的位移量作為計算各週期的位置補償量。

步驟S206：控制器20依據端效器24各週期於直線軌跡的直線偏移量及各週期的位置補償量計算各週期的端效器位置(如空間座標)，並依據各週期 的端效器位置計算各週期的移動控制命令。前述移動控制命令是用以控制端效器24自端效器當前位置前往指定的位置。

步驟S207：控制器20通過姿態軌跡規劃模組42依據物件進入姿態及端效器起始姿態規畫一組姿態插補軌跡。前述姿態插補軌跡是用來使端效器24自端效器起始姿態轉動至成為可對處於物件進入姿態的物件進行作業的姿態。

步驟S208：控制器20通過旋轉速度曲線模組430依據物件旋轉速度計算端效器24的旋轉速度曲線。並且，前述旋轉速度曲線可以使端效器43追上物件後，隨物件轉動而維持可對物件進行作業的姿態。

步驟S209：控制器20通過姿態插補模組431計算物件旋轉角度，依據物件進入姿態與旋轉角度計算物件的當前姿態來作為姿態補償量，並依據姿態補償量執行四元素插補以獲得物件的四元素旋轉座標。前述四元素插補屬於機械運動學的現有技術，其技術細節於此不再贅述。

步驟S210：控制器20依據物件的四元素旋轉座標及姿態插補軌跡執行姿態融合以獲得物件的旋轉矩陣，並依據旋轉矩陣產生旋轉控制命令。

步驟S211：控制器20判斷端效器24是否追上物件。

若控制器20判斷端效器24追上物件，則執行步驟S212：控制端效器24對物件執行預測的自動化作業(如抓取、放置、加工或檢測等等)。

步驟S213：控制器20判斷是否完成此物件的自動化作業。於一實施例中，控制器20是依據是否自動化作業的所有步驟都執行完畢，或者依據物件是否抵達預設的完成位置來判斷是否完成此物件的自動化作業。

若控制器20判斷完成自動化作業，則結束此物件的追蹤控制；若控制器20判斷自動化作業尚未完成，則執行步驟S214以繼續追蹤此物件。

值得一提的是，當執行自動化作業期間，輸送帶23仍持續輸送物件(即物件會持續移動且姿態持續改變)，而端效器24會維持與物件相同的速度並隨物件改變姿態，來於移動中完成自動化作業。

以圖6A至圖6E為例，當物件被輸送至物件進入位置600時會觸發前述觸發裝置21，而使端效器24開始追蹤物件。當物件被移動至位置603時，端效器24追上物件，與物件維持相同的移動速度與對應姿態，並開始對物件執行自動化作業。當物件被移動至位置604時，自動化作業完成，端效器24停止跟隨物件，並可進一步回到端效器起始位置610。

若控制器20判斷端效器24未追上物件，則執行步驟S214：控制器20判斷物件的移動速度(如輸送帶23的速度)是否改變。具體而言，控制器20可持續地自編碼器22讀取(如每隔一週期讀取一次)編碼器資訊(如物件的位置或速度)，並依據編碼器資訊判斷物件的速度是否改變。

若物件的移動速度沒有改變，則控制器20再次執行步驟S211以判斷是否追上物件。

若物件的移動速度改變，則控制器20再次執行步驟S201-S211以更新物件位置與物件姿態，決定並執行新的控制命令。

雖於前述實施例中，是以於單一弧形路徑輸送物件來進行說明，但不以此限定本發明的輸送帶23的路徑。

請參閱圖8，為本發明一實施態樣的自動化作業系統的連續弧形路徑的示意圖。

圖8的輸送帶70具有三個區域R1-R3，並分別設置有機械設備71-73。

區域R1與區域R2為弧形路徑，故可直接應用本發明的於弧形路徑中控制端效器追蹤物件的方法來控制機械設備71、72對輸送帶70上的物件進行追蹤與作業。

區域R3為直線路徑，可直接應用現有的於直線路徑中控制端效器追蹤物件的方法(如圖1所示)來控制機械設備73對輸送帶70上的物件進行追蹤與作業。

藉此，本發明可有效利用弧形路徑的空間。

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

S10-S17:第一追蹤步驟

Claims (10)

1. 一種於弧形路徑中控制端效器追蹤物件的方法，運用於具有一輸送帶及一端效器的一自動化作業系統，該輸送帶用以輸送一物件並具有一弧形路徑，該方法包括以下步驟：a)取得進入該弧形路徑的該物件的一物件進入姿態及一物件進入位置；b)取得一物件移動速度及一物件旋轉速度；c)規畫自一端效器起始位置到該物件進入位置的一直線軌跡；d)依據該物件移動速度計算用以使該端效器追上該物件的一移動速度曲線及用於該直線軌跡的一位置補償量；e)依據該物件進入姿態及一端效器起始姿態規畫一姿態插補軌跡；f)依據該物件旋轉速度計算用以使該端效器旋轉至與該物件的姿態對應的一旋轉速度曲線及用於該姿態插補軌跡的一姿態補償量；g)依據該直線軌跡、該移動速度曲線、該位置補償量、該姿態插補軌跡及該姿態補償量產生一控制命令；及h)執行該控制命令來控制該端效器朝該物件移動並旋轉姿態，其中於該端效器追上該物件後，該端效器與該物件的移動速度相同，且該端效器與該物件的姿態對應變化。
2. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，其中該自動化作業系統更包括一觸發裝置，該方法於該步驟a)之前更包括一步驟i)於通過該觸發裝置偵測到該物件進入該弧形路徑時，執行該步驟a)至該步驟h)。
3. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，其中該物件的姿態是以一座標系表示，步驟b)是計算該輸送帶的該弧形路徑的線速 度作為該座標系的該物件移動速度，依據該物件移動速度計算一物件移動角度，並依據該物件移動角度計算該座標系的該物件旋轉速度。
4. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，更包括一步驟j)於該端效器追上該物件後，控制該端效器對該物件執行一自動化作業。
5. 如請求項4所述的於弧形路徑中控制端效器追蹤物件的方法，其中該自動化作業包括對該物件執行抓取、放置、加工或檢測的至少其中之一操作。
6. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，更包括一步驟k)於偵測到該物件移動速度改變時，再次執行該步驟a)至該步驟h)。
7. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，其中該步驟d)包括以下步驟：d1)依據該物件移動速度計算該移動速度曲線，其中該移動速度曲線是用以指示該端效器的一端效器移動速度的變化；及d2)基於該移動速度曲線計算該端效器於各週期的一弧形移動距離及一弧形移動角度，依據各該弧形移動距離及各該弧形移動角度執行一弧形插補以獲得各該週期的一弧形補償位置，並依據該物件進入位置及各該週期的該弧形補償位置計算各該週期的該位置補償量。
8. 如請求項7所述的於弧形路徑中控制端效器追蹤物件的方法，其中該步驟g)包括一步驟g1)依據該端效器於該直線軌跡的各該週期的直線偏移量及各該週期的該位置補償量計算該端效器於各該週期的一端效器位置，並依據各該週期的該端效器位置計算該端效器於各該週期的一移動控制命令。
9. 如請求項1所述的於弧形路徑中控制端效器追蹤物件的方法，其中該步驟f)包括 f1)依據該物件旋轉速度計算該旋轉速度曲線；及f2)依據該旋轉速度曲線計算各週期的一物件旋轉角度，依據該物件進入姿態與各該週期的該物件旋轉角度執行一四元素插補以獲得該物件於各該週期的一四元素旋轉座標作為各該週期的該姿態補償量。
10. 如請求項9所述的於弧形路徑中控制端效器追蹤物件的方法，其中該步驟g)包括一步驟g2)依據各該週期的該四元素旋轉座標執行一姿態融合以獲得該物件於各該週期的一旋轉矩陣，並依據各該旋轉矩陣產生各該週期的一旋轉控制命令。

Images