• 沒有找到結果。

整合GNSS與INS量測資訊的地平面運動軌跡估測

N/A
N/A
Protected

Academic year: 2021

Share "整合GNSS與INS量測資訊的地平面運動軌跡估測"

Copied!
82
0
0

加載中.... (立即查看全文)

全文

(1)

國 立 交 通 大 學

電控工程研究所

碩 士 論 文

整合 GNSS 與 INS 量測資訊的地平面運動軌跡估測

Motion Trajectory Estimation on Earth Surface by

Integrating GNSS and INS Measurements

研 究 生: 鄭 期 元

指導教授: 胡 竹 生 博士

(2)

整合 GNSS 與 INS 量測資訊的

地平面運動軌跡估測

Motion Trajectory Estimation on Earth Surface by Integrating

GNSS and INS Measurements

研 究 生:鄭期元 Student:Chi-Yuan Cheng

指導教授:胡竹生 博士 Advisor:Prof. Jwu-Sheng Hu

國 立 交 通 大 學

電 機 與 控 制 工 程 研 究 所

碩 士 論 文

A Thesis

Submitted to Institute of Electrical 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

August 2013

Hsinchu, Taiwan, Republic of China

(3)

i

整合 GNSS 與 INS 量測資訊的

地平面運動軌跡估測

研究生:鄭 期 元

指導教授:胡 竹 生 博士

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

摘 要

本論文提出了一套以卡爾曼濾波器整合鬆散耦合之GNSS/INS 系統應用於軌跡紀錄 中,離線適應性估測卡爾曼濾波器量測雜訊的方法。衛星定位系統中,環境會影響衛星 訊號的接收,使量測雜訊的標準差隨環境與時間變化,因此估測量測雜訊相當重要。在 以往提出的線上估測方法中,利用移動平均法,對過去視窗區間的卡爾曼創新序列估測 當下的量測雜訊,僅在雜訊標準差隨時間變化不大的環境有較佳的結果,例如行駛於無 遮蔽的高速公路上;但無法應付雜訊標準差變化快的環境,例如行駛於眾多高樓的市區 中,接收衛星的數量會受高樓遮蔽影響,造成雜訊的標準差變動程度增加。 本論文利用可調維度的 Savitzky-Golay濾波器,針對以當下取樣點為視窗區間中心, 延遲取樣後所得到卡爾曼創新序列資訊,以可調變維度的局部多項式回歸法做量測雜訊 的估測,並以統計學 F 檢定法適應性找出相對應維度,效果優於未經延遲取樣、僅參考 過去區間資訊且固定維度的移動平均法,能估測雜訊標準差變化環境下的量測雜訊,在 運動軌跡紀錄的結果優於以往線上估測的方法。模擬結果本方法的水平誤差為 0.3m,較 其它方法有約 1 倍精準度的提升。

(4)

ii

Motion Trajectory Estimation on Earth Surface by

Integrating GNSS and INS Measurements

Student: Chi-Yuan Cheng

Advisor: Prof. Jwu-Sheng Hu

Institute of Electrical and Control Engineering

Abstract

This thesis proposes a new method which estimates the Kalman filter’s measurement noise covariance matrix in a loosely coupled GNSS/INS sensor fusion system. The signals received from the satellites are heavily influenced by the environmental conditions, and will cause the noise variance to change dramatically. Thus, estimating the measurement noise to improve the fusion quality is very important. Most of the proposed methods use the moving average method to estimate the measurement noise online from the innovation sequence of the Kalman filter in the window interval. However, those methods can only have a good performance in a quasi-static environment, e.g., freeway where the signals are rarely blocked. In dynamic environments such as the urban area, the satellite signals are often blocked by the buildings, resulting in a large range of the noise variance. This thesis proposes to use Savitzky-Golay filter with degree adaptation (ADSG) to estimate the measurement noise. The ADSG filter uses local polynomial regression method to fit the delayed-sample innovation sequence in the window interval, and adjust the polynomial order by the statistics F-test. Experiments show that the proposed method’s horizontal position error is about 0.3m, which is better than several existing methods.

(5)

iii

誌 謝

從大三做專題開始進入 xLab 這個大家庭,到現在碩二畢業,轉眼間就過了四年, 回頭檢視自己在這四年中著實成長了不少,首先最要感謝的是恩師 胡竹生老師,他對 於做研究認真不苟且的態度、面對問題追根究底的精神以及解決問題的邏輯批判能力與 方法都深深影響了我,使我在往後的研究能以工程師的角度去解決問題,且老師的研究 涉及各個領域,也使得每週實驗室報告的時候常常能有新的斬獲。另外一位恩人是博士 班 孫冠群 Judo 學長,從我還是什麼都不會的小專題生開始一路提拔我、照顧我整整四 年,想必一定很累吧哈哈。Judo 學長的研究態度完全繼承了胡老師的風格,切入問題總 是用最犀利的角度,常常我的疑問被他一句話就點醒了,當場讓我佩服的五體投地,使 我往後在面對問題時會在心中假想一個 judo 學長會怎麼反擊我,還滿有幫助的!另外還 要感謝不吝於分享與指教的阿吉學長、常關心我近況數學超強的明唐學長,把我綽號意 外降級的大師兄學長,還有每次都熱情邀情我買便當使我充滿歉意的昭男學長、當助教 很貼心又很仁慈打扮又很潮的耕維學長、畫立體機構很帥的德洋學長、有實力又很有魅 力,很懂得交際的振華學長,外表看似小孩智慧卻過於常人而且跟我一樣悶騷但比我帥 很多的阿法學長。另外還有同屆從大學開始一起做專題上來、一起比過漫長龍騰比賽的 冠宏、哲宇,最早來實驗室又最認真、硬體能力超強為人又開朗的阿文,我心中做研究 的學習對象,兼具各項研究能力也跟我一樣愛看戲劇的鳴遠,交際能力很強很大器、身 旁常常圍繞著女生讓人羨慕的小山東,熱愛 3C、電影分析及鋼彈讓我大開眼界的大夢。 另外打籃球很狂野的宗翰學長,長的斯斯文文但很幽默的建廷學長,認真求學又認真享 受人生,做人體貼的哲鳴學長,好久以前系網就認識的學弟佑軒,跟我同領域也熱衷於 球賽與戲劇擁有奇特眼睛的凱翔,很享受人生又滿有實力的凱傑,做事認真負責且個性 溫和的小綜,個人風格很強讓人印象深刻的小樂。 最後要特別感謝我的父母從小栽培我念書,使我今日能順利拿到碩士學位,沒有你 們就不會有今天的我。另外還有一路相互支持、友誼長存、共同奮鬥的好姐妹冠頭家族, 還有陪我渡過最難熬的求學生涯,每次見到都讓我很抒壓,把我照顧的無微不至的品勻, 以及讓我在最後的衝刺階段依然能每天充滿活力與歡樂的 Kuri&Kirua。最後感謝交通大 學提供充足的學習環境,願將來能盡自己能力,報答母校栽培之恩。

(6)

iv

目 錄

摘 要 ... I ABSTRACT ... II 誌 謝 ... III 目 錄 ... IV 表 列 ... VI 圖 列 ...VII 第一章 緒論... 1 1.1 研究動機... 1 1.2 研究目標... 1 1.3 文獻回顧... 2 1.4 論文貢獻... 4 1.5 論文架構... 4 第二章 座標系統... 5 2.1 座標系統定義... 5 2.1.1 慣性座標系 ... 5 2.1.2 地心地固座標系 ... 6 2.1.3 地理座標系 ... 6 2.1.4 導航座標系 ... 7 2.1.5 附體座標系 ... 7 2.2 座標系旋轉... 7 2.2.1 方向餘弦法 ... 7 2.2.2 四元數法 ... 8 2.2.3 尤拉角法 ... 9 2.3 座標系間的轉換關係... 10 2.3.1 地理與地心地固座標系轉換 ...11 2.3.2 地心地固與導航座標系轉換 ... 12 2.3.3 導航與附體座標系轉換 ... 12 2.4 旋轉矩陣的動態方程式... 14 第三章 全球導航衛星系統與慣性導航系統之整合... 15 3.1 全球導航衛星系統... 15 3.1.1 全球導航衛星系統介紹 ... 16 3.2 慣性導航系統... 19 3.2.1 慣性導航方程式 ... 19 3.3 慣性導航誤差模型... 21 3.3.1 慣性導航誤差方程式 ... 21

(7)

v 3.4 以卡爾曼濾波器實現鬆散耦合的 GNSS/INS 系統 ... 23 3.4.1 卡爾曼濾波器 ... 24 3.4.2 鬆散耦合的 GNSS/INS 系統 ... 25 第四章 基於適應性調變維度之 SAVITZKY-GOLAY 濾波器的雜訊估測 ... 29 4.1 SAVITZKY-GOLAY濾波器 ... 29 4.2 適應性調變維度之 S-G 濾波器(ADSG)... 32 4.3 基於 ADSG 的雜訊估測法... 35 第五章 模擬與實驗結果分析... 39 5.1 加入已知雜訊的模擬分析... 39 5.2 實際道路測試... 48 5.2.1 iPhone5 與實驗室整合之感測資訊記錄器硬體 xGIC 介紹 ... 48 5.2.2 汽車行駛軌跡估測結果 ... 50 第六章 結論... 64 6.1 研究成果... 64 6.2 未來展望... 64 附錄 ... 65 參考文獻 ... 68

(8)

vi

表 列

表 2.2.1 尤拉座標系旋轉序列 ... 9 表 3.1.1 全球目前主要的衛星導航系統 ... 16 表 5.1.1 系統 OXTS RT3003 硬體規格 ... 40 表 5.1.2 模擬中各方法的估測誤差方均根比較(單位:公尺) ... 46

表 5.2.1 手機IPHONE5 慣性儀硬體規格與記錄軟體APP,SENSOR DATA ... 48

表 5.2.2 慣性儀 ADIS16480 硬體規格... 49

表 5.2.3 衛星接收模組 GPS-M2 硬體規格 ... 50

(9)

vii

圖 列

圖 2.1.1 慣性、地心地固、導航座標系 ... 6 圖 2.2.1 向量與四元數法轉換 ... 9 圖 2.2.2 姿態角名稱介紹 ... 10 圖 2.3.1 導航與附體座標系投影關係 ... 13 圖 3.1.1 衛星系統三角定位法 ... 17 圖 3.4.1 卡爾曼濾波器演算法流程圖 ... 25 圖 3.4.2 鬆散耦合之 GNSS/INS 架構 ... 28 圖 4.1.1 S-G 濾波器原理示意,M=2,N=2 ... 30 圖 4.1.2 S-G 濾波器的頻率響應 ... 31 圖 4.1.3 S-G 截止頻率與維度關係圖 ... 31 圖 4.2.1 模擬加入高斯雜訊的 S-G 估測訊號... 33 圖 4.2.2 ADSG 法濾高斯雜訊結果與對應維度 ... 33 圖 4.2.3 固定維度 S-G 濾濾高斯雜訊模擬結果 ... 34 圖 4.3.1 模擬卡方分布訊號做濾波 ... 36 圖 4.3.2 適應性維度與固定維度之 S-G 濾波器估測卡方分部訊號模擬結果... 37 圖 4.3.3ADSG 離線估測量測雜訊流程圖 ... 38 圖 5.1.1 資料包感測器安裝位置圖 ... 39 圖 5.1.2 資料包實驗平台 ... 39 圖 5.1.3 模擬中未經整合之 GNSS 與 INS 軌跡圖... 41 圖 5.1.4 模擬中經 GNSS/INS 整合後的軌跡圖 ... 41 圖 5.1.5 模擬中估測的量測雜訊標準差(北) ... 42 圖 5.1.6 模擬中估測的量測雜訊標準差(東) ... 42 圖 5.1.7 模擬中量測雜訊標準差估測誤差(北) ... 43 圖 5.1.8 模擬中量測雜訊標準差估測誤差(東) ... 43 圖 5.1.9 模擬中的位置誤差(北) ... 44 圖 5.1.10 模擬中的位置誤差-以 KNOWNR 為參考基準(北)... 44 圖 5.1.11 模擬中的位置誤差(東) ... 45 圖 5.1.12 模擬中的位置誤差-以KNOWNR 為參考基準(東) ... 45 圖 5.1.13 衛星 GNSS 資料偏移之 A 處放大圖 ... 46 圖 5.2.1 實驗平台XGIC 硬體全圖 ... 49 圖 5.2.2 實驗平台架設圖 ... 50 圖 5.2.3 前進方向加速度濾波前後比較-IPHONE5... 51 圖 5.2.4 前進方向加速度濾波前後比較-XGIC ... 51 圖 5.2.5 導航角角速度濾波前後比較-IPHONE5... 52 圖 5.2.6 導航角角速度濾波前後比較-XGIC ... 52

(10)

viii 圖 5.2.7 交大環校未整合的原始軌跡-IPHONE5... 53 圖 5.2.8 加入 NHC 與車體僅往前行駛限制的原始軌跡-IPHONE5 ... 53 圖 5.2.9 經整合後的交大環校軌跡圖-IPHONE5... 54 圖 5.2.10 交大環校軌跡 A 區放大圖-IPHONE5... 54 圖 5.2.11 交大環校軌跡記錄(北)- IPHONE5 ... 55 圖 5.2.12 交大環校軌跡記錄(東)- IPHONE5 ... 55 圖 5.2.13 量測雜訊估測(北)- IPHONE5 ... 56 圖 5.2.14 量測雜訊估測(東)- IPHONE5 ... 57 圖 5.2.15 加入 NHC 與速度方向的交大環校原始軌跡圖-XGIC ... 58 圖 5.2.16 經整合後的交大環校軌跡圖-XGIC ... 58 圖 5.2.17 交大環校軌跡記錄(北)-XGIC ... 59 圖 5.2.18 交大環校軌跡記錄(東)-XGIC ... 59 圖 5.2.19 交大環校速度記錄(北)-XGIC ... 60 圖 5.2.20 交大環校速度記錄(東)-XGIC ... 60 圖 5.2.21 姿態角記錄結果-XGIC... 61 圖 5.2.22 量測雜訊估測(北)- XGIC ... 62 圖 5.2.23 量測雜訊估測(東)- XGIC ... 62

(11)

1

第一章 緒論

1.1

研究動機

行車記錄器(Event Data Recorder,EDR),擁有類似飛機的黑盒子紀錄器的功 能,可以連續記錄車輛瞬間的行駛速度、行徑軌跡、車輛方向、影像與時間,在 交通意外發生時,扮演著相當重要的角色。不僅能幫助相關單位能快速釐清事故 發生的經過,同時也保護了駕駛自身的權益。隨著科技的進步與各國政府法規的 大力推動下,行車紀錄器的發展越發蓬勃。 然而目前市面上銷售的行車記錄器中,絕大多數是以攝影機效能為主打,強 調的是鏡頭拍攝的最大廣角角度與 CMOS 元件的感光能力,但在現今的法律中, 行車記錄器的影像畫面僅能做為輔佐證據,法律規範的效力上相當有限。即便少 數產品中有搭載重力計(G-sensor),其重力計的功能也僅用於偵測碰撞的發生, 並未有任何軌跡記錄的功用。 在未來,行車記錄器若是能更佳完善地與慣性儀系統整合,記錄更精準的運 動軌跡,相信能加速推動行車記錄器在法律規範或是交通安全層面上的落實發展, 保障社會上更多人的權益。

1.2

研究目標

本論文研究目標為利用 GNSS/INS 整合系統,進行車輛運動軌跡、速度與 車輛姿態的估測。一般來說,GNSS/INS 整合系統都是應用在即時導航上,所面 臨的技術問題是線上估測雜訊的方法,它影響到導航定位的精準度,造成導航上 的誤差。然而在行車紀錄器的應用中並不講求即時導航,而是講求更精準的運動

(12)

2 軌跡、速度與姿態估測以還原事發真相。為達此目標,本研究將致力於離線估測 雜訊的方法,期望藉由較精準的雜訊估測,達到更精準的運動軌跡紀錄。

1.3

文獻回顧

大多數論文使用卡爾曼濾波器時,利用事前的統計,將雜訊的共變異矩陣 假設為已知的常數項。在雜訊標準差不太會隨著環境與時間改變的應用下,這個 假設是合理的,系統狀態的估測結果也相當不錯。然而在雜訊標準差容易變化的 應用中,卡爾曼濾波器必須要有能適應性調變的能力,才能針對其改變做出對系 統狀態更佳的修正。本節整理了三種目前常見的基於創新序列(innovation sequence)的適應性調變卡爾曼濾波器演算法以及其它文獻適應性估測雜訊的方 法。創新序列為不同取樣時間下,實際量測值減去系統預測之量測值的差量, ˆ z z   ,所累積的一段序列資訊。 第一種為基於調變過程雜訊比例的適應性估測法[7],加入了一個參數S ,k 利用當下與統計區間內的創新序列值做比較,可適應性調變卡爾曼濾波器中過程 雜訊的共變異矩陣Q 的權重,而影響卡爾曼增益(Kalman Gain)於量測項與系統k 預測項間的權衡。若Sk 1,則表示系統處於較不穩定的狀態,因此放大過程雜 訊,使量測項有更多的權重。Sk 1為一般的卡爾曼濾波器。Sk 1表示系統處 於較穩定的狀態,使系統端有更多的權重,[17]也利用類似概念推導適應性調變 的強健濾波器。

第二種為基於多個模型的適應性估測法(Multiple Model Adaptive Estimation, MMAE)[10],此方法早在 1965 年即被提出,利用了多個不同模型的卡爾曼濾波 器同時做平行運算,以不同模型下高斯分布之創新序列的機率密度函數

(13)

3 型算出的結果做加權。此方法的優點為在多個模型之中,若其中有一個模型描述 較為精準的情形下,該模型的相信機率值會快速的收歛到 1,其它模型的相信機 率則收歛至 0,但缺點為若在多個模型之中沒有一個模型能精準描述,則此方法 的收歛速度相當的慢。由於此方法運算量相當可觀,且必須對模型有一定程度的 了解,因此實用性並不高。在目前介紹的兩個方法中,雖然具有適應性調變的能 力,但都未對雜訊共變異矩陣QkRk直接估測。 第三種為基於創新序列的適應性雜訊估測法,Mehra[1][2]利用創新序列與 其共變異矩陣理論值的一致性,藉由算出創新序列的期望值,能直接對QkRk 做適應性估測。此方法的基本原理也被延伸應用在許多估測雜訊的論文中,如 Mohemad[3]以最大似然原則推導創新序列期望值,[9]並應用於 GPS/INS 的整合 系統中,Odelson[4]、Akesson[5]利用自我共變數矩陣之最小平方法估測量測雜 訊,Adbel-Hafez[6]將其應用於 GPS/INS 整合系統中,[15]利用可調變視窗大小 的移動平均法估測變化的量測雜訊。本論文所使用的估測方法也屬於第三種方法 的範疇,利用可適應性調變維度的 S-G 濾波器算出創新序列的期望值,再藉由 與其共變異矩陣理論值的一致性,估測量測雜訊。 其它非利用創新序列的估測方法如[18]假設衛星雜訊為色雜訊(Color Noise), 並以成形濾波器估測之。[16][27]利用衛星與接收器之間,幾何與誤差的關係參 數,精度因子(Dilution of Precision,DOP),利用精度因子含有時變資訊的特性, 對量測雜訊進行估測。

(14)

4

1.4

論文貢獻

1. 提出一套 GNSS/INS 整合系統中離線適應性估測卡爾曼濾波器量測雜訊的新 方法。 2. 本研究的方法效果優於以往線上估測方法,能估測雜訊標準差隨環境與時間 變化下的量測雜訊。

1.5

論文架構

本論文架構包含了三個部分,分別為探討 GNSS/INS 系統的基本背景知識、 估測量測雜訊演算法的介紹,與軟硬體模擬與實作的驗證。 第一部份: 第二章:座標系統 第三章:全球導航衛星系統與慣性導航系統之整合 第二部份: 第四章:基於適應性調變維度之 Savitzky-Golay 濾波器的雜訊估測。 第三部份: 第五章:軟硬體設計與實驗結果 第六章:結論

(15)

5

第二章 座標系統

在做軌跡紀錄或是導航應用時,根據所設計的系統與所使用感測器不同, 會用到各種不同的座標系統。本章將介紹本研究所使用到的座標系的定義,以及 座標系與座標系間轉換的方法與旋轉矩陣的動態方程式,對這些座標系從基本去 認識。在本研究中使用到的座標系包含: 1. 慣性座標系 2. 地心地固座標系 3. 地理座標系 4. 導航座標系 5. 附體座標系

Equation Chapter 2 Section 1

2.1

座標系統定義

在做 GNSS/INS 系統整合之前,必須先把各座標系的意義弄清楚。本節會 介紹所用到的五個座標系的定義,並把座標系通用的名詞整理列出。

2.1.1 慣性座標系

慣性座標系(Inertial Frame)是在物體運動行為符合牛頓第一運動定律下所定 義的座標系統,該座標系統沒有明確定義出原點的位置,且可以自由定義三個彼 此互相垂直的座標軸。一般定義座標原點於地球質心處,Z 軸指向地理北極,即 地球自轉的轉軸,另外 X、Y 軸隨意定義,如圖 2.1.1。

(16)

6

圖 2.1.1 慣性、地心地固、導航座標系

2.1.2 地心地固座標系

地心地固座標系(Earth-centered Earth-fixed Frame)也常被叫做地球座標系 (Earth Frame),定義座標原點於地球質心,Z 軸指向地理北極,X 軸指向赤道與 本初經線的交點。Y 軸再以 X 與 Z 軸由右手定則得出。也因此地心地固座標系 會隨著地球自轉而同步旋轉,如圖 2.1.1。

2.1.3 地理座標系

地理座標系(Geodetic Frame)因為沒有定義出原點與三個垂直的軸,說起來 並不算是一個卡式座標系(Cartesian Coordinate System),用的是經度(Longitude)、 緯度(Latitude)、離地表高度(Height or Altitude)來表示物體在地球表面上的位置, 也因此也有人稱其為 LLH 或 LLA frame。在衛星定位系統中所取得的位置資訊, 即是用地理座標系來表示。

(17)

7

2.1.4 導航座標系

導航座標系(Navigation Frame)常用在地球表面做區域性的小範圍移動,必需 先在地球表面選取一個參考原點,也有人稱做本地地理座標系(Local Geodetic Frame)並以該參考原點對地球做一切平面,因此也有人稱做弦切座標系(Tangent Frame)。導航座標系定義其 N 軸指向地理北極,E 軸指向地理東方,D 軸為切平 面的法向量,指向地心,平行於重力方向,因此也稱做 NED 或 ENU 座標系, 如圖 2.1.1。

2.1.5 附體座標系

附體座標系(Body Frame)定義在移動的載體上,如汽車、飛機。其原點定義 在載體質心,X 軸定義為載體運動的前進方向,Y 軸定義於載體的側方向,Z 軸 以 X 與 Y 軸由右手定則得出。在應用中,加速規與陀螺儀皆是在附體座標系下 表示。

2.2

座標系旋轉

關於座標系的旋轉的方法,一般來說有三種常見的表示方法 1.方向餘弦法 2.四元數法 3.尤拉角法。本論文使用尤拉角法,將介紹方向餘弦與四元數法後, 對尤拉角法特別討論。

2.2.1 方向餘弦法

方向餘弦法(Direct Cosine Method,DCM)利用幾何特性,能得知其單位向量 的分量與座標系基底向量夾角為一餘弦關係。轉換關係如式(2.1)所示。

(18)

8

, , ,

, , ,

, , ,

cos( ) cos( ) cos( )

cos( ) cos( ) cos( )

cos( ) cos( ) cos( )

a b a b a b a b a b a b a b a b a b b b a a i i j i k i b a i j j j k j i k j k k k                        p R p R (2.1) , a b i j  表示 a 座標系中的基底向量 i 與 b 座標系中的基底向量 j 的夾角

2.2.2 四元數法

四元數法(Quaternion)利用複數建立的四維空間運作[29],利用四維空間描述 旋轉過程,基本代數原則如式 2.2。 2 2 2 1 , q w x y z etc                  i j k i j k ijk ij i j k j i ji (2.2) 在對向量做座標旋轉時,可以想像成將一目標向量沿著待轉旋轉軸旋轉某角度的 關係,角度與三維轉軸向量的四維空間可用四元數法描述如式 2.3。θ為待轉角 度,a為待轉轉軸向量。 (cos( / 2),sin( / 2) ) ( , , , ) q   aw x y z (2.3) 四元數法把目標向量從三維向量轉換至四維向量 v,並與四元數描述的待轉旋轉 行為 q 做運算,得到旋轉後的四維向量 w,如式(2.4) * wqvq (2.4) 運算後的向量再轉換回三維空間,如圖 2.2.1。式 2.4 能以旋轉矩陣表示如式(2.5), 其中每個元素可以用四元二次項函數來表示,因此不會有旋轉矩陣遇到奇異值的 問題。 2 2 2 2 2 2 1 2 2 2 2 2 2 0 2 2 1 2 2 2 2 0 2 2 2 2 1 2 2 0 0 0 0 1 q y z xy wz xz wy xy wz x z yz wx R xz wy yz wx x y                    (2.5)

(19)

9

圖 2.2.1 向量與四元數法轉換

2.2.3 尤拉角法

根據尤拉角(Euler Angle Rotation)轉換定理[29],任意兩個獨立且正規化正交 的座標系,可以對座標軸用三次以下的旋轉描述其轉換關係,並且不限定一個軸 只能旋轉一次(但同一個軸不能連續旋轉二次以上,因為這相當於只旋轉一次)。 排列組合的結果一共有 12 種旋轉方式,如表 2.2.1。本研究使用 Z->Y->X 的旋 轉序列。 表 2.2.1 尤拉座標系旋轉序列 三軸 各轉 一次

XYZ YZX ZXY

XZY YXZ ZYX

某軸 重複 旋轉 XYX YZY ZXZ XZX YXY ZYZ 依尤拉角定理將 a 座標系轉至 b 轉標系拆成三次旋轉。首先對 Z 軸旋轉 α 度 ' cos( ) sin( ) 0 ' sin( ) cos( ) 0 ' 0 0 1 a a a x x y y z z

                               再來對Y’軸(不是 Y 軸)旋轉 β 度 '' cos( ) 0 sin( ) ' '' 0 1 0 ' '' sin( ) 0 cos( ) ' x x y y z z

                              

(20)

10 最後再對 X”軸(不是 X 軸)旋轉 γ 度後,向量轉換至 b 座標系 ''' 1 0 0 '' ''' 0 cos( ) sin( ) '' ''' 0 sin( ) cos( ) '' b b b x x x y y y z z z

                                         由以上三式推導可以得式(2.6) b a b b a a b a

x

x

y

R

y

z

z

 

 

 

 

 

 

 

 

 

 

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) b a c c c s s R s s c c s s s s c c s c c s c s s c s s s c c c                                            (2.6) 事實上,當在描述空間中載體的姿態時,會以翻滾角(roll-ϕ)、俯視角(pitch-θ)、 航向角(yaw 或 heading-ψ)稱呼之,分別對應到 γ,β,α 角,如圖 2.2.2。 圖 2.2.2 姿態角名稱介紹

2.3

座標系間的轉換關係

在做感測器整合時,必須要把所有感測器的座標系轉到同一個座標系上, 一般來說都是將感測器的資訊轉到 ECEF 或是 NED 座標系上。在本研究做軌跡

(21)

11

紀錄的應用上,由於是在地表做小範圍的運動,本研究傾向於把所有座標系上的 資訊都轉到導航座標系 NED 上運算。

2.3.1 地理與地心地固座標系轉換

在把 GNSS 訊號從地理座標系轉換到 NED 座標系時,可以分成兩個步驟, 先將 GNSS 訊號由地理座標系轉換到 ECEF 座標系,再由 ECEF 座標系轉至 NED 座標系。地理座標系與 ECEF 的轉換公式如式(2.7)。 2 ( ) cos cos ( ) cos sin [ (1 ) ]sin e e e e e e x N h y N h z N e h                           (2.7) 2 2 2 2 2 3/ 2 2 2 6, 378,137.0 m 1 / 298.257223563 (1 ) 63567520 m = 0.08181919 (1 ) (1 sin ) 1 sin a b a a b a a e a e R f R R f R R e R R e M e R N e               : : : : : : : : a b e e h e R R f M N

緯度 經度 高度 地球離心率 地球半長軸長 地球半短軸長 地球扁率 本初經線曲率半徑 :卯酉圈曲率半徑

(22)

12

2.3.2 地心地固與導航座標系轉換

ECEF 座標系在做座標旋轉至 NED 座標系之前,必需在 ECEF 上先選取一 個基準點做為 NED 座標系中的參考原點,轉換公式如式(2.8)所示。由於 NED 座 標系的使用條件限制是物體運動必須遵守小範圍移動為前提,因此此座標轉換的 旋轉矩陣在本研究的應用中,可以視為常數。 0 n p r e p r d p r p s c s s c x x p c c y y p c c c s s z z                                                 (2.8) : longitude : latitude ( ) : cosine operator ( ) : sine operator c s    

2.3.3 導航與附體座標系轉換

在利用慣性儀做軌跡紀錄時,這一個轉換關係十分的重要。因為在加速規 中量到的物理量是加速度,其中包含了物體運動的線性加速度與重力的投影分量, 如圖 2.3.1,若這個轉換關係不夠精確,重力分量所造成的偏壓值變成了線性加 速度,使在做線性加速度積分與二次積分至速度與位置時的發散。而此座標轉換 除了將載體上慣性儀速度與位置資訊轉換到 NED 座標系外,還包含了載體的姿 態角資訊,也就是說所有軌跡紀錄的必要資訊都與這個轉換相關。根據尤拉角(2.3) 式,可以得知附體-導航座標系的轉換矩陣Rbn,算出在附體座標系中的加速規所 量到的重力分量,式(2.9)。

(23)

13 0 0 b b n n c c c s s g s s s c c s s s s c c s c g s c c s c s s c s s s c c c g g c c                                                                            g R g (2.9) : roll : pitch : yaw    值得注意的是,在利用重力推算姿態時,附體座標系的重力分量中只能從 加速規反推得到翻滾角與俯視角的解,在做軌跡紀錄時最重要的航向角資訊無法 直接從重力分量求解,因此需要靠陀螺儀與磁力計做感測器結合推算,這也是用 慣性儀做姿態偵測的最大挑戰之一。 圖 2.3.1 導航與附體座標系投影關係

(24)

14

2.4

旋轉矩陣的動態方程式

當載體在做旋轉運動時,其相對於導航座標系的關係也跟著在改變,這與 載體姿態的推算相當有關,因此需建立一個動態方程式去描述這個旋轉關係。先 觀察尤拉角公式(2.6),將 a 到 b 座標系的旋轉(a、b 可以代表任意座標系)矩陣從 時刻 t 轉至到 t+∆t 做小角度旋轉的假設,對三角函數留下泰勒展開式的一次項做 近似(sin(x)≈x,cos(x)≈1),且假設旋轉矩陣從 t 轉至得到 t+∆t 的角速度為常數後, 得到式(2.10)。 1 1 ( ) , ( ) 1 a a smallangle ba ba R skew t skew                                                      I I (2.10) 根據微分基本定義,代入式(2.7),可推導得到旋轉矩陣的動態方程式,式(2.11)。 0 0 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) lim lim ( ) b b a a smallangle b b b a b b a a a ba a a t t b a a ba R t t R t R R t t R t R t I t R t R t t t R t                       (2.11)

(25)

15

第三章 全球導航衛星系統與慣性導航系統之整合

全球導航衛星系統(Global Navigation Satellite System,GNSS)與慣性導航系 統(Inertial Navigation System,INS)兩者的特性正好相反。GNSS 的優點為長時間 精準度不受影響,但缺點是取樣頻率太慢且易受環境影響,短時間的精準度不夠。 INS 的特性正好相反,優點是取樣頻率快,短時間內精準度夠,但長時間則會有 偏壓與準位漂移的缺點。兩者優缺點正好互補,適合做感測器結合,其應用於導 航上,至今已有十餘年的歷史,但由於當時慣性導航系統造價昂貴,金額動輒數 百萬台幣,因此並未被廣泛地應用。但在今日微機電系統 (Micro Electro

Mechanical Systems,MEMS)製程技術發展越來越純熟,低價位的 GNSS/INS 整 合應用在近幾年又熱絡了起來。

本章將會簡單介紹全球導航衛星系統後,推導慣性導航方程式,並介紹一般 慣性導航系統中,最常見的誤差模型(Error Model)架構[33][34]。最後,介紹慣性 導航系統如何透過卡爾曼濾波器(Kalman filter)與全球導航衛星系統做感測器結 合,實現本研究所使用的 GNSS/INS 整合系統的完整架構。

Equation Chapter (Next) Section 1

3.1

全球導航衛星系統

最早衛星系統是用在軍事用途上,但到後來衛星系統所帶來的效益已遠遠 經超當初設計的目的,現今被廣泛應用於船隻、車輛與無人飛機的定位上。在本 論文中即使用衛星系統做輔助,提升軌跡紀錄的精準度。

(26)

16

3.1.1 全球導航衛星系統介紹

在全球導航衛星系統(GNSS)中,最有名也最普及的為美國的 GPS 衛星系統。 其它還有近年來修復完善的俄羅斯的 Glonass 衛星系統、歐洲地區的伽利略 (Galileo)衛星系統,以及最近發展快速的中國北斗(Beidou)衛星系統,如表 3.1.1 (來源:維基百科)。而近幾年來,由於政治等因素,促成同時使用多個衛星系統 的發展,其中就屬 GPS/Glonass 的雙衛星系統最為普及,甚至在 2012 年 6 月以 後出產的智慧型手機及平板,都內建 GPS/Glonass 雙衛星系統接收器。使用者可 以直接從接收器模組的輸出格式中,得到以 1Hz(視接收器而定,通常即為 1Hz) 更新定位點的經、緯、高度資訊。 表 3.1.1 全球目前主要的衛星導航系統

System GPS GLONASS COMPASS Galileo Political

entity

United States Russian Federation China European Union Coding CDMA FDMA/CDMA CDMA CDMA

Orbital

height 20,180km(12,540mi) 19,130km(11,890 mi) 21,150km(13,140 mi) 23,220km(14,430mi) Period 11.97hours(11 h58 m) 11.26hours(11 h16 m) 12.63hours(12 h38 m) 14.08hours(14 h5 m) Evolution Per sidereal day 2 17/8 36/19 17/10 Number of satellites At least 24 31, including,24 operational,1 in preparation,2 on maintenance,3 reserve,1 on tests 5geostationary orbit(GEO) satellites, 30medium Earth orbit(MEO) satellites

4test bed satellites in orbit,22operational satellites budgeted Frequency 1.57542GHz(L1signal) 1.2276 GHz(L2 signal) Around1.602GHz(SP) Around1.246GHz(SP) 1.561098GHz (B1) 1.589742GHz (B1-2) 1.20714GHz (B2) 1.26852GHz (B3) 1.164–1.215GHz (E5a,E5b) 1.260–1.300GHz (E6) 1.559–1.592GHz (E2-L1-E11) Status Status Operational,CDMA in

preparation

Operational CDMA in preparation

(27)

17 雖然實作上直接讀取位置資訊就可以做定位,但為了對衛星定位系統的特 性有進一步的了解,仍必須探討其特性,從而了解衛星定位系統的優點與缺點。 衛星定位系統的原理其實相當簡單[35],即利用空間中的三角定位法算出一個交 叉點(見圖 3.1.1 中 A 點)。理論上在三維空間中,能用三個方程式解三個未知數, 因此至少需要 3 個衛星才能做三角定位。但事實上,地面接收器與各衛星間存在 著時間誤差,該時間誤差也造成了定位上的不精確,使得定位的結果存在著誤差 範圍(見圖 3.1.1 中 B 所圍成的區域)。 圖 3.1.1 衛星系統三角定位法 這也使得除了原本三維空間的未知數外,還多了一項時間誤差共 4 個未知數 ( , , ,X Y Zt),因此至少需要 4 顆以上的衛星才能做準確定位,衛星定位的基本 定位原理推導如下。首先,定義衛星訊號發送至接收器接收的時間差

t

measured式(3.1),與衛星至接收器間的偽距離 PSR (Pseudo Range)為衛星與接收器間真實 距離加上光速移動了誤差時間的錯誤距離,如式(3.2)。 0 measured t t t      (3.1) 0 0 ( ) measured PSR t          c t t c R t c (3.2)

(28)

18 從這也可以看出當接收器有 1 微秒的時間誤差,就會造成 300 公尺的錯誤距離。 各個衛星至接收器的偽距離可表示如式(3.3),小標 sat_i 標示為第 i 顆位星,user 表示使用者接收器。 2 2 2 _ _ _ 0 ( ) ( ) ( )

i sat i user sat i user sat i user

PSRXXYYZZ   t c (3.3) 現在依式(3.4)定義接收器的空間位置。小標 est 表示接收器位置附近隨意選取的 一個已知座標點。項表示位置誤差向量。 user est user est user est X X x Y Y y Z Z z                                 (3.4) 將(3.3)式做一階尤拉的線性化後,得到式(3.5) _ _ _ _ 0 _ _ _ _ 0 _ _ _

est i est i est i

i est i

est sat i est sat i est sat i

est i

est i est i est i

R R R PSR R x y z c t x y z X X Y Y Z Z R x y z c t R R R                         (3.5) 最後將 4 個衛星的偽距離寫成 4 條方程式並寫成矩陣後,得到式(3.6) _1 _1 _1 _1 _1 _1 _ 2 _ 2 _ 2 _ 2 _ 2 _ 2 _ 3 _ 3 _ 3 0 _ 3 _ 3 _ 3 _ 4 _ 4

est sat est sat est sat

est est est

est sat est sat est sat

est est est

est sat est sat est sat

est est est

est sat es est X X Y Y Z Z c R R R X X Y Y Z Z x c R R R y z X X Y Y Z Z c t R R R X X Y R                             1 1 _1 2 _ 2 3 _ 3 4 _ 4 _ 4 _ 4 _ 4 _ 4 est est est est

t sat est sat

est est PSR R PSR R PSR R PSR R Y Z Z c R R                                       (3.6) 由於(3.3)式為一非線性的二次函數,有一組極值解,因此可以將算出的誤差項重 新帶回(3.4)式,更新已知座標點的位置,用牛頓法的概念重複做遞回式運算至誤 差項收歛至小於可接受的誤差值為止。此外,若有 n>4 顆位星則可以利用最小平 方法算出更正確的誤差,這也是為什麼當接收器觀測到的衛星數量越多,定位準 確度會越高的原因。

(29)

19

3.2

慣性導航系統

在利用加速規與陀螺儀做運動軌跡的估測應用上,必須了解物體做線性運 動或是旋轉運動的物理原理以及其動態變化的關係以推導運動方程式。由於在 ECEF 座標系上能以較精簡且明確的說明各參數的物理意義,將會在 ECEF 座標 系推導方程式。但實際在本研究的應用中是於 NED 座標系下操作。

3.2.1 慣性導航方程式

首先描述慣性座標系與 ECEF 座標系中的位置向量(3.7),小標 i 與 e 分別表 示慣性與 ECEF 座標系,l 為兩座標系原點間的向量。 i i e i er   r R l (3.7) (3.7)式對時間做二次微分後,可以得出(3.8) 2 i i e i e i e i e e e     r R r R r R r l (3.8) 而根據 2.4 節所導出的動態方程式(2.8)可以得到(3.9) i i e eeie R R Ω (3.9) 對(3.9)式再做一次微分,得到Rie的二次微分項(3.10) ( ) i i e i e i e e e eeieeiee ieieie R R Ω R Ω R Ω Ω Ω (3.10) 整理(3.9)(3.10)代入(3.8)式後,可推導出(3.11)式 2 ( ) i i e i e e i e e e e i e e ie e ie ie ie       r R r R Ω r R Ω Ω Ω r l (3.11) 又由古典力學得知,在慣性座標系中的合力可以拆成重力與慣性力加總的結果, 而力除上物體質量即為物體的加速度,即(3.12)。g 為重力加速度向量,s 為慣性 力除上質量的加速度向量,事實上他有個物理名稱叫做比力(specific force),值得 注意的是他的物理量不是力,而是加速度。順帶一提,加速規量到的加速度就是

(30)

20 附體座標系下的比力,可以想像當物體做自由落體運動時,運動加速度即為重力, 這時加速規所測得比力值為零。 i  i i r g s (3.12) 結合(3.11)(3.12)式,方程式左右各乘上旋轉矩陣Rei後,可以推導出 ECEF 座標 系中慣性導航方程式的一般式(3.13) 2 e e e e e e e e e e e ie ie ie ie       g s r Ω r Ω Ω r Ω r l (3.13) 其中Ωeie為地球自轉向量的反對稱矩陣,且由於 ECEF 座標系是繞著與慣性座標 系相同的 Z 軸旋轉,轉速為

ie 7.292 1 e5(rad s/ ),因此可以表示成相當精 簡的形式。(3.13)式之中,不考慮右邊第三項地球自轉造成的向心力,與右邊第 四項地球自轉的角加速度造成的力(假設地球等速自轉),又最末項

l

e

0

。將 (3.13)式簡化後成(3.14),此式可以解釋成物體的線性加速度為比力補償掉重力加 速度,再扣掉科氏力造成的加速度後的結果。 2 e e e e e ie    r g s Ω r (3.14) 本研究的目標是做軌跡紀錄,記錄內容包含位置、速度與姿態的資訊,因此需要 想辦法整合這些資訊,用系統方式描述動態方程式如(3.15) 2 e e e e b e e e b ie e e b b b eb                     r v v R s g Ω v R R Ω (3.15) 其中Ωbebb eb ω 的反對稱矩陣,ωbeb可表示為 b b b e ebibRe ie ω ω ω 。 在本研究中,加速規可量得

s

b,陀螺儀可量得ωbib。至此已能初步的利用加速規 與陀螺儀做慣性導航。(3.15)式在 NED 推導,可改寫為(3.16),[32]。

(31)

21 (2 ) n n n n b n n n n b ie en n n b n n b b ib in b                     r v v R s g Ω Ω v R R Ω Ω R (3.16) [ cos 0 sin ] tan [ ] n n e T ie e ie ie ie n e n e T en e e e v v v N h M h N h               ω R ω ω

3.3

慣性導航誤差模型

在 3.1 節中得到慣性導航的方程式,理論上該方程式已可用來做導航。但實 際應用上,由於工程技術上並沒有完美的加速規與陀螺儀,必定存在著雜訊與偏 壓值,這些缺點經過積分後,對位置、速度與姿態的推算結果會造成相當大的誤 差,但若是能對其之間的關係用數學模型來描述,找出誤差結果的雜訊來源做修 正,則可以適度的改善其造成的誤差。

3.3.1 慣性導航誤差方程式

此小節的目標是找出位置、速度、與旋轉的誤差應該如何用數學描述,並 推導出誤差模型的動態方程式。要推導出動態方程式,必須先討論速度項導函數 (加速度)的誤差來源。首先定義誤差為演算法中的理論值減去實際運算值,並將 旋轉矩陣Reb的旋轉誤差如(2.7)方式描述成小角度的旋轉,其中

Ω

為旋轉誤差 向量的反對稱矩陣。 ˆ ˆ ˆ ˆ ˆ ˆ ( ) b b b e e e e e e e e e b b b ie ie ie e e bb

            s s s v v v g g g v v v ω ω ω R I Ω R

(32)

22 將上列參數代回(3.15)式,可以重新寫成(3.17) ( ) ( ) 2 ( ) e e e b b e e e e e b ie              v v I Ω R s s g g Ω v v (3.17) 若假設(3.17)中誤差的二次項以及重力誤差可以忽略,並利用反對稱矩陣的特性, 整理後可以得出速度誤差的動態方程式,如(3.18) 2 e e e e e b e e b ie          Ω s S ε v S ε R s Ω v (3.18) 我們對(3.18)等號右邊造成加速度誤差的原因做討論。右邊第一項為由於旋轉矩 陣的誤差,使比力分量不正確而造成線性加速度值的誤差,右邊第二項為加速規 本身的誤差造成,包含了偏壓與雜訊。末項則是科氏力的誤差造成。 接著探討旋轉項導函數(角速度)的誤差來源。首先將Reb改寫為實際運算值 對時間微分後的式子(3.19a) (3.19b),整理後得到方程式(3.19c) ˆ ˆ ˆ ˆ ( ) ( ) ( ) ( ) e e b b b eb e e e b b b e e e b b b b b eb eb                R R Ω R I Ω R Ω R I Ω R Ω R I Ω R Ω Ω (3.19) 若不考慮(3.19c)中誤差的二次項並消去左右相同項後,得到(3.20) e e b b b eb     Ω R R Ω (3.20) 同乘上Rbe,並利用反對稱矩陣的特性改寫為向量的形式後,得到(3.21) ( ) e b e b b beb bieib     ε R ω R ω ω (3.21) 要得到ωbie必須要透過附體-ECEF 座標系的旋轉矩陣,但由於假設這個旋轉存在 著誤差,也就是式 3.22 ˆ ˆb b e iee ie ω R ω (3.22) 利用 ˆReb  (I Ω R) eb做反矩陣運算,並以泰勒展開做一階近似得到(3.23) 1 ˆb b( ) b( ) e ee       R R I Ω R I Ω (3.23)

(33)

23 再將(3.22)代入(3.23)式中,得到(3.24) ( ) b b b e ie ie e ie b b e b ie e ie ie           ω ω R I Ω ω ω R Ω ω Ω ε (3.24) 最後由(3.21)與(3.24)得到最終旋轉誤差的動態方程式(3.25) e b e bib ie    ε R ω Ω ε (3.25) 我們同樣對造成角速度誤差的來源做討論。右邊第一項為陀螺儀的誤差造成,包 含了準位漂移與雜訊。右邊第二項則由於旋轉矩陣的誤差,使地球自轉分量有誤 造成。 觀察加速度與旋轉的誤差來源中,由於地球自轉的轉速極慢,造成的影響 不大,可以看出真正影響速度與旋轉誤差的,為加速規與陀螺儀的誤差造成。 最後整理(3.18)(3.25)式,用系統的方式描述位置、速度、旋轉,得到 ECEF 座標系下,9 個狀態的誤差模型(3.26)。 2 e e e e e e b e e b ie e e b e e b ib ie                           r v v S ε R s Ω v ε R ω Ω ε (3.26) 上式若於 NED 下推導,可改寫為(3.27)式,[32]。 (2 ) n n n n en n n n n b n n e b ie en n n b n n b ib in                             r v Ω r v S ε R s Ω Ω v ε R ω Ω ε (3.27)

3.4

以卡爾曼濾波器實現鬆散耦合的 GNSS/INS 系統

有了 3.2 與 3.3 對慣性系統的認識後,了解到在實際應用上單純用低價的慣 性系統做軌跡紀錄是相當困難的一件事。因此在 1990 年代就有許多文獻提出利 用全球導航衛星系統的輔助,透過卡爾曼濾波器將這兩種優缺點彼此互補的導航 系統做感測器結合。

(34)

24

3.4.1 卡爾曼濾波器

卡爾曼濾波器(Kalman filter)為匈牙利數學家 Rudolf E. Kálmán 於 1960 年所 提出,由於其實作上的效能相當驚人,迄今仍廣泛應用於各領域,如車輛控制、 影像與聲音的處理與太空梭的控制等。只要能用矩陣描述系統狀態的轉換關係及 感測器量測值與系統狀態的關係,就能利用遞迴式運算算出系統的預測狀態值, 並透過感測器測量值來對預測狀態值做修正,估測出最佳化的系統狀態。其系統 模型可以描述如下。 1 ~ (0, ) ~ (0, ) k k k k k k k k k k k N N       x Ax Bu w z Hx v w Q v R

其中 A 為系統狀態轉移矩陣(State Transition Matrix),B 為控制項,本研究因不對 系統做控制,此項可以省略, H 為觀測矩陣(Observation Matrix)。

k

w 與vk分別為系統的過程雜訊(Process Noise)與量測雜訊(Measurement Noise),

w 與k vk與平均值為零的白高斯雜訊(Zero Mean White Gaussian Noise),此時得 出的狀態是最佳化的估測結果,演算法的流程如圖 3.4.1 所示。

(35)

25 圖 3.4.1 卡爾曼濾波器演算法流程圖

3.4.2 鬆散耦合的 GNSS/INS 系統

在 3.27 式中推導了 NED 座標系下,9 個狀態的誤差模型,並說明了加速規 與陀螺儀的誤差如何造成影響。在卡爾曼濾波器中,假設雜訊項為平均值為零的 白噪聲雜訊,為了滿足這項假設,最常見的方法為對加速規與陀螺儀的誤差建立 一階的高斯馬可夫(1st order Gauss-Markov)數學模型,對其誤差做白化(Whitening), 把加速規與陀螺儀的漂移值放入模型,從 9 個狀態誤差模型擴增為 15 個,系統 的動態方程式描述如下: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) t t t t t t t t t

    x F x G w z H x v 其中系統狀態x( )t 與過程雜訊 ( )w t 分別為 _ _ ( ) ( ) acc T e e b b gyro T acc gyro a bias g bias

t r v b b t w w w w            x w 系統狀態矩陣 ( )F t 與動態雜訊矩陣 ( )G t 分別為

(36)

26 15 15 15 12 0 0 0 0 -(2 ) 0 0 0 - 0 ( ) 1 0 0 0 - 0 1 0 0 0 0 -0 0 0 0 0 0 0 ( ) 0 0 0 0 0 0 0 0 0 n I en n n n n ie en b n n in b t a g n b t n b                                                        Ω Ω Ω S R Ω R F R G R I I 為了要在卡爾曼濾波器下實現演算法,必須對動態方程式以一階的線性尤拉法做 離散化(Discretization)[32],其中

Q

W

w

( )

t

的共變異矩陣,

T

s為系統的取樣頻 率。 k d,k k k 1 2 s T T T W W s T T            A I F Q A G Q G G Q G A 離散化後的系統可以用卡爾曼濾波器表示如下 1 k k k k k k k k

     x A x w z H x v 現在加入 GNSS 系統的輔助,實現 GNSS/INS 的整合系統架構。首先定義量測向 量

z

k為 INS 用慣性導航方程式得出的三維位置向量,減去 GNSS 測得的三維位 置向量。下標 true 代表無誤差的理想值, v 代表 GNSS 系統的雜訊。 k , ,

=(

) (

)

ins gnss

true ins true r gnss

ins r gnss

z

r

r

r

r

r

v

r

v

(37)

27

與卡爾曼濾波器的量測方程式做系數比較,可以發現觀測矩陣

H

k 相當精簡,且

非時變(time variant)的矩陣,量測雜訊也只跟 GNSS 項的位置雜訊有關,並沒有 任何 INS 的參數,與 INS 資訊完全獨立,這也是這個架構廣泛應用於 GNSS/INS 整合的最大原因。

0

0

0

0

3 15

k

H

I

而事實上,以誤差模型為基礎的此系統,在另一層意義上為擴增式卡爾曼濾 波器(Extented Kalman Filter,EKF)系統,若把導航方程式描述為如(3.28)的非線

性函數,

z

k為位置、速度、姿態,

a

k 為量測得到的加速度與角速度值。 1 ˆ , ˆk ( ,ˆk k) z k z f z aw (3.28) , , ˆ [ ] ˆ e e T k k k k T k acc k gyro k z r v a s

 

將參數假設如下

ˆ

ˆ

k k k k k k

z

z

z

a

a

a

並對(3.28)函數微分做線性展開後得到(3.29) 1 1 ( , ) 1, 2, , k k k k k k k k z k z z f z aFzFaw (3.29) 1, 2, ( , ) ( , ) , k k f z a f z a F F z a       將

z

k1

f z a

( ,

k k

)

代入後,可以得到與誤差模型同樣的結果如式(3.30),其中F1,kF2,k分別為狀態轉移矩陣A 的上半部與下半部。 k 1 1, 2, , k k k k k z k z F z F a w      (3.30)

(38)

28 也就是說若在導航方程式做 EKF 整合相當於用誤差模型做 CKF 整合。多數 GNSS/INS 的文獻中,宣稱以 EKF 或 CKF 的方法整合,貌似不同方法但實質上 是在說明同一件事。 最後,若利用 GNSS 得到的位置、速度值當做量測訊號,並把卡爾曼濾波 器估測到的 INS 資訊反饋(Feedback)回加速規、陀螺儀與慣性系統方程式做即時 修正,則稱為鬆散耦合(Loosely Coupled)的 GNSS/INS 系統。其架構圖為圖 3.4.2 所示。若 GNSS 量測訊號為偽距離及載波相位(Carrier Phase)未經 GPS 晶片處理 過的則稱緊密耦合(Tightly Coupled)系統

(39)

29

第四章 基於適應性調變維度之 SAVITZKY-GOLAY

濾波器的雜訊估測

要增進 GNSS/INS 的精準度,大致可以分為二個方向。一是對 INS 系統建 立更準確的數學模型,二是對過程雜訊與量測雜訊進行估測,本論文研究方向為 後者。在 GNSS/INS 整合系統架構中,過程雜訊僅與 INS 的加速規與陀螺儀有關。 INS 雜訊特性不易變化,可以藉由事前統計校正出過程雜訊,較沒有做適應性估 測的必要性,因此本研究致力於對量測雜訊估測的演算法開發。衛星定位系統中, 環境會影響衛星訊號的接收,使量測雜訊的標準差隨環境與時間變化,例如行駛 於市區受高樓遮蔽與多路徑傳輸的影響,造成雜訊標準差的變化快速,因此估測 量測雜訊相當重要。本論文提出一套離線估測量測雜訊的方法,使用可適應性調 變維度的 S-G 濾波器,改善以往線上估測方法中,利用固定維度且未經取樣延 遲之移動平均法,無法根據環境變化做適應性調變的缺點。本章將介紹適應性調 變維度的 S-G 濾波器,並將其應用在卡爾曼濾波器的量測雜訊估測上。

Equation Chapter (Next) Section 1

4.1

Savitzky-Golay 濾波器

Savitzyky-Golay(S-G)濾波器為 1964 年由 Abraham Savitzky 與 Marcel J.E.Golay 所提出的局部多項式回歸演算法,在給定視窗大小(2M+1)與多項式的 維度 N 後,以最小平方法對所選取的視窗取樣點用一個最佳的多項式去做曲線 擬合(curve fitting),式(4.1),一般用於平滑濾波與微分應用。 2 2 0 ( ( ) [ ]) ( [ ])

arg min

M M N k N k n M n M k N p n x n a n x n

      

 

a

(4.1) (4.1)式的最小平方解可以用矩陣的方式描述如(4.2),推導詳見附錄。

(40)

30 -1 0 [a aN]T T T    a (A A) A x Hx (4.2) 0 1 2 0 1 2 0 1 2 0 1 2 ( ) ( ) ( ) ( ) ( 1) ( 1) ( 1) ( 1) 1 0 0 0 1 1 1 1 N N N N M M M M M M M M                             

A

從上式可知,當 M 與 N 決定後,矩陣 H 為一個常數矩陣,因此可以用查表的方 式來簡單完成 H 的運算,不需要每次都做反矩陣運算。最後在移動的視窗中, 取視窗中心取樣點(n=0)代表該區間濾波後的結果,即為多項式的參數

a

0式(4.3), 示意圖如圖 4.1.1 所示。 圖 4.1.1 S-G 濾波器原理示意,M=2,N=2 ("‧"為輸入訊號,"x"為多項式取樣點,"o"為輸出, 實線表示擬合的多項式,虛線為系統脈衝響應) 0 0, [ 0] (0) [ ] M m m M y n p a h x m     

(4.3) (4.3)式可以解釋為訊號 x 與線性非時變系統做數位摺積(Discrete Convolution)的 結果,這也意味著能用訊號系統的方法分析其系統的脈衝響應(Impulse Response), 了解 S-G 濾波器的特性,相關特性整理如下[21][22]。

(41)

31  平滑濾波特性(𝑎 ) 為 Type-1 濾波器(低通濾波器),如圖 4.1.2,於維度 0/1,2/3,…有相同的平滑濾波 結果。 圖 4.1.2 S-G 濾波器的頻率響應  微分特性(𝑎 ) 為 Type-3 濾波器(帶通濾波器),與一般微分器為高通濾波器不同,具有壓抑高頻 的效果,且於維度1/2,3/4,…有相同的微分結果。根據平滑濾波與微分的維度特 性,在決定維度時會以奇數做挑選[25]。  3dB 點的截止頻率關係 fc=(N+1)/(3.2M-4.6) M>25 且 N<M,圖 4.1.3,N 越大(或 M 越小),頻寬也越 大,平滑濾波的結果與原訊號越像,也因此較不平滑,反之亦然。 圖 4.1.3 S-G 截止頻率與維度關係圖  為永久延遲系統,且移動平均法為多項式維度 N=0 的特例

(42)

32

4.2

適應性調變維度之 S-G 濾波器(ADSG)

若要做適應性調變的 S-G 濾波器(Adaptive-degree SG filter,ADSG)有兩個方 向。一是改變視窗大小做適應性調變,另一方向為改變維度做適應性調變,本論 文方法屬後者。根據曲線擬合後的殘差的平方和,利用統計學的 F 檢定得到多項 式的維度[23]。F 檢定常被用來比較低維度的簡化模型(Reduced Model)與原始模 型的必要性。簡化模型的殘差平方和一定會比原始模型的還要大,但若這兩個模 型的殘差平方和相差不多,則我們傾向於使用較低維度的簡化模型,而這個差異 程度的比較準則就是透過 F 檢定來判斷。

(

) / 1

/ 2

R

RSS

RSS

v

F

RSS v

RSS

為原始模型的殘差平方和,

RSS

R為簡化模型的殘差平方和,n1、n2 為模 型多項式的維度,

v

1

n

2

n

1

v2 L n2 1 。虛擬程式碼如下: k=1; while k<k_max F_alpha=finv(1-alpha,v1,v2); Fx=((RSSR-RSS)/v1)/(RSS/v2); k=k+2; if Fx<F_alpha break; end end order=k-2;

(43)

33 為了比較 ADSG 與固定維度的 SG 濾波器在訊號時變下的效能,以加入高 斯雜訊於不同變化頻率訊號模擬,如圖 4.2.3。 圖 4.2.1 模擬加入高斯雜訊的 S-G 估測訊號 濾波結果圖 4.2.2,可看出 ADSG 在會拉高維度適應高頻,低頻變化時則維持在 低維度,適應性調變維度能力也使得結果上能滿足低頻與高頻處的濾波表現。 圖 4.2.2 ADSG 法濾高斯雜訊結果與對應維度 0 20 40 60 80 100 120 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 adaptive degree 0 20 40 60 80 100 120 0 1 2 3 4 5 6 7 8 9 polynomial degree

(44)

34 反觀固定維度的 S-G 濾波器則沒有調適的功能,如圖 4.2.3。N=1 中,因為 頻帶最窄,高頻訊號被濾掉後無法滿足訊號濾波的表現,N=7 的結果則因有較多 的高頻成份,在原訊號低頻處有不平滑的波形,表現皆不如 ADSG 法。 圖 4.2.3 固定維度 S-G 濾濾高斯雜訊模擬結果 0 20 40 60 80 100 120 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 polynomial degree = 1 0 20 40 60 80 100 120 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 polynomial degree = 3 0 20 40 60 80 100 120 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 polynomial degree = 5 0 20 40 60 80 100 120 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 polynomial degree = 7

(45)

35

4.3

基於 ADSG 的雜訊估測法

在卡爾曼濾波器實現 GNSS/INS 中,GNSS 訊號的取樣頻率較 INS 慢,因此 演算法在有 GNSS 訊號時才能做系統狀態修正,如下所示。 其中實際量測值減去系統預測之量測值的差量, z zˆ,其累積的序列稱之為 卡爾曼創新序列(Innovation Sequence)。創新序列有個特性是,它每次取樣所帶來 的都是新的資訊,跟之前的資訊無關,為一白高斯訊號。而其共變異矩陣 k C 恰 為卡爾曼增益(Kalman Gain)的分母項,式(4.4),因此影響了系統狀態做修正時, 分配多少比重在相信系統預測與實際量測值之間,造成最佳化系統狀態的不同。 k C ˆ ˆ ( )( ) ˆ ˆ ( )( ) ˆ ˆ ˆ ( )( ) 2( ) T T T T T T T T E E z z z z E Hx v Hx Hx v Hx E H x x x x H Hx Hx v vv HP H R                                      (4.4) 在 Mohamed[3]的研究中,假設 k C 為常數不變,並利用最大似然原則 (Maximum Likelihood,ML)估測量測雜訊共變異矩陣 ˆRk,其結論如下。 k 1 1 ˆ C ˆ ˆ k k k T j j j k L T k v k k L R C H P H          

if (GNSS data available) x_est=x_pre+K(z-z_pre); P_est=(I-KH)P_pre; else x_est=x_pre; P_est=P_pre;

數據

圖  2.1.1 慣性、地心地固、導航座標系
圖  2.2.1 向量與四元數法轉換
圖  3.4.2 鬆散耦合之 GNSS/INS 架構
圖  5.1.1 資料包感測器安裝位置圖 圖  5.1.2 資料包實驗平台
+5

參考文獻

相關文件

§§§§ 應用於小測 應用於小測 應用於小測 應用於小測、 、 、統測 、 統測 統測、 統測 、 、考試 、 考試 考試

Harrington (1994) 認為倫理規範的目的在闡明責任,其研究透過責任的否 認 (Denial of Responsibility, RD) 這項人格特質與倫理規範的互動來進行測 量,並以資訊系統相關的軟體盜拷

事前事後比較((前測 前測 前測 前測 前測//後測 前測 前測 前測 後測 後測 後測 後測 後測 後測 後測))研究設計 研究設計 研究設計 研究設計 研究設計

• 測驗 (test),為評量形式的一種,是觀察或描述學 生特質的一種工具或系統化的方法。測驗一般指 的是紙筆測驗 (paper-and-pencil

將基本學力測驗的各科量尺分數加總的分數即為該考生在該次基測的總 分。國民中學學生基本學力測驗自民國九十年至九十五年止基測的總分為 300 分,國文科滿分為 60

要上傳 NCBI 註解序列必須要做的流程為基因預測、rRNA 預測、跟 tRNA 預 測。做基因預測後還要做基因比對才可以上傳 NCBI,如圖 34 所示。在 NCBI

二、 工作行為與活動:以工作過程、活動、行為來衡量績效,這

並且利用裂紋感測器兩端支腳張開與閉合時電壓訊號的改變,量測梁 的上下端所承受的彎矩應變。此外運用應變規與 LVDT