本發(fā)明涉及一種具有輸送帶及機(jī)器設(shè)備端效器的生產(chǎn)作業(yè)系統(tǒng)，特別是運(yùn)用于該生產(chǎn)作業(yè)系統(tǒng)的軌跡規(guī)劃與控制方法。

背景技術(shù)：

具輸送帶的生產(chǎn)作業(yè)系統(tǒng)主要具有輸送帶(conveyer)及機(jī)器設(shè)備的端效器(end-effector)，該輸送帶用于輸送物件(object)，而該端效器用于對(duì)該輸送帶上的物件進(jìn)行取、放、加工等動(dòng)作。

該輸送帶主要是依據(jù)馬達(dá)的轉(zhuǎn)動(dòng)而運(yùn)轉(zhuǎn)，因此，該生產(chǎn)作業(yè)系統(tǒng)的控制器可借由馬達(dá)的轉(zhuǎn)動(dòng)角度與速度得知該輸送帶的速度(即，該物件的移動(dòng)速度)，借此預(yù)測該物件于任一時(shí)間點(diǎn)在該輸送帶上的位置。具體地，現(xiàn)有的生產(chǎn)作業(yè)系統(tǒng)一般是由該控制器預(yù)測該物件于一第一時(shí)間點(diǎn)的一預(yù)測位置，再控制該端效器進(jìn)行靜止坐標(biāo)下的直線運(yùn)動(dòng)，以一特定速度朝該預(yù)測位置移動(dòng)。借此，該端效器可于該第一時(shí)間點(diǎn)到達(dá)該預(yù)測位置，以抓取該物件。

但，該端效器主要是朝該預(yù)測位置進(jìn)行直線運(yùn)動(dòng)，但該端效器的移動(dòng)速度與該物件的移動(dòng)速度(即，輸送帶的速度)并不一定相同。因此，當(dāng)該端效器于該預(yù)測位置到達(dá)該物件的位置上時(shí)，容易因?yàn)樗俣炔煌蕉斐扇　⒎懦鲥e(cuò)的問題。另外，由于速度不同步，該生產(chǎn)作業(yè)系統(tǒng)將可能無法在該輸送帶持續(xù)運(yùn)轉(zhuǎn)的情況下，借由該端效器直接對(duì)該輸送帶上的該物件進(jìn)行加工程序。

另外，上述控制方式是依據(jù)該輸送帶的速度來預(yù)測該物件的該預(yù)測位置，因此該生產(chǎn)作業(yè)系統(tǒng)不允許管理者即時(shí)于線上改變?cè)撦斔蛶У乃俣?。否則，一但該輸送帶的速度被改變，該端效器將無法正確地追隨于該輸送帶上移動(dòng)的該物件。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明所要解決的技術(shù)問題是提供一種機(jī)器設(shè)備端效器于生產(chǎn)作業(yè)系統(tǒng) 的軌跡規(guī)劃與控制方法，可依據(jù)輸送帶的速度規(guī)劃端效器移動(dòng)時(shí)采用的速度曲線，使得端效器于追上輸送帶上的待處理物時(shí)，與待處理物具有相同的移動(dòng)速度。

為了實(shí)現(xiàn)上述目的，本發(fā)明提供了一種機(jī)器設(shè)備端效器于生產(chǎn)作業(yè)系統(tǒng)的軌跡規(guī)劃與控制方法，運(yùn)用于具有一輸送帶及一機(jī)器設(shè)備的一端效器的一生產(chǎn)作業(yè)系統(tǒng)，其中該輸送帶輸送一待處理物，該待處理物于該輸送帶上設(shè)定有一目標(biāo)坐標(biāo)系，該軌跡規(guī)劃與控制方法包括：

a)取得該目標(biāo)坐標(biāo)系的一原點(diǎn)位置及該端效器的一起始位置；

b)取得該目標(biāo)坐標(biāo)系的移動(dòng)速度及該端效器的移動(dòng)速度；

c)通過一直線軌跡規(guī)劃模塊依據(jù)該原點(diǎn)位置及該起始位置規(guī)劃一直線路徑；

d)通過一速度同步規(guī)劃模塊依據(jù)該目標(biāo)坐標(biāo)系的移動(dòng)速度及該端效器的移動(dòng)速度規(guī)劃一速度曲線；

e)依據(jù)該直線路徑與該速度曲線產(chǎn)生一控制命令；及

f)依據(jù)該控制命令控制該端效器朝該目標(biāo)坐標(biāo)系的方向移動(dòng)，并且于追上該目標(biāo)坐標(biāo)系時(shí)，該端效器的移動(dòng)速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度相等。

如上所述，其中該生產(chǎn)作業(yè)系統(tǒng)還包括一觸發(fā)裝置，并且該步驟a之前還包括一步驟a0：于該觸發(fā)裝置被觸發(fā)后執(zhí)行該步驟a至該步驟f，其中該觸發(fā)裝置于檢測該待處理物出現(xiàn)在該輸送帶上時(shí)被觸發(fā)。

如上所述，其中該步驟e系周期性地依據(jù)該直線路徑產(chǎn)生一點(diǎn)位坐標(biāo)，依據(jù)該速度曲線產(chǎn)生一偏移量(offset)，并依據(jù)該點(diǎn)位坐標(biāo)與該偏移量產(chǎn)生該控制命令。

如上所述，其中該目標(biāo)坐標(biāo)系為一靜止坐標(biāo)系，該目標(biāo)坐標(biāo)系的移動(dòng)速度為零，該偏移量為零。

如上所述，其中該目標(biāo)坐標(biāo)系為一動(dòng)坐標(biāo)系，該目標(biāo)坐標(biāo)系的移動(dòng)速度相等于該輸送帶的速度。

如上所述，其中該生產(chǎn)作業(yè)系統(tǒng)連接外部的一終端機(jī)，并且該軌跡規(guī)劃與控制方法還包括：步驟g)接受該終端機(jī)的控制以即時(shí)調(diào)整該輸送帶的速度。

如上所述，其中該步驟d包括下列步驟：

d1)判斷該目標(biāo)坐標(biāo)系的移動(dòng)速度是否改變；

d2)于該目標(biāo)坐標(biāo)系的移動(dòng)速度改變時(shí)，判斷該目標(biāo)坐標(biāo)系的移動(dòng)速度與該端效器的移動(dòng)速度是否相等；

d3)該速度同步規(guī)劃模塊于該目標(biāo)坐標(biāo)系的移動(dòng)速度與該端效器的移動(dòng)速度相等時(shí)進(jìn)入一已同步程序；及

d4)依據(jù)該已同步程序產(chǎn)生該速度曲線，其中依據(jù)該速度曲線產(chǎn)生的該偏移量不具有一位移落后補(bǔ)償值。

如上所述，其中該步驟d還包括下列步驟：

d4)該速度同步規(guī)劃模塊于該目標(biāo)坐標(biāo)系的移動(dòng)速度與該端效器的移動(dòng)速度不相等時(shí)進(jìn)入一追蹤程序；

d5)依據(jù)該追蹤程序產(chǎn)生該速度曲線，其中依據(jù)該速度曲線產(chǎn)生的該偏移量具有該位移落后補(bǔ)償值；

其中，該速度同步規(guī)劃模塊于該端效器的位移落后補(bǔ)償完畢，并且該端效器于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量追上該目標(biāo)坐標(biāo)系的位移量后進(jìn)入該已同步程序。

如上所述，其中該步驟f中，該端效器依據(jù)該偏移量從一端效器初始速度(ve(0))開始加速，于一第一時(shí)間(t1)到達(dá)一端效器第一速度(ve(t1))，于一第二時(shí)間(t2)到達(dá)一端效器最大速度(vemax)，并且于到達(dá)該端效器最大速度后開始減速，再于一第三時(shí)間(t3)到達(dá)一端效器第三速度(ve(t3))。

如上所述，其中該端效器第一速度與該端效器第三速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度相同，該端效器最大速度大于該目標(biāo)坐標(biāo)系的移動(dòng)速度，并且該端效器于該第三時(shí)間完成位移落后的補(bǔ)償。

本發(fā)明的有益效果為：

本發(fā)明依據(jù)輸送帶的實(shí)際速度來規(guī)劃端效器朝輸送帶上的待處理物移動(dòng)時(shí)采用的速度曲線，使得當(dāng)端效器追上待處理物時(shí)，端效器的移動(dòng)速度恰與待處理物的移動(dòng)相同。借此，在待處理物持續(xù)于輸送帶上移動(dòng)的狀況下，本發(fā)明的端效器可更精準(zhǔn)地對(duì)待處理物進(jìn)行取、放與加工等程序。

另，本發(fā)明于輸送帶的速度被改變時(shí)，可即時(shí)地修改上述的速度曲線，以對(duì)端效器進(jìn)行位移落后的補(bǔ)償。借此，可在不影響端效器確實(shí)地追隨輸送帶上的待處理物的前提下，允許管理者依照實(shí)際需求隨時(shí)于線上調(diào)整輸送帶的速度。

以下結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)描述，但不作為對(duì)本發(fā)明的限定。

附圖說明

圖1為本發(fā)明的第一具體實(shí)施例的生產(chǎn)作業(yè)系統(tǒng)架構(gòu)圖；

圖2為本發(fā)明的第一具體實(shí)施例的生產(chǎn)作業(yè)系統(tǒng)示意圖；

圖3為本發(fā)明的第一具體實(shí)施例的設(shè)備控制器架構(gòu)圖；

圖4為本發(fā)明的第一具體實(shí)施例的軌跡規(guī)劃與控制流程圖；

圖5為本發(fā)明的第一具體實(shí)施例的控制命令產(chǎn)生流程圖；

圖6為本發(fā)明的第一具體實(shí)施例的速度同步規(guī)劃模塊狀態(tài)流程圖；

圖7為本發(fā)明的第一具體實(shí)施例的速度曲線示意圖。

其中，附圖標(biāo)記：

1生產(chǎn)作業(yè)系統(tǒng)

11觸發(fā)裝置

12馬達(dá)編碼器

13輸送帶

14設(shè)備控制器

141直線軌跡規(guī)劃模塊

142速度同步規(guī)劃模塊

15端效器

2終端機(jī)

c1控制命令

s10～s20控制步驟

s200～s208產(chǎn)生步驟

s30～s48規(guī)劃步驟

a1第一面積

a2第二面積

l1速度曲線

t0初始時(shí)間

t1第一時(shí)間

t2第二時(shí)間

t3第三時(shí)間

vo(t0)目標(biāo)坐標(biāo)系初始速度

ve端效器速度

ve(t0)端效器初始速度

ve(t1)端效器第一速度

ve(t3)端效器第三速度

vemax端效器最大速度。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明的結(jié)構(gòu)原理和工作原理作具體的描述：

首請(qǐng)參閱圖1與圖2，分別為本發(fā)明的第一具體實(shí)施例的生產(chǎn)作業(yè)系統(tǒng)架構(gòu)圖及生產(chǎn)作業(yè)系統(tǒng)示意圖。本發(fā)明公開了一種生產(chǎn)作業(yè)系統(tǒng)的軌跡規(guī)劃與控制方法(以下簡稱為該方法)，該方法運(yùn)用于如圖1、圖2所示的生產(chǎn)作業(yè)系統(tǒng)1。但，本發(fā)明的該方法主要用于規(guī)劃并控制在任兩個(gè)坐標(biāo)系(動(dòng)坐標(biāo)系或靜止坐標(biāo)系)之間進(jìn)行移動(dòng)的軌跡，并不限定于該生產(chǎn)作業(yè)系統(tǒng)1上。

首先如圖1所示，本實(shí)施例中，該生產(chǎn)作業(yè)系統(tǒng)1主要具備一輸送帶13(conveyer)及一機(jī)器設(shè)備(例如機(jī)器人，圖未標(biāo)示)的一端效器15(end-effector)，該輸送帶13用于輸送一或多個(gè)物件，該端效器15受控制而朝向該輸送帶13上的該些物件移動(dòng)，以對(duì)該些物件進(jìn)行取、放或其他加工程序。

更具體地，該生產(chǎn)作業(yè)系統(tǒng)1還包括一馬達(dá)編碼器12及一設(shè)備控制器14，該馬達(dá)編碼器12電性連接該輸送帶13，該設(shè)備控制器14電性連接該端效器15及該馬達(dá)編碼器12。該輸送帶13借由一馬達(dá)(圖未標(biāo)示)的轉(zhuǎn)動(dòng)而運(yùn)作，該馬達(dá)編碼器12主要連接該馬達(dá)，以控制該馬達(dá)并記錄各項(xiàng)編碼器信息(例如馬達(dá)的轉(zhuǎn)角度數(shù))。本發(fā)明中，該馬達(dá)編碼器12于被觸發(fā)時(shí)將該編碼器信息傳輸至該設(shè)備控制器14，該設(shè)備控制器14可依據(jù)該編碼器信息計(jì)算該輸送帶13的速度，借此判斷該輸送帶13上的該些物件目前的位置。

請(qǐng)同時(shí)參閱圖2，于圖2的實(shí)施例中，該輸送帶13上有三個(gè)待處理物3，各該待處理物3分別于該輸送帶13上被設(shè)定一坐標(biāo)系，并具有一坐標(biāo)系原點(diǎn) o(x,y,z)。于下述說明中，將僅以其中一個(gè)該待處理物3，及該待處理物3的一目標(biāo)坐標(biāo)系為例，舉例說明。

具體地，若該輸送帶13正常運(yùn)作并帶動(dòng)該待處理物3于該輸送帶13上移動(dòng)，則該目標(biāo)坐標(biāo)系屬于動(dòng)坐標(biāo)系(即，原點(diǎn)o的位置會(huì)不斷改變)；反之，若該輸送帶13靜止不動(dòng)，則該目標(biāo)坐標(biāo)系屬于靜止坐標(biāo)系(即，原點(diǎn)o的位置不會(huì)改變)。于該目標(biāo)坐標(biāo)系為動(dòng)坐標(biāo)系的情況下，該目標(biāo)坐標(biāo)系的移動(dòng)速度(即，該待處理物3的移動(dòng)速度)與該輸送帶13的速度相同；于該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系的情況下，該目標(biāo)坐標(biāo)系的移動(dòng)速度為零。

該設(shè)備控制器14主要是接收該編碼器信息，判斷該目標(biāo)坐標(biāo)系的原點(diǎn)位置(即，該待處理物3的位置)，并控制該端效器15朝該目標(biāo)坐標(biāo)系的方向移動(dòng)。本發(fā)明的主要技術(shù)特征在于，該設(shè)備控制器14依據(jù)該目標(biāo)坐標(biāo)系的原點(diǎn)位置、該端效器15的起始位置、該目標(biāo)坐標(biāo)系的移動(dòng)速度及該端效器15的移動(dòng)速度進(jìn)行計(jì)算，以規(guī)劃該端效器15朝該目標(biāo)坐標(biāo)系的方向移動(dòng)所需采用的軌跡及速度。

承上所述，通過該設(shè)備控制器14的控制，該端效器15最終可到達(dá)該目標(biāo)坐標(biāo)系的原點(diǎn)位置，并且于到達(dá)該原點(diǎn)位置時(shí)，該端效器15與該目標(biāo)坐標(biāo)系具有相同的移動(dòng)速度。如此一來，該端效器15可較精準(zhǔn)地對(duì)該待處理物3進(jìn)行取、放、加工等程序。

值得一提的是，本發(fā)明中，該生產(chǎn)作業(yè)系統(tǒng)1還具有一觸發(fā)裝置11，電性連接該馬達(dá)編碼器12及該設(shè)備控制器14。該觸發(fā)裝置11主要是于檢測到該待處理物3出現(xiàn)在該輸送帶13時(shí)被觸發(fā)，并于觸發(fā)后控制該馬達(dá)編碼器12傳輸該編碼器信息至該設(shè)備控制器14。借此，該設(shè)備控制器14可在該觸發(fā)裝置11被觸發(fā)時(shí)獲得該目標(biāo)坐標(biāo)系當(dāng)下的原點(diǎn)位置。并且，若該目標(biāo)坐標(biāo)系為動(dòng)坐標(biāo)系，則該設(shè)備控制器14可依據(jù)該編碼器信息持續(xù)計(jì)算該目標(biāo)坐標(biāo)系目前的原點(diǎn)位置。

更具體地，如圖2所示，該觸發(fā)裝置主要可為朝向該輸送帶13的一出口(圖未標(biāo)示)設(shè)置的一攝影機(jī)。該攝影機(jī)持續(xù)拍攝該出口，并且于該待處理物3由該出口被送上該輸送帶13時(shí)被觸發(fā)。但，上述說明僅為一個(gè)具體實(shí)施例，不以此為限。

續(xù)請(qǐng)參閱圖3，為本發(fā)明的第一具體實(shí)施例的設(shè)備控制器架構(gòu)圖。本實(shí)施 例中，該設(shè)備控制器14主要包括一直線軌跡規(guī)劃模塊141及一速度同步規(guī)劃模塊142。該直線軌跡規(guī)劃模塊141主要接收該目標(biāo)坐標(biāo)系的一原點(diǎn)位置以及該端效器15的一起始位置，再依據(jù)該原點(diǎn)位置與該起始位置規(guī)劃一直線路徑。并且，該直線軌跡規(guī)劃模塊141依據(jù)該直線路徑周期性地輸出一點(diǎn)位坐標(biāo)(x,y,z)，以控制該端效器15進(jìn)行移動(dòng)。

值得一提的是，若該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系，則該端效器15可直接依據(jù)該直線路徑朝該目標(biāo)坐標(biāo)系的方向前進(jìn)(即，該速度同步規(guī)劃模塊142的輸出為零)。

該速度同步規(guī)劃模塊142主要接收該目標(biāo)坐標(biāo)系的移動(dòng)速度及該端效器15的移動(dòng)速度，再依據(jù)該二移動(dòng)速度規(guī)劃一速度曲線。并且，該速度同步規(guī)劃模塊142依據(jù)該速度曲線周期性地輸出一偏移量(offset)。

本實(shí)施例中，若該目標(biāo)坐標(biāo)系為動(dòng)坐標(biāo)系，則該目標(biāo)坐標(biāo)系的移動(dòng)速度相等于該輸送帶13的速度。于此情況下，無論該端效器15的移動(dòng)速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度是否相等，該設(shè)備控制器14皆必須同時(shí)依據(jù)該點(diǎn)位坐標(biāo)及該偏移量控制該端效器15進(jìn)行移動(dòng)。借此，可令該端效器15在朝向該目標(biāo)坐標(biāo)系的方向進(jìn)行直線移動(dòng)時(shí)，同時(shí)補(bǔ)償該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量。

反之，若該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系，則該目標(biāo)坐標(biāo)系的移動(dòng)速度為零。于此情況下，該速度同步規(guī)劃模塊142輸出的該偏移量為零，該設(shè)備控制器14僅依據(jù)該直線軌跡規(guī)劃模塊141輸出的該點(diǎn)位坐標(biāo)控制該端效器15朝該目標(biāo)坐標(biāo)系的方向進(jìn)行直線移動(dòng)。

本發(fā)明中，該設(shè)備控制器14主要依據(jù)該直線路徑及該速度曲線產(chǎn)生一控制命令c1，并依據(jù)該控制命令c1對(duì)該端效器15進(jìn)行控制，以令該端效器15可依據(jù)指定的軌跡與速度朝該目標(biāo)坐標(biāo)系移動(dòng)。更具體地，該設(shè)備控制器14系依據(jù)上述的點(diǎn)位坐標(biāo)及偏移量周期性地產(chǎn)生該控制命令c1，以一步步控制該端效器15朝該目標(biāo)坐標(biāo)系移動(dòng)，并且使該端效器15在追上該目標(biāo)坐標(biāo)系時(shí)，與該目標(biāo)坐標(biāo)系具有相同的移動(dòng)速度。

通過該直線軌跡規(guī)劃模塊141及該速度同步規(guī)劃模塊142，本發(fā)明的該方法可同時(shí)適用于：1.靜止坐標(biāo)系(該端效器15初始狀態(tài)為靜止)至靜止坐標(biāo)系(該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系)的軌跡規(guī)劃；2.靜止坐標(biāo)系(該端效器15初始狀態(tài)為 靜止)至動(dòng)坐標(biāo)系(該目標(biāo)坐標(biāo)系為動(dòng)坐標(biāo)系)的軌跡規(guī)劃；3.動(dòng)坐標(biāo)系(該端效器15初始狀態(tài)為移動(dòng))至靜止坐標(biāo)系(該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系)的軌跡規(guī)劃；及4.動(dòng)坐標(biāo)系(該端效器15初始狀態(tài)為移動(dòng))至動(dòng)坐標(biāo)系(該目標(biāo)坐標(biāo)系為動(dòng)止坐標(biāo)系)的軌跡規(guī)劃。

于上述實(shí)施例中，該速度同步規(guī)劃模塊142是直接接收該目標(biāo)坐標(biāo)系的移動(dòng)速度以進(jìn)行該速度曲線的規(guī)劃。然而于另一實(shí)施例中，該速度同步規(guī)劃模塊142也可直接接收該目標(biāo)坐標(biāo)系的該原點(diǎn)位置，并借由該原點(diǎn)位置的變化自行計(jì)算該目標(biāo)坐標(biāo)系的移動(dòng)速度，不加以限定。

值得一提的是，本發(fā)明中，該速度同步規(guī)劃模塊142會(huì)持續(xù)判斷該目標(biāo)坐標(biāo)系的移動(dòng)速度是否改變(即，該輸送帶13的速度是否改變)。當(dāng)判斷該目標(biāo)坐標(biāo)系的移動(dòng)速度改變，造成該端效器15與該目標(biāo)坐標(biāo)系的位移量有落差時(shí)，該速度同步規(guī)劃模塊142依據(jù)該端效器15當(dāng)前的移動(dòng)速度及該目標(biāo)坐標(biāo)系改變后的移動(dòng)速度重新規(guī)劃該速度曲線。并且，依據(jù)重新規(guī)劃的該速度曲線來周期性產(chǎn)生該偏移量，借以更新該控制命令c1，其中，該偏移量會(huì)具有一位移落后補(bǔ)償值。

如此一來，即使人員于該待處理物3的正常移動(dòng)過程中改變了該輸送帶13的速度，該端效器15仍可依據(jù)更新后的該控制命令c1補(bǔ)償于該目標(biāo)坐標(biāo)系的移動(dòng)方向上落后(或超前)的位移量，并于位移落后補(bǔ)償完畢后，與該目標(biāo)坐標(biāo)系保持相同的移動(dòng)速度。

如圖1所示，該生產(chǎn)作業(yè)系統(tǒng)1還可連接外部的一終端機(jī)2，較佳地，該生產(chǎn)作業(yè)系統(tǒng)1可通過網(wǎng)路連接遠(yuǎn)端的該終端機(jī)2。本實(shí)施例中，該終端機(jī)2可提供一控制介面(圖未標(biāo)示)，該控制界面提供該生產(chǎn)作業(yè)系統(tǒng)1的管理人員于線上對(duì)該輸送帶13的速度進(jìn)行調(diào)整。

如上所述，當(dāng)該輸送帶13的速度被改變后，該速度同步規(guī)劃模塊142會(huì)重新規(guī)劃該速度曲線，再依據(jù)重新規(guī)劃的該速度曲線產(chǎn)生該些周期性的偏移量，以進(jìn)行位移量的補(bǔ)償。因此在本發(fā)明中，該生產(chǎn)作業(yè)系統(tǒng)1可在不停止該輸送帶13的運(yùn)作的前提下，直接接受該終端機(jī)2的控制以即時(shí)調(diào)整該輸送帶13的速度。借此，可大幅降低傳統(tǒng)的生產(chǎn)作業(yè)系統(tǒng)為了調(diào)校端效器的移動(dòng)路徑與移動(dòng)速度，以令端效器可正確地追隨速度調(diào)整后的輸送帶，而必須先暫停該輸送帶13的運(yùn)作所帶來的損害。

續(xù)請(qǐng)參閱圖4，為本發(fā)明的第一具體實(shí)施例的軌跡規(guī)劃與控制流程圖。圖4所公開的方法主要運(yùn)用于如圖1、圖2所示的該生產(chǎn)作業(yè)系統(tǒng)1，但不加以限定。

首先，該生產(chǎn)作業(yè)系統(tǒng)1判斷該觸發(fā)裝置11是否被觸發(fā)(步驟s10)，具體地，該觸發(fā)裝置11于檢測到該待處理物3出現(xiàn)在該輸送帶13上時(shí)被觸發(fā)。于該觸發(fā)裝置11未被觸發(fā)前，該設(shè)備控制器14不規(guī)劃上述該直線路徑與該速度曲線。

該觸發(fā)裝置11被觸發(fā)后，該設(shè)備控制器14分別取得該待處理物3與該端效器15的位置信息(步驟s12)，并且，分別取得該待處理物3與該端效器15的速度信息(步驟s14)。具體地，該步驟s12中，該設(shè)備控制器14是借由該直線軌跡規(guī)劃模塊141取得該待處理物3的該目標(biāo)坐標(biāo)系的該原點(diǎn)位置以及該端效器15的該起始位置。并且，該步驟s14中，該設(shè)備控制器14是借由該速度同步規(guī)劃模塊142取得該目標(biāo)坐標(biāo)系的移動(dòng)速度以及該端效器15的移動(dòng)速度。

接著，該直線軌跡規(guī)劃模塊141依據(jù)該目標(biāo)坐標(biāo)系的該原點(diǎn)位置以及該端效器15的該起始位置規(guī)劃該直線路徑(步驟s16)，該速度同步規(guī)劃模塊依據(jù)該目標(biāo)坐標(biāo)系的移動(dòng)速度以及該端效器15的移動(dòng)速度規(guī)劃該速度曲線(步驟s18)。接著，該設(shè)備控制器14依據(jù)該直線路徑與該速度曲線產(chǎn)生該控制命令，并依據(jù)該控制命令控制該端效器15朝該目標(biāo)坐標(biāo)系移動(dòng)(步驟s20)。并且，當(dāng)該端效器15追上該目標(biāo)坐標(biāo)系時(shí)，該端效器15的移動(dòng)速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度相等。

請(qǐng)同時(shí)參閱圖5，為本發(fā)明的第一具體實(shí)施例的控制命令產(chǎn)生流程圖。圖5系用于進(jìn)一步詳細(xì)描述上述圖4中的該步驟s20。

如圖5所示，本實(shí)施例中，該設(shè)備控制器14借由該直線軌跡規(guī)劃模塊141系依據(jù)該直線路徑產(chǎn)生該點(diǎn)位坐標(biāo)(步驟s200)，借由該速度同步規(guī)劃模塊142依據(jù)該速度曲線產(chǎn)生該偏移量(步驟s202)，再依據(jù)該點(diǎn)位坐標(biāo)及該偏移量產(chǎn)生該控制命令(步驟s204)。并且，該設(shè)備控制器14依據(jù)該控制命令控制該端效器15朝該目標(biāo)坐標(biāo)系移動(dòng)(步驟s206)。

于該端效器15到達(dá)該目標(biāo)坐標(biāo)系的該原點(diǎn)位置前(即，該直線路徑未走完之前)，該設(shè)備控制器14會(huì)周期性地產(chǎn)生該點(diǎn)位坐標(biāo)；而于該目標(biāo)坐標(biāo)系停止移動(dòng)前，該設(shè)備控制器14會(huì)周期性地產(chǎn)生該偏移量。該設(shè)備控制器14系持續(xù) 判斷該生產(chǎn)作業(yè)系統(tǒng)1是否停止運(yùn)作(步驟s208)，并于該生產(chǎn)作業(yè)系統(tǒng)1停止運(yùn)作前持續(xù)執(zhí)行該步驟s200至該步驟s206。

續(xù)請(qǐng)參閱圖6，為本發(fā)明的第一具體實(shí)施例的速度同步規(guī)劃模塊狀態(tài)流程圖。圖6用于描述該速度同步規(guī)劃模塊141如何適當(dāng)?shù)匾?guī)劃該速度曲線并產(chǎn)生該偏移量。

首先，該設(shè)備控制器14判斷該速度同步規(guī)劃模塊142是否處于一閑置狀態(tài)(idle)(步驟s30)。具體地，當(dāng)該輸送帶13上不具有該待處理物3、該待處理物3停止移動(dòng)(即，該目標(biāo)坐標(biāo)系為靜止坐標(biāo)系)或是該待處理物3已處理完成而該端效器15不需繼續(xù)跟隨時(shí)，該速度同步規(guī)劃模塊142處于該閑置狀態(tài)(步驟s30)。

于該閑置狀態(tài)中，該速度同步規(guī)劃模塊142持續(xù)接收該編碼器信息，以判斷該待處理物3是否出現(xiàn)在該輸送帶13上，或是已存在的該待處理物3的移動(dòng)速度是否改變(例如，該目標(biāo)坐標(biāo)系由靜止坐標(biāo)系轉(zhuǎn)變?yōu)閯?dòng)坐標(biāo)系)。于上述任一條件判斷為是時(shí)，該速度同步規(guī)劃模塊142離開該閑置狀態(tài)。

于離開該閑置狀態(tài)后，該速度同步規(guī)劃模塊142判斷該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的移動(dòng)速度(ve)與該目標(biāo)坐標(biāo)系目前的移動(dòng)速度(vo)是否相等(步驟s32)。

若該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的移動(dòng)速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度相等，表示該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量與該目標(biāo)坐標(biāo)系的位移量不具有位移落差(即，每次移動(dòng)的位移量相等)。因此，該速度同步規(guī)劃模塊142直接進(jìn)入一已同步程序(synchro)，依據(jù)該已同步程序規(guī)劃該速度曲線(步驟s40)，并且依據(jù)該速度曲線產(chǎn)生該偏移量(步驟s42)。如上所述，由于該端效器15與該目標(biāo)坐標(biāo)系沒有位移落差，因此該速度同步規(guī)劃模塊142依據(jù)在該步驟s40中規(guī)劃的該速度曲線所產(chǎn)生的該偏移量中，將不會(huì)具有該位移落后補(bǔ)償值。

若于該步驟s32中，該速度同步規(guī)劃模塊142判斷該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的移動(dòng)速度與該目標(biāo)坐標(biāo)系的移動(dòng)速度不同，則該速度同步規(guī)劃模塊142進(jìn)入一追蹤程序(chasing)，依據(jù)該追蹤程序產(chǎn)生該速度曲線(步驟s34)，并且依據(jù)該速度曲線產(chǎn)生該偏移量(步驟s36)。由于移動(dòng)速度上的差異，該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量將會(huì)與該目標(biāo)坐標(biāo)系 的位移量有落差，因此該步驟s36中產(chǎn)生的該偏移量將具有該位移落差補(bǔ)償值。

具體而言，于該步驟s34中，該速度同步規(guī)劃模塊142是依據(jù)該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量(pe)與該目標(biāo)坐標(biāo)系的位移量(po)的差異，以及該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的移動(dòng)速度(ve)與該目標(biāo)坐標(biāo)系的移動(dòng)速度(vo)之間的差異，規(guī)劃出該速度曲線，使得依據(jù)該速度曲線所產(chǎn)生的該偏移量具有該位移落后補(bǔ)償值。

該設(shè)備控制器14依據(jù)該步驟s36中產(chǎn)生的該偏移量來產(chǎn)生該控制命令c1，并且判斷該端效器15的位移落后是否補(bǔ)償完畢(步驟s38)。即，判斷該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量與該目標(biāo)坐標(biāo)系的位移量是否相等(pe＝po)。若該端效器15的位移落后尚未補(bǔ)償完畢，則回到該步驟s36，該設(shè)備控制器14持續(xù)依據(jù)具有該位移落后補(bǔ)償值的該偏移量產(chǎn)生該控制命令c1。若該端效器15的位移落后補(bǔ)償完畢，則該速度同步規(guī)劃模塊142進(jìn)入該已同步程序，并執(zhí)行上述步驟s40及該步驟s42。

該步驟s42后，該速度同步規(guī)劃模塊142判斷該目標(biāo)坐標(biāo)系的移動(dòng)速度是否改變(步驟s44)，并且于該目標(biāo)坐標(biāo)系的移動(dòng)速度改變時(shí)，再次進(jìn)入該追蹤程序并執(zhí)行上述該步驟s34至該步驟s42。

若該目標(biāo)坐標(biāo)系的移動(dòng)速度沒有改變，該速度同步規(guī)劃模塊142判斷該端效器15是否要停止跟隨該目標(biāo)坐標(biāo)系(步驟s46)，例如，該目標(biāo)坐標(biāo)系是否轉(zhuǎn)變?yōu)殪o止坐標(biāo)系，或該待處理物3是否已處理完畢。若該端效器15要停止跟隨該目標(biāo)坐標(biāo)系，則該速度同步規(guī)劃模塊142返回該閑置狀態(tài)。

若于該步驟s46中判斷為否，則該速度同步規(guī)劃模塊142進(jìn)一步判斷該生產(chǎn)作業(yè)系統(tǒng)1的運(yùn)作是否終止(步驟s48)，并且于該生產(chǎn)作業(yè)系統(tǒng)1終止前，回到該步驟s42。

參閱圖7，為本發(fā)明的第一具體實(shí)施例的速度曲線示意圖。圖7中以一個(gè)具體實(shí)施例來說明上述的速度曲線l1，但此僅為一范例，該速度同步規(guī)劃模塊142的運(yùn)作不以此為限。于本實(shí)施例中，系設(shè)定該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的移動(dòng)速度為“ve”，而該目標(biāo)坐標(biāo)系的移動(dòng)速度為“vo”。

如圖7所示，假定該端效器15的一端效器初始速度ve(t0)為零，而該目標(biāo)坐標(biāo)系的一目標(biāo)坐標(biāo)系初始速度vo(t0)為一個(gè)定速。由于此時(shí)該端效器初始 速度ve(t0)與該目標(biāo)坐標(biāo)系的移動(dòng)速度vo不同，因此該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量(pe)與該目標(biāo)坐標(biāo)系的位移量(po)會(huì)具有落差。為了補(bǔ)償該位移落差，該速度同步規(guī)劃模塊142會(huì)進(jìn)入該追蹤程序。

當(dāng)該速度同步規(guī)劃模塊142處于該追蹤程序中時(shí)，該控制命令c1中的該偏移量會(huì)具有該位移落后補(bǔ)償值。該端效器15受該控制命令c1控制開始加速，并且于一第一時(shí)間t1到達(dá)一端效器第一速度ve(t1)，其中該端效器第一速度ve(t1)與該待處理物3的移動(dòng)速度vo相同。然而，由于此時(shí)該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量pe(t1)還沒追上該目標(biāo)坐標(biāo)系的位移量po(t1)，因此該端效器15需持續(xù)加速，以補(bǔ)足位移的落后。該端效器15接著于一第二時(shí)間t2到達(dá)一端效器最大速度vemax，其中該端效器最大速度vemax大于該目標(biāo)坐標(biāo)系的移動(dòng)速度vo。

當(dāng)該端效器15到達(dá)該端效器最大速度vemax后，該端效器15會(huì)開始減速，并于一第三時(shí)間t3到達(dá)一端效器第三速度ve(t3)。于該第三時(shí)間t3時(shí)，該端效器第三速度ve(t3)與該待處理物3的移動(dòng)速度vo相同，并且該端效器于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量pe(t3)追上了該目標(biāo)坐標(biāo)系的位移量po(t3)。此時(shí)，該端效器15的位移落后補(bǔ)償完畢，該速度同步規(guī)劃模塊142進(jìn)入該已同步程序。

值得一提的是，如圖7所示，在本實(shí)施例的該速度曲線l1中，由該端效器第一速度ve(t1)、該端效器最大速度vemax及該端效器第三速度ve(t3)所構(gòu)成的一第一面積a1，與由該端效器初始速度ve(t0)、該目標(biāo)坐標(biāo)系初始速度vo(t0)及該端效器第一速度ve(t1)所構(gòu)成的一第二面積a2相同。通過上述規(guī)則，該速度同步規(guī)劃模塊142可快速計(jì)算該端效器最大速度vemax，進(jìn)而規(guī)劃出該速度曲線l1。如此一來，該生產(chǎn)作業(yè)系統(tǒng)1可確保在該端效器15于該目標(biāo)坐標(biāo)系的移動(dòng)方向上的位移量pe追上該目標(biāo)坐標(biāo)系的位移量po時(shí)，該端效器15的移動(dòng)速度恰好與該目標(biāo)坐標(biāo)系的移動(dòng)速度相同。

當(dāng)然，本發(fā)明還可有其它多種實(shí)施例，在不背離本發(fā)明精神及其實(shí)質(zhì)的情況下，熟悉本領(lǐng)域的技術(shù)人員當(dāng)可根據(jù)本發(fā)明作出各種相應(yīng)的改變和變形，但這些相應(yīng)的改變和變形都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護(hù)范圍。