無人載具之導航與控制(III)

無人載具之導航與控制(III)

Navigation and Control of Unmanned Vechicles(III)

計劃編號：NSC-89-2612-E-002-007

執行期限：８９年８月１日至９０年７月３１日

主持人：王立昇 國立台灣大學應用力學研究所 教授

一、前言

：

處理載具姿態或方位判定之問題是導航系統中一個重 要的環節。對於汽車而言，也許有一個指北針告訴駕駛人大 略的方位就足夠了，但是對於在天空中、水面下甚至宇宙中 航行的載具，就必須更精確的描述其俯仰、滾轉與方位角等 資訊，以便更精確的控制載具之運動。 一般而言，慣性導航系統利用加速規提供載具相對於慣 性空間的位移變化，陀螺儀提供載具相對於慣性空間姿態的 變化。但是利用多組加速規透過適當的擺置方式，亦可同時 提供載具位移與姿態的變化[17]，其與利用陀螺儀量測載具 姿態不同之處在於當載具姿態變化時，其上每一點相對於慣 性空間的角速度都是相同的，但是每一點的加速度卻是不同 的，我們可以利用這個特點，透過加速規多種不同之擺置位 置與方位而得到最適合之量測。 加速規所量測之線性加速度，經過計算可獲得載具角速 度與角加速度，其中角速度交乘項在角速度通過零點時會有 姿態判定分支的問題，必須依靠角加速度量測值來輔助判 定，並且由於其量測值對於姿態估測具有不可觀測性，故系 統漂移誤差將隨時間增加而擴大。GPS 載波平滑碼定位法， 消除鐘差、大氣干擾等誤差源，提高 GPS 定位之精度，其 精度足以修正加速規之漂移誤差、消除姿態判定分支錯誤， 並可輔助加速規對於載具姿態之判定。 加速規與 GPS 具有多種整合方式以估測載具之最佳姿 態，我們利用所簡化之基線方位判定模型，根據此基線方位 判定原理設計變角加速度與等角加速度兩種濾波器模型，使 用卡爾曼濾波器之最佳估測原理來設計加速規與 GPS 整合 方位估測演算法，由於加速規對於角速度量測方程非線性之 特性，我們使用擴增卡爾曼濾波器來處理此一非線性系統。 為了驗證演算法之特性與效用，我們撰寫了數值模擬程 式模擬基線方位判定實驗，並以此設計基線單軸旋轉實驗。 在實驗部分，我們將兩台 Motorola Oncore 8CH GPS 接收機 安裝在水平鋁桿之兩端以構成一基線，鋁桿繞垂直軸轉動， 利用載波平滑碼定位法得到基線之向量，並於基線兩端各裝 置一枚 SUMMIT 34103A 三軸加速規以量測端點加速度的 變化。透過實驗結果可知，整合加速規角速度與角加速度值 與 GPS 基線向量之演算法可以達到估測最佳方位角之目 的。 二、加速規量測基線方位角

：

考慮載具單軸旋轉運動之方位角量測問題。我們在載具 上選取 1、2 兩點構成一基線，並假設此固定長度之基線繞 軸 Zi 旋轉，其旋轉角度可用以描述載具之方位，兩組二軸 加速規分別擺在 1、2 兩點，加速規指向如圖一所示。 則方位角角速度和角加速度與 1、2 兩點相對於 O 點之向心 與切線加速度，其關係如下： Xi Ax2 Ax1 Ay1 Ay2 Yi Zi , Zb θ Yb Xb 1 2 O 圖一、單軸方位角量測加速規配置模型 1 2 1 r A Ax− x = θ&& （1）

_{( )}

1 2 1 2 r A Ay − y = θ& （2） 其中：

r

1：加速規兩端之基線長度

A

x1

,

A

y1

,

A

x2

,

A

y2：1、2 兩點相對於 O 點之向心與 切線加速度。 兩組加速規之 x、y 軸量測值如下所示：

A

x1

=

A

x1

+

n

x1 （3）

A

y1

=

A

y1

+

n

y1 （4）

A

x2

=

A

x2

+

n

x2 （5）

A

y2

=

A

y2

+

n

y2 （6） 其中：

n

x1_、

n

y1_、 2 x

n

、

n

y2_{：加速規之量測雜訊} 並令： （7） （8） 則（4）與（5）式可改寫為 （9） （10） 其中： （11）

w

~

=

n

x1

−

n

x2 （12） 由（9）式得知，透過相對載具座標系中心之向心加速 度量測獲得方位角之角速度絕對值，其順逆轉不可辨識， 透過相對載具座標系中心之切線加速度量測獲得方位角之 角加速度，積分角加速度得到方位角之角速度，可判定順 逆轉，但其積分所得之角速度會有累積誤差效果，故適當 利用這兩種量測值可得到方位角之最佳解。 根據上述基線方位角與加速規量測線性加速度之關 係，將接續討論加速規方位角判定之演算法，並介紹兩種 濾波器模型，第一種是變角加速度系統模型，第二種是等 角加速度系統模型。由（9）式我們可知

a

n是

θ

&的二次函數， 此系統乃是非線性系統，我們分別考慮以一階與二階擴增 卡爾曼濾波器（EKF）建立基於加速規量測之方位角判定 演算。 (1)變角加速度系統模型： 令

ω &

=

θ

（13） 狀態向量可取為：

_











=

ω

θ

x

（14） 其連續時間狀態方程如下式所示：

( )

( )

( )

v

( )

t

r

t

a

t

Ax

t

x

t

~

1

0

0

1









+

















+

=

&

（15） 其中：

_











=

0

0

1

0

A

（16） 取樣間隔

∆

t

之離散時間狀態方程：

(

)

( )

( )

v

( )

k

r

k

a

t

k

Fx

k

x

t

+













∆

+

=

+

1

0

1

（17） 其中：

_











∆

=

1

0

1

t

F

（18）

( )

k

v

之誤差協方差矩陣如下所示：

( )

( )

( )

q

v

t

t

t

t

Q

















∆

∆

∆

∆

=

₂ 2 3

2

1

2

1

3

1

（19） 由（9）式，濾波器模型之量測方程式可以系統狀態表示如 下： （20） 其中： 令 （21） (20）式是非線性量測方程，其 Jacobian 與 Hessian 分別為：

h

x

[

k

,

x

( )

k

]

=

[

0

2

r

1

ω

( )

k

]

（22）

[

( )

]

_











=

1

2

0

0

0

,

r

k

x

k

h

_xx （23） 為求得（17）與（20）式系統之最佳估測，可採用卡爾曼 濾波器之遞迴程序，首先我們給定系統狀態初始值

x

ˆ

( )

0

0

與 初始協方差矩陣

P

( )

0

0

，計算狀態預估

x

ˆ

(

k

k

+

1

)

與其協方 差矩陣

P

(

k

+

1

k

)

，並進行系統量測預估，得一階 EKF 之系 統輸出預估與其預估誤差協方差矩陣如下：

a

ˆ

n

(

k

+

1

k

)

=

r

1

ω

ˆ

(

k

+

1

k

)

2 （24）

(

1

)

4

ˆ

(

1

) (

22

1

)

(

1

)

2 2 1

+

+

+

+

=

+

r

k

k

P

k

k

R

k

k

S

ω

(25） 其中是狀態預估協方差矩陣

P

(

k

+

1

k

)

第二列第二行之元 素。卡爾曼濾波器增益值：

W

(

k

+

1

)

=

P

(

k

+

1

k

)

h

x

[

k

+

1

,

x

(

k

+

1

)

]

S

−1

(

k

+

1

)

（26） 從（22）、（25）與（26）式可看出當方位角之角速度預估 值

ω

ˆ

(

k 1

+

k

)

_{為 零 時 ，}

h

x

[

k

+

1

,

x

(

k

+

1

)

]

歷 經 一 奇 異 點 （singular point），且

S

(

k

+

1

) (

≅

R

k

+

1

)

_{，使卡爾曼濾波器} 增益值

W

(

k

+

1

)

振盪且發散，造成系統於角速度零點附近 時，演算法判定方位角產生分支錯誤，導致濾波器失效。 為求改善上述問題，將系統輸出預估與其預估誤差協 方差矩陣泰勒展開至二階，如下所示：

a

ˆ

n

(

k

1

k

)

r

ˆ

(

k

1

k

)

r

1

P

22

(

k

1

k

)

2 1

+

+

+

=

+

ω

（27）

(

1

)

4 ˆ

(

1

) (

1

)

2

(

22

(

1

)

)

2

(

1

)

2 1 22 2 2 1 + + + + + + = + r k kP k k r P k k Rk k S ω （28） 則系統在低角速度時，

S

(

k

+

1

)

可較合理的反應預估量測值 之誤差，

W

(

k

+

1

)

_{較為穩定，使濾波器能比較穩定之運作。} 將（27）與（28）式代入（25）式，並計算狀態更新

(

1

1

)

ˆ

k

+

k

+

x

與

P

(

k

+

1

k

+

1

)

，再進行下一步之狀態預估， 如此反覆運算，即擴增卡爾曼濾波器之遞迴程序。 (2) 等角加速度系統模型： 假設方位角之加速度變化量

θ&&

&

( )

t

很小可以忽略，則可 以考慮繞固定軸等角加速度旋轉之系統，如下式所示：

θ&&

&

( )

t

=

0

（29） 若忽略雜訊，則方位角

θ

( )

t

只與時間相關，這種模型稱為 多項式模型（polynomial models）。在真實世界中，加速度 會受一些輕微擾動，我們將這些輕微擾動視為在連續時間 下的白色雜訊（white-noise），此連續系統方程如下所示： （30） 基於（30）式之狀態向量如下所示： （31） 連續時間狀態方程如下式所示：

_x

( )

_t

_A

_x

( )

_t

_v

~

( )

_t

1

0

0





















+

=

&

（32） 其中：





















=

0

0

0

1

0

0

0

1

0

A

（33） 取樣間隔

∆

t

之離散時間狀態方程：

x

(

k

+

1

)

=

Fx

( ) ( )

k

+

v

k

（34） 其中：

( )





















∆

∆

∆

=

1

0

0

1

0

2

1

1

2

t

t

t

F

（35）

( )

k

v

之誤差協方差矩陣為：

( )

( )

( )

( )

( )

( )

( )

( )

v

q

t

t

t

t

t

t

t

t

t

Q





























∆

∆

∆

∆

∆

∆

∆

∆

∆

=

2

6

2

3

8

6

8

20

3 3 4 3 4 5 （36） 濾波器模型之量測方程式可以系統狀態表示如下：

( )

( )

( )

( )

k

w

( )

k

k

r

k

a

k

a

t n

₊













=













α

ω

2 1 （37） 令

[

( )

]

( )

( )













=

k

r

k

r

k

x

k

h

α

ω

1 2 1

,

（38） （37）式是非線性之量測方程，其 Jacobian 與 Hessian 如下 式所示：

( )

( )

_











=

1 1

0

0

0

2

0

r

k

r

k

h

_x

ω

（39）

( )

_











=

0

0

0

0

0

0

1

_k

h

_xx ，

( )

_











=

0

0

0

0

2

0

₁ 2

_k

r

h

_xx ，

( )

_











=

0

0

0

0

0

0

3

k

h

_xx （40） 本系統類似變角加速度系統，在一階 EKF 中會有演算法判 定方位角順逆轉產生分支錯誤的問題，使濾波器失效，故 採用二階 EKF 方法計算量測預估與其誤差協方差矩陣如 下： （41）

(

)

(

) (

)

(

)

(

)

₍

₁

₎

0 0 0 1 2 1 0 0 1 1 ˆ 4 1 2 22 2 1 33 2 1 22 2 2 1 + +       + +       + + + = + k R k k P r k k P r k k P k k r k S ω （42） 其中

P

22

(

k

+

1

k

)

是狀態預估協方差矩陣

P

(

k

+

1

k

)

第二列 第 二 行 之 元 素 。

P

₃₃

(

k

+

1

k

)

是 狀 態 預 估 協 方 差 矩 陣

(

k

k

)

P

+

1

第三列第三行之元素。 （34）與（37）式之模型假設系統等角加速度旋轉， 對於變角加速度旋轉運動，此模型可能會濾掉較高頻之角 速度變化，使旋轉運動之角速度與角度估測量失真。相對 地，（17）與（20）式並不假設等角速度或等角加速度旋轉， 此模型可適用變角加速度旋轉運動，表現運動角加速度變 化之行為特性，以符合真實物理系統之本質特性作繞固定 軸旋轉角和角速度之最佳估測。特別是如果我們想要量測 系 統 之 動（jitter），則必須採用變角加速度模型。 對於非線性系統量測方程之奇異點所導致之濾波器失 效，將於後章配合數值模擬作討論，而其系統低轉速之分 支問題則可由 GPS 量測值協助判定。 三、GPS/INS 整合導航系統設計： 整合上述加速規及差分式 GPS 之量測值，作繞單軸旋 轉方位角之最佳估測，結合 3-6 節所述之系統狀態與量測方 程設計一加速規/GPS 整合方位判定演算法，如圖二所示。 Multi-rate Extended Kalman filter DGPS CSC Algorithm 計算方位角 4 組單軸加速規 雙 GPS 接收機 計算繞固定軸旋 轉之向心與切線 加速度 an t a θ _{( )} j k, ˆ θ ( )k,j ˆ ω ( )k,j ˆ α 1 x A 2 y Ax2 A 1 y A 100 次/sec 1 次/sec bx y b 圖二、加速規與 GPS 整合系統示意圖 加速規/GPS 整合定向演算法，是結合一秒擷取一次訊 號之全球定位系統（GPS）和一秒擷取 100 次資料之加速規 （Accelerometer）作單軸方位判定之研究。本系統之演算 法主要使用多重取樣頻率擴增卡爾曼濾波器（Multi-rate Extended Kalman Filter）作為演算法核心，實際整合 GPS 和加速規之量測資料。 GPS 與加速規整合系統架構如圖三所示，

z

GPS

( )

k

,

0

與

( )

k

j

y

_ACC

,

分別為 GPS 和加速規之觀測量，假設加速規與 GPS 之量測資訊取樣頻率是 n：1，在 GPS 取樣時間 和 之間，加速規濾波器執行了 n 次預估和更新工 作，加速規濾波器之初始估計 由 GPS 量測更新所提 供。加速規濾波器之估測值為 ，其中 。 而在 時間點之 GPS 量測更新 由 及 GPS 測量值 所合成。

GPS 加速規 GPS Update 加速規filter

(

k n

)

xˆ −1,

( )

,0 ˆ k x

( )

k j x ,ˆ

( )

,0 ˆ k x

( )

k j yACC ,

( )

k,0 zGPS 圖三、GPS 與加速規整合系統 變角加速度方位角整合判定模型如下式所示： （1）系統狀態方程： ( ) ( ) ( )( ) ( ) v( )k j r j k a t j k j k t j k j k t , , 0 , , 1 0 1 1 , 1 , 1 +         ∆ +         ∆ =     + + ω θ ω θ

1

,

,

1

,

0

−

=

n

j

L

（43） （2）加速規量測方程：

( )

k j r

( )

k j

( )

k j an , , ACC , 2 1ω +ε = n j=1,2,L, （44） （3）GPS 量測方程：

( ) ( )

k,0 =θ k,0 +wGPS

( )

k,0 θ （45） 且

(

)

(

)



_



≡



_



( )

( )



_









+

+

n

k

n

k

k

k

,

,

0

,

1

0

,

1

ω

θ

ω

θ

（46） 其中：

( )

k

j

a

_n

,

與

a

_t

( )

k

,

j

分別為加速規所量測之向心及切線 加速度。

( )

k

,

0

θ

是由（3.14）式計算所得到之方位角。

( )

{

v ,

k

j

}

_、

{

w

GPS

( )

k

,

0

}

、

{

ε

ACC

( )

k

,

j

}

皆 是 零 均 值

（zero-means）之白色雜訊序列（white random sequences），

L

,

2

,

1

=

k

。 等角加速度方位角整合判定模型如下所示： （1）系統狀態方程：

(

)

(

)

(

)

( )

( )

( )

( )

k

j

v

( )

k

j

j

k

j

k

t

t

t

j

k

j

k

j

k

,

,

,

,

1

0

0

1

0

2

1

1

1

,

1

,

1

,

2

+









































∆

∆

∆

=





















+

+

+

α

ω

θ

α

ω

θ

1

,

,

1

,

0

−

=

n

j

L

（47） （2）加速規量測方程：

( )

k

j

( )

k

j

a

,

_



,

2





ω

（48） （3）GPS 量測方程： （49） 且

( )

( )

( )

(

)

(

)

(

)





















+

+

+

≡





















0

,

1

0

,

1

0

,

1

,

,

,

k

k

k

n

k

n

k

n

k

α

ω

θ

α

ω

θ

（50） 其中：

( )

k

j

a

n

,

與

a

t

( )

k

,

j

分別為加速規所量測之向心及切線 加速度。

( )

k

,

0

θ

是由（3.14）式計算所得到之方位角。

( )

{

v ,

k

j

}

、

{

w

_GPS

( )

k

,

0

}

、

{

ε

_ACC

( )

k

,

j

}

皆 是 零 均 值 （zero-means）之白色雜訊序列（white random sequences），

L

,

2

,

1

=

k

_。 變加速度模型中，利用加速規切線加速度

a

t

( )

k

,

j

輔助 高取樣頻率濾波器作狀態和量測預估，利用向心加速度

( )

k

j

a

n

,

作量測更新，並更新狀態。而在等加速度模型中， 模型遵照等加速度的假設來作狀態和量測預估，而加速規 所測之切線加速度

a

t

( )

k

,

j

與向心加速度

a

n

( )

k

,

j

則作預估 狀態之更新。 透過 GPS 方位角量測量可於整秒時修正整合系統之累 積誤差，降低方位角誤差協方差值

P

₁₁

( )

k

k

之大小，避免因 加速規量測量對方位角之不可觀測性，使系統誤差協方差 矩陣

P

( )

k

k

因長時間運作而過大。 四、系統架構與實驗成果： 在實驗部分，我們設計了一繞單軸旋轉之轉檯，如圖 四所示，從事加速規估測方位角之實驗與加速規和 GPS 整 合估測之實驗，利用變角加速度與等角加速度模型之濾波 器分別解算，並比較這兩種濾波模型對方位角估測之不同。 Ax Ay Az Ax Ay Az 三軸加速規 GPS GPS 三軸加速規 圖四、基線單軸旋轉轉檯 比較兩種模型之差異，等角加速度模型對於角速度之 估測 較為平滑，其原因是由於該模型假設系統作等角加 速度運動，所以在角加速度估測值 變化不大的情形下， 角速度估測值自然平滑。變角加速度模型直接利用 協助 預估角速度估測值 ，能夠立即反應轉動之角加速度變

化，故其

ω)

較等角加速度模型更能反應真實旋轉運動中角 速度細緻的變化。 等角加速度模型對於角速度之估測

ω)

較為平滑，所以 其模型在低轉速時比較不易發生方位角正反轉判斷錯誤的 問題。而變角加速度模型對於角速度之估測ω)較易振盪， 使其在低轉速時較容易發生方位角正反轉判斷錯誤，利用 二階 EKF 可使低轉速之角速度估測值ω)較為平滑，減少判 斷錯誤的機率。 五、結論： 本文以多取樣頻率之擴增卡爾曼濾波器整合加速規與 GPS 量測值以估測平面運動載具之方位角。我們針對此系 統設計了等角加速度與變角加速度兩種濾波器模型，並探 討這兩種系統模型之特性。 由於加速規之非線性角速度量測方程，使濾波器之增 益值會有來回穿越零點之特性，由實驗結果可知，利用二 階擴增卡爾曼濾波器可以有效降低當角速度穿越零點時， 濾波器增益值振盪且發散的問題，進而降低角速度估測值 之振盪現象，並減少方位角正反轉判定錯誤發生之機率。 加速規/GPS 整合系統判定方位角可以消除單獨用加速 規系統低轉速時正反轉判定之錯誤與其累積誤差，並改善 GPS 訊號取樣頻率低之困擾，確保 GPS 訊號失鎖時系統之 正常運作。 在後續研究方面，可將此單軸定向之演算法，擴展到 載具之三維姿態估測。 六、參考文獻：

[1] Wells, D. et al., Guide to GPS Positioning, Canadian GPS Associates, 1987.

[2] Kaplan, E. D., Understanding GPS Principles and Applications, Artech House, Inc., 1996.

[3] Fr ench, G. T., Understanding the GPS, GeoResearch,Inc.,1996.

[4] Hofmann-Wellenhof B. and Lichtenegger H. and Collins J ., Global Positioning System Theory and Practice, Springer-Verlag Wien, 1997.

[5] Motor ola, Oncore Receiver user ’s guide, Motorola, 1994.

[6] Summit,34100A Theory of Operation, Summit, 1997. [7] Papoulis, A., Probability, Random variable, and

Stochastic Process, McGraw-Hill, 1984.

[8] Yaakov Bar -Shalom, Thomas E. For tmann,Tracking and Data Association, Academic Press, 1987.

[9] Rober t Gr over Br own, Patr ick Y. C. Hwang,

Introduction to Random Signals and Applied Kalman Filtering with Matlab Exercises and Solutions, John Wiley &Sons, 1997

[10] . Gr ewal, M. S., and Andr ews, A. P., Kalman filtering theory and practice, Prentice-Hall, Inc., 1993.

[11] J ay Far r ell, Matthew Bar th, The Global Positioning System and Inertial Navigation, McGraw-Hill, 1998. [12] Kenneth R. Br itting, Inertial Navigation System

Analysis, John Wiley &Sons, 1976.

[13] Anthony Lawr ence, Modern Inertial Technology Navigation, Guidance, and Control, Springer-Verlag Wien, 1998.

[14] Myr on Kayton, Walter R. Fr ied, Avionics Navigation Systems, John Wiley &Sons, 1969.

[15] H. M. Peng, Y. T. Chiang, F. R. Chang and L.-S. Wang, 2000,“Maximum-Likelihood-Based Filtering for Attitude Determination via GPS Carrier Phase,” Position, Location, and Navigation Symposium, San Diego. [16] N. K. Mital, A. I. King, “Computation of Rigid-Body

Rotation in Three-Dimensional Space From Body-Fixed Linear Acceleration Measurements”, Journal of Applied Mechanics, Vol. 46, pp. 925-930, 1979.

[17] J eng-Heng Chen, Sou-Chen Lee, Daniel B. DeBr a, “Gyroscope Free Strapdown Inertial Measurement Unit by Six Linear Accelerometers”, Journal of Guidance Control and Dynamics, Vol. 17, No. 2, pp. 286-290, 1994. [18] 任思聰, 實用慣導系統原理,宇航, 1988. [19] 董緒榮, 張守信, 華仲春,GPS/INS 組合導航定位及其 應用, 國防科技大學, 1998. [20] 秦永元, 汪淑華等,卡爾曼濾波與組合導航原理, 西北 工業大學, 1998. [21] 黃國興,慣性導航系統原理與應用,全華, 中華民國 85 年 2 月. [22] 莊智清, 黃國興,e-navigation 電子導航,全華, 中華民 國 90 年 4 月. [23] 宋真坦等, 飛機通訊與導航系統, 高立, 中華民國 87 年 2 月. [24] 吳明達, 全球定位系統整合載波平滑碼定位法, 台大 應力所碩士論文, 中華民國 87 年 10 月. [25] 陳浩祥,長時間載波平滑碼定位法, 台大應力所碩士 論文, 中華民國 88 年 6 月.

0 50 100 150 200 250 300 350 400 450 500 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 Time(sec) A ng ul a r r a te (r a d /se c) 等角加速度模型 角速度估測值 藍: 加速規與GPS 一階EKF 紅: 加速規與GPS 二階EKF 橙: 加速規 一階EKF 綠: 加速規 二階EKF 圖四、等角加速度模型之角速度估測值

ωˆ

( )

k

0 50 100 150 200 250 300 350 400 450 500 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 time(sec) a ng ul a r r a te (r a d /se c) 變角加速度模型 角速度估測值 藍: 加速規與GPS 一階EKF 紅: 加速規與GPS 二階EKF 橙: 加速規 一階EKF 綠: 加速規 二階EKF 圖五、變角加速度模型之角速度估測值

ωˆ

( )

k