機器人控制與錯誤診斷系統

94  Download (0)

全文

(1)

國 立 交 通 大 學

電機與控制工程學系

碩士論文

機器人控制與錯誤診斷系統

Control and Actuator Fault Diagnosis of Robot Manipulators

 

 

 

研 究 生:翁茂樵

指導教授:蕭得聖 博士

中華民國九十八年七月

(2)

機器人控制與錯誤診斷系統

Control and Actuator Fault Diagnosis of Robot Manipulators 

 

研 究 生:翁茂樵

Student:Mao-Chiao Weng

指導教授:蕭得聖 博士

Advisor:Dr. Te-Sheng Hsiao

 

 

國立交通大學 電機與控制工程學系 碩士論文 A Thesis

Submitted to Department of Electrical and Control Engineering College of Electrical Engineering

National Chiao Tung University in partial Fulfillment of the Requirements

for the Degree of Master

In

Electrical and Control Engineering July 2009

Hsinchu, Taiwan, Republic of China

(3)

機器人控制與錯誤診斷系統

學生:翁茂樵   

 

 

指導教授:蕭得聖博士 

 

國立交通大學電機與控制工程學系碩士班 

摘 要

本研究在一個二軸平面的機械手臂實驗平台上,進行控制器與錯誤診斷的整合 測試。在控制器的部分,使用非追蹤型卡曼濾波器(UKF)進行角速度估測,將此估測 與透過差分所得到的角速度,分別代入計算扭力控制器演算法觀察其效果差異。此 外將提出一個僅需位置回授的新型控制器與上述兩個控制器一起比較。在錯誤偵測 與判別中,實現二階廣義偽貝氏演算法(GPB-2)及交互多模型(IMM)錯誤演算法的測 試,其中所使用之估測器為控制器部分的UKF 估測器。由實驗結果可以看出,僅需 位置回授的新型控制器在只有位置回授的情形下,效能更勝傳統的計算扭力控制器, 而錯誤偵測與判別系統也能夠在系統發生錯誤後的極短時間內,告知使用者系統已 經發生錯誤,並判斷出錯誤發生的位置,不僅增加了安全性,也降低硬體成本,增 加未來智慧型機器人能夠同時擁有安全與效能兼備的可能性。

(4)

Control and Actuator Fault Diagnosis of Robot Manipulators

Student:Mao-Chiao Weng

Advisor:Dr. Te-Sheng Hsiao

Department of Electrical and Control Engineering

National Chiao Tung University

ABSTRACT 

This study experimentally evaluates the performance of various kinds of controllers and actuator fault diagnosis system on a two-joint robotic manipulator. On the part of con-trollers, we use UKF(Unscented Kalman Filter) to estimate (angular) velocities, and com-pare the performance of the computed-torque controllers with velocities estimated by UKF and backward difference filters respectively. Moreover, we propose a new control method which only requires position feedback. On the part of actuator fault diagnosis system, we combine UKF , GPB-2(General Pseudo Bayesian of Order 2) and IMM(Interacting Mul-tiple Model) algorithms to detect and identify actuator failures of the robotic manipulator. The experimental results show that the new control method has better performance than other controllers. The results also show that the fault diagnosis system can correctly detect and isolate actuator failures in a short time.

(5)

致謝

首先誠摯的感謝指導教授蕭得聖博士,老師悉心的教導以及對我研究能力的信任,讓我 能夠順利的在實驗平台上實現控制理論及錯誤診斷演算法,不僅在理論上有更清楚的認識, 在實作方面的經驗也獲益匪淺,可謂獲益良多。 本論文的完成另外亦得感謝我的口試委員林源倍教授及徐保羅教授。因為有各位老師在 口試中給的建議及方向,使得本論文能夠更完整而嚴謹。 兩年裡的日子,感謝暉鈞、宗明、毅鴻、永洲學長不厭其煩的指出我研究中的缺失,且 總能在我迷惘時為我解惑,也感謝東新同學的共同砥礪。念冀及志誠學弟的加油與鼓勵,也 讓我備感溫馨。 女友亞儒在背後的默默支持更是我前進的動力,沒有亞儒的體諒、包容,相信這兩年的 生活將是很不一樣的光景。 最後,要感謝我摯愛的家人,謝謝父母多年來的養育與栽培,有您們在生活上的精神及 支持,我才能在求學階段無任何後顧之憂。當然,也要感謝我的妹妹逸芃,每周末晚上幫我 煮的溫馨泡麵,也增加了我熬夜研究的動力。

(6)

目錄

摘 要 ... I ABSTRACT ... II 目錄 ... IV 圖目錄 ... VI 表目錄 ... IX 第一章 緒論 ... 1 1.1 研究動機與目的 ... 1 1.2 論文貢獻 ... 2 1.3 論文架構 ... 2 第二章 相關研究 ... 3 2.1 機器手臂控制相關研究 ... 3 2.2 機器人錯誤偵測與判別相關研究 ... 4 第三章 機械手臂動態模型與控制器設計 ... 6 3.1 機械手臂的動力學模型 ... 6 3.2 計算力矩控制器 ... 10 3.3 觀察器設計 ... 11 3.4 僅需位置回授的新型控制器 ... 15 3.4.1 線性控制器的設計 ... 17 3.4.2 非線性控制器的設計 ... 20 第四章 錯誤偵測與判別技術 ... 25 4.1 以模型為基礎的錯誤偵測技術 ... 25 4.2 多模型架構 ... 27 4.2.1 廣義偽貝氏法 ... 31 4.2.2 交互多模型法 ... 34 4.3 階層式錯誤偵測與判別系統 ... 37 第五章 實驗結果與討論 ... 40 5.1 機械手臂硬體實驗平台 ... 40 5.2 控制器實驗結果 ... 45 5.2.1 UKF 估測器效果測試 ... 45 5.2.2 計算力矩控制器實驗結果 ... 48 5.2.3 僅需位置回授的新型控制器實驗結果 ... 55 5.2.4 性能分析與比較 ... 59

(7)

5.2.5 強健性測試 ... 61 5.3 錯誤偵測實驗結果 ... 65 5.4 錯誤判別實驗結果 ... 69 第六章 結論及未來工作 ... 80 6.1 結論 ... 80 6.2 未來工作 ... 81 參考文獻 ... 82

(8)

圖目錄

圖 1 兩軸平面機械手臂示意圖 ... 7 圖 2 直流馬達示意圖 ... 8 圖 3 計算力矩控制器系統方塊圖 ... 10 圖 4UKF 與 EKF 效能比較圖 ... 12 圖 5UKF 流程圖 ... 14 圖 6 差分角速度與命令軌跡,下圖為局部放大圖 ... 15 圖 7 不需要速度估測的控制器方塊圖 ... 16 圖 8 增大的受控廠 ... 18 圖 9 MODEL-BASED錯誤偵測系統架構圖 ... 26 圖 10 靜態多重模型估測流程圖[7] ... 28 圖 11GPB-2 流程圖[7] ... 33 圖 12GPB-1、GPB-2 及 IMM 的估測性能比較圖[8] ... 34 圖 13IMM 流程圖[7] ... 36 圖 14 階層式錯誤偵測與診斷系統流程圖 ... 39 圖 15 二軸平面機械手臂 ... 40 圖 16 二軸機械臂實驗平台硬體方塊圖 ... 41 圖 17 大軸角速度比較圖(軌跡 1) ... 46 圖 18 小軸角速度比較圖(軌跡 1) ... 46 圖 19 大軸角速度比較圖(軌跡 2) ... 47 圖 20 小軸角速度比較圖(軌跡 2) ... 47 圖 21 計算力矩控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 1 差分) ... 48 圖 22 計算力矩控制器角度追蹤誤差(軌跡 1 差分) ... 49 圖 23 計算力矩控制器兩軸輸入電壓(軌跡 1 差分) ... 49 圖 24 計算力矩控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 1UKF) ... 50

(9)

圖 25 計算力矩控制器角度追蹤誤差(軌跡 1UKFV.S 差分) ... 50 圖 26 計算力矩控制器兩軸輸入電壓(軌跡 1UKF) ... 51 圖 27 計算力矩控制器,10 秒前使用差分增益,10 秒後使用 UKF 增益(軌跡 1 差分) ... 51 圖 28 計算力矩控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 2 差分) ... 52 圖 29 計算力矩控制器角度追蹤誤差(軌跡 2 差分) ... 52 圖 30 計算力矩控制器兩軸輸入電壓(軌跡 2 差分) ... 53 圖 31 計算力矩控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 2UKF) ... 53 圖 32 計算力矩控制器角度追蹤誤差(軌跡 2UKFV.S 差分) ... 54 圖 33 計算力矩控制器兩軸輸入電壓(軌跡 2UKF) ... 54 圖 34 非線性控制器開啟前及開啟後的比較 ... 55 圖 35 僅需位置回授的新型控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 1)56 圖 36 僅需位置回授的新型控制器角度追蹤誤差(軌跡 1) ... 56 圖 37 僅需位置回授的新型控制器兩軸輸入電壓(軌跡 1) ... 57 圖 38 僅需位置回授的新型控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 2)57 圖 39 僅需位置回授的新型控制器角度追蹤誤差(軌跡 2) ... 58 圖 40 僅需位置回授的新型控制器兩軸輸入電壓(軌跡 2) ... 58 圖 41 計算力矩 (差分及 UKF)與僅需位置回授控制器的角度追蹤誤差比較圖(軌跡 1) ... 59 圖 42 角度追蹤誤差範數(1-NORM)比較圖(軌跡 1) ... 60 圖 43 計算力矩 (差分及 UKF)與僅需位置回授控制器的角度追蹤誤差比較圖(軌跡 2) ... 60 圖 44 角度追蹤誤差範數(1-NORM)比較圖(軌跡 2) ... 61 圖 45 計算力矩 (差分)強健性測試追蹤誤差比較圖 ... 62 圖 46 計算力矩 (差分)強健性測試角度追蹤誤差範數(1-NORM)比較圖 ... 62 圖 47 計算力矩 (UKF)強健性測試追蹤誤差比較圖 ... 63 圖 48 計算力矩 (UKF)強健性測試角度追蹤誤差範數(1-NORM)比較圖 ... 63 圖 49 僅需位置回授控制器的強健性測試角度追蹤誤差比較圖 ... 64 圖 50 僅需位置回授控制器的強健性測試角度追蹤誤差範數(1-NORM)比較圖 ... 64 圖 51 實驗1 的角度、角速度估測及輸出電壓 ... 66

(10)

圖 52 實驗 1 動力學模型與運動學模型機率估測(模型 1 及模型 4) ... 67 圖 53 實驗 2 的角度、角速度估測及輸出電壓 ... 67 圖 54 實驗2 動力學模型與運動學模型機率估測(模型 1 及模型 4) ... 68 圖 55 實驗 3 的角度、角速度估測及輸出電壓 ... 68 圖 56 實驗 3 動力學模型與運動學模型機率估測(模型 1 及模型 4) ... 69 圖 57 實驗 4 的角度、角速度估測及輸出電壓 ... 70 圖 58 實驗 4 各軸發生錯誤之機率估測 ... 71 圖 59 實驗 5 的角度、角速度估測及輸出電壓 ... 72 圖 60 實驗 5 各軸發生錯誤之機率估測 ... 72 圖 61 實驗 6 的角度、角速度估測及輸出電壓 ... 73 圖 62 實驗 6 各軸發生錯誤之機率估測 ... 74 圖 63 實驗 7 的角度、角速度估測及輸出電壓 ... 75 圖 64 實驗 7 各軸發生錯誤之機率估測 ... 75 圖 65 實驗 8 的角度、角速度估測及輸出電壓 ... 76 圖 66 實驗 8 各軸發生錯誤之機率估測 ... 77 圖 67 實驗 9 的角度、角速度估測及輸出電壓 ... 78 圖 68 實驗9 各軸發生錯誤之機率估測及衰減曲線 ... 78

(11)

表目錄

表 1 機械手臂硬體參數 ... 41 表 2 馬達及減速機構參數 ... 41 表 3 參數單位對應表 ... 43 表 4 模型參數表 ... 44 表 5 僅需位置回授的新型控制器設計參數表 ... 55 表 6 大軸在不同控制器下的強健性比較 ... 65 表 7 小軸在不同控制器下的強健性比較 ... 65 表 8 系統錯誤種類表 ... 66 表 9 錯誤偵測實驗結果列表 ... 79 表 10 錯誤判別實驗結果列表 ... 79

(12)

第一章

緒論

1.1

研究動機與目的

智慧型機器人自1970 年起,開始大量運用於工業製造生產上,例如汽車、機械 與半導體等產業,歷經數十年的科技躍進,與智慧型機器人相關的感測、認知、控 制與通訊技術愈趨成熟,以往只能存在於電影中,能夠與人類互動的智慧型機器人 將不再只是一個夢想。 在機器人運動控制的設計與分析領域裡,也有許多優秀的控制理論上被提出。 然而在大部分的控制演算法中,速度被都假設為可量測的,但實際上,量測速度的 轉速計(tachometer)通常帶有比較大的雜訊,並且也會增加機器人的成本。因此,如 何在沒有速度的資訊下維持控制的效能是一個值得研究的問題。 此外,在著重研究於機器人控制與功能設計的同時,可靠度與安全性也必須納 入設計的考量中。可靠度是指機器人自身能夠長期於妥善的狀況下運作,而安全性 乃指機器人運作時不應對人類以及週遭環境造成傷害。這往往是最容易被忽略的部 份,但卻是智慧型機器人能否讓人們安心的與它們互動及商品化的重要關鍵,因為 一個不安全不可靠的產品絕對不可能為公司帶來利潤,反而是一種累贅與商譽的損 壞。 本論文的目的就是要透過一個已建立之二軸平面機械手臂實驗平台,來驗證已 知的控制法則,並設計一個僅需位置回授的新型控制器與已知的控制法則作比較。 除此之外還會加入機器人錯誤偵測與識別(Fault Detection and Identification)的能力, 利用事先建立好的各種系統錯誤模型及適當的估測器,依據角度量測資訊及控制器 的扭力命令去估測各種系統錯誤模型應有的輸出及狀態,藉此去計算各模型下機率 來進行比較,判斷出錯誤是否發生以及其發生錯誤的種類。

(13)

1.2

論文貢獻

本研究在一個二軸平面的機械手臂實驗平台上,進行控制器與錯誤偵測與診斷 的整合測試。在控制器的部分,實現非追蹤型卡曼濾波器(UKF)進行角速度估測,將 此估測與透過差分所得到的角速度,分別代入計算扭力控制器演算法觀察其效果差 異。此外將提出一個僅需位置回授的新型控制器與上述兩個控制器一起比較。在錯 誤偵測與判別中,實現廣義偽貝氏演算法(General Pseudo Bayesian of Order 2,簡稱 GPB-2)以及交互多模型法(Interacting Multiple Model,簡稱 IMM) 的錯誤演算法測試,

其中所使用的估測器為控制器部分所使用的UKF 估測器。由第五章中可以看出,僅 需位置回授的新型控制器在只有位置回授的情形下,效能更勝傳統的計算扭力控制 器,而錯誤偵測與判別系統也能夠在系統發生錯誤後的極短時間內,告知使用者系 統已經發生錯誤,並判斷出錯誤發生的位置。

1.3

論文架構

本篇論文共分為六章:第一章為緒論,主要說明研究動機與目的;第二章介紹 機器手臂的控制理論與錯誤偵測及識別的相關文獻;第三章介紹控機械手臂的動態 模型、UKF 估測器、控制器的設計及其理論推導。第四章將介紹觀察器及錯誤偵測 與判別技術,針對IMM、GPB-2、錯誤模型等作進一步的介紹。第五章將之前的控 制理論和錯誤偵測與判別系統,放置於二軸機械臂的實驗平台上進行實驗,探討不 同控制器的結果及錯誤判別與診斷的效能;最後,第六章對兩年來的研究做一結論 並展望未來可以進行的研究方向。

(14)

第二章

相關研究

2.1

機器手臂控制相關研究

在J. J. Craig[1]與 R. Kellly[2]的著作中,針對機械手臂的控制方法採用計算力矩 的控制器。這個方法牽涉非線性項的計算和消除,化簡系統成為一個線性的系統, 以下簡述之。考慮動力學方程式 = + + + τ M(q)q C(q,q)q G(q) B(q)    (1) 計算力矩的控制法則給定如下: d v p = + + + + + τ M(q)[q K q K q] C(q,q)q G(q) B(q)     (2) 其中KvK 為對稱正定的矩陣,p q~=qdq表示角度的誤差。將(1)式帶入(2)式可得 (3),適當的選擇KvK 這兩個增益值,調整控制的效能。 p d + v + p = q K q K q 0    (3) J. J. E. Slotine 在[9]使用各軸追蹤運動的追縱誤差去驅動參數的適應,而且證明 了適應的前饋控制加比例微分控制器的整體追蹤收斂性,它不需要加速度的量測, 並利用電腦模擬與實際實驗其可行性。適應性控制器在參數的不確定之下,能夠達 到整體誤差收斂。它能夠依據給定的適應性參數估測法則,得到參數,對無法準確 知道動力學模型的參數是一個可行的辦法。 H. Berghuis 在[12]中使用各軸的追縱誤差及各軸濾波後力矩的預測誤差去驅動 參數的適應,接著建立以模型為基礎的控制器。其證明此方式比傳統的比例微分控 制器有較好的效能。 A. Tayebi 在[14]使用Lyapunov-like 函式,並針對機械手臂軌跡追蹤的問題設計 反覆學習適應性控制演算法,此方法不需要加速度的量測資訊及系統未知參數的範 圍,只需要簡單的比例微分控制器及正定的適應性學習增益即可較傳統的反覆學習 適應性控制演算法有更好的效能。

(15)

T. Hsiao 在[3]提出的位置回授控制器設計,這個控制器是應用在車輛橫向運動 的控制。文中說明在設計狀態回授控制器時,觀察器沒有被考慮進去,外加的觀察 器改變了原來的狀態回授控制系統,因而不能保證控制器的強健。這個控制器設計 提出兩個步驟的設計程序,將狀態回授控制器轉換到一個輸出回授的控制器而不會 改變閉迴路的特性。

2.2

機器人錯誤偵測與判別相關研究

M.L. McIntyre 在[10]將機械臂錯誤情形做分類,分別為致動器卡住(locked -joint fault)、不受力甩動(free-swing fault)、階梯錯誤(ramp fault)或是飽和錯誤(saturated fault)。 主要透過預測值與實際值誤差和非線性觀測器與來進行轉軸致動器的錯誤偵測和識 別,其主要貢獻在於利用濾波器使計算力矩不需使用到角加速度,因為若由角度作 兩次差分或角速度作一次差分往往會因為產生高頻雜訊的干擾而失真,造成力矩計 算的極大誤差,因此利用濾波器濾掉角加速度來進行力矩計算與比較,此濾波方法 也會應用於底下的模型參數估測。此外由於觀察器不會受到控制器影響,故強健度 較好,不容易受到雜訊干擾。

Jin-Ho Shin 與 Ju-Jang Lee 於[11]中提出適用於機器人致動器的錯誤偵測與強健 的錯誤矯正控制,其討論錯誤發生為致動器錯誤並僅討論馬達自由甩動的情況,馬 達卡住或飽和不在討論範圍內。該論文首先將偵測與識別各別分為一個平台,兩個 平台的方法都很直接。錯誤偵測部分由計算力矩控制器(Computed Torque Controller) 事先給定軌道,再用感測器去獲得實際的軌道,若兩者相差 大於臨界值則錯誤發生。 而錯誤識別的平台也是檢測事先給定的系統錯誤的軌道是否與感測器量測值相同,

來確定哪個軸的致動器故障。若有一個n 軸機械臂,其中有 p 個軸故障,則事先給

定的參考訊號須有C 個。 np

De Luca, A.Mattone,R. 於 [13] 中提出一種基於廣義慣量(generalized momenta) 與經由 理論設計狀態觀測器的機械臂轉軸錯誤偵測方法。該論文先將致動器發生錯 誤的問題重新改寫為一針對線性系統設計觀察器的問題。因此設計一組識別器

(16)

(identifier) 代表一種錯誤狀況,每一個識別器由皆為線性濾波器,並會產生一個相對 應的剩餘值訊號,來決定該錯誤發生與否。雖然該論文最後進行實驗,的確可以決 定系統錯誤的種類,但作者仍有提到這是依據量測雜訊(measurement noise)事先設定 門檻(threshold)的結果,也就是門檻的設定將會影響錯誤偵測的結果。 根據上述文獻分析,在控制器的部分,雖然可透過適應性估測及濾波估測誤差 等方式來改善控制效果,但仍需要有速度回授資訊才能進行控制,所以本論文在控 制器的設計中要探討如何解決僅有位置回授的問題。在錯誤診斷的文獻中,也提出 許多能夠正確的偵測甚至診斷錯誤類型的文獻,但在實際應用上,仍需進一步改善 效能與降低計算量。因此本論文所設計的階層式錯誤診斷便是用來解決多模型估測 法計算量過大之問題,使這套診斷系統能應用於即時控制平台上,快速而正確地診 斷出系統的錯誤。

(17)

第三章 機械手臂動態模型與控制器設計

本章節中將分 為兩大部份,第一部份會介紹論文中所使用的兩軸機械手臂系統, 其包含了動力學及致動器(馬達)的模型,並將兩個模型歸納整理出完整的機械手臂動 態模型。第二部分為控制器的設計,有計算力矩控制器及僅需位置回授的新型控制 器。計算力矩控制器是已經被廣為使用的控制器,主要是將非線性項參數代入系統 消掉後,再設計一組PID 控制器來做控制,但是其所需的回授資訊包含角速度訊號, 本論文實驗平台沒有速度感測器,因此只能透過差分的方式或設計一組觀察器來得 到角速度的訊號,因此本論文將會比較這兩種控制方式所得出的結果。觀察器的設 計部分,由於本論文的實驗平台為二軸機械手臂,是一個非線性系統,故選用非追 蹤型卡曼濾波器(Unscented Kalamn Filter,簡稱 UKF)來當作這個系統的觀察器。

僅需位置回授的控制器是一種新的控制方法,它只需回授角度資訊,不需要使 用差分的方式得到速度回授資訊,因為一般而言,差分會造成雜訊放大。同時,配 合適當的非線性控制器,即可達到較計算力矩控制器優異的控制效果。計算力矩控 制器是一個已經被廣泛使用的控制器[1][2],在此做為與第二個控制器比較的對照組, 以便觀察其結果差異。

3.1

機械手臂的動力學模型

兩軸平面機械手臂如圖 1 所示,在本論文中稱 Link1 為大軸,Link2 為小軸。由 Lagrangian Equation[1]可得到機械手臂的動力學方程式: + + + = M(q)q C(q,q)q G(q) B(q) τ    (4) 其詳細內容如下 2 2 2 2 1 1 2 1 2 2 2 1 2 2 1 2 2 2 2 1 2 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 cos( ) cos( ) cos( ) c c c c c c c c m l m l m l m l l q I I m l m l l q I m l m l l q I m l I ⎡ + + + + + + + ⎤ = ⎢ + + + ⎥ ⎣ ⎦ M(q) 2 1 2 2 2 2 1 2 2 1 2 2 1 2 2 1 sin( ) sin( )( ) sin( ) 0 c c c m l l q q m l l q q q m l l q q − − + ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ C(q,q)    

(18)

1 1 2 1 1 2 2 1 2 2 2 1 2 ( ) cos( ) cos( ) cos( ) c c c m l m l g q m l g q q m l g q q + + + ⎡ ⎤ = ⎢ + ⎥ ⎣ ⎦ G(q) 1 1 1 1 2 2 2 2 sign( ) sign( ) c c b q f q b q f q + ⎡ ⎤ = ⎢ + ⎥ ⎣ ⎦ B(q)     其中M(q) 為手臂的質量慣性矩陣,C(q,q) 為向心力及柯氏力項矩陣,G(q) 為重 力項向量,B(q) 為摩擦力項向量,τ為致動器傳至兩軸上的扭力。此處假設摩擦力 B(q) 為庫倫摩擦力與黏滯摩擦力,黏滯摩擦力為黏滯摩擦力係數b乘上角速度,庫倫 摩擦力為庫倫摩擦力係數fc 乘上角速度的方向性,摩擦力係數皆設為常數。 2 q 1 q 1 l 1 c l 1 m 2 m 2 l 2 c l

y

2 I 1 I

x

圖 1 兩軸平面機械手臂示意圖 在此系統中,角度q與扭力τ為可量測之已知狀態,角速度q及角加速度q為未知 狀態。動力學模型的動態方程式可表示成下列型式: ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ 1 1 2 2 1 -1 2 q q q d = q q dt M(q) [τ(t) - C(q,q)q - G(q)] q       (5) 接著考慮致動器的部分。此兩軸機械手臂實驗系統的扭力是由兩顆直流馬達所 供給,為了使控制更加準確,必須要將馬達的物理參數一併考慮進動力學模型之中。

(19)

典型的直流馬達驅動系統如圖 2 所示,一般可由二階線性微分方程式來表示其電氣 機械等效電路模型,進而導出馬達電樞的輸入電壓v與輸出力矩τ 之間的動態方程式, 如(6)式所示[1]。 a b a m m 2 a a K K τ K J q + f q + q + = v R r rR    (6) 其中 ) ( ) ( ) ( ) ( ) / ( ) ( 2 V v r rad q N-m N-m f rad V-s K R N-m/A K kg-m J m b a a m :電樞電壓 :齒輪減速比率 :負載軸的角度 即機械手臂力矩 , 力矩 :在齒輪組後的負載軸 :轉子的摩擦係數 :反電動勢常數 :電樞電阻 :馬達力矩常數 :轉子的轉動慣量 τ Ω ) ( ) ( V Motor Ka Kb Ra fm Jm 1:r q τ 圖 2 直流馬達示意圖 若考慮此系統為兩軸機械臂,則可由(6)得 + + τ2 = Jq Fq Kv r   (7) 其中 1 2 0 0 m m J J ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ J 1 1 2 2 0 0 0 0 a b m a a b a K K f R f f K K f R ⎡ ⎛ ⎞ ⎤ + ⎢ ⎜ ⎟ ⎥ ⎡ ⎤ ⎝ ⎠ ⎢ ⎥ = ⎛ ⎞ ⎣ ⎦ ⎢ + ⎜ ⎥ ⎢ ⎥ ⎣ ⎦ F 

(20)

1 1 2 1 1 0 0 0 1 0 a a a a K R r K K K R r ⎡⎛ ⎞ ⎤ ⎢⎜ ⎟ ⎥ ⎡ ⎤ ⎝ ⎠ ⎢ ⎥ = ⎛ ⎞ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ K  將(7)式帶入(4)式後就可得到完整機械手臂動力學模型,如(8)式所示。 ( + ) 2 2 2 2 2 M(q) r J C(q,q) G(q) F(q) q + q + + + Bq = v r K r K r K r K      (8) 最後將(8)重寫,得到(9)式的形式。 ( ) + ( , ) + ( )+ ( )+ M q q C q q q G q   F q Bq = v (9) 其中 2 2 2 2 1 1 2 1 2 2 2 1 2 2 1 2 1 2 2 2 1 2 2 2 2 2 1 1 1 1 2 2 2 2 2 1 2 2 2 2 2 2 2 2 2 2 2 2 2 1 1 ( 2 cos( ) ) ( cos( ) ) 1 1 ( cos( ) ) ( ) c c c m c c c c c m m l m l m l m l l q I I J m l m l l q I K r K r m l m l l q I m l I J K r K r+ + + + + + + + ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ + + + + ⎥ ⎢ ⎥ ⎣ ⎦ M(q) 2 1 2 2 2 2 1 2 2 1 2 2 2 1 1 1 1 2 1 2 2 1 2 2 2 1 1 ( sin( ) ) ( sin( )( )) 1 ( sin( ) ) 0 c c c m l l q q m l l q q q K r K r m l l q q K r + ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ C(q,q)      1 1 2 1 1 2 2 1 2 2 1 1 2 2 1 2 2 1 1 1 [( ) cos( ) cos( )] 1 ( cos( )) c c c m l m l g q m l g q q K r m l g q q K r+ + + ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ + ⎥ ⎢ ⎥ ⎣ ⎦ G(q) 2 1 1 1 1 2 1 1 2 2 2 2 2 2 2 2 1 ( ) 1 ( ) b q r f K r b q r f K r+ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ + ⎥ ⎢ ⎥ ⎣ ⎦ Bq    1 1 2 1 1 2 2 2 2 2 sign( ) sign( ) c c f q K r f q K r ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ F(q)   

(21)

3.2

計算力矩控制器

計算力矩的控制法則給定如下: ( ) d v p I dt = + + +

+ + + + v M(q)[q K q K q K q ] C(q,q)q G(q) F q      Bq (10) q q q~= d − 表示角度的誤差。計算力矩控制器主要分為線性部分及非線性部分, 非線性的部分是透過參數估測所得出模型參數值代入後消掉。因此將(9)帶入(10)即可 得到:

d v p I M(q)q = M(q)(q + K q + K q + K qdt)     (11) 因為M(q)為正定矩陣,因此可整理出: v p I dt + + +

= q K q K q K q    0 (12) 此部分即為線性控制器的部分。其中KvK 和p KI為對稱正定的矩陣,在實現 這個PID 控制器時,會調整KvK 和p KI的值,透過一些調整PID 參數的經驗法則, 便可以在效能與穩定之間取一個平衡點,而不會因增益過大使得機械手臂的致動器 飽和發生震動,或因增益過小使得誤差過大。計算力矩控制器系統方塊圖如下圖 3 所示。 (q) M v K P K I K 1 S d q d q d q d q q ( , ) + ( )+ ( )+ C q q q G q  F q Bq q q 圖 3 計算力矩控制器系統方塊圖

(22)

3.3

觀察器設計

在線性系統中,最被廣泛應用的莫過於Rudolph E. Kalman 博士於1960年所發表 的卡曼濾波器(Kalman Filter,簡稱KF),此濾波器利用遞迴的方式,即透過狀態預測 (predictor)和狀態修正(corrector)的反覆運算,求得在高斯雜訊下的最小均方誤差 (Minimum mean Square error)解。

考 慮 一 個 線 性 系 統 :    x(k +1) = Ax(k) + Bu(k) + Gv(k) y(k) = Cx(k) + Du(k) + w(k) (13) ⎡ ⎤ ⎣ TQ(k) = E v(k)v (k) 為 處 理 雜 訊 的 協 方 差 矩 陣,R(k) = E w(k)w (k) T為 量 測 雜 訊 的 協 方 差 矩 陣 卡 曼 濾 波 器 演 算 法 如 下 : 狀 態 估 測 ˆ ˆ x(k +1| k) = Ax(k | k) + Bu(k) (14)

[

][

]

{

ˆ ˆ T

}

P(k +1| k) = E x(k +1 | k) - x(k +1) x(k +1| k) - x(k +1)

[

][

]

{

ˆ ˆ T T T T

}

= E A x(k | k) - x(k) x(k | k) - x(k) A + Gv(k)v (k)G T T = AP(k | k)A + GQ(k)G (15) 狀 態 修 正 ⎡ ⎤ ⎣ T -1-1 P(k + 1 | k + 1) = P(k + 1 | k) + C R(k) C ⎡ ⎤ ⎣ T -1-1 = P(k + 1 | k) + C R(k) C ⎡ ⎤ ⎣ ⎦-1 T T = P(k + 1 | k) - P(k + 1 | k)C R(k) + CP(k + 1 | k)C CP(k + 1 | k) (16) T -1 L(k + 1) = P(k + 1 | k + 1)C R(k) (17)

(23)

[

]

ˆ ˆ ˆ

x(k +1| k +1) = x(k +1| k) +L(k +1) y(k)-Cx(k +1| k) + Du(k) (18)

而根據上一節的介紹得知,本論文的實驗平台為二軸機械手臂,從動力學模型

可以得知此系統為一個非線性系統,因此上述的KF 並不適用,必須選擇非線性系統

的觀察器。常應用於非線性系統的觀察器有擴展型卡曼濾波器(Extended Kalman fil-ters,簡稱 EKF),以及非蹤跡型卡曼濾波器(Unscented Kalman filfil-ters,簡稱 UKF)。 擴展型卡曼濾波器(EKF)的做法是將非線性系統進行線性化,即對非線性系統進行一 階近似的泰勒級數展開。但這樣無可避免地會產生線性化誤差,而且當系統過於剛 性(Stiff)導致線性化假設不成立時,採用這種算法會導致濾波器性能下降,甚至造成 發散。

非蹤跡型卡曼濾波器(UKF)是 由 Simon J. Julier 與 Jeffrey K. Uhlmann[6]於 1997 年提出的,此濾波器直接使用系統的非線性模型,並利用「對輸入訊號進行高斯轉 換近似比對非線性系統做近似來的容易」的概念,去計算一組權重取樣平均後,再 去做狀態估測及修正。EKF 與 UKF 的效能比較如圖 4 所示。由此可看出,UKF 所

估測出的結果明顯優於EKF。因此在本論文中將會採用 UKF 作為錯誤估測系統的觀

察器,下面將會詳細介紹UKF 的內容及運算步驟。

圖 4 UKF 與 EKF 效能比較圖

(24)

(

)

(

)

x(k +1) = f x(k),u(k) + v(k) y(k) = h x(k) + w(k) (19) 其 中 f h 皆 為 非 線 性 轉 換 , ⎡ ⎤ ⎣ TQ = E v(k)v (k) 為 處 理 雜 訊 的 協 方 差 矩 陣,R = E w(k)w (k) T為 量 測 雜 訊 的 協 方 差 矩 陣 系 統 模 型 建 立 好 之 後 , 接 著 開 始 進 行UKF主 要 的 三 個 步 驟 : 1. Sigma-Point

UKF 與其他種類的卡曼濾波器不同的地方就是 Unscented 變換。Unscented 變換 是一個計算隨機變數經非線性轉換之統計量的方法,且建立在近似一個機率分佈比 近似一個任意的非線性函數簡單之原則上。而為了計算Unscented 變換,必須先產 生採樣點(Sigma -Points)。假設系統的狀態 x(k) 為 n 維,則須得到 2n+1 的取樣點χi及 權重wi: ˆ 0 χ = x(k | k) (20)

(

)

ˆ i i χ = x(k | k) + (n + κ)P(k | k) i = 1, ,… n 0 κ w = n + κ (21) i+n 1 w = 2(n + κ) i = 1, ,… 2n 其中 ˆx(k | k)為狀態 x(k) 在給定y(1),...,y(k)之條件下的條件期望值,P(k | k) 為

在給定y(1),...,y(k)之條件下的條件協方差(conditional covariance),κ為尺度設計參數,

做為微調之用。 2. 狀 態 估 測 將取樣點(20)輸入非線性系統(19)式可得: i i i i χ = f(χ ,u(k)) y = h(χ ) (22)

(25)

接著將權重(21)及(22)式所計算出之各取樣點所對應的狀態及輸出,分別帶到下列式 中計算預測的狀態、輸出及協方差: ˆ

2n i i i=0 x(k +1 | k) = w χ (23) ˆ

2n i i i=0 y(k +1| k) = w y (24) ˆ ˆ − −

2n T i i i i=0 P(k +1 | k) = w (χ x(k +1 | k))(χ x(k +1 | k)) + Q (25) 3. 狀 態 修 正 使用權重(21)式與預測值(23)、(24)及(25),計算輸出的協方差及互協方差: ˆ − ˆ −

2n T yy i i i i=0 P = w (y(k +1 | k) y )(y(k +1 | k) y ) + R (26) ˆ ˆ − −

2n T xy i i i i=0 P = w (χ x(k +1 | k))(y(k +1 | k) y ) (27) 最後使用上面兩式(26)及(27),加上最新的輸出值來計算修正後的狀態及協方差: ˆ ˆ -1 ˆ xy yy x(k +1| k +1) = x(k +1| k) + P P (y(k +1)- y(k +1| k)) (28) -1 T xy yy xy P(k +1| k +1) = P(k +1| k) P P P (29) 整 個UKF的 架 構 如 圖 5所 示 圖 5 UKF 流程圖

(26)

3.4

僅需位置回授的新型控制器

在3.2 節中介紹了計算力矩控制器的控制方法,雖然簡單而且容易設計,但是它 必需回授速度資訊是一大缺點。在一般的系統中,速度資訊通常由差分的方式產生, 差分會把雜訊放大而使訊號發生大幅度的震盪,如圖 6 所示。此缺點會限制控制器 增益的調整,降低控制器的效能,當增益調整過大時甚至會出現致動器飽和的現象。 有鑑於此,本論文提出一種新型控制器,此控制器尚未被應用於機械手臂的控制 中。此控制器主要有三個部分,第一部分先將機械手臂動力學方程式(9)中的線性項 及非線性項分別獨立出來,利用[3]中提出之設計位置回授控制器的方式,設計出關 於線性項部份的控制器。第二部分則將剩餘的非線性項部份,則利用編碼器所量測 得的角度以及命令角速度和角加速度,分別帶入非線性項動力學模型中,計算出應 有的前饋補償命令。第三部分透過Lyapunov method[4]設計一組非線性控制器,補償 剩餘的誤差值。在此控制器的設計過程中,僅需回授角度的量測值,系統效能便能 超越在前一節所介紹的計算力矩控制器。整個控制器的方塊圖如圖 7 所示。 圖 6 差分角速度與命令軌跡,下圖為局部放大圖 0 2 4 6 8 10 12 14 16 18 20 -3 -2 -1 0 1 2 3 sec rad /s ec

Desired angular speed Differential angular speed

0 0.2 0.4 0.6 0.8 1 1 1.5 2 2.5 sec ra d/ se c

Desired angular speed Differential angular speed

(27)

d

q

d

q

measure q q q q d q 圖 7 不需要速度估測的控制器方塊圖 由(9)式中可以看出,M(q) 矩陣內含有線性常數矩陣部分及非線性項部分,分別 令為M0及M (q) ,並將1 τ拆成τLinτffτN L,可整理出: ( ( , ) ( ) ( )) + = − + + + 0 Lin ff NL 1 M q Bq τ  + τ + τ M (q)q C q q q G q   F q (30) h(q,q,q) M q C q q q G q  = 1+ ( , ) + ( )+F q( ) ,並將上式表示成動態方程式: N N 1 1 2 2 1 1 2 2 0 0 1 0 0 0 0 0 0 1 0 0 + ( - ) 0 0 0 0 q q q q d q q dt q q ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ Lin ff NL -1 -1 0 0 = τ + τ + τ h(q,q,q) - M B M B x A x          (31) 將(31)式的非線性項暫時忽略,只考慮線性項的部分便可整理成 Lin x = Ax + Bτ (32) 1 2 1 2 q q q q ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎢ ⎥⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎣ ⎦ 1 0 0 0 y = = Cx 0 1 0 0   (33) 其中τff =M (q)q1 d +C(q,q )q d d +G(q) F q+ ( )d ,主要的作用在於消除非線性動態 h(q,q,q)  的前饋控制輸出。τLin則是根據(32)式線性項部分所設計出的線性控制器輸

(28)

出,而τN L為非線性控制器輸出,其目的在使閉迴路系統有強健的穩定性。 此兩軸機械臂系統控制器的目的為角度追蹤,因此設角度及角速度的命令軌跡 為xd =

[

q1d q2d q1d q2d

]

T,線性組合部份的命令電壓τd為: 1 2 1 2 d d d d q q q q ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎡ ⎤ = ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ d 0 τ B M     (34) 而目前的系統狀態為x=

[

q1 q2 q1 q2

]

T,所以可以得到角度及角速度的追蹤 誤差 1 2 1 2 T q q q q ⎡ ⎤ = ⎣ ⎦ x     ,其中x = x xd τLin =τLinτd。最後改寫(32)得到新的 狀態空間表示如(35)式所示,在此仍先暫時忽略非線性項,只考慮線性項的部分。   x = Ax + BτLin (35) y = Cx (36)

3.4.1 線性控制器的設計

此控制器的設計方法採用[3]中所提出的位置回授控制器設計,主要應用在車輛 橫向運動的控制。在很多系統中,某些狀態難以量測,因此常在系統中設計觀察器 去估測所要知道的狀態,並加上狀態回授控制器結合成輸出回授控制器來控制系統。 雖然此種控制方式能夠保證系統穩定,卻不能保證系統的強健性依然良好,原因在 於設計狀態回授控制器時,觀察器的動態特性沒有被考慮進去,使得外加的觀察器 改變了原來狀態回授控制系統所導致。 因此,位置回授控制器[3]為了解決這個問題,提出了兩階段設計程序使狀態回 授控制器轉換成輸出回授的控制器,而不會改變其閉迴路的特性。在此控制器的設 計中,首先在(35)式所表示的受控廠前加上一個濾波器使其成為一個增大的受控廠, 這個濾波器的作用主要是將增大的受控廠利用LQ 方法(Linear quadratic methods)得 到一個狀態回授控制器,最後再將此對增大受控廠設計的狀態回授控制器轉換成對

(29)

原來受控廠的輸出回授控制器。 第一階段 假設G(s)是機械手臂動力學方程式有關於線性項(35)的轉移矩陣。加入一個濾波 器Λ(s) 到電壓輸入端以增大受控廠 G(s)。如圖 8 所示。

Λ(s)

G(s)

q + + Lin τ τLin d d q q q q  + − u Lin τ τd q + + 圖 8 增大的受控廠 其中 1 2 1 0 ( ) ( ) 1 0 ( ) D s s D s ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ Λ (37) 1 1 1 0 ( ) r r r D s =ss − + +" λs+λ ,係數λi是設計的參數。將Λ寫成如(18)式的可 控制典型式(controllable canonical form) 。

0 1 2 1 0 1 2 1 0 1 0 0 0 0 0 0 1 0 0 0 0 1 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 1 r r λ λ λ λ λ λ λ λ − − ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = + ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − − − − ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ Lin Lin τ τ u " " # # # % # # # "  " " # # # % # # # " (38) 如果r>1,則必須計算τLin的微分項,會大幅增加計算量。因此選擇r=1,則 0 ) (s = sD ,所以(38)式可以化簡為(39)式。 0 0 0 1 0 0 0 1 λ λ − ⎡ ⎤ ⎡ ⎤ = + = − ⎣ ⎦

Lin Lin τ Lin τ

(30)

這個增大的受控廠的輸入是u,輸出是qq。從(35)式和(39)式,可以得到它的 狀態空間的表示為 N 0 0 d dt λ λ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥+⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦ ⎣ ⎦ -1 -1 -1 0 0 0 Lin1 Lin1 1 Lin2 Lin2 2 a1 a 0 I 0 0 q q 0 0 0 0 0 0 -M B -M -M q q 0 0 0 0 0 = 0 0 - 0 τ τ 0 0 1 0 u 0 0 0 τ 0 0 0 1 u u B x A                  (40) 觀察(40)式,可看到這個增大的受控廠依然可控制。因此選擇適當的狀態回授增 益,可以任意分配閉迴路的特徵值。令狀態回授控制器的增益為u = −Kx ,則:

[

]

1q 2q 3 ⎡ ⎤ ⎢ ⎥ = − = − = − − − ⎢ ⎥ ⎣ ⎦ 1 2 3 Lin Lin q u Kx K K K q K K K τ τ           11 12 1 21 22 1 31 32 13 14 2 23 24 2 33 34 k k q k k q k k k k q k k q k k ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤ = − ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦⎣ ⎦ ⎣ ⎦τLin        (41) 第二階段 由於系統中並不是所有狀態都可以由量測得到,因此必須將狀態回授轉換成輸

出回授的形式。而ˆuτˆLin分別表示u和τLin的拉氏轉換。從圖 8 中,可以發現

ˆ = ˆ

Lin

τ Λu,所以(41)式的拉式轉換可以表示為

(

1 1 2 2 1 2 3 1 2

)

ˆLin = − ( )s qˆ qˆ T +qˆ qˆ ⎤T + ⎡ˆLin ˆLin T

τ Λ K   sK   K τ τ (42) 將上式移項整理後,可以得到輸出狀態回授控制方程式:

(

)

1 1 3 1 2 1 2 ˆ = −[( )s + ]( + )qˆ qˆ T ⎣ ⎦ Lin τ Λ K K sK   (43) (43)式展開可得: 1 11 12 1 2 21 22 0 0 31 34 0 31 0 4 32 33 2 2 ˆ 1 ˆ ˆ ( ) ( )( ) ˆ a a q a a s λ λ k k s λ k λ k k k q ⎡ ⎤ ⎡ ⎤ = ⎢ ⎥ ⎢ ⎥ + + + + + + + − ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ Lin Lin τ τ     (44) 其中

(31)

2 11 21 11 21 0 34 32 23 11 0 34 32 13 2 12 22 12 14 0 34 32 24 12 0 34 32 14 2 21 23 13 23 0 31 33 21 13 0 31 33 11 2 22 24 14 24 0 31 33 22 1 [ ( ) ] ( ) [ ( ) ] ( ) [ ( ) ] ( ) [ ( ) ] a k s k k k k k s k k k k a k s k k k k k s k k k k a k s k k k k k s k k k k a k s k k k k k s k λ λ λ λ λ λ λ = + + + − + + − = + + + − + + − = + + + − + + − = + + + − + 40+k31)−k k33 12 從(41)式到(44)式的轉換只有使用到運動學上的關係,不需要使用觀察器或速度 估測器。透過這個線性控制器,並配合感測器量測到的角度、命令的角速度和角加 速度代入機械手臂的動力學方程式中,再加上非線性項算出的應有的扭力,便完成 了線性部分的控制設計。 增益矩陣[K K K1, 2, 3]的選擇會因為控制目標的不同而異,例如效能、強鍵性、 致動器的限制等。在這裡應用LQ 方法(Linear quadratic methods)來決定增益,考慮如 圖 8 增大的受控廠,機械手臂的狀態為x,致動器所能提供的輸入為u。LQR(Linear

quadratic regulator)控制器的成本函數(cost function)為

(

)

0 T T J =

x Qx u Ru  +  dt (45) 其中Q∈ R6×6是半正定矩陣,R是正定矩陣,狀態回授的增益可以經由求出J 的 極小而產生。這個最佳化的增益[K1 K2 K ] = R B P,P 是正定矩陣,滿足 Riccati 3 -1 Ta1 equation T -1 T a a a1 a1 A P + PA -PB R B P + Q = 0 (46)

3.4.2 非線性控制器的設計

在設計非線性控制器之前,先透過Lyapunov function 來判斷系統是否穩定。根 據Lyapunov 全域穩定定理,系統能夠保證穩定須滿足下列條件: 1. V(x) > 0 ,∀ ≠x 0 (positive definite ) 2. V(x) = 0 ,when x = 0 3. V(x) < 0 ,∀ ∈x \n 因此將上一節中的系統中狀態x帶入Lyapunov function: T V = x Px  (47) 其中P 為滿足(46)式 Riccati equation 之正定矩陣。對(47)微分後可得到:

(32)

T T V = x Px + x Px     (48) 由(31)及(40)式整理得出: a a1 a2 ff NL x = A x + B (-Kx) + B (τ - h(q,q,q) + τ )     (49) 其中 0 0 0 0 ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ a2 -1 0 B M 。將(49)式代入(48)式可得 T T T a a1 a a1 ff NL a2 V = x((A - B K) P + P(A - B K))x + (τ - h(q,q,q) + τ ) B Px      T a2 ff NL +x PB (τ - h(q,q,q) + τ )   (50) 根據Lyapunov’s stability,若存在一線性非時變系統x = Fx ,則此系統能夠穩定 的充分必要條件為: 存在一組正定對稱矩陣P 及Q,P>0且Q>0,則F P + PF = -QT 在(50)式中,x =(A - B K xa a1 ) 已在上一節中證明為一個穩定的線性系統,根據(46) 的可得知AaBa1K 滿足 Riccati equation,則必定存在一組正定矩陣QP ,因 此(50)可以改寫為 ( ) ) − + − − T T T -1 T -1 T T T a a1 a1 a a1 a1 ff NL a2 T a2 ff NL -1 T T T T a1 a1 ff NL a2 a2 ff NL V = x A P PB R B P PA -PB R B P x + (τ - h(q,q,q) + τ ) B Px + x PB (τ - h(q,q,q) + τ ) = x( PB R B P Q x + (τ - h(q,q,q) + τ ) B Px + x PB (τ - h(q,q,q) + τ )                  T T T ff NL a2 a2 ff NL = -xQx + (τ - h(q,q,q) + τ ) B Px + x PB (τ - h(q,q,q) + τ )        (51) 其中Q PB R B P Q 。觀察上式結果,可以發現= a1 -1T a1T + xQx 必定會大於0,因此 現在必須針對後面兩項進行設計,使Lyapunov function V 必然小於 0。 首先,考慮一可控制

(controllable)

的線性非時變系統為: ζ ζ ζ ζ ζ ϕ ζ = + = A B Z C  (52) 其轉移函數為:H(s)=C sI Aζ( − ζ)−1Bζ ,並且符合嚴格正實(strictly positive-real-SPR)的條件。令 T a2 Z = (PB ) x 通過此 SPR 系統,並設計非線性控制器輸出

(33)

( )t α ϕ = − NL τ 。其中x 包含速度誤差資訊,因此在設計過程中需要透過一些轉換將速 度項取代掉: ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎣ ⎦ T a2 1 2 3 Lin q Z = (PB ) x [E E E ] q τ     (53) ( )s ϕ = 1 2 3 Lin 1 2 3 Lin H(s)Z(s) = H(s)[E q + sE q + E τ ] = (H(s)E + sH(s)E )q + H(s)E τ

  

  (54)

從(54)可看出H(s)E + sH(s)E1 2H(s)E3都是真分,故ϕ( )s 可僅從角度與力矩資 訊獲得。由於系統加入一SPR 濾波器,因此須改寫 Lyapunov function(47)及(51)得: 2 1 ( ) 2 α α α α + + − T T ss V = x Px  ζ Γζ K (55) T T ff NL ff NL V = xQx + (τ - h(q,q,q) + τ ) Z + Z (τ - h(q,q,q) + τ )       ( ) α α T α T α T +α α α− ss + ζ Γζ + ζ Γζ + ζ Γζ   K (56) 其中αss為常數,目的在使

α

能收斂至αss。將(52)式代入(56)中重新整理: ( T ) T ζ ζ ζ ζ

α

α

α

α

T + + T + T + T V = xQx + ζ A Γ ΓA ζ   Z B Γζ ζ ΓB Z ζ Γζ ( ) α αϕ αϕ α α α + T T − − T + − ff ff ss (τ - h(q,q,q)) Z + Z (τ - h(q,q,q))    Z Z K (57) 在此引入Kalman-Yakubovish-Popov 定理[5],其論述如下: 若系統H s( )=C sI( A)−1B為一個m m× 的轉移矩陣,並且符合嚴格正實(strictly positivereal-SPR)的條件,則必存在兩個正定矩陣P 及ζ Q ,使得 ζ ζ ζ ζ T T A P + P A = -Q PB = C (58) 根據此定理便可將(57)重新化簡: ( ) ζ α

α

α

α

α α

− − T + T + − + T T ss ff ff V = xQx   ζ Q ζ ζ Γζ K (τ -h) Z+ Z (τ -h) (59) 對後兩項取範數 2 ≤ × T T ff ff ff (τ - h) Z + Z (τ - h) (τ - h) Z

(34)

其中 1 1 ( d d d ( )) (d ( )) = + + + − + + + ff (τ - h) M (q)q C(q,q )q  G(q) F q M (q)q C(q,q)q G(q) F q    = (M (q)q C(q,q )q1 +  d dC(q,q)q F q + ( )dF q( )) ≤ M (q) q1  + C(q,q )qd dC(q,q)q  + F q( )dF q( )) ≤δ( )t 故可整理成: ( ) 2 ζ α

α

α

α

α α

δ

− − T + T + − + ss V = xQx   ζ Q ζ ζ Γζ K Z (60) 由(60)式可看出第一項必定小於 0,令存在一個g,且g≥2δ Z 。接著將(60)剩 餘項皆令其小於0: ( ) 0 α ζ

α

T + +

α

α α

− −

α

Tss ζ Γζ g K ζ Q ζ   (61) 根據線性系統理論,若A 為n n× 矩陣,則 x AxTA x ,其中2 A ≥max( )

λ

。 因此可進一步得到αζ Q ζT ζ ≤αK ζβ 2 2 γ ≤ T ζ Γζ K ζ 。將上述之上界代入(61)後移項 整理可得: ( ) β γ α α α α α − ≤ − − 2 2 ss K ζ g K ζ K  (62) 根據Lyapunov 全域穩定定理,若系統為穩定,則V(x) < 0 且V(x) 0。觀察(55)式,> 可知第一項及第三項都恆大於0,只要能夠保證α 永遠大於0,就能符合 Lyapunov 全域穩定定理的條件。因此選定參數Kα >0及αss >0,討論(55)式的兩種情形: 1. 若 0 γ α α α < < − 2 ss K ζ K SS SS β β β α 22 < ×ε α 2 = K ζ K ζ K ζ g,其中ε為略大於1 的常數,而 max{2δ ,ε αSS β } = × 2 g Z K ζ 。由此可知分子小於0,但在此希望α 能夠趨近於 SS α ,所以α必須大於0。故 γ − α(α −α) 0< 2 ss K ζ K ,並得到:

(35)

γ α α α< − 2 ss K ζ K (63) 已知V( ) 0, t>0t ≤ ∀ ,則V( )t 必為遞減函數,便可從(55)推得: 2 1 ( ) 2Kα α α− ssV(t) V(0) V (64) ≤ < 其中V(0) 為初始值,為一個已知常數,而 V 為一個比 V(0) 恆大之定值。將(64)移 項後可得: 2 2 0 α α α α α < ssV ≤ < ss+ V K K (65) 又因為V 及Kα為可自行設定之參數,因此可確保α 恆大於0。 2. 若 γ α ε α× >α α> − 2 ss ss K ζ Kγ 2α(α −α) 0> ss K ζ K ,又( SS γ ) β β SS β g α α −K 2 22 < ×ε α 2 = ζ K ζ K ζ K ζ K 所以α 小於 0 使得α 趨近αSS。 由上述的推導可以得知,當 =max{2δ ,ε α× SS β 2} g Z K ζ 時,可以保證α 能夠確 實落在下界 γ α 2 K ζ K 及上界ε α× SS β 2 K ζ 的區間中,並且α 恆大於0。因此最後可以得 到下列兩式,證明出此非線性控制器恆為穩定: 2 1 ( ) 0 2 α α α α + + − > T T ss V = x Px  ζ Γζ K (66) 2 ( ) 0 ζ α

α

α

δ

α

α α

− − T + T + + − < ss V = xQx   ζ Q ζ ζ Γζ Z K (67)  

(36)

第四章 錯誤偵測與判別技術

在第二章中,介紹了在錯誤偵測與判別的一些種類與方法。而本論文在這裡所 採用的是以模型為基礎的錯誤偵測和識別(Model-based FDI)。此方法主要是利用理論 系統和實際系統間的輸出不同計來算其剩餘值(Residuals),藉此判斷系統是否出錯。 本章分為四節,第一小節介紹以模型為基礎的錯誤偵測系統的三大區塊,分別 是:建立錯誤系統模型、剩餘值產生和剩餘值評估。第二小節將介紹多模型組態, 這部分就是系統的剩餘值評估,其中產生剩餘值的觀察器使用3.3 節介紹的非追蹤型

卡曼濾波器(Unscented Kalamn Filter,簡稱 UKF)。在此會介紹兩種多模型組態演算 法,分別為二階廣義偽貝氏演算法(General Pseudo Bayesian of Order 2,簡稱 GPB-2) 以及交互多模型法(Interacting Multiple Model,簡稱 IMM),這兩種演算法的模型估測

準確度大致相同,但複雜度以GPB-2 較高,本論文將會討論這兩種演算法所呈現的 結果。最後一節是階層式錯誤偵測與判別系統,其目的在於當系統正常運作時,僅 運行錯誤偵測;當判斷系統有錯誤發生時,再啟動錯誤判別系統,以降低系統在正 常運作時的運算負荷量。

4.1

以模型為基礎的錯誤偵測技術

以模型為基礎的錯誤偵測和識別(Model-based FDI)的定義為:「事先給定系統錯 誤的描述以及系統的數學模型,並配合感測器所量測得到的系統輸入與輸出資訊, 以藉此來進行系統狀態的確認」。做法分為三個步驟: 第一步為建立錯誤系統模型,在此的目的是要將系統預期會發生的各種錯誤以 數學模型的方式來描述,例如運動學模型、動力學模型等等。也因此系統模型的精 確與否,對於系統錯誤與偵測有相當大的影響。 第二是將感測器量測之實際訊號和經過數學模型的估測訊號相減產生剩餘值 (Residual Generation)。所計算出的剩餘值必須能夠有某種程度的代表系統是否出錯。 因此,剩餘值在系統正常運作的時候必須趨近於零,而在系統出錯時必須遠大於零。

(37)

理想的情況下,剩餘值的大小和系統的輸入和輸出無關,只與系統狀態有關。此步 驟為最重要的一步,因為若產生的剩餘值能夠明確地表現系統的狀態,則下一步剩 餘值評估(Residual Evaluation)的部份將容易設計。 第三步為剩餘值評估(Residual Evaluation),其作法為對上一步所產生的剩餘值利 用相似值(Likelihood)或決定法則(Decision Rule)進行評估,以確定系統是否真的有錯 誤發生。一般的作法是設定一個門檻(threshold)對即時產生的剩餘值進行測試,也可 對剩餘值進行移動平均(Moving Averages)再進行測試。其架構圖如圖 9 所示。 System Residual Generation Residual Evaluation Input Output Sensor

Model-based FDI System Residual

Fault Information

(38)

4.2

多模型架構

多模型架構,顧名思義,就是指一個系統的行為是由兩個以上且有限數量的模 型來加以描述。在錯誤診斷的應用中,乃透過同時運行數個錯誤系統描述模型來與 實際系統輸出比較差異,藉此辨別系統此時與哪種錯誤系統描述模型最接近。所有 的模型皆為互斥,而且其機率和為1,因此在任意時間下,系統只會有一個機率最高 的系統描述模型。而依據方法的不同又可分為兩種類型:

第 一 類 : 靜 態 多 重 模 型(Static Multiple Model)

所謂靜態多重模型是指在運行模型估測時,不會依估測的結果切換模型。在此 模型下估測可推導如下[7]:

k k-1 j j j k-1 k-1 j j k-1 k-1 k-1 j j r k-1 k-1 i i i=1 μ (k) P{M | Z } = P{M | z(k), Z } p[z(k) | Z ,M ]P{M | Z } = p[z(k) | Z ] p[z(k) | Z ,M ]P{M | Z } = p[z(k) | Z ,M ]P{M | Z }

k-1 j j r k-1 i i i=1 p[z(k) | Z ,M ]μ (k -1) = j = 1,...,r p[z(k) | Z ,M ]μ (k -1) (68) 其中M :第 j 個 模 型 , j =1,2,…,r j z(k) :第 k 個 時 間 點 時 的 系 統 資 訊 , 在 此 是 指 系 統 的 輸 入 量 測 與 輸 出 量 測 k Z :到 第 k 個 時 間 點 時 為 止 的 所 有 資 訊 ( (0)z , (1)z ,…, ( )z k ) j μ (k) :第 k 個 時 間 點 時 , 系 統 在 第 j 個 模 型 下 的 機 率 ˆj x (k | k):由 ( )z k 估測第 k 個 時 間 點 估 測 到 的 第 j 個 模 型 的 狀 態 j P (k | k):由 ( )z k 估測第 k 個 時 間 點 估 測 到 的 第 j 個 模 型 的 協 方 差 由(68)式可以觀察到分子項 k-1 j p[z(k) | Z ,M ],所代表的意義是在第 k-1 的時間點

(39)

時為止的所有資訊Zk-1及經 過第 k 個時間的第 j 個模型M (k)下,第 k 個時間點所得j 到的輸出資訊為z(k) 的機率。也因此,這項的值越高代表在這個時間點模型M (k)的j 機率越高,也就是相似度(Likelihood)越高。在本論文中所使用的估測器為 UKF,它 是對每個要估測的狀態點進行高斯近似,令其相似值為Λ (k) p[z(k) | Z ,M ]jk-1 j j j = N[v (k);0,S (k)],其中v (k) 為實際量測輸出與估測輸出之差值,又稱為革新值j

(innovation),S (k) 為 covaricance of innovation,j N[v (k);0,S (k)]意即j j v (k) 的機率分j

布為高斯分布,相對應的mean 為 0,covaricance 為S (k)。由此可得出此刻 k 的狀態j 機率分布函數為:  ˆ

r k j j j j=1 p[x(k) | Z ] = μ (k)N[x(k);x (k | k), P (k | k)] (69) 將 每 個 狀 態 的 機 率 合 併 就 可 以 得 到 所 估 測 的 狀 態 及 協 方 差 : ˆ

r ˆj j i=1 x(k | k) = μ (k)x (k | k) (70) ˆ ˆ ˆ ˆ

r j j j T j j=1 P(k | k) = μ (k){P (k | k) +[x (k | k) - x(k | k)][x (k | k) - x(k | k)] } (71) 靜態多重模型估測之流程圖如下圖 10 所示 圖 10 靜態多重模型估測流程圖[7]

(40)

第 二 類 : 動 態 多 重 模 型(Dynamic Multiple Model) 而動態多重模型相較於靜態多重模型,則是在進行估測的時候多了模型切換的 動作。定義動態多模型方程式: x(k) = F[M(k)]x(k -1) + v[k -1,M(k)] (72) z(k) = H[M(k)]x(k) + w[k,M(k)] (73) 其中F M(k) ,

[

]

H M(k) 為在 k 時間點的模型的系統轉移函數,

[

]

v k -1,M(k) ,

[

]

[

]

w k,M(k) 為因為時間變動與不同模型下的處理雜訊和量測雜訊。

由於動態多重模型會切換模型,為了要套用靜態多重模型(Static Multiple Model) 的公式,則記錄每一個model 在時間上的切換順序(l th mode history through time k)。

將到時間k 的第l 個模型歷史紀錄表示為 l,l k,l k,l k i i M = {M , . . . , M } l = 1 , . . . , ri 為k,ll個模型歷史紀錄在第k 個時間點對應的模型索引,其值為 1~r。定義模型間的切 換事 前 機率: k-1,s j j i ij P{M (k) | M } = P{M (k) | M (k -1)} = p (74) 其中p 為從ij Mi切換到M 的機率。 j

接著就可以仿照靜態多重模型估測器(Static Multiple Model Estimator)的寫法,將

給定k 時間點之前之狀態估測 x(k)的機率化為每一個模型歷史紀錄的機率乘上其對 應的相似值(Likelihood):

rk k k,l k k,l k l=1 p[x(k) | Z ] = p[x(k) | M , Z ]p{M | Z } (75) 其中p{M | Z }k,l k 為 第 k 個 時 間 點,系 統 在 第l個 模 型 歷 史 下 的 機 率,仿 照 之 前 Static Model 下 即 為μk,l :

(41)

k,l k,l k k,l k-1 k,l k-1 k,l k-1 k,l k-1 k-1,s k-1 j k,l k-1 k-1,s k-1 k-1,s j k,l k-1 k-1,s k-1,s j μ = P{M | Z } = P{M | z(k), Z } 1 = p[z(k) | M , Z ]P{M | Z } c 1 = p[z(k) | M , Z ]P{M (k),M | Z } c 1 = p[z(k) | M , Z ]P{M (k) | M , Z }μ c 1 = p[z(k) | M , Z ]P{M (k) | M c 1 = p[z(k c k,l k-1 k-1,s j i k,l k-1 k-1,s ij ) | M , Z ]P{M (k) | M (k -1)}μ 1 = p[z(k) | M , Z ]p μ c (76) 其中c 為正規化係數 將(76)代 入 (75)可 得 :

k k r k k,l k k,l k l=1 r k,l k k,l l=1 p[x(k) | Z ] = p[x(k) | M , Z ]P{M | Z } = p[x(k) | M , Z ]μ (77) 由(77)可看動態模型的估測會因為模型的歷史數量不停的增加,造成濾波器數量 無止境的增加,因此演算法需要改進,才能進行實際的應用。以下將介紹兩種近似 估測法。

(42)

4.2.1 廣義偽貝氏法

廣義偽貝氏法(General Pseudo Bayesian)[7]主要是用來解決歷史資訊Zk一直無限

增長的問題,由(69)式即可看出。因此 GPB-1 在此使用前一個時刻所估測的狀態及協 方差來近似取代歷史資訊:

{

}

⎡ ⎤ ⎡ ⎤ ⎣ ⎦

⎣ ⎦ r k k k j j j=1 p x(k) | Z = p x(k) | M (k), Z P M (k) | Z ⎡ ⎤ ⎣ ⎦

r k-1 j j j=1 = p x(k) | M (k), z(k), Z μ (k) ⎡ ⎤ ≈

r j j j=1p x(k) | M (k), z(k), x(k -1 | k -1), P(k -1 | k -1) μ (k) (78) 而模型機率μ 為: j

{

k

} {

k-1

}

j j j μ (k) P M (k) | Z = P M (k) | z(k), Z

{

}

⎡ ⎤ ⎣ j k-1j k-1 1 = p z(k) | M (k), Z P M (k) | Z c

{

} {

}

r k-1 k-1 j j i i i=1 1 = Λ (k) P M (k) | M (k -1), Z P M (k -1) | Z c

r j ij i i=1 1 = Λ (k) p μ (k -1) c (79) 其中p 為ij Mi切換到M 的機率,只與前一刻的狀態有關。而j

r j

r ij i j=1 i=1 c = Λ (k) p μ (k -1), 為正規化係數。最後將模型機率代回(70)式及(71)式即可得到新的估測狀態及協方差。 (79)與 靜 態 模 型 (68)最 大 的 不 同 在 於 (79)須 乘 上 模 型 間 切 換 機 率 。 GPB-2 與 GPB-1 的原理相同,但訊 號 經 過 兩 層 的 濾 波 器 來 進 行 估 測 。 雖 然 計 算 出 來 的 估 測 值 會 比 GPB-1 來 準 確。相 對 的,計 算 量 也 比 GPB-1 成 長 平 方 倍 。(78)式 將 改 寫 為 : ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎣ ⎦

∑∑

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ r r k k k k j i i j j j=1 i=1 p x(k) | Z = p x(k) | M (k),M (k -1),Z P M (k -1) | M (k),Z P M (k) | Z ⎡ ⎤ ⎣ ⎦

∑∑

r r k j i i|j j j=1 i=1 = p x(k) | M (k),M (k -1), Z μ (k -1 | k)μ (k) ˆ ⎡ ⎤ ≈

∑∑

r r i i j i i|j j j=1 i=1p x(k) | M (k),M (k -1), x (k -1 | k -1), P (k -1 | k -1) μ (k -1 | k)μ (k) (80)

數據

圖 4 UKF 與 EKF 效能比較圖

圖 4

UKF 與 EKF 效能比較圖 p.23
圖 9  Model-Based 錯誤偵測系統架構圖

圖 9

Model-Based 錯誤偵測系統架構圖 p.37
圖 13 IMM 流程圖[7]

圖 13

IMM 流程圖[7] p.47
圖 37 僅需位置回授的新型控制器兩軸輸入電壓(軌跡 1)    軌跡 2  圖 38 僅需位置回授的新型控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 2 )       

圖 37

僅需位置回授的新型控制器兩軸輸入電壓(軌跡 1) 軌跡 2 圖 38 僅需位置回授的新型控制器實驗結果,依序為上臂大軸及下臂小軸角度軌跡(軌跡 2 ) p.68
圖 39 僅需位置回授的新型控制器角度追蹤誤差(軌跡 2)   

圖 39

僅需位置回授的新型控制器角度追蹤誤差(軌跡 2) p.69
圖 56 實驗 3 動力學模型與運動學模型機率估測(模型 1 及模型 4)        5.4   錯誤判別實驗結果  錯誤判別為階層式錯誤偵測與判別系統的後端,使用模型 2、模型 3 及模型 4, 即透過運動學模型和動力學模型於大軸與小軸的列組合,作為判斷大軸或小軸的運 作正常與否的分類模型。在錯誤判別實驗中,量測雜訊平均值為 0 標準差為 0.001 的 高斯雜訊,動力學模型及運動學模型處理雜訊分別為平均值為 0 標準差為 0.0023 及 0.003 的高斯雜訊,錯誤偵測在系統運作 1 秒後啟動。當

圖 56

實驗 3 動力學模型與運動學模型機率估測(模型 1 及模型 4) 5.4 錯誤判別實驗結果 錯誤判別為階層式錯誤偵測與判別系統的後端,使用模型 2、模型 3 及模型 4, 即透過運動學模型和動力學模型於大軸與小軸的列組合,作為判斷大軸或小軸的運 作正常與否的分類模型。在錯誤判別實驗中,量測雜訊平均值為 0 標準差為 0.001 的 高斯雜訊,動力學模型及運動學模型處理雜訊分別為平均值為 0 標準差為 0.0023 及 0.003 的高斯雜訊,錯誤偵測在系統運作 1 秒後啟動。當 p.80
圖 64 實驗 7 各軸發生錯誤之機率估測   

圖 64

實驗 7 各軸發生錯誤之機率估測 p.86

參考文獻

相關主題 :