應用強健性重複滑動模型控制於機械手臂定位追跡控制器設計

全文

(1)國立臺灣師範大學機電工程學系碩士論文指導教授:陳美勇博士應用強健性重複滑動模型控制於機械手臂定位追跡控制器設計 Design a Robust Repetitive Sliding Mode Controller for Robot Manipulator Position and Tracking Control. 研究生 : 張煒騰撰中. 華. 民. 國. 1. 0. 4. 年. 1. 月.

(2) 摘要本研究利用非線性控制理論實現重複控制結合滑動模型控制於非線性系統之四軸機械手臂上，使得輸出的追跡效能具有較高精度，且在外部干擾訊號以及系統未知項影響下有較佳的強健性。. 機械手臂精密運動控制在實際的應用上經常面對不同型態的負載，如不可預測的外部干擾訊號、系統未知項(System Uncertainty)，或是系統模型自身的影響：重力、柯氏力、慣性力等等，在不同型態的負載影響下使得系統輸出效能的精準度與穩定性受到影響。為了有效提高系統的輸出效能，必須對外部干擾訊號以及系統未知項的影響加以抑制。本研究致力於機械手臂的精密運動控制。其中滑動模型控制設計用來消除外部干擾訊號以及系統模型的影響，增加系統的強健性。結合重複控制抑制系統未知項的影響，即使系統包含未知項依然能夠達成追跡控制。. 關鍵詞: 重複控制(Repetitive Control)、滑動模型控制(Sliding Mode Control)、機械手臂(Manipulator). i.

(3) Abstract In this study, the Repetitive Control (RC) combine with Sliding Mode Control (SMC) by nonlinear control theorem is applied on four axis manipulator, making the tracking performance more precisely and the system uncertainty have better robustness under the effect of outloop disurbances.. Manipulator preciseness control is mostly applied in many diffirent types of payload, such as the unpredictability outloop disturbance, system uncertainty or the impact of the model itself gravity, goriolis force, inertia force. Diffirent types of payload impact effect and cause system preciseness and stability loss. In order to improve the output efficiency of the system, suppressing the influence of outloop disturbances and systems uncertainty is necessary. This paper proposes the SMC that have hight performance combined with the RC has high performance on to suppressing the disterbances and enhancing the robustness.. Keywords: Repetitive Control、Sliding Mode Control、Manipulator. ii.

(4) 誌謝在我寫下這段文字的當下，表示研究所生涯到了尾聲，人生即將開啟另一個階段。在研究所跨領域的學習階段，透過各領域的集思廣益、分享知識，經過學習與共識達到研究的目標。因此，能夠完成本論文，必須感謝很多人。首先感謝是我的指導教授陳美勇老師。老師平常給予我們空間廣泛涉獵知識，尋找適切的研究方向。並且在研究遇到瓶頸時，提供意見與突破瓶頸的動力。感謝老師對於研究的啟發，並且在平常的生活中教導我們待人處世的道理，讓我們時常提醒自己用正面的態度面對問題。在碩士階段中，這些皆使得生活與研究都有豐碩的收穫與幫助。在此，致上衷心的感謝。此外，感謝口試委員. 莊鎮嘉教授、郭重顯. 教授、練光祐. 教授，在百忙. 中仍不吝撥冗給予本論文許多指導與勉勵，使得本論文能夠更加嚴謹與周延，也讓研究能夠更加完善。研究是條孤獨的路，苦想著尚待解決的問題，在時間的壓力下，內心時常感到煎熬。然而，感謝實驗室的學長. 哲勝、希哲、東諺、秉剛、昀翰，夥伴. 智翔、玠虢、一豪、高遠，在求學的道路上互相關心與砥礪，在研究上可以互相討論並交換意見。因為和諧融洽的研究室氣氛，彼此間建立了革命的情感，在研究的道路上感到更加充實。最後感謝家人們多年來對我的栽培與照顧，在求學過程中全力給予我支持與體諒，使我能夠從人生的低潮中走出，蛻變成為研究生，最後順利的完成學業。因此將本論文獻給我最敬愛的家人們，以及所有關心我的師長、同學與諸多朋友們，願一起分享這份喜悅，另外也期勉實驗室的各位成員都可以順利完成自己的目標。. 張煒騰１０４年. iii. 謹誌于師大.

(5) 目錄摘要………………………………………………………………………………i Abstract …………………………………………………………………………ii 致謝……………………………………………………………………………..iii 目錄……………………………………………………………………………..iv 圖目錄…………………………………………………………………………vi 表目錄…………………………………………………………………………viii 第一章緒論…………………………………………………………………….1 1.1 前言……………………………………………………………………1 1.2 文獻回顧………………………………………………………………3 1.3 研究動機與目的…………...…...………………………..……………9 1.4 本論文之貢獻……………………..…………………………...…….10 1.5 本論文之架構………..………………………………………………11 第二章機械手臂系統模型…………………………………………………...12 2.1 D-H 座標系統…………………..…………………………………..12 2.2 正向運動學…………………….…………………………………..15 2.3 逆向運動學…………..……………………………………………..19 2.4 動力學模型………………..…………………………………………21. iv.

(6) 第三章系統控制器理論及設計………………….…………………………27 3.1 控制系統…………………………………………………………..27 3.2 滑動模型控制……………….…………………………………….28 3.3 重複控制理論…………………………………………………….31 3.4 Lyapunov 穩定性理論…………..…………………………………34 第四章實驗設備……………………………………………………………...36 4.1 AI 直流伺服馬達…………………………………………………..37 4.2 PXI 6230 DAQ 資料擷取卡………...……………………………..40 4.3 H-Bride...……………….…………....……………………………..41 4.4 人機介面及圖形化程式軟體……..……………………………..42 第五章模擬與實驗……………………………………………………..…….43 5.1 實驗結果…………….………..…………………………………..45 第六章結論及未來展望 …………………………………………………….55 參考文獻……………………………………………………………………….56. v.

(7) 圖目錄圖 1-1 典型重複控制系統模型……………………………………………………..3 圖 1-2 重複控制追跡………………………….…………………………………..3 圖 1-3 二軸機械手臂示意圖………………….…………………………………..5 圖 1-4 週期訊號關節角度誤差………………….…………………………………..5 圖 1-5 非週期訊號關節角度誤差……………………………………..……………..5 圖 1-6 延遲變量重複控制器方塊圖…………………...………..…………………..6 圖 1-7 延遲變量律方塊圖…………………….……………..……………………..6 圖 1-8 高階重複控制設計……………………………………….………………..6 圖 1-9 強健適應性前饋重複控制器系統方塊圖…………………………………..7 圖 1-10 適應性前饋控制方塊圖………………………………………..……………7 圖 1-11 適應滑動模型控制……………………………………………………….8 圖 2-1 D-H 座標系統示意圖………………………………….…………………12 圖 2-2 D-H 座標系統配置圖……………………………...…..……………………15 圖 2-3 機械手臂初始位置示意圖……………………………………………….18 圖 2-4 機械手臂運動解耦..……………..………………………………………..19 圖 2-5 機械手臂簡化後示圖…..……………………..….………………………20 圖 3-1 可變結構控制系統…………………………………..………..……………28 圖 3-2 迫近條件、滑動條件與滑動平面之關係………….………………………30 圖 4-1 實驗設備整體系統方塊圖……………………….……..…………………36 圖 4-2 RX-Series 馬達……………………………..…….………………………37 圖 4-3 機械手臂架構…………………………………..…….……………………38 圖 4-4 AI 伺服馬達角位示意圖…………………………………….………………39 圖 4-5 直流伺服馬達控制器與編碼器腳位圖…………….………..……………39. vi.

(8) 圖 4-6 PXI 6230 DAQ 資料擷取卡...……………..…….………..………………40 圖 4-7 H-Bridg 外觀圖………………………….………..…………….………41 圖 4-8 H-Bridg 電路圖………………..……….……….………………………41 圖 5-1 控制程序…………………………………………………………………...43 圖 5-2 LabVIEW2010 啟動頁面……………………….…………….……………44 圖 5-3 MATLAB 程式啟動頁面………………………..………………..………44 圖 5 - 4 R S M C q1 動態軌跡響應 … … … … . . … … … … . . … … … … … … … … 4 5 圖 5 - 5 R S M C q1 穩態誤差響應 … … . . … … … … … … . . … … … … … … … … 4 5 圖 5 - 6 R S M C q 2 動態軌跡響應 … … … . .… … … … … . . … …… … … … … … 4 6 圖 5 - 7 R S M C q2 穩態誤差響應 … … . . … … … … … . . … … … … … … … … 4 6 圖 5 - 8 R S M C q3 動態軌跡響應 … … … . . … … … … . . … … … … … . … … … 4 7 圖 5 - 9 R S M C q3 穩態誤差響應 … … . . … … … … … . . … … … … … … … … 4 7 圖 5-10 RSMC q 4 動態軌跡響應……………….....…………..……………………48 圖 5-11 RSMC q 4 穩態誤差響應………………..……………..……………………48 圖 5-12 加入外部非週期干擾訊號， q1 動態軌跡響應……………..……….…49 圖 5-13 加入外部非週期干擾訊號， q1 控制量…….…………………………..49 圖 5-14 加入外部非週期干擾訊號， q2 動態軌跡響應…………………………50 圖 5-15 加入外部非週期干擾訊號， q2 控制量………………………..………50 圖 5-16 加入外部非週期干擾訊號， q3 動態軌跡響應….………………………51 圖 5-17 加入外部非週期干擾訊號， q3 控制量……………………..……………51 圖 5-18 加入外部非週期干擾訊號， q 4 動態軌跡響應……………………………52 圖 5-19 加入外部非週期干擾訊號， q 4 控制量……………………………………52 圖 5-20 機械手臂末端點定位………………….……..……………………………53 圖 5-21 機械手臂畫圓實驗結果…………………….……..………………………54. vii.

(9) 表目錄表 2-1D-H 座標系統參數……………..………………….……………………15 表 4-1AI 伺服馬達規格表…….……………………………………………….48 表 4-2 PXI 6230 規格……………………..…………………………………………50 表 4-3 TA7291P 腳位功能………………………………………………….……….58. viii.

(10) 第一章緒論 1.1 前言隨自動化工業蓬勃發展，高效率、高精密度製程技術需求與日俱增。機械手臂具有類似人類手臂靈活度，並且可依據不同的工作環境提供適當輸出力量，進而對運動控制有高精密定位及工作穩定性，因此廣泛應用於汽車工業、模具製造、太空科技、電子製程、醫療系統等各種領域中。機械手臂控制受到許多因素影響，特別是不同軸向之間非線性動態耦合現象、馬達因摩擦以及慣量產生之馬達死區、桿件撓性、負載變動、外部干擾訊號等等，產生之非線性系統不確定量，影響機械手臂位置控制的精度、穩定性與輸出效能等等。因此抑制非線性系統不確定量影響與外部干擾訊號增加系統強健性為目前機械手臂控制上迫切改善之需求。強健是控制系統重要的環節之一。當系統存在未預期的參數、外部干擾影響，依然能夠提供良好的輸出性能，則稱此系統具有強健性。強健控制之基本架構有兩種，H-infinity 與滑動模型控制(Sliding-mode contol，SMC)。其中滑動模型控制具備匹配式雜訊、可變結構控制(Variable structure control)之優勢，加上設計直觀，因此應用相當普遍。隨著機械手臂應用領域越來越廣泛，設計控制器抑制與補償雜訊影響，提供高精準度的輸出性能，成為精密定位控制重要的關鍵。近幾年應用於機械手臂控制之基本架構分別為：(1)利用滑動模型控制消除系統參數影響與外部干擾訊號；(2)結合學習控制架構估測系統不確定量，加以抑制。工業應用上，機械手臂經常執行人工不便進行、耗時費力或重複性之工作，如零件裝配、金屬焊接、產品檢測、貨物搬運等等。為了達到重複性動作，必須輸入週期性訊號，針對週期性訊號有效追蹤或抑制，消除穩態誤差。能夠達成此目標之控制法種類繁多，如 PID 控制、可變結構控制、模糊控制(Fuzzy control) 與重複控制(Repetitive control，RC)。其中重複控制是針對週期性訊號有效控制之 1.

(11) 代表性控制方法，針對週期性訊號有效追蹤或抑制，達成消除週期性響應誤差之控制目標。然而使用重複控制器於非週期性訊號輸入或負載變動時，會降低系統相對穩定度，同時影響系統效能。伺服馬達體積小、重量輕，並且擁有精確的位置控制，近年來廣泛應用於機器人進行各項研究與任務。伺服馬達之所以能夠達成精準定位控制，主要來自於其中包含可變電阻可以量測馬達角度值，並且透過控制晶片及其電路控制馬達。然而控制晶片內部已包含撰寫好的控制理論(例如 PID)，為將自行研究之控制理論運用於伺服馬達，將馬達內部控制理論之影響消除，可採用幾種方式：(1)將研究之控制法重新寫入晶片(2)使用自行編寫之控制晶片。前者需將控制晶片各腳位與電路量測出來，並採用初始開發環境編寫控制器、重新燒錄，如此將耗費大量研發時間。近年以來，有學者設計出功能齊全、易於開發之微控制器。其主要精神在於加速產品研發與設計之時間，藉由開放電路設計，使用者能夠廣泛應用於所需之裝置。本論文所使用的控制器設計方法，利用滑動模型控制消除系統參數影響與外部干擾訊號，結合重複控制估測系統未知係數，並消除其影響。另外採用 Arduino 控制面板取代原先馬達內部的控制晶片，將馬達可變電阻之訊號透過控制面板回授，並控制馬達。最後期望運用滑動模型控制理論結合重複控制理論，完成高精度之軌跡追蹤與干擾抑制之週期運動控制。. 2.

(12) 1.2 文獻回顧重複控制是 20 世紀 80 年代初期，由日本學者[1]提出一種具有學習能力的控制理論。在控制系統中建立數學模型，使控制系統能夠透過自主學習改善追蹤週期性訊號之精準度，進而達到對週期訊號之追縱或抑制任意週期之干擾訊號。重複控制理論之基本架構來自於內部模型(The Internal Model Princople)[2]，此原理敘述，在漸進穩定之閉迴路系統中，若能在系統之順向路徑上建立包含輸入訊號之數學模型，則系統就具有漸進追蹤與抑制同週期干擾訊號之能力。 Inoue 等學者[1]提出在重複控制系統之延遲路徑當中串聯低通濾波器，用以提高重複控制系統的穩定度。不久之後 Hara 等人[3]更將改良之重複控制系統推廣應用於 MIMO 系統上，奠定重複控制發展之基礎。然而實現重複控制最大之困難在於使系統維持穩定，其基本要求為系統本身必須是漸進穩定系統，加入重複控制器之後，提供適當之補償量，用以補償原系統相位落後之影響。典型重複控制如圖 1-1 所示，此為目前應用最為廣泛之重複控制系統結構，其控制系統主要目標為補償量之最佳化，提供適當控制量於系統，降低穩態誤差，如圖 1-2 所示。. 圖 1-1 典型重複控制系統模型. 圖 1-2 重複控制追跡 3.

(13) 重複控制設計方式主要分為兩種，分別為非線性系統線性化、適應性重複控制。前者利用補償器之概念設計重複控制補償量，使系統穩定。後者則運用 Lyapunov 穩定法則，證明控制器之穩定性。 Kazumasa Kaneko, Roberto Horowitz[4]等學者，提出非線性重複控制器設計架構，應用於機械手臂控制。此控制系統利用事先所建立之內部模型，對系統參數進行估測，執行機械手臂之位置控制與速度控制。本論文由此架構設計適應重複控制於機械手臂之非線性系統當中，利用重複控制對於週期訊號具有良好的追蹤效能以及抑制干擾之能力，在任意週期干擾訊號情況下，系統模型不確定項造成系統性能之影響進行估測與抑制。重複控制對於週期性訊號之訊號追蹤以及干擾抑制是相當有效的控制理論，然而與一般具有學習能力之控制理論存在類似之限制：第一，系統暫態性能、第二，收斂速度無法滿足需求。具有學習能力之控制理論，需要不特定時間掌握當前訊號、誤差之計算，並且透過不斷修正才能夠達到適當之穩態誤差，特別在授控系統模型複雜與數學計算量大時，將較多的學習時間，影響系統控制之效能。欲提供良好的暫態與穩態之性能、提升響應速度，滑動模型控制為相當具有代表性之控制理論。. 4.

(14) Xiao-Dong Li, Tommy W. S. Chow, John K. L. Ho, and Hong-Zhou Tan 等學者 [5]，提出了類滑動模型之重複學習控制，應用於二軸機械手臂之非線性系統。滑動模型控制主要概念來自於可變結構控制，定義滑動方程式限制允許之誤差，當滑動方程式到達所定義之邊界則啟動滑動模式。實驗模擬當中，控制訊號分別為  d(1)  (0.8  0.2 sin(0.25t ' ))0.5 sin(0.25t ' ) 、  d( 2)  (0.6  0.2 sin(0.25t ' ))0.5 sin(0.25t ' ). 首先將欲控制之系統狀態透過類滑動模型(Quasi-Sliding Mode)調整，減少誤差量，再將調整後之系統狀態帶入重複控制當中計算。如此可以增加系統暫態性能，減少系統收斂時間，並且可以提升系統定位精準度。圖 1-4 為重複學習控制 RLC 針對週期訊號之穩態誤差效能，圖 1-5 為重複學習控制 RLC 針對週期訊號追跡誤差效能。. 圖 1-3 二軸機械手臂示意圖. 圖 1-4 週期訊號關節角度誤差. 圖 1-5 非週期訊號關節角度誤差. 5.

(15) Ezequiel Bajonero Canonico, Ewout van der Laan, Sjirk Koekebakker and Maarten Steinbuch 等學者[6]，提出強健延遲可變重複控制。此論文利用高階重複控制結合延遲變量律，應用於印表機之馬達控制，解決重複控制週期不精確時造成控制器無法完整將干擾訊號抑制之問題，進而改善系統強健性。實驗當中，建立高階內部系統模型於控制器當中，提升系統週期與實際週期有誤差時之強健性，既使不精確之週期存在仍能滿足控制需求，此外延遲變量系統模型於控制器當中，有效解決重複控制抑制不精確週期訊號之能力，將兩種控制特性結合，形成對變動週期訊號具有強健性之控制器設計。圖 1-6 控制器系統方塊圖。圖 1-7 時間延遲變量律與週期關係圖、圖 1-8 延遲變量律系統方塊圖、圖 1-9 高階重複控制設計方塊圖。. 圖 1-6 延遲變量重複控制器方塊圖. 圖 1-7 延遲變量律方塊圖. 圖 1-8 高階重複控制設計. 6.

(16) Chih-Hsien Chunga, Min-Shin Chen 等學者[7]，提出強健適應性前饋重複控制器。利用適應性前饋控制之概念設計重複控制，達成週期訊號之追跡與週期干擾訊號之抑制。此控制器面對未知干擾訊號時，適應律更新為直覺性之架構而非估測型。其優點分別如下：第一為適應增益直可以直接調整，不用經過穩定性驗證分析。第二為此控制器不僅可應用於極小相位系統，亦可應用於非極小相位系統。第三為控制律在干擾訊進入系統時能夠獨立運作。在系統模型之參數精準以及原系統必須漸進穩定的條件下，當週期干擾訊號進入系統，適應性前饋控制能夠及時由事先建立內部模型中找出與週期干擾訊號相同週期之控制量，並且抑制干擾訊號對系統之影響，隨著每個週期不斷的修正誤差量，系統漸進穩定，達到控制需求之穩態誤差值，如圖 1-10 所示。. 圖 1-9 強健適應性前饋重複控制器系統方塊圖[6]. 圖 1-10 適應性前饋控制方塊圖. 7.

(17) Shafiqul Islam, Xiaoping P. Liu 等學者[8]發表了一篇應用於機械手臂之強健性滑動模型控制器。此篇論文將滑動模型作為主要控制器，專門抑制干擾訊號以及系統模型之影響，為增加控制系統對系統未知項係數之強健性，在滑動模型控制當中加入適應性控制，估測系統未知項參數，形成適應性滑動模性控制(ASMC)。當適應性參數估測量 ˆ  4 時，(a)第一軸追跡誤差、(b)第二軸追跡誤差、(c)第一軸控制量、(d)第二軸控制量分別如圖 1-11 所示。. 圖 1-11 適應滑動模型控制實際值(左)與估測之(右) 滑動模型控制具備匹配式雜訊、可變結構控制之優勢，加上設計直觀，與良好的強健性，時常搭配適應控制、類神經網路、模糊控制等具有學習能力之控制器，加強控制系統之精準度，提升控制系統對於各項未知系統參數、未知干擾訊號之強健性。利用滑動模型控制搭配具備適應性的控制法成為現今許多高階控制器設計的策略，如[9][10][11][12][13]，因具備上述之良好之特性，學術研究上應用相當廣泛。. 8.

(18) 1-3 研究動機與目的隨自動化工業蓬勃發展，高效率、高精密度製程技術需求與日俱增。機械手臂具有類似人類手臂靈活度，並且可依據不同的工作環境提供適當輸出力量，進而對運動控制有高精密定位及工作穩定性，因此廣泛應用於汽車工業、模具製造、太空科技、電子製程、醫療系統等各種領域中。機械手臂控制受到許多因素影響，特別是不同軸向之間非線性動態耦合現象、馬達因摩擦以及慣量產生之馬達死區、桿件撓性、負載變動、外部干擾訊號等等，產生非線性系統不確定量，影響機械手臂位置控制的精度、穩定性與輸出效能等等。因此抑制非線性系統不確定量影響與外部干擾訊號，增加系統強健性為目前機械手臂控制上迫切改善之需求。一般市面上之伺服馬達內部經常包含撰寫好控制演算法之控制晶片、可變電阻量測角度值以及開發端之控制程式。然而對於研究開發控制法之研究團隊來說，這些套裝式配置有幾項限制：第一為研究之控制法無法單純控制機械手臂，其控制量將會受到控制晶片內部控制演算法影響，第二為套裝之控制開發環境，控制晶片與面板之電路未知，不易將自行開發之控制器編譯入控制晶片內。因此本研究選擇使用已知電路之控制面板，結合電腦端編譯開發之控制演算法，線上即時調整控制參數。本研究之目標為開發一套線上參數調整之精密運動機械手臂，其控制晶片能夠透過電腦端直接撰寫研究開發之強健重複滑動模型控制器。在本研究中具挑戰力之處在於機械手臂非線性模型與非線性干擾訊號之影響，如(1)機械手臂模型之系統鑑別、(2)系統未知項之影響、(3)機械手臂慣量產生之馬達死區、桿件撓性、負載變動等影響。因此重複控制與滑動模型控制器之設計與整合為克服干擾及提升控制精度與穩定性之重要關鍵。若上述各項影響能夠預先補償，則以上各種性能規格之要求將達到及完成。. 9.

(19) 1.4 本論文之貢獻本研究之貢獻可分為三大部分，第一：我們開發重複控制結合滑動模型控制，分別將系統未知參數影響與模型參數之影響消除，對週期訊號進行追跡與週期干擾訊號之抑制。第二：運用已知電路之控制面板，取代原伺服馬達之控制晶片，使控制演算法能夠線上編譯與修正，使研究開發之控制器能夠直接控制馬達，不受內部控制演算法影響。第三：運用已知電路之控制面板結合電腦編譯控制演算法，取代原伺服馬達之控制晶片，並且能夠線上調整控制參數，提升研究開發之速度，應用與開發環境更具開放性。. 簡略整理本論文之貢獻可分為四部分 1. 運用已知電路控制面板取代原伺服馬達之控制晶片，結合電腦端控制演算法之線上編譯，提供研究開發之控制理論能夠直接應用於伺服馬達。 2. 結合已知電路控制面板與電腦端控制演算法之線上編譯，將控制理論能夠直接於機械手臂上實現，提升研究開發之速度。 3. 建立機械手臂之非線性模型。 4. 重複滑動模型控制之設計，應用於機械手臂之定位與追跡控制。. 10.

(20) 1.5 本論文之架構本論文一共分為六章節，各章標題及內容簡略說明如下 [第一章]:緒論在此部分將詳細說明研究背景及目標，並藉由參考文獻回顧，得知在過去相關領域研究中，所使用的控制器設計方式，及所達成之運動控制精度。 [第二章]:機械手臂模型介紹機械手臂種類、控制原理、動力學。 [第三章]:控制器理論與設計設計強健性重複滑模控制器(RRSMC)，同時針對週期性訊號輸入與干擾的環境下，提出對非線性模型影響、系統未知參數影響以及未知改善與控制策略，其詳細理論及推導過程將在此部份說明。 [第四章]:實驗設備介紹線上參數調整之精密運動機械手臂中所使用之各項研究設備規格，其中包含伺服線性馬達、驅動器、控制面板、人機介面程式編譯軟體。 [第五章]:實驗結果與討論在本部分共分為三大部分，第一為強健性重複滑動模型控制器針對控制器之內部模型、干擾訊號、系統輸入訊號具有相同週期時之控制效能。第二為針對控制器之內部模型、干擾訊號、系統輸入訊號不具有相同週期時之控制效能。第三為針對控制器之干擾訊號不為週期訊號時之控制效能。 [第六章]:結論及未來方向根據整體研究的效果及實驗的目標達成程度，進行結論，並說明未來目標。. 11.

(21) 第二章機器械手臂系統模型當我們進行剛體運動分析時，主要可分為兩個部份：單純描述剛體隨著時間之運動變化，不考慮作用力與質量之影響我們稱之為運動學。討論造成剛體運動之原因，稱之為動力學。本章節運用[9]使用之方法，建立正向運動學與逆向運動學，用來了解命令給予機械手臂時機械手臂末端點之位置，以及了解當前機械手臂位置。並且透過尤拉-拉格朗日方程式(Euler-Lagrange Eqution)建立機械手臂運動方程式。. 2.1 D-H(Denavit-Hartenberg)座標系統 D-H 座標系統利用各連桿之桿件長、桿件旋轉角度、連桿偏移長度、關節旋轉角度，建立各連桿之間座標轉換矩陣，透過建立各桿件之間座標轉換矩陣，進行正向運動學以及逆向運動學分析。D-H 座標廣泛應用於多維度之連桿座標系統，其優點在於只需透過桿件長、桿件旋轉角度、連桿偏移長度、關節旋轉角度等，四個參數即可推算出各軸之間轉換矩陣。. 圖 2-1 D-H 座標系統示意圖. 12.

(22) 圖 2-1 當中各參數意義如下：. ai ： zi 與 zi 1 兩軸間沿著 xi 1 軸平移之距離.  i ： zi 與 zi1 兩軸間沿著 xi 1 軸旋轉之角度 d i ： zi 與 zi 1 兩軸間沿著 zi 1 軸平移之距離.  i ： zi 與 zi1 兩軸間沿著 zi 1 軸旋轉之角度各軸之間基本姿態之齊性轉換矩陣 H (Homogeneius transformations)其定義如下 R d  H   0 1. (2  1). 其中 R 為 i-1 座標與 i 座標之旋轉量，d 為 i-1 座標與 i 之平移量，基本姿態之向量分別如下所示. Ai  Rotz , Transz ,d Transx,d Rotx, i. i. c s   0  0.  s c 0 0. c s   0  0.  s c c c s 0. i. i. i. i. i. 0 1 0 0 0  0  1  0. 0 0 1 0. i. i. i. i. i. i. i. s s  c s 1 0 i. 0 1 0 0. i. i. i. i. 0 0  1 0 0  0  1 d i  0   0 0  0 ai c ai s di 1. i. i. 0 1 0 0.      . 0 ai  1 0 0 0  0 c 1 0  0 s  0 0  0 0. i. i. 0  s c 0 i. i. 0 0 0  1. (2  2). 沿 x 軸平移. Transx ,a. 1 0  0  0. 0 1 0 0. 0 0 1 0. a 0  0  1. (2  3). 13.

(23) 沿 x 軸轉動. Rotx ,. 1 0 0 c   0 s  0 0. 0 0 0  1. 0  s c 0. (2  4). 沿 y 軸平移. Transy ,b. 1 0  0  0. 0 1 0 0. 0 0 1 0. 0 b  0  1. (2  5). 沿 y 軸轉動. Rot y ,.  c  0   s   0. 0 0 0  1. 0 s 1 0 0 c 0 0. (2  6). 沿 z 軸平移. Transz ,c. 1 0  0  0. 0 1 0 0. 0 0 c  1. 0 0 1 0. ( 2  7). 沿 z 軸轉動. Rotz ,. c s   0  0.  s c 0 0. 0 0 1 0. 0 0 0  0. (2  8). 14.

(24) 2.2 正向運動學本論文機械手臂使用 D-H 座標系統建構空間座標，利用正向運動學計算末端點在 D-H 座標中之位置。本研究 D-H 座標系統配置圖如圖 2-2 所示。由圖 2-2 定義座標 0 在機械手臂固定之底座，定義座標 1 於 Roll 角度轉動之轉盤，座標 2 與座標 3 皆定義於同一關節上。. 圖 2-2 D-H 座標系統配置圖. 15.

(25) 表 2-1 D-H 座標系統參數. ai. i. di. i. 1. 0. 0. 0. 1. 2. 0.  90 . d2. 2. 3. 0.  90 . 0. 3. 4. 0. 90. d4. 4. 5. 0. 0. d5. 0. 關節. 由章節 2-1 D-H 座標系統各姿態之轉換矩陣，推算出各軸之間轉換矩陣關係。其中 ci 與 si 分別表示 cosθ與 sinθ分別表示不同關節角度帶入之結果 cos 1  sin  1 A01    0   0.  sin 1 cos 1 0 0. 0 0 1 0. 0 0 0  1. (2  9). cos 2  sin  2 A12    0   0. 0  sin  2 0 cos 2 1 0 0 0. 0 0  d2   1. (2  10). cos 3  sin  3 3 A2    0   0. 0 sin  3 0  cos 3 1 0 0 0. 0 0 0  1. (2  11). cos 4  sin 4 A34    0   0.  sin 4 0 0  cos 4 0 0  0 1 d4   0 0 1. (2  12). 16.

(26)  r11 r 4 1 2 3 4  21 A0  A0 A1 A2 A3 r31  r41. r12 r13 r14  r22 r23 r24  r32 r33 r34   r42 r43 r44 . (2  13). 其中各項所方程式如下所示. r11  s4 (c1s2  c2 s1 )  c3c4 (c1s2  c2 s1 ). ,. r12  c4 (c1 s2  c2 s1 )  c3c4 (c1c2  s1 s2 ). ,. r13  s3 (c1c2  s1 s2 ). ,. r14  d 4 s3 (c1c2  s1 s2 ). ,. r21  c3c4 (c1 s2  c2 s1 )  s4 (c1c2  s1 s2 ). ,. r22  c4 (c1c2  s1 s2 )  c3c4 (c1 s2  c2 s1 ). ,. r23  s3 (c1 s2  c2 s1 ). ,. r24  d4 s3 (c1c2  s2 s1 ). ,. r31  c4 s3. ,. r32  s3 s4. ,. r33  c3. ,. r34  d1  d 2  d 2 c3. ,. r41  0 ， r42  0 ， r43  0 ， r44  1. 17.

(27) 圖 2-3 機械手臂初始位置示意圖透過各關節之間座標轉換矩陣，將原點之矩陣與 A04 轉換矩陣相乘，即可得到機械手臂末端點之位置。其順向空間座標系統數學式如(2-14)所示 P4  A04 P0  x 0 d 4 s3 (c1c2  s1s2 )   y        A04 0  d 4 s3 (c1c2  s1s2 ) z 0  d 2  d 4 c3        1 1  1  . (2  14). 因此得到機械手臂末端點之 xyz 軸之位置. x  d 4 s3 (c1c2  s1s2 ). (2  15). y  d 4 s3 (c1c2  s1 s2 ). (2  16). z  d 2  d 4c3. (2  17 ). 18.

(28) 2.3 逆向運動學本研究在逆向運動學分析當中，考量機械手臂工作平面之需要，定義末端點夾爪工作姿態為 z 垂直向下，因此機械手臂夾爪末端點之位置(2-17)重新定義為 z  d 2  d 4c3  d5. (2  18). 1 0 5 A4   0  0. (2  19). 0 1 0 0. 0 0 0 0  1 d5   0 1. 圖 2-4 機械手臂運動解耦在圖 2-2 當中，座標 3 Roll 角度之轉動並不會影響末端點 c 之位置，為使運動分析更加精簡，本研究將座標 2 與座標 3 定義為同一關節，如圖 2-4 所示。機械手臂夾抓端定義垂直向下於工作平面，夾爪之夾取姿態對於末端點 c 將不會帶來影響，其夾取位置即將末端點 c 加上夾爪長度，分析時將不考慮此長度。因此機械手臂末端點 5 夾取之位置，即末端點 c 之 D-H 座標位置加上 d5 之大地座標 z 軸之長度，如(2-19)所示。由上述之運動學座標定義之條件，對機械手臂進行逆向運動學分析。. 19.

(29) 由(2-15)與(2-16). x  d 4 s3 s1 y  d 4 s3c1 將兩式相除. y d 4 s3 s1 s1    tan1 x d 4 s3 c1 c1\. (2  20). 因此得到. 1  tan 1. y x. (2  21). 再由. r  xc2  yc2 與 s  zc  d1 求得.  2  tan 1. s r. (2  22). 圖 2-5 機械手臂簡化後示意圖. 20.

(30) 2.4 動力學模型在本章節當中將利用尤拉-拉格朗日方程式推導出機械手臂之運動方程式 (Eqation of Motion)。其中我們透過正向運動學之分析結果，取得機械手臂各關節之間座標轉換關係(Jacobian)，由各連桿之質心位置與動能與位能，推導出機械手臂之運動方程式。. 2.4.1 Jacobian 矩陣 Jacobian 矩陣用來轉換不同桿件之間座標關係，在機器人動力分析上經常使用。欲得到桿件運動時線速度與角速度之轉換矩陣，必須先知道桿件質心位置，由線速度、角速度與馬達轉速之關係，定義 Jacobian 矩陣。連桿與馬達之 Jacobian 矩陣定義如下. ζ  Jq. (2  23). . T T 其中 q 為馬達轉速，   v , . . T. v 與  分別為連桿之線速度與角速度，此關係可. 改寫為. v  J vq. (2  24). ω  J ωq. (2  25). 此 Jacobian 矩陣具有 i 列，相當於機械手臂之 i 個關節。欲求得線速度、角速度 Jacobian 矩陣，經由正向運動學分析之 z 軸位置，透過(2-24)與(2-25)計算得到。本研究所使用之機械手臂關節為五軸旋轉關節之 Elbow 型機械手臂，各桿件質心位置分別定義為 lc1 、 lc 2 、 lc 3 、 lc 4 、 lc 5 ，Jacobian 矩陣集合如下.  JV J 5 (q)    J. 1. 1. JV J. 2. 2. JV J. 3. 3. JV J. 4. 4. JV  J  . (2  26). 5. 5. 21.

(31) 桿件質心位置 CM1  0 0 0. T. CM2  0 0 lc 2 . T. CM3  0 0 lc 2 . T. CM4  d 4 s3c12 d 4 s3c12 d 2  lc 4c3 . T. CM5  d 4 s3c12  lc5 s3c12 d 4 s3 s12  lc5 s3 s12 d 2  d 4c3  lc5c3 . T. (2  27 ). 將 CMi 對 q 偏微分，求得 JV. i. 0 0 0 0 0  CM1  JV   0 0 0 0 0 q 0 0 0 0 0 1. 0 0 0 0 0  CM 2  JV   0 0 0 0 0 q 0 0 0 0 0 2. 0 0 0 0 0  CM 3  JV   0 0 0 0 0 q 0 0 0 0 0 3. 0 0 0 d 4 s3 ( s12 ) d 4 s3 ( s12 ) CM 4  JV    d 4 s3c12 d 4 s3c12 0 0 0 q  0 0  lc 4 s3 0 0 4.  d 4 s3 s12  lc 5 s3 s12  d 4 s3 s12  lc 5 s3 s12 d 4c3c12 0 0 CM 5  JV    d 4 s3c12  lc 5 s3c12 d 4 s3c12  lc 5 s3c12 d 4c3c12 0 0 q  0 0  lc 4 s3 0 0 5. 22. (2  28).

(32) Jω可以由機械手臂正向運動學得知。z2 為(2-9)與(2-10)相乘後 R 矩陣之第三行元素，同樣為 z3 (2-9)(2-10)(2-11)相乘後 R 矩陣之第三行元素，z4 為(2-9) (2-10) (2-11) (2-12)相乘後 R 矩陣之第三行元素，z5 為(2-9)(2-10)(2-11)(2-12)(2-19) 相乘後 R 矩陣之第三行元素。主要概念在於，zi 軸對於其他 zi+1 zi+2 zi+3 等軸之影響。其運算後結果如下所示. 0 0 0 0 0  J   0 0 0 0 0 1 0 0 0 0 1. J. J. J. J. 2. 3. 4. 5. 0  c1s2  c2 s1 0 0 0  0 c1c2  s1s2 0 0 0 1 0 0 0 0 0  c1s2  c2 s1 s3 (c1c2  s1s2 ) 0 0  0 c1c2  s1s2 s3 (c1s2  c2 s1 ) 0 0 1 0 c3 0 0 0  c1s2  c2 s1 s3 (c1c2  s1s2 ) s3 (c1c2  s1s2 ) 0  0 c1c2  s1s2 s3 (c1s2  c2 s1 ) s3 (c1s2  c2 s1 ) 0 1 0 c3 c3 0 0  s12 s3c12 s3c12 s3c12   0  c12 s3 s12 s3 s12 s3 s12  1 0 c3 c3 c3 . (2  29). 23.

(33) 2.4.2 尤拉-拉格朗日方程式(Euler-Lagrange Eqution) 機器人領域當中，Euler-Lagrange Eqution 是很常用來分析動力學的方法之一，此方程式定義如下. d L L   f i i  1,..., n dt qi qi. (2  30). 其中 L 稱之為 Lagrange 乘子(Lagrange of system)，fi 為關節 qi 之作用力，i 為矩陣當中第幾項，K 為動能，P 為位能其定義如下所示. LK P. (2  31). 1 K  q T M (q)q   mi j (q) qi q j 2 i, j. (2  32). n. P  mi g T rci   mi g T rci. (2  33). i 1. (2-27)當中，n 為桿件數量，qij 表示 M(q)矩陣當中第 i 列與 j 行其中 n. . M (q)   mi J v (q)T J v (q)  J  (q)T Ri (q) I i Ri (q)T J  (q) i 1. i. i. i. i. . (2  34). 由(2-24)、(2-25)、(2-26)、(2-27)得到. d L d   d k j qj   d k j q j dt q k j j dt   d k j qj   j. i, j. d k j qi. (2  35). qi q j. 將(2-25)對 qk 偏微分. L 1 d k j P   qi q j  qk 2 i , j qk qk. (2  36). 結合(2-29)與(2-30)，(2-24)即為. 24.

(34) d. k j. j.  d 1 di j  P  qi q j  qj    k j   fk 2 qk  qk i , j  qi. (2  37 ). 為了能夠將階數互換， d   d k j 1 d i j  1  d d qi q j    k j  k i  i j qi q j   q 2 qk  q j qk  i , j  qi i, j 2  i.  .   c i j k q i q j. (2  38). i, j. 其中 d  1  d d ci j k :  k j  k i  i j  2  qi q j qk . (2  39). 接著我們定義. gk . P qk. (2  40). 將(2-31)改為 n. n. n.  d k j qj   ci j k qi q j  g k  f k j 1. k  1,..., n. (2  41). i 1 j 1. 將上式精簡化，最後得到運動方程式. (2  42).   C(q, q )q  g(q)  f M(q) q M11  2m3l4 s12 s3  m4l4 s3  2m4l4 lc5 s3  m4lc5 s3  I1 2. 2. 2. 2. 2. 2. 2. 2. 2. M12  s3 (2m3l4 s12  m4l4  2m4l4 lc5  m4lc5 ) 2. 2. 2. 2. 2. 2. M13  2m3l4 c12 s12 c3 s3 2. 2. 2. M 21  s3 (2m3l4 s12  m4l4  2m4l4 lc5  m4lc5 ) 2. 2. 2. 2. 2. 2. M 22  2m3l4 s12 s3  m4l4 s3  2m4l4 lc5 s3  m4lc5 s3  I 2 2. 2. 2. 2. 2. 2. 2. 2. 25. 2.

(35) M 23  2m3l4 c12 s12 c3 s3 2. 2. 2. M31  2m3l4 c12 s12 c3 s3 2. 2. 2. M32  2m3l4 c12 s12 c3 s3 2. 2. 2. M33  2m3l4 s12 s3  m4l4 s3  2m4l4 lc5 s3  m4lc5 s3  I 3 2. 2. 2. 2. 2. 2. 2. 2. 2. M 44  I 4 M14  M24  M34  M 41  M42  M43  0 C11  C12  C21  C22  s3 (3dq3l4 m3c3  2dq3l4 m4c3  2dq3lc5 m4c3 2. 2. 2.  2dq3l4 m3c12 c3  4dq3l4lc5m4c3  2dq1l4 m3c12s12s3  2dq2l4 m3c12s12s3 ) 2. 2. 2. 2. C31  dq3l4 m3 s12c12  dq2l4 m4 s3c3  dq1lc5 m4 s3c3  dq2lc5 m4 s3c3  dq1l4 m4 s3c3  dq1l4 lc5m4 s3c3 2. 2. 2. 2. 2. 2.  2dq2l4lc5m4 s3c3  2dq3l4 m3 s12c12c3  2dq1l4 m3c12c3 s3  2dq2l4 m3c12c3 s3 2. 2. 2. 2. C32  dq3l4 m3 s12c12  dq2l4 m4 s3c3  dq1lc5 m4 s3c3  dq2lc5 m4 s3c3  dq1l4 m4 s3c3  dq1l4 lc5m4 s3c3 2. 2. 2. 2. 2. 2.  2dq2l4lc5m4 s3c3  2dq3l4 m3 s12c12c3  2dq1l4 m3c12c3 s3  2dq2l4 m3c12c3 s3 2. 2. 2. 2. C33  dq3lc 4 m3 s3c3  dq2l4 m3 s2c2  dq1l4 m3 s12c12 2. 2. 2.  dq2lc5 m4 s3c3  2dq1l4 m3c12s12c3  2dq2l4 m3 s12c12c3  2dq3l4 m3c12 c3 s3 2. 2. 3. 2. C13  C23  l4 m3 s3 (dq1c3  dq2c3  2dq1c12 c3  2dq2c12 c3  2dq3c12s12s3 ) 2. 2. 2. C14  C24  C34  C41  C42  C43  C44  0 g3  gs3 (l4 m4  lc 4 m3  lc5m4 ). g1  g 2  g 4  0. 26. 3. 2. 2.

(36) 第三章系統控制器理論及設計 3.1 控制系統控制機械手臂系統需要考慮許多因素，特別是不同軸向之間非線性動態偶合現象、馬達因摩擦以及慣量產生之馬達死區、桿件撓性、負載變動、外部干擾訊號，所產生之非線性系統不確定量，影響機械手臂位置控制的精度、穩定性與輸出效能等等。因此我們考慮一個包含系統未知項和外部干擾訊號之非線性機械手臂方程式：. M (1  )(q)q(t )  C(1  )(q, q )q (t )  g (1  )(q)  u(t )  d (t ). (3  1) 其中.   n  1 分別為機械手臂關節之馬達角度、角速度以及角加速度之狀態 q 、 q 與 q. 向量。 M(q) n  n 為對稱正定之轉動慣量矩陣， C(q,q)q  n  1 為包含向心力、科氏力之向量， g(q) n  1 為重力產生之力矩向量， u(t) n  1 為系統控制量， d(t).  n  1 為外部干擾訊號，  指的是系統不確定項，重新整理 (3  1). M (q)q(t )  C(q, q )q (t )  g (q)  L(t )  u(t )  d (t ). (3  2). 其中 L(t)  n  1 為系統所有未知項之總和. (t )  C (q, q )q (t )  g (q)) L(t )  (M (q)q. (3  3). 將 (3  2) 整理. q  M 1 [Cq (t )  g (q)  u(t )  d (t )  L(t )]. (3  4). 本論文控制器設計之策略為： (1) 滑動模型控制(Sliding Mode Control) 利用滑動模型控制可變結構之特性與良好之匹配性，並且具有良好之強健性，而且具有良好強健性，因此適合用來消除系統模型之影響與抑制干擾訊號。 (2) 重複控制(Repetitive Control) 重複控制針對週期函數之訊號追蹤與抑制週期訊號具有良好的控制效能，因此用在抑制系統模型未知項之影響，在各週期間不斷補償模型參數。 27.

(37) 3.2 滑動模型控制滑動模型控制在 1950 年代由蘇聯一群學者及工程師將可變結構系統技術應用於馬達控制上，發現滑順模型(sliding mode)之系統行為。滑動模型控制因為其設計方式直觀，並且具備匹配式雜訊及可變結構控制之優勢，加上具有良好之強健性與良好的暫態響應，因此被廣泛地應用。滑動模型控制兩項優勢當中：匹配式雜訊係指雜訊與控制訊號之間的關係，當授控制系統的系統不確定量與外部干擾訊號等影響系統性能之雜訊能夠被控制訊號掌握並且消除，則可稱為匹配式雜訊；可變結構控制係指透過控制訊號使系統狀態迅速收斂，同時維持系統誤差在理想範圍內。. 圖 3-1 可變結構控制系統由可變結構控制之特性，當系統需擁有滑動模型控制，必須具備條件為二： (1)迫近條件(approaching condition)：存在一個滑動平面，使所有的軌跡於有限時間內接觸到此滑動平面，並且維持在此平面，軌跡不再離開此平面。 (2)滑動條件(sliding condition)：使軌跡進入滑動平面後，維持在定義的邊界範圍內，順此滑動平面滑向平衡點。本論文應用滑動模型控制可變結構之特性與良好的匹配式雜訊效能，作為主要控制器，用來消除系統模型之影響與抑制干擾訊號。. 28.

(38) 由 (3  4) 定義控制函數當中包含等效控制量(Nominal Control)、滑動模型控制 (Sliding Mode Control)、重複控制(Repetitive Control). u(t )  ueq  u SMC  u RC. (3  5). 滑動模型控制律目標為消除外部干擾訊號以及系統模型之影響，使系統能夠達成期望之穩態誤差。定義馬達角度誤差以及速度誤差方程式：. e  q  qd. e  q  q d. (3  6). q 為機械手臂實際角度， q d 是期望角度訊號，滑動平面可定義為：. s(t )  e(t )  ce(t ). (3  7). 其中 c  n  n 為正定對角矩陣，將系統方程式由(3-1)式用 s 表示 Ms  Cs  g  u  L(t )  M (qd  ce)  C(qd  ce). 欲達到控制目標，必須滿足下列逼近條件( Reaching Condition)： s(t )  s(t )  0 t  0. (3  8). 由 (3  8) 可以得到 s(t )  δSMC sgn(s(t)) t  0, δSMC  0. d(t)  δSMC. (3  9). 因此得到 ueq  M (qd  ce)  C (q d  ce)  g. (3  11). uSMC  δSMC sgn(s(t)). (3  12). 29.

(39) 圖 3-2 迫近條件、滑動條件與滑動平面之關係. 30.

(40) 3.3 重複控制理論重複控制在 1980 年代由一群日本學者[1]提出，基於內部模型理論，針對週期性重複動作不斷學習的控制法。重複控制是針對週期性訊號有效控制之代表性控制方法，針對週期性訊號有效追蹤或抑制，達成消除週期性響應誤差之控制目標。然而使用重複控制器於非週期性訊號輸入或負載變動時，會降低系統相對穩定度，同時影響系統效能。將此控制法應用於機械手臂非線性系統[5]，在控制系統當中，針對週期訊號，事先建立內部模型，並且在相對應之系統未知訊號進入時，立即予以抑制，此控制設計方法亦稱為適應前饋控制消除法，本論文採用此設計方式，針對機械手臂進行週期訊號追跡控制與週期干擾訊號抑制。由 (3  4) 與 (3  5) 可以知道，重複控制 u RC 之控制目標為消除系統未知項 L(t) 對系統效能之影響. u RC  (M (q)q(t )  C (q, q )q (t )  g (q)). (3  13). 定義參考角度與參考角速度. q r  q d   0 e( )d. (3  14). qr  qd  e(t ). (3  15). qr  qd  e(t ). (3  16). t. 其中  為正定常數，參考角度誤差與參考速度誤差分別如下所示. er  q  q r. er  q  qr. (3  17 ). 為讓控制律更加精簡與直觀，定義期望軌跡所需之狀態向量為.  (t ) : [qdT (t ), q dT (t ), qdT (t )]T. (3  18). 將定義之參考角度與參考角速度分別帶入 (3  13) ，整理後得到重複控制律 31.

(41) u RC  wˆ ( )  Fp e  Fv er (t )   e er (t ) 2. (3  19). ˆ ( ) 為系統估測函數， Fp 、 Fv 與  為正定常數。欲得到需要之角度位置訊其中 w 號以及角速度 q 與 q ，我們建立估測器估測此項參數。. qˆ p  qˆ v  (I  )q~p. (3  20). qˆ v  qd  q~p. (3  21). qˆ p 與 qˆ p 分別為角度估測量與角速度估測量，  為正定對角矩陣。其中角度誤差、角速度誤差分別定義為 q~p  q  qˆ ， q~ p  q  qˆ 建立參考位置估測誤差. eˆr  (q p  q d )   (q p  qd )  eˆ p  eˆ p. (3  22). 因此可以得到新的誤差訊號. z p  e p  q~p. (3  23). 當 w(t ) 為週期函數 w(t )  w(t  T ) ，利用傅立葉及數(Fourier Series)定義有限項函數 N. N. i 1. i 1. wr (t)  θ0   θi,c cos(iω0 t)   θi,s sin(iω0 t).   T (t)θ d. (3  24). 其中 0  2 / T 週期之頻率，T 為週期， θ0 、 θi,c 與 θi,s 是未知常系數。 (t) R 2N 1 2 N 1 為有限項之矩陣， θ d  R 為有限項之未知參數，分別如下所示.  (t )  [1 cos(0t ) sin(0t ) ... cos(N0t ) sin(N0t )]T. (3  25). θ d  [θ0. (3  26). θ1,c. θ1, s. ... θ N,c. θ N, s ]T. 32.

(42) 有限項級數趨近法之結果若要更精確，可藉由 N 之增加使得 wr (t) 更為精確。將 (3  20) 至 (3  26) 定義之參數，整理後帶入 (3  19) ，新的重複控制律為. u RC  wˆ ( )  Fp e  Fv eˆr   ( ) T ( ) z p. (3  27 ). 將等效控制量 (3  11)、滑動模型控制量 (3  12)、重複控制量 (3  27 ) 帶入 (3  5) 即為重複滑動控制之控制量。. 33.

(43) 3.4 Lyapunov 穩定性理論 Lyapunov 穩定性理論之概念來自於 Aleksandr Mikhailovich Lyapunov，起初用來描述動力學上運動穩定性之分析。若有一系統平衡點 xe = 0，則可用 Lyapunov 分析其穩定性，當系統軌跡均能夠維持在平衡點 xe 附近，並且至少漸進穩定狀態，即可稱此系統在 xe 屬於 Lyapunov 穩定。Lyapunov 穩定性理論用以判定動態系統之平衡點 xe = 0 穩定性，其定義分別描述於下： . 定理一：. x  0 為非線性系統 x  f (x) 之平衡點，其中 x  x(t ) ， t  0 ，若能夠找到適當定義之 K 系列 V ( x)  0 與 V 滿足： V V V  x  f (x) x x  V   x1. .... V     f1 ( x) ... xn . fn ( x)  0 T. (3  28). 使 x(t ) 在 t  0 都保持在所定義之邊界，則稱此系統在 x  0 為漸進穩定 (Asymptotically Stable)或邊界穩定(Marginally Stable)。 . 定理二：. x  0 為非線性系統 x  f (x) 之平衡點，其中 x  x(t ) ， t  0 ，若能夠找到正 V 定 V ( x)  0 ，且 V ( x)  f ( x)  0 為負定，則稱此系統在 x  0 為漸進穩定， x 亦即 lim x(t )  0 。 V  0 & V  0  V & x  0 t . . 定理三： x  0 為非線性系統 x  f (x) 之平衡點，其中 x  x(t ) ， t  0 ，若能夠找到正定 V (x) 滿足： 1.. 1 x  V ( x)   2 x p. p. V p V ( x)  f ( x)   2 x  0 x 則稱此系統在 x  0 為指數穩定(Exponantially Stable)。. 2.. 34.

(44) . 定理四：(LaSalle 定理). . . 若系統 x  f (x) 存在 V ( x)  0 & V ( x)  0 ，則 lim x(t )  x  n V ( x)  0 t . 配合觀察器(Observable)確認 x  0 。本篇論文利用 Lyapunov 進行重複滑動模型控制系統之穩定度分析，選擇 Lyapunov 函數如下： 1 V  s T M s  V1  V2 (3  29) 2 其中 S 如 (3  7) 所示，而 1 1 V1  {e T ( Fp  2  ( M  m )e 2 2.  q~pT Qq~p  erT M (q)er  q~rT M (q)q~r } 1 ~ V2  {1 / 2 ( (t )){e  q~ p }   1 / 2 }2 2. (3  30) (3  31). 其中 Q 為正定矩陣，  M 、  m 為 M (q) 特徵值之最大值與最小值。將 (3  29) 微分. V  s T M s  V1  V2. (3  32). 分別將 (3  30) 與 (3  31) 微分，結合滑動模型控制量 (3  12) 與重複控制量 (3  27 ) ，帶入至 (3  32). V  s T M s  V1  V2  s T (u SMC  u RC  L(t ))  V1  V2  s T (u SMC )  s T  SMC sgn(s(t ))  0. (3  33). 由 (3  33) 可看出，第二項與第三項分別被重複控制律消除。Lyapunov 穩定性分析之定理一與定理二， (3  29) 與 (3  33) 滿足 V  0 、V  0 ，證明系統狀態會收斂至滑動平面上。. 35.

(45) 第四章實驗設備在本章節中，將介紹實驗機台及周邊設備-其中將分別介紹 AI 直流伺服馬達、驅動器、資料擷取卡及人機介面程式軟體。整體實驗機台系統架構方塊圖如圖 4-1 所示. 圖 4-1 實驗整體系統方塊圖在圖 4-1 中，透過電腦送出控制訊號透過 PXI6230 給予 H 橋 PWM 命令與接收馬達回授之角度，在電腦端進行數學演算後將數位控制命令傳送給資料擷取卡 (DAQ)，並且將數位訊號轉換為類比訊號，透過訊號放大器將電壓轉換並進行馬達控制。其中，微電腦控制器(MCU)包含了邏輯運、記憶體(RAM)、內建記憶體 (ROM)、輸入/輸出(I/O)、數位類比轉換(A/D、D/A)等關於記憶與運算功能結合在一起。透過 MCU 送出 PWM 訊號給 H-Bridge 用來控制馬達轉向，再將訊號傳送給馬達。受控端部分，馬達接收到訊號後轉動，編碼器包含可變電阻將馬達轉動位置回傳。本研究以 PXI6230 扮演 MCU 及其周邊之腳色。將電腦端訊號傳給 H 橋，處理數位轉類比、類比轉數位之變換等微電腦控制器之功能。特別的是，欲消除馬達本身控制晶片內控制演算法之影響，我們利用將馬達編碼器直接讀取回來，利用電腦擔任計算的腳色，取代原本控制晶片的功能。接下來將逐一介紹本研究所使用之設備及其特色。. 36.

(46) 4.1 AI 直流伺服馬達本研究所使用之馬達為 ROBOTIS 公司之產品，型號為 Dynamicxel 系列之 AI 直流伺服馬達。此伺服馬達內部整合多項功能於其中，例如控制器、驅動器、減速機構與主機連線功能，型號越大之馬達其輸出扭力越大，其空量與負載電壓也越大。因此在本研究當中，依照各軸關節之需求不同，總共使用了三種不同型號之伺服馬達，分別為 RX-64、RX-28 以及 RX-10，如圖 4-2 所示。. 圖 4-2 RX-Series 馬達馬達配置如下：馬達配置為第 0 軸 yaw 軸轉動之關節使用 1 顆 RX-64 馬達，第 1 軸使用 2 顆 RX-64 之馬達，此二軸於機械手臂之基座端，承受較大之力矩與機構慣性之影響，因此使用輸出扭力較大之 RX-64 馬達。在第 3 軸與第 4 軸分別使用 1 顆 RX-28 馬達。最後在機械手臂末端夾爪部分使用兩顆 RX-10 馬達。其配置圖如圖 4-3 所示，馬達規格如表 4-1 所示。 37.

(47) 透過第二章機器人學觀點，將機械手臂簡化，如圖 4-3 所示. 圖 4-3 機械手臂架構表 4-1AI 伺服馬達規格表馬達名稱. RX-10. RX-28. RX-64. 重量(g). 64.5. 72. 125. 尺寸(mm). 35.6  50.6  25.5. 35.6  50.6  25.5. 40.2  61.1 41.0. 齒輪比. 193:1. 193:1. 200:1. 使用電壓(V). 12. 18.5. 18.5. 最大扭矩 (kgf  cm). 13. 37. 52. 無負載速度(rpm). 54. 85. 64. 解析度. 0.29∘( 300∘/ 1024 ). 38.

(48) Dynamixel 伺服馬達包含四組角位，PIN 1 為接地端，PIN 2 為電源輸入端， PIN 3 與 PIN 4 分別負責傳送馬達腳位移與角速度之訊號。而相對的兩個伺服馬達之間，其角位上下相反，如圖 4-4 所示。. 圖 4-4 AI 伺服馬達角位示意圖本研究利用已知電路之微電腦控制器取代馬達內部控制器，並將馬達內部用來讀取角度值訊號之編碼器線路腳位與已知電路之控制器連接。馬達編碼器腳位如圖 4-5 所示。. 圖 4-5 AI 直流伺服馬達控制器與編碼器腳位圖 39.

(49) 4.2 PXI 6230 DAQ 卡我們利用 PXI 集合資料擷取卡，一方面接收馬達回授，一方面將電腦端控制演算法計算結果，經 PWM 轉換後最後由此資料擷取卡輸出。. 圖 4-6 NI PXI-6230 表 4-2 NI PXI-6230 規格 Analog Input Channels. 8. Max AI Range. ±10 V. Analog Output Channels. 4. Max AO Range. ±10V. Digital Inputs. 6. Digital Outputs. 4. Digital Voltage. 5 V TTL/CMOS. Max DO Current Drive. 10 mA. 40.

(50) 4.3 H-Bridge 本研究中使用 H-Bridge 作為馬達正反轉之驅動，製造商為日本 TOSHIBA 公司，型號 TA7291P，其外觀與方塊圖以及腳位如圖 4-7、圖 4-8 與表 4-3 所示. 圖 4-7 H-Bridge 外觀圖. 圖 4-8 H-Bridge 電路圖表 4-3 TA7291P 腳位功能 Pin No. 7 8 4 1 5 6 2 10. Symble Vcc Vs Vref GND IN1 IN2 OUT1 OUT2. Function Description Supply voltage terminal for logic Supply voltage terminal for motor driver Supply voltage terminal for control GND terminal Input terminal Input terminal Output terminal Output terminal 41.

(51) 4-4 人機介面及圖形化程式軟體本研究中所使用的人機介面軟體為 LabVIEW_2010 版。此人機介面是一種圖型化的程式語言，也可稱為 G 程式語言。有別於傳統的文字編輯程式語言操作例如 C 語言，圖形化編輯方式能更直觀地進行程式編寫，因而消除了許多傳統程式語言語法上的細節問題。此套程式軟體是特別針對儀器資料擷取、數據量測、資料分析、系統模擬及結果呈現等目標而設計，因此非常適合應用於自動化之相關設備。且 LabVIEW 支援之作業軟體相當廣泛，其中包含 PC Windows 作業系統、 Mac OS 作業系統、Sun SPARCstation 作業系統及 HP-UX HP9000/700 系列之作業系統皆提供支援服務。此外，我們也利用 Matlab. 42.

(52) 第五章實驗結果本研究之目的為設計重複滑動模型控制器應用於機械手臂。藉由設計控制器，抑制系統模型影響、干擾訊號、外部負載以及其他系統未知影響，以提高機械手臂追跡控制之控制精密度。本章節可下列四個部分進行。第一部分：RSMC 於定位控制第二部分：RSMC 於追跡控制第三部分：RSMC 對非週期訊號強健性測試第四部份：RSMC 末端點控制硬體部分，我們使用自行設計之機手臂，搭配 Robotis 馬達，但不包含內建控制晶片，由電腦進行運算透過 PXI 6230 讀取類比訊號以及傳送馬達 PWM 訊號，如圖 5-1 所示。軟體部分，使用 NI 美商國家儀器公司產品 LabVIEW 2010 進行人機介面的開發，如圖 5-2 所示，並且使用美國 Math Works 公司的產品 MATLAB R2010a 輸出實驗數據，如圖 5-3 所示。. 圖 5-1 控制程序. 43.

(53) 圖 5-2 LabVIEW2010 程式啟動頁面. 圖 5-3 MATLAB 程式啟動頁面. 44.

(54) 5-1. 實驗結果 . 實驗結果 1- RSMC 定位控制. 本實驗當中給予機械手臂第一軸與第二軸位置命令，觀察定位與誤差性能 5 4.5 4. joint1 position(rad). 3.5 3 2.5 2 1.5 1 0.5 0. 1. 2. 3. 4. 5 6 time(second). 7. 8. 9. 10. 8. 9. 10. 圖 5-4 RSMC 於 q1 定位控制 3.5. joint2 position(rad). 3. 2.5. 2. 1.5. 1. 1. 2. 3. 4. 5 6 time(second). 7. 圖 5-5 RSMC 於 q2 定位控制 45.

(55) -3. x 10 5 4.5. joint1 error(rad). 4 3.5 3 2.5 2 1.5 1 0.5 0. 1. 2. 3. 4. 5 6 time(second). 7. 8. 9. 10. 9. 10. 圖 5-6 RSMC 於 q1 定位控制穩態誤差 0.025. joint2 error(rad). 0.02. 0.015. 0.01. 0.005. 0. 1. 2. 3. 4. 5 6 time(second). 7. 8. 圖 5-7 RSMC 於 q2 定位控制穩態誤差. 46.

(56) . 實驗結果 2 – RSMC 動態軌跡追蹤. 本實驗當中給予機械手臂第一軸與第二軸弦波命令，觀察定位與誤差性能 6 real joint 1 desire joint 1. 5.5 5. 4 3.5 3 2.5 2 1.5 1. 0. 1. 2. 3. 4. 5 6 time(second). 7. 8. 9. 10. 圖 5-8 RSMC 於 q1 追跡控制 6 real joint 2 desire joint 2 5. joint2 position(rad). joint1 position(rad). 4.5. 4. 3. 2. 1. 0. 0. 5. 10 15 time(second). 圖 5-9 RSMC 於 q2 追跡控制. 47. 20. 25.

(57) 0.04 0.035. joint1 error(rad). 0.03 0.025 0.02 0.015 0.01 0.005 0. 0. 1. 2. 3. 4. 5 6 time(second). 7. 8. 9. 10. 圖 5-10 RSMC 於 q1 追跡控制誤差. 0.07. 0.06. joint2 error(rad). 0.05. 0.04. 0.03. 0.02. 0.01. 0. 0. 5. 10 15 time(seconds). 20. 圖 5-11 RSMC 於 q1 追跡控制誤差. 48. 25.

(58) 實驗結果 3 - RSMC 非週期之干擾訊號測試在本實驗當中，我們在機械手臂四個關節完成定位控制之控制目標時，推動機械手臂作為外部非週期干擾訊號，觀察系統對於非週期訊號之強健性。 0.3. 0.25. joint1 position (radian). 0.2. 0.15. 0.1. 0.05. 0. -0.05. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 3.5. 圖 5-12 加入外部非週期干擾訊號， q1 動態軌跡響應 50. 0. -50. joint4 control valume. . -100. -150. -200. -250. -300. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 圖 5-13 加入外部非週期干擾訊號， q1 控制量 49. 3.5.

(59) 0.1. 0. joint2 position (radian). -0.1. -0.2. -0.3. -0.4. -0.5. -0.6. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 3.5. 圖 5-14 加入外部非週期干擾訊號， q2 動態軌跡響應 500. 0. joint2 control valume. -500. -1000. -1500. -2000. -2500. -3000. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 圖 5-15 加入外部非週期干擾訊號， q2 控制量. 50. 3.5.

(60) 0 -0.05. joint3 position (radian). -0.1 -0.15 -0.2 -0.25 -0.3 -0.35 -0.4 -0.45 -0.5. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 3.5. 圖 5-16 加入外部非週期干擾訊號， q3 動態軌跡響應 300. 250. joint4 control valume. 200. 150. 100. 50. 0. -50. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 圖 5-17 加入外部非週期干擾訊號， q3 控制量. 51. 3.5.

(61) 0.25. 0.2. joint4 position (radian). 0.15. 0.1. 0.05. 0. -0.05. -0.1. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 3.5. 圖 5-18 加入外部非週期干擾訊號， q 4 動態軌跡響應 50. joint3 control valume. 0. -50. -100. -150. -200. 0. 0.5. 1. 1.5 2 time (second). 2.5. 3. 圖 5-19 加入外部非週期干擾訊號， q 4 控制量. 52. 3.5.

(62) . 實驗結果 4 - RSMC 非週期之干擾訊號測試在此實驗當中，我們給予機械手臂末端點兩個位置命令，使機械手臂在兩點. 進行來回定位控制，並且觀察其控制效能。. 圖 5-20 機械手臂末端點定位 . 實驗結果 5 – RSMC 末端點控制本實驗給予機械手臂末端點位置， x  0.295385 y  0.12 * s i n 1(* pi * t ). z  0.12 * cos( 1* pi * t )  0.18 ，由第二章第三節逆向運動學推導 1  tan 1 y / x.  2  tan 1[( z  d1 ) / x 2  y 2 ] 其中 d1 為機械手臂桿件.  2 ，因此得到關節. 1、關節 2 之參考訊號。. 53. ，. 1 之長度，將 x、y、z 分別帶入 1 、.

(63) 圖 5-21 機械手臂畫圓實驗結果綜合本研究，重複滑動模型控制具備滑動模型良好的強健性，並且針對週期訊號也能夠有效抑制。此外重複滑動模型控制能夠在干擾訊號為非週期時，仍然保有強健性。由實驗 1 我們觀察到，機械手臂在一秒內即完成定位，第一軸保持在 0.005 徑度，第二軸雖然具有較大負載，仍保持在 0.02 徑度。實驗 2 當中，給予機械手臂第一關節與第二關節弦波訊號，在此追跡實驗當中，訊號不斷變化，控制器需提供相當反應速度與控制量。由圖 5-8 與圖 5-9 可觀察到，第二軸相對第一軸追跡效能較差，再由圖 5-10 與圖 5-11 可更清出看出，第一軸誤差較第二軸小之外，第二軸因為負載較大因此在移動時有相當程度地晃動。在實驗 3，我們在機械手臂四個關節完成定位控制之控制目標時，推動機械手臂作為外部非週期干擾訊號，觀察系統對於非週期訊號之強健性。在此實驗當中，外部干擾訊號持續並且變化量大，控制器必須及時提供足夠大的力量抑制干擾訊號，當控制量越大，機械手臂晃動的跡象越是明顯。各關節施予非週期干擾訊號時，控制器皆有效的抑制干擾，使手臂回到定位點。實驗 4 我們利用機械手臂末端點控制進行畫圓，量測結果最大誤差為 0.5 釐米。. 54.

(64) 第六章結論及未來展望首先我們在第二章，進行了機械手臂運動學分析以及動力學模型之建立，以幫助我們在控制器的設計中，能夠運用理論進行驗證控制命令給予馬達完成控制目標。接著我們設計了重複控制與滑動模型控制，進行四軸機械手臂之位置控制，利用滑動模型控制強健之特性，做為主要控制器，進行干擾訊號與系統模型影響的抑制，結合重複控制針對週期訊號良好的控制效能，改善控制效能。最後我們設計開發控制器電路，利用電腦進行邏輯運算並且利用 PXI6230 進行訊號接收與傳送，取代一般伺服馬達中的控制晶片。我們成功利用電腦配合資料擷取卡取代馬達內部晶片控制馬達，使自行研究的控制演算法能夠避開控制晶片內部控制器的影響，直接控制馬達。由第五章實驗結果，滑動模型控制結合重複控制，在定位控制與週期訊號追蹤控制時，皆能保持良好的系統控制效能，並且在非週期外部干擾訊號加入時，仍然能夠保有相當的強健性能。未來期待能夠透過加入其他控制演算法，提升控制效能。並且導入其他 PWM 演算法，使訊號輸出更迅速減緩機械手臂晃動問題。最後期望藉由本研究的拋磚引玉，能引起更多學者對於機械手臂的興趣，以更多、更廣、更深的知識理論，提升機械手臂的精密定位響應。藉以提升機械手臂控制對於全人類業界與生活上之便利性。. 55.