旋翼型無人載具之高頻寬控制設計

國防科技學術合作研究計畫成果報告

旋 翼 型 無 人 載 具 之 高 頻 寬 控 制 設 計

計畫編號：NSC 96-2623-7006-013-D

執行期間：96 年 1 月 1 日至 96 年 12 月 31 日

計畫主持人： 詹劭勳 教授

共同主持人：

執行單位：國立成功大學航空太空工程學系

中華民國 97 年 2 月 18 日

目 錄

第一章 緒 論

1.1 研究動機與目的 7 1.2 UAH 停懸穩定控制之條件 7

第二章 UAH 系統架構

2.1 航電系統 8 2.2 實際航電硬體架構 8 2.3 機載架構 9 2.4 遙控訊號與自動控制訊號切換架構 9 2.5 新的遙控訊號與自動控制訊號切換器架構 10 2.6 導航系統架構 11 2.6.1 系統簡介 11 2.6.2 REAL-TIME KINEMATIC (RTK) 13

第三章 UAH 控制系統設計

3.1 控制方法 15 3.1.1 非線性控制理論之滑動模式控制設計 15 3.1.2 遭遇之困難及解決途徑 17 3.1.3 解決方式 17 3.2 慣性導航系統 Inertial Navigation System (INS) 18 3.3 擴展式卡爾曼濾波器 Extended Kalman Filter (EKF) 19

第四章 實驗結果與討論

4.1.1 滑動模式控制之模擬結果 20 4.1.2 滑動模式控制之討論 21 4.2.1 飛測資料 21 4.2.2 實際飛行測試結果 22 4.2.3 模糊控制之討論 27 4.3 導航系統之實際測試 28 4.3.1 靜態測試 28

4.3.2 動態測試 32 4.3.3 飛行測試之導航資料 34

第五章 結論與未來工作

5-1 控制系統方面 36 5-2 感測系統方面 36

參考文獻

37

研發成果資料

39

圖目錄

圖 2-1 航電系統配置架構示意圖 8 圖 2-2 航電盒圖 8 圖 2-3 直昇機機構系統示意圖 9 圖 2-4 舊的訊號切換器示意圖 9 圖 2-4-1 舊的訊號切換器輸出( Roll ) 10 圖 2-4-2 舊的訊號切換器輸出( Pitch ) 10 圖 2-5 新的訊號切換器示意圖 10 圖 2-6 Crossbow AHRS400CC-100 11 圖 2-8 Xsens 公司製造之 Mti 12

圖 2-8 NovAtel FlexPak-G2L GPS receiver 13

圖 2-9 (RTK，2 組) GPS 接收器修正其量測誤差[2] 14 圖 3-1 控制系統架構圖 14 圖 3-2 滑動模式控制之誤差相平面圖 15 圖 3-3 SMC 控制器與感測元件整合設計的架構圖 17 圖 3-4 邊界層示意圖 18 圖3-5 導航方程式流程圖 18 圖 3-6 EKF 流程圖 19 圖 4-1 (無干擾)橫向輸出響應圖 20 圖 4-2 (有干擾)橫向輸出響應圖 20 圖 4-3 (無干擾)縱向輸出響應圖 20 圖 4-4 (有干擾)縱向輸出響應圖 20 圖 4-5 (無干擾)主旋翼輸出響應圖 20 圖 4-6 (有干擾)主旋翼輸出響應圖 20 圖 4-7 (無干擾)尾旋翼輸出響應圖 21 圖 4-8 (有干擾)尾旋翼輸出響應圖 21 圖 4-9-1 PWM 訊號值輸出 22 圖 4-9-2 PWM 訊號值輸出 22

圖 4-9-3 Roll 與 Pitch 之 PWM (sine input)訊號值輸出 23

圖 4-10-1 三軸姿態角輸出 23

圖 4-10-2 三軸姿態角速率輸出 24

圖 4-11 GPS 輸出 25 圖 4-12-1 (Roll)橫向姿態控制輸出 25 圖 4-12-2 (Roll)橫向姿態控制輸出(放大) 26 圖 4-13-1 (Pitch)縱向姿態控制輸出 26 圖 4-13-2 (Pitch)縱向姿態控制輸出(放大) 27 圖 4-14 AHRS輸出量測值經由導航方程式積分得速度變化圖 28 圖 4-15 AHRS 輸出量測值經由導航方程式積分得位置變化圖 29 圖 4-16 EKF 回傳速度、位置修正導航方程式積分得速度變化圖 30 圖4-17 EKF回傳速度、位置修正導航方程式積分得位置變化圖 30 圖4-18 EKF回傳全部估測值修正導航方程式得速度變化圖 31 圖4-19 EKF回傳全部估測值修正導航方程式得位置變化圖 32 圖4-20 動態測試GPS使用RTK輔助，經由EKF修正得到速度變化圖 33 圖 4-21 動態測試 GPS 使用 RTK 輔助，經由 EKF 修正得到位置變化圖 33 圖 4-22 動態測試 GPS 使用 RTK 輔助，經由 EKF 修正得到載具路徑圖 34 圖 4-23 GPS/INS 整合之飛測數據，經由 EKF 修正得姿態變化圖 34 圖 4-24 GPS/INS 整合之飛測數據，經由 EKF 修正得速度變化圖 35 圖 4-25 GPS/INS 整合之飛測數據，經由 EKF 修正得位置變化圖 35

表目錄

表 2-1 Crossbow AHRS 規格表 12

表 2-2 MTi 規格表 12

表 2-3 NovAtel FlexPak-G2L GPS receiver 13

表 4-1 AHRS 輸出量測值經由導航方程式積分產生之誤差量 29

表 4-2 經由 EKF 修正導航方程式積分所產生之誤差量(回傳速度、位置) 31 表 4-3 經由 EKF 修正導航方程式積分所產生之誤差量(全部回傳，更新) 32

第一章 緒 論

1.1 研究動機與目的 近年來由於科技日新月異、技術發展快速，小型電腦、電子機械元件的體積 與重量減少，於是近幾年來人們常利用機器人代替自己做一些危險的工作，而無 人載具更可進一步在接受命令之後，自行前往人類無法到達的嚴苛環境下工作， 近如拆彈小組利用機器人執行拆彈任務，遠如火星上無人載具執行地質與地貌的 勘查任務，皆能替代人員完成危險的任務，因此 UAV 勢必成為未來飛行器的一 個趨勢。 無人直昇機( UAH )在交通不便的地方，比地面上無人載具或空中之無人飛 機更具機動性。因為直昇機本身為六自由度( 6DOF )之物體，比起單純在地面上 移動的無人載具更能避開障礙物，且在空中能迅速的抵達目的地，對於無人直昇 機來說，飛機只能在空中盤旋，UAH 卻可以停懸在半空中，更有利於執行定點 的任務。 旋翼型飛行載具與固定翼飛行載具兩者在較動態的特性上有顯著的差異，旋 翼型飛行載具在姿態以及瞬間速度的變化量，遠比固定翼飛行載具更具非線性與 複雜性。另一方面若以控制方法來設計系統，常碰到兩方面的問題：一為系統的 不確定性；二為外界干擾影響。控制系統在這兩項的不確定因素下，往往使性能 偏離預期的目標。於是本計劃將考慮模糊控制( Fuzzy Control)與 SMC (Sliding Mode Control) 控制方法或是其他控制方法來嘗試解決這些問題，使得旋翼型無 人載具有較高頻寬的非線性控制設計，才能符合快速的姿態以及瞬間速度變化特 性。 在所有 UAH 飛航的條件成形之前，停懸穩定控制為其飛航之先決條件，因 此本團隊之研究目標，是以完成直昇機自主飛行的前置步驟「停懸穩定控制」為 主。 1.2 UAH 停懸穩定控制之條件 1.機上航電系統總重量不可超過直昇機最大載重； 2.動態時姿態穩定，姿態角之角速度變化量為較小； 3.停懸狀態下主旋翼轉速維持穩定，旋翼面保持水平，且主旋翼及尾舵攻角 於操作範圍內。

第二章 UAH 系統架構

2.1 航電系統

本系統採用研華科技 PCM-3370 單板電腦，並以 Compact Flash 記憶卡，作 為運算處理與資料儲存；各個機載元件穩定之工作電壓，使用研華科技 Power Supply PC/104-plus Module 電源供應模組。整體的航電系統架構如(圖 2-1)。

圖 2-1 航電系統配置架構示意圖 2.2 實際航電硬體架構 圖 2-2 航電盒圖 5 5vv 7 7..44vv 1 111..11vv P Poowweerr P Prroonngg P Poowweerr S Suuppppllyy P PCC110044 P PIIIIII993333 S SVVCC D Deeccooddee S SSSCC S SWW W WMM G GPPSS I IMMUU S Swwiittcchh U USSBB C COOMM C COOMM11 C COOMM22

W

Wi

in

nd

do

ow

ws

s

X

XP

P

-

-

E

E

2.3 機載架構

機載作業系統為 Windows XP Embedded，其作業系統是微軟公司針對崁入式 系統所特別開發的軟體，其具有即時多工，且搭配系統軟體程式以 Borland C++ Builder 撰寫；在 Windows XP Embedded 之作業系統下，系統軟體程式能夠依據 需要進行資料傳遞與溝通，各感測器所得到的資料，則匯整至 Data base，以供 需要之程式取用或儲存，程式整體架構如(圖 2-1) 所示。 圖 2-3 直昇機機構系統示意圖 2.4 遙控訊號與自動控制訊號切換架構 圖 2-4 舊的訊號切換器示意圖 GPS Controller Transceiver Transceiver Tx (RC) OBC Desired WM Servos IMU Decode ( PIC ) WM S Swwiittcchh Tx (RC) _ + Relay Receiver Servos OBC Decode ( PIC ) SSC ( PIC )

圖 2-4-1 舊的訊號切換器輸出( Roll ) 圖 2-4-2 舊的訊號切換器輸出( Pitch ) 2.5 新的遙控訊號與自動控制訊號切換器架構 由(圖 2-4-1) 與圖(2-4-2) 可知，舊的訊號切換器設計方式會產生高頻雜訊， 容易影響飛行控制安全，於是將(圖 2-4 ) 改善如下(圖 2-5 )。 圖 2-5 新的訊號切換器示意圖 0 5 0 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0 3 5 0 4 0 0 4 5 0 5 0 0 8 0 0 1 0 0 0 1 2 0 0 1 4 0 0 1 6 0 0 1 8 0 0 2 0 0 0 2 2 0 0 t i m e ( 0 . 1 s e c ) PW M P i t c h ( P W M ) S W ( P W M ) S i n e I n p u t ( P W M ) 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 8 0 0 1 0 0 0 1 2 0 0 1 4 0 0 1 6 0 0 1 8 0 0 2 0 0 0 2 2 0 0 t i m e ( 0 . 1 s e c ) PW M R o l l ( P W M ) S W ( P W M ) S i n e In p u t ( P W M ) Relay Receiver Servos OBC Decode ( PIC ) SSC ( PIC )

2.6 導航系統架構 2.6.1 系統簡介

目前導航系統整體架構以兩大子系統為主，一套為 Attitude Heading

Reference System (AHRS)，另一套為 Global Positioning System (GPS) 接收器。 AHRS使用Crossbow公司製造之AHRS400CC-100，(圖2-6) (表2-1) 所示，此AHRS 由三軸加速度計、三軸陀螺儀、電子羅盤與內部溫度感測器整合而成，其中溫度 感測器是用來補償系統內慣性量測元件因溫度變化而產生之誤差；GPS接收器部 分使用NovAtel FlexPak-G2L GPS receiver，(圖2-8) (表2-3) 所示。

導航系統團隊將這兩個元件利用擴展式卡爾曼濾波器進行整合，與先前階段 這兩個元件獨立運作相比，整合後的系統提供飛行載具一個更精確更高可用性的 導航解。由於系統籌載重量考量，以及此Crossbow AHRS之量測值輸出有時會產 生不連續的跳動和載具動作超過感測器極限時會短暫無法提供可靠之量測值，基 於這兩項原因，我們在計畫下半年使用Xsens MTi(圖2-7) (表2-2) 取代開發 GPS/INS演算法所使用之Crossbow AHRS。 圖2-6 Crossbow AHRS400CC-100

Accuracy

Roll and pitch static accuracy < ± 0.5°

Roll and pitch dynamic accuracy < ± 2.0° RMS

Heading static accuracy < ± 1.5°

Heading dynamic accuracy < ± 3° RMS

Angular rate performance

Input range: roll, pitch, yaw < ± 100° /sec

Bias: roll, pitch, yaw < ± 0.05° /sec (angle mode)

< ± 1.0° /sec Acceleration performance

Input Range: X/Y/Z < ± 2 G

Bias: X/Y/Z < ± 8.5 mG

Data rate 60 Hz (angle mode)

100 Hz

表 2-1 Crossbow AHRS 規格表

圖 2-7 Xsens 公司製造之 Mti

Dynamic Range all angles in 3D

Angular Resolution 0.05 (degree) RMS

Static Accuracy (roll/pitch) < 0.5 (degree)

Static Accuracy (heading) < 1.0 (degree)

Dynamic Accuracy 2 (degree) RMS

Update Rate user settable, max 120 Hz

圖 2-8 NovAtel FlexPak-G2L GPS receiver Position, velocity, and time accuracy

Single point accuracy L1 1.8 m CEP

Single point accuracy L1/L2 1.5 m CEP

Velocity accuracy 0.03 m/s RMS

Time accuracy 20 ns RMS

Measurement precision

L1 C/A-code 6 cm RMS

L2 P(Y)-code 25 cm RMS (AS on)

L1 carrier phase 0.75 mm RMS

L2 carrier phase 2 mm RMS

Data Rate

Measurements 20 Hz

Position 20 Hz

表 2-3 NovAtel FlexPak-G2L GPS receiver

2.6.2 REAL-TIME KINEMATIC (RTK) 由於 GPS 存在著誤差來源影響我們定位的精確度，因此使用支援 RTK 的 GPS 接收器可以利用載波相位即時差分的技術進行修正，其方法是以 2 組 GPS 接收機搭配無線傳輸模組使用，一組裝置於移動載具，一組放置已知位置做觀測 點來即時提供修正量，藉此架構可達次公分級精確度並提供飛行載具更好的導航 解[1]；(圖 2-8) 為其系統架構圖。

圖 2-8 (RTK，2 組) GPS 接收器修正其量測誤差[2]

第三章 UAH 控制系統設計

本研究以考慮成本及載重能力，並以雷虎公司的 R-90 遙控直昇機為載具，但 其中機身的控制是非常錯綜複雜，而且 UAH 飛航的條件成形之前，停懸穩定控 制為其飛航之先決條件，因此本團隊之研究目標，是以完成直昇機自主飛行的前 置步驟「停懸穩定控制」為主，於是針對停懸模式設計之姿態穩定控制。 圖 3-1 控制系統架構圖

+

X

d

X

δ

roll pitch

δ

−

e

Servo _DynamicsR 90 I IMMUU Filter Sliding Controller PWM Sensor Fuzzy Controller PID Controller Test Control Mode

3.1 控制方法 3.1.1 非線性控制理論之滑動模式控制設計 以往古典控制方法來設計系統，常碰到兩方面的問題：一為系統的不確定 性。古典設計方法，須先知道受控系統正確的數學模型，才能達到精確的控制， 符合期待的性能。惟實際受控系統受到非線性、時變、不確定性、與複雜性等因 素的影響，要建立正確的數學模型有其困難；二為外界干擾的影響。干擾係來自 於外在環境不可預期的影響。控制系統在不確定性與外界干擾雙重的作用下，往 往使性能偏離預期的目標。

滑動模式控制( Sliding Mode Control, SMC ) [3,4]與古典控制相異之處在於 它的控制器結構需依條件不斷的切換，如(圖 3-1) 所示；理論上，有無限多種可 能之控制律可用來控制系統結構之切換。在 SMC 中引入滑動面( Sliding Surface ) 的設計，可使系統具有快速的時間響應、良好的控制性能，以及系統較不易受系 統參數變動所影響；其應用領域如：機器人的導引控制[5]、馬達控制[6]、車輛 速度控制、訊號處理、無人飛機控制[7]、彈道導航控制[8]等等。 圖 3-2 滑動模式控制之誤差相平面圖 SMC 控制方法的設計理念係配合量測元件及滑動條件作為基礎架構，如(圖 3-4) 所示。其控制方法有二個基本要求：一為必須先設計一個滑動面，由切換 的控制法則迫使系統軌跡進入此滑動面；二為在滑動面上的軌跡要滿足滑動條件 及保證滑動模式的存在性。而控制法則的改變是根據事先已設定好的切換條件， 藉由控制訊號的切換而直接抵達系統的平衡點使系統穩定，所以滿足滑動條件及 保證滑動模式的存在性，系統軌跡就能在此滑動面附近運動，並且朝著控制目標 點前進。 考慮一個單輸入輸出(SISO)n階非線性微分方程式以描述一個動態系統如 下： ( ) _f

(

_{x x x}( ) _t

)

_u

_{( )}

_t xn = ,&,_K, n−1, + (1)

e

e

_&

0

=

s

0

>

s

0

<

s

性參數： f f f = ˆ+Δ (2)

( )

X t F f ≤ , Δ (3) fˆ 為系統函數估測值，Δ 為 f 的變動量，f X =

[

x,x&,_L,x( )n−1

]

T， F 為系統未確定 量的邊界值。設計滑動模式控制器來追蹤訊號x_d

( )

t ，當t→∞時系統的輸出x

( )

t 能夠逼近x_d

( )

t ，即：

( )

( )

[

]

0 lim − = ∞ → xt xd t t (4) 追蹤誤差定義為：

( ) ( )

t x t x

( )

t e = − _d (5) 在滑動模式控制中，為了使系統軌跡進入滑動面後，依此滑動面滑入目標點。首 先必須設計一個滑動面，根據誤差量，在 n R 狀態空間裡定義一個時變的線性微 分方程式s

( )

X,t [9]：

( )

e

( )

t dt d t x s n 1 , − ⎟ ⎠ ⎞ ⎜ ⎝ ⎛ ₊ = λ ，s =

{

X s

( )

X,t =0

}

(6) 其中λ 是一個大於零的常數，而且此滑動平面經過超平面原點上的方程式為

( )

X,t =0 s ，稱之為滑動平面(Sliding Surface)。一旦選定滑動平面後，就可以開 始著手設計控制器，並定義滑動條件，表示如下： s s s&≤−η (7) 其中η >0，並進一步將(14)式整理為：

( )

s s s s s s s dt d _η − ≤ = = sgn 2 1 2 & & 即必須滿足以下的不等式

( )

s ≤−η s sgn& 其中

( )

⎩ ⎨ ⎧ < − > = 0 1 0 1 sgn s s s 考慮(2)式的非線性系統含有不確定項，為了滿足滑動條件，我們選擇加入一個 不連續的控制器v 。因此控制器_s u可分為 uˆ 和v 兩項，即： _s s v u u= ˆ+ (8) 為了證明所設計的控制器u滿足滑動條件的要求，選取 Lyapunov 函數：

( )

2 2 1 s s V = (9)

控制目標在於設計適當的u，使得V&

( )

s <0。 我們希望V& ≤−ηs ，確保系統的穩定性及滿足滑動條件如(14)式： 於是可推得k−F−η ≥0，即： η + ≥ F k (10) 當(10)成立時，代表系統軌跡穩定收斂至誤差為零的附近。 ⊗ k − k + x − e 圖 3-3 SMC 控制器與感測元件整合設計的架構圖 3.1.2 遭遇之困難及解決途徑 事實上，在控制訊號中含有一個不連續的控制器v 此控制器中使用了一個不s 連續的切換函數(sgn)，而這個函數必須藉著無窮大的切換頻率才可能完成，可是 這種極高頻率的切換速度在現實的物理系統中是無法實現的，因此一般都只是利 用極高速的切換元件來取代，這樣的話，系統軌跡一定會在s=0兩側的極小空 間中不斷來回跳動，造成高頻抖震現像。 3.1.3 解決方式 為了改善抖震的問題，通常引入邊界層：

( )

{

=Φ

}

= X s X t B , (11) 因為切換條件無法在極短的瞬間內完成，所以切換條件一定要面臨到延遲的效 應，使得系統軌跡只能在滑動模式的兩側不斷的來回振盪。目前最常採用的方式 是以邊界層觀念( boundary layer )取代滑動面來加以改進，如(圖 3- 4) 。

e

e

&

0

=

s

Φ

ε

圖 3-4、邊界層示意圖 滑動面s的邊界範圍如(圖 3- 4) ，誤差也是在ε 的邊界範圍，其中 Φ 為邊界層， ε 為誤差邊界範圍。

3.2 慣性導航系統 Inertial Navigation System (INS)

慣性導航系統是由AHRS與導航方程式組合而成 AHRS量測並輸出姿態、角 速度與加速度，經由座標轉換及導航方程式做積分運算，可得到載具的速度和位 置[10][11]；如(圖3-5) 為導航方程式流程圖，Equation(1)為其座標轉換矩陣，由 載具本身座標轉換至NED座標。

其中Equation(19) 座標轉換矩陣，載具本身座標轉換至NED座標

⎥

⎥

⎥

⎦

⎤

⎢

⎢

⎢

⎣

⎡

−

+

−

+

+

+

−

=

φ θ φ θ θ φ θ ψ φ ψ φ θ ψ φ ψ θ ψ φ θ ψ φ ψ ψ θ ψ φ ψ θ ψ

C

C

S

C

S

C

S

S

S

C

S

S

S

C

C

C

S

C

S

C

S

S

S

S

C

C

S

C

C

C

_NEDbody (19)

)

sin(

),

sin(

),

sin(

:

,

,

)

cos(

),

cos(

),

cos(

:

,

,

:

:

:

ψ

θ

φ

ψ

θ

φ

ψ

θ

φ

ψ θ φ ψ θ φ

S

S

S

C

C

C

angle

Heading

angle

Pitch

angle

Roll

3.3 擴展式卡爾曼濾波器 Extended Kalman Filter (EKF)

由於慣性量測元件之加速計(Accelerometer)與陀螺儀(Gyroscope)輸出之量測 直存在一誤差，透過導航方程式的積分之後此誤差對於速度和位置姐的影響會隨 時間遞增，因此須經由濾波器來修正；擴展式卡爾曼濾波器可做非線性估測 [12]，由GPS提供6個觀測值(observation)，位置與速度，AHRS經由導航方程式輸 出12個量測值(measurement)，位置、速度、姿態、加速度，藉由6個觀測值與12 個量測值，擴展式卡爾曼濾波器可輸出12個估測值，位置、速度、姿態、加速度 偏差量(bias)，回傳給導航方程式修正其誤差，藉此增加導航系統之精確度以及 可靠度；(圖3-6) 為EKF流程圖。 圖3-6 EKF流程圖

第四章 實驗結果與討論

4.1.1

滑動模式控制之模擬結果 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) Q ( ra d/ s) Q Output Q d= 0 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) Q ( ra d /s ) Q Output Q_d= 0 圖 4-1 (無干擾)橫向輸出響應圖 圖 4-2 (有干擾)橫向輸出響應圖 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) P ( ra d /s ) P Output P d= 0 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) P ( rad/ s ) P Output P_d= 0 圖 4-3 (無干擾)縱向輸出響應圖 圖 4-4 (有干擾)縱向輸出響應圖 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) W ( m /s ) W Output W_d= 0 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) W ( m /s ) W Output W_d= 0 圖 4-5 (無干擾)主旋翼輸出響應圖 圖 4-6 (有干擾)主旋翼輸出響應圖

0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10 Time(ses) R ( ra d /s ) R Output R_d= 0 0 1 2 3 4 5 6 7 8 9 10 -2 0 2 4 6 8 10 Time(ses) R (r a d /s ) R Output R_d= 0 圖 4-7 (無干擾)尾旋翼輸出響應圖 圖 4-8 (有干擾)尾旋翼輸出響應圖 4.1.2 滑動模式控制之討論 在第一個模擬中，(圖 4-1) 、(圖 4-3) 、(圖 4-5) 及(圖 4-7) 為直昇機姿態在 無干擾情形下的輸出模擬結果，可以發現本研究所設計的控制器可以使直昇機系 統的姿態，在極短的時間內達到目標值，並維持動作的穩定與正確性。 在第二個模擬中，(圖 4-2) 、(圖 4-4) 、(圖 4-6) 及(圖 4-8) 為直昇機姿態在 有干擾情形下的輸出模擬結果，我們經由模擬的結果可發現，仍然可以很快追蹤 到目標且穩定的收斂至零的附近。 4.2.1 飛測資料 日期：2007/08/02 時間：約 08:00-09:00 地點：西港大橋旁 天氣：良好，無風 實驗參與人員：林建宏、曾笠哲、鄭憲君、鄭宇翔、胡意婷 載具操作：鄭憲君 影像紀錄：林建宏、鄭宇翔、胡意婷 地面站監控：曾笠哲、鄭宇翔 實驗項目：

1. 系統鑑別 ( Sine input for system identification )

原先預計按照之前設計之實驗方式，切換自動飛行模式，讓 OBC 自動送出 sine signal ，但實驗當天切換時發現動作過大，因此載具操作者決定以自行操作 的方式，嘗試 sine input；兩個動作分別進行數次，結果將以圖說明。

2. 模糊控制器測試 ( Fuzzy controller test )

曾笠哲同學撰寫之 fuzzy control [13]，在此之前已先於航太草皮進行手控停 懸測試，進行手控輸入與 fuzzy controller 輸入之比對，確認 fuzzy controller 之 輸入合理後，再實際進行 fuzzy control 之飛行測試；所有實驗項目均限制於 roll

4.2.2 實際飛行測試結果 1. 開回路系統測試 1.1. 遙控器之 PWM 訊號值 0 1000 2000 3000 4000 5000 6000 1.2 1.4 1.6 1.8x 10 4 0 1000 2000 3000 4000 5000 6000 1.4 1.6 1.8x 10 4 PW M 0 1000 2000 3000 4000 5000 6000 1 1.5 2x 10 4 time(0.1sec) Roll(PWM) Pitch(PWM) Throttle(PWM) 圖 4-9-1 PWM 訊號值輸出 0 1000 2000 3000 4000 5000 6000 1.2 1.4 1.6 1.8x 10 4 Heading (PWM) 0 1000 2000 3000 4000 5000 6000 0 2 4x 10 4 PW M SW(PWM) 0 1000 2000 3000 4000 5000 6000 1 2 3x 10 4 time(0.1sec) Collective pitch(PWM) 圖 4-9-2 PWM 訊號值輸出

0 1000 2000 3000 4000 5000 6000 1.2 1.3 1.4 1.5 1.6 1.7 1.8x 10 4 time(0.1sec) PW M Roll(PWM) Pitch(PWM)

圖 4-9-3 Roll 與 Pitch 之 PWM (sine input)訊號值輸出

1.2. IMU 之姿態輸出值 0 1000 2000 3000 4000 5000 6000 -20 0 20 40 0 1000 2000 3000 4000 5000 6000 -10 0 10 20 A n gl e (de g) 0 1000 2000 3000 4000 5000 6000 0 200 400 time(0.1sec)

0 1000 2000 3000 4000 5000 6000 -100 0 100 200 0 1000 2000 3000 4000 5000 6000 -100 0 100 A n gl e rat e (d eg /s ec ) 0 1000 2000 3000 4000 5000 6000 -100 -50 0 50 time(0.1sec) 圖 4-10-2 三軸姿態角速率輸出 0 1000 2000 3000 4000 5000 6000 -10 0 10 0 1000 2000 3000 4000 5000 6000 -10 0 10 A c c e le ra ti o n (m /s e c 2 ) 0 1000 2000 3000 4000 5000 6000 -30 -20 -10 0 time(0.1sec) 圖 4-10-3 三軸加速度輸出

1.3. GPS 輸出值 0 2000 4000 6000 23.1058 23.106 23.1062 23.1064 0 2000 4000 6000 120.2106 120.2108 120.211 p o s it io n 0 2000 4000 6000 0 10 20 30 time(0.1sec) 0 2000 4000 6000 0 5 10 0 2000 4000 6000 0 200 400 ve lo ci ty (m /s e c ) 0 2000 4000 6000 -5 0 5 time(0.1sec) 圖 4-11 GPS 輸出 2. 閉回路系統測試 2.1. 模糊控制之橫向姿態控制 0 500 1000 1500 2000 2500 3000 3500 4000 4500 -20 -15 -10 -5 0 5 10 15 time(0.1sec) O u tp ut A n g le( de g ) Roll Angle SW Desired Angle 0 500 1000 1500 2000 2500 3000 3500 4000 4500 -15 -10 -5 0 5 10 15 time(0.1sec) E rr o r A ngl e (d eg) Error Angle SW

3200 3250 3300 3350 3400 3450 3500 3550 3600 3650 3700 -6 -4 -2 0 2 4 6 8 10 12 time(0.1sec) O u tp ut A n g le( de g) Roll Angle SW Desired Angle 3200 3250 3300 3350 3400 3450 3500 3550 3600 3650 3700 -15 -10 -5 0 5 10 15 time(0.1sec) E rr o r A n g le( de g) Error Angle SW 圖 4-12-2 (Roll)橫向姿態控制輸出(放大) 2.2. 模糊控制之縱向姿態控制 0 500 1000 1500 2000 2500 3000 3500 4000 -20 -15 -10 -5 0 5 10 15 time(0.1sec) O u tp ut A n gl e (de g ) Pitch Angle SW Desired Angle 0 500 1000 1500 2000 2500 3000 3500 4000 -15 -10 -5 0 5 10 15 time(0.1sec) E rr o r A ngl e( deg ) Error Angle SW 圖 4-13-1 (Pitch)縱向姿態控制輸出

2400 2450 2500 2550 2600 2650 2700 2750 2800 2850 -6 -4 -2 0 2 4 6 8 10 12 time(0.1sec) O u tput A ngl e( deg) Pitch Angle SW Desired Angle 2400 2450 2500 2550 2600 2650 2700 2750 2800 2850 -6 -4 -2 0 2 4 6 8 10 12 time(0.1sec) E rr o r A ngl e( deg) Error Angle SW 圖 4-13-2 (Pitch)縱向姿態控制輸出(放大) 4.2.3 模糊控制之討論 ¾ 開回路系統測試之飛測資料分析 (圖 4-9) 、(圖 4-10) 及(圖 4-11) 為切換自動模式(0)，讓 OBC 自動送出 sine signal 給直昇機做姿態輸出響應的飛測資料，但實驗當天因切換時發現動作量會 過大，因此載具操作者決定以自行操作的方式，嘗試用人為操控做出類似 sine input 的方式，使直昇機姿態在無干擾情形下完成飛測輸出結果；於是從飛測後 所收集的資料，可以發現本次的收到的資料，無發現雜訊量過大情形，所以可以 提供較好的實驗資料做系統模型，並可進一步了解直昇機在開回路系統的設計下 姿態輸出響應的情形。 ¾ 閉回路系統測試之資料分析 (圖 4-12) 及(圖 4-13) 為切換自動模式(1)，讓 OBC 自動控制直昇機的 Roll 及 Pitch 的姿態飛測資料，而自動模式(1)為模糊控制法則，此種控制設計方式不 須要系統的數學模型就能夠去實現控制直昇機，但是由於直昇機系統本身太過於 複雜，所以本實驗先從各別獨立單軸姿態控制設計，而現階段初步的控制設計是 一次飛測只能控制單軸姿態，所以目前的技術已經可以初步做單軸姿態控制，我 們可以從(圖 4-12-2) 及(圖 4-13-2) 中可以發現實際輸出的姿態有追蹤到我們預 定目標，而且從飛測資料中可以知道閉迴路系統加入模糊控制律是可以讓整體系 統穩定控制，不過在控制性能響應上較差，需要花一段時間來調整控制器的增益 值。

4.3 導航系統之實際測試 4.3.1 靜態測試 系統靜態測試是提供我們驗證 GPS/INS 演算法正確性與效能的方式，藉由 靜態測試可以看出系統的誤差以及其趨勢，藉此找出較佳的參數設定並對演算法 正確性獲得驗證。 先觀察 AHRS 輸出姿態與加速度，經由導航方程式積分得到速度與位置解 (圖 4-14) 與(圖 4-15) ，可觀察到速度與位置的垂直方向誤差皆大於水平方向， 由於加速計的偏差垂直方向大於水平方向，尤其受重力影響甚大；(表 4-1) 為導 航方程式誤差量。 圖4-14 AHRS輸出量測值經由導航方程式積分得速度變化圖

圖4-15 AHRS輸出量測值經由導航方程式積分得位置變化圖 速度誤差(m/s) 位置誤差(m) North 0.10096 6.77521 East 0.08479 5.17184 Down 1.48821 90.63609 表4-1 AHRS輸出量測值經由導航方程式積分產生之誤差量 我們使用擴展式卡爾曼濾波器作修正，回傳速度與位置估測值至導航方程 式；(圖 4-16) 及(圖 4-17) 為其修正結果，(表 4-2) 為經由 EKF 修正之誤差量。

圖4-16 EKF回傳速度、位置修正導航方程式積分得速度變化圖

速度誤差(m/s) 位置誤差(m) North 0.00483 0.01615 East 0.00371 0.01224 Down 0.08072 0.27020 表4-2 經由EKF修正導航方程式積分所產生之誤差量(回傳速度、位置) 以下我們使用擴展式卡爾曼濾波器回傳 12 個估測值，對導航方程式作物差 修正；(圖 4-18) 與(圖 4-19) 為修正結果，(表 4-3) 為經由 EKF 修正之誤差量。 圖4-18 EKF回傳全部估測值修正導航方程式得速度變化圖

圖4-19 EKF回傳全部估測值修正導航方程式得位置變化圖 速度誤差(m/s) 位置誤差(m) North 0.00478 0.01599 East 0.00322 0.01048 Down 0.00654 0.02168 表 4-3 經由 EKF 修正導航方程式積分所產生之誤差量(全部回傳，更新) 4.3.2 動態測試 使用 GPS 由 RTK 輔助，輸出觀測值經由擴展式卡爾曼濾波器修正，經由擴 展式卡爾曼濾波器修正後，可看出 GPS 觀測值變得穩定，尤其是在垂直方向有 較穩定的輸出；(圖 4-19) 、(圖 4-20) ，(圖 4-21) 分別為速度、位置、路徑圖。

圖4-20 動態測試GPS使用RTK輔助，經由EKF修正得到速度變化圖；藍色：

GPS，紅色：EKF，綠色：導航方程式。

圖4-21 動態測試GPS使用RTK輔助，經由EKF修正得到位置變化圖；藍色：

圖4-22 動態測試GPS使用RTK輔助，經由EKF修正得到載具路徑圖；藍色： GPS，紅色：EKF。 4.3.3 飛行測試之導航資料 經由靜態測試與動態測試後，可證明擴展式卡爾曼濾波器對於導航方程式的 修正有極大幫助；以下(圖 4-22) 至(圖 4-24) 是取自 UAH(Raptor 90 RC)的飛行 量測數據經由擴展式卡爾曼濾波器修正結果，受限於機載電腦(on-board computer) 傳輸速度，資訊更新皆為 10Hz，此飛行測試並未對 GPS 使用 RTK 修正。

圖 4-23 GPS/INS 整合之飛測數據，經由 EKF 修正得姿態變化圖；紅色：EKF，

圖 4-24 GPS/INS 整合之飛測數據，經由 EKF 修正得速度變化圖；藍色：GPS，

紅色：EKF，綠色：導航方程式。

圖 4-25 GPS/INS 整合之飛測數據，經由 EKF 修正得位置化圖；藍色：GPS，紅

第五章 結論與未來工作

5-1 控制系統方面 本計劃之目的在於發展旋翼型無人載具之高頻寬控制設計，其中包含了高頻 寬導航系統建立與整合以及後續的高頻寬控制系統建構；另一方面本實驗室將採 用各類控制理論如：模糊控制、非線控制理論之滑動模式控制等方法，並且搭配 高頻寬感測器，使無人直昇機達成預期平穩的控制，並且完成必須之軟硬體建置 整合目標；但是由於直昇機本身太過於複雜，所以在航電系統上需要長期更新與 調整，其目的就是要提高航電系統可靠度的品質，這樣才能夠做進一步的控制器 設計，但是由於直昇機系統本身太過於複雜，所以本實驗會先從各別獨立單軸姿 態控制設計，而現階段初步的控制設計是一次飛測只能控制單軸姿態，所以目前 的技術已經可以初步做單軸姿態控制，使整體系統獲得穩定控制，不過在控制性 能響應上較差，需要花一段時間來調整控制器的增益值，於是往後的計劃進度將 會繼續朝向 Roll 與 Pitch 同時兩軸姿態穩定控制為目標。 5-2 感測系統方面 導航方程式經由擴展式卡爾曼濾波器修正後，確實可大幅增加準確度，能有 效的修正加速計產生之偏差量，也能減緩因 GPS 受到干擾時其量測上的大幅偏 移，但是此擴展式卡爾曼濾波器是以 GPS 之輸出為觀測值，AHRS 輸出為量測 值，如果當 GPS 天線受到干擾或遮蔽時，此擴展式卡爾曼濾波器輸出之估測值 也將受到極大的干擾，如何修正並降低擴展式卡爾曼濾波器因 GPS 不穩定時還 能有效運作，是我們將來著重的地方。 另一部分就是系統 AHRS 轉移，由於不同廠生產的 AHRS 對其座標系統的 定義都有所差異，因此我們將會演算法的變更與驗證，將先前的 GPS/INS 整合 做些微的調整以達成使用 Xsens MTi 取代 Crossbow AHRS 的目標。

最後我們將使用多天線 GPS 系統計算出載具姿態[14]，藉此增加擴展式卡爾 曼濾波器的觀測量，使其獲得位置、速度、姿態之觀測量，讓導航系統更加完善 也更加強健。

參考文獻

1. Pratap Misra and Per Enge, Global Positioning System-Signals, Measurements,

and Performance second Edition, 2006.

2. OEM4 Family of Receivers User Manual – Installation and Operation, NovAtel, Incorporated, 21 June. 2001.

3. Utkin, V. I., Variable structure with sliding mode-A Survey, IEEE Trans Automatic

control, No. 2, pp. 212-222, 1977.

4. Slotine, J. J. E., Sliding controller design for nonlinear systems, International

Journal of Control, No. 2, pp. 421-434, 1984.

5. Ukin, V. I., and Jurgen Gulder, Sliding mode control for gradient tracking and robot navigation using artificial potential fields, IEEE, Vol. 11, no. 2, pp. 247-254, 1995.

6. Lee, J. H., Ko, J. S., Chun, S. K., Lee, J. J., and Youn, M. J., Design of continuous sliding mode controller for BLDDM with prescribed tracking performance, IEEE

Power Electronics Specialists Conference, Toledo, Spain, pp. 770-775, 1992.

7. Waslander, S. L., and Hoffmann, G. M.., Multi-Agent Quadrotor Testbed Control Design: Integral Sliding Mode vs. Reinforcement Learning, IEEE International Conference on Intelligent Robots and Systems, pp. 468-473, 2005.

8. Fu, L. C., and Fu, K. Y., Design of Optimal Midcourse Guidance Sliding-Mode Control for Missiles with TVC, IEEE, Vol. 39, no. 3, pp. 824-837, 2003. 9. Slotine, J. J. E., Applied nonlinear control, Prentice-Hall, NJ, 1991. 10. Myron Kayton and Walter R. Fried, Avionics Navigation Systems second

edition,1997

11. D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation

Technology,1997.

Astronautics, National Cheng Kung University, June 2007

14. Leen Vander Kuylen, Frank Boon and Andrew Simsky, “Attitude

DeterminationMethods Used in the PolaRx2@ Multi-antenna GPS Receiver,” ION GNSS 2005,Long Beach, California.

國防科技學術合作計畫研發成果資料表

日期：2008 年 02 月 18 日 計畫名稱：旋翼型無人載具之高頻寬控制設計 計畫主持人：詹劭勳 計畫編號：NSC 96-2623-7-006-013-D

期刊

Journal of Aeronautics, Astronautics and Aviation (EI Journal), 2008, -Navigation and Guidance System Design for Unmanned Aerial Vehicle

論文

研討會

1. 96 年空軍官校航太年會 – 複合型滑動模式控制器應用於無人 直昇機姿態穩定之控制 2. 96 年中國航太學會 – 垂直起降無人飛行載具之導航系統設計

技術報告

申請

獲得

專利

應用

與軍方研發機構

互動之具體研發

成果

可推廣於民間產

業之技術或可開

發之產品

1.可推廣民間發展陸地無人載具或飛行無人載具之姿態系統設計 2.可發展控制器整合技術 3.可提供民間雙頻衛星定位系統之應用技術 4.可推廣民間發展陸地無人載具或飛行無人載具之導航系統設計 5.可發展感測器整合技術

可推廣之產業別

（如無限通訊、微

機電等）或可能技

轉之廠商

本表若不敷使用，請自行影印使用。