應用可變步長適應滑模結合指數律演算法於機械手臂追跡之控制器設計

全文

(1)國立臺灣師範大學機電工程學系碩士論文指導教授：陳美勇博士應用可變步長適應滑模結合指數律演算法於機械手臂追跡之控制器設計 Design a Variable Step-Size Adaptive Sliding Mode Controller with Exponential Law for Robot Arm Tracking. 研究生：楊智翔撰中. 華. 民. 國. 1. 0. 4. 年. 1. 月.

(2) 摘要本研究主要在於結合指數律(exponential law, EL)於可變步長適應滑模控制器(variable step size adaptive sliding mode controller, VSSASMC)並應用於機械手臂的追跡。在設計控制器時考慮到機械手臂的不確定量與外界干擾，於是本研究選擇具有良好強健性的滑動模式控制器為主控制器。而滑動模式控制中有一設計參數為切換增益(switching gain)，此參數必須大於系統的干擾和不確定量的上界(upper bound)，但是通常我們並無法直接知道上界值只能通過重覆測試調整。為了使系統能夠應付未知邊界的不確定量與干擾，本研究加入適應控制調整滑動模式中的上界參數，使控制器能應付多變的情況。而適應控制本身則會使系統響應變慢，因此引入了指數律使系統更快收斂。而指數律不只可以與滑動模式控制結合達到減小跳切現象的效果；同時也能和適應控制結合成可變步長適應控制，使適應律的步長依誤差而調整。並且通過 Lyapunov 函數及 Barbalat 引理證明系統穩定性。最後經由實驗驗證此控制器的性能。. 關鍵詞：可變步長適應控制、滑動模式控制、機械手臂，上界，指數律。. i.

(3) Abstract In this study, we design an adaptive sliding mode controller which is applied on trajectory tracking of robot arms. Consider the uncertainties and external disturbances of a robot arm, we choose the sliding mode control (SMC) to be major one. We need to decide a switching gain bigger than the upper bound of system uncertainty. Usually we can’t figure out the upper bound of system uncertainty, we only decide a switching gain by trials and errors. Therefore, we propose an adaptive control to tune the switching gain of SMC that would be able to handle the unknown disturbances and uncertainties.. Subsequently, the adaptive control makes system transient response slowly, so we introduce an exponential law (EL) to make system transient response faster. We not only combine EL with SMC to reduce chattering, but also combine EL with adaptive control to be the variable step size adaptive control which step size is adjust by error. Then we proof the stability of system by Lyapunov function and Barbalat’s Lemma. Consequently, the experiment results show excellent performance of this controller.. Keywords：variable step-size adaptive control, sliding mode control, robot arm, upper bound, exponential law.. ii.

(4) 誌謝能夠順利完成這本論文首先感謝陳美勇教授的指導，不但提供良好的實驗環境及設備，也在研究上提供許多想法及建議讓我有足夠的能力、知識完成此論文。在研究之外的問題老師也不吝給予幫助，更教導我們做事的態度與待人處世的道理。. 相當感謝口試委員郭重顯教授、莊鎮嘉教授及練光祐教授，對本論文不足之處提出指正及建議，使得本論文可以更加完善。. 在此也要感謝實驗室中的同學們秉剛、昀翰、玠虢、高遠、煒騰、威任、維斌、敬棋等人，在實驗室中踴躍的討論使我可以了解到更多領域的知識，也充實了自己的能力。. 最後感謝家人們，父母、哥哥以及姑姑對我的栽培及照顧，幫助家裡大大小小的事情，使我有更多的精力能放在學業上，很高興不負期望的完成了碩士學業。. 楊智翔民國一零四年一月精密運動控制實驗室. iii.

(5) 目錄摘要......................................................................................................................... i Abstract................................................................................................................... ii 誌謝.......................................................................................................................iii 目錄.......................................................................................................................iv 表目錄...................................................................................................................vi 圖目錄..................................................................................................................vii 第一章緒論..........................................................................................................1 1.1 前言...........................................................................................................1 1.2 文獻回顧...................................................................................................2 1.3 研究動機與目的.......................................................................................6 1.4 本論文之貢獻...........................................................................................7 1.5 論文架構...................................................................................................8 第二章機器人學理論基礎..................................................................................9 2.1 機械手臂運動學.......................................................................................9 2.1.1 Denavit–Hartenberg 參數................................................................9 2.1.2 正向運動學.....................................................................................11 2.1.3 逆向運動學.....................................................................................13 2.2 機械手臂動力學.....................................................................................16 2.2.1 Lagrange 方程式............................................................................16 第三章控制器理論設計....................................................................................21 3.1 Lyapunov 穩定度理論............................................................................21 3.2 滑動模式控制.........................................................................................22 3.3 指數律.....................................................................................................24 3.4 適應性控制.............................................................................................25 iv.

(6) 第四章控制器設計............................................................................................27 4.1 滑模控制結合指數律.............................................................................27 4.2 可變步長適應滑模控制結合指數律.....................................................30 4.3 Lyapunov 穩定度分析............................................................................32 第五章實驗結果與討論....................................................................................35 5.1 實驗設備.................................................................................................35 5.1.1 SCARA 機械手臂..........................................................................35 5.1.2 驅動器............................................................................................36 5.1.3 資料擷取卡....................................................................................37 5.1.4 軟體................................................................................................38 5.2 實驗結果.................................................................................................39 5.2.1 雙軸分別定位控制........................................................................39 5.2.2 雙軸分別連續定位控制.................................................................42 5.2.3 雙軸同動追跡控制.......................................................................46 5.2.4 強健性控制.....................................................................................49 5.2.5 機械手臂末端畫圓實驗................................................................52 第六章結論與未來展望....................................................................................53 參考文獻..............................................................................................................54. v.

(7) 表目錄表 2-1 SCARA 機械手臂之 D-H 參數表............................................................11 表 5-1 系統參數表…..........................................................................................36 表 5-2 機械手臂第一、二軸定位實驗誤差......................................................42 表 5-3 機械手臂第一、二軸追跡實驗誤差......................................................49 表 5-4 強健性實驗誤差......................................................................................51. vi.

(8) 圖目錄圖1-1 控制器強健性模擬......................................................................................2 圖1-2 (a)加入與(b)未加入指數律的滑動模式控制器響應比較.........................3 圖1-3 馬達追跡響應圖..........................................................................................4 圖1-4 參數估測情況..............................................................................................4 圖1-5 追跡響應圖..................................................................................................5 圖1-6 可變步長適應控制響應比較圖..................................................................6 圖2-1 SCARA 機械手臂示意圖............................................................................9 圖2-2 第 i 個連桿的 D-H 參數示意圖..................................................................10 圖2-3 SCARA 機戒手臂幾何參數定義..............................................................10 圖2-4 二軸平面手臂示意圖................................................................................14 圖2-5 逆向運動學多組解示意圖........................................................................14 圖2-6 逆向運動學求解........................................................................................15 圖2-7 SCARA 機戒手臂模型參數定義..............................................................18 圖3-1 圖 3-1 系統狀態隨時間收斂至滑動面再到平衡......................................22 圖3-2 滑動面上的跳切現象示意圖....................................................................23 圖3-3 自我調變控制器示意圖............................................................................26 圖3-4 參考模型控制器示意圖............................................................................26 圖4-1 p=1 時，不同 α 的 si－usw,i 特性曲線圖.......................................................29 圖4-2 α=1 時，不同 p 的 si－usw,i 特性曲線圖.......................................................30 圖5-1 SCARA 型四軸機械手臂..…………...……….……................................35 圖5-2 系統參數標示圖........................................................................................36 圖5-3 驅動器功能接線圖....................................................................................37 圖5-4 研華科技公司 PCI-1720U 資料擷取卡...................................................37 圖5-5 研華科技公司 PCI-1240U 運動控制軸卡.................................................38 vii.

(9) 圖5-6 第一軸馬達定位響應情況. ......................................................................39 圖5-7 第一軸馬達定位誤差................................................................................40 圖5-8 2 秒後第一軸馬達定位誤差.....................................................................40 圖5-9 第二軸馬達定位響應情況........................................................................40 圖5-10 第二軸馬達定位誤差..............................................................................41 圖5-11 2 秒後第二軸馬達定位誤差...................................................................41 圖5-12 第一、二軸馬達控制量............................................................................41 圖5-13 2 秒後第一、二軸馬達控制量.................................................................42 圖5-14 第一軸馬達連續定位響應情況..............................................................43 圖5-15 第一軸馬達連續定位誤差......................................................................43 圖5-16 第一軸馬達連續定位暫態誤差..............................................................43 圖5-17 第一軸馬達連續定位穩態誤差..............................................................44 圖5-18 第一軸馬達控制量..................................................................................44 圖5-19 第二軸馬達連續定位響應......................................................................44 圖5-20 第二軸馬達連續定位誤差......................................................................45 圖5-21 第二軸馬達連續定位暫態誤差..............................................................45 圖5-22 第二軸馬達連續定位穩態誤差..............................................................45 圖5-23 第二軸馬達控制量..................................................................................46 圖5-24 第一軸馬達追跡響應情況......................................................................47 圖5-25 第一軸馬達追跡誤差..............................................................................47 圖5-26 第一軸馬達控制量..................................................................................47 圖5-27 第二軸馬達追跡響應情況......................................................................48 圖5-28 第二軸馬達追跡誤差..............................................................................48 圖5-29 第二軸馬達控制量..................................................................................48 圖5-30 第一軸馬達追跡響應情況......................................................................49 圖5-31 第一軸馬達追跡誤差..............................................................................50 viii.

(10) 圖5-32 第一軸馬達控制量..................................................................................50 圖5-33 第二軸馬達追跡響應情況......................................................................50 圖5-34 第二軸馬達追跡誤差..............................................................................51 圖5-35 第二軸馬達控制量..................................................................................51 圖5-36 5-36 機戒手臂畫圓實驗結果單位:公分..............................................52. ix.

(11) 第一章緒論 1.1 前言機械手臂早期被應用於傳統工業中，其快速且高精度的特性非常適合用來作重複制式化的工作。隨著機械手臂技術的發展，這種特性已經可以取代人力達到自動化生產，如此一來不但可以減少成本也能增加工作時間提升產量。除了成本及產量的考量還有工作環境的問題，像是半導體、微機電等常常需要高溫或無塵室的環境，製程中也會通入一些化學氣體，考慮到這些安全上的問題就很適合利用機械手臂代替人力。機械手臂除了在工業上的應用還有像是深海探勘，航太方面的外星探勘，醫療方面的手術型機械手臂[1]、機器手義肢等不同領域中都能看到機械手臂被廣泛應用。另外，機械手臂還有多種不同操作方式，例如在人體加裝感測器元件，以直觀方式操作機械手臂[2]，或是機械外骨骼，甚至搭配攝影機進行遠端操作[3]以及結合自走車進行區域探索[4]。而控制機械手臂的困難點在於其高度非線性的系統模型，以串連式機械手臂而言各個關節的耦合度相當高，加上桿件撓性、馬達死區、摩擦力等非線性因素造成機械手臂模型中存在相當多的不確定量。因此在前人研究中，多以具有良好強健性的控制器來控制機械手臂[5][6][7]。. 1.

(12) 1.2 文獻回顧如先前所提到的有相當多應用於機械手臂控制上的控制器都需要良好的強健性。 Mehmet Ö nder Efe 提出了適應神經模糊推論系統 (adaptive neuro-fuzzy inference system)作為一種參數調整方式並結合滑動模式控制來提高控制器的強健性用於控制二軸機械手臂[8]。在模擬中，2 秒時在夾爪端加入 5 公斤的負重，並在 5 秒時去掉負重。9 秒時同樣加入 5 公斤的負重，並在 12 秒時去掉負重，15 秒後機械手臂靜止，如圖 1-1。可以看出經過第一次加入負重的調整後，第二次加入負重時對機械手臂產生的影響相當小，足以證明此控制器擁有良好的強健性。. 圖 1-1 控制器強健性模擬[8]. 2.

(13) Charles J. Fallaha，Maarouf Saad，Hadi Youssef Kanaan，Kamal Al-Haddad 等人將指數律(exponential reaching law)與滑動模式控制結合，來改善滑動模式控制的收斂速度及跳切現象[9]，如圖 1-2。而指數律主要特性為有上下邊界的函數，其函數大小會隨著誤差由大到小從上界以指數函數收斂至下界。由圖中可以看出加入指數律的滑動模式其收斂速度及跳切現象確實都比未加入的好。. 圖 1-2 (a)加入與(b)未加入指數律的滑動模式控制器響應比較[9]. 3.

(14) C.C. Cheng，S.H. Chien，F.C. Shih 等人的研究中設計了一個強健性適應可變結構控制器[10]。在文獻中做了一個假設，系統不確定量(uncertainty)的大小與系統狀態的大小之間存在著一個線性組合的關係，因此可以藉由適應控制估測係數來得到系統不確定量的上界，並將此上界參數用於滑動模式控制器設計中，其響應情況如圖 1-3、圖 1-4。. 圖 1-3 馬達追跡響應圖[10]. 圖 1-4 參數估測情況[10]. 4.

(15) 在適應控制中的參數估測通常會造成系統暫態響應變糟，而為了加快適應律的收斂速度會將步長加大，但是此方法存在著一個問題，當誤差已經收斂到一個微小範圍了，但是參數估測卻仍在進行中，透過不斷的積分被放大。而且此估測參數通常會被控制器使用，導致控制量也跟著不斷放大造成內部不穩定的情況。如同 Zi-Jiang Yang，Youichirou Fukushima，Pan Qin 的研究中遇到的問題[11]。這篇文獻設計一適應滑動模式控制器，同樣以適應控制估測不確定量的上界，再將上界參數用於滑動模式控制中，其響應結果如圖 1-5。其中 e 為追跡誤差，r 為滑動模式控制的滑動函數(sliding function)，u 為控制量， wˆ 為干擾估測量，ˆ 為適應控制所估測的系統不確定量上界，且. ˆ 為|r|的相關函數。從圖中可以看出 r 已經收斂到一個範圍，但是ˆ 卻不斷的積分，且導致控制量也跟著不斷增加。. 圖 1-5 追跡響應圖. 5.

(16) Raymond H. Kwong，Edward W. Johnston 的研究中提出了可變步長適應控制演算法[12]。通過將適應律中的增益設計為誤差相關函數，使得估測參數時在誤差大時就具有高增益時的收斂速度，而誤差小時具有維持穩定的特性，如圖 1-6 所示。. 圖 1-6 可變步長適應控制響應比較圖. 1.3 研究動機與目的本研究以滑動模式控制作為主要控制器，利用其強健性的特色來控制機械手臂。而滑動模式控制中需要找到一個系統不確定量的上界參數(upper bound)，此參數必須保證大於等於系統中所有不確定量和外部干擾等未知參數。而在實際應用中，此上界參數通常無法事先準確估測，只能藉由機械手臂的工作內容和工作環境大略的猜測，再加上重複實驗調整出可以使用的參數值。但是此方式過於繁瑣，於是本研究使用適應性方式調整上界參數值， 6.

(17) 如此一來控制器就能自行估測此參數，不用再藉由重複實驗調整同時也能夠應付更多變的環境。考慮到適性滑動模式控制的一些缺點，像是適應控制響應速度較慢，滑動模式控制有跳切現象等問題。本研究引入了可變步長適應控制演算法，以及指數律來改良控制器效能，並且希望最後設計出的控制器依然保有良好的強健性及穩定性。. 1.4 本研究之貢獻本研究設計一適應滑動模式控制器，考慮到適應控制容易造成暫態響應變糟，使用可變步長適應控制演算法改善適應控制的收斂速度及穩定性，而可變步長適應控制及滑動模式控制分別結合指數律，用來加快收斂速度適應控制在估測參數時需要花費時間，容易造成系統暫態響應變差。為了改善系統暫態響應，我們希望可以提升適應控制的估測速度，但是同時也要避免在 1.2 節中提到的內部不穩定情況。因此選擇可變步長 (variable step-size)的適應律作為提升估測速度的方式。在適應律中的增益值通常設為常數，而在文獻中也有將此增益設為變數的研究[12][13][14]。在這些研究中會將此增益值設計為誤差的函數，並且有著當誤差大時增益隨之變大以便加快收斂速度；以及誤差小時增益隨之變小，以保持穩定的特性。那麼如果在誤差可以收斂到很小的時候，可變步長適應控制是不是就沒有他的重要性了呢？其實在一般的參數估測時適應律都是誤差的相關函數，而誤差本身有正、負值，因此積分後整體影響不明顯。而在本論文中是要估測系統不確定量的上界，經過推導後適應律會是誤差的絕對值或是誤差的範數(norm)，絕對值和範數的計算結果都是大於等於零的值所以在積分後有明顯的影響，因此本研究需要加入可變步長適應控制的技術。在 1.2 節中我們提到了指數律(exponential law, EL)與滑動模式控制的結 7.

(18) 合[9]，並且這樣的結合有效加快滑動模式控制之收斂速度以及減小跳切現象。同時也發現指數律的特性與可變步長適應控制中的增益值特性相同，於是在本研究中更進一步的將指數律與可變步長適應滑動模式控制作結合。. 1.5 論文架構本論文一共分為六個章節，內容分別如下。第一章：緒論了解機械手臂控制中前人之研究，以及說明研究目的。第二章：機器人學理論基礎介紹機器人學中運動學及動力學，並推導本研究使用的機械手臂模型。第三章：控制器理論設計介紹本研究所設計之控制器基本理論。第四章：控制器設計說明本研究之控制器設計方式及過程推導。第五章：實驗結果與討論藉由實驗驗證此控制器應用於機械手臂上之效能。第六章：結論與未來展望針對本研究內容進行總結，並針對不足之處提出未來目標。. 8.

(19) 第二章機器人學理論基礎 2.1 機械手臂運動學本研究所使用之機器手臂為 Selective Compliance Assembly Robot Arm (SCARA)型機械手臂，包含兩個轉動關節、一個移動關節與末端夾爪，圖 2-1 為其模型示意圖。SCARA 機器手臂的特點是第三軸移動軸只影響機械手臂末端高度，而一、二軸共同影響著末端的水平位置，由於垂直位置和水平位置彼此獨立不會互相影響，所以在分析和操作上來的相對容易。. 圖 2-1 SCARA 機械手臂示意圖. 2.1.1 Denavit–Hartenberg 參數在機器手臂的控制中，為了了解各個馬達的角度與機器手臂末端位置之間的關係，我們會用到兩種方法。分別是將各軸馬達的角度換算成末端位置的正向運動學(forward kinematic )；以及將末端位置換算成各軸角度的逆向運動學(inverse kinematic)。在計算正逆向運動學之前，我們必須先了解每個連桿的幾何性質，最常見的方式即是使用 Denavit-Hartenberg(D-H)參數描述每個連桿的幾何參數。在定義 D-H 參數之前我們要先定義好每個連桿的座標系，在習慣上我們會將基座定為座標系 0，每個關節的旋轉軸或移動軸定為 z 軸，x i 軸與 z i-1 9.

(20) 軸垂直，x i 軸與 z i-1 軸相交，手臂末端與前一軸座標系相同。決定完座標系後，我們可以描述連桿的幾何性質。以第 i 個連桿為例，其中共有四個幾何參數需要定義：ai 表示沿 x i-1 方向上 z i-1 到 z i 的距離；αi 表示繞 x i-1 方向上 z i-1 到. z i 的角度；di 表示沿 z i 方向上 x i-1 到 x i 的距離；θi 表示繞 z i 方向上 x i-1. 到 x i 的角度，如圖 2-2。. 圖 2-2 第 i 個連桿的 D-H 參數示意圖接下來依照上述方式對 SCARA 機戒手臂進行座標定義，座標系 0 定在基座上，座標系 1 定在第 2 軸的軸心上，座標系 2 定在第 3 軸的質心上，座標系 3 定在機械手臂末端，如圖 2-3。並建立 D-H 參數表方便後續推導正向運動學，表 2-1。 z0. z1. θ1. y1. θ2 x2 x1 a1. a2. d1. Δd3. y2 z2. y0. x3 y3. x0 圖 2-3 SCARA 機戒手臂幾何參數定義 10. z3. d3.

(21) 表 2-1 SCARA 機械手臂之 D-H 參數表 i. ai. αi. di. θi. 1. a1. 0. d1. θ1. 2. a2. 180. 0. θ2. 3. 0. 0. d3+ Δd3. 0. 2.1.2 正向運動學在正向運動學中，我們需要知道每個軸的旋轉或移動量對機械手臂末端位置的影響。在對機械手臂定義完座標系和 D-H 參數後，我們可以利用座標系的平移和旋轉矩陣得出每個軸之間的轉換矩陣(transformation matrix)。依照當下的座標系對 z i 軸旋轉 θi 角，移動 di 距離，再對 x i 軸移動 ai 距離，旋轉 αi 角，寫出轉換矩陣 Ai。 Ai  Rot z , Trans z , Trans x ,a Rot x , i. i. i. i. 0 0  1 0  cos i  sin  i  sin  cos i 0 0   0 1 i    0 0 1 0  0 0   0 0 1  0 0  0 0 0  1 0 0 ai   1  0 1 0 0  0 cos i  sin  i    0 0 1 0  0 sin  i cos i   0 0  0 0 0 1  0 cos i  sin  i cos i sin  i sin  i   sin  i cos i cos i  cos i sin  i   0 sin  i cos i  0 0 0 . 0 0 0  1 di   0 1 0  0  0   1  ai cos i 0. ai sin  i di 1. (2-1).      . 再將 D-H 參數表代入，可得到轉換矩陣 A1、A2、A3。為了方便表示，本篇論文將 cosθi，sinθi，cos(θi+θj)，sin(θi+θj)以 ci，si，cij，sij 表示。 11.

(22)   A1     . c1.  s1. 0. s1. c1. 0. 0. 0. 1. 0. 0. 0.   A2     . c2. s2. 0. s2.  c2. 0. 0. 0. 1. 0. 0. 0.   A3     . a1c1  a1s1  d1   1  a2c2  a2 s2  0   1 . 1. 0. 0. 0. 1. 0. 0. 0. 1. 0. 0. 0.  0  d 3  d 3   1  0. (2-2). 得到每個關節之間的轉換矩陣後我們可以推算出機械手臂末端與基座的轉換矩陣如(2-3)。在已知機械手臂連桿長度以及每個關節的旋轉、移動量時，我們可以藉由此轉換矩陣計算出以基座座標表示的機械手臂末端位置 dx、dy、 dz。.  r11 r12 r r 0 T3  A1 A2 A3   21 22  r31 r32  0 0. r13 r23 r33 0. dx  d y  dz   1. (2-3). 其中，. r11  c12 r12  s12. r13  0 r21  s12 r22  c12 12.

(23) r23  0 r31  0 r32  0. r33  1 d x  a1c1  a2c12 d y  a1s1  a2 s12. d z  d1  d3  d3. 2.1.3 逆向運動學在操作機械手臂時，我們會先決定機械手臂末端的位置，再計算每個馬達該轉到的角度並進行控制，此計算過程即為逆向運動學。通常求解方式分為代數法、幾何法、數值法等。代數法在求解過程中相當複雜，但是不會用到幾何空間概念；而幾何法的困難程度會受到機構複雜度影響，通常會將代數法和幾何法一起使用；數值法則是使用數值方法迭代求解，優點是不用通過複雜的線性代數計算，缺點是求解過程需要花費較多的時間。由於 SCARA 機械手臂的結構容易分析，所以本研究使用幾何法求解。首先機械手臂末端之 z 軸座標只與第三軸馬達移動量相關，所以可視為二軸平面手臂，圖 2-4。在已知機械手臂末端座標 E(xe, ye, ze)情況下計算出 θ1 與 θ2。. 13.

(24) 圖 2-4 二軸平面手臂示意圖在計算逆向運動時會遇到兩個困難點：一是對於同一個機械手臂末端座標點求得的解可能是多組解如圖 2-5，通常會以加入限制條件的方式求得唯一解；第二個困難點是解的型式，我們需要特別留意分數型式的項次並且確保不會有分母為零的情況發生，解決方式則是盡量以桿長作為分母的函數或是對機械手臂的工作空間做限制避開奇異點。. 圖 2-5 逆向運動學多組解示意圖. 接下來我們根據圖 2-6 開始推導逆向運動學，為了得到唯一解我們對 θ2 加入大於零的限制條件。. 14.

(25) N O2. M. O1. 圖 2-6 逆向運動學求解. 首先根據餘弦定理從ΔO1O2E 我們可以求得(2-4). xe2  ye2  a12  a22 c2  A 2a1a2. (2-4). 再由三角函數關係得到(2-5)，並計算反三角函數(2-6)就能求得 θ2。 s 2   1  A2. (2-5).  2  atan2( 1  A2 , A). (2-6). 而我們希望藉由限制 θ2 得出唯一解，所以 θ1 必須是 θ2 的函數這樣對 θ2 的限制才有意義。θ1 可由∠EO1M 減去∠EO1N 求得(2-7). 1  EO 1M - EO 1 N  atan2( ye , xe )  atan2(EN , O1 N). (2-7).  atan2( ye , xe )  atan2(a2 s 2 , a1  a2 c2 ). 15.

(26) 2.2 機械手臂動力學 2.2.1 Lagrange 方程式運動學是以幾何參數描述物體間位置、速度和加速度的關係，與作用力無關。而動力學描述的是當物體受到作用力時所產生的行為，所以需要考慮到作用力大小以及物體本身慣量。在機器人領域中最見的是以 Lagrange 方程式來描述系統的動態方程式。Lagrange 方程式是一種基於能量守恆的觀點所推導出來的用以描述物體運動行為的方程式。藉由 Lagrange 方程式(2-8)推導機器手臂的動態方程式。. d L L  τ dt q q. (2-8). L  K U 其中 L 是拉格朗日函數(Lagrangian)定義為動能減掉位能。q 為廣義座標中位置向量，以本研究而言 q 為三個關節的旋轉及移動量 q  [1  2 q  [1 2. d3 ]T 。. d3 ]T 為廣義座標中速度向量，τ 為系統所受外力。由於位能與. 速度無關，所以可以將(2-8)表示成(2-9). d K K U   τ dt q q q. (2-9). 根據(2-9)，我們需要計算整個系統的動能 K 和位能 U，而 K 可以表示成系統中每一個連桿的線動能與轉動動能的和，如(2-10)。. 1 n K   (mi vTC v C  ωTi I C ωi ) 2 i1 i. i. (2-10). i. 其中，mi、 v C 、 I C 、i 分別表示第 i 個連桿的質量、質心的線速度向量、 i. i. 轉動慣量矩陣、角速度向量。而我們可以透過 Jacobian 矩陣 J v 和 J  將(2-10) i. i. 中的第 i 個連桿的質心速度向量 v C 和角速度向量i 以廣義速度座標表示， i. 16.

(27) 而同時 K 也能表示成. 的二次型式. n 1 1 K  q T  (mi J Tv J v  J T I C J  )q  q T Mq 2 i 1 2 i. i. i. i. i. (2-11). n. M   (mi J Tv J v  J T I C J  ) i. i 1. i. i. i. (2-12). i. 其中 v C  J v q ， ωi  J  q 。於是我們可以看出 M 為包含質量與轉動慣量的 i. i. i. 正定對稱矩陣。將(2-11)代入(2-9)可得.  (q)q  K  U  τ   M M(q)q q q  q  K ] 針對第 k 行而言其中的 [M q.  q  [M. (2-13). M K 1 M ij ]k   ( kj  )qi q j q qi 2 qk i, j. (2-14). i,j 表示 M 矩陣的第 i 行第 j 列。由於 M 是對稱矩陣，我們可以進一步將(2-14)改寫為(2-15)  q  [M. M kj M ki M ij K 1 ]k   (   )qi q j q 2 i , j qi q j qk. (2-15). 1 M kj M ki M ij 並以 Cijk 表示 (   ) 2 qi q j qk. C(q, q ) 中第(k, j)個元素為 n. Ckj   Cijk (q)qi. (2-16). i 1. 而 G (q) 第 k 行表示為. Gk . U   qk qk. m g i. T0. PC. (2-17). i. i. 其中 gT 是[0, 0, g]T，g 為重力常數。 0 PC 是每一個連桿質心的位置座標並以 i. 座標系 0 表示。將上述(2-12)、(2-15)、(2-16)、(2-17)代入(2-13)中就得到了 17.

(28) 系統的動態方程式。. M(q)q  C(q, q )q  G(q)  τ. (2-18). 接下來我們開始推導本研究所使用的 SCARA 機械手臂的動態方程式，首先進行參數定義 mi 為第 i 個連桿的質量， aC 為第 1、2 連桿的質心位置， i. Izzi 為第 i 軸繞 z 軸轉動的轉動慣量。如圖 2-7。 z0. z1. θ1. y1. θ2. x1,2 a1. aC. a2 aC. 1. d1. y2. 2. m2 g. m1 g. m3 g. y0 y3. x0. d3 Δd3 x3 z2,3. 圖 2-7 SCARA 機戒手臂模型參數定義. 首先我們計算出每個連桿的質心位置座標 0 PC (2-19) i. 0. PC  [aC c1 1. 0. 1. 1. d1 ]T. PC  [a1c1  aC c12 a1s1  aC s12 d1 ]T 2. 0. aC s1 2. PC  [a1c1  a2 c12 3. 2. a1 s1  a2 s12. d1  d 3  d 3 ]T. 接著將 0 PC 對 q 偏微分求得 J v (2-20) i. i. 18. (2-19).

(29)  aC s1 Jv    aC c1 q  0  0 PC. 1. 1. 1. Jv. Jv. 1.  a1 s1  aC s12    a1c1  aC c12 q  0  0 PC. 2. 2. 2. 2.  a1 s1  a2 s12    a1c1  a2 c12 q  0  0 PC. 3. 3. 0 0 0 0 0 0.  aC s12. 0 0 0. 2. aC c12 2. 0  a2 s12. 0 0   1. a2 c12 0. (2-20). 而 J  我們可以很容易從機械手臂的結構上了解。關節 1 的轉動慣量 Izz1 只影 i. 響關節 1 的 z 軸方向的轉動，所以 J  第三行第一列為 1 其他元素為 0。關節 1. 2 的轉動慣量 Izz2 影響關節 1 和關節 2 的 z 軸方向轉動，所以 J  第三行的第 2. 一列的第二列為 1 其他元素為 0。關節 3 本身不會轉動，所以 J  中所有元素 3. 皆為 0。 0 0 0  J   0 0 0 1 0 0 1. J. J. 2. 3. 0 0 0   0 0 0 1 1 0 0 0 0   0 0 0 0 0 0. (2-21). 將(2-20)、(2-21)帶入(2-12)我們可以得到系統的 M 矩陣，M 矩陣中的元素如下列方程式(2-22). 19.

(30) M 1,1  aC2 m1  (a12  aC2  2a1aC c2 )m2  (a12  a22  2a1a2 c2 )m3  I zz1  I zz 2 1. 2. 2. M 1, 2  M 2,1  (aC2  a1aC c2 )m2  (a22  a1a2 c2 )m3  I zz 2 2. 2. M 1,3  M 3,1  0. M 2, 2  aC2 m2  a22 m3  I zz 2 2. M 2 , 3  M 3, 2  0 M 3,3  m3. (2-22). 求得 M 矩陣後，再根據(2-14)-(2-16)式的推導，我們可以得到 C 矩陣的元素如下 C1,1  a1s2 (aC m2  a2 m3 )q 2 2. C1, 2  a1s2 (aC m2  a2 m3 )( q1  q 2 ) 2. C2,1  a1s2 (aC m2  a2 m3 )q1 2. C1,3  C2, 2  C2,3  C3,1  C3, 2  C3,3  0. (2-23). 而根據(2-17)、(2-19)式，可以得到 G 向量中的元素，如下. G1  G2  0. G3  m3 g. (2-24). 至此，我們完成了 SCARA 機械手臂動態方程式的推導。. 20.

(31) 第三章控制器理論 3.1 Lyapunov 穩定度理論在非線性控制中，我們很難用古典控制的分析方式來判斷一個系統的穩定與否，這時 Lyapunov 穩定度的分析方式是一個不錯的選擇。以一個非線性系統為例. x  f (t , x). (3-1). 而我們希望此系統的狀態可以在平衡點附近保持其穩定性，因此根據其狀態選擇一正定的 Lyapunov 函數(3-2)，且只有在平衡點時 V (t, x) = 0。. V (t , x)  0. (3-2). 接著將(3-2)對時間作一次微分得到. (t, x)，再對. 如果. (t, x) ≤ 0 則系統狀態為有界，但不保證. (t, x) > 0 則系統發散；如果. 會漸進穩定；如果. (t, x)可能的情況進行探討。. (t, x) < 0 則確保系統狀態收斂至平衡點。. 我們在進行系統的穩定度分析時可能只能確保. (t, x) ≤ 0，很難證明到. (t, x) < 0。在這種情況下要判斷系統究竟是不是漸進穩定就需要 Barbalat 引理來做更進一步的分析。 Barbalat 引理：如果有一函數 g(t)，其積分在 t→∞時是有界的(3-3). lim 0 g ( )d   t. (3-3). t . 且 g(t)為單調遞減函數，則當 t→∞時 g(t) →0(3-4)。 lim g (t )  0. (3-4). t . 若是不易直接證明 g (t)為非遞增函數，可從 g (t ) 存在且有界來得到 g(t)為單調遞減函數的結果。但是到目前為止並沒有一個通用的方式來協助我們找到一個一定符合穩定條件的 Lyapunov 函數 V(t, x)，而找不到適當的 Lyapunov 函數也不代表 21.

(32) 系統就是不穩定的。. 3.2 滑動模式控制滑動模式控制(sliding mode control)的主要想法是將系統狀態拉至滑動面(sliding surface)上，而在滑動面上的狀態會自動收斂至平衡點，如圖 3-1 所示。滑動模式控制的最大優點就是擁有良好的強健性，可以抑制干擾對系統的影響；而缺點則是當系統狀態收斂至滑動面上後，滑動模式的控制量會有明顯的跳切現象(chattering)。 x(0). x(∞). 圖 3-1 系統狀態隨時間收斂至滑動面再到平衡點. 以一個二階線性系統為例，其動態方程式如下：. x  ax  bu. (3-5). 為了使系統狀態收斂至平衡點，也就是 x 與最後會收斂到 0，因此定義滑動函數(sliding function)為： s  x  x. (3-6). 其中為正實數。當 s = 0 時我們可以解出 x = e-t，所以當 t→∞時，x→0，並且收斂速度受到影響。我們可以根據系統模型(3-5)及滑動函數(3-6)設計出控制律， u  ueq  usw. (3-7). 其中， 22.

(33) ueq  b1 (ax  x ) usw  b1k sgn(s). for s  0 1,  sgn(s)  0, for s  0  1, for s  0  其中 k 為正實數。而控制律中會包含兩部分，一是等效控制量 ueq 用來消除模型的影響，另一個是切換項 usw 負責將系統狀態拉至滑動面上。usw 中有著符號函數(sign function) sgn() 會造成 s 在 0－、0＋附近變化時，控制量會有±k 大小的變化也就是跳切現象，圖 3-2。目前也有相當多的文獻在探討如何消除滑動模式中的跳切現象，而最常見的作法則是以飽和函數 (saturation function) sat() 替換符號函數。. (x(0), (0)) (x(∞), (∞)) x. +x=0 圖 3-2 滑動面上的跳切現象示意圖. 接下來使用 Lyapunov 穩定度分析，首先選擇 Lyapunov 函數為(3-8)式，再將(3-8)式對時間作一次微分得到(3-9). V. 1 2 s 2. (3-8) 23.

(34) V  ss  s ( x  x )  s (ax  bu  x ). (3-9).  k s 0 而滑動模式的優點為良好的強健性，如果系統中存在著不確定量(uncertainty) 或干擾(disturbance)可以通過調整 k 值大小，使保持小於等於零，系統即可保持穩定。缺點是當 k 值變大時跳切現象會變劇烈。. 3.3 指數律在先前的 3.2 節中我們提到了滑動模式控制的基本設計方式，在過去也有相當多文獻對滑動模式控制加以改良，而本節要引入指數律(exponential law)於滑動模式控制中[9][15]，並說明加入指數律後如何改善收斂速度及減小跳切現象。從(3-9)中我們可以看出當滿足(3-10)系統就會收斂至滑動面上. s  k sgn(s). (3-10). 將(3-10)積分可以得到滑動函數從初始狀態收斂到滑動面上的上升時間(rise time) tr。由(3-11)可以看出滑動函數的收斂速度除了跟初始狀態相關外，還跟增益值 k 有關。. . sgn(s)ds  k 0 dt s (0) 0. tr . tr. s(0) k. (3-11). 將指數律(3-12)加入到(3-10)可以得到(3-13) E (s)    (1   )e  s ， 0    1 ，   R  ， p  N  p. 24. (3-12).

(35) s  . k sgn(s) E (s). (3-13). 指數律 E(s)為 s 的函數，當 s 從∞收斂到 0 時 E(s)會從 δ 收斂到 1，的大小也會從 k/以指數函數收斂至 k，也就是在一開始 s 大時會有較大使得暫態響應速度加快；並且在 s 收斂至滑動面時減小增益使得跳切現象不會過於激烈，同時又可以符合原先控制器的設計參數讓系統保持穩定。同樣分析滑動函數從初始狀態收斂到滑動面上的上升時間(rise time) tr. .   (1   )e  s ds  k 0 dt s ( 0) 0. tr. p.   s(0)  s ( 0) (1   )e s ds  ktr 0.  tr .  s(0). . p. (1   ) 0  s e ds k s ( 0 ) p. (3-14) k  s(0) s(0) (1   ) 0  s 由於所以，由此可以看出滑動模式控 t   e ds  0 r k k k s (0) p. 制加入指數律後的上升時間比沒有加入指數律的還小，因此可以確認加入指數律確實可以加快滑動模式控制的收斂速度。. 3.4 適應性控制適應控制(adaptive control)是屬於非線性控制的一種。最早在 1950 年代被提出，其概念是控制器能根據外界環境變化，再經由回授調整控制器特性的一種適應環境的控制器。適應控制至今已經發展得相當成熟，也有相當多的文獻提出新的適應控制架構，而在適應控制中可分為兩大類：一是自我調變控制器(self-tuning controller)，圖 3-3；另一種是參考模型控制器(model reference control)，圖 3-4。. 25.

(36) r. ＋. e. y. u. 控制器. 系統. －. 估測器. 圖 3-3 自我調變控制器示意圖 ym. 參考模型. ＋適應律＋ r. －. e 控制器. u. －系統 y. 圖 3-4 參考模型控制器示意圖. 若是控制器中會使用到系統中的未知參數時，我們需要設計一個估測器來估測系統中的未知參數，並把估測結果供給控制器使用，這樣的控制架構稱為自我調變控制器。自我調變控制器可以容易得結合不同的控制器與估測器，使用上較為彈性。而缺點則是無法保證估測到的參數的正確性，以及在適應的過程中通常系統的暫態響應也會較差。而參考模型控制器運作方式則是先設計一個參考的閉迴路模型，其輸出表示受控系統期望的響應，與真實響應比較後得到誤差再藉由適應律調整控制器使得受控系統輸出動態符合期望。. 26.

(37) 第四章控制器設計 4.1 滑模控制結合指數律考慮一個包含不確定量和外部干擾的非線性機械手臂動態方程式，如下所示：. (M  M)(q)q(t )  (C  C)(q, q )q (t )  (G  G)(q)  u(t )  d(t ). (4-1). q、、 ∈ ℜn 是馬達角度、角速度和角加速度的狀態向量。u(t) ∈ ℜn 是控制量，d(t) ∈ ℜn 是外界干擾。M(q) ∈ ℜn×n 是對稱正定的質量和轉動慣量矩陣， C(. ) ∈ ℜn 是向心力、柯氏力向量，G(q) ∈ ℜn 是重力造成的力矩向量，. ΔM(q)、ΔC(. )、ΔG(q)是模型的不確定項。其中 M(q)-2C(. )為斜對稱. 矩陣(skew-symmetric)，並且對任一向量 x(4-2)皆成立。. xT M(q)  2C(q, q )x  0. (4-2). 接著重新整理(4-1)式將不確定項合併為一個向量變數方便後續判斷邊界範圍：. Mq  Cq  G  u  . (4-3).   Cq  G]   d  [Mq 其中 Δ 包含所有系統的不確定量。又因為是 t、q、的函數(4-4). (t , q, q )  (M  M) 1 (q)[u(t )  d(t )  (C  C)(q, q )q  (G  G)(q)] q. (4-4). 所以可將 Δ 視為 Δ(t,q, )，並假設 Δ 是有界的(bounded)並且存在著正實數 γ0、 γ1、γ2滿足(4-5)關係[10].  i (t , qi ,q i )   0,i   1,i qi (t )   2,i qi (t )  i. (4-5). 其中 i 表示機械手臂中的第 i 軸，並以 Γi 表示 Δi 的邊界大小。之所以刻意將 Δ 不以表示是因為藉由馬達回授可以直接得到的訊號只有 q，如果要得到必須將訊號進行兩次微分，而兩次微分後的訊號會過於雜亂，因此在 Δ 的 27.

(38) 假設中不將考慮在內。後續會說明如何用適應控制估測 γ0, i、γ1, i、γ2, i 參數值。接下來設計滑動模式控制器，一開始先定義切換函數 s (switching function)與誤差 e 為 s  e  Λe. (4-6). e  q  qd. (4-7). 其中 qd 為馬達角度命令，Λ ∈ ℜn×n 為正定的對角矩陣。再將系統動態方程式從(4-3)式改以 s 表示：.  d  e )  C(q d  e) Ms  Cs  G  u    M(q. (4-8). 一般而言，滑動模式控制的控制律會有兩個部分，一部分是等效控制量 ueq 用來消除模型的影響；另一部份切換項 usw 是根據切換函數切換控制量，使切換函數保持在滑動面，通常如下所示： u  ueq  u sw. (4-9). 根據(4-8)式可得，  d  e )  C(q  d  e)  G u eq  M(q. (4-10). u sw  K sgn(s). (4-11). K ∈ ℜn×n 為正定的對角矩陣，此參數如果過大會造成跳切現象相當劇烈；過小則會使系統不穩定。因此本研究在滑模控制中加入指數律[9]，可以有效減小跳切現象，同時也能加快暫態響應。將(4-11)式加入指數律後可得：. u sw  KE 1 sgn(s). (4-12). E  diag( E1 ,..., En ) ， Ei   i  (1   i )e . i. si. pi. (4-13). Ei 即為指數律方程式。其中，0 < δi < 1，αi、pi > 0。加入指數律後系統隨著 28.

(39) 切換函數 si 收斂至滑動面上的同時，usw,i 的大小也會從 Ki /δi 以指數函數收斂至 Ki；換句話說，在一開始誤差大時控制器會有較大的增益，使得暫態響應速度加快；並且在系統收斂至滑動面時減小增益使得跳切現象不會過於激烈，同時又可以符合原先控制器的設計參數讓系統保持穩定。關於指數律參數之選擇，首先我們判斷系統滑動函數的範圍，接著繪出 si－usw,i 特性曲線圖。從圖中可以看出 usw,i 有著上下邊界分別為 Ki/δi 和 Ki；調整 αi 可以決定 U 型曲線的寬度，調整 pi 可以使得 U 型曲線的底部尖銳或平坦，如圖 4-1、圖 4-2。而本篇論文對於指數律的參數選擇過程則是先設計 δi 決定 usw,i 的最大值。接著判斷出系統在沒有指數律時 si 的範圍，再調整 αi 和 pi 使得 U 型曲線的範圍有效的包含著 si 的範圍，這樣才能確保指數律有完整的發揮效用；同時使 U 型曲線在 si 接近 0 時是平滑的，這是為了避免當 si 已經收斂到接近 0 時控制量 usw,i 卻有很大跳動。 |usw,i| Ki/i. Ki. si 圖 4-1 p=1 時，不同 α 的 si－usw,i 特性曲線圖. 29.

(40) |usw,i| Ki/i. Ki. si 圖 4-2 α=1 時，不同 p 的 si－usw,i 特性曲線圖. 4.2 可變步長適應滑模控制結合指數律在適應律中的步長(step-size)通常會設為常數，步長的大小會影響估測參數的收斂速度。而在文獻中也有將此步長設為變數的研究[12][13][14]。在這些研究中會將此步長設計為誤差的函數，並且有著當誤差大時步長隨之變大以便加快收斂速度；以及誤差小時步長隨之變小，以保持內部穩定。因為雖然誤差收斂至一個很小的值，可時同時又乘上了很大的步長並且積分。這會導致即使誤差已經收斂，但是適應控制的估測值會依然隨時間增加造成內部不穩定的現象。所以我們會希望此步長有著隨誤差大而變大，以及隨誤差小而變小的特性。我們發現 ERL 也有相同的特性，於是在本研究中將 ERL 與可變步長適應控制作結合，設計方式如下所述。由於在(4-5)式中已經討論過 Δ 的邊界範圍，因此根據(4-5)、(4-12)式加入適應控制估測項為： 30.

(41) u sw  (K  ˆ )E 1 sgn(s). (4-14). 其中，. ˆ  diag(ˆ1 ,..., ˆ n ) ˆ i  ˆ0,i  ˆ1,i qi  ˆ2,i qi. (4-15). ˆ0,i 、 ˆ1,i 、 ˆ2,i 為估測參數。根據(4-14)、(4-15)式設計適應律如下： 1 γˆ0,i  s Ei i. (4-16). 1 γˆ1,i  q s Ei i i. (4-17). 1 γˆ2,i  q s Ei i i. (4-18). 依照這樣的設計方式，即使原先設計的 K 過小也能藉由 ˆ 彌補，不須再重複的測試調整參數。適應律中的 μ 則是為了更進一步的調整估測器的收斂行為。由於估測參數時會使用到系統的輸出誤差，所以如果控制器收斂得比估測器快，適應律就無法繼續進行更新，這時可以藉由調整 μ 使得適應律收斂速度比控制器快。為了能夠將適應律(4-16)-(4-18)對應到可變步長適應控制，我們在實作中將適應律以離散方式表示為 γˆ0,i (k  1)  γˆ0,i (k ) . t s (k ) Ei (k ) i. (4-19). γˆ1,i (k  1)  γˆ1,i (k ) . t q (k ) si (k ) Ei (k ) i. (4-20). 31.

(42) γˆ2,i (k  1)  γˆ2,i (k ) . t q (k ) si (k ) Ei (k ) i. (4-21). 其中 β 是常數，本篇論文設定 β 為 0.99，Δt 為取樣時間。. 4.3 Lyapunov 穩定度分析為了確保系統的穩定，切換函數 s 和所有估測參數的誤差必須會收斂。首先定義參數誤差 ~j ,i  ˆ j ,i   j ,i ，而本研究採用三軸機械手臂，所以索引 i = 1, 2, 3 表示機械手臂軸數，而索引 j = 0, 1, 2 表示三個 γ 參數。選擇 Lyapunov 函數如下：. 1 1 2 3 V  s T Ms   ~j2,i 2 2 j  0 i 1. (4-22). 將(4-22)式對時間作一次微分得到 V 2. 3. 1  V  sT M s  sT Ms   ~j ,i ~ γ j ,i 2 j 0 i 1. (4-23). 其中將 Ms (4-8)以及控制律(4-10)(4-14)代入，並且整理成純量型式(4-24) 2. 3. 1   d  e )  C(q d  e)]   ~j ,i ~ V  sT M s  sT [Cs  G  u    M(q γ j ,i 2 j 0 i 1 3.  {si [ i 1. 2 ki ˆ sgn(si )  i sgn(si )   i ]   ~j ,i ~ γ j ,i } Ei Ei j 0. (4-24). ~ 其中 si s gn(si )  si ， si i  si i  si i 。為了整理出 i 我們在式子中加入 ~ i i ˆ i i i   ，  和可以合併成， i 則是保留。就能證明 V  0 (4-25)。 Ei Ei Ei Ei Ei Ei. 32.

(43) 3 2 k ˆ V   {( i  i  i ) si   ~j ,i ~ γ j ,i } E E i 1 j 0 i i ~ 3 2 ki i i   [(   i ) si  ( si   ~j ,i ~ γ j ,i )] E E E i 1 j 0 i i i. (4-25). 接下來代入適應律(4-16)~(4-18)，其中 ~ 2 i   ( si   ~j ,i ~ γ j ,i ) E i 1 j 0 i ~ ~ 3  0,i   1,i qi  ~2,i qi   [ si   (~0,i ~ γ0,i  ~1,i ~ γ1,i  ~2,i ~ γ2,i )] E i 1 i 3. (4-26). 0. 又因為 0 < i ≤ Ei ≤ 1，所以 Γi / Ei - Γi ≥ 0 3 k  V   ( i  i  i ) si  0 Ei i 1 Ei. (4-27). 由於 V 是負半定因此還需要更進一步的以 Barbalat 引理來驗證狀態是否在 t   時會收斂至 0。我們設計了一 Lyapunov 函數 V (t )  0 並推導出 V (t )  0 ，由這兩個條件可以知道 V(t)是有界的， V (t )  , t.  0 ；以及 V(t). 為單調遞減函數， V (t1 )  V (t2 ), if t1  t2 。 t 首先證明 lim 0 V ( ) d   t . t t lim 0 V ( ) d  lim 0 V ( )d  V (0)  V () t . t . (4-28). 因為 V (t )  0，V (0)  V ()，所以 V (0)  V ()  V (0)。又因為 V(0)是有界的，所以 t lim 0 V ( ) d  V (0)  . (4-29). t . 接下來我們通過證明 V(t ) 也是有界來確認 V (t ) 為均勻連續函數， V (t ) 中 si 為 33.

(44) 時間函數， Ei 又為 si 的函數。 3 3  si (t ) si (t )  V (t )   k i  i  i Ei ( si )   (k i  i )  i si (t )  Ei ( si ) Ei ( si ) i 1 i 1  . (4-30). E   sgn(s i ) Ei  s i 3   V ds s   sgn(s )s V    (k i  i ) i i  i s dt Ei2 i 1    . (4-31). . E    i pi si s. pi 1. . sgn(si ) (1   i )e.  i si. pi. 其中 ki 、 i 、  i 、 pi 、  i 都是常數， Ei 為指數律是有界函數， si 、 si 是系統狀態 q 、 q 與參考命令 q d 、 q d 的線性組合也是有界，故 V(t ) 有界且 V (t ) 為均勻連續函數，從 V (t ) 的型式及(4-29)式我們可以得知 V (t )  L1  L 同時 V(t ) 又是有界 V(t )  L ，所以 lim V (t )  0 (4-32)。而(4-32)式要成立唯有所有的 si t . 皆收斂至 0(4-33)。 s (t )  3 lim V (t )   lim  ki  i  i Ei ( si ) i 0 t  t  Ei ( si )   i 1. (4-32). lim si (t )  0, i  1,2,3. (4-33). t . 通過 Barbalat 引理我們證明了當時間區近無窮大時系統的狀態會收斂至滑動面上，因此整個閉迴路系統是穩定的。. 34.

(45) 第五章實驗結果與討論 5.1 實驗設備 5.1.1 SCARA 機械手臂本研究所使用的機械手臂為台灣仿真科技股份有限公司所製造的 SCARA 型四軸機械手臂圖 5-1。第一軸馬達編碼器 1 圈 5000 刻度，最大轉速為 3000 rpm，搭配減速比為 1:60 的減速機。並在左右各加裝一極限開關防止碰撞，左右極限間馬達可轉動的角度為 242.1042 deg。第二軸馬達編碼器 1 圈 5000 刻度，最大轉速為 3000 rpm，搭配減速比為 1:30 的減速機。同樣在左右加裝極限開關，左右極限間馬達可轉動的角度為 275.5116 deg。第三軸馬達編碼器 1 圈 2000 刻度，最大轉速為 300 rpm。搭配螺距(pitch)為 4 mm 的導螺桿，上下極限間的長度為 106.2360 mm。第四軸夾爪端則是以步進馬達搭配減速機再帶動齒輪組達到抓取功能。相關參數見圖 5-2、表 5-1。. 圖 5-1 SCARA 型四軸機械手臂 35.

(46) Izz1. Izz2 a1. aC. a2 aC. 1. d1. 2. d3 m2. m1. m3. 圖 5-2 系統參數標示圖. 表 5-1 系統參數表質量 (m). 桿長 (a). 軸心離質心距離(ac). 轉動慣量 (Izz). 連桿一. 10 kg. 0.198 m. 0.0990 m. 0.026 kgm2. 連桿二. 6 kg. 0.185 m. 0.0925 m. 0.011 kgm2. 連桿三. 2 kg. 0.264 m. 0m. 0 kgm2. 5.1.2 驅動器驅動器是由 SANYO DENKI 公司生產，型號為 PV1A015EM51P00。驅動器內部設定為四倍頻，由此可計算出三軸的解析度。第一軸解析度為 1pulse. 360 deg 1 1  (齒輪比)  (四倍頻)  3  10 -4 deg 5000 pulse 60 4. (5-1). 第二軸解析度為 1pulse. 360 deg 1 1  (齒輪比)  (四倍頻)  6 10 -4 deg 5000 pulse 30 4. 第三軸解析度為 36. (5-2).

(47) 1pulse. 360 deg 4 mm 1  (螺距)  (四倍頻)  5  10 -4 mm 2000 pulse 360 deg 4. (5-3). 外部設定. AC 電源回生電阻. 編碼器訊號. 馬達電源. 控制訊號. 圖 5-3 驅動器功能接線圖. 5.1.3 資料擷取卡本研究使用的資料擷取卡為 Advantech 研華科技公司生產的運動控制軸卡 PCI-1240U 與資料擷取卡 PCI-1720U 兩張，圖 5-4、圖 5-5。 PCI-1720U 的規格如下，4 通道獨立輸出，解析度為 12 位元，slew rata 為 2V/μs，輸出範圍單極性 0~5V、0~10V，雙極性± 5V、±10V。. 圖 5-4 研華科技公司 PCI-1720U 資料擷取卡. 37.

(48) PCI-1240U 的規格如下，擁有 4 通道，輸出最大可達 4Mpps，提供 2/3 軸線性補差功能，2 軸圓弧補差功能，訊號輸出類型為 CW/CCW 或脈波加方向兩種。數位 I/O 方面最大輸入頻率為 4kHz，輸入電壓低電位為 3V、高電位為 10~50V；輸出電壓為 5~40V。編碼器訊號輸入類型為 A/B 相或加/減兩種，最大輸入頻率為 1MHz，輸入電壓低電位為 2V、高電位為 5~30V。. 圖 5-5 研華科技公司 PCI-1240U 運動控制軸卡. 5.1.4 軟體本研究所使用的軟體是由 MathWorks 所開發的 MATLAB 及其中的 Simulink。MATLAB 是 MATrix LABoratory 的縮寫，其程式語法是屬於高階程式語言，程式語法與 C 接近但是在矩陣和向量的運算處理上比 C 來得方便許多。另外 MATLAB 也有相當多種類的工具箱(toolbox)供不同領域的研究者使用。Simulink 是圖形化的程式語言，使用者是使用許多具有不同功能之方塊組合撰寫出想要的程式功能，與一般使用程式碼撰寫不同，撰寫程式上也較為直觀。而 Simulink 中也提供程式碼撰寫功能可與圖形化方塊一起使用，增加方便性。 38.

(49) 5.2 實驗結果在此小節中，我們將對第四章中所設計的控制器實際應用在機械手臂上，確認其效能。在實驗中使用到的可變步長適應滑動模式控制器參數如下：Λ = diag(18, 18)，K = diag(12, 12)，μ = 0.5。指數律參數如下： δ = [0.1, 0.1]T，α = [10, 10]T， and p = [1.5, 1.5]T，並且限制控制量的最大值為 6、最小值為-6。取樣時間為 1/12000 秒。. 5.2.1 雙軸分別定位控制首先對第一軸與第二軸分別作定位控制，控制命令為大小 15 deg 的步階輸入並通過. 5 的低通濾波器使上升坡度不會過大。在定位控制實驗中，可 s5. 以看出系統暫態響應時間不到 1 秒，最大超越量分別為 5.112%和 3.754%。而我們將 2 秒後的系統響應結果放大檢視圖 5-8、圖 5-11、圖 5-13，可以看出第一、二軸的馬達的定位誤差皆以收斂至最小解析度，其中因為第一軸的馬達慣量較大且乘載著較大重量，所以誤差收斂速度較第二軸來得稍慢一點。表 5-2 為第一、二軸的最大誤差及穩態時的方均根誤差。. 圖 5-6 第一軸馬達定位響應情況 39.

(50) 圖 5-7 第一軸馬達定位誤差. 圖 5-8. 2 秒後第一軸馬達定位誤差. 圖 5-9 第二軸馬達定位響應情況. 40.

(51) 圖 5-10 第二軸馬達定位誤差. 圖 5-11. 2 秒後第二軸馬達定位誤差. 圖 5-12 第一、二軸馬達控制量. 41.

(52) 圖 5-13. 2 秒後第一、二軸馬達控制量. 表 5-2 機械手臂第一、二軸定位實驗誤差單位最大誤差 (deg) (max error) -0.7668 第一軸 -0.5631 第二軸. 穩態方均根誤差 (root mean square error) 6.4297×10-4 5.5069×10-4. 5.2.2 雙軸分別連續定位控制在此實驗中我們測試在進行多次定位後是否依然有著良好的精度。每次定位的行程為 15 deg 時間為 2 秒，第一、二軸響應情況如圖 5-14、圖 5-19 所示。誤差如圖 5-15、圖 5-20，我們檢視其中的最後一次定位結果，如圖 5-16、圖 5-17、圖 5-21、圖 5-22，可以看出在定位的誤差上比原先 5.2.1 節的實驗大了一些，但是精度還是可以維持在 10-3 deg 的等級。圖 5-18、圖 5-22 則是第一、二軸的控制量。. 42.

(53) position (deg). 80 60 40 20 0 0. q. d1. q1 2. 4. time (sec). 6. 8. 10. 圖 5-14 第一軸馬達連續定位響應情況. 5. error (deg). 0 -5 -10 -15 0. 2. 4. time (sec). 6. 8. 10. 8.8. 9. 圖 5-15 第一軸馬達連續定位誤差. error (deg). 0.5. 0. -0.5. -1 8. 8.2. 8.4 8.6 time (sec). 圖 5-16 第一軸馬達連續定位暫態誤差 43.

(54) -3. -2.6 x 10. error (deg). -2.8 -3 -3.2 -3.4 9. 9.2. 9.4 9.6 time (sec). 9.8. 10. 圖 5-17 第一軸馬達連續定位穩態誤差. 6. control input. 4 2 0 -2 -4 -6 0. 2. 4. time (sec). 6. 8. 10. 圖 5-18 第一軸馬達控制量. position (deg). 80 60 40 20. q. d2. q 0 0. 2. 2. 4. time (sec). 6. 圖 5-19 第二軸馬達連續定位響應 44. 8. 10.

(55) 5. error (deg). 0 -5 -10 -15 0. 2. 4. time (sec). 6. 8. 10. 8.8. 9. 圖 5-20 第二軸馬達連續定位誤差. 1. error (deg). 0.5 0 -0.5 -1 -1.5 8. 8.2. 8.4 8.6 time (sec). 圖 5-21 第二軸馬達連續定位暫態誤差 -3. error (deg). -2 x 10. -3. -4. -5 9. 9.2. 9.4 9.6 time (sec). 9.8. 圖 5-22 第二軸馬達連續定位穩態誤差 45. 10.

(56) 6. control input. 4 2 0 -2 -4 -6 0. 2. 4. time (sec). 6. 8. 10. 圖 5-23 第二軸馬達控制量. 5.2.3 雙軸同動追跡實驗完成定位實驗後，接下來我們確認加入和未加入指數律的控制器對追跡控制的性能如何，在以下比較中其他控制參數皆相同，唯一差別在於加入指數律與否。在此實驗中第一軸和第二軸馬達參考命令(reference command)為 15sin(0.4πt)。在追跡實驗中由於參考命令不斷變化，又加上一、二軸之間彼此耦合，因此在同時作動時控制上難度是大於定位實驗許多的。圖 5-24、圖 5-27 為實驗結果，誤差結果顯示於圖 5-25、圖 5-28，詳細誤差比較則顯示於表 5-3。而圖 5-26、圖 5-29 則是第一、二軸馬達的控制量。在實驗中可以看出加入指數律後控制器確實能夠減少跳動，以及精度也較未加入指數律的控制器高。. 46.

(57) 15. q. d1. 10. q with EL. position (deg). 1. 5. q without EL 1. 0 -5 -10 -15 0. 2. 4. time (sec). 6. 8. 10. 圖 5-24 第一軸馬達追跡響應情況. 0.4. error (deg). 0.2 0 -0.2 -0.4 0. e with EL 1. e1 without EL 2. 4. time (sec). 6. 8. 10. 圖 5-25 第一軸馬達追跡誤差. 6. u1 with EL. control input. 4. u1 without EL. 2 0 -2 -4 -6 0. 2. 4 圖 5-26. time (sec). 6. 第一軸馬達控制量 47. 8. 10.

(58) 15. qd2. position (deg). 10. q2 with EL. 5. q without EL 2. 0 -5 -10 -15 0. 2. 4. time (sec). 6. 8. 10. 圖 5-27 第二軸馬達追跡響應情況. 0.3. error (deg). 0.2 0.1 0 e with EL. -0.1 -0.2 0. 2. e2 without EL 2. 4. time (sec). 6. 8. 10. 圖 5-28 第二軸馬達追跡誤差. 6. u2 with EL. control input. 4. u2 without EL. 2 0 -2 -4 -6 0. 2. 4. time (sec). 6. 圖 5-29 第二軸馬達控制量 48. 8. 10.

(59) 表 5-3 機械手臂第一、二軸追跡實驗誤差，單位：deg 第一軸加入指數律未加入指數律最大方均根最大方均根誤差誤差誤差誤差 0.2481 0.1763 0.3665 0.2345. 第二軸加入指數律未加入指數律最大方均根最大方均根誤差誤差誤差誤差 0.1604 0.1043 0.1976 0.1199. 5.2.4 強健性實驗在此實驗中我們承續 5.2.3 的實驗並確認控制器的強健性如何，在實驗中加入的干擾如下：d1 = d2 = 2sin(0.8πt)sin(0.2πt)。實驗結果顯示於圖 5-30、圖 5-33，圖 5-31、圖 5-34 為誤差結果。而圖 5-32、圖 5-33 則是第一、二軸馬達的控制量。從圖 5-25、圖 5-31 可以看出加入誤差後第一軸馬達的誤差範圍差異不大；而在第二軸的表現上卻被干擾影響得較為明顯，原因是第一軸的控制量範圍大約是±4 左右而加入的干擾大小是±2 影響程度約 50%，而第二軸控制量範圍在±2 左右，影量程度約為 100%，因此加入干擾後第二軸的變化程度比第一軸大得多。. 15. qd1. position (deg). 10. q1 with EL. 5. q without EL 1. 0 -5 -10 -15 0. 2. 4. time (sec). 6. 圖 5-30 第一軸馬達追跡響應情況. 49. 8. 10.

(60) 0.6. e1 with EL. error (deg). 0.4. e1 without EL. 0.2 0 -0.2 -0.4 0. 2. 4. time (sec). 6. 8. 10. 圖 5-31 第一軸馬達追跡誤差. 6. u1 with EL. control input. 4. u1 without EL. 2 0 -2 -4 -6 0. 2. 4 圖 5-32. time (sec). 6. 10. 第一軸馬達控制量. 15. qd2. 10. position (deg). 8. q2 with EL. 5. q without EL 2. 0 -5 -10 -15 0. 2. 4. time (sec). 6. 圖 5-33 第二軸馬達追跡響應情況 50. 8. 10.

(61) 0.4. error (deg). 0.2 0 -0.2 -0.4 0. e with EL 2. e2 without EL 2. 4. time (sec). 6. 8. 10. 圖 5-34 第二軸馬達追跡誤差. 6. u2 with EL. control input. 4. u2 without EL. 2 0 -2 -4 -6 0. 2. 4 圖 5-35. time (sec). 6. 8. 10. 第二軸馬達控制量. 表 5-4 強健性實驗誤差，單位：deg 第一軸加入指數律未加入指數律最大方均根最大方均根誤差誤差誤差誤差 0.3024 0.018 0.4745 0.2518. 第二軸加入指數律未加入指數律最大方均根最大方均根誤差誤差誤差誤差 0.2432 0.1187 0.3276 0.1471. 51.

(62) 5.2.5 機械手臂末端畫圓實驗先前 2 組實驗已經驗證了控制器對於馬達控制的性能，接下來我們要驗證機械手臂末端位置的控制性能。在此實驗中我們嘗試在 10 秒中用機械手臂末端畫一個直徑 10 公分的圓。在程式中加入先前章節所推導出的逆向運動學程式，我們可以把原先第一、二軸馬達的參考命令，改為輸入機械手臂末端點的 XYZ 座標點進行控制。圖 5-20 為實驗結果。其中藍色線為機械手臂所繪製的圓，黑色線為理想圓，節點附近的藍色線條是固定筆時在紙上留下的痕跡。量測結果最大誤差約為 4 毫米左右。. 5. 28. 30. 34 32 x - axis. 36. 38. 圖 5-36 機戒手臂畫圓實驗結果單位:公分. 52. -5. y - axis. 0.

(63) 第六章結論與未來展望本論文在第二章中推導了 SCARA 型機械手臂之正逆向運動學以及動態方程式，並將其應用在控制器設計及實驗上。在控制器的推導中，首先我們考慮到機械手臂在控制上的困難處，像是耦合度相當高，模型不確定性高等因素，所以在控制機械手臂時大多使用具有良好強健性之控制器。在第四章中本論文假設系統不確定量為系統狀態之函數，並以滑動模式控制器為主控制器，加入適應控制估測系統不確定量之參數，使適應滑動模式控制可以應用在系統不確定量之邊界值為未知的情況中。考慮到使用適應控制估測會造成系統暫態響應變慢，因此引入了指數律於滑動模式控制加快系統暫態響應同時可以減緩跳切現象。而指數律同時也能和適應控制結合成可變步長適應控制，依照誤差調整適應律的步長可加快收斂速度外還能避免適應控制產生內部不穩定的問題。第五章的實驗中我們進行了雙軸分別的定位實驗、雙軸同動追跡、機械手臂末端畫圓三組實驗。並且在定位精度上成功的達到了系統的最小解析度， 3×10-4 及 6×10-4 度；同動追跡實驗穩態時方均根誤差為 0.2934 及 0.1760 度；在機械手臂末端畫圓實驗中最大誤差約為 3 毫米左右。未來希望可以加入其他控制演算法提升同動追跡的性能。由於在正逆向運動學中會使用到機械手臂的幾何長度，因此量測的精度會影響到末端位置的精度，而在一般控制中卻無法藉由回授修正此問題。若是能夠加入影像視覺技術將機械手臂末端位置回授並加以控制就能更進一步提升性能。. 53.

(64) 參考文獻 [1] Stephen R. Platt, Jeff A. Hawks, and Mark E. Rentschler, “Vision and Task Assistance Using Modular Wireless In Vivo Surgical Robots,” IEEE Transactions on Biomedical Engineering, vol. 56, no. 6, pp.1700-1710, June 2009. [2] Pedro Neto, J. Norberto Pires, and A. Paulo Moreira, “Accelerometer-Based Control of an Industrial Robotic Arm,” The 18th IEEE International Symposium on Robot and Human Interactive Communication, pp. 1192-1197, September 2009. [3] Jonathan Kofman, Xianghai Wu, Timothy J. Luu, and Siddharth Verma, “Teleoperation of a Robot Manipulator Using a Vision-Based Human–Robot Interface,” IEEE Transactions on Industrial Electronics, vol. 52, no. 5, pp.1206-1219, October 2005. [4] Zhijun Li, Shuzhi Sam Ge, Martin Adams, and Wijerupage Sardha Wijesoma, “Adaptive Robust Output-Feedback Motion/Force Control of Electrically Driven Nonholonomic Mobile Manipulators,” IEEE Transactions on Control Systems Technology, vol. 16, no. 6, November 2008. [5] Luca Massimiliano Capisani, and Antonella Ferrara, “Trajectory Planning and Second-Order Sliding Mode Motion/Interaction Control for Robot Manipulators in Unknown Environments,” IEEE Transactions on Industrial Electronics, vol. 59, no. 8, pp. 3189-3198, August 2012. [6] S.H. Park and S.I. Han, “Robust-tracking Control for Robot Manipulator with Deadzone and Friction Using Backstepping and RFNN Controller,” IET Control Theory and Applications, vol. 5, iss. 12, pp. 1397-1417, 2011. [7] Divyesh Ginoya, P. D. Shendge, and S. B. Phadke, “Sliding Mode Control 54.

(65) for Mismatched Uncertain Systems Using an Extended Disturbance Observer,” IEEE Transactions on Industrial Electronics, vol. 61, no. 4, pp.1983-1992, April 2014. [8] Mehmet Ö nder Efe, “Fractional Fuzzy Adaptive Sliding-Mode Control of a 2-DOF Direct-Drive Robot Arm,” IEEE Transactions on Systems, vol. 38, no. 6, December 2008. [9] Charles J. Fallaha, Maarouf Saad, Hadi Youssef Kanaan, and Kamal Al-Haddad, “Sliding-Mode Robot Control with Exponential Reaching Law,” IEEE Transactions on Industrial Electronics, vol. 58, no. 2, pp.600-610, February 2011. [10] C.-C. Cheng, S.-H. Chien, and F.-C. Shih, “Design of Robust Adaptive Variable Structure Tracking Controllers with Application to Rigid Robot Manipulators,” IET Control Theory and Applications, vol. 4, iss. 9, pp. 1655–1664, 2010. [11] Zi-Jiang Yang, Youichirou Fukushima, and Pan Qin, “Decentralized Adaptive Robust Control of Robot Manipulators Using Disturbance Observers,” IEEE Transactions on Control Systems Technology, vol. 20, no. 5, pp.1357-1365, September 2012. [12] Raymond H. Kwong, and Edward W. Johnston, “A Variable Step Size LMS Algorithm,” IEEE Transactions on Signal Processing, vol. 40, no. 7, pp.1633-1642, July 1992. [13] Sheng Zhang and Jiashu Zhang, “New Steady-State Analysis Results of Variable Step-Size LMS Algorithm with Different Noise Distributions,” IEEE Signal Processing Letters, vol. 21, no. 6, pp.653-657, June 2014. [14] Hsu-Chang Huang and Junghsi Lee, “A New Variable Step-Size NLMS 55.

(66) Algorithm and Its Performance Analysis,” IEEE Transactions on Signal Processing, vol. 60, no. 4, pp.2055-2060, April 2012. [15] Aimeng Wang, Xingwang Jia, and Shuhui Dong, “A New Exponential Reaching Law of Sliding Mode Control to Improve Performance of Permanent Magnet Synchronous Motor,” IEEE Transactions on Magnetics, vol. 49, no. 5, pp.2409-2412, May 2013.. 56.

(67)