3.5 交叉偶合與預補償應用於力量控制
3.5.1 驅動軸(actuator)機械模型
由模擬可以看出在尖角處時，雖然 CCS 與 CCPM 在轉角處輪廓誤差都有放大的反應，
但由模擬的結果可以看出 CCPM 對於輪廓誤差的改善還是比 CCS 好，且輪廓誤差能夠 快速的修正回來，由 IAE 值更可看出輪廓誤差降低約一半左右。
3.5 交叉偶合與預補償應用於力量控制
3.5.1 驅動軸(actuator)機械模型
在進行力量控制前，須先建立驅動器(actuator)的模型，由於力量控制是以電壓為輸入 訊號，然後送出所需要的力矩，所以首先步驟即為建立馬達的模型，而機台的運動方式 為由馬達的旋轉帶動皮帶，然後再經由皮帶帶動滾珠螺桿使得桿件做直線運動，進而使 上平板形心點跑出所需的軌跡，而以下為驅動軸的模型建立，其內容包括馬達模型與滾 珠螺桿模型，圖 3.19 為馬達與驅動桿的簡易示意圖。
圖 3.19 馬達與滾珠螺桿示意圖
式(3.33)即為一般馬達的近似模型，以上為馬達模型的動態，接下來將包含上桿件的動
( ) Kt a( ) d ( )t
( , )
圖 3.23 三軸平台交叉偶合控制結合力量控制
3.5.3 交叉偶合預補償控制結合力量控制
圖 3.24 三軸平台交叉偶合預補償控制結合力量控制
3.6 模擬與討論
力量控制與位置控制的整體架構有很大的不同，且力量控制考慮了整體系統的動態，
它能處理有關力在系統中的反應。力量控制是以力當成輸入訊號去控制整個機構，在模 擬部份，亦是利用第二章軌跡二及軌跡四兩組軌跡來做模擬，控制架構圖(3.23)以力量 控制為基礎除了補償空間中的追蹤誤差，並考慮在工作空間的輪廓誤差以做輪廓誤差的 補償，而圖(3.24)則是結合了預補償方法補償ㄧ速度項來消除輪廓誤差，在架構中所需 注意到的一點即為工作空間與軸空間互相的轉換。
( 1 ) 、 軌 跡 二 （ 第 二 章 ） 力 量 控 制 模 擬
表 3.7 軌跡二交叉偶合結合力量控制之控制器參數表
PD for Force Control PI for Contour Error PI for Precompensation
P gain D gain P gain I gain P gain I gain
CCS 180 80 60 40 0 0
CCPM 180 80 60 40 35 20
Structure 3：交叉偶合控制結合力量控制(圖 3.23)
圖 3.25 軌跡二工作空間進給誤差與輪廓誤差示意圖(FC+CCS) Structure 4：交叉偶合預補償控制結合力量控制(控制架構圖 3.24)
圖 3.26 軌跡二工作空間進給誤差與輪廓誤差示意圖(FC+CCPM) 表 3.8 軌跡二 FC+CCS 與 FC+CCPM 之 IAE 比較表
工作空間位置誤差 工作空間輪廓誤差
IAE 值
z α、β z α、β
CCS 0.5317 0.0865 0.0102 0.0152
CCPM 0.5137 0.0934 0.0184 0.0054
( 2 ) 、 軌 跡 四 （ 第 二 章 ） 力 量 控 制 模 擬
表 3.9 軌跡四交叉偶合結合力量控制之控制器參數表
PD for Force Control PI for Contour Error PI for Precompensation
P gain D gain P gain I gain P gain I gain
CCS 260 90 40 30 0 0
CCPM 260 90 40 30 4.2 0.08
Structure 3：交叉偶合控制結合力量控制(圖 3.23)
圖 3.27 軌跡四工作空間進給誤差與輪廓誤差示意圖(FC+CCS) Structure 4：交叉偶合預補償控制結合力量控制(圖 3.24)
圖 3.28 軌跡四工作空間進給誤差與輪廓誤差示意圖(FC+CCPM)
表 3.10 軌跡四 FC+CCS 與 FC+CCPM IAE 比較表
工作空間位置誤差 工作空間輪廓誤差
IAE 值
z α、β z α、β
CCS 0.4843 0.0217 6.6913e-004 0.0024 CCPM 0.4825 0.0277 6.6263e-004 0.0014
在力量控制的模擬中，分別由表 3.8 及表 3.10 明顯的看到加入了預補償方法可以明顯
F 為機器手端(end-effector)與環境接觸的接觸力
結合式(3.33~3.36)可得[24]
圖 3.31 系統與外界環境接觸力方塊圖 將ep = − 、rd r ef =fd −f 代入式(3.55)，可得
− = (( − )− )
d f f d p o
f e K r e r ( 3 . 5 6 ) 將(3.56)重新整理後，可得
( ) ( ) ( )
f d f d o f p d f d o f d
e = f −K r −r +K e = f −K r −r +K r −r ( 3 . 5 7 ) 將式(3.57)代入式(3.54)整理可得
0 0
( ) ( ) ( )
t t
d v p w f i f d d v d p d w d f o i d f o
M r k rii+ i+ k +k K r+k K
∫
rdτ =M rii+k ri +k r +k f +k r +k∫
f +k r dτ(3.58) 式(3.58)即為混合位置/力量控制法則，由式(3.58)可知當輸入期望位置、力量及手端與環 境接觸的靜止點(未變形點)，即可求出經過補償修正過後的實際位置值，在由式(3.55)求 出與環境的接觸力，然後作回授控制。
3.7.2 交叉偶合結合混合位置/力量控制
圖 3.32 交叉偶合結合混合位置/力量控制
第 四 章 實 驗 與 討 論
4.1 實驗機台與校正
首先，3RPS 機構如圖 4.1 所示，PCI-8134 運動控制卡加裝在電腦 PCI 插槽內，其運 動方式為利用電腦下命令，經由轉接板將訊號送至 Driver，然後使馬達旋轉，再經由導 螺桿使桿件做直線運動，藉由裝在桿件上的光學尺量測現在桿件的實際位置，在回授給 電腦做處理，其中光學尺為 RENISHAW 公司所出的線性光學尺，型號為 RGH41X，解 析度達1 mμ ，滾珠螺桿其精度為 C5 等級，行程為5cm，導程誤差為18 mμ ，而控制器設 計方面則由電腦利用程式去撰寫所完成。
圖 4.1 3RPS 實際機台示意圖
PCI-8134 控制卡能做位置控制與速度控制，卡的輸出訊號為 pulse train，最大的 pulse 輸出頻率為 2.4Mpps(pulse/per sec)。整個機台的運作為利用馬達的旋轉來帶動導螺桿，
使三根桿件來做直線的運動，即將旋轉運動轉成直線運動，進而帶動上平板，使平板作 出所需的軌跡，此處，控制訊號與桿件直線運動的關係為：1pulse=10−6m，代表當 command 下 1pulse 的命令時，桿件伸長或縮短10−6m，所以要使桿件伸長 1mm 需下 1000pulse 的 command。由於機構為平行式機構，所以進給速率(pulse/s)的控制是以上平 板的形心點為基準(即形心點的速度)，然後再藉由 Inverse-kinematics(式 2.6~2.18 )求出所 需的桿長進給速率。
(第二章)算出所需的桿長，以桿長為訊號換算成所需的 pulse 命令送給馬達，然後由三根 軸同時做所需的伸長量，再藉由光學尺回授目前實際的位置，實驗一開始，先對控制系 統的工作性能做分析，將以步階函數為測試訊號分析三根桿件的工作性能，下圖為三根 桿件的開回路步階響應圖，圖中橫軸為時間，縱軸為桿件位置。
圖 4.2 桿件一步階響應圖
圖 4.3 桿件二步階響應圖
圖 4.4 桿件三步階響應圖 表 4.1 桿件步階響應性能表
桿件一 桿件二 桿件三
最大超越量 % 0.0035 0.0286 0.0028
延遲時間(s) 0.1 0.08 0.25
上升時間(s) 0.2 0.16 0.5
安定時間(s) 0.1 5.5 4.2
圖 4.2~4.4 分別為三根桿件的步階響應圖，由圖中很明顯的看出伺服馬達的反應非常 快，而上表為響應的性能表，由圖中可看出桿件二震盪較大且安定時間最長，這與桿件 硬體設計有關，在控制方面，若能有效的提升 0~1 秒之間的輸出響應性能，必能大大的 提升軌跡追蹤能力。
4.2 開迴路(open loop)系統實驗
以下為利用第二章所模擬出來的軌跡二，以及直線運動軌跡來做實驗，實驗過程中，
參數 p 、v、a分別代表上平板形心的位置、速度與加速度，對於給的位置控制訊號必 須經由 Inverse-Kinematic(第二章)來求三根桿件須給多少訊號(即 pulse 訊號)，而對於加 工的進給速率方面，著重於上平板的形心速率，而並不是考慮三根桿件的速率，所以實 驗過程亦是給定形心的加工速率然後再經 Inverse-Kinematic(第二章)來算出三根桿件所 需的輸出速率(即 pulse/sec)，如此即可完成所需的軌跡規劃，圖 4.5 為由第二章軌跡二 所規劃的形心速度所求出所需三根桿件的進給速率。
圖 4.5 桿件進給速率圖
下圖為由實驗所得的三根桿件位置圖，藍色為所需的理想位置，而紅色代表實際的位 置值。
圖 4.6 開迴路系統實驗桿件理想與實驗位置比較圖
圖 4.7 開迴路系統實驗桿件誤差圖
圖 4.9 工作空間進給速率v=
(
0 0.54 0.54)
T之桿件進給速率圖圖 4.10 工作空間進給速率v=
(
0 0.54 0.54)
T之桿件理想與實驗位置比較圖圖 4.11 工作空間進給速率v=
(
0 0.54 0.54)
T之桿件誤差圖圖 4.12 進給速率v=
(
0 0.54 0.54)
T之工作空間理想與實驗位置比較圖 表 4.2 工作空間進給速度v=(
0 0.54 0.54)
T實驗桿件 IAE 比較表IAE 值 桿件一 桿件二 桿件三
第二章 軌跡二 1.1717 1.1901 1.1733
工作空間進給速率
(
0 0.54 0.54)
Tv=
2.6845 2.7548 2.6904
表 4.3 工作空間進給速度v=
(
0 0.54 0.54)
T實驗工作空間 IAE 比較圖 4.14 開回路系統直線運動軌跡實驗工作空間誤差圖
圖 4.15 開回路系統直線運動軌跡實驗各桿件誤差圖
以上實驗為開回路系統控制，接下來為加入 PID 控制閉迴路實驗，而其實驗目的為降低 桿件的誤差，以及工作空間的誤差。
4.3 未偶合(uncouple system)系統實驗過程
圖 4.16 為未偶合系統控制架構圖，在連續類比領域，其 PID 控制的輸出為
p i d
u k e k edt k de
= +
∫
+ dt (4.1) 其中 e 為誤差量，k 、p k 、i k 為增益值，而在實際實驗上，電腦為一數位訊號，所以必d1 ( 1) d ( 2 1 2) PID for Position Error
P gain I gain D gain
PID Control For link error 250 370 45
表 4.5 直線運動軌跡未偶合控制架構實驗 PID 參數表 PID for Position Error
P gain I gain D gain
圖 4.17 PID 控制理想與實驗比較圖(第二章軌跡二)
圖 4.18 PID 控制桿件誤差圖(第二章軌跡二)
圖 4.19 PID 控制工作空間誤差圖(直線運動軌跡)
圖 4.20 PID 控制桿件誤差圖(直線運動軌跡) 表 4.6 未偶合 PID 控制_軌跡二實驗 IAE 值
IAE 值 桿件一 桿件二 桿件三
開迴路實驗 1.1717 1.1901 1.1733
PID 未偶合控制實驗 0.6311 0.686 0.6704
表 4.7 未偶合 PID 控制_直線運動軌跡實驗 IAE 值
Step2：將所規劃的工作空間軌跡經由 Inverse kinematic 轉換，算出所需的桿長長度，再 經由類比轉成數位訊號(pulse)，將訊號傳送給驅動器使桿件做伸長縮短動作
經過負回授後再由 Inverse kinematic 轉換可得桿件的誤差量，然後經由控制器(PID controller)算出下一點的控制訊號，再將訊號送給驅動器
Step6：在完成步驟 2~4 的同時，可利用第三章公式(3.6)、(3.12)、(3.13)求得目前的輪廓 誤差量，接下來步驟 7 為輪廓誤差的補償
Step7：對於輪廓誤差的補償為利用向量合成的觀念，由式 (3.14)，此時須再由 Inverse kinematic 轉換算出相對應桿件輪廓誤差的桿長，再由負回授可得桿件的誤差，將此誤差 量經由控制器(PID controller)算出下一點的控制訊號，將其與 Step5 訊號相加當成下一點 的控制訊號做輪廓誤差的補償
Step8：重複步驟 2~7 可完成交叉偶合控制系統實驗
Step9：對於預補償部分，由步驟 1 可求得工作空間的進給速度(或亦可自行規劃出上平 板的進給速度，注意此處的進給速度是給定上平板的速度，並不是給定桿件的進給速 度)，由步驟 6 中所求得的輪廓誤差可求出所需補償的速度量，由式(3.17~3.21)，然後再 經由 Inverse kinematic 轉換算出所需補償的桿件速度量
Step10：將桿件速度積分可得所需的桿件位置，如此可得桿件的進給誤差，由式
(3.22~3.24)，再由 step5 算出控制訊號
Step10：重複步驟 1~10 可完成交叉偶合預補償控制系統實驗
上述 10 個步驟為完整的控制實驗過程，在實驗程式的撰寫方面，是以 Borland C++ 為 開發工具，作為與機台溝通的橋樑，以下為實驗所建立的人機介面圖：
圖 4.21 Borland C++實驗人機介面圖
以下所做的實驗主要有第二章軌跡二、第二章軌跡四、具尖角軌跡、漩渦狀軌跡四組，
以用來比較模擬與實驗上的差別，並可驗證出模擬的正確性，在軌跡執行時的起始點為
由第二章所規劃的軌跡當t=0時為初始運動點，而實驗過程中控制器所用的參數値是用
由第二章所規劃的軌跡當t=0時為初始運動點，而實驗過程中控制器所用的參數値是用