泛用型嵌入式運動控制系統設計(II)

(1)

中文摘要

為滿足日趨複雜且多樣的運動控制工程應用，嵌入式控制系統便因應而生。嵌入式系統為硬體及軟體的綜合體，包括核心硬體電路、作業系統以及應用程式的建立。

本計劃已完成一個具有運動控制曲線產生電路及閉回路控制回路的控制晶片，並成功的應用所設計開發之泛用型嵌入式運動控制系統於直線倒單擺控制系統。其實現方法為設計Nios嵌入式系統以及FPGA PID與模糊控制器實現斜面直線倒單擺之甩上及平衡控制；其控制目標為先將倒單擺由初始下垂位置逐漸甩上至垂直位置附近，然後隨即控制倒單擺於滑台某特定點處維持平衡。

關鍵字：嵌入式系統、FPGA、運動控制 IC、PID/Fuzzy 控制器、倒單擺控制

Abstract

As motion control systems have got more complex and variety, the embedded motion control systems have become a possible solution. An embedded system is a combination of hardware and software, including circuit design, an real-time operation system, and application programs. We have designed a FPGA based motion control IC with s-curve trajectory generation and servo control functions. For illustration of embedded motion control applications, we have designed a Nios embedded motion control system and FPGA PID/Fuzzy controllers to swing up a sloped linear inverted pendulum from the stationary hanging state to the upright position and followed by balancing it about the vertical position and positioning the cart to the desired position.

Keywords: Embedded System, FPGA, Motion Control IC, PID/Fuzzy Control, Inverted Pendulum Control

(2)

目錄

第一章緒論 2

第二章運動軌跡設計 6

第三章運動軌跡數位電路設計 23

第四章 PID 控制器之數位電路設計 35

第五章運動控制介面電路設計 43

第六章運動控制晶片實現與實驗結果 46 第七章斜面直線倒單擺之甩上及平衡控制 64

第八章結論與展望 78

參考文獻 79

(3)

第一章緒論 1.1 研究動機及目的

近年來，自動化機械工業快速發展，多軸平台的應用日益增廣，其工作的複雜性及其精度的要求亦大為提昇。多軸平台的應用包括數值控制工具機、產業機器人、移動機械平台及泛用型機器人等[1]。多軸平台中的多軸同步控制、軌跡控制及智慧型控制，使得控制系統的控制器及軟體的設計逐漸變得龐大且複雜，所以多軸運動控制系統的成本和設計難度已面臨困難。

多軸平台在運動軌跡控制中，著重於位置控制的精確性，因為各軸的速度響應均有其實際上的限制，急劇的速度變化使機台產生震動，造成精確性下降和機件的損壞，所以平順的速度控制是非常重要的。在多軸平台中，運動控制器將位置控制命令轉為馬達的運轉速度控制命令，為減少急遽的速度變化，馬達的轉動速度曲線必須為多次式的速度曲線，其中以S-Curve 速度曲線為使用於馬達速度規劃最普遍的設計。

運動控制器需利用回授控制，使馬達能達到精準的位置控制，馬達的位置回授值，經由命令解碼器後輸入運動控制器，產生馬達的閉迴路回授命令，常見的閉迴路控制方式為PID 控制。

由於多次方的速度曲線演算法或閉迴路控制皆需繁複的運算，須配合高工作頻率的處理器，故實現運動控制器的方式通常為數位訊號處理器(DSP)或運動控制卡，如四軸控制卡，但其成本高且設計複雜使得此方式逐漸不為工業界接受，亦有可產生S 型速度曲線及 PID 控制的運動控制晶片，如 Nippon 公司的 PCL6045 或 National 的 LM628，此方式優點為成本低且設計簡單，但市場上的此類晶片有命令輸出誤差及命令條件限制等缺點，亦不易作精準的控制[2~5]。故本文提出對應多次方速度曲線輸出脈衝的方法，其不但有較低複雜度的演算，且脈衝無輸出誤差，並能以硬體或積體電路來實現。

目前一般常見之馬達控制模式有三種，如圖1.1 所示。第一種方式為位置控制器直接送出脈衝命令去驅動步進馬達；第二種方式為位置控制器送出脈波格式的位置命令，予脈波型伺服馬達驅動器，驅動器將其與馬達編碼器之位置回授值比較之後，以閉迴路方式精確控制馬達；第三種方式為馬達編碼器之位置回授值輸入位置控制器內，經閉迴路方式計算控制命令，並以電壓型式驅動速度控制型馬達驅動器。本計畫建立此三種平台，作為電路設計驗證的對象。

(4)

IPC

位置控制器 Driver M

脈衝型式 (a) 步進馬達控制模式

IPC

脈波型式(PWM) 伺服控制

迴路

位置回授 (b) 脈波型伺服馬達控制模式

IPC

電壓型式

伺服控制迴路

速度回授 V

位置回授

速度型伺服驅動器

(c) 電壓型伺服馬達控制模式

圖1.1 常見之馬達控制模式 (a)步進馬達控制模式 (b) 脈波型伺服馬達控制模式 (c) 電壓型伺服馬達控制模式

1.2 研究方法

本計畫的研究目的為設計適合硬體架構的運動軌跡方法，並研製一顆運動控制晶片，控制的對象包括步進馬達、脈波型、電壓型伺服馬達與線性馬達。本晶片包括運動軌跡產生電路及閉迴路控制電路的設計，其中以 PID 控制法作為閉迴路控制電路的實現。由於一般特殊應用晶片(ASIC)製作的流程由規格制定、電路設計、晶片製作，

(5)

到成品的完成需花費很長的時間，製作成本也很高，並不符合經濟效益與研究進度 [6]。故本計畫以可重複燒錄，以硬體模擬數位電路的 FPGA 邏輯元件來實現晶片設計。

由於數位積體電路十分龐大，若直接以硬體邏輯閘直接設計，設計時間十分冗長及具有錯誤難以偵測的缺點。故本計畫使用Verilog 硬體描述語言(hardware description language，HDL)來設計數位積體電路，其優點有設計規格具有完整及明確性、容易描述及錯誤偵測、符合時間及經濟效益等[7]。

本計畫研究方法流程說明如下：第一步，根據過往技術與相關領域，研究與設計運動軌跡方法；第二步，依據馬達特性，制訂運動控制晶片的規格與架構；第三步，

將運動控制晶片的架構，分為數個獨立的功能模組；第四步，針對每個功能模組，以 Verilog 硬體描述語言來描述並進行電路設計；第五步，各功能模組進行軟體的模擬與驗證；第六步，以 Xilinx 公司的 FPGA 與數位邏輯分析儀，來進行硬體層次的設計與驗證；第七步，整合獨立的功能模組並進行軟體及硬體層次的驗證；第八步，設計馬達的驅動電路與人機介面，以完成整個控制系統，其方法流程如圖 1.2 所示。

開始

運動軌跡規劃方法設計

晶片規格制定

晶片模組化分析

各模組電路設計

軟體層次模擬驗證

F P G A 硬體層次模擬驗證

各模組電路整合

軟體與硬體層次模擬驗證

驅動電路與人機介面設計

完成

Y

Y N

圖1.2 研究方法流程圖

(6)

1.3 計畫報告架構

本計畫報告共分為八章。第一章是緒論，對本計畫的研究動機與目的，及研究方法做簡單的說明。第二章是運動軌跡設計，針對過往技術的缺點，設計一速度曲線規劃方法來增進軌跡控制的效果。第三章是運動軌跡電路設計，以數位積體電路實現第二章的方法。第四章是 PID 控制器設計，以數位積體電路實現 PID 控制法則。第五章是運動控制介面電路設計，實現串列埠通訊模組之電路及模組電路整合。第六章為運動控制晶片實現與實驗結果，包括實驗發展系統介紹與說明，與運動控制晶片的實驗結果分析。第七章為嵌入式運動控制系統之應用實例，斜面直線倒單擺之甩上及平衡控制。第八章為結論與展望，對於研究過程與成果進行討論與檢討。

(7)

第二章運動軌跡設計 2.1 速度曲線的型式

速度曲線可以由數段曲線段組成，如加速曲線、減速曲線、等速曲線等，各曲線段有個別的時間區段。因為馬達必須平順的輸出扭力，故馬達電樞之電流變化必須為連續，不能有急遽的變化，所以馬達之轉動速度曲線應為連續的。但若速度曲線微分一次後的加速度曲線，具有不連續點，代表此加速度曲線為不連續曲線，而在此不連續點處會使馬達產生振動，造成馬達損壞及控制失效等問題。為避免上述情況，限制加速度曲線為連續曲線是必要的。當加速度曲線為連續曲線時，速度曲線至少為二方次曲線，而位置曲線至少為三次方曲線。等速曲線、梯形曲線及S-curve 曲線為常見的速度曲線，其中等速曲線為零次方曲線；梯形曲線為一次方曲線；S -curve 曲線為二次方曲線，以圖2.1 表示各速度曲線之加速度及速度的關係[8]。

(a)等速度曲線

(b) 梯形速度曲線

(c) S-Curve 速度曲線

圖2.1 常見速度曲線之加速度及速度的關係，(a)等速度曲線、(b)梯形速度曲線以及 (c)S-Curve 速度曲線

(8)

2.2 數位差分解析法

在一般多軸平台中，通常需要依照各軸要求之路徑來產生各運動軸之座標移動量，使多軸平台能夠精準完成要求之工作。從要求之連續路徑轉成各軸座標移動量，

須以軌跡規劃(trajectory planning)來完成此一工作。軌跡規劃可近似產生每個時間點上該軸所需之座標移動量，其近似法則之精確與否，攸關運動軌跡之精確性。

以馬達為多軸平台的驅動器時，若以二維曲線表示移動軌跡，橫軸為時間軸，縱軸表示馬達轉動量，如圖2.2 所示。各點表示馬達轉動角對應每個時間點的關係，將各點依序用直線予以連接，將得到一直線片段連續線。若各時間點越接近，片段連續線將越逼近平滑連續曲線，此平滑連續曲線即為理想移動軌跡。

T

0 2T 3T 4T 5T 6T 7T 8T

馬達運動軌跡連續曲線

時間馬達轉動量

( )t θ

圖2.2 馬達轉動量與時間示意圖

觀察其中一切割片段如圖 2.3 所示。馬達轉動從(kT, (θ kT )) 點沿直線移至 ( (k+1) , ((T θ k+1) ) )T 點，又其直線斜率為轉動速度，故馬達實際上是以等速度方式移動，期間所經過的時間為T 。對以一個脈衝命令轉動一次的馬達而言，若要實現圖 2.3 之等速運動，相當於自(kT, (θ kT )) 點將θ( (k+1) )T − (kT )θ 個脈衝在時間T 內均勻輸出，而數位差分解析法(DDA)是一個很好的解決方法。

0 KT 時間

馬達轉動量

(K+1)T ( )t

θ

圖2.3 移動路徑之切割片段

(9)

以數位差分解析法(DDA)在一個輸出週期(T )內均勻輸出脈衝個數( )時，將在輸出週期(T )內以增量累加數次，此累加的次數(

N

N M)須大於輸出脈衝個數( )。每當

累加值大於 M 時，輸出一個脈衝，並將累加值減去 M 以繼續累加。以表2.1 表示此輸出方法。

N

表2.1：DDA 脈衝輸出（M =8，N =5）

累加次數(M) 增量 (N ) 累加值 DDA 脈波輸出

1 5 5 L

2 5 2 H

3 5 7 L

4 5 4 H

5 5 1 H

6 5 6 L

7 5 3 H

8 5 0 H

以數位差分解析法輸出脈衝中，通常以組合數段等、加、減速工作段來達成多次方速度曲線的實現，其中各工作段具不同執行時間及輸出脈衝數，除需耗費的大量時間運算，因各工作段有不同的限制條件，使得相連接之工作段間的速度變化為非連續性，不能滿足連續速度曲線的規劃[9]。當執行時間的值與輸出脈衝的值越相近時，

其執行段中產生的脈衝越不均勻，此現象將使得脈衝輸出的速度曲線產生不規則變化，並導致脈衝輸出個數溢位或不足的現象。由上所述，數位差分解析法無法滿足硬體及多次方速度曲線的要求及限制，故本計畫提出不同的方法來解決此議題，並得以硬體簡易地來描述及實現。

2.3 S-Curve 運動軌跡設計

常見於工業界的SMT 設備、鑽孔、熔接或電梯等多軸平台，其軌跡規劃為點對點控制。點對點軌跡控制，著重目標定位的精確性，其移動速度之變化及移動時間之限制亦為重要的條件。

在使用伺服馬達作為多軸平台的制動器時，利用脈衝訊號作為驅動器的命令訊號，一個脈衝將使馬達轉動的一個編碼器脈波輸出的距離，或如步進馬達一個脈波對應一步轉角。故在實作上大部分以輸入馬達的脈衝訊號個數來決定馬達轉動角度的大小，而脈衝訊號的頻率變化則決定馬達轉動的速度變化與加速度大小，頻率越快的脈衝訊號使馬達轉動速度越快；頻率越慢的脈衝訊號使馬達轉動速度越慢[10]。所以控制脈衝的頻率變化就能產生馬達速度大小的變化及轉動時間的限制。

(10)

2.3.1 規劃速度曲線方法之設計

以二度空間表示速度曲線中，縱軸為速度值及橫軸為時間座標，單個時間值對應單個速度值，表現在此二度空間上的速度曲線必為連續的。本方法設計速度曲線由數段曲線段所組成，若積分一個二次方之速度曲線後，得到位置曲線；若微分此速度曲線一次為加速度曲線，微分兩次為急衝度曲線。其中位置曲線為三次方曲線；加速度曲線為一次方曲線；急衝度曲線為零次方曲線。零次方曲線是由數段的常數段所組成的，而每個常數段為不連續。

因為零次方曲線的各段曲線值為常數，所以使用者最好描述且也最好設計，故本方法先設計零次方曲線，再經由積分來得到速度曲線，此零次曲線可以為急衝度曲線、加速度曲線或速度曲線。若此零次方曲線為急衝度曲線，則積分兩次後得到速度曲線，若此零次方曲線為加速度曲線，則積分一次後得到速度曲線。因為各段曲線必須滿足邊界條件，使得積分後的曲線為連續曲線。S-Curve 速度曲線為兩次方曲線，

故其零次方曲線為急衝度曲線，以圖2.4 表示規劃 S-Curve 速度曲線之方法流程。

積分積分

時間時間時間

急衝度加速度速度

圖2.4 規劃 S-Curve 速度曲線之方法流程

常數段的值可能為正數、負數或零。若積分此零次方曲線k 次，積分後在正數及負數段將為k 次曲線；在零數段將為 k-1 次曲線，其中 k 為正整數。設計零次方曲線時，每個常數段有個別的時段，累加每個時段後的值，稱為總輸出時間。如第一段的時段為30 秒；第二段的時段為 20 秒，總輸出時間為 50 秒。

設計零次方曲線時，分兩情況討論，情況一，欲得到兩次方多項式的速度曲線，

需設計急衝度曲線；情況二，欲得到一次方多項式的速度曲線，需設計加速度曲線。

在情況一中，若常數段的值為正數，在速度曲線為二次方遞增曲線；若常數段的值為負數，在速度曲線為二次方遞減曲線；若常數段的值為零，此段速度曲線將視此段速度曲線的邊界值為正數、負數或零而不同，若初始值為正數，速度曲線為一次方遞增曲線；若初始值為負數，速度曲線為一次方遞減曲線；若初始值為零，速度曲線為零次曲線。在情況二中，若常數段的值為正數，在速度曲線為一次方遞增曲線；若常數

(11)

段的值為負數，在速度曲線為一次方遞減曲線；若常數段的值為零，速度曲線為零次方曲線。

2.3.2 脈衝輸出門檻值計算方法之設計

以脈衝的頻率變化表示速度曲線的大小變化，積分速度曲線一次後為位置曲線，

位置曲線的值與輸出脈衝個數的累加值為遞增的關係。若希望在一輸出時間內輸出個脈衝，並對應特定之速度曲線，以表示位置曲線的最末值。當系統之輸出時間未開始時，脈衝沒有輸出，則位置曲線的值為零；輸出時間增加時，脈衝開始輸出，

位置曲線的值亦隨時間而增加；若輸出時間結束時，第個脈波輸出，此時的位置曲線值為。故得知個脈波產生後，位置曲線的值為，可推算第 1 個脈衝產生時，位置曲線的值為除以的商數，令此商數為輸出門檻值(Threshold)。以圖 2.5 表示此方法。

N P

N

P N P

P N

圖2.5 數位差分解析法之示意圖

脈衝輸出門檻值為速度總和除以輸出脈衝數，其中位置曲線的值為速度值的累加，故位置曲線之最末值為速度值的總和。在輸出時間開始後，累加各時間點上之速度值，當累加值達一倍輸出門檻值時，輸出第一個脈衝，達兩倍輸出門檻值時，輸出第二個脈衝，以此類推，達倍輸出門檻值時便輸出第個脈衝，其中倍的輸出門檻值為。脈衝輸出門檻值之數學式子為：

P

N N N

P

( ) ( )

( )

Threshold = P

N 輸出門檻值速度總和

輸出脈波數 (2.1) 以上述方法產生對應速度曲線之脈衝命令時，需先預先求得其速度總和值(P)

(12)

以得到脈衝輸出門檻值，當使用者規劃的速度曲線越複雜時，預先計算脈衝輸出門檻值的過程越為煩複，使計算時間及硬體的記憶空間越大，不適合以硬體架構來描述或建立，且此一過程會使命令無法即時輸出，造成各段脈衝命令輸出延遲，系統無法即時控制[11]。為改善上述缺點，本方法建立一零次方曲線之初始模組，並利用速度總和與輸出時間之關係，來求得速度總和( )，以大量減少運算時間及過程。對應不同之等速速度曲線、梯形速度曲線及S-Curve，輸出脈衝變化之示意圖，以圖 2.6 表示。

P

速度曲線

速度

時間 0

時間

(a) 等速度曲線

速度曲線

速度

時間 0

時間

(b) 梯形速度曲線

速度曲線

速度

時間

0 時間

(c) S-Curve 速度曲線

圖2.6 以數位差分解析法輸出脈衝變化之示意圖(a)等速度曲線 (b)梯形速度曲線 (c)S-Curve 速度曲線

本方法針對不同的速度曲線規劃，建立不同的初始模組，規劃此模組之參數包含有積分次數(D)、比例和(T )、初始模組的各段常數值、初始模組的各段比例值及 臨界脈衝數(N )等。

(13)

設計零次方曲線時，每個常數段皆有個別的時段，累加各時段後的和為輸出時 間。設定比例和(T )參數，以此比例和代表輸出時間除以某一常數後的商，各時段除 以此一常數的值為各時段所代表的比例，比例和的選擇為可充分表示各曲線段的比例即可，故可以為任意數，但比例和若越大則運算量將越大。簡言之，將零次方曲線的時間軸縮小，使輸出時間為縮小為某一常數，以比例值來表示各常數段的時段，稱縮小後的零次方曲線為初始模組曲線，其中各常數段的值不改變。

積分次數參數( )為以積分初始模組得到位置曲線的方式中需積分的次數。若零次方曲線為急衝度曲線，積分次數參數為三；若零次方曲線為加速度曲線，積分次數參數為二，以此類推。

D

由上述可知，實際速度曲線為放大初始模組之時間軸的曲線，令此放大倍率為擴 增參數( G )。並表示擴增參數為輸出脈衝數(N )除以臨界脈衝數(N )的商，臨界脈衝數為小於比例和兩倍以上的數，此關係於2.3.3 節中證明。本方法以積分初始模組的方法得到初始模組之速度曲線，計算初始模組的速度總和(P )及最大速度(V_m)，

並藉由初始模組的積分次數參數、擴增參數，計算實際速度曲線之速度總和及最大速度值，其計算方法如(2.2)式。並以圖 2.7 表示此方法流程。

( )P 實際速度曲線之速度總和

( )^P [ ( )^G ( )^D ]

=初始模組之速度總和 × 擴增參數 ^{積分次數參數} (2.2)

積分初始模組

零次方曲線

初始模組加速度曲線

積分

P 初始模組

速度曲線

P

實際速度曲線 (2.2 )式

0 T T

T G T

0

0 0

圖2.7 以初始模組計算速度總和之方法

(14)

利用初始模組之速度總和經由(2.2)式的計算，可求得之實際速度曲線之速度總和。將此結果代入(2.1)式，可得到脈衝輸出門檻值，此關係以(2.3)式表示，其中 N 為輸出脈衝數。

( )

P P GD

Threshold

N N

= = ×

脈衝輸出門檻值 (2.3)

梯形速度曲線與S-Curve 速度曲線為最常用於點對點運動軌跡控制，以下列證明可利用(2.2)式求得此兩速度曲線的速度總和。

一、梯形速度曲線

設計初始模組之零次方曲線為加速度曲線，如圖2.8(a)所示。此初始模組為三個常數段，第一段之比例值為T₁，常數值為A；第二段之比例值為T₂，常數值為0；第三段之比例值為T₁，常數值為-A。其中，擴增常數( )為已知，比例和(G T )為

1 2

T + +T T₁。積分此初始模組一次以求得速度曲線，並計算其速度總和(P )。

(a) 加速度曲線 (b) 速度曲線圖2.8 梯形速度曲線之初始模組

(15)

計算初始模組之速度總和

1 2 1

1 2 1 1

2 1

1 1

0 0 0

1 1

0 0 0 0

1 1

0 0

1 2 1

( )

T T T

t t t

T T T T

t t t t

T T

t t

P At AT A T

At AT AT At

AT AT AT T T

= = =

= = = =

= =

= + + −

= +

∑ ∑ ∑

∑ ∑ ∑ ∑

∑ ∑

t

利用(2.2)式求得速度總和P′

2

1( 2 1) P′ =G P^D =G P² =AG T T +T

求實際速度曲線之速度總和P

1 2 1

1 2 1 1

2 1

1 1

0 0 0

1 1

0 0 0 0

1 1

0 0

1 2 1

2

1 2 1

( ) ( ) ( ) ( ) ( ) ( )

( )( ) ( )( )

GT GT GT

t t t

GT GT GT GT

t t t t

GT GT

t t

P At A GT A GT t

At A GT A GT At

A GT A GT A GT GT GT G AT T T

= = =

= = = =

= =

= + + −

= +

∑ ∑ ∑

∑ ∑ ∑ ∑

∑ ∑

.

可得到誤差為

2

1( 2 1)] ( 1)( 2 1)

perr = −P P′= −P G P² = G AT T²[ +T −G AT T +T =0

由於誤差為零，得可以(2.2)計算梯形速度曲線之速度總和。

二、 S-Curve 速度曲線

設計初始模組之零次方曲線為急衝度曲線，如圖2.9 (a)所示。此初始模組為七個常數段，第一段之比例值為T₁，常數值為A；第二段之比例值為T₂，常數值為0；第三段之比例值為T ，常數值為−B;第四段之比例值為T ，常數值為0；第五段之比例

(16)

值為T₃，常數值為−B；第六段之比例值為T₂，常數值為 0；第七段之比例值為T₁，常數值為A。其中，擴增常數( G )為已知，比例和(T )為T₁+ + + + + +T₂ T₃ T₄ T₃ T₂ T₁。另S-Curve 為起始速度與末速度皆為零之速度曲線，故限定AT₁ =BT₃可滿足此條件。

積分此初始模組兩次以求得速度曲線，並計算其速度總和(P )。

(a) 急衝度曲線

(b)速度曲線

圖2.9 S-Curve速度曲線之初始模組

計算初始模組之速度總和

(17)

1 2 3 4

3 2

2 2 2 2 2

1 1 1 1 2 1 1 1

0 0 0 0

2 2 2 2 2

1 3 3 1 1 2 1 3 3 1 1 2

0 0

2 2

1 3 3 3

1 1 1 1 1

( ) ( ) ( ( )) (

2 2 2 2 2

1 1 1 1 1

) [( ) ] [(

2 2 2 2 2

1 1

) ]

2 2

T

T T T

t t t t

T T

t t

P

At AT AT t AT AT T AT t Bt AT AT T

AT T BT AT AT T AT T BT Bt AT AT T

AT T BT BT BT

= = = =

= =

=

+ + + + + − + +

+ − + + + − − + ⎧⎨ +

⎩

+ − − −

∑ ∑ ∑ ∑

∑ ∑

2

1 2 2

3 1 1 2 1 3 3

0

2

3 2 3

1 1

[( ) ]

2 2

( 1 ) 2

T

t

t AT AT T AT T BT BT

BT T BT t At

=

⎫+ ⎧ + + − −

⎬ ⎨

⎭ ⎩

− − − ⎫⎬

⎭

∑ ¹₂ ³²

3 3 3 3

1 2 2 4

3 3 3 3 3

4 4 4

2 2 2 2

1 1 1 1 2 1 1

0 0 0 0 0 0 0 0

2 2 2

1 2 1 3 3 1 1 2 1 3 3

0 0 0 0 0 0 0 0

1 1 1 1 1

2 2 2 2 2

1 1 1 1

2 2 2 2

T T T T

t t t t t t t t

T T T T T

T T T

t t t t t t t t

At AT AT t AT AT T AT t Bt AT

AT T AT T BT AT AT T AT T BT Bt

= = = = = = = =

= + + + + + − +

+ + − + + + − −

∑ ∑ ∑ ∑ ∑ ∑ ∑ ∑

2

2 2 2 2 2 1 1 1

1 1 1 1

3 3

1 2 2

2 2 2

1 1 2 1 3 3 3 1 1 2 1 3

0 0 0 0 0 0 0 0

2 2

3 3 2 3

0 0 0 0

2 2

1 3 1

0 0 0 0 0

1 1

2 2

1

2

T T T T T T T T

t t t t t t t t

T T T T

t t t t

T T

T T T

t t t t t

AT AT T AT T BT BT t AT AT T AT T

BT BT T BT t At

At Bt AT t BT t AT t

= = = = = = = =

= = = =

= = = = =

+ + + − − + + +

− − − +

= − + − + −

∑ ∑ ∑ ∑ ∑ ∑ ∑ ∑

∑ ∑ ∑ ∑

∑ ∑ ∑ ∑ ∑ ¹ ³ ¹² ² ¹² ³ ^{1 2 3}

0

2 2 2 2 3 2

1 4 1 2 4 1 3 4 3 4 1 3 1 2 3 1 3 3 1 2

2 2 3 2 2 2

1 2 1 2 3 3 2 1 1 2 1 3 3 1 3 2 1

1 1

2 2

1 1 1 1

2 2 2 2

1

2

T

t

BT t AT T AT T AT T T

AT T AT T T AT T T BT T AT T AT T T AT T BT AT T AT T AT T T BT T AT AT T AT T BT T BT T T

=

+ + +

+ + + − + + + − +

+ + − + + + − −

∑

1 2

3 3 3 3 3

1 1 1 2 2 2 2

1 3 1

2 2 2 2

1 1

3 1 3 1 2 3 1 4 1 2 4 1 3 4 3 4 1 3

2 3 2 2

1 2 3 1 3 3 1 2 1 2 1 2 3

( 1)(2 1) ( 1)

( 1)(2 1) ( 1) ( 1)

6 6 2 2 2

( 1) 1 1 1 1

2 2 2 2 2

1 1

2 2

T T T T T

T T T T T T T

A B AT BT AT

BT T T AT T ATT T AT T ATT T ATT T BT T AT T

ATT T ATT BT AT T ATT ATT T

+ + +

+ + + +

= − + − +

− + + + + + + − +

+ + − + + + ₃² ₂ ₁³ ₁² ₂

2 2

1 3 3 1 3 2 1

3 3 3 3 3

1 1 1 1 1 2 2

1 3 1

2 2 3

1 2

( 1)(2 1) ( 1)

( 1)(2 1) ( 1) ( 1)

6 6 2 2 2

( 1) 5 2 2

BT T AT AT T AT T BT T BT T T

T T T T T

T T T T T T T

A B AT BT AT

BT T T A

− + +

+ − −

+ + +

+ + + +

= − + − +

− + + ₁² ₃ _{1 2 3} 1 ₁² ₄ _{1 2 4} _{1 3 4} 1 ₃² ₄ 1 ₁² ₃

3

2 2 2

T T + ATT T + AT T +ATT T +ATT T − BT T + AT T

3 2 2 2 3 2

3 1 2 1 2 3 2 1 3 1 3 2 1

1 3 1

2BT 2AT T AT T BT T 2AT BT T BT T T

− + + − + − − (2.4)

(18)

引入限制條件AT₁=BT₃於(2.4)式之第一、二、三、四項中，將其簡化如(2.5)式

3 3 3 3

1 1 1 1

1 3 1 3

2 2

3 3 3 3

1 1 1 1

2 2 2 2

1 1 3 3 3 3 1 1

1

( 1)(2 1) ( 1)

6 6 2 2

2 3 1 3 3

[ ] [ ]

6 6 6 6

2 3 1 2 3 1 3 3 3 3

[ ] 6

T T T T

AT BT AT BT

T T T T

AT AT AT AT

T T T T T T T T

AT

+ + +

− + −

+ + +

= − + −

+ + − − − + + − −

=

2 2

2 3

3 1

1 1 3 1

1 1

[ ]

6 6 6

T T

AT − AT T AT

= = − (2.5) 引入限制條件AT₁=BT₃於(2.4)式之第五、六項中，將其簡化如(2.6)式

2 2 2 2

1 3

1

2 2 2 2

( 1) ( 1)

2 2

( ( 1) ( 1)) 2

T T T T

AT BT

AT T T T T

+ +

−

= + − +

= (2.6) 0 將(2.5)式、(2.6)式代入(2.4)式中，可將其簡化如下

2 3 2 2

1 3 1 1 3 1 2 3 1 4 1 2 4 1 3 4 3 4

2 3 2 2 2 3 2

1 3 3 1 2 1 2 3 2 1 3 1 3 2 1

1 1 5 1 1

6 6 2 3 2 2

1 1 3 1

2 2 2 2

P AT T AT AT T AT T T AT T AT T T AT T T BT T AT T BT AT T AT T BT T AT BT T BT T T

= − + + + + + −

+ − + + − + − −

2

利用(2.2)式求得速度總和P′

2 3 2 2

1 3 1 1 3 1 2 3 1 4 1 2 4 1 3 4 3 4

2 3 2 2 2 3 2

1 3 3 1 2 1 2 3 2 1 3 1 3 2 1

1 1 5 1 1

( 3

6 6 2 2 2

1 1 3 1

)

2 2 2 2

P G P G P

G AT T AT AT T AT T T AT T AT T T AT T T BT T AT T BT AT T AT T BT T AT BT T BT T T

′ = =

= − + + + + + −

+ − + + − + − −

D 3

3 2

如(2.4)式、(2.5)式與(2.6)式之計算過程，求得實際速度曲線之速度總和 P

(19)

2 3 2

1 3 1 1 3 1 2 3

2 2

1 4 1 2 4 1 3 4 3 4

2 3 2 2 2

1 3 3 1 2 1 2 3 2

1 1 5

( )( ) ( ) ( ) ( ) 3 ( )( )( )

6 6 2

1 1

( ) ( ) ( )( )( ) ( )( )( ) ( ) ( )

2 2

1 1 3

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

2 2 2

P A GT GT A GT A GT GT A GT GT GT

A GT GT A GT GT GT A GT GT GT B GT GT A GT GT B GT A GT GT A GT GT B GT GT

= − + +

+ + + −

+ − + + −

3 2

1 3 1 3 2 1

3 2 3 2 2

1 3 1 1 3 1 2 3 1 4 1 2 4 1 3 4 3 4

2 3 2 2 2 3 2

1 3 3 1 2 1 2 3 2 1 3 1 3 2

1 ( ) ( ) ( ) ( )( )( ) 2

1 1 5 1 1

( 3

6 6 2 2 2

1 1 3 1

)

2 2 2 2

A GT B GT GT B GT GT GT

G AT T AT AT T AT T T AT T AT T T AT T T BT T AT T BT AT T AT T BT T AT BT T BT T T

+ − −

= − + + + + + −

+ − + + − + − −

2

可得到誤差為

err 0

p = −P P′= −P G P³ =

由於誤差為零，得可以(2.2)計算 S-Curve 速度曲線之速度總和。

2.3.3 輸出脈衝最大速度與時間限制

使用脈衝作為馬達的控制命令時，為使馬達即時響應命令變化，脈衝的頻率必須低於馬達的響應頻寬，故限制輸出脈衝之最大速度是必要的[12]。以兩脈衝之間隔時間倒數為此脈衝輸出的速度，其最短間隔時間表示輸出脈衝的最大速度。在時間軸上，若脈衝寬度為∆T，最短兩脈衝間的時間以 S T∆ 表示，其中設定為間隔倍率參數，為了使脈衝完整輸出，間隔倍率( )必須大於或等於一個脈衝寬度，此關係以圖 2.10 表示。

S S

1

Vm

S T

= × ∆

圖2.10 以間隔倍率( )表示最短間隔時間 S