利用多天線GPS 技術進行載台姿態定位

國立臺灣大學工學院土木工程學系 碩士論文

Department of Civil Engineering College of Engineering

National Taiwan University Master Thesis

利用多天線 GPS 技術進行載台姿態定位 The Rigid Platform Attitude Determination Using

Multi-antenna Global Positioning System

劉柔妤 Liu, Jou-Yu

指導教授：韓仁毓 教授 Advisor: Han, Jen-Yu

中華民國 100 年 7 月

July 2011

誌謝

在研究所的兩年中，謝謝最重要的家人們的支持，而能夠順利的完成論文，

最感謝的就是我的指導教授韓仁毓老師，無論是在課業、研究甚至在做人處事的 方面，韓老師都給予我許多啟發與幫助，一路走來真的很感謝韓老師耐心又仔細 的指導，讓我在研究的路上少了許多跌跌撞撞，也收穫良多，韓老師給我許多的 空間與彈性，遇到問題的時候，老師總是會利用各種方式幫我解決或是指引我，

真的覺得很幸運也很開心有韓老師擔任我的指導教授！還要謝謝趙鍵哲老師、林 老生老師、葉大綱老師擔任我的口試委員，無論是在論文的呈現或是寫作上都給 予我許多寶貴的建議，讓我的論文可以更完整；謝謝徐百輝老師在課業上給了我 許多知識與幫助，還有在seminar 的時候總是給予我寶貴的意見，讓我發現我平常 自己沒有注意到的問題；謝謝CAE 組的康仕仲老師借我實驗用的機器人載台，讓 我可以把理論應用在實際移動的載台上，而使得我的研究更完整。

再來要感謝的是我的研究夥伴吳宇，無論是在修課上的問題或是研究上的問 題，謝謝吳宇盡心盡力的幫助我，有你當我的研究夥伴我真的很幸運；謝謝張祐 銓總是在我心情不好的時候聽我抱怨，還有常常在很熱的天氣頂著大太陽幫我完 成實驗；小樂學姐還有蘇煒哲學長感謝你們在課業上的幫忙，也很高興可以跟你 們當好朋友，讓我在碩一的日子充滿歡笑，好喜歡每個禮拜都要跟你們一起meeting 的時候喔！還有珍祥學姐，真的很謝謝你常常幫我看文章還有修改英文文法，以 後有需要幫忙請盡量找我喔！子龍學長還有志丞學長雖然平常與你們接觸的機會 比較少，但是有你們的關心跟寶貴的意見都讓我的研究之路走的更順利喔，謝謝 你們；彭念豪學長跟莊子毅學長就像是測量組的班長，有你們在的場合每次都很 熱鬧很好玩，謝謝你們帶給我很多歡笑；謝謝嘉玳學姐常常提醒我一些系上的規

II 

定，給了我很大的幫助也常常通融我；君芸學姐這兩年來實在是麻煩你太多了，

無論是在課業上或研究上都真的謝謝你；謝謝依屏學姐借給我很難找的INS 的書，

那兩本書在我的研究上幫了很大的忙喔！余徐維學長總是在關鍵的時刻給我寶貴 的意見與鼓勵，每次都讓我覺得很感動；感謝鈺珊學姐、寧軒學姐、璧華學姐，

除了課業上的幫忙之外，還有在儀器室值班的時候常常給你們添麻煩了；謝謝CAE 組的李正豪同學，沒有你的幫助我的實驗沒有辦法這麼順利的完成，麻煩了你好 多次真是不好意思耶！孟稚開、陳家輝、黃琇蔓、李庭誼、廖晟福可以跟妳們當 同學六年真的很難得耶，未來的路上說不定還會一直遇到請多多指教囉！謝謝怡 萱、怡潔總是貼心的關心我的研究進度與生活小瑣事，還常常聽我講一些有的沒 的真是辛苦你們了，以後也要繼續加油喔！有了維碩、佳頤、建邑的加入，相信 未來的GPS 組會一直很有活力，跟你們相處的時候總是讓我很開心，很高興在快 畢業的時候又多了你們三個好朋友；耿帆、怡岑、彥瑋、中豪、任軒，跟你們一 起聊八卦真的很有趣，祝你們一年後全部都順利畢業！謝謝所有在研究上幫助過 我的人，有你們的幫忙才能讓我的研究所生涯過的這麼順利而且多采多姿！

摘要

本研究之主要目標為建立多天線全球衛星定位系統姿態參數高效解算技術與 誤差分析方法。首先，為了配合移動載台之高動態特性，研究中引入具有快速解 算效能之非迭代轉換參數解算方式，並比較各種方法之解算條件與效率，接著針 對姿態參數解算成果建立誤差推估以及分析方式，藉以明確評估姿態參數品質。

而透過數值模擬以及實際應用所獲得之成果顯示，所提之多天線GPS 求解姿態參 數之技術搭配高效率之非迭代解法，可在無任何外部先驗資訊的條件下，快速求 得可靠之載台姿態參數，對於具有高動態之移動載台應用可提供各高效且可靠之 姿態求解方案。

關鍵字：全球衛星定位系統、多天線系統、姿態求解、非迭代解法

IV 

Abstract

This study aims to develop an efficient multi-antenna GPS technique and quality evaluation approach that can be used to determine the attitude parameters for a moving platform. First, considering the highly dynamic behavior of a moving platform, non-iterative analysis approaches were introduced and their efficiency and reliability were analyzed. Then, a rigorous error analysis approach was established to give definite indications on the obtained attitude parameters. Based on the numerical results from both simulated and real data, it is proven that the proposed approach is capable of giving accurate attitude parameter estimates independently without the need for auxiliary information. Consequently, the multi-antenna GPS system can serve as a reliable resolution of the attitude determination for moving platforms.

Key Words ： Global Positioning System 、 Multi-antenna system 、 Attitude determination、non-iteration solution

目錄

誌謝 ... I 摘要 ... III Abstract ... IV 目錄 ... V 表目錄 ... VI 圖目錄 ... VII

第一章 緒論 ... 1

1-1 研究背景 ...1

1-2 研究動機與目的 ...2

1-3 論文架構 ...3

第二章 相關背景 ... 4

2.1 載台姿態參數定義 ...4

2.2 常見的用以獲取姿態參數之系統 ...6

2.2.1 慣性導航系統 ... 6

2.2.2 全球定位系統 ... 9

2.3 相關應用 ...19

第三章 以多天線GPS 進行姿態定位之方法介紹 ... 22

3.1 傳統式最小自乘法 ...22

3.2 直接解算姿態法 ...23

3.3 非迭代最小自乘擬合法 ...25

3.4 普克斯丁法 ...27

3.5 羅德里格矩陣法 ...28

3.6 非迭代線性轉換法 ...30

3.6.1 非迭代線性轉換法之加權模式 ... 30

3.6.2 非迭代線性轉換法誤差估計 ... 31

第四章 數值實驗 ... 35

4.1 模擬資料驗證 ...35

4.2 實地測試 ...48

第五章 結論與未來工作建議 ... 61

5.1 結論 ...61

5.2 未來工作建議 ...62

參考文獻 ... 64

VI 

表目錄

表2-1、不同等級之陀螺儀精度(胡智祐，2009) ... 9

表2-2、全球定位系統之誤差來源 ... 14

表2-3、現有姿態定位技術比較 ... 19

表3-1、各方法之比較 ... 34

表4-1、非迭代解法利用三點坐標解出之旋轉矩陣 R 的行列式值 ... 36

表4-2、加入判斷式後的行列式值 ... 37

表4-3、NISLT 法與 LM 法各時刻平均誤差(各時刻皆為 10000 次平均) ... 38

表4-4、方位角公式與矩陣式的結果 ... 42

表4-5、利用不同方法推估誤差之結果(重複實驗 100 次) ... 44

表4-6、實驗比較用真值 ... 46

表4-7、不同情況下之解算結果 ... 46

表4-8、Topcon GR-3 GPS 精度 ... 49

表4-9、Leica TCR 1202 R300 全測站儀精度 ... 50

表4-10、3DM-GX1 之參數 ... 51

表4-11、多天線 GPS 與 IMU 靜止測試之標準差(基線長度：1.663m) ... 54

表4-12、多天線 GPS 與 IMU 靜止測試之標準差(基線長度：23.894m) ... 55

表4-13、兩種感測器在各方向之平均值與標準差 ... 57

表4-14、兩種感測器在各方向之平均值與標準差(直線來回 4 次) ... 59

圖目錄

圖2-1、姿態參數對應之旋轉軸 ... 4

圖2-2、航空攝影示意圖 ... 5

圖2-3、各種姿態定義之關係 ... 5

圖2-4、環架型慣性導航系統 ... 6

圖2-5、固裝型慣性導航系統 ... 7

圖2-6、機械式慣性導航系統(左：維基百科，2009；右：維基百科，2006) ... 8

圖2-7、VG035P 光纖陀螺儀(SDI，2009) ... 9

圖2-8、全球定位系統定位原理 ... 12

圖2-9、地面一次差示意圖 ... 15

圖2-10、空中一次差示意圖 ... 16

圖2-11、二次差示意圖 ... 17

圖2-12、利用坐標轉換求解參數示意圖 ... 18

圖2-13、GMAS 系統(He et al.,2004) ... 20

圖3-1、天線坐標系統與地區坐標系統示意圖 ... 24

圖3-2、直接旋轉與先鏡射再旋轉示意圖 ... 27

圖3-3、方位角示意圖 ... 31

圖4-1、非迭代解法利用三點坐標解姿態參數的結果 ... 35

圖4-2、加入判斷式後的解算成果 ... 36

圖4-3、NISLT 法與 LM 法重複實驗 10000 次之平均誤差 ... 38

圖4-4、隨機誤差大小與 RMSE 之關係(以 pitch 為例) ... 39

圖4-5、系統誤差大小對 RMSE 值之影響(以 pitch 為例) ... 40

圖4-6、實驗配置軌跡圖與角度變化量 ... 41

圖4-7、不同基線長度與不同隨機誤差下的角度估計誤差 ... 43

圖4-8、不同方法推估之誤差與真誤差之比較 ... 44

圖4-9、模擬資料(左：衛星天空可視圖；右：天線配置圖) ... 45

圖4-10、主站距離較近時之解算成果(10m) ... 47

圖4-11、主站距離較遠時之解算成果(3km) ... 47

圖4-12、Topcon GR-3 GPS ... 48

圖4-13、Leica TCR 1202 R300 全測站儀 ... 49

圖4-14、3DM-GX1 IMU ... 50

圖4-15、P3-AT 機器人 ... 52

圖4-16、實驗裝置示意圖 ... 52

圖4-17、GPS 基線長度與 GPS-全測站差值及 DOP 間之關係 ... 53

圖4-18、多天線 GPS 與 IMU 靜止穩定度比較(基線長度：1.663m) ... 54

VIII 

圖4-19、多天線 GPS 與 IMU 靜止穩定度比較(基線長度：23.894m) ... 55

圖4-20、矩形實驗軌跡示意圖 ... 56

圖4-21、矩形實驗之結果 ... 56

圖4-22、圓弧實驗軌跡示意圖 ... 57

圖4-23、圓弧實驗之結果 ... 58

圖4-24、沿一直線來回 4 次實驗軌跡示意圖 ... 58

圖4-25、沿一直線來回 4 次實驗之結果 ... 59

圖5-1、整合多天線 GPS 與陀螺儀解算姿態之流程圖 ... 63

第一章 緒論

1-1 研究背景

導航是獲取載台在各個時刻的位置、速度與姿態等參數的一門學問，在西元 前數世紀，世界各地的文明，如中國、希臘等，即發展了關於導航的概念及方法。

在十五、六世紀航海時代來臨後，導航的必要性更是與日俱增，所需的感測器如 量測方向之羅盤儀(compass)、量測速度之計程儀(log)等更成為不可或缺之工具。

二十世紀後，隨著航空工業與無線電之發展，許多新型導航系統因而誕生。1929 年左右，發展出方向陀螺儀，成為最常被用來提供姿態參數之感測器。1973 年，

美國國防部為了軍事定時、定位與導航之目的，發展了全球定位系統(Global Positioning System, GPS)，從此為導航定位寫下嶄新的一頁。

GPS的發展提供了使用者準確的位置、時間與速率等資訊，但在許多應用中，

使用者不只需要上述資訊，亦欲獲得移動載台的姿態參數。在常見的移動載台導 航系統中，除了GPS所提供的位置坐標、速率、時間資訊外，載台的方位資訊常是 由慣性導航系統(Inertial Navigation System, INS)所提供的。因此，一般導航應用中 常利用GPS及INS整合的系統來同時獲得準確的三維位置坐標、速率、姿態參數以 及時間資訊等。

INS是利用慣性感測裝置(Inertial Measurement Unit, IMU)中的加速規與陀螺儀 來推算各時刻的位置與姿態。INS依據精度之不同，可分為戰術級、導航級與戰略 級 (莊智清與黃國興，2001)。此系統的優勢在於可不需外來訊號即可完成定位，

且不受電磁波干擾，在無法接收到足夠GPS訊號時仍能正常運作，這點在大樓林立 的都市區域十分有用。然而，INS所提供的位置與姿態參數之誤差皆會隨時間累 積。此外，INS在使用之前，必須先進行起始校正以去除系統誤差以及獲得初始姿

2 

態，而此起始校正往往耗時過久，無法即時啟動。

雖然GPS原本是用來做為定位與計時的系統，但利用GPS來解算姿態參數的技 術在近年來亦受到廣泛的討論。而在利用GPS求解姿態參數的應用上，可分為單天 線與多天線兩種，其中又以多天線之精度較高。多天線GPS是利用GPS測量來獲得 內部天線向量，透過坐標轉換的概念來解算姿態。利用兩個以上獨立且靠近的天 線，多天線GPS系統可精確估計移動載台之姿態參數。

1-2 研究動機與目的

姿態參數在許多應用中皆扮演著重要且不可或缺的角色，如在一般飛行載台 的導航中，需有姿態參數以進行導航姿態定位、航空攝影測量之解算外方位參數、

光達坐標系統與區域坐標系統之連結、海上施工平台之姿態定位、工程應用上如 施工機具之對位與機器人定位等。

目前廣泛應用於求取姿態參數之 INS 可不需倚賴外來訊號完成定位，且隨著 材料科學之發展精度較以往提高了許多，但其有誤差隨時間累積以及需要起始校 正的問題。除了INS 之外，目前 GPS 具有相當高的定位精度，且被廣泛的用於各 個領域，許多相關的技術也逐漸被發展出來，如將多天線GPS 技術所獲得之天線 坐標應用於大地監測或作姿態參數之解算，雖具有相當高的精度且無誤差累積之 問題，但有訊號遮蔽的問題以及成本較低等級之INS 高出許多。INS 與 GPS 兩種 系統皆各有其優缺點，而在姿態定位的領域中，利用陀螺儀來求解的技術已被廣 為接受且漸趨成熟，且目前鮮少有人利用GPS 來解算姿態參數，因此本研究欲探 討利用多天線GPS 技術進行姿態定位之技術，並配合高效率以及高可靠度之求解 姿態參數的方法，期望能建立一套以多天線GPS 技術為基礎的載台姿態定位最佳 分析方式。

1-3 論文架構

第一章為前言與研究動機；第二章介紹了相關背景，包含姿態參數的定義、

常見的姿態定位技術以及相關的應用；第三章則介紹了多天線GPS 解算姿態參數 之原理以及可用之解算方法，分別介紹了最小自乘法與5 種非迭代解法；第四章 為實驗部分，以模擬資料進行之方法驗證，並將所提之方法落實到實際實驗任務 上；第五章為結論與未來工作建議。

4 

第二章 相關背景

本章首先將介紹姿態參數的定義與應用、一些常見的姿態定位技術原理以及 相關研究與應用。

2.1 載台姿態參數定義

姿態參數(Attitude Parameters)，是用以表示空間坐標系中的角度關係。如圖 2-1 所示，一般可將姿態參數定義為左右搖擺(roll)、前後顛簸(pitch)、方位角(yaw)，

分別對應於x、y、z 軸的旋轉角。除了位置資訊外，roll、pitch、yaw 可用來描述 載體在某一時刻之姿態，進而定出其與絕對坐標系統之轉換關係。

圖2-1、姿態參數對應之旋轉軸

姿態參數在各個領域中常扮演著重要的角色，如航空攝影測量，須有拍攝影 像當時之外方位參數才能解算點位坐標，如圖2-2 所示；在移動載台的導航中，利 用姿態參數可得知航向等資訊，進一步判斷航行之目標；在光達應用中，roll、pitch、

yaw 可定出掃描當時的姿態，配合掃描光線，即可定出點雲之坐標；亦或是在工程 應用之載具操作時，如施工機具、機器人控制等，都需要定出姿態，才可正確的

航向

roll  pitch 

yaw 

x  y 

z 

控制或擺放，因此可有效地減少人力成本。在絕大多數移動載台的應用中，若只 有點位坐標資訊，只能定出該時刻之位置，但無法得知載台相對於空間坐標系統 之關係，利用三軸的旋轉角roll、pitch、yaw，即可描述載台之姿態資訊，也因此 姿態參數在運動中的載台之應用上是不可或缺的重要資訊，而也使得解算姿態參 數的課題備受重視。

圖2-2、航空攝影示意圖

一般來說，感測器所求解到之姿態參數為載台在該時刻相對於其初始時刻之 姿態，而非相對區域坐標框架下之姿態，若已知載台初始時刻相對於區域坐標框 架下之姿態，則可將求解到之姿態改正為相對區域坐標框架下之姿態。除此之外，

載台與感測器間亦有一姿態關係，即感測器三軸所指之方向可能並非與載台之三 軸重合，而此關係可經由率定程序校正，各種姿態定義之關係如圖2-3 所示：

圖2-3、各種姿態定義之關係

天線初始姿態 載台初始姿態

解算之姿態

載台坐標系轉 換到區域坐標

區域坐標框 架下之姿態

率定

6 

2.2 常見的用以獲取姿態參數之系統

一般在解算姿態參數的課題上，如導航應用等，較常利用 INS 來獲得姿態參 數資訊。除了INS 之外，目前亦有不同的做法，如利用多天線 GPS 技術來獲得姿 態參數資訊等。因此，本節將介紹常用的姿態定位技術之原理，分為INS 與 GPS 兩部分。

2.2.1 慣性導航系統

慣性導航系統(Inertial Navigation System, INS)是目前在導航及其他應用上，最 常被用來獲得姿態參數的系統。INS 利用加速規(accelerometer)來量測非重力之加 速度，而利用陀螺儀來進行載台旋轉角速度之量測。

INS 可分為兩種型式：環架型(gimbal)與固裝型(strapdown)。環架型是將加速 規與陀螺儀放置於一平台上，由於平台較穩定，使得環架型INS 有如較簡易之陀 螺儀、高精度、易校正對準等優點，但在設計與維護上則較複雜。環架型INS 如 圖2-4 所示。

圖2-4、環架型慣性導航系統

固裝型 INS 則是逐漸普及之型式，將加速規與陀螺儀直接固定在載台上。此 種系統之成本較低，且其機構簡單，但在計算上較為繁複，且須承受較大動態範 圍之輸入。固裝型INS 如圖 2-5 所示。

圖2-5、固裝型慣性導航系統

無論是哪種形式的 INS，其特點皆為可不需仰賴外來訊號及發射訊號完成定 位，但由於其利用積分的方式來獲得位置與姿態參數，其誤差會隨時間累積，此 外，INS 在啟動時需要進行起始校正，導致無法瞬間啟動。

用來獲得姿態參數的陀螺儀一般可分為機械式陀螺儀及光學式陀螺儀(莊智清 等，2001)。機械式陀螺儀是利用角動量守恆的原理來提供角速度，其最主要的元 件為一個高速轉動的轉子(rotor)以及其環架，如圖 2-6 所示，而兩者搭配可達到角 速度偵測與方向維持之目的，因此環架的型式決定了陀螺儀的功能。轉子的轉動 速度與其角動量成正比，因此越大的角速度具有越靈敏的偵測能力，故一般都要 求轉子要有極高且持續的轉速。

8 

圖2-6、機械式慣性導航系統(左：維基百科，2009；右：維基百科，2006) 光學式陀螺儀的原理是利用量測順逆光波之相位差，此相位差的大小與角速 度成正比，來達到角速度量測之目的。目前在光學式陀螺儀中，大致可分為兩類：

雷射陀螺儀(Ring Laser Gyro, RLG)與光纖陀螺儀(fiber optics gyros, FOG)。雷射陀 螺儀的主要誤差來源包括零點偏置、共振模式跳動、量化誤差、鎖死(lock-in)以及 溫度所造成之誤差等。除了各元件的材料皆會受溫度影響之外，其他的誤差來源 皆是起因於製造過程之不完美，且此些誤差無法完全去除。而鎖死誤差為隨時間 累積之誤差來源，此誤差雖然可被補償，但各種方法皆無法完全將其去除。光纖 陀螺儀相較於雷射陀螺儀，有結構較簡單、體積小、改善鎖死現象等優點，但亦 會受到溫度之影響，目前尚在發展中，僅中低精度的光纖陀螺儀已產業化，如圖 2-7 所示。

圖2-7、VG035P光纖陀螺儀(SDI，2009)

目前陀螺儀依照其精度不同，可分為導航等級(Navigation grade)、戰術等級 (Tactical grade)以及微機電等級(MEMS grade)三種等級，而各種等級之精度如表 2-1 所示：

表2-1、不同等級之陀螺儀精度(胡智祐，2009)

等級 導航 戰術 微機電

陀螺儀飄移量

( ) hr

D

2.2.2 全球定位系統

全球定位系統(Global Positioning System , GPS) 原是美國國防部為了軍事需 要而發展的，但經過測試發現，GPS 具有很高的定位精度，因此 GPS 的發展為許 多相關領域寫下了嶄新的一頁。在測量領域中，GPS 大大的降低了測量外業所需 的人力及時間，且平差後之控制點精度約為1mm~3mm 之間(楊光等，2003)。GPS

10 

亦在導航定位中提供的瞬時的位置坐標與時間資訊，而現在已成為最重要的導航 工具之一。以下將簡單介紹GPS 的定位原理，以及其誤差來源。

a. 全球定位系統之定位原理

全球定位系統之發展改善了以往測量工作受限於時間、通視性等因素，提供 了一種不受時間、地點以及天候之影響的全新測量技術(Leick, 2004)。經過測試，

GPS 靜態測量精度可達公厘或公分精度，即時動態測量(Real Time Kinematic, RTK) 精度亦能達到公分級，而其時間與速度精度亦分別能達到毫微(10^-9)秒與公分級。

因此，GPS 被廣泛的應用在導航、測量、大氣、森林等各種不同領域中。在測量 領域中，GPS 除了可全天候作業外，更改善了傳統控制點間須可通視的問題，且 其精度高，觀測時間短，操作簡便，而節省了不少外業測量的成本，惟須保持一 定的上空開闊度。

GPS 可分為三個部分，太空部分、控制部分與使用者部分：太空部分指的是 衛星本身，由27 顆衛星組成，其中三顆是備用衛星，但平常也可進行定位使用。

這27 顆衛星分布在距地球表面 20200 公里左右之高空，分布在 6 個軌道面上，與 地球赤道面傾角為55 度，每個軌道面上至少有四顆衛星，並採用圓形軌道以增加 地面可見範圍與全球覆蓋率。GPS 衛星每 11 時 58 分繞行地球一圈，因此在地球 上任何地方、任何時間皆能同時收到4 顆以上之衛星訊號，利用已知的衛星坐標 以及衛星訊號，即可隨時隨地的測定使用者所在之地面位置。

GPS 衛星信號中，包含了載波、測距碼與導航訊息。每一顆衛星發射 L1、L2 載波來傳送各種訊息。每顆GPS 上皆設有原子鐘，其產生之基本頻率為

10.23MHz。以此為基本組成兩種不同的虛擬隨機噪聲碼(Pseudo Random Noise, PRN)，一種是 C/A 碼，另一種為 P 碼。C/A 碼僅調制在 L1 載波上，而 P 碼則調 制在L1、L2 上。除此之外，載波上還有包含衛星星曆、衛星工作狀態、時間系統、

衛星時鐘運行狀態、大氣折射改正等之衛星導航訊息(Navigation Message)。

控制部分指的是地面監控系統，當衛星在軌道中運行，衛星的位置即由一系 列描述衛星運動及其軌道參數之星曆計算而得，而星曆即是地面監控系統所提供 的。除了提供星曆外，地面監控系統亦須保持各個衛星之時間同步，其監控每顆 衛星之時間，並藉由導航訊息傳送改正數給使用者。而各個監控站隨時追蹤每一 顆衛星，再將數據傳送給主控制站，再由主控制站彙整所有資訊成導航訊息傳送 給衛星，更新衛星內之資料，進而傳送給使用者。

使用者部分指的是接收衛星訊號之接收儀，依使用目的、所需精度不同，而 有不同的GPS 接收儀，如車用 GPS、量測型 GPS 等。接收儀必須能夠接收到運行 中之GPS 衛星訊號，即時計算測站之位置、速度與時間。

GPS 定位之基本概念即為空中後方交會，將天空中之衛星當作已知控制點，

將接收儀設置在待測點上，利用測距的方式，測定待測點之三維坐標。如圖2-8 所示，一段測距資料可以知道待測點可能分布在一個球面上，而兩段測距資料則 可將範圍縮小到兩個球面交會的一個圓上，當有第三段距離觀測量加入，則可知 道待測點為空間中兩點之其中一點。至此，理論上即可知道待測點之坐標，因為 兩點中只有一點之坐標位於地球表面上。但在實際應用上，仍須接收四顆衛星之 資料，是由於衛星時鐘與接收儀時鐘不同步所致，而將兩者之時間差視為第四個 未知數，則需要四個觀測量才可求解。

12 

圖2-8、全球定位系統定位原理

GPS 定位依據使用之觀測量不同，可分為虛擬距離觀測與載波相位觀測兩 種。虛擬距離觀測的方式是利用PRN 碼量測距離，接收衛星之電碼訊號，並與自 行產生的電碼進行匹配，當相關性達到最高時會得到一個時間偏移τ，代表衛星發 射時衛星之時間與到達時接收儀之時間之差值，將此差值乘上光速即可得到測距 觀測量。但在實際應用中，衛星與接收儀的時間是不同步的，發射時間是以衛星 時鐘為準，而接收時間卻是以接收儀時鐘為準，兩者間存在一個時鐘誤差dt。因 此，在虛擬距離觀測中，有四個未知數，包括接收儀三維坐標，以及此時鐘誤差 dt，則方程式可寫為：

( ^{S r} ) ( ^{2 S r} ) ( ^{2 S r} ) ²

P

X

X

Y

Y

Z

Z

cdt

擬距離觀測方程式可寫為：

( ) P

P = + ρ c dt − dT + dtrop + dion + ε

20200km 

一段距離觀測量可確定 待測點在一個球面上

兩段距離觀測量可確定 待測點在一個圓上

三段距離觀測量可確定待測點為在三個球面交 出兩點之其中一點，而只有一點在地球表面上

其中，P 為量測得到之虛擬距離，ρ 為衛星與接收儀間之真實距離，c 為光速，dt 為接收儀時鐘誤差，dT 為衛星時鐘誤差，dtrop 為對流層誤差，dion 為電離層時鐘 誤差，εP為雜訊及多路徑效應。一般而言，利用虛擬距離觀測所得到之精度，大 約在數十公分至數公尺間。

GPS 載波相位觀測的原理是利用已知訊號之波長，乘上載波的個數即可得到 距離。其原理與電子測距相似，不同之處在於電子測距儀是經由反射回到儀器本 身，所用的時間基準是同一個；而GPS 是由衛星發射至接收儀，時間基準不同，

因此會受到時鐘誤差之影響。由於衛星訊號之相位不會受到都普勒(Doppler)效應 之影響，因此衛星發射訊號時刻與接收儀收到訊號時之載波相位是相同的。

接收儀在剛開始接收訊號時僅能量測到小數週波值，而衛星與接收儀間之整 數週波值卻無法得知，此整數週波值稱為週波未定值(Cycle Ambiguity) N0。若整數 週波值已知，則可計算衛星到接收儀間之精密距離。要求得正確的整數週波值，

可利用連續觀測的方式，計算觀測期間之整數週波值。只要訊號不中斷，週波未 定值N0並不會改變。GPS 之載波相位觀測量可寫為：

( ) L

L = + ρ c dt − dT + dtrop − dion + × + λ N ε

其中，L 為量測得到之載波相位觀測量，ρ 為衛星與接收儀間之真實距離，c 為光 速，dt 為接收儀時鐘誤差，dT 為衛星時鐘誤差，dtrop 為對流層誤差，dion 為電離 層時鐘誤差，εL為雜訊及多路徑效應。

b 全球定位系統之誤差來源

GPS之誤差來源依誤差的性質，可分為系統誤差與偶然誤差兩種：系統誤差包 括了衛星軌道誤差、衛星時鐘誤差、接收儀時鐘誤差、大氣折射誤差等。一般常 見的改正方法有三種：

14 

1. 將誤差當作位之參數一併平差求解，如將時鐘誤差視為未知數。

2. 建立系統誤差的改正模式，如電離層模式等。

3. 利用差分的方式消除，如利用地面一次差消除衛星時鐘誤差。

偶然誤差則包含了多路徑效應與觀測誤差。若將各種誤差之影響化算到衛星 到接收儀的距離上，所造成之誤差如表2-2所示：

表2-2、全球定位系統之誤差來源

誤差來源 對距離觀測量之影響

衛星

1. 軌道誤差 2. 時鐘誤差

1.5m~15m

傳播路徑

1. 對流層延遲誤差 2. 電離層延遲誤差 3. 多路徑效應誤差

1.5m~15m

接收儀

1. 觀測誤差 2. 相位中心變化 3. 時鐘誤差

1.5m~5m

c 差分定位

無論是利用虛擬距離觀測量或是載波相位觀測量來量測點位，都無法利用一 個完全符合的模式來去除觀測量誤差，如衛星時鐘誤差、大氣效應等。雖然無法 去除單一觀測量中所含有之系統誤差，但若利用兩筆以上之觀測量相減來解算，

則可消除觀測量間相同或相似之系統誤差，而此種方式即為差分定位。以下將介 紹常見的差分定位原理。

c.1 地面一次差

地面一次差是指利用兩部接收儀對同一顆衛星進行觀測，如圖 2-9 所示，並將 兩部接收儀所得之觀測量相減，而由於基於衛星之誤差對此兩部接收儀之效應相 同，因此在相減(差分)之後，可消除衛星時鐘誤差 dT。

圖2-9、地面一次差示意圖 若以載波相位觀測方程式來表示：

L ρ c dt dtrop dion λ N ε L

Δ = Δ + Δ + Δ − Δ + Δ + Δ (2.4)

由式(2.4)中可發現地面一次差消除了衛星時鐘誤差dT。實際上，利用地面一

衛星 1

接收儀 1 接收儀 2

16 

次差的方式亦可消除與同顆衛星有關之誤差，且由於衛星訊號傳遞至兩相距不遠 之接收儀路徑幾乎一致亦可消除大部分的大氣誤差。

c.2 空中一次差

空中一次差的概念類似於地面一次差，其利用一部接收儀同時對兩顆衛星之 觀測量來進行差分，即可得到空中一次差之觀測量，如圖2-10 所示。由於是利用 同一部接收儀同時觀測，空中一次差可消除接收儀時鐘誤差dt。

圖2-10、空中一次差示意圖 若以載波相位觀測方程式來表示：

L ρ c dT dtrop dion λ N ε L

∇ = ∇ − ∇ + ∇ − ∇ + ∇ + ∇ (2.5)

由式(2.5)中可發現空中一次差可消除接收儀時鐘誤差dt。

c.3 二次差

若結合兩部接收儀與兩顆衛星之觀測量，即可得到二次差之觀測量，。二次 差可利用兩個地面一次差之結果再進行差分得到，亦可利用兩個空中一次差之結 果再進行差分得到，而兩種方式之結果相同，如圖2-11 所示。

衛星 1

接收儀 1

衛星 2

圖2-11、二次差示意圖 若以載波相位觀測方程式來表示：

L ρ dtrop dion λ N ε L

∇Δ = ∇Δ + ∇Δ − ∇Δ + ∇Δ + ∇Δ (2.6)

由式(2.6)中可發現二次差同時消除了衛星與接收儀的時鐘誤差。由於二次差的 方式可以同時消除衛星時鐘誤差、接收儀時鐘誤差以及大部份之大氣誤差，因此 目前大部分的GPS軟體皆採用二次差的方式來解算。

利用差分的方式來求解，可消除大部分之誤差，如時鐘誤差、大氣誤差等，

但差分會使得觀測量數目減少，如一次差將兩個觀測量相減成為一個新的觀測 量。差分的次數越多，雖然可消除更多之誤差，但同時也捨棄了更多可用之觀測 量。此外，差分亦導致觀測量間產生相關性，因此在實際應用上大多僅使用二次 差之方式來解算。

d 全球定位系統姿態定位原理

隨著GPS 發展愈趨成熟，除了最直接的單點定位之外，許多衍生的應用方式 被廣泛的討論與使用，如相對定位、精密單點定位等。在大多數的應用中，GPS 常用以提供位置、速度與時間資訊， 近來亦有用 GPS 獲取姿態的技術發展。利用 GPS 來解算姿態參數，可分為單天線系統與多天線系統兩種做法。鄭憲君(2008)

衛星 1

接收儀 1

衛星 2

接收儀 2

18 

將單天線GPS 用於無人直升機系統之姿態量測，利用 GPS 觀測量獲得速度資訊 後，透過卡爾曼濾波估算加速度再反推其主旋翼方向，即可得到所謂的虛擬姿態。

利用此技術計算的虛擬姿態，在高動態情況下有所不足，因此其採用搭配陀螺儀 的角速度資訊，以濾波互補的方式得到一組較佳的虛擬姿態。

除了單天線 GPS 之外，亦有多天線 GPS 之發展。多天線 GPS 利用兩個以上 擺放鄰近之天線所獲得之觀測量，來進行後續分析以及計算。如圖2-12 所示，用 多天線GPS 來解算姿態是利用各時刻之點位坐標視為轉換後坐標，而將初始時刻 坐標視為轉換前坐標，則此坐標差異可用一個旋轉矩陣R 與平移量來表示。利用 坐標轉換的概念求得旋轉矩陣R 後，即可解算姿態參數 roll、pitch、yaw，而坐標 轉換之通用公式為：

'

x = R x + t

其中，R 代表旋轉矩陣，t 代表平移參數。而經過前人的研究顯示多天線 GPS 技術 應用於姿態參數解算之精度可輕易達到數弧分(arc-min)以內(Lu, 1995)。

圖2-12、利用坐標轉換求解參數示意圖

u

e n 1 

2  3  x 

z  y 

1’

2’ 

3’ 

x' 

z’ 

R、t

y’ 

根據以上的分析，可發現利用 GPS 進行載台姿態定位有單天線與多天線兩 種，而在大多數的移動載台上都具有單天線GPS，因此單天線系統不需加入額外 的天線為其一大優勢，但在使用上可能須配合其他感測器才得以獲得較佳之姿 態。多天線系統透過坐標轉換的概念，可準確的估算姿態參數，且利用一次差的 概念消除了大部分的系統誤差亦使得精度有明顯提升，因此本研究採用多天線 GPS 的方式來進行姿態參數的解算。表 2-3 為現有姿態定位技術之比較。

表2-3、現有姿態定位技術比較

姿態定位系統 原理 優點 缺點

陀螺儀 機械式-角動量守

恆

不需外來訊號即 可完成定位

誤差隨時間累積 及需起始校正 光學式-順逆光波

相位差

GPS 單天線-虛擬姿態 無誤差累積問

題，精度較以往 提升

訊號遮蔽 多天線-坐標轉換

2.3 相關應用

在本節中將介紹本研究相關的應用，如多天線 GPS、機器人定位等，分述如 下。

Lu et. al(1993)提出不獨立的多天線 GPSCard™系統以及利用一個搖擺物操作 的3DF 系統，可有效的解決多天線 GPS 在海面應用由於晃動較大而時常發生的多 路徑效應以及週波脫落之問題。此篇報告提出可計算剛體載台yaw、pitch、roll 的 公式，並利用了處理點位坐標數據的最小自乘演算法。並且測試了一個利用天線

20 

基線限制計算的方法。結果顯示無論是3DF 系統或 GPSCard™系統皆可達成即時 動態解算。姿態參數的精度也可控制在15 弧分以內。

GNSS載波相位觀測量的整數週波值是高精度導航與姿態量測的關鍵。

Teunissen(2007)提出了將多天線GPS之觀測量計算為姿態參數的通用公式，可同時 處理所有GNSS衛星之觀測量。此方法可有效的解決週波未定值對於姿態解算之影 響，且利用整數週波的限制條件可提升參數精度。

為節省成本，有利用單一接收儀接收多個天線資訊的GMAS(GPS

Multi-Antenna Switch, GMAS)系統之發展(He et al., 2004)，如圖2-13所示，此系統 依序地接收各天線資訊，可只利用一個接收儀完成多個天線之坐標解算作業。經 過其試驗證明，只要數據處理過程得當，GMAS之成果與同時使用多個接收儀的成 果一致。但此種作法，由於其作法為依序的接收不同天線的坐標資訊，故各天線 觀測量並非同步取得，因而僅能應用於點位監測等固定式的量測，不適用於動態 的載台姿態量測。

圖2-13、GMAS系統(He et al., 2004)

張嘉強等(2003)針對近海測量之作業型態，如測船之姿態量測、測船軌跡圖之 繪製及測深基準面之傳遞等項目，探討GPS 運用之成效與分析。而其 GPS 測量相 應之精度為2cm，並以多天線 GPS 的方式來進行測船之姿態量測，搭配直接解算 法(Lu, 1995)來完成姿態解算。

胡毓盛(2008)利用多天線GPS進行國道三號三維形變之監測，並比較了e-GPS 與一機多天線GPS效能之優劣。根據其研究結果，利用一機多天線GPS系統之精度 可達N方向0.18cm、E方向0.25cm、h方向0.57cm，而利用e-GPS之精度則為N方向 1.44cm、E方向1.51cm、H方向2.96cm。雖然e-GPS有施測迅速及成本低之優勢，但 以精度考量僅能作為較大位移監測作業之工具。

楊光等(2004)應用多天線 GPS 於大壩變形監測之研究，發展了一機多天線監 測系統以降低成本。一機多天線監測系統指的是一個接收儀利用時間間隔的方 式，接收多個天線之資訊。此系統在傳輸距離大於60m 時，訊號損失會相當大，

甚至完全失去訊號，因此加入了GPS 訊號低噪聲放大器來增強其效果。而經由研 究顯示，只要解算合理，此系統所得到之成果與使用多個接收儀一致，且大大降 低了成本。

除了較常見之應用外，目前 GPS 也被應用於機器人定位上。Seward D et.

al(2007) 介紹一種機器人式的挖掘器，討論可行之發展、導航系統，並使其自動化 做決策以及改變其行為來完成使用者的指令。由於差分GPS 提升了精度與可靠 度，提供了建築機具自動化的可能性，但受限於安全性問題以及其經濟效益以致 於此概念一直以來僅在理論階段。其所利用的定位工具：GPS 是裝載在屋頂上，

使用的是單一天線，利用差分技術的結果，X-Y 坐標可達 25mm，Z 坐標可達 50mm，資料更新是 5Hz，並搭配一個電子羅盤來提供方位資訊，若成本許可則利 用精度較高的多天線系統作為方位資訊之提供。

利用自動化的方式來操作農業機具可提高效率，減少人力成本以及解決了人 身安全的問題，但一直以來卻受限於感測器之不足而無法發展。而隨著載波差分 GPS技術(Carrier Phase Differential GPS)的發展，利用低價GPS進行定位與定向之精 度增加，利用O’Connor et al(1996)提出的方式，定位精度可達方位角標準差小於1 度，位置坐標標準差小於2.5cm，使得農業自動化的可行性大幅提升。

22 

第三章 以多天線 GPS 進行姿態定位之方法介紹

多天線GPS的姿態定位方式是利用點位坐標為其輸入之觀測量，由於其利用多 個靠近的天線接收到觀測量來解算，而同樣時刻擺設鄰近之天線點位所接收到之 衛星幾乎相同，而其訊號之傳遞路徑亦十分相近，使得其對流層、電離層等系統 誤差相似，其觀測量誤差間具有高相關性，因此利用此技術預期將可消除大多數 之系統誤差，以提升姿態參數之精度。本研究即採用多天線GPS技術來求解姿態參 數，而多天線GPS解算姿態參數的方式是利用坐標轉換之概念，除了最小自乘法之 外，亦有許多坐標轉換的方式可應用在多天線GPS求解姿態參數的課題上，因此在 此介紹與比較以點位坐標轉換為基礎的姿態求解方法，包括傳統式最小自乘法、

直接解算姿態法、非迭代最小自乘擬合法、普克斯丁法、羅德里格矩陣法以及非 迭代線性轉換法之計算公式以及其特點。

3.1 傳統式最小自乘法

最小自乘法(Least Square Method, LSM)利用改正數平方和最小的概念來解出 參數之最適解。利用最小自乘法解姿態參數是利用坐標轉換之概念，而坐標轉換 觀測方程式為：

x' Rx T (3.1) = +

其中，x’為轉換後的坐標，x 代表轉換前的坐標，

R R ⁼ z ( ^yaw ) ( R y ^pitch ) R x ^{( roll )}

' cos( ) sin( ) 0 cos( ) 1 sin( ) 1 0 0 ' sin( ) cos( ) 0 0 0 0 0 cos( ) sin( ) ' 0 0 1 sin( ) 0 cos( ) 0 sin( ) cos( )

z z y y x

z z x x y

y y x x z

x x t

y y t

z z t

θ θ θ θ

θ θ θ θ

θ θ θ θ

⎡ ⎤

⎡ ⎤ ⎡ − ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥

⎢ ⎥ ⎢= ⎥⎢ ⎥⎢ − ⎥ ⎢ ⎥ ⎢ ⎥+

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢ ⎥ − ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦⎣ ⎦⎣ ⎦ ⎣ ⎦ ⎣ ⎦

(3.2)

其中，

θ _z

θ _y

θ _x

對式(3.2)中之未知數 roll、pitch、yaw 分別進行偏微分可組成係數矩陣 B，並將起 始值代入求得f 矩陣，再利用最小自乘法求解公式迭代解算每次之改正數 Δ：

( ^T ) ^{-1 T}

Δ = B PB B Pf

其中，P 代表權矩陣。迭代計算至改正數小於容許值。

在大多數的應用中，雖然可利用最小自乘法來解坐標轉換參數，求得參數的 正確估值。但在無法給定正確起始值或需要高解算效率的應用上，若需要經過多 次的迭代計算才能收斂，亦或是發散而無法解算出參數估值，皆無法應用最小自 乘法，而導致最小自乘法之適用性較為侷限。

3.2 直接解算姿態法

直接解算姿態法(Direct attitude determination approach, DAD)將三個天線用特 殊的擺放方式以及與一般習慣不同的旋轉順序，可直接由公式中求得姿態參數(Lu, 1995)：

(

roll

pitch

yaw

i

y x z i

b R R R l (3.4)

其中，b 代表由載台坐標框架(Antenna Body Frame, ABF)坐標組成的矩陣，

_i

_i

24 

y 軸代表航向，因此 y 軸對應之旋轉角為 roll，x 軸對應之旋轉角為 pitch。

如圖3-1 所示，將天線一放置在原點，天線二擺放在 yA軸(此方法定義 yA軸 為航向)上，而天線 3 則放在由天線 1、2 定義出的 xyA平面上。

圖3-1、天線坐標系統與地區坐標系統示意圖 如此可得到天線坐標系統的坐標：

1

2 12

3 3 3

0 0 0

0 0

0 a

a b

a x y

⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ = ⎥

⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦

(3.5)

其中，

b 代表天線 1、2 的距離。有別於一般旋轉順序 X-Y-Z，此法是依序旋轉 ₁₂

2, 2,

arctan

^l

l

yaw x

y

⎛ ⎞

= − ⎜⎜⎝ ⎟⎟⎠ (3.6)

2,1

2 2

2, 2,

arctan

l l

pitch z

x y

⎛ ⎞

⎜ ⎟

= ⎜⎝ + ⎟⎠

(3.7)

3, 3,

arctan ' '

l l

roll z

x

⎛ ⎞

= − ⎜⎜⎝ ⎟⎟⎠ (3.8)

xA

’

天線1

LLF

天線2

xL

yL 天線3

yA

ABF

其中，x’、z’為被 yaw、pitch 角旋轉過之坐標系統，即：

( ) ( )

( ) ( )

( ) ( )

( ) ( )

3, 3,

3, 3,

3, 3,

' 1 0 0 cos sin 0

' 0 cos sin sin cos 0

' 0 sin cos 0 0 1

l l

l l

l l

x yaw yaw x

y pitch pitch yaw yaw y

z pitch pitch z

⎡ ⎤ ⎡ ⎤ ⎡ ⎤⎡ ⎤

⎢ ⎥=⎢ ⎥ ⎢− ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥

⎢ ⎥ ⎢⎣ − ⎥ ⎢⎦ ⎣ ⎥⎦⎢ ⎥

⎣ ⎦ ⎣ ⎦

(3.9)

此法雖然可以快速的獲得姿態參數，但為利用簡單的公式進行計算，坐標 軸旋轉的順序與一般定義不同。而在計算roll 時，強制天線 3 的 z 坐標須為 0：

3, 3,

3, 3,

3,

cos( ) 0 sin( ) '

0 1 0 '

0 sin( ) 0 cos( ) '

b l

b l

l

x roll roll x

y y

roll roll z

⎡ ⎤

⎡ ⎤ ⎡ − ⎤

⎢ ⎥

⎢ ⎥ ⎢= ⎥⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦

(3.10)

利用式(3.10)的約制，雖可快速求解到參數估值，但在點位坐標含有誤差時，是否 可以使用這樣的條件以及其對精度可能的影響亦不清楚。此外，此方法在解算上 只需利用兩個向量來解姿態參數，在觀測量資訊較多的情形下，多餘觀測量並無 法一併考慮，而導致使用此方法之解並非為一最佳解(Keong, 1999)。

3.3 非迭代最小自乘擬合法

非迭代最小自乘擬合法(Non-iterative Least Square Fitting) (Arun et al., 1987)利 用轉換前後點位與重心之向量，組成一矩陣H，再利用奇異值分解法(Singular Value Decomposition, SVD)解出其旋轉矩陣R，進而解出平移參數T。其計算方法如下：

' 1 n

T T

i i i

q q

=

=

∑

H UΛV (3.11)

其中，

q _i

p _i

p q _i ^'

p _i ^'

p ^'

∧ T

=

R VU (3.12)

26 

解算出旋轉矩陣R之後，可利用R矩陣各分量求得姿態參數。而平移參數T則 為：

∧ ' ∧

= −

T p R p (3.13)

非迭代最小自乘擬合法中，提到正交矩陣可分為旋轉(Rotation)矩陣或鏡射 (Reflection)矩陣兩種，若R矩陣的行列式值(Determinant)為1，則為旋轉矩陣；若R 矩陣的行列式值為-1，則為鏡射矩陣。

如圖 3-2 所示，在三維空間中，欲利用三點來解轉換前後坐標間之坐標轉換參 數，則有可能直接解到旋轉矩陣R，此時行列式值 det(R)等於 1；而也有可能先鏡 射再旋轉，同樣可用來描述轉換前後坐標之關係，但解出來之姿態參數值卻是錯 誤的。為防止出現鏡射矩陣的情況，須在公式中加入判斷式。若將H 矩陣作奇異 值分解後，其中有任一個奇異值為0，則其旋轉矩陣R 的公式可改為：

∧ T

=

^'

R V U (3.14)

其中，V’是將 V 中對應奇異值為 0 的欄(column)變號之後得到的新矩陣。例如，

若對角矩陣Λ 中，第三個奇異值為 0，則將 V 矩陣中的第三欄變號得到 V’。如此，

利用此判斷式便可正確計算參數值。

圖3-2、直接旋轉與先鏡射再旋轉示意圖

3.4 普克斯丁法

普克斯丁法(Procrustean Solution)利用誤差最小化的原理，配合利用轉換前後 點位組成的矩陣X、X’、中心化矩陣C(centering matrix)組成之矩陣做奇異值分解 (SVD)，可求得旋轉矩陣R(Awange et al., 2005)：

1

T

=

_n

n

C I ll (3.15)

T ^'

T

X CX UΛV (3.16)

其中，l是由1組成的n×1矩陣，n為參予計算點位之個數，Λ為一對角矩陣。利用式 (3.16)奇異值分解之結果，可得到旋轉矩陣R如式(3.12)：

∧ T

=

R VU (3.12)

解算出旋轉矩陣R之後，可利用R矩陣各分量求得姿態參數。而平移參數T則 為：

1  1

det(R)=1  det(H)=‐1 

2  3

3 2 1

3 2

rotation 

前 後

det(R)=1  rotation 

reflection 

28 

1 ^{T T}

n

⎛ ∧ ⎞

= ⎜ − ⎟

⎝ ⎠

T X ' X R l

3.5 羅德里格矩陣法

羅德里格矩陣法(Lodrigues Matrix, LM)之概念為旋轉矩陣R 是由三軸的旋轉 角所組成的，因此其九個元素是由三個獨立元素所組成的。假設一個反對稱矩陣

0 0

0 γ β

γ α

β α

− −

⎡ ⎤

⎢ ⎥

= ⎢ − ⎥

⎢ ⎥

⎣ ⎦

S

三個元素表示為(Yao et al., 2007)：

2 2 2

2 2 2

2 2 2

1 2 2 2 2

1 2 2 1 2 2

2 2 2 2 1

α β γ γ αβ αγ β

γ αβ α β γ α βγ

αγ β α βγ α β γ

⎡ + − − − − − ⎤

⎢ ⎥

= ⎢ − − + − − − ⎥

Δ ⎢⎣ + − − − + ⎥⎦

R (3.18)

其中，Δ = +1

α ²

β ²

γ ²

( ) ( )

( ) ( ) ( )

1

1 1 1

(1) (2)

(3) 1 , 2

2

T

T

−

− − −

⎧ = −

⎪ ⎪

= − +

⎨ ⎪

⎪ + = + = + −

⎩

S S

R I S I S

I S I R S I R I

(3.19)

其中，I 為3×3 之單位矩陣。

如果利用坐標差值的方式，可僅考慮兩組點位

( p p 1 , 2 )

( q q 1 , 2 )

( )

1 2 1 2

p − p = R q − q

可將上式改寫為：

v = Rw

其中，

^{1 2 1 2}

1 2 1 2

, ,

p p q q

p p q q

− −

= =

− −

v w

利用以上之公式可得到：

( )

v - w = S v + w

可將上式改寫為：

l = Sb

其中，l = v - w b = v + w ，則可得： ,

0 0

0

z y x

z x y

y x z

b b l

b b l

b b l

α β γ

⎡ − − ⎤⎡ ⎤ ⎡ ⎤

⎢− ⎥ ⎢ ⎥ ⎢ ⎥=

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

⎣ ⎦

(3.24)

上式中，係數矩陣為相依矩陣，因此需要至少兩個不平行的向量來求解，則可將 上式改寫為：

3 3 3 1

n × ×

3 1 n

×

其中，B 為係數矩陣，^x=

( ^{α β γ}

) ^T

( ) ^{T -1 T}

x = B B B l

將求解到之參數代入R 矩陣之公式，即可求得旋轉矩陣 R。

30 

3.6 非迭代線性轉換法

非迭代線性轉換法(Non-Iterative Solution Linear Transformations, NISLT)透過 線性轉換，將參數的解用簡單的代數形式來表示，且不需初始值即可應用在參數 值很大的情況下。線性轉換的方式可增加解算效率，且不會降低參數的精度。利 用點位坐標差值以及線性代數技巧，可直接求得坐標轉換參數，再利用計算得到 之旋轉矩陣R 求解姿態參數。則各個轉換參數的解可由下列公式計算(Han, 2010)：

( ) ¹

T T − T

=

ΔX' ΔX ΔX ΔX UΛV

其中，ΔX’為轉換後的坐標差值與其法向量，ΔX 代表轉換前的坐標差值與其法向 量，Λ 為一對角矩陣。則旋轉矩陣 R 為：

∧ T

=

R UV (3.12)

解算出旋轉矩陣R之後，可利用R矩陣各分量求得姿態參數。而平移參數T可 利用R計算：

'

mean x x

∧

∧

T R (3.28)

3.6.1 非迭代線性轉換法之加權模式

NISLT 法求解旋轉矩陣 R，如前所述，將轉換前後的點位坐標差值向量組成 之向量，利用奇異值分解法即可求得旋轉。但當觀測量精度不一時，可利用NISLT 法之加權模式來求解。在轉換前後之坐標差值向量中加入權矩陣P：

( ) ( ^T ) ( ^{T T} ) ^{− 1}

=

G PΔX' PΔX ΔX P PΔX

利用式(3.29)求得之 R 矩陣進行奇異值分解，即可求得旋轉矩陣 R。

3.6.2 非迭代線性轉換法誤差估計

利用 NISLT 法解算參數，可不需迭代計算，亦沒有鏡射矩陣的問題，應用十 分廣泛，但缺點為無法直接獲得參數精度。參數精度在各種應用上皆有其必要性，

而NISLT 法是一個具有明確線性代數定義的方法，因此以下首先介紹以最小自乘 概念出發的方式如何估計NISLT 法之誤差，再推導直接由 NISLT 法公式出發的誤 差估計公式。

a 最小自乘法誤差傳播

在二維平面上可用來描述姿態的角度只有一個，即方位角 yaw，如圖 3-3 所示：

圖3-3、方位角示意圖

方位角由 y 軸起始，順時旋轉計算。而方位角可僅利用兩點坐標來計算，其 公式為：

2 1

2 1

arctan(

dx

X X

dy Y Y

ϕ

− (3.30)

因此，可利用式(3.30)來進行二維的姿態參數誤差傳播：

1 1 × 1 4 4 4 4 1 × × × T

xx xl ll xl

Σ = J Σ J

y 

x  yaw 

32 

其中，

i

F x

⎡∂ ⎤

= ⎢ ⎥⎣∂ ⎦ J

xl

2 1

2 2 2

2 1 2 1 2 1

2 1 2

2 1

2 1

2 2 2

2 1 2 1 2 1

2 1 2

2 1

2 1 2 1

2 2 2

2 2 1 2 1 2 1

2 1 2

2 1

2 1

1

2

1

2

( )

( ) ( ) ( )

( )(1 )

( )

( )

( ) ( ) ( )

( )(1 )

( )

( ) ( )

( ) ( ) ( )

( ) (1 )

( )

( )

1

1

Y Y

X X Y Y X X

Y Y

Y Y

Y Y

X X Y Y X X

Y Y

Y Y

X X X X

X X Y Y X X

Y Y

Y Y F

X

F X

F Y

F X

Y

X

∂ = − =

∂ +

∂ = =

∂ +

∂ = =

− −

− − −

−

∂ +

∂ =

∂

+ −

−

− − −

− +

−

− −

− − −

− +

−

− −

_{2 1}

2 2 2

2 2 1 2 1 2 1

2 1 2

2 1

( )

( ) ( ) ( )

( ) (1 )

( )

X X

X X Y Y X X

Y Y

Y Y

− −

− − + −

− +

−

=

利用式(3.31)即可直接求得而二維平面姿態參數 yaw 之估計誤差。

若為三維空間之應用，可利用最小自乘法的概念來推估NISLT法求解之參數精 度。樂怡岑(2010)的研究中推算NISLT法的估計誤差，其誤差估計的公式為：

( ( ) ^{T -1 T} ) ( ^{T ( T ) -T} )

xx ll

Σ = B B B A Σ A B B B (3.32)

其中，B代表未知數的係數矩陣，A代表觀測量的係數矩陣。

由式(3.32)可發現其原理是利用最小自乘法的方式來組成係數矩陣 A、B，進 而推算參數精度，而非直接利用NISLT 法的公式推估。

b 非迭代線性轉換法誤差傳播

三維的旋轉矩陣 R 可用三個點位來求解，再化算為姿態參數。因此姿態參數 的估計誤差為點位坐標的函式，亦可利用點位坐標之精度來進行誤差傳播。NISLT 法是以

( x y z 、 1

1 1 ) ( x y z 2

2 2 )

( x y z 坐標計算任意兩組坐標差值與三點組成 3

3 3 )

的平面法向量的方式組成ΔX’、ΔX 矩陣，由式(3.33)即可求解 R 矩陣：

( ) ¹

T T −

=

R ΔX' ΔX ΔX ΔX

其中，

2 1 2 1 2 1

3 1 3 1 3 1

'

n n n

X X Y Y Z Z

X X Y Y Z Z

X Y Z

− − −

⎡ ⎤

⎢ ⎥

=⎢ − − − ⎥

⎢ ⎥

⎣ ⎦

ΔX 、

2 1 2 1 2 1

3 1 3 1 3 1

n n n

x x y y z z

x x y y z z

x y z

− − −

⎡ ⎤

⎢ ⎥

=⎢ − − − ⎥

⎢ ⎥

⎣ ⎦

ΔX 。

由於姿態參數的誤差是由轉換前後點位傳播而得，因此根據誤差傳播定理可 推估其誤差：

( )

^R {

^;

^'

} ^{T R}

vec _R = vec Δ _X vec Δ _X

Σ J Σ J

其中，

( )

R

A

J D R I I ，

( ^T ) ^{1 T} ( ^{( ) ( T )} ) ^{( ) T}

A A A A T T T

= − = ⊗ Δ ⊗ Δ ⊗ Δ

D  D D D I I X I X I I I X

D A = ⊗ Δ ( I X I ) T

I

T

利用式(3.34)即可直接獲得參數的估計誤差，且不需經過繁複的計算過程。

非迭代最小自乘擬合法、Procrustean 法、LM 法與 NISLT 法解算出旋轉矩陣 R 之後，可利用(3.35)計算姿態參數：

32 33

31

2 2

11 21

21 11

arctan

arctan

arctan

r r

roll r

pitch

r r yaw

r r

⎡ ⎛− ⎞ ⎤

⎢ ⎜ ⎟ ⎥

⎝ ⎠

⎢ ⎥

⎢ ⎥

⎡ ⎤ ⎢ ⎛ − ⎞⎥

⎢ ⎥= ⎢ ⎜ ⎟⎥

⎢ ⎥ ⎢ ⎜⎝ + ⎟⎠⎥

⎢ ⎥

⎣ ⎦ ⎢⎢⎢⎣ ⎛⎜⎝− ⎞⎟⎠ ⎥⎥⎥⎦

(3.35)

根據以上五種方法之概念及公式介紹，可將各方法之特點整理如表 3-1 所示：

34 

表3-1、各方法之比較

方法 迭代與起始

值給定

是否須加入 判斷式

點位佈設位 置

特點

LSM 是 否 無限制 於解算後可直接獲得

參數精度

DAD 否 否 固定 若有較多之觀測量無

法一併求解。

Non-iterative

LS Fitting 否 是 無限制 需加入判斷式 Procrustean 否 是 無限制 需加入判斷式

LM 否 否 無限制 利用兩組坐標差值向

量即可求解。

NISLT 否 否 無限制 加 入 法 向 量 一 併 求 解。

第四章 數值實驗

4.1 模擬資料驗證

本節以模擬數據的方式進行理論驗證與比較各種不同方法之特性以及參數精 度之探討。

模擬資料驗證一：非迭代法之解算條件

在介紹非迭代最小自乘擬合時，有提到此方法須加入判斷式以避免解算過程 中出現鏡射矩陣，而Procrustean 法以及 NISLT 法亦是利用奇異值分解法來求解參 數，因此欲測試在此三種利用奇異值分解求解參數的非迭代解法中，是否都會出 現行列式值為-1 的情形。本實驗利用每時刻相同的角度增量 droll=3 度，dpitch=11 度，dyaw=13 度，測試 10 個時刻三種非迭代解法皆未加入判斷式時，解算成果如 圖4-1 所示：

1 2 3 4 5 6 7 8 9 10

-200 0 200

三 點 的 解 算 成 果 LS Fitting

epoch

姿態參數(deg)

roll pitch yaw 真值

1 2 3 4 5 6 7 8 9 10

-200 0 200

三 點 的 解 算 成 果 Procrustean

epoch

姿態參數(deg)

1 2 3 4 5 6 7 8 9 10

-200 0 200

三 點 的 解 算 成 果 NISLT

epoch

姿態參數(deg)

圖4-1、非迭代解法利用三點坐標解姿態參數的結果

由於模擬的 roll、pitch、yaw 是隨時間等速增加的函式，因此解算之圖形應為

36 

一直線，而圖4-1 中只有 NISLT 法求得之姿態參數為與時間成正比的等速函式。

依據上述正交矩陣的特性，進一步觀察非迭代最小自乘擬合與Procrustean 法的行 列式值，如表4-1 所示：

表4-1、非迭代解法利用三點坐標解出之旋轉矩陣 R 的行列式值

det(R)

LS Fitting 1 -1 -1 1 1 -1 1 -1 1 1 Procrustean -1 -1 -1 1 1 1 1 1 1 1

NISLT 1 1 1 1 1 1 1 1 1 1

由表 4-1 可知，非迭代最小自乘擬合與 Procrustean 法不定期會出現行列式值 為-1 的情形，而導致其解得錯誤的姿態參數。要解決此問題需要額外加入在非迭 代最小自乘擬合法中提出之判斷式，以將鏡射矩陣化算為旋轉矩陣R。加入判斷 式之後的結果如圖4-2 所示，可發現非迭代最小自乘擬合法、Procrustean 法的姿態 參數軌跡為直線。

1 2 3 4 5 6 7 8 9 10

0 50 100 150

三 點 的 解 算 成 果 加 入 判 斷 式

LS Fitting ( )

epoch

姿態參數(deg)

roll pitch yaw 真值

1 2 3 4 5 6 7 8 9 10

0 50 100 150

三 點 的 解 算 成 果 加 入 判 斷 式

Procrustean ( )

epoch

姿態參數(deg)

1 2 3 4 5 6 7 8 9 10

0 50 100 150

三 點 的 解 算 成 果 NISLT

epoch

姿態參數(deg)

圖4-2、加入判斷式後的解算成果