連續路徑輪廓控制在3RPS平行式工具機上的發展及實驗

全文

(1)連續路徑輪廓控制在 3RPS 平行式工具機上的發展及實驗. 研究生：張偉倫. 指導教授：秦繼華博士. 國立交通大學機械工程學系中文摘要連續路徑軌跡追蹤在正交工具機上已經有相當成熟的發展，但是對非正交工具機，到目前為止，仍未有相同的發展。本論文係針對 3RPS 並聯式工具機，發展出連續路徑追蹤上的輪廓誤差補償與預補償系統。本文針對此 3RPS 工具機，提出完整的系統架構，分別用牛頓法與虛功法建立動力方程式，並比較兩者在模擬演算上的執行速度。本文所構建的交叉偶合系統，與交叉偶合預補償系統，經多種軌跡測試，證明可行。最後本文以實驗驗證所建立的交叉偶合系統與交叉偶合預補償系統，由實驗可知，本文所發展的系統，在該實驗用 3RPS 並聯式工具機上，具有成效。. i.

(2) The Development and Experiments of Continuous Path Contour Control on a 3RPS Parallel Machine Tool. Student: Wei-Lun Chang. Advisor: Dr. Jih-Hua Chin. Department of Mechanical Engineering National Chiao-Tung University. Abstract Continuous trajectory tracking has been well developed on machine tools of orthogonal nature. For non-orthogonally structured machine tools there is no comparable development. This study developed continuous trajectory cross-coupled contour tracking system, and contour tracking system with pre-compensation for 3RPS parallel machine tool. System structure is first constructed. Dynamic equations are derived by Newton’s method and by virtual work principle. A comparison of execution performance between systems from the two methods is performed.. The constructed cross-coupled contour tracking system and the. system with pre-compensation are evaluated by different types of trajectory. The developed systems are finally implemented on an empirical 3 RPS parallel machine tool. The empirical evaluation has shown that the developed systems are successful and effective.. ii.

(3) 誌謝終於熬過來了，論文的內容是我一點一滴踏實寫出來的，總算也能對自己有個交代，當然了，首先要謝謝我的指導教授秦繼華老師的指導與勉勵，還有之前學長努力的研究，我才有今日的成果，再來就是我的同窗好友小毛(孫晏晞)，我從他的身上學習到很多東西、表哥及學弟雄雄、小孔等，還有曾經在我課業上幫過許多忙的人，真的是很感謝你們！在研究所的日子裡，深深的體會到要學以致用並不是那麼一件容易的事情，而要做" 研究"，更是不容易，在做研究當中，若是沒有附加一些創造力或想像力，而只是單純的一板一眼的做，我想是會非常無趣的，而且常常會做不出來，除此之外，會發現自己是如此的渺小，自己會的東西竟然是這麼的少，所以唸的過程並不是如此的順遂，在當中並無不想過要放棄的念頭，我想我在這邊有很大的成長，也讓我的人生觀有很大的轉變，對於看事情的態度也都用積極、正面的想法去解決，且使我對自己未來所需要面臨的挑戰有更進一步的準備，不僅如此，不管在課業上、情緒、處事及抗壓方面的確都學習到不少。再來就是要謝謝我的女友上惟，是妳讓我在研究所的生活增添了許多色彩，有妳的陪伴，讓我不覺得孤單，在生活上，妳帶給我許多的歡樂，也幫我消除掉許多的壓力，很高興妳能跟我一起走過來，最後要感謝的就是我的父母了，沒有他們的栽培跟用心，我想我不會有今天的成就，在我最困難的時候，我的父母一直支持著我，也讓我在沒有經濟的壓力下順利的完成學位，總算結局是完美的，也沒有辜負他們的期望，真的是很感激我的父母！唯有經過如此的經歷，才能更讓我享受生命，讓我知道我所需要的是什麼！. iii.

(4) 目錄中文摘要 .......................................................................................................................................i 英文摘要 ......................................................................................................................................ii 誌謝 ......................................................................................................................................iii 目錄 ......................................................................................................................................iv 圖目錄 ...............................................................................................................................vi 表目錄 ...............................................................................................................................ix 符號說明 .........................................................................................................................x. 第一章緒論 ........................................................................................................................1 1.1 研究動機 .......................................................................................................................1 1.2 文獻回顧 .......................................................................................................................3 1.3 論文大綱 .......................................................................................................................4 第二章機構特性探討 ...................................................................................................5 2.1 機構特性 .......................................................................................................................5 2.1.1 自由度(Degress of Freedom)分析 .........................................................................5 2.1.2 機構控制分析 ........................................................................................................6 2.2 機構運動學(kinematic)分析..........................................................................................7 2.2.1 桿件與上平板運動學分析(Link and Platform kinematic analysis)......................7 2.3 前向運動學(Forward Kinematic) ............................................................................... 11 2.4 動力學(Dynamics)分析 ..............................................................................................13 2.4.1 牛頓法(Newton-Euler Method) ...........................................................................13 2.4.2 虛功法(Virtual Work Method) .............................................................................16 2.4.3 Platform Jacobian矩陣與Link Jacobia 矩陣[11] .................................................18 2.5 模擬與討論 .................................................................................................................22 軌跡一： .....................................................................................................22 軌跡二： .....................................................................................................23 軌跡三： .....................................................................................................24 軌跡四： .....................................................................................................25 2.6 工作空間(Work Space)分析 .......................................................................................27 2.7 控制準備 .....................................................................................................................29 2.7.1 交叉偶合控制 ......................................................................................................29 2.7.2 交叉偶合預補償控制 ..........................................................................................30 第三章交叉偶合預補償控制與混合位置 / 力量控制 ...............................32 3.1 輪廓誤差模式 .............................................................................................................32 3.1.1 路徑輪廓誤差 ......................................................................................................32 3.1.2 方向輪廓誤差 ......................................................................................................33 3.2 速度預補償 .................................................................................................................35 3.3 多軸並聯式工具機交叉偶合與交叉偶合預補償控制流程規劃 .............................37 3.4 模擬與討論 .................................................................................................................41 ( 1 ) 、軌跡二（第二章）模擬 ..............................................................41 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................41 iv.

(5) Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................42 ( 2 ) 、軌跡四（第二章）模擬 ..............................................................44 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................44 Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................45 ( 3 ) 、具有尖角軌跡的模擬 ....................................................................47 3.5 交叉偶合與預補償應用於力量控制 .........................................................................48 3.5.1 驅動軸(actuator)機械模型...................................................................................48 3.5.2 交叉偶合控制結合力量控制 ..............................................................................52 3.5.3 交叉偶合預補償控制結合力量控制 ..................................................................54 3.6 模擬與討論 .................................................................................................................55 ( 1 ) 、軌跡二（第二章）力量控制模擬 ......................................55 Structure 3：交叉偶合控制結合力量控制(圖 3.23)........................................56 Structure 4：交叉偶合預補償控制結合力量控制(控制架構圖 3.24)............56 ( 2 ) 、軌跡四（第二章）力量控制模擬 ......................................57 Structure 3：交叉偶合控制結合力量控制(圖 3.23)........................................57 Structure 4：交叉偶合預補償控制結合力量控制(圖 3.24)............................57 3.7 混合位置/力量控制 ....................................................................................................58 3.7.1 接觸環境模型探討 ..............................................................................................60 3.7.2 交叉偶合結合混合位置/力量控制 .....................................................................62 第四章實驗與討論 ......................................................................................................63 4.1 實驗機台與校正 .........................................................................................................63 4.2 開迴路(open loop)系統實驗.......................................................................................66 4.3 未偶合(uncouple system)系統實驗過程....................................................................72 4.4 交叉偶合與預補償系統架構實驗 .............................................................................76 ( 1 ) 、軌跡二（第二章）實驗 ..............................................................78 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................78 Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................79 ( 2 ) 、軌跡四（第二章）實驗 ..............................................................80 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................80 Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................81 ( 3 ) 、具尖角軌跡實驗 ................................................................................83 ( 4 ) 、漩渦狀軌跡實驗 ................................................................................85 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................86 Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................87 ( 5 ) 、軌跡二（第二章）實驗驗證 ..................................................88 Structure 1：交叉偶合控制(CCS，圖 3.3) ......................................................89 Structure 2：交叉偶合預補償控制(CCPM，圖 3.4).......................................89 第五章結論 ......................................................................................................................95 參考文獻 ....................................................................................................................................96. v.

(6) 圖目錄圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖. 2.1 3RPS並聯式機構示意圖 ...............................................................................................6 2.2 控制流程圖 ...................................................................................................................6 2.3 上平板、桿件與下平板間的空間向量圖 ...................................................................8 2.4 工作空間軌跡圖 ...........................................................................................................9 2.5 桿件長度變化圖 ...........................................................................................................9 2.6 Newton-Raphson method解前向運動學流程圖 .........................................................12 2.7 第i桿件的外力圖[10] .................................................................................................14 2.8 第i驅動桿旋轉座標系[11]..........................................................................................16 2.9 軌跡一的工作空間圖 .................................................................................................22 2.10 軌跡一的桿長變化圖 ................................................................................................23 2.11 軌跡一的桿件輸出力圖 ...........................................................................................23 2.12 軌跡二的工作空間圖 ...............................................................................................23 2.13 軌跡二的桿長變化圖 ...............................................................................................24 2.14 軌跡二的桿件輸出力圖 ...........................................................................................24 2.15 軌跡三的工作空間圖 ...............................................................................................24 2.16 軌跡三的桿長變化圖 ...............................................................................................25 2.17 軌跡三的桿件輸出力圖 ...........................................................................................25 2.18 軌跡四的工作空間圖 ...............................................................................................25 2.19 軌跡四的桿長變化圖 ...............................................................................................26 2.20 軌跡四的桿件輸出力圖 ...........................................................................................26 2.21 工作空間流程圖 .......................................................................................................28 2.22 工作空間圖 ...............................................................................................................29 2.23 輪廓誤差與預補償幾何關係圖[6] ..........................................................................30 2.24 交叉偶合控制示意圖[6] ..........................................................................................30 2.25 交叉偶合預補償控制示意圖[6] ..............................................................................31 3.1 輪廓誤差示意圖 .........................................................................................................32 3.2 預補償示意圖 .............................................................................................................35 3.3 並聯式工具機交叉偶合控制流程圖 .........................................................................38 3.4 並聯式工具機交叉偶合預補償控制流程圖 .............................................................40 3.5 軌跡二交叉偶合控制(CCS)期望與實際軌跡示意圖 ...............................................41 3.6 軌跡二工作空間進給誤差與輪廓誤差示意圖(CCS) ...............................................42 3.7 軌跡二驅動桿進給誤差與輪廓誤差示意圖(CCS) ...................................................42 3.8 軌跡二交叉偶合預補償控制(CCPM)期望與實際軌跡示意圖 ...............................42 3.9 軌跡二工作空間進給誤差與輪廓誤差示意圖(CCPM) ...........................................43 3.10 軌跡二驅動桿進給誤差與輪廓誤差示意圖(CCPM) .............................................43 3.11 軌跡四交叉偶合控制(CCS)期望與實際軌跡示意圖 .............................................44 3.12 軌跡四工作空間進給誤差與輪廓誤差示意圖(CCS) .............................................45 3.13 軌跡四驅動桿進給誤差與輪廓誤差示意圖(CCS) .................................................45 3.14 軌跡四交叉偶合預補償控制(CCPM)期望與實際軌跡示意圖 .............................45 3.15 軌跡四工作空間進給誤差與輪廓誤差示意圖(CCPM) .........................................46 3.16 軌跡四驅動桿進給誤差與輪廓誤差示意圖(CCPM) .............................................46 3.17 具尖角軌跡示意圖 ...................................................................................................47 vi.

(7) 圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖. 3.18 具尖角軌跡工作空間輪廓誤差CCS與CCPM比較圖 ............................................48 3.19 馬達與滾珠螺桿示意圖 ...........................................................................................49 3.20 馬達輸出電壓與上桿件位移方塊線圖[31] ............................................................51 3.21 力量控制示意圖[29] ................................................................................................52 3.22 三軸平台力量控制示意圖 .......................................................................................52 3.23 三軸平台交叉偶合控制結合力量控制 ...................................................................53 3.24 三軸平台交叉偶合預補償控制結合力量控制 .......................................................54 3.25 軌跡二工作空間進給誤差與輪廓誤差示意圖(FC+CCS)......................................56 3.26 軌跡二工作空間進給誤差與輪廓誤差示意圖(FC+CCPM) ..................................56 3.27 軌跡四工作空間進給誤差與輪廓誤差示意圖(FC+CCS)......................................57 3.28 軌跡四工作空間進給誤差與輪廓誤差示意圖(FC+CCPM) ..................................57 3.29 混合位置/力量控制架構圖[24] ...............................................................................58 3.30 接觸力的系統模型示意圖 .......................................................................................60 3.31 系統與外界環境接觸力方塊圖 ...............................................................................61 3.32 交叉偶合結合混合位置/力量控制 ..........................................................................62 4.1 3RPS實際機台示意圖 .................................................................................................63 4.2 桿件一步階響應圖 .....................................................................................................65 4.3 桿件二步階響應圖 .....................................................................................................65 4.4 桿件三步階響應圖 .....................................................................................................66 4.5 桿件進給速率圖 .........................................................................................................67 4.6 開迴路系統實驗桿件理想與實驗位置比較圖 .........................................................67 4.7 開迴路系統實驗桿件誤差圖 .....................................................................................68 4.8 開迴路系統實驗工作空間理想與實驗比較圖 .........................................................68. 圖 4.9 工作空間進給速率 v = ( 0 0.54 0.54 ) 之桿件進給速率圖 ..................................69 T. 圖 4.10 工作空間進給速率 v = ( 0 0.54 0.54 ) 之桿件理想與實驗位置比較圖 ............69 T. 圖 4.11 工作空間進給速率 v = ( 0 0.54 0.54 ) 之桿件誤差圖 ........................................70 T. 圖 4.12 進給速率 v = ( 0 0.54 0.54 ) 之工作空間理想與實驗位置比較圖 ....................70 T. 圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖. 4.13 開回路系統直線運動軌跡實驗 ...............................................................................71 4.14 開回路系統直線運動軌跡實驗工作空間誤差圖 ...................................................72 4.15 開回路系統直線運動軌跡實驗各桿件誤差圖 .......................................................72 4.16 Uncoupled System控制圖 ..........................................................................................73 4.17 PID 控制理想與實驗比較圖(第二章軌跡二) .........................................................74 4.18 PID 控制桿件誤差圖(第二章軌跡二) .....................................................................74 4.19 PID 控制工作空間誤差圖(直線運動軌跡) .............................................................75 4.20 PID 控制桿件誤差圖(直線運動軌跡) .....................................................................75 4.21 Borland C++實驗人機介面圖 ...................................................................................77 4.22 軌跡二實驗交叉偶合控制(CCS)期望與實際軌跡示意圖 .....................................78 4.23 軌跡二實驗工作空間進給誤差與輪廓誤差示意圖(CCS) .....................................78 4.24 軌跡二實驗驅動桿進給誤差與輪廓誤差示意圖(CCS) .........................................78 4.25 軌跡二實驗交叉偶合預補償控制(CCPM)期望與實際軌跡示意圖 .....................79 4.26 軌跡二實驗工作空間進給誤差與輪廓誤差示意圖(CCPM) .................................79 4.27 軌跡二實驗驅動桿進給誤差輪廓誤差示意圖(CCPM) .........................................79 4.28 軌跡四實驗交叉偶合控制(CCS)期望與實際軌跡示意圖 .....................................80 vii.

(8) 圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖圖. 4.29 4.30 4.31 4.32 4.33 4.34 4.35 4.36 4.37 4.38 4.39 4.40 4.41 4.42 4.43 4.44 4.45 4.46 4.47. 軌跡四實驗工作空間進給誤差與輪廓誤差示意圖(CCS) .....................................81 軌跡四實驗驅動桿進給誤差與輪廓誤差示意圖(CCS) .........................................81 軌跡四實驗交叉偶合預補償控制(CCPM)期望與實際軌跡示意圖 .....................81 軌跡四實驗工作空間進給誤差與輪廓誤差示意圖(CCPM) .................................82 軌跡四實驗驅動桿進給誤差與輪廓誤差示意圖(CCPM) .....................................82 具尖角軌跡實驗桿件進給速率圖 ...........................................................................83 具尖角軌跡實驗CCS與CCPM比較圖 ....................................................................85 漩渦狀軌跡工作空間與各桿位置圖 .......................................................................86 漩渦狀軌跡實驗交叉偶合控制(CCS)期望與實際軌跡示意圖 .............................86 漩渦狀軌跡實驗工作空間進給誤差與輪廓誤差示意圖(CCS) .............................87 漩渦狀軌跡實驗驅動桿進給誤差與輪廓誤差示意圖(CCS) .................................87 漩渦狀軌跡實驗交叉偶合預補償控制(CCPM)期望與實際軌跡示意圖 .............87 漩渦狀軌跡實驗工作空間進給誤差與輪廓誤差示意圖(CCPM) .........................88 漩渦狀軌跡實驗驅動桿進給誤差與輪廓誤差示意圖(CCPM) .............................88 驗證軌跡二實驗各桿件誤差與輪廓誤差示意圖(CCS) .........................................89 驗證軌跡二實驗各桿件誤差與輪廓誤差示意圖(CCPM) .....................................89 軌跡二輪廓誤差實驗與模擬比較圖 .......................................................................91 軌跡四輪廓誤差實驗與模擬比較圖 .......................................................................92 尖角軌跡輪廓誤差實驗與模擬比較圖 ...................................................................93. viii.

(9) 表目錄表表表表表表表表表表表表表. 2.1 平行式機構規格表 .....................................................................................................21 2.2 牛頓法與虛功法執行效率比較表（時間單位：秒） .............................................26 3.1 軌跡二PID控制參數表 ...............................................................................................41 3.2 軌跡二CCS與CCPM IAE比較表 ...............................................................................43 3.3 軌跡四PID控制參數表 ...............................................................................................44 3.4 軌跡四CCS與CCPM之IAE比較表 ............................................................................46 3.5 具尖角軌跡PID控制參數表 .......................................................................................47 3.6 具尖角軌跡CCS與CCPM IAE比較表 .......................................................................48 3.7 軌跡二交叉偶合結合力量控制之控制器參數表 .....................................................55 3.8 軌跡二FC+CCS與FC+CCPM之IAE比較表..............................................................56 3.9 軌跡四交叉偶合結合力量控制之控制器參數表 .....................................................57 3.10 軌跡四FC+CCS與FC+CCPM IAE比較表...............................................................58 4.1 桿件步階響應性能表 .................................................................................................66. 表 4.2 工作空間進給速度 v = ( 0 0.54 0.54 ) 實驗桿件IAE比較表 ...............................70 T. 表 4.3 工作空間進給速度 v = ( 0 0.54 0.54 ) 實驗工作空間IAE比較 ...........................71 T. 表表表表表表表表表表. 4.4 軌跡二未偶合控制架構實驗PID參數表 ...................................................................73 4.5 直線運動軌跡未偶合控制架構實驗PID參數表 .......................................................73 4.6 未偶合PID控制_軌跡二實驗IAE值 ..........................................................................75 4.7 未偶合PID控制_直線運動軌跡實驗IAE值 ..............................................................76 4.8 軌跡二實驗CCS與CCPM IAE比較表 .......................................................................80 4.9 軌跡四實驗CCS與CCPM IAE比較表 .......................................................................82 4.10 具尖角軌跡實驗CCS與CCPM IAE比較表 .............................................................85 4.11 漩渦狀軌跡實驗控制器增益値表 ...........................................................................85 4.12 漩渦狀軌跡軌跡實驗CCS與CCPM IAE比較表 .....................................................88 4.13 驗證圓軌跡實驗CCS與CCPM IAE比較表 .............................................................90. ix.

(10) 符號說明 F ：degrees of freedom of a mechanism λ ：degrees of freedom of the space in which a mechanism is intended to function n ：number of links in a mechanism，including the fixed link j ：number of joints in a mechanism，assuming that all the joints are binary fi ：degrees of relative motion permitted by joint i R(ϕ , φ ,θ ) ：Roll-Pitch-Yaw Euler Angles ai ：the i-th universal joint bi ：the i-th ball joint Li ：link vector of i-th link li ：length of i-th link ni ：the unit vector of i-th link Si ：position of i-th ball joint in generalized coordinate •. Si ：the velocity of i-th ball joint ••. Si ：the acceleration of i-th ball joint •. Li ：axial velocity of i-th link. wi ：the angular velocity of i-th link ••. Li ：axial acceleration of i-th link α i ：angular acceleration of i-th link J ：Jacobian matrix q ：generalized coordinate of z 、 α 、 β riu ：distance of the mass center of upper link to ball joint rid ：distance of the mass center of lower link to universal joint miu ：mass of upper link mid ：mass of lower link aiu ：acceleration of mass center of upper link aid ：acceleration of mass center of lower link ui 、 vi ：universal joint direction vector I u ：mass moment of inertia of upper link I d ：mass moment of inertia of lower link I p ：mass moment of inertia of upper platform G ：gravitational acceleration ••. x p ：acceleration of the mass center on upper platform m p ：mass of upper platform fi ：actuator output force Ri ：transformation matrix from i-th moving link to base coordinate. x.

(11) J p ：platform Jacobian i. J id 、 i J iu ：link Jacobian τ ：actuator output force E ：tracking error ε ：contour error Vk ：precompensation velocity vector U ：control signal Er ：contour error of workspace. Eo ：the orient error of α 、 β V ：average unit velocity of desired and actual velocity Eor ：the orient contour error of α 、 β. ErLi ：the link contour error L f ,i ：reference position of i-th link EL ,i ：position error of i-th link Li ( S ) ：transfer function of i-th link IAE：Integral Absolute-Error CCS：cross coupled system CCPM：multiple cross coupled precompensation method f e ：the output force of workspace. ~. f p ：position controller. ~. f f ：force controller. FC：force control T：sampling time Pd ：desired position Pa ：actual position Va ：actual tangent unit velocity vector Vd ：desired tangent unit velocity vector. xi.

(12) 第一章緒論 1.1 研究動機在早期機器人的相關研究中多半以串聯式機器人為主，串聯式機器人具有較大的工作空間以及較高的操控靈敏等優點，但因其具有懸臂樑結構，後級驅動軸是前級驅動軸之負載，使得此類機器人的剛性較低、承載力低、在高速以及高負載的情況下運作容易產生振動，以及各關節誤差會累積在輸出端等缺點，使得精度難以提昇。並聯式機器人的設計早在 1940 年代即被提出，1965 年 Stewart 提出六軸機器人又稱為史都華平台(Stewart Platform)，當時此機構的設計概念是用來製作飛行模擬器[1]，之後並聯式機器人不論是在機構設計或控制方面的研究便不斷的進行。與傳統串聯式機器人比較，並聯式機器人有以下之優點： 1. 高精度：並聯式機器人不會累積各支柱誤差，因此高精度的要求容易達成。 2. 高剛性：因為史都華平台每一根驅動桿皆為二力構件(two-force member)，僅在軸向受壓力或張力，應力與變形都相對於串聯式機器人來的小。 3. 低慣性：並聯式機器人的機械結構較串聯式機器人簡單，減少了許多框架(frame)及移動軸(moving axis)的質量，因此移動慣性小，在相同的驅動力下可以有較大的加速度，系統反應頻寬也較大，適合用來解決高速切削的問題。 4. 高力重比(force to weight ratio)：並聯式機器人可以將負載分散於各致動軸上，所以能提供較高的力重比。 5. 構造模組化：由於並聯式機器人的構造較簡單，各驅動桿及接頭均可採用模組化設計，所以在設計、生產、校正、維護的流程可更精簡，製造及庫存成本也會降低。. 在 CNC 機器運動控制方面，輪廓誤差補償的技術是晚近的發展重點。自 Sarachik 和 Ragazzini [2]提出交叉偶合系統(Cross-Coupled System)後，交叉偶合結構歷經不斷改良、演化，已成為運動軌跡控制中廣為使用的一種方法。其中 Koren [3]提出對稱性交叉偶合控制器的架構，證實於雙軸控制系統中使用偶合控制可以改善輪廓誤差，在 1991 年又提出可變增益交叉偶合控制器[4]，根據加工路徑形狀修正其輪廓誤差的計算方式與控制器增益值。Huan [5] 提出一種工具機的軌跡預補償方法，利用速度預補償，得到良好的軌跡精度，Chin and Tsai [6] 將預補償法進一步發展到機器人上，後來 Chin and Lin [7] 整 1.

(13) 合預補償與交叉偶合兩項特徵，提出工具機的交叉偶合與預補償方法，之後 Chin et al [8] 用模糊控制器來降低輪廓控制中的計算量，改進高速進給下的輪廓精度，Lue et al [9] 將預補償法延伸構建在複合型五軸工具機的運動軌跡控制上，不過尚未考慮機器動態，也無實驗驗證。由於機器進行加工時，外界負載、摩差力和慣性矩等皆會影響循跡精度，另外 CNC 控制指令的執行速度、加減速度控制、伺服控制等特性亦會影響機器的動態反應，除此之外，震動亦是影響動態的另ㄧ重要因素，這些都是在到目前為止的研究中，尚未充分觸及的議題。因此本論文的目的，在於針對一個三軸實驗用運動平台，做包含上述考量之系統研發與實驗驗證。本論文建立系統的位置控制架構，推導軸空間與工作空間的轉換方程式，建立交叉偶合與交叉偶合預補償兩種方法控制，此外，架構是以工作空間為基礎，所以如何利用前向運動學(Forward kinematic)來做線上工作空間(workspace)的即時(Online)控制以算出輪廓誤差的補償以達完整的閉迴路控制是關鍵環節，論文中更以模擬與實驗來驗證所提之架構。在系統動態中，動態行為是以動力學(Dynamic)為基礎所建立的，本文中實驗用三軸平台的動態模型，係利用牛頓法[10]與虛功法[11]來推導建立，並做兩種動態模型的比較。對於控制而言，準確的動態模型十分重要，建立動態模型後，可以利用電腦數值模擬做控制與分析，選擇及設計適合的控制方法，且在動態模型的分析中將知道作用在接頭上面的力和力矩，因此將能事先選擇能匹配整個系統的軸承或驅動器。在控制方面，除了以交叉偶合預補償為基礎架構之外，更利用推導出來的動態模型結合交叉偶合與預補償來做力量控制[12]，再以力量控制為基礎去做軌跡的追蹤，並分析交叉偶合與預補償方法在軌跡控制方面的差異性，在[12]中對力量控制有很完整的敘述，對於混合位置/力量(Hybrid Position/Force Control)控制也有很完整的說明，在實務上，力量與位置往往必須同時考量，如點焊機器人，一方面必須與外界環境保持一接觸力來完成點焊的工作，另ㄧ方面又必須控制它的位置，所以必須同時考慮力量與位置兩種因素來完成工作，除此之外，在大多數的工具機應用場合也都是必須考慮力量與位置同時控制，才能達到高品質加工。本論文對於平行式機構線上即時(online)工作空間(workspace)的補償控制，及交叉偶合與交叉偶合預補償兩種即時控制的架構，有完整的發展與驗證，在系統動態的力量控制中，亦將預補償控制方法運用於力量控制上，且討論交叉偶合與預補償方法在輪廓誤差上的表現，並分析輪廓誤差在力量控制架構中的影響。. 2.

(14) 1.2 文獻回顧在探討機構動態模型之前，必須先決定機構運動學(kinematic)，平行式機構的運動學可分為前向運動學(forward kinematics)與逆向運動學(inverse kinematic)兩部份，給定一組支柱關節的位移量，分析活動平台位置與方向屬於前向運動學問題，相反的，給定活動平台的一組位置與方向，求解各支柱所需位移量的問題，則為逆向運動學問題。有關平行式機構問題，如早期史都華平台的研究，大多集中在分析工作空間大小、奇異點位置分佈與迴避問題、順向運動學、機敏性(dexterity)、與機械效能分析等，一直到 90 年代初期以後，才有動態模型和運動控制等相關的研究被發表出來。對於平行式機構動態方程式的推導，最常使用的方法有牛頓法[10]、[13,~15]，虛功法 [11]、[16, 17]，其中[17]定義了 Link Jacobian 來簡化系統的動態方程式，其方法證實比牛頓法來的更有效率。力量控制的目的在於使手端（end-effector）與環境接觸時，其力量保持在一穩態值，用來保護機器及工件，及維持工作精度。在平台應用方面，隨著使用環境不同其控制需求有所差異，在飛行模擬器上要求加速度與軌跡，娛樂用運動模擬器則較寬鬆，應用在工具機上時，除位置控制外，亦須考慮力量控制，通常是將位置控制器和力量控制器分離設計，將兩者在工作空間中分離及線性化，發展線性控制器，追蹤期望之位置及力量軌跡，以切向動作最為位置控制器、正向力作為力量控制器之設計基本考量。ㄧ般而言，力量控制包含以下幾種方法：剛性控制(Stiffness control)[18]、阻抗控制(Impedance. control)[19]、混合位置/力量控制(Hybrid Position/Force control)[12]、[20~24]，在剛性控制方面，主要是控制系統的剛性（順應性），使控制器的作用像一個彈簧。而阻抗控制最先由 Hogan[19]所提出，阻抗的意思即剛性再加上阻尼，其目的是要達到一目標阻抗，使得接觸力可以被控制在一定的範圍內，混合位置/力量控制最先是由 Paul 與. Shimano[20]提出，主要是結合位置與力量的資訊，配合軌跡中之各種環境的限制，算出所需要的控制力矩。. 3.

(15) 1.3 論文大綱第一章緒論：說明論文研究動機，並對並聯式機構與控制做文獻回顧第二章機構特性探討：說明整體的機構組成架構，並分析機構的運動學、動力學。動力學部份對牛頓法與虛功法的執行效率做比較，探討兩者應用在即時(online)控制上的優劣。做系統模擬，最後分析機構之工作空間與初步探討控制方法。第三章交叉偶合預補償控制與混合位置/力量控制：建立完整的閉迴路控制，以作為後續實驗驗證的基礎，控制法則考慮了交叉偶合、交叉偶合預補償、力量控制，及混合位置/力量控制，而對於誤差量的修正，係利用 PID 控制器，最後以模擬來比較各控制法則第四章實驗結果與討論：本章完成了第三章所提的控制架構的實驗，並討論實驗的結果，且進一步確定適當的控制方法第五章實驗心得與結論：此章節為實驗後的結論，及實驗上的心得與改進. 4.

(16) 第二章機構特性探討在本章節中將討論實驗用三軸平行式機構的機構特性，首先先討論機構構型，其中包含上平板、下平板、驅動桿、以及伺服馬達，且介紹機器的整個運作流程，再來將針對機構桿件做位置分析、速度分析、加速度分析，以及上平板的運動學(kinematic)分析，及機構動力學(Dynamic)分析，此處將利用牛頓法[10]與虛功法[11]兩種方法推導，並做比較。本文所探討的，是一個 3RPS 平行式機構(Revolute-Prismatic-Spherical)，其組成方式為上平板與桿件的球接頭相接，下平板與桿件的萬象接頭相接，其中萬象接頭鎖死一個旋轉的自由度，透過驅動桿的長度變化可得到三個自由度的運動，其組裝示意圖如圖. (2.1)所示，其中 ai ,i =1~3 代表球接頭、 bi ,i =1~3 代表萬象接頭、上平板與下平板的形心到球接頭與萬象接頭的半徑分別為 105 及 150 (mm)。. 2.1 機構特性 2.1.1 自由度(Degress of Freedom)分析此平行式機構具有三個自由度，自由度的推導可由[11]得知： F = λ (n − j − 1) + ∑ f i. (2.1). 其中. F ：degrees of freedom of a mechanism. λ ：degrees of freedom of the space in which a mechanism is intended to function n ：number of links in a mechanism，including the fixed link. j ：number of joints in a mechanism，assuming that all the joints are binary fi ：degrees of relative motion permitted by joint i 所以可得其自由度為. F =6×(8-9-1)+(3 × 1+3 × 1+3 × 3)=3 由接頭的拘束關係，我們選擇工作空間的自由度為 F = fun( z, α , β ) ，除此之外，亦定義. FL = fun(l1 , l2 , l3 ) 為軸空間(Link Space)的參數。. 5.

(17) 圖 2.1 3RPS 並聯式機構示意圖 2.1.2 機構控制分析本小節將介紹整個機構的控制流程。在結構上，此並聯式平行機構是由三個 AC 伺服馬達(AC SERVO DRIVER)帶動三根驅動桿的伸長縮短，然後透過電腦，經過傳輸線及轉接卡控制整個機台。在控制卡方面，選用的為 ADLINK PCI-8134 控制卡，為一四軸控制的控制卡，而伺服馬達為 Panasonic MINAS MSDA5A1A1A，在驅動軸上皆附有光學尺負責實際訊號的量測。其控制流程圖如圖所示：. 圖 2.2 控制流程圖. 6.

(18) 2.2 機構運動學(kinematic)分析本小節將以空間中的幾何向量分析驅動桿及上平板的運動學及動力學。首先建立上平板與下平板的轉換矩陣，此處，以尤拉角(Euler Angle)[11]來表示此轉換矩陣，尤拉角表示式為： ⎡1 0 R ( x, ϕ ) = ⎢⎢ 0 cϕ ⎢⎣ 0 sϕ. ⎡ cφ R ( y, φ ) = ⎢⎢ 0 ⎢⎣ − sφ ⎡cθ R ( z ,θ ) = ⎢⎢ sθ ⎢⎣ 0. 0 ⎤ − sϕ ⎥⎥ cϕ ⎥⎦. (2.2). 0 sφ ⎤ 1 0 ⎥⎥ 0 cφ ⎥⎦. (2.3). − sθ cθ 0. 0⎤ 0 ⎥⎥ 1 ⎥⎦. (2.4). 上述中 c表示cosine 、 s表示sine 。上面的表示式分別代表對 x 、 y 、 z 的旋轉，所以整個轉換矩陣可以寫成 ⎡cφ cθ R (ϕ , φ , θ ) = R( z ,θ ) R( y, φ ) R( x, ϕ ) = ⎢⎢ sφ cθ ⎢⎣ − sθ. cφ sθ sϕ − sφ cϕ sφ sθ sϕ + cφ cϕ cθ sϕ. cφ sθ cϕ + sφ sϕ ⎤ sφ sθ cϕ − cφ sϕ ⎥⎥ ⎥⎦ cθ cϕ. 2.2.1 桿件與上平板運動學分析(Link and Platform kinematic analysis) 上平板、桿件與下平板間的空間向量關係見圖 2.3：. 7. (2.5).

(19) 圖 2.3 上平板、桿件與下平板間的空間向量圖圖中， ai 、 bi 分別代表球接頭與萬象接頭，在圖中已將球接頭與萬象接頭視為與桿件一體， qi = R × ai ，其中 R 代表上平板相對於下平板的轉換矩陣，由圖可以看出. Li = qi + t − bi. (2.6). Li 代表桿件向量，故可得桿長 li = Li i Li ，桿長單位向量 ni =. Li ，由(2.6)可看出給定 t (即 li. 工作空間參數 z, α , β )，則可以求出各軸的桿長，此即為逆向運動學(Inverse Kinematic)，由逆向運動學，再加上桿長最大、最小伸長長度限制、以及接頭限制的關係，可以求出此並聯式機構的工作空間(Work Space)。該並聯式機構桿長規格為(其中已將球接頭與萬象接頭大小包含進桿長中)：250< li <315 (mm)，且 246<z<310 (mm)、. −0.38 < α < 0.38(rad ) 、 −0.38 < β < 0.38(rad ) 。以下為模擬給訂一組工作空間的軌跡，然後求出各軸的桿長變化，假設. t = 0 ~ 5(秒 ). z. α. β. Sampling time. 260 + 5 × t. 0 + 0.05 × t. 0 + 0.02 × t. 0.1 (s). (mm). (rad). (rad). 8.

(20) 圖 2.4 工作空間軌跡圖. 圖 2.5 桿件長度變化圖此時要分析球接頭的速度與加速度，由圖可看出 Si = t + qi = t + R × ai. (2.7). 將(2.7)式對時間微分可得球接頭的速度為 •. •. Si = t + w × qi. (2.8). 其中 w 代表上平板的角速度，將(2.8)式對時間微分可得球接頭的加速度為 ••. ••. Si = t + α × qi + w × ( w × qi ). (2.9). 其中 α 代表上平板的角加速度。再來要討論桿件的軸向速度與角速度，此處分析桿件的軸向速度與加速度是為了可以得到一 Jacobian matrix，此 Jacobian matrix 有助於前向運動學(Forward Kinematic)及動力學的推導，由(2.8)式與 ni 做內積可得球接頭速度大小在桿長方向的分量，可得桿件的軸向速度為 •. •. Li = Si ini. (2.10). 而桿件角速度為 •. n ×S wi = i i li. (2.11). 9.

(21) ，注意(2.9)式的 wi 是桿件的角速度，並不是上平板的角速度，此時同樣的將(2.10)式對時間微分可得桿件的軸向加速度 ••. ••. •. Li = S i ini + S i i( wi × ni ). (2.12). 且由驅動軸軸向方向不會旋轉的特性，可知拘束條件 ni i wi = 0 、 ni iα i = 0 及 ni × wi = 0 ，其中 α i 即為桿件的角加速度，最後整理可得 ••. •. n × S − 2 Li wi αi = i i li. (2.13). 由(2.10)式，可知 •. •. •. Li = Si ini = (t + w × qi )ini. (2.14). 將(2.14)式展開可得三條方程式 •. •. •. •. L1 = z in1 + (α + β )i(q1 × n1 ) •. •. •. (2.15). •. L 2 = z in2 + (α + β )i(q2 × n2 ) •. •. •. (2.16). •. L 3 = z in3 + (α + β )i(q3 × n3 ). (2.17). 將(2.14)式化成矩陣表示式可得[10] ⎛•⎞ ⎜z⎟ • ⎜•⎟ Li = J −1 ⎜ α ⎟ ⎜•⎟ ⎜β ⎟ ⎝ ⎠. (2.18). 或表示成 δ L = J −1δ P 、 p = [ z , α , β ]. (2.19). 此處要注意到為什麼要用 J −1 來表示，因為 Jacobian Matrix 的定義為對軸空間變數(廣義座標)的偏微分，所以此處所得到的 Jacobian Matrix 是 inverse 的。由上式可知當輸入 p 對時間的變化，經過 Jacobian matrix 的轉換，則可求出各軸桿長對時間的變化，其中 J 即為 Jacobian matrix，可表示成 ⎛ n1,T z ⎜ J = ⎜ n2,T z ⎜ n3,T z ⎝. (q1 × n1 )Tx (q2 × n2 )Tx (q3 × n ). T 3 x. (q1 × n1 )Ty ⎞ ⎟ (q2 × n2 )Ty ⎟ (q3 × n3 )Ty ⎟⎠. −1. (2.20). 此機構的奇異點會發生在當 det( J ) = 0 ，所以在分析機構時，需注意是否會碰到奇異點，或者規避奇異點，此部分已有相當的討論[25]。由(2.18)可看出桿件與上平板的關係為以 10.

(22) Jacobian matrix 為轉換因子，其亦可想成如前述上平板座標係對下平板座標系中存在一. 轉換矩陣的關係，上述運動學的討論是之後要推導動力學的準備工作。. 2.3 前向運動學(Forward Kinematic) 在之前提到當給定 ( z , α , β ) 可以得到三根軸的桿長，此即為逆向動學，而前向運動學則為：給訂三根軸軸長，然後算出 ( z , α , β ) ，但是前向動學的解並不是如此簡單，由於機構為並聯式機構，且因為接頭限制的關係，所以當給定三根軸長，所得到的 ( z , α , β ) 並不是唯一解，但是實際上符合平板的姿態只有一解，所以這就是前向運動學困難的地方。在此處，將利用 Newton-Raphson method[9]去解前向運動學的數值解，基本上，此問題即為非線性方程式求根的問題，首先，定義 F (q) = l (q ) − lgiven. (2.21). l (q) 為由 inverse kinematic 所求得的桿長，lgiven 為給定的桿長，若當 q = [ z, α , β ] 為所需要的前向運動學的解時，則(2.21)式則會等於零。將(2.21)式寫成三個封閉迴路方程式 3. 2 F (q) = ∑ (li (q ) 2 − lgiven ). (2.22). i =1. 將式(2.22)式以泰勒級數展開可得 3. F (q ) |q = q ( n +1) = F (q ) |q = q ( n ) + ∑ ( i =1. ∂F |q = q ( n ) )Δq ∂q. (2.23). ∂F1 (q) ⎞ ⎟ ∂β ⎟ ∂F2 (q) ⎟ ⎟ ∂β ⎟ ∂F3 (q) ⎟ ⎟ ∂β ⎠. (2.24). 其中 ⎛ ∂F1 (q ) ⎜ ⎜ ∂z ⎜ ∂F (q) ∂F |q = q ( n ) = ⎜ 2 ∂q ⎜ ∂z ⎜ ∂F3 ( q) ⎜ ⎝ ∂z. 由[9]可知，. ∂F1 (q ) ∂α ∂F2 (q) ∂α ∂F3 (q) ∂α. ∂F 等於 J −1 ，如此可得 ∂q. q( n +1) = q( n ) − (. ∂F |q = q ( n ) ) −1 ( F (q ) |q = q ( n +1) − F (q ) |q = q ( n ) ) ∂q. (2.25). 整理可得 q( n +1) = q( n ) − J (q( n ) )(l (q( n ) ) − lgiven ). (2.26). 11.

(23) 上述推導步驟為. Step 1：給定初始值 q = [ z, α , β ] 、及實際桿長 lgiven ，並由 inverse kinematic 算出 l (q( n ) ) Step 2：由 J (q( n ) )(l (q( n ) ) − lgiven ) 可得誤差值 Δq Step 3：由(2.26)的式子可計算出新的姿態，若所求的 Δp 沒有在容許誤差範圍內，則將 q( n +1) 設為 q( n ) ，再將它代入式(2.26)去算新的誤差值 Δp. Step 4：重複上述步驟直到誤差值 Δp ≤ 容許誤差，即可得到所求的 [ z, α , β ] 在步驟 1 中的初始姿態 q 必須猜測的到，若是不正確的 q ，有可能會導致不正確的解，通常是以前次計算所得到的上平板姿態為猜測值，也因此在過程中所得到的誤差值是以其為基準，但須注意在算 Jacobian matrix 時需注意是否會有奇異點發生。. 圖 2.6 Newton-Raphson method 解前向運動學流程圖以下為利用 Newton-Raphson method 來算前向運動學的電腦模擬，參數數值如下表： lgiven = [256.28 268.6 297.07]. 猜測值 q(1) = [280,pi/22,pi/22]. q = [270,pi/18,pi/20]. 容許誤差 Δq = 1× 10−6. 疊帶法過程如下表. Z(mm). α (rad). β (rad). 1. 280. pi/22. pi/22. 2. 270.0001. 0.1745. 0.1571. 3. 270.0000. 0.1745. 0.1571. 12.

(24) 由上表可看出 Newton-Raphson method 收斂速度很快，僅需三次的疊帶即可達到所需的容許誤差，但若是不正確的猜測值很可能會造成無法收歛，或不正確的答案。. 2.4 動力學(Dynamics)分析在此處，將利用牛頓法(Newton-Euler Method)[10]與虛功法(Virtual Work Method)[11]來推導此平行機構的動力學分析，而為什麼要分析此機構的動力學？其目的在於將此平行式機構建立一數學模型，利用此數學模型來表示系統的物理特性，即系統的動態特性，進而利於分析與控制，此數學模型是利用給予工作空間參數 [ z , α , β ] ，然後求出三根軸所需的出力，此稱為逆向動力學(Inverse Dynamics)，其可用來做力量的控制(Force. Control)，而順向動力學(Forward Dynamics)則可用來做工作空間位置控制(Position Control)。在牛頓法上，利用座標之間的轉換，力量、力矩的平衡來獲得物體的受力狀況。牛頓法的特點在於觀念清晰，只要將外力圖與慣性力圖以相對應的方程式來表示即可，除此之外，對於串聯式機構，只要一層一層的做好座標轉換的工作，就可以把系統表現出來，但是相對於元件多且互相偶合的並聯式機構而言，牛頓法就顯的方程式多而且太煩雜。相對的，虛功法和 Lagrangian 法近年來逐漸被利用在並聯機構的動態分析上，能量屬於純量而不是向量，利用此觀點，得以大幅度的減少方程式的數目，因此增加了運算的效率，對於數學轉換複雜的並聯式機構而言特別適合。虛功法的核心觀念在於能量守恆，以並聯機構而言，平台與驅動機構的移動或轉動，必然來至於各驅動器所輸入的能量，因此可以能量的觀點，將系統簡潔的表示出來。在推導之前，為了簡化動態模型以便於之後控制系統的設計，將提出幾個適當的假設，又不會影響模擬系統的真實性，其中包含. (1) 假設可動平板為剛體 (2) 忽略節點及軸與軸之間的摩擦力 2.4.1 牛頓法(Newton-Euler Method). 此動力學分析包含了驅動桿及上平板的動態方程式，在使用牛頓法之前，首先須了解機構的受力情況，先取出單一桿件做外力圖的分析，外力圖如圖 2.7 所示. 13.

(25) 圖 2.7 第 i 桿件的外力圖[10] 圖中， riu 、 rid 分別為球接頭到上桿質心的距離及萬象接頭到下桿質心的距離，其中 rid = 110(mm)，r iu = 45(mm)，li 為桿長，miu 、mid 為上桿及下桿的質量，miu = 0.838(kg ) ， mid = 0.985(kg ) ， Ri 、 Fi a 、 Fi n 分別為平板給的作用力。. 由前面的運動學分析，可得上下桿質心的加速度為[10] •. ••. aiu = (li − riu ) wi × ( wi × ni ) + (li − riu )α i × ni + 2wi × li ni + li ni. (2.27). aid = rid wi × ( wi × ni ) + rid α i × ni. (2.28). 此時取桿件的力平衡方程式可得[10] miu (li − riu )ni × G + mid rid ni × G + li ni × Fi n + M i. = ( I u + I d )α i − ( I u + I d ) wi × wi + miu (li − riu )ni × aiu + mid rid ni × aid. (2.29). 將(2.29)式整理過後可得 li ni × Fi n + mi ci = N i N i = −miu (li − riu )ni × G − mid rid ni × G + ( I u + I d )α i − ( I u + I d ) wi × wi + miu (li − riu )ni × aiu + mid rid ni × aid (2.30). 其中 I u 、 I d 為上下桿件的質量慣性矩、 M i 為接頭所提供的外力矩[10]，這邊需注意到，若當桿件的兩端都是使用球接頭的話，因為兩端的自由度相同，所以沒受到任何拘束，故 Mi = 0 。. 14.

(26) M i = mi ci. (2.31). ci = ui × vi. (2.32). mi = ( N i ini ) /(ci ini ). (2.33). 其中 ui 、 vi 代表萬象接頭插銷方向的向量。此時可得 Fi n = ( N i × ni − mi ci × ni ) / li. (2.34). 假設 Fi a 為軸向作用力， Fi a = fi a ni. (2.35). 由(2.35)式可得上平板的動態方程式為[10] 3. 3. i =1. i =1. ••. −∑ f i a ni − ∑ Fi n + m p G = m p x p 3. 3. 3. i =1. i =1. i =1. (2.36). −∑ f i a qi × ni − ∑ qi × Fi n − ∑ M i = I pα − I p w × w. (2.37). ••. 其中 x p 為形心 [ z, α , β ] 的加速度、 m p 為上平板的質量、 I p 為上平板質量慣性矩、 G = [0 0 −9800]T (mm / s 2 ) 為重力加速度。. 將式(2.36)、(2.37)寫成矩陣形式可得. ⎛ f1a ⎞ ⎜ a⎟ T ⎜ f2 ⎟ = J C ⎜ f 3a ⎟ ⎝ ⎠. (2.38). 其中[10] 3 •• ⎛ ⎞ n − − m G m x p p p ∑ Fi ⎜ ⎟ i =1 ⎟ C =⎜ 3 3 ⎜ ⎟ n ⎜ − I pα + I p w × w − ∑ qi × Fi − ∑ M i ⎟ i =1 i =1 ⎝ ⎠. (2.39). 接下來要求在軸空間(joint space)驅動上桿件的輸出力，可得. fi = miu aiu ini − f i a − miu G ini. (2.40). 將(2.40)式寫成矩陣的形式為 ⎛ f1 ⎞ ⎛ miu (a1u − G )in1 ⎞ ⎜ ⎟ ⎜ ⎟ F = ⎜ f 2 ⎟ = ⎜ miu (a2u − G )in2 ⎟ − J T C ⎜ f ⎟ ⎜ m ( a − G )i n ⎟ 3⎠ ⎝ 3 ⎠ ⎝ iu 3u. (2.41). (2.41)式為軸空間的輸出力，將其轉成工作空間(work space)的輸出力則為. 15.

(27) τ = ( f z τ α τ β ) = J −T F T. (2.42). 或寫成[10]. τ =J. −T. ⎛ miu (a1u − G )in1 ⎞ ⎜ ⎟ ⎜ miu (a2u − G )in2 ⎟ − C ⎜ m ( a − G )i n ⎟ 3⎠ ⎝ iu 3u. (2.43). 2.4.2 虛功法(Virtual Work Method). 此處，將利用虛功法[11]來推導此三軸並聯式機構，利用虛工原理來推導動態模型的優點在於：拘束力一開始可以被消去，不需要像 Newton-Euler 法一樣去計算在每個接頭上的作用力，所以可以提升計算的效率。首先，要先建立驅動桿與下平板的轉換矩陣，此目的在於可以簡化方程式計算的複雜度，減少程式中使用外積(cross product)的指令，如此會得到驅動桿的 Jacobian Matrix，定義為 Link Jacobian Matrices。其中，轉換矩陣的選擇為 w − v − w Euler Angles[11]，即一開始先對 Z 軸旋轉 φi 角度，可得 x ' y ' z ' 坐標系，然後再對 y ' 軸旋轉 θi 角度可得 xi yi zi 坐標系。. 圖 2.8 第 i 驅動桿旋轉座標系[11] 其轉換矩陣可寫成 ⎛ cos φi cos θi ⎜ Ri = ⎜ sin φi cos θi ⎜ − sin θ i ⎝. − sin φi cos φi 0. cos φi sin θ i ⎞ ⎟ sin φi sin θ i ⎟ cos θ i ⎟⎠. (2.44). 16.

(28) ⎛0⎞ ⎜ ⎟ 由驅動桿作標系來看 zi 軸方向的單位向量為 i ni = ⎜ 0 ⎟ ，且因為 ni = Ri i ni ，所以可得 ⎜1⎟ ⎝ ⎠ ⎛ cos φi sin θ i ⎞ ⎜ ⎟ ni = ⎜ sin φi sin θi ⎟ ⎜ cos θ ⎟ i ⎝ ⎠. (2.45). 且 cos θi = niz 、 cos φi =. n nix 、 sin θi = n ix2 + n iy2 、 sin φi = iy sin θi sin θi. 此時要將球接頭的速度轉成驅動桿座標系下的速度，由(2.8)式可得 •. i. •. Si = i R S i. (2.46). 其中 i R = RiT 將(2.6)式微分並乘上 i R ，整理後可得球接頭速度 •. i. •. Si = Li i ni + Li i wi × i ni. (2.47). 再將(2.47)式微分一次可得球接頭的加速度為 •• i. ••. •. Si = Li i ni + 2 Li i wi × i ni + Li iα i × i ni + Li i wi × ( i wi × i ni ). (2.48). 將(2.47)式對 i ni 兩邊取內積可得可得驅動桿沿軸向的速度為 •. •. L i = i Si , z. (2.49). 將(2.47)式對 i ni 兩邊取外積可得可得驅動桿角速度. ⎛ i • ⎞ ⎜ − Si , y ⎟ 1⎜ • ⎟ i wi = ⎜ i Si , x ⎟ li ⎜ ⎟ ⎜ 0 ⎟ ⎜ ⎟ ⎝ ⎠. (2.50). 同理，將(2.48)式兩邊分別對 i ni 取內積及外積可得軸向加速度 •. ••. •• i. Li = S i , z +. i. •. Si2, x + i Si2, y. (2.51). li. 及桿件角加速度為. 17.

(29) i i ⎛ i ii ⎞ i i ⎜ −li Si , y + 2 Si , z Si , y ⎟ ii i i ⎟ 1⎜ i α i = 2 ⎜ li i Si , x − 2 i Si , z i Si , x ⎟ li ⎜ ⎟ 0 ⎜ ⎟ ⎜ ⎟ ⎝ ⎠. (2.52). 注意到(2.50)與(2.25)式中的第三個分量皆等於零，代表桿件不會自旋，即不會繞本身的軸方向旋轉。此時可以求出上下桿件的質心速度與質心加速度為[11] • ⎛i • ⎞ ⎛ ⎞ i ⎜ Si , x ⎟ ⎜ (li − riu ) Si , x ⎟ • ⎟ r ⎜ • ⎟ 1⎜ i vid = id ⎜ i Si , y ⎟ 、 i viu = ⎜ (li − riu ) i Si , y ⎟ li ⎜ li ⎜ ⎟ ⎟ • ⎜ 0 ⎟ ⎜ l iS ⎟ i i,z ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ ⎝ ⎠. (2.53). i i ii i i ⎞ ⎛ ⎛ i ii ⎞ i i i i i l S − 2 S S l ( l r ) S 2 r S S − + ⎜ i i iu i , x i, z i,x ⎟ iu i, z i, x ⎟ ⎜ i i,x ⎜ ii i i ⎟ ii i i ⎟ ⎜ r 1 和 i aid = id2 ⎜ li i Si , y − 2 i Si , z i Si , y ⎟ 、 i aiu = 2 ⎜ li (li − riu ) i Si , y + 2riu i Si , z i Si , y ⎟ li ⎜ li ⎜ ⎟ ⎟ i i ii 2 2 i i i i ⎜ ⎟ ⎜ −( S + S ) ⎟ ⎜ ⎟ i ,x i ,y ⎜ li2 i Si , z + riu ( i S 2 + i S 2 ) ⎟ ⎝ ⎠ i ,x i ,y ⎝ ⎠. (2.54). 到目前為止，已經將桿件在 Link Space 中的運動學分析完畢，接下來要找分析上平板與桿件的運動學關係，其之間的關係以 Platform Jacobian 矩陣與 Link Jacobia 矩陣為轉換因子，以下為推導。 2.4.3 Platform Jacobian 矩陣與 Link Jacobia 矩陣[11]. 由(2.18)式的 Jacobian 矩陣乘上 i R 可得驅動桿座標系下的 Jacobian 矩陣為 i. J i−1 = i RJ −1. (2.55). ⎛ i Ji,x ⎞ ⎜ ⎟ 且 i J i = i RJ i = ⎜ i J i , y ⎟ ⎜ i Ji,z ⎟ ⎝ ⎠ i. (2.56). J i 即為在驅動桿座標系下的 Jacobian Matrix。. 由(2.18)、(2.49)與(2.55)式可得. 18.

(30) ⎛•⎞ ⎜z⎟ • ⎜•⎟ Li = i J i , z ⎜ α ⎟ ⎜•⎟ ⎜β ⎟ ⎝ ⎠. (2.57). 將(2.57)式整理成矩陣形式可得 ⎛•⎞ ⎜z⎟ • ⎜•⎟ L = J p ⎜α ⎟ ⎜•⎟ ⎜β ⎟ ⎝ ⎠. (2.58). ⎛ 1J1, z ⎞ ⎜ ⎟ 其中 J p = ⎜ 2 J 2, z ⎟ ⎜ 3 J 3, z ⎟ ⎝ ⎠. (2.59). 上式的 J p 即稱為 Platform Jacobian，由(2.18)式可知 Platform Jacobian 即等於 J −1 。同樣的，重新整理(2.50)、(2.53)式可得 ⎛ (li − riu ) i J i , x ⎞ ⎛ − i Ji, y ⎞ ⎛ i J i,x ⎞ r ⎜ ⎟ ⎟ ⎟ 1⎜ 1⎜ i wi = ⎜ i J i , x ⎟ x p 、 i vid = id ⎜ i J i , y ⎟ x p 、 i viu = ⎜ (li − riu ) i J i , y ⎟ x p li ⎜ li ⎜ li ⎜ i ⎟ ⎟ ⎟ ⎝ li J i , y ⎠ ⎝ 01×3 ⎠ ⎝ 01×3 ⎠. (2.60). ⎛z⎞ ⎜ ⎟ 其中 x p = ⎜ α ⎟ ⎜β ⎟ ⎝ ⎠. 合併(2.60)式可得 •. i. •. •. •. xid = i J id x p 、 i xiu = i J iu x p. (2.61). 其中 i J id 、 i J iu 即為 Link Jacobian 矩陣 ⎛ li i J i , z ⎞ ⎛ 01×3 ⎞ ⎟ 1⎜ 1⎜ ⎟ i J id = ⎜ − i J i , y ⎟ 、 i J iu = ⎜ − i J i , y ⎟ li ⎜ i li ⎜ i ⎟ ⎟ ⎝ Ji,x ⎠ ⎝ Ji,x ⎠. (2.62). 推導完所有的運動學之後，接下來要建立動態方程式，最後再帶入虛工方程式求輸出力，首先，上平板的動態方程式為. 19.

(31) •• ⎛∧⎞ ⎛ ⎞ f⎟ f Mg M x + − ⎜ p ⎟ F = ∧ = ⎜ ext ⎜⎜ ⎟⎟ ⎜ n − Iα − w × Iw ⎟ ⎠ ⎝ n ⎠ ⎝ ext. (2.63). 其中 f ext 、 next 為作用在質心的外力與力矩。同樣的，作用在驅動桿上下質心位置的力與力矩可表示成 i. i. ⎛ i f ⎞ ⎛ mid i Rg − mid i aid ⎞ Fid = ⎜ i id ⎟ = ⎜ ⎟ i i i ⎝ nid ⎠ ⎝ − I d α i − wi × ( I d wi ) ⎠. (2.64). ⎛ i f ⎞ ⎛ miu i Rg − miu i aiu ⎞ Fiu = ⎜ i iu ⎟ = ⎜ ⎟ i i i ⎝ niu ⎠ ⎝ − I u α i − wi × ( I u wi ) ⎠. (2.65). 根據虛功原理(作用在虛位移期間所作之功，若系統達平衡，則力乘上虛位移等於零)，虛工方程式可寫成[11] 3. δ LTτ + δ xTp F + ∑ (δ i xidT i Fid + δ i xiuT i Fiu ) = 0. (2.66). i =1. 其中 τ 為驅動桿的輸出力，且由前面推導可知. δ L = J pδ x p 、 δ i xid = i J id δ x p 、 δ i xiu = i J iuδ x p. (2.67). 將(2.67)式帶入(2.66)式整理過後可得 3. δ xTp ( J Tpτ + F + ∑ i J idT i Fid + i J iuT i Fiu ) = 0. (2.68). i =1. 因為虛位移 xTp 不等於零，所以上式可改寫成 3. J Tpτ + F + ∑ i J idT i Fid + i J iuT i Fiu = 0. (2.69). i =1. 在這邊需注意到， F 是以固定座標系來表示，但是 i Fid 、 i Fiu 是以軸空間座標系來表示，所以需將 i Fid 、 i Fiu 分別乘上 i J idT 、 i J iuT 來轉換成卡式座標系。接著將(2.62)帶入(2.69)式整理可得 J Tp (τ + Fz ) + F + J xT Fx + J Ty Fy = 0. (2.70). 其中. ⎛ 1J1, x ⎞ ⎛ 1J1, y ⎞ ⎛ f1u , z ⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ Fz = ⎜ f 2u , z ⎟ ； J x = ⎜ 2 J 2, x ⎟ ； J y = ⎜ 2 J 2, y ⎟ ⎜f ⎟ ⎜ 3 J 3, x ⎟ ⎜ 3 J 3, y ⎟ ⎝ 3u , z ⎠ ⎝ ⎠ ⎝ ⎠. (2.71). 20.

(32) ⎛ (r1d f1d , x + n1d , y + (l1 − r1u ) f1u , x + n1u , y ) / l1 ⎞ ⎜ ⎟ Fx = ⎜ (r2 d f 2 d , x + n2 d , y + (l2 − r2u ) f 2u , x + n2u , y ) / l2 ⎟ ⎜ (r3d f3d , x + n3d , y + (l3 − r3u ) f 3u , x + n3u , y ) / l3 ⎟ ⎝ ⎠. (2.72). ⎛ (r1d f1d , y − n1d , x + (l1 − r1u ) f1u , y − n1u , x ) / l1 ⎞ ⎜ ⎟ Fy = ⎜ (r2 d f 2 d , y − n2 d , x + (l2 − r2u ) f 2u , y − n2u , x ) / l2 ⎟ ⎜ (r3d f 2 d , y − n3d , x + (l3 − r3u ) f 3u , y − n3u , x ) / l3 ⎟ ⎝ ⎠. (2.73). 重新整理(2.69)式可得輸出力. τ = − Fz − J p−T ( F + J xT Fx + J Ty Fy ). (2.74). 以上是利用牛頓法與虛功法來建立此平行機構的數學模型，此模型已詳細的推導出來，最後將提出牛頓法與虛功法的差異性，和利用牛頓法與虛功法在使用電腦模擬的執行效率上做個比較，以作為一個總結。由前面的推導過程可以很明顯的看出使用牛頓法時，接頭的反作用力必須能夠計算出來，即必須考慮接頭的形式為何，然後去算 M i ，再來是分析作用在桿件與上平板的外力與慣性力，然後再帶入牛頓第二運動定律，此時可得到所有的外力，最後再取出上桿件做分析，可得到驅動上桿件的輸出力；而虛功法的特點在於，在驅動桿與下平板桿建立轉換矩陣，然後會得到每根桿件與下平板的轉換矩陣，即可建立一 Link Jacobian matrix，亦即桿件上所有的物理量皆以桿件座標系來表示，除此之外，虛功法優點在於不用去算拘束力，最後再將所有物理量帶入虛功方程式，求桿件的輸出力。. 以下為利用牛頓法與虛功法所做的電腦模擬，及運算時間比較。其模擬的內容為给定一個空間軌跡，然後算出其完成此空間軌跡三根桿件所需要的輸出力。此平行式機構的所有參數規格整理如下表，其中質量的數據為以電子磅秤所量得，慣性矩為由幾何形狀及尺寸所求得：表 2.1 平行式機構規格表上平板. 上桿件. 下桿件. 質量(Kg). 1.44. 0.838. 0.985. 質量慣性矩. 0.02086 ×106. 0.0000943 ×106. 0.0001108 ×106. (kg- mm2 ). 21.

(33) 重力加速度 g=9800 mm / sec 2. 萬向接頭到下桿質心位. 球接頭到上桿質心位置. 置的距離 rid=110 mm. 的距離 riu=45 mm. 2.5 模擬與討論以下將利用牛頓法(式 2.27~2.43)與虛功法(式 2.44~2.74)做系統模擬，計算桿件的輸出力，並比較此兩種方法的效率，利用此兩種方法所計算出來的軸輸出力是相同的。此處模擬四組空間中軌跡，然後求每根稈件所需的輸出力，其軌跡規劃如下，其中 p 、 v 、 a 分別代表上平板形心的位置、速度與加速度，桿長部分為由逆向運動學(式 2.6)所求得：軌. 跡. 一. ：. ⎛ 250 + t 2 ⎞ ⎛ 2×t ⎞ ⎛ 2⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ p = ⎜ 0 ⎟ 、 v = ⎜ 0 ⎟ 、 a = ⎜ 0 ⎟ ，時間為 0~7 秒 ⎜ 0 ⎟ ⎜0⎟ ⎜ 0 ⎟ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠. 圖 2.9 軌跡一的工作空間圖. 22.

(34) 圖 2.10 軌跡一的桿長變化圖. 軌. 跡. 二. 圖 2.11 軌跡一的桿件輸出力圖. ：. 270 0 ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ p = ⎜ 0.2 × cos(2 / 7 × pi × t ) ⎟ 、 v = ⎜ −0.4 / 7 × pi × sin(2 / 7 × pi × t ) ⎟ 、 ⎜ 0.2 × sin(2 / 7 × pi × t ) ⎟ ⎜ −0.4 / 7 × pi × cos(2 / 7 × pi × t ) ⎟ ⎝ ⎠ ⎝ ⎠ 0 ⎛ ⎞ ⎜ ⎟ a = ⎜ −0.8 / 49 × pi × cos(2 / 7 × pi × t ) ⎟ ，時間為 0~7 秒 ⎜ −0.8 / 49 × pi × sin(2 / 7 × pi × t ) ⎟ ⎝ ⎠. 圖 2.12 軌跡二的工作空間圖. 23.

(35) 圖 2.13 軌跡二的桿長變化圖. 軌. 跡. 三. 圖 2.14 軌跡二的桿件輸出力圖. ：. 260 + 5 × t 5 ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ p = ⎜ 0.2 × cos(2 / 7 × pi × t ) ⎟ 、 v = ⎜ −0.4 / 7 × pi × sin(2 / 7 × pi × t ) ⎟ 、 ⎜ 0.2 × sin(2 / 7 × pi × t ) ⎟ ⎜ −0.4 / 7 × pi × cos(2 / 7 × pi × t ) ⎟ ⎝ ⎠ ⎝ ⎠ 0 ⎛ ⎞ ⎜ ⎟ a = ⎜ −0.8 / 49 × pi × cos(2 / 7 × pi × t ) ⎟ ，時間為 0~7 秒 ⎜ −0.8 / 49 × pi × sin(2 / 7 × pi × t ) ⎟ ⎝ ⎠. 圖 2.15 軌跡三的工作空間圖. 24.

(36) 圖 2.16 軌跡三的桿長變化圖. 軌. 跡. 四. 圖 2.17 軌跡三的桿件輸出力圖. ：. 260+3 × t 3 ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ p = ⎜ pi/180 × t × cos(t ) ⎟ 、 v = ⎜ 1/180 × pi × cos(t )-1/180 × pi × t × sin(t ) ⎟ 、 ⎜ pi/180 × t × sin(t ) ⎟ ⎜1/180 × pi × sin(t )+1/180 × pi × t × cos(t ) ⎟ ⎝ ⎠ ⎝ ⎠ 0 ⎛ ⎞ ⎜ ⎟ a = ⎜ -1/90 × pi × sin(t )-1/180 × pi × t × cos(t ) ⎟ ，時間為 0~7 秒 ⎜ 1/90 × pi × cos(t )-1/180 × pi × t × sin(t ) ⎟ ⎝ ⎠. 圖 2.18 軌跡四的工作空間圖. 25.

(37) 圖 2.19 軌跡四的桿長變化圖. 圖 2.20 軌跡四的桿件輸出力圖. 以上四個軌跡為此機構的數學動態模型模擬，接下來比較牛頓法與虛功法在電腦模擬上各自所需的時間，此指令是由 matlab 軟體所提供的 tic、toc 兩個指令，其計算時間的條件為：由電腦開始讀進 p 、 v 、 a 這三個參數，然後至完成一個軌跡，並算出各桿件所須輸出的力為止，設定此為一個週期，下表為軌跡一到軌跡四分別使用牛頓法與虛功法所需花的時間表。. 表 2.2 牛頓法與虛功法執行效率比較表（時間單位：秒）牛頓法虛功法. 牛頓法虛功法. 軌跡一 0.9690. 0.5620. 軌跡三 1.3600. 0.7500. 軌跡二 1.1250. 0.5940. 軌跡四 1.0160. 0.5620. 由表中可以很明顯的看出虛功法所需的時間幾乎都是牛頓法的一半，這是因為使用牛頓法時，機台每個結構的力平衡方程式都要被計算一次，因此影響數值計算的效率，若是硬體速度不夠快，可能會導致無法達到及時控制的要求；而虛功法因為拘束力一開始就被消去，因此相關計算的方程式較少，所以在計算效率上會比牛頓法快，但是，上面兩種方法對於數值計算上面還是有一定的複雜度，相對於串連式機構來講，明顯的複雜許多，而對於傳統工業電腦可能無法滿足即時控制，所以如何適度的簡化方程式，以提升計算的效率，這也是另一研究議題，而此議題在本研究中不多做討論。接下來，將對四種軌跡的模擬做個總結，由圖 2.11 可以看出，當讓平板沿著 z 軸上升，且具有一保持常數的加速度 az = 2mm / s 2 ，其他參數保持常數，可以很明顯的看出. 26.

(38) 三根桿件的輸出力是相等的，而且因為當平板往上升時，桿件的向量會越來越接近垂直，所以桿件輸出力在水平方向的分量便會減小，故由圖可看出桿件的輸出力會越來越小，而由其他圖亦可很明顯的看出當桿件伸長時，其輸出力會越來越小，反之亦然，其道理都是一樣的，由以上的結果，可以證明建立的模型符合物理現象。以上已將此平行式機構的數學模型完整的推導出來，接下來的討論將對此模型做控制與分析。. 2.6 工作空間(Work Space)分析此平行式機構具有三個自由度，在此節，將分析此機構的工作空間，一般而言，適用於工具機加工環境的工作空間定義為全部指向工作空間(Total Orientation Workspace, TOW)，意即在此空間中可動板上的參考點能夠讓 end-effector 於此區間內任意運動。故. 此機構的 TOW 定義為[25]：. {. }. ΩT = p | 246 < z < 310(mm) ∈ R 3 , −0.38 < α < 0.38(rad ), −0.38 < β < 0.38(rad ). 以下將依據此一 TOW 定義並配合逆向運動學來求此機構的工作空間，其利用的方法為採取疊加法並將其代入(2.6)式，逐一計算出相應之桿長，以便檢驗桿長是否符合其極限，從而標定工作空間之邊界，簡單的說，即是先由幾何及接頭的限制，求出 alpha、beta 的最大最小值，然後再逐一的去找 z 軸的最大最小值，以下為求工作空間的流程圖與工作空間圖。. 27.

(39) 圖 2.21 工作空間流程圖. 28.

(40) (一). (二). (三). (四). 圖 2.22 工作空間圖圖 2.22 中，圖(一)表示的是 z 軸的最大值、圖(二)表示的是當時的 z 軸最小值；而圖(三)、圖(四)分別是相對應的 alpha 、 beta 值。. 2.7 控制準備 2.7.1 交叉偶合控制. 多軸進給系統的動態誤差包含：(1)追蹤誤差(position tracking error vector)、(2)輪廓誤差(contour error vector)。在軌跡追蹤上，傳統目標是追蹤誤差，其定義為真實的刀尖與理想位置的距離，但在多軸系統的控制下，因為軸與軸的特性不同，常會造成互相偶合的效應，所以若只有單純的做追蹤誤差的補償往往是無法有效的達到控制的精度，所以為了消除偶合現象，需再對輪廓誤差作補償，其定義為真實的位置與理想軌跡的最短距離，以下為交叉偶合的控制概念。考慮 XY 平面之直線軌跡追蹤， pa 為刀具真實位置、 pd 為刀具理想位置，而追蹤誤 29.

(41) 差 E = Pd − Pa 、輪廓誤差 ε = x p sin φ − y p cos φ ，其中 Pa = ( x p , y p ) ，則輪廓誤差在 XY 平面上的分量為 ε x = −ε sin φ 、 ε y = ε cos φ ，控制架構如圖所示，由圖中可以很明顯的看出偶合的現象(即輪廓誤差)為由工作空間自由度互相影響所造成，若應用於並聯式機構須再做 Forward kinematic 與 Inverse kinematic 互相的轉換以做補償。. 圖 2.23 輪廓誤差與預補償幾何關係圖[6]. 圖 2.24 交叉偶合控制示意圖[6] 2.7.2 交叉偶合預補償控制. 預補償控制為交叉偶合控制的修正型，其概念為利用空間中幾何關係找出輪廓誤差與速度的關係式，藉此求出需補償的速度，修正出預補償的路徑，進而有效的消除輪廓誤 30.

(42) 差，其控制示意圖如下. 圖 2.25 交叉偶合預補償控制示意圖[6] 在預補償的演算架構中定義一個預補償的速度向量為 Vk = K v ε ，其中 K v 為預補償增益。因此修正的速度包含進給速度與預補償的修改速度，並且表示如下： V = Vt + Vk. (2.75). 其中 Vt = Vb t 且 t 為切線速度向量。因此修正的速度在 X 與 Y 的分量表示如下： V x = Vb cos φ − K v ε sin φ 、 V y = Vb sin φ + K v ε cos φ. (2.76). 藉由將修正的速度對時間積分，可以得到新的參考位置，並且以離散的模式表示如下： x f (n) = x f (n − 1) + TV x 、 y f (n) = y f (n − 1) + TV y. ( 2.77). T 表示 sampling time interval，由下列的關係式可以得到進給位置誤差： e x (n) = e x (n − 1) + [ x f (n) − x p (n)] 、 e y (n) = e y (n − 1) + [ y f (n) − y p (n)]. (2.78). 綜合上述，可得到控制訊號如下： U x (n + 1) = K ex e x (n) + K εx ε x (n) 、 U y (n + 1) = K ey e y (n) + K εy ε y (n). (2.79). 目前為止，已經說明交叉偶合及交叉偶合預補償，接下來將替本文之 3RPS 並聯式工具機建造交叉偶合預補償系統，其內容在第三章會有完整的說明。. 31.