TWI728757B - 直接姿態回授控制方法及直接姿態回授控制機器 - Google Patents

Abstract

本發明提供一種直接姿態回授控制方法及直接姿態回授控制機器，藉由直接量測工具相對工件之實際姿態，進行姿態誤差補償，消除機器結構及驅動機構中之幾何誤差及熱誤差。姿態回授控制機器在工具端及基本端設置有雙球桿姿態量測機構，以之量出工具運動或定位時之實際姿態，在編碼器定義姿態與設定姿態間得出追蹤姿態誤差，在實際姿態與編碼器定義姿態間得出連結姿態誤差，控制器對追蹤姿態誤差進行位置回授控制，對連結姿態誤差進行姿態補償控制，如此，直接姿態回授控制機器的幾何誤差及熱誤差被消除。

Description

直接姿態回授控制方法及直接姿態回授控制機器

本發明是有關於一種回授控制方法及回授控制機器，且尤其是有關一種直接姿態回授控制方法及直接姿態回授控制機器。

CNC工具機或工業用關節機器的驅動機構中之線性軸或轉動軸作定位運動時會有定位誤差，一般以六個誤差項來表示，以CNC工具機之X軸或工作台做線性定位運動為例，定位誤差包含工作台在X、Y、Z三個方向出現之線性誤差，及對X、Y、Z軸轉動之翻滾(Roll)、俯仰(Pitch)及偏擺(Yaw)轉動誤差，驅動機構中軸與軸間也有垂直度或平行度誤差。理想工業用關節機器之手臂中各個轉動軸間之機構參數可用例如DH機構參數加以描述，但是因為製造及組裝因素，各個機構參數也有幾何誤差。因此，如何有效地減少機器之定位誤差，一直是業者努力的目標。

機器除了在驅動機構中有幾何誤差，在操作時由於受到內部熱源及外部環境溫度變化或熱源影響，機器會出現熱變形，內部熱源包括運動元件間之摩擦，例如主軸軸承、滾珠螺桿螺帽、導軌等，伺服馬達能量轉換時會產生熱量，外部熱源例如是環境溫度、操作人員之體溫或日光照射。要提高機器之精度，必須排除或降低機器之熱誤差，例如以溫度感測器量出機器結構及元件或組件之實際溫度，由熱誤差模型算出機器之熱誤差並加以補償。

機器結構及驅動機構中的幾何誤差及熱誤差最終造成工具相對工件運動或定位時有姿態誤差，包括位置誤差及指向誤差，先進的控制器中可以建立機器的幾何誤差模型，例如以雷射干涉儀量出三線性軸機器之21項幾何誤差，進行機器之幾何誤差補償，但是這21項幾何誤差會因機器之溫度狀態而變化，影響補償效果。

在美國專利US5681981A、US6226884B1及US6662461B2中可見具史都華平台機構(Stewart Platform Mechanism)之量測裝置及方法，其中第一平台及第二平台以六支可伸縮量測桿連接形成平行機構，藉由量測各量測桿之長度，在量測電腦內算出第一平台相對第二平台間之座標關係。在美國專利US7810248B2中可見一種座標量測機器，其中連接二平台間之伸縮桿有特殊安排，形成 有利於工件進出之工作空間。在美國專利US2019/0072373A1中可見一種六連桿座標量測機器，其中連接二平台間之伸縮桿有氣壓軸承設計。

這些具有史都華平台機構之量測裝置或機器有諸多缺點，由於機構參數有幾何誤差及熱誤差，使這些量測裝置之量測精度不高，應用有限。在同一申請人之台灣專利TWI585363B「應用於量測之雙球桿系統及其誤差補償方法」中提出針對雙球桿之幾何誤差及熱誤差補償分法，然仍有其缺失存在。

為了提供更精準的機器，本發明提供一種直接姿態回授控制方法及直接姿態回授控制機器。

依據本發明之一態樣之一實施方式，提供一種直接姿態回授控制方法，應用於一直接姿態回授控制機器，直接姿態回授控制機器包含一控制器、一雙球桿姿態量測機構、複數馬達及一工具，雙球桿姿態量測機構設置於直接姿態回授控制機器之一基本端及一工具端間且包含一工具端平台設置於工具端、一基本端平台設置於基本端、至少六支工作雙球桿連接工具端平台及基本端平台、以及至少一參考雙球桿設置於一參考板，工具端平台包含至少三球窩，基本端平台包含至少三球窩，直接姿態回授控制方法以一姿態補償控制配合一位置回授控制，姿態補償控制包含一 起始化步驟、一座標轉換關係建立步驟、一實際姿態計算步驟及一位置補償值計算步驟。於起始化步驟中，使控制器建立前述至少一參考雙球桿及各工作雙球桿之一中心距離函數，及建立雙球桿姿態量測機構之一工具平台座標系與一基本平台座標系間之座標轉換關係。於座標轉換關係建立步驟中，使控制器依據一內建或是載入之設定值或一機上量測，建立基本平台座標系與一參考座標系的座標轉換關係。於實際姿態計算步驟中，使控制器讀入各工作雙球桿之一工作位移量測值，計算工具的一實際姿態。於位置補償值計算步驟中，使控制器依一補償目標姿態及實際姿態得出各馬達的一增量位置補償值及一位置補償值，其中，姿態補償控制使用各馬達之補償控制器，由各馬達之位置補償值得出補償驅動值，位置回授控制使用各馬達之位置控制器，由位置設定值與位置實際值之差，得出驅動值，驅動值與補償驅動值相加後輸出給各馬達的驅動器，各馬達的位置補償值維持有效，直到姿態補償控制更新為止。

依據前述之直接姿態回授控制方法的複數實施例，其中，姿態補償控制之取樣時間大於等於位置回授控制之取樣時間。

依據前述之直接姿態回授控制方法的複數實施例，其中，補償目標姿態為編碼器定義姿態或是設定姿態。

依據前述之直接姿態回授控制方法的複數實施例，其中，以賈可比逆矩陣得出各馬達之增量位置補償值。

依據前述之直接姿態回授控制方法的複數實施例，其中，由各馬達之編碼器定義姿態位置值減去各馬達之等效位置實際值，得出各馬達之增量位置補償值。

依據前述之直接姿態回授控制方法的複數實施例，其中，補償控制器是比例控制器。

依據前述之直接姿態回授控制方法的複數實施例，其中，各馬達之補償控制器與位置控制器相同，此時以各馬達之位置補償值與各馬達之位置設定值相加後，輸入位置控制器得出補償後之驅動值。

依據前述之直接姿態回授控制方法的複數實施例，其中，對各馬達之位置補償值進行低通濾波處理，得出無振動因素之位置補償值。

依據前述之直接姿態回授控制方法的複數實施例，其中，於起始化步驟中，驅動至少一馬達使各工作雙球桿過原點以起始化控制器內建或是載入的各工作雙球桿的中心距離函數。

依據前述之直接姿態回授控制方法的複數實施例，其中，機上量測包括分別驅動至少二馬達定位，由雙球桿姿態量測機構之量測結果得出參考座標系，及得出基本平台座標系與參考座標系的座標轉換關 係。

依據前述之直接姿態回授控制方法的複數實施例，其中，於位置補償值計算步驟中，使用一次逼近方法或是迭代多次逼近方法計算得出各馬達之增量位置補償值。

依據前述之直接姿態回授控制方法的複數實施例，其中，控制器讀入參考雙球桿之熱誤差量測值，由熱誤差量測值計算並補償雙球桿姿態量測機構各個機構參數之熱誤差，包括工具端平台及基本端平台上球窩位置之熱誤差及工作雙球桿中心距離之熱誤差。

依據前述之直接姿態回授控制方法的複數實施例，其中，實際姿態計算步驟中，控制器讀出被一觸發訊號同步栓鎖之各工作位移量測值，算出工具末端量測球的中心之座標位置。

依據前述之直接姿態回授控制方法的複數實施例，其中，直接回授控制機器更包含安裝基本端平台之一支架，且於實際姿態計算步驟中，依參考雙球桿的熱誤差量測值計算並補償支架高度之熱誤差。

依據前述之直接姿態回授控制方法的複數實施例，其中，於實際姿態計算步驟中，依參考雙球桿的熱誤差量測值計算並補償工具長度之熱誤差。

依據前述之直接姿態回授控制方法的複數實施例，其中，控制器對實際姿態進行頻譜分析，得出直接回授控制機器之振動資訊。

依據本發明之另一態樣之一實施方式提供一種直接姿態回授控制機器，其包含一機器本體、複數馬達、一控制器及一雙球桿姿態量測機構，機器本體包含一工具端及一基本端，且工具端設置工具，馬達設置於機器本體，控制器驅動馬達，使工具相對基本端之一工件運動，雙球桿姿態量測機構設置於基本端與工具端之間且包含一工具端平台、一基本端平台、至少六支工作雙球桿、一參考板及至少一參考雙球桿，工具端平台位於工具端且包含至少三球窩；基本端平台位於基本端且包含至少三球窩，至少六支工作雙球桿，連接於工具端平台的球窩及基本端平台的球窩間，各工作雙球桿包含二量測球，以各工作雙球桿的位移感測器量出二量測球的中心間之距離，參考板包含至少二參考球窩，參考雙球桿包含二參考量測球，設置於參考板上至少二參考球窩之間，以一位移感測器量出二參考量測球中心間之距離，其中，控制器以雙球桿姿態量測機構作為姿態誤差之量測裝置，以參考雙球桿作為雙球桿姿態量測機構機構參數熱誤差之量測裝置，由雙球桿姿態量測機構量測得出工具之實際姿態及連結姿態誤差，及各馬達之位置補償值或是補償驅動值，消除工具之連結姿態誤差。

依據前述之直接姿態回授控制機器的複數實施例，可更包含一主軸座及一主軸，主軸座設置於機器本體，主軸連接於主軸座且包含一主軸外殼，其中， 工具端平台固定於主軸座或主軸外殼上。

依據前述之直接姿態回授控制機器的複數實施例，其中，直接姿態回授控制機器可以為一多軸工具機、一多軸關節機器或一平行連桿驅動機器。

依據前述之直接姿態回授控制機器的複數實施例，其中，工具可以為一刀具、一量測探頭、一觸發探頭、一共軛聚焦傳感器、一位移量測雷射或是一光學量測鏡頭。

依據前述之直接姿態回授控制機器的複數實施例，其中，工具可以產生一觸發訊號，以觸發訊號同步栓鎖各工作雙球桿之工作位移量測值。

依據前述之直接姿態回授控制機器的複數實施例，可更包含一參考球，其設置於基本端平台或是工作台上。

依據前述之直接姿態回授控制機器的複數實施例，可更包含一刀長量測機構，其設置於基本端平台或是工作台。

依據前述之直接姿態回授控制機器的複數實施例，其中，工具端平台及基本端平台上之複數球窩可以有不對稱分佈。

依據前述之直接姿態回授控制機器的複數實施例，其中，基本端平台及工具端平台上的球窩可固定於熱脹係數介於0ppm/℃(μm/m/℃)與2ppm/℃間之一材料上。

依據前述之直接姿態回授控制機器的複數實施例，可以更包含一支架，基本端平台安裝於支架上。

依據前述之直接姿態回授控制機器的複數實施例，其中，控制器可以包含一即時通訊網路，各工作雙球桿及參考雙球桿藉由即時通訊網路回傳各工作位移量測值及熱誤差量測值。

依據前述之直接姿態回授控制機器的複數實施例，其中，即時通訊網路是即時乙太通訊網路(EtherCAT)。

依據前述之直接姿態回授控制機器的複數實施例，其中，各工作雙球桿及參考雙球桿之位移感測器可以為一雷射干涉儀、一光學尺或一磁性尺。

10,10a,10b:直接姿態回授控制機器

11:底座

11a,11b,11d:六軸機器手臂

12,12a,12b:工作台

13b:支架

110,110a,110b:工件

120:墊高塊

120a:工件安裝座

130:刀長量測裝置

131a,131b:參考球

200b:控制器

210b:視窗操作模組

211b:人機介面

220b:運動控制核心模組

221b:解譯器

222b,222d:插補器

2221d:路徑插補器

2222d:指向插補器

223b,223d:加減速規劃器

224d:逆轉換器

225d:正轉換器

226d:時間延遲元件

227d:驅動系統

228d:積分器

230b:即時通訊網路

240b:驅動器

250b:數位計數器

3:Z運動平台

300,300a,300b,300c,300d:雙球桿姿態量測機構

310,310a,310b,310c:基本端平台

311,311a,311b:定位層

312,312a,312b:支承層

320,320a,320b,320c:工具端平台

321,321a,321b:定位層

322,322a,322b:支承層

330,330a,330b,330c:工作雙球桿

4:主軸座

400,400a,400b:參考雙球桿

5:主軸

500,500a,500b:參考板

510,520:磁性球窩

6,6a,6b:工具

800b:馬達

B₁~B₆:球窩

P₁~P₆:球窩

Y:軸

{B}:基本平台座標系

{P₀}:起始工具平台座標系

{P_n}:實際工具平台座標系

{R}:參考座標系

91:位置回授控制

911:位置控制器

92:姿態補償控制

921:補償控制器

S01:起始化步驟

S02:座標轉換關係建立步驟

S03:實際姿態計算步驟

S04:位置補償值計算步驟

FF:前饋控制

J ^-1:賈可比逆矩陣

P _A :實際姿態

P _E :編碼器定義姿態

P _r :參考姿態

P _S :設定姿態

E _a :實際姿態誤差

E _l :連結姿態誤差

E _f :追蹤姿態誤差

V _cmd,i :馬達驅動值

θ _i,a ,θ_1,a ,θ_6,a :等效位置實際值

θ _i,E ,θ_1,E ,θ_6,E :編碼器定義姿態位置值

θ _i,s ,θ_1,s ~θ_6,s :位置設定值

θ _i,enc ,θ_1,enc ,θ_6,enc :位置實際值

△θ _i,comp ,△θ_1,comp ,△θ_6,comp :增量位置補償值

θ _i,comp ,θ_1,comp ,θ_6,comp :位置補償值

:工具平台座標系與基本平台座標系的座標轉換關係

:實際工具平台座標系與基本平台座標系的座標轉換關係

:實際工具端點座標系與基本平台座標系的座標轉換關係

:工具端點座標系與基本平台座標系的座標轉換關係 ^R T _B :基本平台座標系與參考座標系的座標轉換關係

:工具端點座標系與參考座標系的座標轉換關係

:工具端點座標系與工具平台座標系的座標轉換關係

:編碼器定義姿態下實際工具端點座標系與參考座標系間的座標轉換關係

:實際工具端點座標系與實際工具平台座標系的座標轉換關係

:實際工具端點座標系與參考座標系的座標轉換關係

第1圖繪示依照本發明一實施例之一種直接姿態回授控制機器之示意圖；

第2圖繪示依照本發明另一實施例之一種直接姿態回授控制機器之示意圖；

第3圖繪示依照本發明又一實施例之一種直接姿態回授控制機器之示意圖；

第4圖繪示第3圖實施例之直接姿態回授控制機器之控制架構方塊示意圖；

第5圖繪示依照本發明之一種直接姿態回授控制方法的 方塊流程圖；

第6圖繪示第5圖實施例之直接姿態回授控制方法之工具的姿態示意圖；

第7圖繪示第5圖實施例之直接姿態回授控制方法的一控制流程圖；

第8圖繪示第5圖實施例之直接姿態回授控制方法的另一控制流程圖；

第9圖繪示第5圖實施例之直接姿態回授控制方法的又一控制流程圖；

第10圖繪示第5圖實施例之直接姿態回授控制方法的再一控制流程圖；以及

第11圖繪示依照本發明更一實施例之一種直接姿態回授控制機器的雙球桿姿態量測機構示意圖。

以下將參照圖式說明本發明之實施例。為明確說明起見，許多實務上的細節將在以下敘述中一併說明。然而，閱讀者應瞭解到，這些實務上的細節不應用以限制本發明，也就是說，在本發明部分實施例中，這些實務上的細節是非必要的。此外，為簡化圖式起見，一些習知慣用的結構與元件在圖式中將以簡單示意的方式繪示；並且重複之元件將可能使用相同的編號或類似的編號表示。

此外，本文中當某一元件(或機構或模組等) 「連接」、「設置」或「耦合」於另一元件，可指所述元件是直接連接、直接設置或直接耦合於另一元件，亦可指某一元件是間接連接、間接設置或間接耦合於另一元件，意即，有其他元件介於所述元件及另一元件之間。而當有明示某一元件是「直接連接」、「直接設置」或「直接耦合」於另一元件時，才表示沒有其他元件介於所述元件及另一元件之間。而第一、第二、第三等用語只是用來描述不同元件或成分，而對元件/成分本身並無限制，因此，第一元件/成分亦可改稱為第二元件/成分。且本文中之元件/成分/機構/模組之組合非此領域中之一般周知、常規或習知之組合，不能以元件/成分/機構/模組本身是否為習知，來判定其組合關係是否容易被技術領域中之通常知識者輕易完成。

本發明之一目的在於提供一種直接姿態回授控制方法，藉由直接量測工具相對工件之實際姿態，亦即工具端點的實際位置及工具的實際指向，得出姿態誤差，驅動馬達進行姿態補償控制，以消除機器結構及驅動機構中之幾何誤差及熱誤差。

應用本發明之直接姿態回授控制方法的機器可稱為直接姿態回授控制機器，直接姿態回授控制機器的驅動機構可以是串聯式或是並聯式，以複數個馬達來驅動工具相對工件運動，直接姿態回授控制機器可以是多軸工具機或多軸關節機器手臂，也可以是平 行連桿驅動機器(或稱平行機構機器)。在直接姿態回授控制機器之工具端及基本端間設置有一雙球桿姿態量測機構，雙球桿姿態量測機構包括安裝在工具端之一工具端平台、安裝在基本端之一基本端平台及至少六個工作雙球桿。工具端平台及基本端平台各包含至少三個球窩；六個工作雙球桿連接工具端平台上之球窩及基本端平台上之球窩。雙球桿姿態量測機構可以另外包括一參考板及至少一支參考雙球桿，參考板包含複數個參考球窩，至少一支參考雙球桿被置於參考板之二參考球窩上，此參考雙球桿用於雙球桿姿態量測機構的機構參數熱誤差補償，直接姿態回授控制機器之控制器以此雙球桿姿態量測機構，量出工具運動或定位時之實際姿態，進行直接姿態回授控制。

在本發明之直接姿態回授控制機器，不針對機器結構及驅動機構中之幾何誤差及熱誤差配置個別之感測器，而是在工具端及基本端設置雙球桿姿態量測機構，機器結構及驅動機構中各個零組件之幾何誤差、熱誤差所造成工具相對工件之總成六維姿態誤差，由此雙球桿姿態量測機構量測得出，然後得出各馬達之位置補償值，補償控制器輸入位置補償值產生馬達之補償驅動值，與位置控制器產生之馬達之驅動值相加，輸出至驅動器驅動馬達，使本發明之直接姿態回授控制機器成為無機構參數誤差、無幾何誤差及無熱誤差之理想機器。

做為直接姿態回授控制之量測裝置，雙球桿姿態量測機構之量測精度必須非常高，位移及轉動角度之量測精度最好是微米及角秒(arc-second)等級或是更好。雙球桿姿態量測機構本身也有幾何誤差及熱誤差，包括工作雙球桿之幾何誤差及熱誤差，及工具端平台及基本端平台上球窩中心的位置誤差及熱誤差，習知之並聯量測機構不能準確消除這些誤差，精度有限。

工作雙球桿要由其位移量測值得出其二末端量測球中心間之絕對距離，簡稱中心距離，為了提升雙球桿姿態量測機構之精確度，本發明之直接回授控制機器中，控制器可以內建或是載入各個工作雙球桿之中心距離函數，此中心距離函數以工作雙球桿之位移量測值為輸入，以其準確之中心距離為輸出，於起始化步驟中啟動各工作雙球桿之中心距離函數。此中心距離函數可以藉助一校正板而建立，此校正板上設置有多個磁座，特定二磁座間之中心距離為已知，校正板提供至少二個已知之中心距離，校正時將工作雙球桿置於二已知中心距離之磁座上，此時，工作雙球桿之位移量測值與此已知之中心距離構成一校正點，以至少二校正點可以建立工作雙球桿之中心距離函數。校正點數量越多，精度也越好。

本發明可以另外提供設置於參考板上之至少一參考雙球桿，依據此參考雙球桿之熱誤差量測值得 出因環境溫度變化所造成之工作雙球桿的熱誤差，由控制器加以補償。另一方面，更依據參考雙球桿之熱誤差量測值得出工具端平台及基本端平台上各個球窩中心位置的熱誤差，並加以補償，如此，在本發明之直接姿態回授控制機器中，雙球桿姿態量測機構中所有機構參數之幾何誤差及熱誤差均被補償，因而環境溫度變化在5℃內時，雙球桿姿態量測機構之量測精度仍然可以達到微米及角秒等級。在本發明之直接姿態回授控制機器中，也針對工具之熱誤差，例如刀具或是量測探頭長度之熱誤差，依據參考雙球桿之熱誤差量測值加以補償，藉由本發明中之設計及方法，使本發明之直接姿態回授控制機器有微米及角秒等級之定位精度。

參考雙球桿之中心距離函數可以是內建於控制器中或是起始化時載入，當工作雙球桿或是參考雙球桿是使用絕對式位移感測器時，在機器開機後控制器可以直接啟動其中心距離函數；當工作雙球桿是使用增量式位移感測器時，開機後控制器可以藉由驅動機器例如走Z軸，使各個工作雙球桿過原點而啟動其中心距離函數。參考雙球桿是使用增量式位移感測器時，由於其不具驅動機構，要有額外手段，例如以手動或是輔助裝置使之過原點，以啟動其中心距離函數。

直接姿態回授控制機器可以是多軸工具機， 也可以是多軸關節機器，機器上安裝使用之工具可以是主軸帶動之刀具，例如銑刀，也可以是固定不動之量具，包括量測探頭、共軛聚焦傳感器、雷射位移量測器、光學量測鏡頭等。為精準計算工具端點之位置與工具指向，控制器要同步讀出所有工作雙球桿之工作位移量測值，其可藉由即時計時器(real-time timer)之定時中斷進行，當工具是量測探頭時，可另外藉由一觸發訊號同步栓鎖所有工作雙球桿之工作位移量測值，再由控制器讀出。工具是量測探頭時，在基本端平台或是置放工件之工作台上可另外設置一參考球，以校正探頭末端量測球的中心座標位置；而若工具是刀具時，基本端平台或是置放工件之工作台上可以另外設置一刀長量測裝置，用以校正刀長。

在本發明之直接姿態回授控制機器中，工具端平台及基本端平台可以是獨立設計，藉由適當之轉接件分別安裝於直接姿態回授控制機器的工具端及基本端，另外也可以與直接姿態回授控制機器進行一體設計，要點是，工具端平台必須可以靈敏反應出機器之幾何誤差及熱誤差所造成之工具端點位移及工具指向變化，基本端平台則必須考慮工件的進出及固定方式。在一實施例中，直接姿態回授控制機器是有主軸之多軸工具機，工具端平台是經由轉接件固定在主軸座的端面或是主軸外殼上，可以隨同主軸之熱變形而產生位移及偏轉。在另一實施例中，直接姿態回授控 制機器是多軸關節機器手臂，工具端平台是固定在工具安裝座上，工具則是固定在工具端平台上。

工具端平台及基本端平台各包含三個或六個球窩，當工具端平台及基本端平台各包含三個球窩時，一球窩上可連接二個工作雙球桿；當工具端平台及基本端平台各包含六個球窩時，一個球窩上可連接一支工作雙球桿，且工具端平台及基本端平台共設置至少六個工作雙球桿，而構成史都華平台機構。對標準史都華平台機構而言，工具端平台及基本端平台上六個球窩之位置是對稱周期分佈，例如以20度及100度之角度間隔，周期分佈於一圓周上。為避免各工作雙球桿間或是與機器零組件之干涉，或是為了增加特定方向之工作行程，也可以刻意偏移一些球窩之位置，而各個工作雙球桿之最大中心距離及最小中心距離則與球窩之分佈相配合，可以互不相同。在一實施例中，基本端平台上的六個球窩是不對稱分佈於一半圓圓周範圍，以避開Z運動平台及主軸座，工具端平台上之六個球窩也有對應之位置偏移。

工具端平台及基本端平台上之球窩中可設有磁鐵，而允許工作雙球桿末端之球吸附於球窩上，以構成球軸承結構。球窩可以例如是以三點或是以圓弧支撐吸附之球窩，此時球窩與球之間有滑動接觸，也可以在球與球窩間設置滾珠，形成滾動接觸，如此有較佳耐磨特性。在另一實施例中，工具端平台或基本 端平台上之球窩可以改成球，此時工作雙球桿的末端即是相配合之球窩。

在本發明之直接姿態回授控制機器中，工具端平台及基本端平台上之球窩與工作雙球桿末端之球構成球軸承結構(簡稱球軸承)，此球軸承的中心位置必須不受外力及熱的影響。為降低或排除工具端平台及基本端平台上各個球軸承中心位置因環境溫度變化而產生熱誤差，球窩最好是固定在熱脹係數小於2ppm/℃之材料上，甚至是接近零熱脹係數(小於0.1ppm/℃)之材料上，例如INVAR因瓦合金、SUPER INVAR超因瓦合金、碳纖維複合材料、LEX-ZERO^®合金或石英等，或是微晶玻璃，例如ZERODUR^®微晶玻璃或CLEARCERAM-Z^®微晶玻璃。

在直接姿態回授控制機器中，工具及工件可以在工具端平台、基本端平台及六個工作雙球桿所包覆之空間之內或外。在一實施例中，直接姿態回授控制機器是三線性軸工具機，且主軸(高速主軸)由X軸及Z軸驅動，工件是固定在Y軸工作台上，工具端平台是藉由一轉接件固定在主軸外殼上，基本端平台是中空環形，被固定在Y軸工作台上，工件是固定在基本端平台之中空部分，六個工作雙球桿連接工具端平台及基本端平台之間，而將刀具及工件均包覆在內，另外，在基本端平台上可安裝刀長量測裝置，以之校 正刀長。

在另一實施中，直接姿態回授控制機器包含一六軸機器手臂及一工作台，工作台可為花崗岩製成，六軸機器手臂被固定於此工作台上，工具端是六軸機器手臂未端的工具安裝座，基本端是工作台上的工件安裝面，工具可為一量測探頭，工具端平台可被固定在工具安裝座上，量測探頭可固定在工具端平台上，基本端平台可被固定於工作台上且具有中空部位，工件被置於此中空部位，六支工作雙球桿連接工具端平台及基本端平台，量測探頭及工件是位在工具端平台、基本端平台及六支工作雙球桿包覆之空間內，量測探頭有觸發訊號，可使控制器同步讀出六支工作雙球桿之位移量測值，以計算得出量測探頭末端量測球的中心座標位置，基本端平台上或是工作台上可另外固定一參考球，做為座標校正用。此直接姿態回授控制機器可以用於小工件之座標量測。

在另一實施例中，直接姿態回授控制機器包含一六軸機器手臂、一工作台及一支架，工具是量測探頭，工件固定在工作台上，支架也是固定於工作台上，基本端平台被固定於支架上，工具端平台固定在六軸機器手臂的工具安裝面上，六支工作雙球桿連接工具端平台及基本端平台，形成倒立式的雙球桿姿態量測機構，因而量測探頭及工件是在工具端平台、基本端平台及六個工作雙球桿包覆之空間外。工作台上 可固定有一參考球，其做為座標校正用。此實施例之直接姿態回授控制機器手臂的好處是，可以用於中小工件之座標量測，工件之操作及進出空間較方便。由於基本端平台固定於支架上，故支架最好用低熱脹係數高強度之材料製成，例如INVAR因瓦合金或是LEX-ZERO^®合金，以避免支架之熱誤差影響量測結果，此外，亦可對支架高度之熱誤差進行補償，以確保直接姿態回授控制機器的精度。

在一實施例中，控制器可透過一即時通訊網路，其例如為乙太即時通訊網路(EtherCAT)，讀回各工作雙球桿的工作位移量測值、參考雙球桿的熱誤差量測值及馬達的位置實際值，也藉之送出馬達驅動值，如此可以大大減少配線數量。於雙球桿姿態量測機構中，工作雙球桿之位移感測器可以是絕對式或是增量式之雷射干涉儀、光學尺或是磁性尺，在起始化後工作雙球桿有絕對中心距離。

本發明提供一種直接姿態回授控制方法，其中，在工具端平台上定義有一工具平台座標系，在基本端平台上定義有一基本平台座標系，在機器上定義有一參考座標系，工具平台座標系中球軸承中心之座標位置、基本平台座標系中球軸承中心之座標位置、及六支工作雙球桿之中心距離構成雙球桿姿態量測機構之機構參數，進行直接姿態的回授控制時，以此機構參數做機構正逆轉換計算，算出工具平台座標系相 對基本平台座標系之六維座標轉換關係，再算出工具在參考座標系之實際姿態，包括三位置及三轉動角。

本發明之直接姿態回授控制方法，可以由參考雙球桿之熱誤差量測值得出雙球桿姿態量測機構機構參數之熱誤差，包括該些工作雙球桿中心距離之熱誤差，及工具端平台及基本端平台上各個球軸承中心位置之熱誤差，並加以補償，以補償後之機構參數做機構轉換計算。

直接姿態回授控制機器在機器結構及驅動機構中有靜態機構參數誤差及幾何誤差，也有準靜態熱誤差，控制器驅動工具相對工件運動時有動運動態誤差，最終造成工具端點的位置誤差及工具的指向誤差，亦即工具姿態誤差。機器手臂控制器中之插補器進行正向及逆向機構轉換時所使用的機構參數是理想機器手臂之機構參數，因此，在逆向機構轉換中由工具之設定姿態得出各個馬達之位置設定值，是理想機器手臂中各個馬達之位置設定值。

一般工具機或機器手臂之控制器進行位置回授控制時，由各個馬達之位置設定值與量出之馬達位置實際值算出追蹤誤差，位置控制器依此追蹤誤差產生馬達驅動值，例如速度指令。控制器在驅動多個馬達運動時，實際運動軌跡會偏離設定路徑，產生軌跡誤差(Contour error)。控制器中有提供控制及補償方法，用以降低甚至消除追蹤誤差或是軌跡誤差，例 如前饋控制(Feed Forward,FF)、軌跡控制、摩擦力補償控制及切削力補償控制等，實現直接姿態的回授控制時最好能保留控制器中這些現有之控制或補償方法。

本發明之直接姿態回授控制方法中，除了雙球桿姿態量測機構之工具平台座標系及基本平台座標系外，還有機器之參考座標系或理想軸座標系、工具端點座標系，各個座標系間有座標轉換關係，控制器可對這些座標系進行管理。進行直接姿態的回授控制時，要由雙球桿姿態量測機構中各個工作雙球桿的工作位移量測值，經一系列複雜之計算才能得出工具的實際姿態及姿態誤差，以姿態誤差進行姿態回授控制，這姿態誤差計算極為耗時，而且計算時間變動大，將之安排在控制迴路中執行會有死時(dead time)問題，對馬達伺服控制造成不利影響。

因而在本發明之直接姿態回授控制方法中，拆解姿態誤差為連結姿態誤差及追蹤姿態誤差，以雙球桿姿態量測機構量測得出工具之一實際姿態，以各個馬達之位置實際值減去位置補償值後做機構正轉換計算，得出一工具姿態，稱之為編碼器定義姿態，由實際姿態與編碼器定義姿態得出連結姿態誤差，姿態補償控制處理連結姿態誤差，得出各個馬達的位置補償值及補償驅動值，另一方面，由設定姿態與編碼器定義姿態得出追蹤姿態誤差，位置回授控制處理追蹤 姿態誤差，得出各個馬達的驅動值。由於連結姿態誤差變化緩慢，姿態補償控制不必與位置回授控制有相同之取樣時間。

本發明之直接姿態回授控制方法之優點因而是，維持控制器原有之高取樣頻率即時位置回授控制，另外可用一低取樣頻率之姿態補償控制與即時位置回授控制配合，解決了直接姿態回授控制中因姿態誤差計算造成之死時問題，使直接姿態回授控制可以由獨立運作而又相互配合之即時位置回授控制及姿態補償控制完美達成。即時位置回授控制與姿態補償控制可以安排在相同或是不相同之計時器中斷服務程式執行，可以有自己的控制器，姿態補償控制之取樣時間最好大於等於位置回授控制之取樣時間。在一實施例中，位置回授控制與姿態補償控制使用同一位置控制器，此時，可以姿態補償控制得出之各個馬達之位置補償值修正各個馬達之位置設定值。

直接姿態回授控制方法在不同之機器應用中衍生不同之應用方法，在一實施例中，直接姿態回授控制機器是座標量測機，雙球桿姿態量測機構的工具端平台上安裝的工具為量測探頭，且以量測探頭之觸發訊號同步栓鎖雙球桿姿態量測機構中各工作雙球桿之工作位移量測值，控制器讀出後進行量測探頭末端量測球中心的座標位置計算。

在一實施例中，基本端平台是固定於一支架 上，支架則是與工件一起固定於工作台上，基本平台座標系與工件座標系間有一偏置量，支架及工具於垂直方向之熱誤差會影響工具的端點在工件座標系內的座標值。在一實施例中，建立支架的熱誤差與參考雙球桿的熱誤差量測值間之關係，由參考雙球桿即時的熱誤差量測值得出支架高度之熱誤差，並加以補償。在一實施例中，工具的端點在工具端工具平台座標系內也有一偏置量，建立工具長度熱誤差與參考雙球桿的熱誤差量測值間之關係，由參考雙球桿即時的熱誤差量測值得出工具長度之熱誤差，並加以補償。

請參考第1圖，其中第1圖繪示依照本發明一實施例之一種直接姿態回授控制機器10之示意圖。直接姿態回授控制機器10包含一機器本體(未標示)、複數馬達(未標示)、一控制器(未繪示)、一雙球桿姿態量測機構300、一參考板500及至少一參考雙球桿400。機器本體包含一工具端(未標示)及一基本端(未標示)，且工具端設置工具6，馬達設置於機器本體上，控制器設置於機器本體外，且控制器驅動馬達使工具6相對一工件110位移。雙球桿姿態量測機構300設置於基本端與工具端之間且包含一工具端平台320、一基本端平台310及至少六工作雙球桿330，工具端平台320位於工具端；基本端平台310位於基本端，至少六工作雙球桿330連接工具端平台320及基本端平台310。參考板500不受馬達驅動，且參考板500 包含至少二磁性球窩510、520，前述至少一參考雙球桿400設置於參考板500之至少二磁性球窩510、520上。直接姿態回授控制機器10可更包含一主軸座4及一主軸5，主軸座4設置於Z運動平台3上，主軸5連接於主軸座4，工具端平台320是固定於主軸5外殼上。

在第1圖中，直接姿態回授控制機器10可更包含一工作台12及一底座11，工作台12位於底座11上且可沿Y軸位移，而工件110位於工作台12上，為了進行工具6相對工件110之直接回授，主軸5的末端與工作台12間裝有雙球桿姿態量測機構300。工具端平台320可包含複數球窩(未標示)，基本端平台310亦包含複數球窩(未標示)，工具端平台320可更包含一定位層321及至少一支承層322，工具端平台320的球窩位於工具端平台320的定位層321；基本端平台310包含另一定位層311及至少另一支承層312，基本端平台310的球窩位於基本端平台310的定位層311。為了減少熱誤差，基本端平台310的球窩及工具端平台320的球窩可固定於一材料上，且材料之熱脹係數介於0-1ppm/℃之間。因此，固定球窩之定位層311、321的熱脹係數可介於0-1ppm/℃之間，而支承層312、322的熱脹係數可介於0-2ppm/℃之間。

具體而言，工具端平台320具有兩層結構， 定位層321可以是石英製成之中空環形板，其上有六個球窩，支承層322是INVAR因瓦合金製成之中空圓柱，支承層322的末端有法蘭，用以承托定位層321，工具端平台320藉支承層322被固定於主軸5外殼上。基本端平台310被固定在工作台12上，也由兩層結構組成，定位層311是石英製成之中空方形板，支承層312是INVAR因瓦合金製成之中空方形板，定位層311上有六個球窩，工件110是固定於基本端平台310中空部位的一墊高塊120上，六支工作雙球桿330連接工具端平台320及基本端平台310，構成直接姿態回授控制機器10，刀具6及工件110是在工具端平台320、基本端平台310及六支工作雙球桿330包覆之空間內。

工具6於第1圖中示例為一刀具，故直接姿態回授控制機器10可更包含一刀長量測裝置130，其設置於基本端平台310，且刀長量測裝置130可用以量測刀具的刀長，然在其他實施例中，刀長量測裝置亦可設置於工作台上，不以此為限。此外，用於熱誤差補償之參考雙球桿400被置於參考板500上，參考板500被置於底座11上。

請參閱第2圖，其繪示依照本發明另一實施例之一種直接姿態回授控制機器10a之示意圖，可以在一般工廠環境下進行小尺寸工件110a之座標量測。在第2圖的實施例中，直接姿態回授控制機器10a包 含一六軸機器手臂11a及一工作台12a，工作台12a可為花崗岩製成，機器手臂11a可設置於工作台12a上。工具端平台320a固定在機器手臂11a末端的工具安裝座(未標示)上，工具6a為一量測探頭且固定在工具端平台320a上，基本端平台310a則固定於工作台12a上，參考板500a設置於工作台12a上。

基本端平台310a有兩層結構，基本端平台310a的定位層311a是石英製成之中空環形板，支承層312a是INVAR因瓦合金製成之圓板，工件110a固定於工件安裝座120a上，工件安裝座120a是固定在支承層312a上。工具端平台320a包含一定位層321a及一支承層322a，六支工作雙球桿330a連接工具端平台320a及基本端平台310a，構成雙球桿姿態量測機構300a，工具6a及工件110a是在工具端平台320a、基本端平台310a及六支工作雙球桿330a包覆之空間內，參考雙球桿400a被置於參考板500a上，工具6a可以產生一觸發訊號，以之同步栓鎖工作位移量測值，再由控制器讀出後算出工具6a末端量測球之中心座標位置，在第2圖的實施例中，工具6a的座標位置即是指量測探頭末端量測球中心的座標位置。此外，直接姿態回授控制機器10a可更包含一參考球131a，其設置於基本端平台310a，更進一步地說，其是設置於支承層312a上，且做為座標之校正用。然在其他實施例中，參考球可設置於工件 安裝平台上，不以此為限。

請參閱第3圖，其繪示依照本發明之一種直接姿態回授控制機器10b之又一實施例示意圖。在第3圖的實施例中，直接姿態回授控制機器10b包含一六軸機器手臂11b、一工作台12b及一支架13b，工作台12b可為花崗岩製成，六軸機器手臂11b及支架13b可設置於工作台12b上，其中，工具端可定義為六軸機器手臂11b未端的工具安裝座，基本端可定義為支架13b上的一基本端平台310b安裝平面，在第3圖中，只繪示出支架13b的右半部，以便顯示其他零件，不以此限制本發明。

在第3圖的實施例中，雙球桿姿態量測機構300b的基本端平台310b安裝於支架13b上，基本端平台310b有兩層結構，定位層311b是石英製成之環形板，由INVAR因瓦合金製成之支承層312b承托，工具端平台320b是藉一轉接件固定在六軸機器手臂11b末端的工具安裝座上，工具端平台320b也有兩層結構，定位層321b是石英製成之環形板，由INVAR因瓦合金製成之支承層322b承托，工具6b為一量測探頭且固定在工具端平台320b上，六支工作雙球桿330b連接工具端平台320b及基本端平台310b，如此，工具6b及工件110b是在工具端平台320b、基本端平台310b及六支工作雙球桿330b包覆之空間外，參考雙球桿400b被置於參考板500b 上。此直接姿態回授控制機器10b的好處是，工件110b之操作及進出空間較大。

此外，在直接姿態回授控制機器10b中，支架13b使用低熱脹係數高強度之材料例如INVAR因瓦合金，可在控制器200b(繪示於第4圖)中建立支架13b高度之熱誤差模型，依據參考雙球桿400b之熱誤差量測值得出此支架13b上的基本端平台310b於垂直方向之熱誤差，並加以補償，亦可依據此熱誤差量測值計算並補償工具6b長度的熱誤差。直接姿態回授控制機器10b可更包含一參考球131b，其設置於工作台12b上，且參考球131b可做為座標之校正用。在此要特別說明的是，支架13b之熱誤差模型可用參考雙球桿400b量出之熱誤差為輸入，如此，可由參考雙球桿400b量出之熱誤差計算出支架13b的熱誤差。類似地，亦可以得到對工具6b的端點的熱誤差。在其他實施例中，支架之熱誤差模型也可以將環境溫度量測值及/或支架溫度量測值為輸入。

請參閱第4圖，並一併參閱第3圖，其中第4圖繪示第3圖實施例之直接姿態回授控制機器10b之控制架構方塊示意圖。直接姿態回授控制機器10b的控制器200b(可為CNC控制器)包括在一視窗操作模組210b下工作之一人機介面211b，及即時環境下工作之一運動控制核心模組220b，其包括一解譯器221b、一插補器222b及一加減速規劃器223b，並 與一位置回授控制91及一姿態補償控制92配合，位置回授控制91及姿態補償控制92將於後方再做說明。控制器200b使用一即時通訊網路230b，此時，各工作雙球桿330b與參考雙球桿400b以即時通訊網路230b回傳各工作位移量測值與熱誤差量測值。

仔細而言，即時通訊網路230b可為乙太即時通訊網路(EtherCAT)，控制器200b可以藉乙太即時通訊網路與六個驅動器240b、七個數位計數器250b通訊，而可同步讀出各馬達800b的位置實際值、各工作雙球桿330b的工作位移量測值及參考雙球桿400b的熱誤差量測值，進行姿態回授控制數據處理，並透過即時通訊網路230b將各馬達800b之驅動值送至各相應之驅動器240b。在此要特別說明地是，工作雙球桿330b及參考雙球桿400b之位移感測器可以是一雷射干涉儀、一光學尺或是一磁性尺，此外，各位移感測器可為絕對式或增量式，本發明不以此為限。

在直接姿態回授控制機器10b中，工具6b(示例為量測探頭)的觸發訊號可同步栓鎖六工作雙球桿330b的工作位移量測值及參考雙球桿400b的熱誤差量測值，控制器200b讀出後計算得出工具6b末端的量測球之中心座標位置，而當工具6b為掃描探頭時，其位移量測訊號也可經由即時通訊網路230b送出。

在機器手臂之機構轉換中，機構正轉換由軸位置值θ=(θ₁,θ₂,θ₃,θ₄,θ₅,θ₆) ^T ，計算得出參考座標系中之姿態P=(X,Y,Z,α,β,γ) ^T ，其中方向可用三轉動角(α,β,γ)表示。假設各個軸的位置值有微量變化，以向量△θ表示，△θ=(△θ₁,△θ₂,△θ₃,△θ₄,△θ₅,△θ₆) ^T ，造成工具姿態有微量變化，以向量△P表示，△P=(△X,△Y,△Z,△α,△β,△γ) ^T ，此微量軸位置值變化△θ與微量姿態變化△P間之關係可用賈可比矩陣J表示，如式(1)：

△P=J．△θ (1)；反之，微量姿態變化△P與微量軸位置值變化△θ間之關係可用賈可比逆矩陣J ^-1表示，如式(2)：

△θ=J ^-1．△P (2)。

請參閱第5圖、第6圖、第7圖、第8圖、第9圖及第10圖，其中第5圖繪示依照本發明之一種直接姿態回授控制方法中姿態補償控制92的方塊流程圖，第6圖繪示第5圖實施例之直接姿態回授控制方法之工具的姿態示意圖，第7圖繪示第5圖實施例之直接姿態回授控制方法的一控制流程圖，第8圖繪示第5圖實施例之直接姿態回授控制方法的另一控制流程圖，第9圖繪示第5圖實施例之直接姿態回授控制方法的又一控制流程圖，第10圖繪示第5圖實施例之直接姿態回授控制方法的再一控制流程圖。在第7圖及第8圖中，馬達驅動系統227d是以二次轉換函數表示，其包括自然頻率ω及阻尼D，另外包括一積 分器228d，轉換函數為1/s，將速度轉為位移。本發明中之直接姿態回授控制方法包括位置回授控制91及姿態補償控制92，本說明書中馬達相關之變數，以數字下標i代表第i馬達，例如θ_1,comp 或θ _i,comp 代表第一或第i馬達之位置補償值，以無數字下標之變數例如θ _comp 代表所有馬達之位置補償值向量。

直接姿態回授控制方法中，姿態補償控制92可包含一起始化步驟S01、一座標轉換關係建立步驟S02、一實際姿態計算步驟S03及一位置補償值計算步驟S04。於起始化步驟S01中，控制器建立各工作雙球桿之中心距離函數，及雙球桿姿態量測機構300d工具端之工具平台座標系{P₀}與基本端基本平台座標系{B}的座標轉換關係

。

於座標轉換關係建立步驟S02中，控制器依據一內建或是載入之設定值，或是進行一機上量測，建立基本平台座標系{B}與參考座標系{R}的座標轉換關係 ^R T _B 。

於實際姿態計算步驟S03中，控制器讀入各工作雙球桿的工作位移量測值，計算出工具的實際姿態P _A 。

於位置補償值計算步驟S04中，姿態補償控制92依據一補償目標姿態及實際姿態P _A ，得出各馬達800b的位置補償值θ _comp 。

第7圖及第8圖之實施例中，位置回授控制 91及姿態補償控制92共用位置控制器911，其是以比例增益值k _v 表示，此時，以第一馬達驅動為例，姿態補償控制92得出位置補償值θ_1,comp ，以之修正位置回授控制91之位置設定值θ_1,s ，第10圖之實施例中顯示位置回授控制91及姿態補償控制92共用位置控制器911，轉換函數是C(s)時之控制流程圖。第9圖之實施例中，姿態補償控制92有自己的補償控制器921，轉換函數是CC(s)，由位置補償值θ _i,comp 得出補償驅動值，與位置控制器911，轉換函數是C(s)，得出之驅動值相加後輸出至驅動器。另外，姿態補償控制92得出之位置補償值在隨後之位置回授控制91控制周期中均為有效，直到姿態補償控制92產生新的位置補償值進行更新。

具體而言，可用齊次轉換矩陣T來表示座標系間之轉換關係。在起始化步驟S01中，起始化雙球桿姿態量測機構300d之機構參數，包括各個工作雙球桿之中心距離函數，及工具端平台與基本端平台上各個球窩(即球軸承)中心之座標位置。在座標轉換關係建立步驟S02中，控制器可以藉由一內建或是載入之設定值建立參考座標系{R}，或是以雙球桿姿態量測機構300d進行機上量測，然後建立參考座標系{R}，在得出參考座標系{R}後，控制器可建立基本平台座標系{B}與參考座標系{R}的座標轉換關係 ^R T _B ，並在完成姿態補償控制之起始化後得出一起始參考姿態P _r ， 及各馬達初始的位置補償值θ _i,comp 及補償驅動值，此時工具平台座標系為{P₀}，雙球桿姿態量測機構300d量出工具平台座標系{P₀}與基本平台座標系{B}的座標轉換關係

，工具端點座標系{T₀}與工具平台座標系{P₀}的座標轉換關係

為固定關係，在安裝工具後經校正得知，工具端點座標系{T₀}與基本平台座標 系{B}的座標轉換關係

因而是

，而工具 端點座標系{T₀}與參考座標系{R}的座標轉換關係 為

，

。

直接姿態回授控制機器是三線性軸機器時，於座標轉換關係建立步驟S02中，可分別驅動機器之X軸、Y軸及Z軸的馬達分別做定位量測，雙球桿姿態量測機構300d量出複數個量測點，擬合量測點分別得出直接姿態回授控制機器之三實際軸線向量l₁、l₂及l₃，以例如l₁為理想向量X，以二向量X及l₂做外積，可以建立理想向量Z，再以Z及X做外積可以建立理想向量Y，以此三相互垂直之向量X、Y及Z可以建立機器之理想軸座標系，可以此理想軸座標系為參考座標系{R}，得出機器實際軸座標系與參考座標系{R}間之座標轉換關係，及基本平台座標系{B} 與參考座標系{R}的座標轉換關係 ^R T _B ，且

。

直接姿態回授控制機器為六軸機器手臂11d時，於座標轉換關係建立步驟S02中可例如驅動第一轉動關節馬達做定位量測，以雙球桿姿態量測機構 300d量出複數個量測點，處理量測點得出一圓心及一圓平面之法線向量，理想Z軸即是通過該圓心，與該法線向量同方向之軸線，同樣再驅動第二轉動關節馬達做定位量測，可以得出一實際Y軸軸線向量，以實際Y軸軸線向量與理想Z軸軸線向量做外積，得出理想X軸軸線向量，以理想Z軸軸線向量及與理想X軸軸線向量做外積，得出理想Y軸軸線向量，再由理想Z軸軸線及與理想Y軸軸線間之公垂線在理想Z軸軸線上之交點，及理想Y軸軸線與六軸機器手臂11d安裝面間之距離(公稱設計值)，可以建立六軸機器手臂11d之參考座標系{R}，及基本平台座標系{B}與參 考座標系{R}的座標轉換關係 ^R T _B ，且

。

如第7圖所示，控制器之插補器222d可包含一路徑插補器2221d及一指向插補器2222d，加減速規劃器223d對加減速進行規劃，依據輸入之工具路徑及指向，即時計算得出工具之設定姿態P _S ，亦即工具端點的設定位置(X,Y,Z)及工具指向，例如是三轉動角(α,β,γ)，P _S =(X,Y,Z,α,β,γ) _S ^T ，並做機構逆轉換計算，得出位置設定值向量θ _s =(θ_1,s ,θ_2,s ,θ_3,s ,θ_4,s ,θ_5,s ,θ_6,s ) ^T 。當工具是固定在工具端平台上，工具端點座標系{T₀}與工具平台座標系{P₀}的座標轉換關係

恆定不變，故實際工具端點座標系{T_n}與實際工具平台座標系{P_n}的座標轉換關係

如式(1)所示，由式(2)可求得實際工具端點座標系{T_n}與參考座標系{R}的座標轉換關係

而式(3)及式(4)如下所示：

在式(3)中，實際工具平台座標系{P_n}與基本平台座標系{B}的座標轉換關係

是由雙球桿姿態量測機構300d量測得出，藉由式(3)，可將實際工具端點座標系{T_n}中之工具位置及指向轉換到參考座標系{R}，得出實際姿態P _A ，P _A =(X,Y,Z,α,β,γ) _A ^T 。實際工具端點座標系{T_n}與基本平台座標系{B}的座標轉換關係為

，編碼器定義姿態下實際工具端點座標系{T_n}與參考座標系{B}間的座標轉換關係為

。

由設定姿態P _S 與量測得出之實際姿態P _A 可以得出實際姿態誤差E _a ，E _a =P _S -P _A =(△X,△Y,△Z,△α,△β,△γ) _A ^T 。由於雙球桿姿態量測機構300d之正逆轉換計算複雜，加上機器手臂之座標系間正逆轉換計算也需要時間，以實際姿態誤差E _a 進行直接姿態回授控制會有死時過大及計算時間不穩定問題。在本發明之直接姿態回授控制方法的姿態補償控制92中，將實際姿態誤差E _a 拆解，由第i個馬達的位置實際值θ _i,enc 減去第i個馬達的位置補償值θ _i,comp ，得出第i個馬達的編碼器定義姿態位置值θ _i,E ，θ _i,E =θ _i,enc -θ _i,comp ，以各個馬達的編碼器定義姿態位置值θ _i,E 構成之編碼器定義姿態位置向量θ _E =(θ_1,E ,θ_2,E ,θ_3,E ,θ_4,E ,θ_5,E ,θ_6,E ) ^T ，以元件號225d之機構正轉換 器FKT做機構正轉換，得出一編碼器定義姿態P _E ，P _E =(X,Y,Z,α,β,γ) _E ^T ，這是理想機器手臂所定義之姿態。編碼器定義姿態P _E 與實際姿態P _A 間之偏差定義為一連結姿態誤差E _l ，E _l =P _E -P _A =(△X,△Y,△Z,△α,△β,△γ) _l ^T ，在本發明之姿態補償控制中，由此連結姿態誤差E _l 得出各個馬達的位置補償值θ _i,comp ，及位置補償值向量θ _comp =(θ_1,comp ,θ_2,comp ,θ_3,comp ,θ_4,comp ,θ_5,comp ,θ_6,comp ) ^T 。

設定姿態P _S 與編碼器定義姿態P _E 間之差是定義為追蹤姿態誤差E _f ，E _f =P _S -P _E =(△X,△Y,△Z,△α,△β,△γ) _f ^T 。上述之設定姿態P _S 、編碼器定義姿態P _E 與實際姿態P _A 可以在參考坐標系{R}得出，也可以在基本平台座標系{B}得出。追蹤姿態誤差E _f 對應到軸座標系中之軸追蹤誤差向量△θ _f ，△θ _f =θ _S -θ _enc =(θ_1,s -θ_1,enc ,θ_2,s -θ_2,enc ,θ_3,s -θ_3,enc ,θ_4,s -θ_4,enc ,θ_5,s -θ_5,enc ,θ_6,s -θ_6,enc ) ^T 。

本發明之直接姿態回授控制方法之一主要特徵是，對追蹤姿態誤差E _f 對應之追蹤誤差向量△θ _f 進行位置回授控制91，另外對連結姿態誤差E _l 進行姿態補償控制92，如此可以快速實現完整的直接姿態的回授控制而無死時問題。

在姿態補償控制92中，可以依運動情況選擇補償目標姿態，得出各個馬達之位置補償值θ _i,comp 。在各個馬達的位置設定值θ _i,s 持續改變之運動過程中，可視馬達之編碼器定義姿態P _E 為補償目標姿態，如第7圖所示，增量位置補償值向量△θ _comp ， △θ _comp =(△θ_1,comp ,△θ_2,comp ,△θ_3,comp ,△θ_4,comp ,△θ_5,comp ,△θ_6,comp ) ^T 可以例如由連結姿態誤差E _l 乘上機器手臂機構轉換的賈可比逆矩陣J ^-1得出，如式(5)：

△θ _comp =J ^-1×E _l (5)。

當工具之設定姿態P _S 不再改變，各個馬達的位置設定值也不再改變，此時可以設定姿態P _S 為補償目標姿態，由實際姿態誤差E _a 計算得出增量位置補償值向量△θ _comp ，如式(6)：

△θ _comp =J ^-1×E _a (6)。

請參閱第8圖，其繪示直接姿態回授控制方法的另一控制流程圖。此時，由元件號224d之機構逆轉換器(Inverse Kinematic Transformation,IKT)做機構逆轉換計算，由量出之工具實際姿態P _A 計算得出各個馬達的等效位置實際值θ _i,a ，等效位置實際值向量為θ _a ，θ _a =(θ_1,a ,θ_2,a ,θ_3,a ,θ_4,a ,θ_5,a ,θ_6,a ) ^T ，再如上所述選擇補償目標姿態，例如第8圖中以編碼器定義姿態P _E 為補償目標姿態，先從編碼器量出之各個馬達的位置實際值θ _i,enc 減去各個馬達的有效之位置補償值θ _i,comp ，得出各馬達的編碼器定義姿態位置值θ _i,E ，θ _i,E =θ _i,enc -θ _i,comp ，再減去各個馬達的等效位置實際值θ _i,a ，即得出各個馬達新的增量位置補償值△θ _i,comp ，如式(7)：

△θ _i,comp =θ _i,enc -θ _i,comp -θ _i,a ,i=1..6 (7)。

在此需特別說明的是，使用式(5)及式(6)時是以一次近似方法得出位置補償值，也可以迭代多次 逼近方法得出較準確之位置補償值，例如牛頓迭代逼近法，或是例如以實際機構參數建立機器實際機構模型做較準確之位置補償值計算，在此不再重複敘述。另外，得出各馬達之增量位置補償值△θ _i,comp 是新的增量位置補償值，因而需要設置儲存器儲存目前有效舊的位置補償值，再由新的增量位置補償值得出更新後的位置補償值，在第7圖及第8圖中，是透過一元件號226d之時間延遲元件TD示意出，假設[kT]表示第k個姿態補償控制周期，[kT-T]表示第k-1個姿態補償控制周期，各馬達之位置補償值之更新如式(8)所示：

θ _i,comp [kT]=θ _i,comp [kT-T]+△θ _i,comp [kT] (8)；此更新的位置補償值θ _i,comp [kT]維持有效直到下次更新。

如果直接姿態回授控制機器是三線性軸工具機，此時直接姿態回授控制之要點是驅動三線性軸補償工具端點位置的誤差，工具的指向誤差不能藉由驅動三線性軸消除，不過，工具端點位置的誤差中已包含有工具指向誤差的影響，即所謂之阿貝誤差，此阿貝誤差可以被直接姿態回授控制消除。

請參閱第7圖、第8圖及第10圖，姿態補償控制92之補償控制器921與位置回授控制91之位置控制器911使用同一控制器，轉換函數為C(s)時，可將補償控制器921與位置控制器911合併，如第10圖所示。在第7圖、第8圖之實施例中，C(s)是比例控制器，增益值是k _v ，此時以馬達之位置補償值 θ _i,comp 修正位置設定值θ _i,s ，再減去位置實際值θ _i,enc 後，乘以增益值k _v ，即得出各個馬達補償後的驅動值V _cmd,i ，如式(9)，此驅動值V _cmd,i 是控制器輸出給各個馬達驅動器之速度指令：

V _cmd,i =k _v ．(θ _i,s +θ _i,comp -θ _i,enc ) (9)。

為避免因為連結姿態誤差計算時間過長造成直接姿態的回授控制迴路中死時的影響，在一直接姿態回授控制方法實施例中，由一即時計時器(real-time timer)中斷服務程式先呼叫執行位置回授控制，再呼叫執行姿態補償控制，姿態補償控制計算出之各個馬達的位置補償值及補償驅動值後，保留給下一位置回授控制周期使用。在另一直接姿態回授控制方法實施例中，補償控制器與位置控制器共用一比例控制器，例如第10圖所示，此時，在第k個位置回授的控制周期馬達之位置設定值θ _i,s ，被前一位置回授控制周期(亦即k-1個控制周期)中姿態補償控制計算得出之位置補償值θ _i,comp 修正，如此，在第k個控制周期中，位置回授控制送出補償後之馬達驅動值V _cmd,i [kT]，由於θ _i,comp [kT-T]是在前一控制周期得出，不造成位置回授控制的死時問題，而驅動值V _cmd,i [kT]如式(10)所示：

V _cmd,i [kT]=k _v ．(θ _i,s [kT]+θ _i,comp [kT-T]-θ _i,enc [kT]) (10)；也可以在一控制周期中，位置回授控制先得出各馬達之驅動值後即做輸出，然後姿態補償控制計算得出各馬達 之補償驅動值後，與驅動值相加後做二次輸出。

請參閱第9圖，位置回授控制91有一元件號為911之位置控制器，轉換函數為C(s)，姿態補償控制92有一元件號為921之補償控制器，轉換函數為CC(s)，可以例如是比例-積分-微分PID控制器。此第9圖中顯示姿態補償控制92使用一獨立之補償控制器921，好處是，可以保留位置回授控制中原有之其他控制器，例如第9圖中示意之前饋控制器(Feed Forward,FF)。在一實施例中，補償控制器921是比例控制器，增益值為k _vc ，位置控制器911也是比例控制器，增益值為k _v ，此時，補償控制器921由各個馬達之位置補償值θ _i,comp 乘以增益值k _vc 得出補償驅動值，與位置控制器911得出之驅動值相加，得出補償後之驅動值V _cmd,i ，如式(11)所示：

V _cmd,i =k _v ．(θ _i,s -θ _i,enc )+k _vc ．θ _i,comp (11)。

另外，由於機器之靜態及準靜態誤差例如幾何誤差及熱誤差等變化緩慢，在一實施例中，姿態補償控制之取樣時間可以遠大於位置回授控制之取樣時間，以節省控制器內中央處理器(CPU)或是數位訊號處理器(DSP)之計算負荷，亦即，姿態補償控制可以不安排在即時位置回授控制之即時計時器中斷服務程式中執行，而安排於另一時間間隔較長之控制周期中執行。一般位置回授控制之取樣時間或是控制周期例如是1ms。

工具之實際姿態中包含有直接姿態回授控制機器所有的靜態與動態行為資訊，例如工具為刀具時的切削力作用資訊、機器軸定位動態資訊或是結構振動資訊，控制器可以對實際姿態進行例如頻譜分析等訊號處理分析，找出直接姿態回授控制機器的結構振動或運動動態問題。在一實施例中，也對位置補償值進行低通濾波數據處理，得出各個馬達無振動變化之位置補償值。

請參閱第11圖，其中第11圖繪示依照本發明更一實施例之一種直接姿態回授控制機器的雙球桿姿態量測機構300c的示意圖。基本端平台310c在上，工具端平台320c在下，而形成倒立式的雙球桿姿態量測機構300c，其中工具端平台320c的球窩P₁~P₆及基本端平台310c的球窩B₁~B₆具有不對稱分佈，基本端平台310c上之球窩B₁~B₆分佈偏向前方半圓範圍，工具端平台320c上之球窩P₁~P₆分佈移向兩側，以避免工具端平台320c及工作雙球桿330c與機器本體發生干涉，有利工件從正前方或橫向進出，配合工具端平台320c的球窩P₁~P₆及基本端平台310c的球窩B₁~B₆分佈，各工作雙球桿330c之最大及最小中心距離與球窩P₁~P₆、B₁~B₆之分佈相配合。換句話說，各工作雙球桿330c具有之最大中心距離及最小中心距離是依照球窩P₁~P₆、B₁~B₆的配置而定。

專業人士可以做出其他變化而不偏離本發明之精神與要點，例如各個馬達位置回授控制之驅動值中包括前饋或是軌跡誤差控制值，雙球桿姿態量測機構中工具端平台或基本端平台與機器之整合方式可以有其他變化，工具端平台或基本端平台上之球窩也可以改為球，此時連接之工作雙球桿的末端即以球窩相配合，工具端平台或是基本端平台上球窩的分佈方式可以依據工作空間需要而變化，工作雙球桿長度也可以依之變化，工作雙球桿與參考雙球桿可以加裝防水防塵護套，以避免汙染，也可對實際姿態進行頻譜分析之外之各類濾波處理，診斷機器之動態行為，算出姿態誤差後，也可以針對姿態補償控制設計特別的補償控制器。

雖然本發明已以實施例揭露如上，然其並非用以限定本發明，任何熟習此技藝者，在不脫離本發明之精神和範圍內，當可作各種之更動與潤飾，因此本發明之保護範圍當視後附之申請專利範圍所界定者為準。

92:姿態補償控制

S01:起始化步驟

S02:座標轉換關係建立步驟

S03:實際姿態計算步驟

S04:位置補償值計算步驟

Claims (29)

1. 一種直接姿態回授控制方法，應用於一直接姿態回授控制機器，該直接姿態回授控制機器包含一控制器、一雙球桿姿態量測機構、複數馬達及一工具，該雙球桿姿態量測機構設置於該直接姿態回授控制機器之一基本端及一工具端間且包含一工具端平台設置於該工具端、一基本端平台設置於該基本端、至少六支工作雙球桿連接該工具端平台及該基本端平台、以及至少一參考雙球桿設置於一參考板，該工具端平台包含至少三球窩，該基本端平台包含至少三球窩，該直接姿態回授控制方法以一姿態補償控制配合一位置回授控制，該姿態補償控制包含：

   一起始化步驟，使該控制器建立該至少一參考雙球桿及各該工作雙球桿之一中心距離函數，及建立該雙球桿姿態量測機構之一工具平台座標系與一基本平台座標系間之座標轉換關係；

   一座標轉換關係建立步驟，使該控制器依據一內建或是載入之設定值，或是一機上量測，建立該基本平台座標系與一參考座標系的座標轉換關係；

   一實際姿態計算步驟，使該控制器讀入各該工作雙球桿之一工作位移量測值，計算該工具的一實際姿態；以及

   一位置補償值計算步驟，使該控制器依一補償目標姿態及該實際姿態得出各該馬達的一增量位置補償值及一位置補償值；

   其中，該姿態補償控制使用各該馬達之一補償控制器，由各該馬達之該位置補償值得出一補償驅動值，該位置回授控制使用各該馬達之一位置控制器，由一位置設定值與一位置實際值之差，得出之一驅動值，該驅動值與該補償驅動值相加後輸出給各該馬達的一驅動器，各該馬達的該位置補償值維持有效，直到該姿態補償控制更新為止。
2. 如請求項1所述之直接姿態回授控制方法，其中，該姿態補償控制之一取樣時間大於等於該位置回授控制之一取樣時間。
3. 如請求項1所述之直接姿態回授控制方法，其中，該補償目標姿態為一編碼器定義姿態或是一設定姿態。
4. 如請求項1所述之直接姿態回授控制方法，其中，以一賈可比逆矩陣得出各該馬達之該增量位置補償值。
5. 如請求項1所述之直接姿態回授控制方法，其中，由各該馬達之一編碼器定義姿態位置值減去各該馬達之一等效位置實際值，得出各該馬達之該增量位置補償值。
6. 如請求項1所述之直接姿態回授控制方法，其中，該補償控制器是一比例控制器。
7. 如請求項1所述之直接姿態回授控制方法，其中，各該馬達之該補償控制器與該位置控制器相同，此時以各該馬達之該位置補償值與各該馬達之該位置設定值相加後，輸入該位置控制器得出一補償後之驅動值。
8. 如請求項1所述之直接姿態回授控制方法，其中，對各該馬達之該位置補償值進行低通濾波處理，得出無振動因素之該位置補償值。
9. 如請求項1所述之直接姿態回授控制方法，其中，於該起始化步驟中，驅動至少一該馬達使各該工作雙球桿過原點以起始化該控制器內建或是載入的各該工作雙球桿的該中心距離函數。
10. 如請求項1所述之直接姿態回授控制方法，其中，該機上量測包括：

    分別驅動至少二該馬達定位，由該雙球桿姿態量測機構之量測結果得出該參考座標系，及得出該基本平台座標系與該參考座標系的座標轉換關係。
11. 如請求項1所述之直接姿態回授控制方 法，其中，於該位置補償值計算步驟中，使用一次逼近方法或是一迭代多次逼近方法計算得出各該馬達之該增量位置補償值。
12. 如請求項1所述之直接姿態回授控制方法，其中，該控制器讀入該參考雙球桿之一熱誤差量測值，由該熱誤差量測值計算並補償該雙球桿姿態量測機構各個機構參數之一熱誤差，包括該工具端平台及該基本端平台上該些球窩位置之熱誤差及該些工作雙球桿中心距離之熱誤差。
13. 如請求項1所述之直接姿態回授控制方法，其中，該實際姿態計算步驟中，該控制器讀出被一觸發訊號同步栓鎖之各該工作位移量測值，算出該工具末端一量測球的中心之一座標位置。
14. 如請求項12所述之直接姿態回授控制方法，其中，該直接回授控制機器更包含安裝該基本端平台之一支架，且於該實際姿態計算步驟中，依該參考雙球桿的該熱誤差量測值計算並補償該支架高度之一熱誤差。
15. 如請求項12所述之直接姿態回授控制方法，其中，於該實際姿態計算步驟中，依該參考雙球桿的該熱誤差量測值計算並補償該工具長度之一熱誤差。
16. 如請求項1所述之直接姿態回授控制方法，其中，該控制器對該實際姿態進行一頻譜分析，得出該直接回授控制機器之振動資訊。
17. 一種直接姿態回授控制機器，包含：

    一機器本體，包含一工具端及一基本端，且該工具端設置一工具；

    複數馬達，設置於該機器本體；

    一控制器，驅動該些馬達，使該工具相對該基本端之一工件運動；以及

    一雙球桿姿態量測機構，設置於該基本端與該工具端之間且包含：

    一工具端平台，位於該工具端且包含至少三球窩；

    一基本端平台，位於該基本端且包含至少三球窩；

    至少六支工作雙球桿，連接於該工具端平台的該些球窩及該基本端平台的該些球窩間，各該工作雙球桿包含二量測球，以各該工作雙球桿的一位移感測器量出二該量測球中心間之距離；

    一參考板，包含至少二參考球窩；及

    至少一參考雙球桿，包含二參考量測球，設置於該參考板之該至少二參考球窩之間，以一位移感測器量出二該參考量測球中心間之距離；

    其中，該控制器以該雙球桿姿態量測機構作為姿態誤差之量測裝置，以該參考雙球桿作為該雙球桿姿態量測 機構機構參數熱誤差之量測裝置，以該雙球桿姿態量測機構量測得出該工具之一實際姿態及一連結姿態誤差，及各該馬達之一位置補償值或是一補償驅動值，消除該工具之該連結姿態誤差。
18. 如請求項17所述之直接姿態回授控制機器，更包含：

    一主軸座，設置於該機器本體；及

    一主軸，連接於該主軸座且包含一主軸外殼；

    其中，該工具端平台固定於該主軸座或該主軸外殼上。
19. 如請求項17所述之直接姿態回授控制機器，其中，該直接姿態回授控制機器為一多軸工具機、一多軸關節機器或一平行連桿驅動機器。
20. 如請求項17所述之直接姿態回授控制機器，其中，該工具為一刀具、一量測探頭、一觸發探頭、一共軛聚焦傳感器、一位移量測雷射或是一光學量測鏡頭。
21. 如請求項17所述之直接姿態回授控制機器，其中，該工具產生一觸發訊號，以該觸發訊號同步栓鎖各該工作雙球桿之一工作位移量測值。
22. 如請求項17所述之直接姿態回授控制機器，更包含：

    一參考球，設置於該基本端平台或是一工作台上。
23. 如請求項17所述之直接姿態回授控制機器，更包含：

    一刀長量測機構，設置於該基本端平台或是一工作台上。
24. 如請求項17所述之直接姿態回授控制機器，其中，該工具端平台上之該些球窩及該基本端平台上之該些球窩具有不對稱分佈。
25. 如請求項17所述之直接姿態回授控制機器，其中，該基本端平台上及該工具端平台上的該些球窩是固定於熱脹係數介於0ppm/℃與2ppm/℃間之一材料上。
26. 如請求項17所述之直接姿態回授控制機器，更包含一支架，該基本端平台安裝於該支架上。
27. 如請求項17所述之直接姿態回授控制機器，其中，該控制器包含一即時通訊網路，各該工作雙球桿及該參考雙球桿藉由該即時通訊網路回傳各該工作雙球 桿之一工作位移量測值及該參考雙球桿之一熱誤差量測值。
28. 如請求項27所述之直接姿態回授控制機器，其中，該即時通訊網路為一即時乙太通訊網路(EtherCAT)。
29. 如請求項17所述之直接姿態回授控制機器，其中，各該工作雙球桿及該參考雙球桿之該位移感測器為一雷射干涉儀、一光學尺或一磁性尺。

Images