攔截飛彈的姿態控制

國立交通大學

電控工程研究所

碩 士 論 文

攔截飛彈的姿態控制

Attitude Control of Missile Interceptor

研究生：邱泰偉

指導教授：林清安 教授

攔截飛彈的姿態控制

Attitude Control System of Missile Interceptor

研 究 生：邱泰偉 Student: Tai-Wei Chiu

指導教授：林清安 教授 Advisor: Prof. Ching-An Lin

國 立 交 通 大 學

電控工程研究所

碩 士 論 文

A Thesis

Submitted to Institute of Electrical and Control Engineering College of Electrical and Computer Engineering

National Chiao-Tung University In Partial Fulfillment of the Requirements

For the Degree of Master In

Electrical and Control Engineering July 2010

Hsinchu, Taiwan, Republic of China

攔截飛彈的姿態控制

學生：邱泰偉 指導教授：林清安 教授

國立交通大學 電控工程研究所

中文摘要

本論文探討攔截飛彈姿態控制系統的設計與分析，數值模擬參考

標準飛彈三型為藍本。姿態控制的目的是要使攔截飛彈的尋標器在終

端導引階段一直追蹤目標。姿態控制的制動器是常值開關式的推力

器。我們探討兩種控制方法：線性控制系統配合脈衝寬度調變(pulse

width modulation, PWM)的合成以及最短時間最佳控制系統

(minimum-time optimal control)，線性控制器部分主要討論脈衝寬度調

變的頻率設定，最短時間最佳控制則透過修改切換曲線及控制律來降

低至動器的切換頻率。控制系統的性能必須透過完整的六自由度模擬

來驗證，模擬結果顯示如果適當的選擇設計參數，線性控制系統配合

脈衝寬度調變以及最短時間最佳控制系統均可有效地達到控制目

標。最後我們加入質心偏移的影響來分析兩種控制系統的性能表現。

Attitude Control System of Missile Interceptor

Student: Tai-Wei Chiu Advisor: Prof. Ching-An Lin

Institute of Electrical and Control Engineering

National Chiao Tung University

Abstract

This paper studies the design and analysis of attitude control system

for a missile interceptor. Simulations use Standard Missile III as a

prototype. The goal of attitude control is to ensure that the infrared seeker

of the interceptor tracks the target in the terminal guidance phase. The

control system employs constant value ON-OFF thruster as actuators. We

investigate two design methods: linear control system with pulse-width

modulation (PWM) implementation and minimum-time optimal control

system. For the linear control system, we focus on how to choose a

proper PWM frequency. In order to reduce the switching frequency of the

minimum-time optimal control system, we modify the switching curve

and the control law. Simulation result indicate that with proper choices of

design parameters, linear control system with PWM and minimum-time

optimal control system both can achieve design goal. Finally, we analyze

the performance of two type control system by considering the effect of

the g.c. offset.

誌 謝

這篇論文夠順利的完成，首先要感謝我的指導教授林清安老師兩

年來的耐心教導，讓我學習到做研究應該具備的態度及遇到問題時，

如何分析問題進ㄧ步的找出解決的方法，讓我在專業領域中有更深的

體會。

其次要感謝實驗室的學長吳建賢、何威德對我的勉勵與指導，在

我學習的過程中提高寶貴的經驗，讓我的論文能夠順利的進行，另外

也感謝 606 實驗室的所有成員在日常生活及研究上的互相協助。

最後要感謝我的家人的給我的支持與鼓勵，感謝我的父母ㄧ路以

來的包容與付出，有他們的陪伴，讓我在求學路上能夠無後顧之憂的

繼續走下去。

目錄

第一章 緒論...1 第二章 運動方程式...3 2.1 座標系統...3 2.1.1 地面座標S (North-East-Down coordinate) ...3 _N 2.1.2 體座標S (Body coordinate) ...3 _B 2.1.3 視線座標...4 2.1.4 球座標...4 2.2 座標轉換...5 2.3 飛彈六自由度運動方程式...6 2.3.1 尤拉方程式(Euler equation) ...6 2.3.2 力矩方程式...6 2.3.3 平移方程式...7 2.4 目標運動方程式...7 第三章 攔截器推力器配置及數學模式...8 3.1 標準三型飛彈動能攔截器之物性...8 3.2 模擬採用的攔截器物性及推力器配置...9 3.3 推力器的數學模式...11 3.3.1 轉向推力器產生的力矩... 11 3.3.2 姿態推力器產生的力矩...12 3.3.3 轉向及姿態推力器產生的加速度...15 3.3.4 推力器動態...16 第四章 線性姿態控制系統設計...17 4.1 線性控制系統...17 4.2 脈衝寬度調變(PWM)的設計 ...19 第五章 最短時間最佳控制系統...27 5.1 理想時間最佳控制設計...27 5.2 切換曲線的調整...29 5.3 輸出切換控制律的調整...31 5.4 設計方法及模擬結果...34 第六章 轉向導引控制...38 6.1 真比例導引律...38 6.2 脈衝寬度調變(PWM)的合成 ...41 6.3 實際攔截情況...41 第七章 質心偏移...45

7.1 質心偏移量的探討...45

7.2 實際攔截的情況...52

第八章 結論...56

圖目錄

圖 2.1 地面座標與體座標示意圖...3 圖 2.2 慣性座標與視線座標及體座標之間的關係...4 圖 2.3 地面座標與飛彈體座標轉換...5 圖 3.1 標準三型飛彈（SM3 Block IA）的動能攔截器(MK142)...8 圖 3.3 轉向推力器示意圖...11 圖 3.4 攔截飛彈的質心偏移示意圖...12 圖 3.5 六個姿態推力器示意圖...13 圖 3.6 六個姿態推力器的質心偏移示意圖...13 圖 4.1 滾轉軸控制系統...18 圖 4.3 脈衝寬度調變(PWM)訊號示意圖 ...20 圖 4.4 線性姿態控制器的頻率響應...21 圖 4.5 三組不同的姿態角命令...22 圖 4.6 PWM 頻率為 15(Hz)的模擬結果...24 圖 4.7 PWM 頻率為 17(Hz)的模擬結果...25 圖 4.8 PWM 頻率為 20(Hz)的模擬結果...26 圖 5.1 滾轉軸時間最佳控制器方塊圖...28 圖 5.2 三個姿態角命令...30 圖 5.3 死區控制律示意圖...32 圖 5.4 延遲切換控制律...33 圖 5.5 姿態角命令為斜坡時的模擬結果...35 圖 5.6 姿態角命令為弦波時的模擬結果...36 圖 5.7 姿態角命令為三角波時的模擬結果...37 圖 6.1 相對位置與球座標之間的關係...38 圖 6.2 實際攔截軌跡...42 圖 6.3 姿態控制為線性控制器的模擬結果...43 圖 6.4 姿態控制為最短時間最佳控制器的模擬結果...44 圖 7.1 假設轉向推力器開啟的情形...47 圖 7.2 姿態角命令為斜坡，(dx,dy,dz)=(6.6,0,0)mm 的角度誤差及噴嘴開啟情況48 圖 7.3 姿態角命令為斜坡，(dx,dy,dz)=(0,1.2,0)mm 的角度誤差及噴嘴開啟情況49 圖 7.4 姿態角命令為斜坡，(dx,dy,dz)=(0,0,1.6)mm 的角度誤差及噴嘴開啟情況49 圖 7.5 姿態角命令為斜坡，(dx,dy,dz)=(6.2,0,0)mm 的角度誤差及噴嘴開啟情況51 圖 7.6 姿態角命令為斜坡，(dx,dy,dz)=(0,3.9,0)mm 的角度誤差及噴嘴開啟情況51 圖 7.7 姿態角命令為斜坡，(dx,dy,dz)=(0,0,3.1)mm 的角度誤差及噴嘴開啟情況52 圖 7.8 最短時間最佳控制，(dx,dy,dz)=(4.8,0,0)mm 的角度誤差 ...53 圖 7.9 修正後的最短時間最佳控制，姿態角誤差及噴嘴開啟情況...55

表目錄

表 3.1 標準三型飛彈動能攔截器(MK142)的物性參數 ...9 表 3.2 模擬時採用的攔截器物性...10 表 3.3 六個姿態推力器各別造成的力矩...14 表 4.1 姿態角命令為斜坡函數的模擬結果...22 表 4.2 姿態角命令為弦波函數的模擬結果...22 表 4.3 姿態角命令為三角波函數的模擬結果...23 表 5.1 不同 k 值的模擬結果...31 表 5.2 不同死區寬度的模擬結果...32 表 5.3 不同延遲切換寬度的模擬結果...33 表 6.1 實際攔截情況的模擬結果...42 表 7.1 姿態控制為線性控制時質心偏移的模擬結果...47 表 7.2 姿態控制器為最短時間最佳控制時質心偏移的模擬結果...50 表 7.3 實際攔截情況下，質心偏移的模擬結果...53 表 7.4 修正後的最短時間最佳控制，質心偏移的模擬結果...54

第一章 緒論

本論文主要探討攔截飛彈在終端航程的姿態控制系統設計與分析。終端航程 的攔截器參考標準三型飛彈(SM-3 Block IA)的動能攔截器(MK142)為藍本，攔截 器上配置一個紅外線尋標器(infrared seeker)及轉向姿態控制系統(Divert and Attitude Control System, DACS)，包含四個側向的開關式(ON-OFF type)的轉向推 力器(divert thruster)和六個開關式的姿態推力器(attitude thruster)。

攔截器在攔截目標時必須靠紅外線尋標器追蹤目標，因此利用姿態推力器控 制攔截器保持目標ㄧ直持續在鎖定的狀態。側向的轉向推力器提供加速度的需求 以達成直接碰撞攔截，所以導引律方面我們採用真比例導引律(true proportional navigation law)[1]，其加速度方向是和視線垂直，所以我們也必須控制攔截器體 座標的 X 軸和視線重合。因此基於以上兩點，我們可以知道姿態控制系統在終 端攔截時的重要性。 本論文探討兩種姿態控制系統，第一種為採用線性常增益姿態角及角速度的 回授控制，並配合脈衝寬度調變(pulse width modulation, PWM)[2]的方式合成實 現開關式的輸出。其中 PWM 的調變頻率為一個需要選定的參數，不能夠超過硬 體的限制，也必須控制誤差在可以接受的範圍內。第二種為最短時間最佳控制系 統(minimum-time optimal control)[3]、[4]，此控制系統具有一個通過原點的切換 曲線，將系統狀態從一個初始值在最短時間控制到切換曲線上後再推到原點，輸 出的命令直接就是開關式的訊號。然而這種控制方法必須要有很大的控制力將系 統狀態控制到切換曲線上，進入切換曲線後透過非常高頻的切換到達原點，在實 際使用上必須經過一些調整，降低切換頻率在可實現的範圍且和誤差之間做一個 取捨。最後我們除了採用典型理想攔截的模擬情況外還加入質心偏移模擬產生姿 態干擾的情形，分別比較兩種姿態控制系統的差異性。

本論文第二章介紹攔截器及目標的運動方程式，第三章介紹攔截器的推力器 配置及數學模式，第四章介紹線性控制系統及 PWM 的合成，第五章介紹最短時 間最佳控制系統，第六章為轉向導引控制及實際攔截情況，第七章探討加入質心 偏移後的影響，第八章為結論。

第二章 運動方程式

飛彈在空間中的運動可分為平移部份與轉動部份，各為三個自由度的運動， 因此整個三維空間中飛彈為六個自由度的運動。此外要在三度空間中描述飛彈動 態就必須要定義所需的座標系統，以下我們介紹所需的所有座標系統及慣性座標 和體座標之間的相互關係，接著分別介紹飛彈六自由度的運動方程式及目標的運 動方程式。 2.1 座標系統 2.1.1 地面座標S (North-East-Down coordinate) N 地面座標S (NED 座標系)的原點為地表的雷達站，為固定不動的座標，_N X_N 軸指向地表平面的北方，Y 軸指向地表平面的東方，_N Z 軸滿足右手定則。如圖N 2.1，假設飛彈在S 座標的位置為_N Kr_m =

[

x_N y_N z_N

]

T，則x 表示飛彈在雷達站北_N 方的距離，y 表示飛彈在雷達站東方的距離，_N − 表示飛彈的高度。 z_N 圖 2.1 地面座標與體座標示意圖 2.1.2 體座標S (Body coordinate) _B 體座標用來表示飛彈本身的姿態變化，為一個動座標系，其原點為飛彈的質 心位置，X 為飛彈的長軸方向，指向前方，_B Y 垂直_B X 指向飛彈右邊，_B Z 則垂_B 直於X ─_B Y 平面，指向飛彈下方，三軸互相垂直且滿足右手定則，如圖 2.1 所_B 示。

2.1.3 視線座標 我們使用視線座標是爲了方便描述攔截飛彈與目標之間的瞄準線，也就是攔 截飛彈與目標在S 座標的相對位置，是爲了與體座標作區別，如圖 2.2 所示。由_N 於真比例導引律的導引加速度與視線垂直，若攔截飛彈的姿態一直對準目標，就 能藉由彈體側向推力器來達成導引加速度。所以視線座標方向是理想攔截飛彈的 姿態方向，而體座標則是實際上攔截飛彈的姿態方向。如果攔截飛彈的姿態已受 控制為一直對準目標，表示體座標的X 與視線座標的_B X 重合，則_L Y_B =Y_L、 B L Z =Z 。 2.1.4 球座標 我們使用球座標是爲了方便推導三維真比例導引律以及描述攔截飛彈的理 想姿態，如圖 2.2 所示。假設目標在S 座標為_N r ，則_t cos cos cos sin sin t t x y z α μ α μ α ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ =_{⎢ ⎥}= _{⎢ ⎥} ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ r r 其中 1 2 2 tan z x y α ₌ − ⎛_⎜ ⎞_⎟ ⎜ ₊ ⎟ ⎝ ⎠ ， 1 tan y x μ ₌ − ⎛ ⎞ ⎜ ⎟ ⎝ ⎠， 2 2 2 = t x +y +z r 。 X_N Y_N Z_N Targe t Missi le

r

t

r

m

r

X_B

a

µ

Radar X_L 圖 2.2 慣性座標與視線座標及體座標之間的關係

2.2 座標轉換 為了描述飛彈的動態，我們推導地面座標與體座標之間的轉換，由地面座標 經過三個角度的旋轉得到飛彈的體座標，這種有次序的角度旋轉稱為尤拉角 (Euler Angle)，通常用來描述一剛體的方位。首先將體座標和地面座標的原點重 合起來，再經過以下有順序的三個旋轉，我們就可以將地面座標旋轉到體座標， 如圖 2.3 所示： 圖 2.3 地面座標與飛彈體座標轉換 1. 固定地面座標的Z 軸旋轉_N ψ 角，直到X 與_N X 重合，得到新的三軸為₁

(

X , Y , Z1 1 1

)

，其中Z1=ZN。 1 N N 1 N 1 N 1 N N cos sin 0 sin cos 0 T 0 0 1 x x x y y y z z z ψ ψ ψ ψ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢_{= −} ⎥ ⎢ ⎥_{= ⋅}⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.1) 2. 在固定Y 旋轉θ 角，直到1 X 與1 X 重合 (B X 也就是B X )，得到新的三軸為2

(

X , Y , Z2 2 2

)

，其中Y2 =Y1。 2 1 1 2 1 2 1 2 1 1 cos 0 sin 0 1 0 T sin 0 cos x x x y y y z z z θ θ θ θ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢₌ ⎥ ⎢ ⎥_{= ⋅}⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢− ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.2) 3. 最後固定X 旋轉₂ φ角，直到Y 與₂ Y 重合、_B Z 與₂ Z 重合，得到體座標為 _B

(

X , Y , Z_{B B B}

)

，其中X_B=X₂。 B 2 2 B 2 3 2 B 2 2 1 0 0 0 cos sin T 0 sin cos x x x y y y z z z φ φ φ φ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢₌ ⎥ ⎢ ⎥_{= ⋅}⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ − ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.3) X N Z N Y N X 1 Z 1 Y 1 ? ? Z_B X_B Y Z 2 X₂ ? Y B ? 2 Y 2 X 1 Z 1 Z 2 X₂ Y 1 ? ?

結合以上(2.1)~(2.3)式，我們可以得到S 座標與_N S 座標的轉換為 B B N N B 3 2 1 N BN N B N N T T T T x x x y y y z z z ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥_{= ⋅}⎢ ⎥_{= ⋅}⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

cos cos cos sin sin

cos sin sin cos sin sin sin sin cos cos sin cos

cos cos sin sin sin sin cos sin sin cos cos cos

BN T θ ψ θ ψ θ ψ φ θ φ ψ ψ φ θ φ ψ φ θ ψ φ θ φ ψ ψ φ θ φ ψ φ θ − ⎡ ⎤ ⎢ ⎥ =_⎢ − + _⎥ ⎢ + − ⎥ ⎣ ⎦ (2.4) 若是要計算從體座標轉換到地面座標的轉換矩陣則為 ( )T NB BN T = T (2.5) 其中 (T_BN)T為T_BN的轉置矩陣。 2.3 飛彈六自由度運動方程式 2.3.1 尤拉方程式(Euler equation) 我們假設(p,q,r)分別為飛彈在滾轉、俯仰及偏航方向的角速度，配合前ㄧ節 定義的尤拉角 ( , , )φ θ ψ ，透過簡單的推導可以得到尤拉角和角速度之間的一階微 分方程式，稱為尤拉方程式[11]：

(

)

(

)

tan sin cos

cos sin

sec sin cos

p q r q r q r φ θ φ φ θ φ φ ψ θ φ φ = − + = − = +    (2.6) 2.3.2 力矩方程式 力矩方程式描述飛彈的角速度與所受力矩的關係在彈體對稱的假設下，方程 式可表示為

(

)

(

)

(

)

x x zz yy xx xx xx y xx zz yy yy z yy xx zz zz M qr M p I I I I I M rp q I I I I M pq r I I I I = − − = = − − = − −    (2.7)

若圓柱體的半徑為 r，長度為 L，質量為 m 且均勻分布，則 2 2 xx mr I = 、 2 2 4 12 yy zz mr mL I =I = + ，其中I ，_xx I ，_yy I 分別為飛彈三軸的轉動慣量，zz M ，x M ，y z M 分別為飛彈三軸所受的力矩。 2.3.3 平移方程式 飛彈的平移運動方程式在(慣性)地面座標下是單純的積分關係，即加速度的 兩次積分為位置，因此飛彈的平移方運動程式可表示為 mx mx my my mz mz v a v a v a = = =    (2.8) m mx m my m mz x v y v z v = = =    (2.9) 其中飛彈在地面座標的加速度分量(a ,_mx a ,my a )， (mz v ,mx v ,my v )為飛彈在地面座mz 標的速度分量，(x ,_m y ,_m z )為飛彈在地面座標的位置分量。 _m 2.4 目標運動方程式 我們假設目標為點質量，不考慮姿態，只考慮目標在三度空間的平移運動， 由於終端攔截的高度很高，忽略空氣阻力，僅考慮受到重力的影響，其運動方程 式以慣性座標的分量表示為 tx tx ty ty tz tz v a v a v a = = =    t tx t ty t tz x v y v z v = = =    其中(a ,_tx a ,_ty a )為目標在慣性座標的加速度分量， (_tz v ,_tx v ,_ty v )為目標在慣性座標_tz 的速度分量，(x ,_t y ,_t z )為目標在慣性座標的位置分量。 _t

第三章 攔截器推力器配置及數學模式

攔截器配備兩種推力器：轉向推力器及姿態推力器。轉向推力器的推力通常 作用在欄截器的質量中心，提供攔截器Y 及_B Z 方向上的加速度。姿態推力器通_B 常配置在攔截器的尾端，提供三軸的力矩。然而在攔截過程中由於燃料消耗造成 質量及質量分布的改變，因此質心的位置也隨著變化。消除轉向推力器因此造成 的干擾力矩(disturbance torque)的影響是姿態控制系統的功能之一。在現存的攔截 系統，如標準三型飛彈的動能攔截器 MK142，轉向及姿態的控制是由一個整合 的軌控與姿控系統(divert and attitude control system, DACS)來完成。

3.1 標準三型飛彈動能攔截器之物性 攔截器的物性資料在分析、設計及模擬時是必需的參數，如重量、尺寸、外 型、推力器配置等，所有資料均參考標準三型飛彈(SM-3 Block IA)的動能攔截器 (MK142)，如圖 3.1 所示。根據所得之資料[6]，MK142 的物性參數如表 3.1 所示。 MK142 配有四個轉向推力器，激發時可單軸提供 1.5-1.8g 的加速度。俯仰軸的 姿態由兩個推力器控制，偏航及滾轉軸的姿態由四個推力器共同控制。燃料的質 量約占總質量的 20%。 圖 3.1 標準三型飛彈（SM3 Block IA）的動能攔截器(MK142)

表 3.1 標準三型飛彈動能攔截器(MK142)的物性參數 質量(kg) 16.798(除燃料外) 4.54(燃料) 21.338(總量) 長度(m) 0.5588 直徑(m) 0.254 轉向推力器推力(N) 314.9184( 4× ) 姿態推力器推力(N) 滾轉(roll) 偏航(yaw) 俯仰(pitch) 15.568( 4× ) 31.5808( 2× ) 3.2 模擬採用的攔截器物性及推力器配置 模擬採用的攔截器的物性大致與標準三型 MK142 攔截器一樣。為了方便 轉動慣量的計算，假設攔截器為圓柱體，長 55.88 公分，半徑 12.7 公分，不含燃 料的質量為 16.34 公斤，燃料質量為 4.99 公斤。體座標的中心訂於質量中心，X_B 軸的方向定為長軸方向。有四個轉向推力器安置於圓柱體的幾何中心(理想的狀 況下，幾何中心與質量中心一致)，其推力在± 及Y_B ± 的方向。姿態推力器配Z_B 置於離圓柱體尾端 5 公分的平面上，模擬時採用六個姿態推力器的配置。圖 3.2 是攔截器推力器的配置圖，其中顯示轉向推力器在± 及Y_B ± 的方向，g.c.為幾Z_B 何中心，c.m.為質量中心。 其中四個轉向推力器能產生最大的的推力為 315 牛頓。六個姿態推力器配置 中，兩個俯仰推力器皆可以產生最大的推力為 31.6 牛頓，四個偏航─滾轉推力 器皆可產生最大推力為 15.8 牛頓。攔截器的物性如表 3.2 所示。

g.c.

X

B

Z

B

Y

B

X

B 0.05m 0.2294m 0.2794m 0.127m 0.2794m g.c. divert thruster attitude thruster 圖 3.2 攔截器圓柱體與推力器配置示意圖 表 3.2 模擬時採用的攔截器物性 六個姿態推力器 質量(kg) 淨重 16.34 燃料 4.99 總重 21.33 長度(m) 0.5588 半徑(m) 0.127 轉向推力器最大推力(N) 315 姿態推力器最大推力(N) 滾轉軸 俯仰軸 偏航軸 15.8 31.6 15.8

攔截器的轉動慣量採用簡單的圓柱體公式計算；圓柱體長度為 L，半徑為 r， 質量為 m，則X ， _B Y ， _B Z 三軸的轉動慣量是分別為 _B 2

2

xx

mr

I

=

， 2 2 4 12 yy zz mr mL I =I = + (3.1) 由於這個計算要準確必須要質量分布均勻，在實際的攔截器上這種情況可能不成 立，因此誤差存在可能不小。不過從模擬的觀點，定性的性質如慣量隨燃料消耗 漸漸減，比定量的準確度重要。用上面的公式計算，在燃料未消耗的狀況下， xx I =0.1720 kg-m2，I =_yy Izz=0.6413 kg-m 2 。 3.3 推力器的數學模式 從 3.2 節我們可以得到攔截器的轉向與姿態推力器的配置設計。推力器的推 力使得攔截器產生加速度與力矩，發生平移與旋轉的現象。在這一節我們將分別 討論攔截器的力矩與加速度。 3.3.1 轉向推力器產生的力矩 四個轉向推力器的配置如圖 3.3 所示，其推力作用於攔截器的幾何中心，分 別在±Y_B及±Z_B的方向上產生推力。 f_DA1,f_DA2,f_DB1,f_DB2 ≥ 為轉向推力器 DA1、0 DA2、DB1 及 DB2 所產生的推力大小。

DA2

DA1

DB1

DB2

Y

B

Z

B

X

B

4 divert thrusters

R

g.c.

圖 3.3 轉向推力器示意圖

我們假設理想狀況下攔截器的質心是在攔截器圓柱體的幾何中心，也就是轉 向推力器所固定的位置，但實際上攔截器在製造的過程中可能產生偏移或是燃料 消耗後質量分部改變的因素，攔截器的質心不一定會在幾何中心的現象，我們稱 為攔截器的質心偏移。 由於攔截器體座標的原點是定義在質心上，質心偏移會造成幾何中心的位置 在體座標下改變為(dx,dy,dz)，當有質心偏移的現象發生時，轉向推力器的推力將 不是作用在質心上，如圖 3.4 所示，使得轉向推力器對攔截器產生了額外的力矩 分別為 1 2 2 1 2 1 1 2 ( ) ( ) ( ) ( ) xd DA DA DB DB yd DA DA zd DB DB M dy f f dz f f M dx f f M dx f f = × − + × − = × − = × − (3.2) 其中M_xd、M_yd、M_zd分別為X ，_B Y 及_B Z 方向的力矩。 _B g.c. c.m. dx dz dy X_B Y_B ZB DA1 DA2 DB1 DB2 圖 3.4 攔截器的質心偏移示意圖 3.3.2 姿態推力器產生的力矩 六個姿態推力器的配置如圖 3.5 所示，其中推力器 AA1，AA2 提供俯仰的 力矩，而推力器 AB1─AB4 共同提供偏航及滾轉的力矩。由於質心偏移使得幾 何中心的座標改變為(dx,dy,dz)，因此改變了姿態推力器的力臂，如圖 3.6 所示， 造成姿態推力器產生的力矩也跟著改變。

AA2

AA1

Y

B

Z

B

X

B 6 attitude thrusters

R

p/4

p/4

AB4

AB1

AB2

AB3

圖 3.5 六個姿態推力器示意圖 g.c. DA1 DA2 DB1 DB2 AA1 AA2 AB1 AB3 La AB4 AB2 c.m. dy XB YB dx dz ZB 圖 3.6 六個姿態推力器的質心偏移示意圖 我們各別討論六個推力器所造成的力矩，如表 3.3 所示，其中 R 為攔截器的 半徑，La為攔截器圓柱體幾何中心到姿態推力器作用平面的距離，以及 1, 2, 1, 2, 1, 2, 3, 4 0 AA AA AB AB AC AC AC AC f f f f f f f f ≥ 為姿態推力器所產生的推力。

表 3.3 六個姿態推力器各別造成的力矩 AA1： AA2： , 1 1 , 1 1 , 1 ( ) 0 x AA AA y AA AA z AA M dy f M La dx f M = × = − × = , 2 2 , 2 2 , 2 ( ) 0 x AA AA y AA AA z AA M dy f M La dx f M = − × = − − × = AB1： AB3： , 1 1 , 1 , 1 1 2 0 ( ) x AB AB y AB z AB AB R M dz f M M La dx f ⎛ ⎞ = −_⎜ − _⎟× ⎝ ⎠ = = − × , 3 3 , 3 , 3 3 2 0 ( ) x AB AB y AB z AB AB R M dz f M M La dx f ⎛ ⎞ = −_⎜ + _⎟× ⎝ ⎠ = = − − × AB2： AB4： , 2 2 , 2 , 2 2 2 0 ( ) x AB AB y AB z AB AB R M dz f M M La dx f ⎛ ⎞ =_⎜ − _⎟× ⎝ ⎠ = = − − × , 4 4 , 4 , 4 4 2 0 ( ) x AB AB y AB z AB AB R M dz f M M La dx f ⎛ ⎞ =_⎜ + _⎟× ⎝ ⎠ = = − × 姿態推力器所造成的總力矩為 , 1 , 2 , 1 , 2 , 3 , 4 , 1 , 2 , 1 , 2 , 3 , 4 , 1 , 2 , 1 , 2 , 3 , 4 xa x AA x AA x AB x AB x AB x AB ya y AA y AA y AB y AB y AB y AB za z AA z AA z AB z AB z AB z AB M M M M M M M M M M M M M M M M M M M M M = + + + + + = + + + + + = + + + + + 根據表 3.3 計算可得

(

1 2 3 4

)

(

1 2

)

(

1 2 3 4

)

2 xa AB AB AB AB AA AA AB AB AB AB R M = × −f + f − f + f +dy× f − f + ×dz f − f − f + f

(

) (

1 2

)

ya AA AA M = La−dx × f − f

(

) (

1 2 3 4

)

za AB AB AB AB M = La−dx × f − f − f + f

由圖 3.5 六個姿態推力器的配置中，AA1 與 AA2 控制俯仰，AB1、AB2、 AB3 與 AB4 則是控制偏航與滾轉。當六個姿態推力器同時作偏航與翻滾時，會 有四種可能的情況，即 AB1、AB2、AB3、AB4 其中三個會同時打開，一個會關 閉。因為六個姿態推力器的設計關係，同時打開時，會造成力量互相抵消，形成 浪費燃料。所以對這四個情況去作調整：

1. 若 AB1、AB3 與 AB4 均為開啟狀態，則 AB1 為開啟狀態，AB3 與 AB4 為關閉狀態。

2. 若 AB2、AB3 與 AB4 均為開啟狀態，則 AB2 為開啟狀態，AB3 與 AB4 為關閉狀態。

3. 若 AB1、AB2 與 AB3 均為開啟狀態，則 AB3 為開啟狀態，AB1 與 AB2 為關閉狀態。

4. 若 AB1、AB2 與 AB4 均為開啟狀態，則 AB4 為開啟狀態，AB1 與 AB2 為關閉狀態。 3.3.3 轉向及姿態推力器產生的加速度 攔截器上的推力總和為 divert attitude = + F F F (3.3) 轉向及姿態推力器所造成的推力分別為 1 2 1 2 0 = divert DB DB DA DA f f f f ⎡ ⎤ ⎢ ₋ ⎥ ⎢ ⎥ ⎢ − ⎥ ⎣ ⎦ F 1 2 3 4 1 2 0 = attitude AB AB AB AB AA AA f f f f f f ⎡ ⎤ ⎢_{− + + −} ⎥ ⎢ ⎥ ⎢ − ⎥ ⎣ ⎦ F 若m為攔截器的質量，由推力器所產生的攔截器加速度為 T f F a m =

3.3.4 推力器動態 雖然推力器為開關式，理論上只有開或關兩個狀態，但是實際上從開到關或 從關到開均無法瞬間達成。我們用一個簡單的一階轉移函數 ( )G s A s A = + 來表示 開關命令與實際起程度的關係，其中 1/A 為時間常數，在模擬中設定 A＝500， 也就是時間常數為 2(ms)。

第四章 線性姿態控制系統設計

攔截目標的過程中，我們希望尋標器能一直對準目標，因此必須靠控制姿態 推力器的開關產生推力，使得攔截器的體座標與視線座標重合。又攔截器姿態是 屬於六自由度時變且非線性的方程組，直接作為設計用的數學模式相當不方便。 爲了方便控制器的設計，必須對姿態方程式做適當的線性化。 由於姿態控制的目標是要使的三軸穩定，其中滾轉角φ維持於零，俯仰角θ 及偏航角ψ 則是分別追蹤一個小角度變化的角度命令，所以我們可以合理的假設 攔截器的滾轉角φ ≈ 、滾轉角的速度0 p≈ 及俯仰角0 θ ≈0，簡化後姿態的動態方 程式會形成三組雙積分的方程式為 p φ= ， x xx M p I =  (4.1) q θ= ， y yy M q I =  (4.2) r ψ = ， z zz M r I =  (4.3) 其中I_xx,I_yy,I 分別為三軸轉動慣量，_zz M M_x, _y,M 分別為三軸的力矩輸出。 _z 4.1 線性控制系統 由於(4.1)~(4.3)三組雙積分方程式分別獨立，我們可以用相同的控制方法， 控制系統使用內外迴路常數增益回授形成的二階理想轉移函數，理想二階系統的 轉移函數可由阻尼比ξ及自然頻率ωn 二個參數來訂定，其通式為 2 2 2 H( ) s 2 s+ n n n s

ω

ξω ω

= + (4.4) 以滾轉軸為例，控制系統的方塊圖如圖 4.1 所示，其中 1 xx G I φ = ，uφ =Mx， p k 為角速度回授增益， k_φ為角度回授增益，φ_c為控制系統的輸入命令，俯仰軸 及偏航軸的方塊圖與圖 4.1 有一樣的結構，除了其中的變數及回授增益不同。

圖 4.1 滾轉軸控制系統 利用圖 4.1 的方塊圖可計算從指令φ_c至角度φ的轉移函數為 2 G H ( ) s pG s+ G k s k k φ φ φ φ φ φ = + (4.5) 同樣的計算可得其他兩軸的轉移函數為 2 G H ( ) s _qG s+ G k s k k θ θ θ θ θ θ = + (4.6) 2 G H ( ) s _rG s+ G k s k k ψ ψ ψ ψ ψ ψ = + (4.7) 其中 1 yy G I θ = 、 1 zz G I ψ = ， kθ、k 為俯仰軸的回授增益， kq ψ 、k 為偏航軸的回授r 增益。因為追蹤是小角度的命令，所以系統頻寬只要能有效地滿足命令追蹤的需 求即可。選擇阻尼比ξ =1/ 2，系統頻寬 5 2 n f ω π = = (Hz)，可得轉移函數為 2 2 2 2 (2 5) 987 1 _{44 987} 2( )(2 5) (2 5) 2 H s s s s φ π π π ⋅ = ≈ + + + ⋅ + ⋅ (4.8) 比較(4.8)式和(4.5)式的係數之後，我們可以得到滾轉軸的回授增益參數為 987 987 169.84 (0) 5.8173 k G φ φ = = ≈ 44 44 7.65 (0) 5.8173 p k G_φ = = ≈ 同理可以設計俯仰軸和偏航軸的回授增益為 987 987 632.92 (0) 1.5605 k k G ψ θ θ = = = = 44 44 28.49 (0) 1.5605 r q k k G_θ = = = =

由於在攔截過程中燃料不斷的消耗，I ,_xx Iyy及I 均會隨時間減小，相對的zz Gφ,Gθ 及G_ψ會隨時間增加，會使得系統頻寬隨時間略為增加。經由上述的設計方法， 我們可以得到線性姿態控制的輸入訊號如下 ( ) ( ) ( ) c p c q c r u k k p u k k q u k k r φ φ θ θ ψ ψ φ φ θ θ ψ ψ = − − = − − = − − 4.2 脈衝寬度調變(PWM)的設計 為了配合開關式推力器的使用，必須將連續的控制訊號轉換成對等的開關訊 號，即 0 或 1 的訊號。脈衝寬度調變(pulse width modulation, PWM)是達到這個目 的的方法之一。透過脈衝寬度調變可將輸入連續訊號的大小依比例轉換成脈衝訊 號的寬度，這種方式廣泛的應用於馬達驅動的伺服控制系統。 假設當輸入的連續訊號是一個低頻的弦波，而參考訊號為固定頻率及振福大 小的鋸齒波，將參考訊號與輸入訊號做比較，就可以得到輸出的脈衝訊號，如圖 4.2 所示。如果輸入訊號較大則輸出為 1，否則為 0。如果鋸齒波的週期為 T 則輸 出為 1 的時間T_ON滿足0≤T_ON ≤ 。定義導通週期(duty cycle, dc)為T TON T ，所以導 通週期的範圍為 0%至 100%。 圖 4.2 脈衝寬度調變訊號示意圖

我們修改 PWM 參考訊號在 duty cycle 內的方式。假設脈衝寬度調變的參考

訊號週期為 T，利用週期一開始的輸入連續訊號 (u )和參考訊號 (_n u_max,n)的比

值，作為 duty cycle 在該週期內的比例，來決定輸出脈衝訊號為 1 的時間，也就

是開關式推力器打開的時間長短。在這邊u 就是線性控制器的輸出訊號，而_n u_max,n

就是三軸所能產生的最大力矩，即 Mxmax，Mymax及 Mzmax。以滾轉軸為例，假設

時間 t 屬於參考訊號的第 n 個週期，也就是 (n-1)T≤t<nT，則在這個週期的導通 週期為 n xmax u min( ,1) M dc= φ 大致上推力器開的時間和輸入u_φn成比例關係，若u_φn>M_xmax，則推力器開 的時間為 T，即 dc=100%，如圖 4.3 所示。 圖 4.3 脈衝寬度調變(PWM)訊號示意圖 在使用 PWM 將輸入的連續訊號轉換成脈衝訊號輸出時，參考訊號的週期 T 或是說頻率是一個可以調整的參數，當選擇的頻率過低，姿態推力器開啟或關閉 的時間過多，會造成角度誤差較大，燃料消耗也會較多；相反的當選擇的頻率過 高，噴嘴還未完全打開產生足夠的推力時，卻有可能收到必須關閉的命令。因此

如何選擇適當的 PWM 頻率使的姿態角的誤差在可接受的範圍內，並且由於受到 硬體的限制，選擇的頻率也不可以讓噴嘴的開關頻率太高。 因為 PWM 是將線性控制器的輸出訊號轉換成脈衝訊號，而脈衝訊號透過傅 麗葉級數分析後可知和線性控制器的訊號有關[8]，因此我們先觀察線性控制器 的頻率響應，畫出(4.8)式的波德圖如圖 4.4 所示，在波德圖中，頻率響應的大小 為-3(dB)時的頻率ω ≈ 31.4(rad)或 f ≈ 5(Hz)，由於線性控制器在這頻率還存在有不 小的控制力，因此我們可以選擇更高頻的 PWM 頻率，保留線性控制器在這頻率 附近控制力提供的貢獻；而在波德圖中，大小約為-20(dB)時的頻率ω ≈ 102(rad) 或 f≈ 16 (Hz)，控制力在此頻率後所能提供的貢獻已經很小，如果再選擇更高頻 的 PWM 頻率所能消除的誤差量已經不明顯，反而有可能讓噴嘴出現不能完全開 啟的情況，估計 PWM 的頻率選擇在 16(Hz)附近，也就是線性控制系統頻寬的 3~4 倍將會比較適當。 1 00 1 01 1 02 1 03 - 7 0 - 6 0 - 5 0 - 4 0 - 3 0 - 2 0 - 1 0 0 1 0 M a gn itud e ( dB ) System: g Frequency (rad/sec): 31.4 Magnitude (dB): -2.93 System: g Frequency (rad/sec): 102 Magnitude (dB): -20.4 Bode Diagram Frequency (rad/sec) 圖 4.4 線性姿態控制器的頻率響應 這邊用一個簡單的數值模擬來進一步幫助 PWM 頻率的選擇，當假設三組不 同的姿態角命令分別為斜坡、弦波和三角波函數，如圖 4.5 所示。並且選擇不同 的 PWM 頻率為 10、12、15、17、20(Hz)，觀察在不同頻率下的角度誤差(取方 均根值)、燃料消耗及噴嘴開啟的情況。模擬結果如表 4.1~表 4.3 所示。

圖 4.5 三組不同的姿態角命令 表 4.1 姿態角命令為斜坡函數的模擬結果 誤差

頻率 φe (degree) θe (degree) ψe (degree)

燃料消耗 (kg) 8 (Hz) 0.4032 0.2584 0.1452 0.0178 10 (Hz) 0.1774 0.1418 0.0670 0.0123 12 (Hz) 0.0886 0.0816 0.0491 0.0089 15 (Hz) 0.0418 0.0561 0.0441 0.0066 17 (Hz) 0.0311 0.0501 0.0433 0.0059 20 (Hz) 0.0181 0.0471 0.0428 0.0044 表 4.2 姿態角命令為弦波函數的模擬結果 誤差

頻率 φe (degree) θe (degree) ψe (degree)

燃料消耗 (kg) 8 (Hz) 0.4010 0.2687 0.1586 0.0181 10 (Hz) 0.1752 0.1581 0.0940 0.0124 12 (Hz) 0.0881 0.1058 0.0819 0.0089 15 (Hz) 0.0473 0.0872 0.0772 0.0069 17 (Hz) 0.0353 0.0833 0.0766 0.0059 20 (Hz) 0.0262 0.0812 0.0765 0.0047 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -1 0 1 time (s) rol l (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -18 -16 -14 time (s) th et a (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 18 20 22 time (s) ps i (d eg re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -1 0 1 time (s) rol l (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -20 -18 -16 time (s) th et a (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 18 20 22 time (s) ps i (d eg re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -1 0 1 time (s) rol l (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -25 -20 -15 time (s) th et a (d eg ree ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 15 20 25 time (s) ps i (d eg re e )

表 4.3 姿態角命令為三角波函數的模擬結果 誤差

頻率 φe (degree) θe (degree) ψe (degree)

燃料消耗 (kg) 8 (Hz) 0.3989 0.2580 0.1451 0.0178 10 (Hz) 0.1397 0.1437 0.0756 0.0113 12 (Hz) 0.0660 0.0834 0.0530 0.0081 15 (Hz) 0.0370 0.0566 0.0459 0.0062 17 (Hz) 0.0286 0.0491 0.0437 0.0055 20 (Hz) 0.0160 0.0467 0.0433 0.0042 到目前為止，從表 4.1~表 4.3 的模擬結果可以發現，當 PWM 頻率在低頻(8Hz) 時，角度誤差很大，需要比較大的控制力來消除誤差，因此噴嘴開啟的時間就會 比較長，燃料也消耗的多。當頻率從 10(Hz)一直增加到 15(Hz)，角度誤差部份 有明顯的改善，可是改善的量也隨的頻率的增加持續的遞減，當頻率從 15(Hz) 增加到 20(Hz)時，角度誤差改善的量已經非常的少，可以證明在線性控制器頻率 響應大小在-20(dB)後所提供的控制力已經下降的非常多了。 除了角度誤差這個指標外，另一個幫助我們選擇 PWM 頻率的就是噴嘴開啟 的情況，四個滾轉─偏航軸噴嘴(AB1~AB4)能產生的最大推力為 15.8 牛頓，兩 個俯仰軸噴嘴(AA1、AA2)所能產生的最大推力為 31.6 牛頓，所以透過觀察每個 噴嘴產生推力的情況來判斷噴嘴是否有正常開啟。當 PWM 頻率為 15(Hz)時的噴 嘴大部分的情況下都能正常的開啟產生推力，只有在姿態角命令為三角波這種變 化比較劇烈的狀況下，噴嘴 AB1 和 AB2 才會有部份無法完全開啟的情形，如圖 4.6 中的(b)所示。當頻率為 17(Hz)時，除了在姿態命令為三角波這個角度變化較 大的情況下比較嚴重外，噴嘴開啟情形大致和頻率為 15(Hz)雷同，如圖 4.7 中(b) 所示。當頻率為 20(Hz)時 AB1 和 AB2 兩個噴嘴普遍都只能開一半就必須要關 閉，如圖 4.8 中(b)所示。雖然在ㄧ般情況姿態角是追蹤一個小角度變化的命令， 但是考慮當有雜訊或其他較大的干擾造成姿態角度較劇烈變化的情形，所以我們 可以選擇 15(Hz)為適當的 PWM 頻率，也就是線性控制器頻寬的 3 倍。

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB 1 0 2 4 6 0 10 20 time (s) AB 2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA 1 0 2 4 6 0 20 40 time (s) AA 2 (a) (b) (a) (b) (a) (b) 圖 4.6 由上到下的姿態角命令分別為斜坡、弦波和三角波函數，PWM 頻率為 15(Hz) (a) 角度誤差，(b)噴嘴開啟情況

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 -0.05 0 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.05 0.1 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 (a) (b) (a) (b) (a) (b) 圖 4.7 由上到下的姿態角命令分別為斜坡、弦波和三角波函數，PWM 頻率為 17(Hz) (a) 角度誤差，(b)噴嘴開啟情況

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.05 0 0.05 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 -0.05 0 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.05 0.1 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB1 0 2 4 6 0 10 20 time (s) AB2 0 2 4 6 0 10 20 time (s) AB3 0 2 4 6 0 10 20 time (s) AB4 0 2 4 6 0 20 40 time (s) AA1 0 2 4 6 0 20 40 time (s) AA2 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.05 0 0.05 time (s) ro lle (d e g re e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) pi tc he (degr e e ) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ya we ( deg ree) 0 2 4 6 0 10 20 time (s) AB 1 0 2 4 6 0 10 20 time (s) AB 2 0 2 4 6 0 10 20 time (s) AB 3 0 2 4 6 0 10 20 time (s) AB 4 0 2 4 6 0 20 40 time (s) AA 1 0 2 4 6 0 20 40 time (s) AA 2 (a) (b) (a) (b) (a) (b) 圖 4.8 由上到下的姿態角命令分別為斜坡、弦波和三角波函數，PWM 頻率為 20(Hz) (a)角度誤差，(b)噴嘴開啟情況

第五章 最短時間最佳控制系統

前一章討論線性姿態控制系統為簡單的常值增益角度及角速度回授的設 計，以及其利用 PWM 的方法達成開關式常值推力器的合成。本章所要討論的是 時間最佳(time-optimal)控制控制系統的設計，直接考慮制動器開關輸出的特性， 使控制系統直接輸出開關式的訊號。這種控制方式是要將系統從一個初始狀態在 最短的時間控制到原點。不過將這種方法應用到姿態跟蹤控制有幾個理論上的假 設和實際合成上的問題需要討論。 和線性控制器一樣我們考慮線性化的攔截器姿態方程式，即(4.1)-(4.3)三個 獨立的雙積分方程式。接著會討論雙積分系統的時間最佳的控制法應用到追蹤控 制的一些問題和必要的假設，並且對於這些問題找尋ㄧ些可行的解決方法。 5.1 理想時間最佳控制設計 由於線性化的姿態動態方程式由(4.1)-(4.3)三個獨立的雙積分系統所組成， 我們考慮一個單輸入雙積分系統之時間最佳控制，系統的狀態方程式寫成 1 2 x = x (5.1) 2 ( ) x =ku t (5.2) 其中 k 為大於 0 的常數，(x ,₁ x )為狀態變數，u 為控制輸入。假設在所有時間 t₂ 控制輸入 u(t)滿足 ( ) 1 u t ≤ (5.3) 又系統(5.1)、(5.2)是可控的(controllable)，因此根據時間最佳控制的理論[8]，時 間最佳控制存在且唯一，而且給定任何一個初始狀態(x₁(0),x₂(0))，時間最佳控 制系統能在最短的時間內將系統的狀態最終能驅動至原點，即最終的狀態為 (x t₁( )f ,x t2( )f )=(0,0)。

根據[3]在(5.3)的限制下，設計出系統(5.1)，(5.2)的切換曲線和最佳控制器為 1 2 2 1 2 s= +x x x (5.4) 1 2 2 1 ( ) sgn( ) 2 u t = − x + x x (5.5) 其中 1 , 0 sgn( ) 1 , 0 y y y + ≥ ⎧ = ⎨_{− <} ⎩ 以滾轉軸為例，假設角度命令為φ_c，我們定義追蹤誤的差為e₁ = − ，根據(4.1)φ φ_c 式可得e₁= − = −φ φ _c p φ_c，令x₁ = 和e₁ x₂ =  ，且假設e₁ φ_c變化足夠緩慢，使得 0 c φ  ，則滾轉軸的動態方程式可表示成 1 2 x =x (5.6) 2 x xx M x I =  (5.7) 令 xmax xx M k I = ， max x x M u M = 則姿態動態可表示成(5.1)、(5.2)的形式，且控制滿足(5.3) 式。因此根據(5.5)，滾轉軸的時間最佳控制u t_φ( )可表示成 1 2 2 1 ( ) sgn( ) 2 u t_φ = − x + x x (5.8) 1 , 0 ( ) 1 , 0 s u t s < ⎧ = ⎨_{− ≥} ⎩ (5.9) 其方塊圖如圖 5.1 所示 圖 5.1 滾轉軸時間最佳控制器方塊圖，其中k =M_xmax/I_xx>0

使用相同的方法可以設計俯仰和偏航的控制器 3 4 4 1 sgn( ) 2 u_θ = − x + x x (5.10) 5 6 6 1 sgn( ) 2 u_ψ = − x + x x (5.11) 其中x₃ = − ，θ θ_c x₄ = −θ θ  ，_c x₅ = −ψ ψ_c，x₆ = −ψ ψ  _c 理論上這個控制方法可以將系統狀態在最短時間內驅動至原點，而且切換的 次數為系統維度減一，以二階系統為例，最多只需切換一次。但是實際系統都受 雜訊影響且狀態量測亦都有誤差，這些都會造成系統狀態的擾動。在我們考慮的 姿態控制系統，還有兩個重要的原因造成控制的連續切換。第一個原因是追蹤的 命令隨時間改變，使得系統狀態不可能一直停留在原點；第二個原因是三軸之間 的耦合會造成的各軸之間互相干擾。 5.2 切換曲線的調整 然而在追蹤控制，控制訊號的連續切換是必要的。考慮到如果切換的頻率太 高，或是說切換的時間太短，以推力器硬體合成控制上有實際的困難。當推力器 為一階動態的時間延遲時，太頻繁的開關命令會造成推力器未全開即進行關閉， 關閉未完成又進行開啟的狀況。因此從實際合成的觀點，有需要對理想的最佳控 制做調整使得切換頻率在一個要求的範圍之內。相對的切換頻率的降低意味著有 應切換而未切換的狀況發生，因此伴隨著追蹤誤差的增大。所以切換條件的調整 是要在切換頻率及追蹤誤差之間做定量的取捨。這種定量的取捨通常需要透過實 際模擬才有辦法明確的驗證。 首先我們調整理想時間最佳控制的切換曲線及控制輸入，調整方式如下 1 2 2 1 2 s= + ⋅ ⋅x k x x (5.12) 1 2 2 1 sgn( ) 2 u_φ = − x + ⋅ ⋅k x x (5.13)

在切換曲線中的第二項加入變數 k，因為x 和角速度有關，而角速度的變化比角₂ 度變化來的劇烈且頻繁，因此藉由設定 0<k<1 的方式來降低角速度對切換曲線 或控制輸入的影響，進而降低噴嘴的開關頻率。又x 是由力矩積分而來的，當產₂ 生的力矩太大時，對於小角度變化的情況必須要用更高頻的開關來控制，因此在 產生力矩較大的方向上可以設定較小的 k 值來降低其開關頻率。 這邊用一個模擬來觀察改變 k=0.5、0.1、0.075、0.05、0.025、0.01 後對噴嘴 開關頻率、角度誤差及燃料消耗的影響。假設姿態角的初始條件為φ₀ = ，0 0 16 o θ = − ，ψ₀ =19o_{，角速度初始值皆為零，姿態角命令如圖 5.2 所示，模擬結果} 如表 5.1 所示。其中 f_AB為滾轉─偏航軸的噴嘴開關頻率，f_AA為俯仰軸的噴嘴開 關頻率。 由表 5.1 中的模擬結果可知，俯仰軸能產生的力矩相較另外兩個方向來的 大，所以可藉由設定較小的 k 值來達成降低開關頻率。然而藉由改變 k 值的方式 雖降低了開關頻率，可是角度誤差也會跟著增加，所以須在角度誤差和開關頻率 間作取捨。也可設定三個方向有不同的 k 值，例如分別改變三個方向的k_φ=0.05、 k_θ=0.025、k_ψ =0.035，得到誤差大小分別為 0.0193°、0.0489°、0.0458°及噴嘴的 開關頻率 f_AB≈ f_AA≈ 20(Hz)。 0 5 10 15 -1 0 1 time (s) rol l ( degr ee) 0 5 10 15 -40 -20 0 time (s) thet a ( degr ee) 0 5 10 15 0 20 40 time (s) ps i ( degr ee) 圖 5.2 三個姿態角命令

表 5.1 不同 k 值的模擬結果 k 值 角度誤差(degree) 開關頻率(Hz) 燃料消耗(kg) 0.0009 0.0042 k=0.5 0.0043 AB f ≈ 大 AA f ≈ 大 0.0895 0.007 0.0046 k=0.1 0.0059 AB f ≈ 40 AA f ≈ 50 0.0895 0.0098 0.0078 k=0.075 0.0113 AB f ≈ 36 AA f ≈ 48 0.0895 0.0193 0.0152 k=0.05 0.0232 AB f ≈ 24 AA f ≈ 35 0.0895 0.0613 0.0489 k=0.025 0.0847 AB f ≈ 12 AA f ≈ 19 0.0895 0.2927 0.2478 k=0.01 0.3795 AB f ≈ 4 AA f ≈ 8 0.0895 5.3 輸出切換控制律的調整 由於控制輸入是一個 sign 的控制律，如(5.9)所示，會造成正反兩個方向的噴 嘴一直呈現互相開關的現象，所以燃料消耗會比較多，因此考慮一個死區控制 律，將切換條件修改為(5.14)，其切換函數的方塊圖如圖 5.3 所示。由於死區控 制律的控制訊號輸出有可能為零，讓正反方向的噴嘴有完全關閉的情況以節省燃 料。另外對於產生力矩較大的俯仰軸方向因為噴嘴關閉，力矩作用的時間減少， 所以開關頻率可以降低。 1, ( ) 0, 1, s u t s s δ δ δ − > ⎧ ⎪ =_⎨ < ⎪+ < − ⎩ (5.14) 其中δ >0 為一可選擇的參數。

圖 5.3 死區控制律示意圖 沿用 5.2 節最後設定三軸的 k 值，k_φ=0.05、k_θ=0.025、k_ψ=0.035，選擇不同 死區的寬度，觀察噴嘴開關頻率、角度誤差及燃料消耗，模擬結果如表 5.2 所示。 表 5.2 不同死區寬度的模擬結果 死區寬度 角度誤差(degree) 開關頻率(Hz) 燃料消耗(kg) 0.2872 0.2134 0.005 0.2962 AB f ≈ 20 AA f ≈ 6 0.0443 0.1167 0.1139 0.002 0.1280 AB f ≈ 20 AA f ≈ 10 0.0582 0.0603 0.0788 0.001 0.0765 AB f ≈ 20 AA f ≈ 14 0.0690 0.0348 0.0606 0.0005 0.0551 AB f ≈ 20 AA f ≈ 17 0.0776 0.0229 0.0517 0.0002 0.0484 AB f ≈ 20 AA f ≈ 19 0.0846 0.0196 0.0491 0.0001 0.0463 AB f ≈ 20 AA f ≈ 19 0.0871 由表 5.2 中的模擬結果可知，死區的寬度越大可以節省越多燃料，對於力矩 較大的俯仰軸方向也可以降低噴嘴的開關頻率，另外 AB1~AB4 四個噴嘴因為受 到滾轉軸和偏航軸互相影響的關係，所以開關頻率並沒有降低的情形。

考慮另一種有延遲切換的繼電器控制律，將切換條件修改為(5.15)，其切換 函數的方塊圖如圖 5.4 所示。利用改變延遲切換的寬度大小來改變噴嘴的開啟時 間，因此可以強制改變開關頻率。沿用k_φ=0.05、k_θ=0.025、k_ψ=0.035，選擇不同 延遲切換寬度，模擬結果如表 5.3 所示。 1, ( ) 1, s u t s s ε ε ε − > ⎧ ⎪ =_⎨ + < ⎪ _< ⎩不變, (5.15) 圖 5.4 延遲切換控制律 表 5.3 不同延遲切換寬度的模擬結果 延遲寬度 角度誤差(degree) 開關頻率(Hz) 燃料消耗(kg) 0.1492 0.2500 0.001 0.3045 AB f ≈ 8 AA f ≈ 9 0.0895 0.0907 0.1665 0.0005 0.1924 AB f ≈ 9 AA f ≈ 10 0.0895 0.0621 0.1196 0.00025 0.1328 AB f ≈ 11 AA f ≈ 12 0.0895 0.0418 0.0896 0.0001 0.0911 AB f ≈ 13 AA f ≈ 15 0.0895 0.0359 0.0770 0.00005 0.0752 AB f ≈ 15 AA f ≈ 16 0.0895 0.0304 0.0712 0.000025 0.0677 AB f ≈ 15 AA f ≈ 16 0.0895

由表 5.3 中的模擬結果可知，延遲切換控制律可以強制改變所有噴嘴的開關 頻率。可是和 sign 控制律相同，會造成正反方向的噴嘴一直互相開關，所以燃 料消耗會比較多。對於力矩較大的俯仰軸方向因為受到噴嘴延遲關閉的影響，力 矩作用的時間比較長，因此相對於死區控制律而言，產生了比較大的角度誤差。 5.4 設計方法及模擬結果 對於以上調整切換曲線及兩種改變控制律的方式，經過模擬結果的驗證後， 可以歸納出一個大略的設計方向如下： 1. 設計最短時間最佳控制法，得到切換函數及控制輸入 2. 為了降低高頻開關在切換函數中加入一變數 k(0<k<1) 3. 使用死區控制律來降低燃料消耗及力矩較大方向的開關頻率(選擇死區寬度) 4. 對於無法降低開關頻率的方向使用延遲切換繼電器的控制律(選擇延遲寬度) 5. 為了對付不同情況，可以結合上述 3 和 4 的方法在不同方向做控制 配合以上的所有數值模擬結果及歸納出的設計的方式，重新設計一個最佳化 的姿態控制法，首先選擇k_φ=0.05、k_θ=0.025、k_ψ=0.035，得到誤差為 0.0193°、 0.0489°、0.0458°、燃料消耗 0.0895(kg)及開關頻率 f_AB≈ f_AA≈ 20(Hz)。為了降低 燃料消耗及開關頻率，採用死區控制律，選擇死區寬度為 0.001，得到誤差大小 為 0.0603°、0.0788°、0.0765°、燃料消耗 0.0690(kg)及開關頻率 f_AB≈ 20(Hz)、 AA f ≈ 14(Hz)。由於控制滾轉─偏航軸的噴嘴開關頻率還是太高，所以俯仰軸維 持死區控制律，在滾轉和偏航方向改由延遲切換控制律來降低其開關頻率，選擇 延遲切換寬度為 0.00005，得到誤差大小為 0.0359°、0.0788°、0.0750°、燃料消 耗 0.0690(kg)及開關頻率 f_AB≈ 15(Hz)、 f_AA≈ 15(Hz)。模擬結果如圖 5.5 所示。當 採用不同的姿態角命令時的模擬結果如圖 5.6 和圖 5.7 所示。

0 5 10 15 -1 0 1 time (s) rol l (degree) 0 5 10 15 -40 -20 0 time (s) th et a ( deg ree) 0 5 10 15 0 20 40 time (s) ps i (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (deg ree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we (deg ree) 0 1 2 3 4 5 0 10 20 time (s) AB 1 0 1 2 3 4 5 0 10 20 time (s) AB 2 0 1 2 3 4 5 0 10 20 time (s) AB3 0 1 2 3 4 5 0 10 20 time (s) AB4 0 1 2 3 4 5 0 20 40 time (s) AA 1 0 1 2 3 4 5 0 20 40 time (s) AA 2 圖 5.5 姿態角命令為斜坡時，得到誤差大小為 0.0359°、0.0788°、0.0750°，燃料 消耗 0.0690(kg)及開關頻率 f_AB≈ 15(Hz)、 f_AA ≈ 15(Hz)。

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -1 0 1 time (s) rol l (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -18 -16 -14 time (s) pi tc h ( de gr ee) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 18 20 22 time (s) y a w ( d egr ee) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (degr e e) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( degree) 0 1 2 3 4 5 0 10 20 time (s) AB 1 0 1 2 3 4 5 0 10 20 time (s) AB 2 0 1 2 3 4 5 0 10 20 time (s) AB 3 0 1 2 3 4 5 0 10 20 time (s) AB 4 0 1 2 3 4 5 0 20 40 time (s) AA 1 0 1 2 3 4 5 0 20 40 time (s) AA 2 圖 5.6 姿態角命令為弦波時，得到誤差大小為 0.0359°、0.0765°、0.0726°，燃料 消耗 0.0685(kg)及開關頻率 f_AB≈ 15(Hz)、 f_AA ≈ 15(Hz)。

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -1 0 1 time (s) rol l (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -20 -18 -16 time (s) pi tc h ( de gr ee) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 18 20 22 time (s) y a w ( d egr ee) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.1 0 0.1 time (s) ro lle (degr e e) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) pi tc he (degree) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 -0.2 0 0.2 time (s) ya we ( degree) 0 1 2 3 4 5 0 10 20 time (s) AB1 0 1 2 3 4 5 0 10 20 time (s) AB2 0 1 2 3 4 5 0 10 20 time (s) AB 3 0 1 2 3 4 5 0 10 20 time (s) AB 4 0 1 2 3 4 5 0 20 40 time (s) AA 1 0 1 2 3 4 5 0 20 40 time (s) AA 2 圖 5.7 姿態角命令為三角波時，得到誤差大小為 0.0359°、0.0785°、0.0755°，燃 料消耗 0.0690(kg)及開關頻率 f_AB≈ 15(Hz)、 f_AA ≈ 15(Hz)。

第六章 轉向導引控制

攔截器除了使用姿態控制器及姿態推力器來調整其姿態，使的紅外線尋標器 能持續的對準目標外，還必須使用器側邊的轉向推力器讓本身轉向，因此需要有 方法能提供側向導引加速度命令，所以真比例導引率是一個合適的選擇，因為其 所產生的導引加速度垂直於相對位置方向，也就是視線方向。此外轉向推力器和 姿態推力器一樣都是開關式的，也是透過脈衝寬度調變(PWM)的合成來實現。 6.1 真比例導引律 假設攔截飛彈在S 座標的位置為_N r ，速度為_m v ，目標在_m S 座標的位置為_N t r ，速度為v ，則相對位置_t r= −r_t r_m=

[

x y z

]

T，相對速度v= =r v_t−v_m，再 將 r 和v轉換成球座標，如圖 6.1 所示， Missile Target µ a

v

m

v

t

r

X

N

Z

N

Y

N 圖 6.1 相對位置與球座標之間的關係 則相對位置可表示為 r e = r r (6.1) 其中

cos cos cos sin sin r e α μ α μ α ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (6.2) 1 tan y x μ ₌ − ⎛ ⎞ ⎜ ⎟ ⎝ ⎠ (6.3) 1 2 2 tan z x y α ₌ − ⎛_⎜ ⎞_⎟ ⎜ ₊ ⎟ ⎝ ⎠ (6.4) 2 2 2 x y z = + + r (6.5) 而相對速度

(

cos

)

r r r d d e e e e e dt dt μ α μ α α = = + = + + v r r r  r r   (6.6) 其中 sin cos 0 e_μ μ μ − ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (6.7) sin cos sin sin cos e_α α μ α μ α − ⎡ ⎤ ⎢ ⎥ = −_{⎢ ⎥} ⎢ ⎥ ⎣ ⎦ (6.8) 且 r e ×e_μ = e_α (6.9) 而從(6.5)式、(6.3)式及(6.4)式分別可以得到下列微分的表示式： 2 2 2 d xx yy zz dt x y z + + = + + r    (6.10) 1 2 2 tan d y yx yx dt x x y μ ₌ − ⎛ ⎞₌ − ⎜ ⎟ ₊ ⎝ ⎠    (6.11)

(

)

(

)

(

)

2 2 1 2 2 2 2 2 2 2 tan z x y z xx yy d z dt _{x y x y x y z} α ₌ − ⎛_⎜ ⎞_⎟= + − + ⎜ ₊ ⎟ _{+ + +} ⎝ ⎠     (6.12) 三維 TPN 導引律的導引加速度方程式為

(

)

tpn c a =N V × Ω (6.13) 其中a_tpn為三維 TPN 導引加速度，Ω 為角速度，N為導引常數，通常為 3~5，V_c 為攔截飛彈接近目標的速度也就是相對速度在視線上面的投影，即

( )

T c = e er r V v (6.14) 而角速度則定義為 2 × Ω =r v r 由(6.1)式及(6.6)式，可以得到 2 er er × Ω =r r = × r   (6.15) 從(6.2)式可得 r e

(

)

cos sin sin cos

cos cos sin sin

0 cos α μ α μ μ α μ α α μ α ⎡ − ⎤ ⎡− ⎤ ⎢ ⎥ ⎢ ⎥ = _{⎢ ⎥}+ _⎢− _⎥ ⎢ ⎥ ⎢_⎣ ⎥_⎦ ⎣ ⎦  

sin sin cos

cos cos sin sin

0 cos μ α μ μ α μ α α μ α − − ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = _{⎢ ⎥}+ _⎢− _⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦   =μcos eα _μ + αe_α (6.16) 將(6.16)式代入(6.15)式並利用e_r×e_μ = 及e_α e_r×e_α = − ，可得 e_μ cos e_α e_μ μ α α Ω =  −  (6.17) 所以從(6.13)式、(6.14)式及(6.17)式可得導引加速度為

( )(

T

)

cos tpn r a = −N v e μ αe_μ+αe_α (6.18) 我們將(6.13)式分解成相對位置與相對速度可以表示的形式，如(6.18)式，透

過(6.1)-(6.12)式計算出各分量，再代入(6.18)式，就可以得到真比例導引的的導引 加速度。 6.2 脈衝寬度調變(PWM)的合成 由(6.18)式得到的真比例導引加速度是在慣性座標下，所以我們必須要把它 轉換到體座標下才能使用，並且作為 PWM 的輸入訊號。 ,B TBN tpn tpn a = ⋅a (6.19) 其中T_BN為慣性座標與體座標的轉換矩陣，a_tpn為三維真比例導引律的導引加速 度。則我們得到轉向推力器 PWM 的輸入訊號為a_tpn,B在Y 和_B Z 的分量。 _B 在表 3.2 中，我們模擬時假設轉向推力器的推力最大值為 315 牛頓，以攔截 器的初始的總質量為 21.33 公斤來計算，則每個轉向推力器所能產生的最大加速 度值大約為 14.7679 2 m/s ，又攔截器在飛行的過程中，質量會因推力器燃燒的關 係不斷的減少，所以我們設計轉向推力器 PWM 的參考訊號為 15 2 m/s ，及選擇 PWM 的頻率為 3(Hz)。 6.3 實際攔截情況 在這一節我們將結合攔截器的姿態控制器及轉向導引控制，觀察攔截器在實 際執行攔截情況時兩者的運作狀況、角度誤差及燃料的消耗。姿態控制方面是用 線性控制器配合 PWM 合成及最短時間最佳控制器兩種方式。攔截器與目標的初 始設定如下： (i) 目標初始位置在S 座標為(198.888, -10.189, -122.605) km。 _N (ii) 攔截飛彈初始位置在S 座標為(159.257, 6.297, -120.184) km。 _N (iii) 目標初始速度在S 座標為(-1520, 1115, -123) m/s。 _N (iv) 攔截飛彈初始速度在S 座標為(1690, -136, -402) m/s。 _N

(v) 攔截飛彈初始姿態尤拉角為

(

φ θ ψ₀, ₀, ₀

) (

= 0, 0.0563, 0.3942− −

)

rad，同初 始視線方向，也就是一開始攔截飛彈是對準目標的。 (vi) 初始角速度

(

p q r0, 0, 0

) (

= 0, 0, 0

)

rad/s。 兩種姿態控制器的模擬結果最小誤失皆距離小於 0.5 公尺，也就是有攔截到目 標。角度誤差和燃料消耗情況如表 6.1 所示，其中角度誤差部分是取其方均根值。 圖 6.2 為實際攔截的軌跡圖，圖 6.3 是姿態控制為線性控制器的模擬圖，圖 6.4 是姿態控制為最短時間最佳控制器的模擬圖。 表 6.1 實際攔截情況的模擬結果 角度誤差 (degree) 燃料消耗 (kg) 線性控制器 0.0076 0.0239 0.0177 1.8808 最短時間最佳 控制器 0.0388 0.0858 0.0900 2.0367 -1.5 -1 -0.5 0 0.5 1 x 104 1.4 1.6 1.8 2 x 105 1.2 1.21 1.22 1.23 1.24 x 105 east north a tti tu d e target missile 圖 6.2 實際攔截軌跡

0 2 4 6 8 10 12 14 -0.02 0 0.02 time (s) rol le (degree ) 0 2 4 6 8 10 12 14 -0.05 0 0.05 time (s) pi tc he (de g ree) 0 2 4 6 8 10 12 14 -0.05 0 0.05 time (s) ya we (de g ree) 0 2 4 6 8 10 12 14 19.4 19.6 19.8 20 20.2 20.4 20.6 20.8 21 21.2 21.4 time (s) m a s s propel lent 0 2 4 6 8 10 12 14 0 100 200 300 400 time (s) DA 1 0 2 4 6 8 10 12 14 0 20 40 60 80 time (s) DA 2 0 2 4 6 8 10 12 14 0 50 100 150 200 250 time (s) DB 1 0 2 4 6 8 10 12 14 0 100 200 300 400 time (s) DB 2 0 2 4 6 8 10 12 14 0 5 10 15 time (s) AB 1 0 2 4 6 8 10 12 14 0 5 10 15 time (s) AB 2 0 2 4 6 8 10 12 14 0 5 10 15 time (s) AB 3 0 2 4 6 8 10 12 14 0 5 10 15 time (s) AB 4 0 2 4 6 8 10 12 14 0 10 20 30 time (s) AA 1 0 2 4 6 8 10 12 14 0 10 20 30 time (s) AA 2 (a) (b) (c) (d) 圖 6.3 姿態控制為線性控制器的模擬結果，(a)為角度誤差，(b)為燃料消耗狀況， (c)為轉向推力器的開關情況，(d)為姿態推力器的開關情況