模擬及討論

為了驗證前述之理論，故用MATLAB 進行模擬，令取樣頻率為 400HZ，利用電腦 產生包含高斯分布的隨機雜訊，其平均值為零，給予加速規的雜訊標準差為0.1m/sec^2，

而磁場感測器雜訊標準差0.01gauss，並設計各種運動方式來瞭解演算法之特性。

INS Quality

Sensor Parameter Unit High Medium Low Accelerometer bias μg ≦50 200-500 ≧1000 Gyro bias deg/hour ≦0.01 0.1~1.0 ≧10

Accelerometer white noise (μg)²/Hz ≦100 ~2500 >2500 Gyro white noise (deg/s) ²/Hz ≦10^-9 ~10^-6 >10^-6

表(4.1) 慣性感測器的偏差及雜訊

4-1 三軸旋轉運動

輸入ωx~ωz角速度皆為弦波訊號，其訊號如下

1.0966 sin(2 )

2.1932 sin(2 )

2.1932 sin(2 ) 2 0 ~ 2sec

x

y

z

f t f t

f t f t

ω π

ω π

ω π

= × ⋅

= × ⋅

= × ⋅ = =

4-1.1 間接量測法

Kalman filter

A

₁

A

₂

A

_3x

A

_3y

A

₄

A

₅

A

₆

(2.26) (3.11) (3.12) (3.15)

M

_x

M

_y

M

_z

ω

_x

ω

_y

ω

_z

q

₁

q

₂

q

₃

q

₄

Z

₁

Z

₂

Z

₃

圖(4.1) 間接量測法訊號流程

輸入各軸的角速度，故可得A1~A6加速規的輸出訊號如圖(4.2)、圖(4.3)及磁場感測 器所量測的訊號如圖(4.4)。利用公式(2.26)(3.11)(3.12)(3.15)計算之後所得的量測值如圖 (4.5)，依序對照方程式的Z1~Z3。把以上所獲得的資訊如圖(4.1)代入擴增卡爾曼估測器及 疊代卡爾曼估測器運算法，因此可獲得ωx~ωz角速度的估測值以及四元素q1~q4的估測 值，其模擬結果如圖(4.6)(4.8)，誤差的收斂情形的如圖(4.7)(4.9)。因為尤拉角比四元數 而言較有物理意義，故利用公式(2.15)把所估測的四元素轉換成尤拉角如圖(4.10)。在圖 (4.10)中淺藍色的’x’為使用加速規及磁場感測器所直接量得的尤拉角，因此由圖(4.10) 中，可發現利用擴增卡爾曼估測器及疊代卡爾曼估測器所估測姿態的確實減少了量測雜 訊的影響。圖(4.12)(4.13)(4.14)為角速度及姿態的誤差標準差(state error standard deviation) 收斂情況，表(4.2)(4.3)(4.4)為經過 2 秒時估測器所計算的誤差標準差，可知IKF收斂精 度比EKF來的好一點。

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -4

-2 0 2 4 6 8

Ax

m/sec2

No.1 No.2 No.3x No.3y

圖(4.2) 間接量測法：3 軸運動之 A1~A3y 加速規訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

5 6 7 8 9 10 11 12 13 14

Ax

m/sec2

No.4 No.5 No.6

圖(4.3) 間接量測法：3 軸運動之 A4~A6 加速規訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

Magnetic measurement

gauss

sec

x axial y axial z axial

圖(4.4) 間接量測法：3 軸運動之 Mx~Mz 磁場感測器訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

wx Error

rad/sec

wy Error

rad/sec

wz Error

rad/sec

sec

圖(4.7) 間接量測法：3 軸運動之角速度誤差值

0 0.5 1 1.5 2

圖(4.10) 間接量測法：3 軸運動之尤拉角估測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.2 -0.1 0 0.1 0.2

roll Error

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.4 -0.2 0 0.2 0.4

pitch Error

degree

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

yaw Error

sec

degree

圖(4.11) 間接量測法：3 軸運動之尤拉角誤差值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

EKF 0.0534215 0.0603691 0.0825561 IKF 0.0530846 0.0679526 0.0816686

表(4.2) 間接量測法：3 軸運動經過 2 秒時角速度誤差標準差

0 0.5 1 1.5 2

EKF 0.00179418 0.00118782 0.00319378 0.000548506 IKF 0.00177664 0.00117169 0.00317014 0.000540283

表(4.3) 間接量測法：3 軸運動經過 2 秒時四元數誤差標準差

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

5 10

roll angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10

pitch angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 2 4 6 8

yaw angle std

sec

degree

EKF IKF

圖(4.14) 間接量測法：3 軸運動之尤拉角誤差標準差

Roll (degree) Pitch (degree) Yaw (degree)

EKF 0.193084 0.135928 0.358407

IKF 0.192943 0.133034 0.355625

表(4.4) 間接量測法：3 軸運動經過 2 秒時尤拉角誤差標準差

4-1.2 直接量測法

Kalman filter

Z

₃

A

₂

A

_3y

A

₄

A

₅

A

₆

(2.26)

M

_x

(Z

₁

) M

_y

(Z

₂

)

ω

_x

ω

_y

ω

_z

q

₁

q

₂

q

₃

q

₄

Z

₄

A

₁

圖(4.15) 直接量測法訊號流程

如 3-2.2 節所述，使用直接量測法可把A3x加速規移除，利用A2~A6加速規所量測的 值經由(2.26)式計算可以獲得Z3、Z4的值。在磁場感測器方面，也移除z軸感測器，而此 時在物體上所測量到的x軸及y軸的磁場強度直接輸入卡爾曼估測器的運算法中，其訊號 流程如圖(4.15)。圖(4.16)(4.17)(4.18)，為加速規及磁場感測器量測的值。由加速規A2~A6

及磁場感測器Mx、My因此可得到Z1~Z4如圖(4.19)。代入擴增卡爾曼估測器及疊代卡爾曼 估測器運算法，因此可獲得ωx~ωz角速度的估測值以及姿態估測值，其模擬結果如圖 (4.20)(4.22)(4.24)，誤差的收斂情形的如圖(4.21)(4.23)(4.25)。在圖(4.24)中淺藍色的’x’

為使用加速規及磁場感測器直接量得的尤拉角，因此由圖(4.24)中，可發現利用擴增卡 爾 曼 估 測 器 及 疊 代 卡 爾 曼 估 測 器 所 估 測 姿 態 的 確 實 減 少 了 量 測 雜 訊 的 影 響 。 圖 (4.26)(4.27)(4.28)為角速度及姿態的誤差標準差收斂情況，表(4.5)(4.6)(4.7)為經過 2 秒時 估測器所計算的誤差標準差，可知IKF收斂精度比EKF來的好一點。

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -4

-3 -2 -1 0 1 2 3 4 5 6

Ax

m/sec2

No.1 No.2 No.3y

圖(4.16) 直接量測法：3 軸運動之 A1~A3y 加速規訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

5 6 7 8 9 10 11 12 13 14

Ax

m/sec2

No.4 No.5 No.6

圖(4.17) 直接量測法：3 軸運動之 A4~A6 加速規訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

Magnetic measurement

gauss

sec

x axial y axial

圖(4.18) 直接量測法：3 軸運動之 Mx、My 磁場感測器訊號

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

wx Error

rad/sec

wy Error

rad/sec

wz Error

rad/sec

sec

圖(4.21) 直接量測法：3 軸運動之角速度誤差值

0 0.5 1 1.5 2

q3 Error

0 0.5 1 1.5 2

圖(4.24) 直接量測法：3 軸運動之尤拉角估測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.2 -0.1 0 0.1 0.2

roll Error

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.2 0 0.2

pitch Error

degree

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

yaw Error

sec

degree

圖(4.25) 直接量測法：3 軸運動之尤拉角誤差值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

EKF 0.0543299 0.069509 0.0785577 IKF 0.0549644 0.0730245 0.0790014

表(4.5) 直接量測法：3 軸運動經過 2 秒時角速度誤差標準差

0 0.5 1 1.5 2

EKF 0.00182298 0.00154747 0.00296722 0.000509682 IKF 0.00183037 0.00156135 0.00297157 0.000510061

表(4.6) 直接量測法：3 軸運動經過 2 秒時四元數誤差標準差

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

5 10

roll angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10 15

pitch angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10

yaw angle std

sec

degree

EKF IKF

圖(4.28) 直接量測法：3 軸運動之尤拉角誤差標準差

Roll (degree) Pitch (degree) Yaw (degree)

EKF 0.194311 0.180333 0.335605

IKF 0.194922 0.18275 0.336073

表(4.7) 直接量測法：3 軸運動經過 2 秒時尤拉角誤差標準差

4-2 俯仰角為 90 時的旋轉運動

由2-3.1 節曾提及當俯仰角(pitch angle)為 90 度時，姿態以尤拉角表示時則會有發散 的現象。間接量測法利用加速規及磁場感器的整合量測出尤拉角的量再轉換至四元數，

因此發散的現象會在此方法中出現。如圖(4.29)(4.30)為模擬運動體在俯仰角 90 度附近 旋轉，則此時的姿態估測失敗。在本論文的所提出的第二個方法直接量測法中，利用感 測器直接量測四元數，由於四元數的優點就是不會有奇異點的問題，因此在直接量測法 中狀態估測器可以正常的運作，如圖(4.31)(4.32)為模擬運動體在俯仰角 90 度附近旋轉。

0 0.5 1 1.5 2

0 0.5 1 1.5 2 -0.02

0 0.02 0.04 0.06 0.08

q1

EKF IKF true

0 0.5 1 1.5 2

0.55 0.6 0.65 0.7 0.75

q2

0 0.5 1 1.5 2

-5 0 5 10

15x 10^-3 q3

0 0.5 1 1.5 2

0.7 0.75 0.8 0.85 0.9

q4

sec

圖(4.31) 直接量測法：俯仰角為 90 度時之四元數估測值

圖(4.32) 直接量測法：俯仰角為 90 度時之尤拉角估測值

4-3 靜止運動

由4-1 節已測試估測器 3 軸旋運動下能成功估測出物體姿態，再進一步測試估測器 在靜止狀態下的穩定性。由本節中的模擬可知演算法在靜止運動時也能正常的運作，並 無飄移及發散的現象。

4-3.1 間接量測法

圖(4.33)(4.34)(4.35)為模擬靜止時加速規及磁場感測器所量測的值，圖(4.36)為 Z1~Z3 的量測值，因此代入卡爾曼濾器故可得角速度估測值如圖(4.37)、姿態估測值如 圖(4.39)(4.41)，圖(4.38)(4.40)(4.42)為誤差隨著時間的變化，圖(4.43)(4.44)(4.45)為誤差 標準差收斂情形，表(4.8)(4.9)(4.10)為經過 2 秒時誤差標準差的值。

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -0.4

-0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4

Ax

m/sec2

No.1 No.2 No.3x No.3y

圖(4.33) 間接量測法：靜止運動時之 A1~A3y 加速規量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

9.4 9.5 9.6 9.7 9.8 9.9 10 10.1 10.2 10.3

Ax

m/sec2

No.4 No.5 No.6

圖(4.34) 間接量測法：靜止運動時之 A4~A6 加速規量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

Magnetic measurement

gauss

sec

x axial y axial z axial

圖(4.35) 間接量測法：靜止運動時之 Mx~Mz 磁場感測器量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

wx Error

rad/sec

wy Error

rad/sec

wz Error

rad/sec

sec

圖(4.38) 間接量測法：靜止運動時之角速度誤差值

0 0.5 1 1.5 2

圖(4.41) 間接量測法：靜止運動時之尤拉角估測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

roll Error

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

pitch Error

degree

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

yaw Error

sec

degree

圖(4.42) 間接量測法：靜止運動時之尤拉角誤差值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

EKF 0.0489364 0.0526945 0.0699814 IKF 0.0493237 0.0602448 0.0696266

表(4.8) 間接量測法：靜止運動經過 2 秒時角速度誤差標準差

0 0.5 1 1.5 2

EKF 0.00167559 0.0009566 0.00297959 0.00030126 IKF 0.00167858 0.000961408 0.00297664 0.000298442

表(4.9) 間接量測法：靜止運動經過 2 秒時四元數誤差標準差

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

5 10

roll angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10

pitch angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 2 4 6 8

yaw angle std

sec

degree

EKF IKF

圖(4.45) 間接量測法：靜止運動時之尤拉角誤差標準差

Roll (degree) Pitch (degree) Yaw (degree)

EKF 0.175456 0.112958 0.32696

IKF 0.175755 0.113943 0.326444 表(4.10) 間接量測法：靜止運動經過 2 秒時尤拉角誤差標準差

4-3.2 直接量測法

圖(4.46)(4.47)(4.48)為模擬靜止時加速規及磁場感測器所量測的值，Z1、Z2 由磁場 感測器直接獲得，Z3 及 Z4 由方程式(2.26)計算出估測器所需的量測值，因此如圖(4.49) 所示。故把 Z1~Z4 代入卡爾曼濾器故可得角速度估測值如圖(4.50)、姿態估測值如圖 (4.52)(4.54)，圖(4.51)(4.53)(4.55)為誤差隨著時間的變化，圖(4.56)(4.57)(4.58)為誤差標 準差收斂情形，表(4.11)(4.12)(4.13)為經過 2 秒時誤差標準差的值。

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -0.4

-0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4

Ax

m/sec2

No.1 No.2 No.3y

圖(4.46) 直接量測法：靜止運動時之 A1~A3y 加速規量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

9.4 9.5 9.6 9.7 9.8 9.9 10 10.1 10.2 10.3

Ax

m/sec2

No.4 No.5 No.6

圖(4.47) 直接量測法：靜止運動時之 A4~A6 加速規量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

Magnetic measurement

gauss

sec

x axial y axial

圖(4.48) 直接量測法：靜止運動時之 Mx、My 磁場感測器量測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

wx Error

rad/sec

wy Error

rad/sec

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.05 0 0.05

wz Error

rad/sec

sec

圖(4.51) 直接量測法：靜止運動時之角速度誤差值

0 0.5 1 1.5 2

圖(4.54) 直接量測法：靜止運動時之尤拉角估測值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

roll Error

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

pitch Error

degree

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-0.5 0 0.5

yaw Error

sec

degree

圖(4.55) 直接量測法：靜止運動時之尤拉角誤差值

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

EKF 0.0504285 0.0636494 0.0722155 IKF 0.0511206 0.0672784 0.0726071

表(4.11) 直接量測法：靜止運動經過 2 秒時角速度誤差標準差

0 0.5 1 1.5 2

EKF 0.00172898 0.00146212 0.00298883 0.000298423 IKF 0.00173872 0.00147368 0.00299294 0.000297954

表(4.12) 直接量測法：靜止運動經過 2 秒時四元數誤差標準差

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

5 10

roll angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10

pitch angle std

degree

EKF IKF

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 5 10

yaw angle std

sec

degree

EKF IKF

圖(4.58) 直接量測法：靜止運動時之四元數誤差標準差

Roll (degree) Pitch (degree) Yaw (degree)

EKF 0.173629 0.180776 0.327432

IKF 0.174406 0.182882 0.327851

表(4.13) 直接量測法：靜止運動經過 2 秒時尤拉角誤差標準差

4-4 間接量測法及直接量測法的優缺點比較

直接量測法有4 條量測方程式而間接量測法只有 3 條量測方程式，因此間接量測法 電腦運算所耗的時間會比直接量測法來的少一點。在估測精度上，此兩法之估測精度差 異不大，可參考表(4.8)(4.9)(4.10)(4.11)(4.12)(4.13)。

間接量測法需要7 個加速度規及 3 軸磁場感測器，而直接量測只需 6 個加速規 2 軸 磁場感測器，因此在實際應用上直接量測法較為經濟。

間接量測法為量測尤拉角轉換至四元數值，所以當俯仰角(pitch angle)為 90 度時使 用此方法估測器會運算失敗。而直接量測法則無此問題，在俯仰角為 90 度時估測器仍 可正常的運作。

由於直接量測法需已知目前位置的磁場向量，因此在使用上只能固定位置，若當目 前位置變更時，使用前需重新校正此時的磁場向量。而間接量測法則無校正的問題，此 法在應用時可在地球上的任意位置使用。

4-5 平面式之純加速規慣性量測單元姿態估測及陀螺儀姿態估測之比較

在傳統的姿態估測中，皆以陀螺儀、加速規及磁場感測器為主要元件。在參考文獻 [2][3][4]就是利用前敘的三種感測元件來完成姿態的估測。為了比較本論文之方法與傳 統的姿態估測，故參考[2]所提出方法來模擬比較兩者的差異。其陀螺儀姿態估測器架構 如圖(4.59)，其中卡爾曼估測器的狀態方程為(2.16)式，量測方程式為(2.15)式。

Kalman filter

A

_x

A

_y

A

_z

(3.11) (3.12) (3.15)

M

_x

M

_y

M

_z

q

₁

q

₂

q

₃

q

₄

Z

₁

Z

₂

Z

₃

ω

_x

ω

_y

ω

_z

圖(4.59) 陀螺儀姿態估測器訊號流程

故模擬結果如表(4.14)(4.15)及圖(4.60)，由表(4.14)當陀螺儀的量測雜訊標準差為 0.2 rad/sec 時，其估測精度大約與本論文所提方法相同，陀螺儀的雜訊愈大其估測效能愈 差，圖(4.60)為表(4.14)的直線圖。表(1.45)本論文的所提出的方法所估測到角速度的標準 差。

Noise (rad/sec) Roll(degree) Pitch(degree) Yaw(degree) Measurement --- 0.59409 0.569463 1.50727

gyro gyro Std 0.03 0.02492 0.029246 0.027 gyro Std 0.1 0.06606 0.078879 0.08434 gyro Std 0.2 0.11333 0.128593 0.15245 gyro Std 1 0.28082 0.279052 0.47802 gyro Std 10 0.56266 0.485279 1.20272 gyrofree --- 0.12035 0.090792 0.1716

表(4.14) 平面式之純加速規慣性量測單元姿態估測與陀螺儀姿態估測的比較

ωx (rad/sec) ωy (rad/sec) ωz (rad/sec) gyrofree 0.0249928 0.0346422 0.0333314

表(4.15) 平面式之純加速規慣性量測單元姿態估測器穩態時的標準差

Gyrofree vs. Gyro

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

roll pitch yaw

de g re e

Measurement gyro Std 0.03 gyro Std 0.1 gyro Std 0.2 gyro Std 1 gyro Std 10 gyrofree

圖(4.60) 平面式之純加速規慣性量測單元姿態估測與陀螺儀姿態估測的比較

第五章 結論及未來計劃

在文檔中 使用平面加速規及磁場感測器之姿態估測器 (頁 46-89)