GPS動態定位演算法與無人載具實驗

國立臺灣大學工學院應用力學研究所 碩士論文

Institute of Applied Mechanics College of Engineering National Taiwan University

Master Thesis

GPS 動態定位演算法與無人載具實驗

GPS Kinematic Positioning Method and Experiment of an Unmanned Vehicle

簡敏琦 Min-Chi Chien

指導教授：王立昇 博士、張帆人 博士

Advisors: Li-Sheng Wang, Ph.D., Fan-Ren Chang, Ph.D.

中華民國 102 年 7 月

July 2013

致謝

感謝指導教授王立昇老師和張帆人老師在這兩年來的教導。王老師對於研究 的耐心堅持，與張老師在學校生活上的悉心照顧，不僅讓我在學識方面上得以成 長，也在豐富了我在生活方面的經驗。這些過程將會成為我之後的人生道路上最 重要的參考，在此向二位指導教授獻上最誠摯的謝意。

感謝學位考試時的各位口試委員：王伯群老師、卓大靖老師和連豊力老師，

對於我碩士論文內容提供的種種建議，讓我的論文能更加周延。

感謝楊淳元學長、簡懋予學長和莊修敏學長在我碩一對於課業方面各種資源 的提供，並在畢業後仍能抽空給予我極多的支援。

感謝彭進業同學和鄔澤民同學於實驗進行時的討論與援助，讓我突破盲點得 以展現研究成果。

感謝王舜瑋學弟和吳光俞學弟在實驗室生活上的協助，且製造各種歡笑令我 得以度過難關。

感謝電機所 Lab207 的同學們，除了研究的幫忙之外，更使我的碩士生涯變得 多采多姿。

最後，在此感謝我的父母簡明哲先生與朱靜慧女士，多年的栽培以及付出，

成為我最大的靠山。如果我在人生上有任何的成就或功績，皆是你們投資於我身 上的一切造成的。

僅以這微薄的研究成果，獻給所有關心我、在我低潮時給予我鼓勵、在我孤 獨時給予我陪伴、在我碰到難關時給予我祝福的人。

摘要

本研究之目的為發展具有精準定位與偵測規避障礙物功能的戶外自動導航車 系統。該系統包括移動站、參考站以及主控站。移動站含有運動功能的馬達制動 系統、定位功能的 GPS 接收系統和電子羅盤、偵測障礙功能的雙眼相機系統和超 音波系統；參考站具有的第二組 GPS 接收機可和移動站搭配提供更多定位資訊；

主控站則利用無線網路，接收移動站的訊息監測載具的行為。

為進行即時 GPS 差分定位，我們採用 LAMBDA(Least-squares Ambiguity

Decorrelation Adjustment)演算法，經由降低整數周波值相關性以提高整數搜尋效率，

以求得移動站和參考站兩組 GPS 接收機差分量之整數周波值。並使用卡曼濾波器 (Kalman Filter)修正基線向量的估測值，配合電子羅盤提供的載具姿態，得到移動 站的動態資訊。

在障礙物偵測部分，我們採用雙眼相機，透過雙眼立體視覺將影像資料解算 得前方障礙物位置，並搭配多方向的超音波感測器偵測處於視覺死角或過於接近 的障礙物。經實驗驗證，利用上述感測器所提供的障礙物資料，結合 GPS 定位及 避障路徑演算規劃，我們所設計的無人載具系統確能即時避障，並順利抵達終點 位置。

Abstract

The purpose of this thesis is to develop an unmanned-vehicle system capable of accurate positioning, obstacle detection, and collision avoidance. The whole system consists of three parts: base station, reference station, and moving vehicle. On the moving vehicle, a servo motor system is used to do motion control; ultrasonic sensors and a stereo camera are adopted for obstacle detection; an electronic compass and a GPS receiver are installed for vehicle positioning. On the reference station, another GPS receiver is used to provide more positioning data. The main station can monitor and control the unmanned vehicle.

For accurate positioning, a pair of GPS receivers can allow us calculate the vehicle position by Difference technique. By using the LAMBDA method (Least-squares Ambiguity Decorrelation Adjustment), we can resolve the integer ambiguity problem in an efficient process. Using Kalman Filter can further improve the performance of positioning, and the kinematic date of the vehicle can be obtained.

For obstacle-avoiding, we integrate the stereo camera system to detect and locate the obstacles in front of the vehicle, and ultrasonic sensors to detect nearby obstacles.

After detecting the obstacles, a map of obstacles is then built, and a feasible path to the destination can be designed by applying the RRT(Rapidly-Random-Tree) technique.

Experimental results show that the design process is sound, and the unmanned vehicle can indeed follow the path to the destination without colliding with obstacles.

目錄

致謝 ... I 摘要 ... II

ABSTRACT ... III

目錄 ... IV 圖目錄 ... VII 表目錄 ... X

第一章 緒論 ... 1

1.1 前言 ... 1

1.2 文獻回顧 ... 1

1.3 研究內容簡述 ... 2

1.4 論文架構 ... 2

第二章 載具系統架構 ... 3

2.1 整體系統架構 ... 3

2.2 硬體設備架構 ... 3

2.3 元件介紹 ... 4

第三章 定位系統 ... 10

3.1GPS 定位系統介紹 ... 10

3.1.1 太空導航衛星部分 ... 10

3.1.2 地面控制部分 ... 10

3.1.3 使用者接收部分 ... 11

3.1.4 GPS 座標系統 ... 11

3.2 差分定位技術 ... 14

3.2.1 虛擬距離(Pseudo-range)觀測量模型 ... 14

3.2.2 載波相位(Carrier-phase)觀測量模型 ... 15

3.2.3 差分定位 ... 15

第四章 即時動態定位演算設計 ... 18

4.1LAMBDA 方法 ... 18

4.1.1 求取周波未定值實數解 ... 18

4.1.2 低相關化轉換 ... 19

4.1.3 整數搜尋 ... 22

4.2 解算位置修正 ... 23

4.2.1 位置解算 ... 23

4.2.2 卡曼濾波器遞迴程序 ... 23

4.2.3 狀態方程式 ... 24

4.2.4 量測方程式 ... 25

4.2.5 強健型卡曼濾波器(Robust Kalman Filter)[8] ... 26

4.3 定位演算法流程 ... 28

第五章 障礙偵測系統 ... 29

5.1 雙眼視覺系統 ... 29

5.2 超音波系統 ... 30

5.2.1 超音波測距原理 ... 30

5.2.2 感測器配置 ... 31

第六章 路徑規劃設計 ... 33

6.1 快速探索隨機樹 ... 33

6.1.1 隨機樹的建構 ... 33

6.1.2 路徑縮短 ... 35

6.2 超音波避障規則庫 ... 35

6.2.1 規則庫介紹 ... 36

6.2.2 避障原理 ... 36

6.3 避障模式整合 ... 38

第七章 實驗結果與分析 ... 39

7.1RTK 定位實驗結果 ... 39

7.1.1 離線測試實驗 ... 39

7.1.2 路測實驗 ... 42

7.2 載具整合實驗 ... 44

7.2.1 雙眼視覺測障與路徑規劃實驗結果 ... 44

7.2.2 避障感測器整合實驗結果 ... 45

7.3 椰林大道路測實驗 ... 46

第八章 結論 ... 49

參考文獻 ... 50

附錄 ... 52

圖目錄

圖 2-1 系統架構示意圖 ... 3

圖 2-2 無人載具硬體架構 ... 4

圖 2-3 DWM-1563.5G 無線網卡 ... 4

圖 2-4 MCP3024 運動控制卡 ... 5

圖 2-5 TSDA-CB 伺服馬達 ... 5

圖 2-6 GES302N-3KVA 不斷電系統 ... 6

圖 2-7 TCM2-50A 電子羅盤 ... 6

圖 2-8 STH-DCSG 雙眼相機 ... 7

圖 2-9 SRF05 超音波感測器 ... 7

圖 2-10

TARTER

IT

圖 2-11

圖 2-12 移動無人載具整體照片 ... 9

圖 3-1 ECEF 座標系統[15] ... 12

圖 3-2 地面一次差分示意圖 ... 16

圖 3-3 空中一次差分示意圖 ... 16

圖 3-4 二次差分示意圖 ... 17

圖 4-1 去相關化前後座標空間比較示意圖 ... 21

圖 4-2 定位演算程序流程圖 ... 28

圖 5-1 左眼影像[14] ... 29

圖 5-2 視差圖[14] ... 29

圖 5-3 網格示意圖[14] ... 30

圖 5-4 超音波感測示意圖 ... 30

圖 5-5 超音波感測器配置圖 ... 31

圖 5-6 FPGA 對超音波感測器提供之觸發訊號[10] ... 32

圖 6-1 隨機樹連接方式(一)[13] ... 33

圖 6-2 隨機樹連接方式(二)[13] ... 34

圖 6-3 RRT 流程 ... 34

圖 6-4 路徑縮短流程 ... 35

圖 6-5 超音波感測器方向整合示意圖 ... 36

圖 6-6 避障模式整合流程圖 ... 38

圖 7-1 虛擬距離二次差分與 RTK 演算(無 KF 修正)定位結果比較 ... 39

圖 7-2 LAMBDA 定位演算與 KF 修正定位結果比較 ... 40

圖 7-3 動態定位演算實驗結果 ... 41

圖 7-4 載波三次差分路測實驗結果 ... 42

圖 7-5 RTK 演算法路測實驗結果 ... 42

圖 7-6 載波三次差分路測實驗結果(各方向距離對時間) ... 42

圖 7-7 RTK 演算路測實驗結果(各方向距離對時間) ... 43

圖 7-8 雙眼視覺與路徑規劃實驗結果 ... 44

圖 7-9 雙眼視覺與超音波感測整合實驗結果 ... 45

圖 7-10 椰林大道實驗標記位置示意圖 ... 46

圖 7-11 椰林大道實驗中繼點分布圖 ... 47

圖 7-12 椰林大道路測實驗結果 ... 48

表目錄

表 3-1 WGS84 座標系統重要參數 ... 12 表 6-1 超音波感測結果與動作對應表 ... 37

第一章 緒論

1.1 前言

近年來無人載具在各方面都有相當多的運用與探討，從家務操作、自動化運 輸，到軍事用途等，無人控制載具可以達到節省人力的效果；或是進入一些環境 惡劣、對人體有危險的區域進行探索與精密操作，以減少工程安全方面的損失。

為符合工作目標的要求，無人載具需要整合各種不同類型的感測器來解讀所 在的環境，並在環境中搜尋出載具可以移動的路線，讓載具可以避開障礙物並朝 目的地前進，才能成功的讓無人載具順利完成任務。

1.2 文獻回顧

我們過去已經利用雙眼相機以及超音波感測器來偵測當前載具的外部環境，

並利用這些資料來規畫無人載具的路徑以閃避障礙物[10][11][12][14]，除了感測無 人載具外部環境之外，無人載具自身的定位也是重要的一環。全球定位系統(Global Positioning System)的技術已經發展很多年，利用 GPS 衛星訊號定位，我們已可用 載波三次差分法(Carrier-phase Triple Difference)進行定位，精度可到達公分等級[1]，

但因為載波相位的特性，使得載波三次差分再發生周波脫落(Cycle slip)時無法解算，

且要取得初始位置的計算時間也較長，若運用在移動載具中，衛星斷訊會使得載 具需要重新初始化定位計算，穩定性並不佳。

利用調整權重比例，將虛擬距離的快速計算定位、和載波相位的高精度，使 用載波平滑電碼(Carrier Smoothed Code, CSC)可將二種觀測量的優點結合，得到定 位快速且曲線平滑之結果，但定位精度仍然受到電碼觀測量之誤差的限制。

1.3 研究內容簡述

本論文利用無人載具裝設的 GPS 單頻接收機所得之觀測資訊，搭配根據 GPS 觀測模型設計的定位演算法，採用 LAMBDA 法解算二次差分的整數周波未定值，

在衛星收訊良好的情況，可得到精準的載具即時動態定位，定位精度可達數十公 分等級。

我們再進行載具各類感測器的整合，取得無人載具環境參數與載具位置，據 以設計路徑與避障策略，使無人載具能完成通過障礙環境抵達終點的整合設計實 驗。

1.4 論文架構

本論文分為七章節：

第一章：緒論。

第二章：介紹無人載具之系統架構，包括整體系統整合、硬體架構和元件簡介。

第三章：介紹 GPS 系統，以及 GPS 定位方法、差分技術。

第四章：利用 LAMBDA 方法求得周波未定值和卡曼濾波器修正的即時動態定位演 算法設計。

第五章：障礙感測器原理介紹，包括雙眼視覺和超音波測距。

第六章：無人載具的避障系統，包括路徑設計方法和避障模式整合。

第七章：定位實驗結果與分析。

第八章：結論與未來工作。

第二章 載具系統架構

2.1 整體系統架構

無人載具系統是由三個子系統構成：移動載具、參考站和主控站。參考站的 GPS 接收機在實驗開始後即持續接收衛星訊號，提供移動站以進行差分定位。主 控站透過無線網路將參考站的資料傳遞給移動站，並可監控移動站的載具系統的 當前狀況。移動站則透過載具上裝備的各種感測器，感應環境中的障礙物，以及 將障礙物資訊和定位解算結果回送主控站，由主控站規劃出避障路徑，再傳送命 令至移動站執行。整體系統架構如圖 2-1 所示。

圖 2-1 系統架構示意圖

2.2 硬體設備架構

無人載具系統由通訊部分、感測單元、控制單元、運動單元與電源供應單元 構成。

通訊單元負責移動站與主控站電腦之間的資料傳輸。感測單元則是將影像、

超音波測距、衛星資料與載具姿態的資訊提供給控制單元。控制單元負責整合處 理各個感測器的資訊，規劃出路徑後下達命令給運動單元，使載具能追蹤規劃出 來的路徑移動和躲避障礙物。感測單元、控制單元和運動單元所需的電源，皆由

主控站 (主控電腦)

移動站 (載具) 參考站

(GPS 接收機)

載具動態、障礙物 位置、追線資訊

差分用衛星資料、

路徑規劃命令

電源供應單元提供。

圖 2-2 無人載具硬體架構

2.3 元件介紹

無人載具系統所搭配的各個元件簡介如下：

3.5G 無線網卡

使用 D-Link 公司的 DWM-156 3.5G 無線網卡，配合門號可使用 3.5G 無線網 路，最大下載速度約 1.4Mps，最大上傳速度約 800Kps。

圖 2-3 DWM-156 3.5G 無線網卡 Power Supply

UPS Sensors

Motion Communication

3.5G

GPS receiver DG14

Controller IPC

Compass TCM2

Ultrasonic Sensors Stereo

Camera

Control Card MPC-3024

Servo Motor

健昇 MCP3024 運動控制卡

運動控制單元採用健昇科技公司的 MPC-3024 四軸運動控制卡，該控制卡可 以同時控制四組伺服馬達系統，並且提供函式庫可以在 Labview、Visual C++、Visual Basic 的環境之下直接引用，控制卡藉由輸入脈波信號來控制伺服馬達的位置與轉 速。

圖 2-4 MCP3024 運動控制卡 東元電機 TSDA-CB 伺服馬達系統

為無人載具的動力輸出單元，配合健昇 MCP3024 運動控制卡來作運動控制，

除了馬達與驅動器之外，另外配上減速齒輪以降低轉速並且提供更大的扭力來推 動無人載具。

圖 2-5 TSDA-CB 伺服馬達

台達電子不斷電系統

採用台達電子的 GES302N-3KVA 不斷電系統，可持續實驗免充電達 3 小時左 右。若電量完全耗盡，充電 8 小時即可回充至 80%電量。

圖 2-6 GES302N-3KVA 不斷電系統 電子羅盤

使用 PNI Corporation 所生產的 TCM2-50A，TCM2-50 具有三軸量測功能，量 測精度可達到 +/- 1.5 度角，使用 rs232 通訊格式作為傳輸介面，實驗中的傳輸速 率(Baudrate)設定為 9600bps，資料更新頻率為 0.1Hz。

圖 2-7 TCM2-50A 電子羅盤

雙眼相機

採用 Videre Design Inc. 所生產的 STH - DCSG 雙眼相機，資料傳輸介面為 1394a，原廠提供一個 SRI Stereo Engine 程式開發介面，讓使用者可以透過自行編 輯的程式直接呼叫函式庫中的函式來執行各種應用。

圖 2-8 STH-DCSG 雙眼相機 超音波感測器

超音波測距系統使用六個由 Devantech 公司所生產的 SRF05 超音波感測器，

距離量測範圍從 1 公分到 3 公尺，更新頻率最高可達 40Hz，需要配合 FPGA 開 發板將回授訊號解讀後轉譯為 rs232 通訊格式，並將資料送回控制單元，實驗中 所使用的 FPGA 開發板為 Altera 公司所生產的 MAX II Starter Kit 開發套件。

圖 2-9 SRF05 超音波感測器

圖 2-10 MAXII Starter Kit FPGA 開發板

GPS 接收機

採用 Ashtech 公司所生產的 DG14 GPS receiver ，該接收機可以接收單頻 L1 載波的資料，同時間最多可以鎖定 12 顆衛星訊號，提供衛星星曆資料、虛擬距離 量以及載波相位觀測量，資料更新頻率為 1Hz。

圖 2-11 DG14 GPS 接收機

將上面介紹的各項設備元件安裝完成後，移動載具的整體實驗硬體如下圖：

圖 2-12 移動無人載具整體照片

上述之無人載具將在應力館頂樓空地及校園椰林大道進行實驗。

第三章 定位系統

3.1 GPS 定位系統介紹

全球衛星定位系統可分為三部分：太空導航衛星部分、地面監控部分、使用 者接收部分[1]。

3.1.1 太空導航衛星部分

目前 GPS 在太空的衛星有 32 顆，平均分布在六個衛星軌道面上，每個軌道面 之間的夾角為 60 度，和赤道面的夾角為 55 度，軌道距離地表 20183 公里，衛星 繞行軌道的週期為 12 恆星時(Sidereal Time)，即 11 小時 58 分世界時(Universal Time)。

GPS 衛星對外廣播二種電波，分別為 L1(1575.42MHz)載波和 L2(1227.60MHz) 載波。L1 載波上搭載了二種虛擬隨機雜訊碼(Pseudo-Random-Noise code)：C/A 碼 (Coarse/Acquisition code)及 P 碼(Precision code)，在 L2 載波上僅搭載 P 碼。

3.1.2 地面控制部分

地面監控部分主要由 1 個主控站、4 個地面控制站和 6 個監測站組成。

主控制站(Master Control Station；MCS)

位於美國科羅拉多州 Falcon 空軍基地 ， 主控制站之主要功能為接收並處理 監視站所傳回之衛星追縱資料來決定新的星曆、時錶修正量等導航訊息。

地面控制站(Ground Control Station；GCS)

分別位於南大西洋之 Ascension 島、印度洋的 Diego Garcia 、北太平洋的 Kwajalein 與美國佛羅里達州的 Canaveral 角。地面控制站之主要工作為接收主控

站所傳來之修正參數，利用 S 波道電波傳至 GPS 衛星，包括新的星曆、時錶修 正量和其他導航之訊息資料。

監視站(Monitor Station；MS)

地面控制站的位址同時也設置了監視站，除了 Ascension、Diego Garcia、

Kwajalein、Cape Canaveral 之外，另外位於夏威夷和科羅拉多( Colorado Springs ) 二處也有監視站，目的為監視各個 GPS 衛星。每個監視站之位置座標均已經精確 測定，並都備有一準確之銫(Cesium)原子鐘與雙頻 P 碼接收器，每 1.5 秒估算一次 虛擬距離(Pseudo range)，再加上氣象及電離層資料，每 15 分鐘作一次平滑(Smooth) 計算，並將資料傳送至主控制站。

3.1.3 使用者接收部分

即一般使用者可使用的 GPS 接收機，其用途為將 GPS 衛星廣播的訊號轉換成 位置、速度、時間等資料。一般民用的接收機只能接收 C/A 碼和載波相位，軍用 的接收機則多了 P 碼。唯美國國防部基於其安全考量，P 碼於 1994 年調變成 Y 碼，

即所謂的 AS 效應( Anti Spoofing )，使得未經授權的使用者無法得知，降低了一般 民間使用者的定位精度。

3.1.4 GPS 座標系統

GPS 空間系統採用的是 WGS84( World Geodetic System 1984 )座標系統，這是 一個以地球質心為原點之地心地固座標( Earth Centered Earth Fixed, ECEF )系統。

如圖(3-1)所示:

圖 3-1 ECEF 座標系統[15]

其中：

O 點：地球質量中心。

Z 軸：與國際時間局 BIH 定義的傳統地極( Conventional Terrestrial Pole，CTP) 平行之座標軸方向。

X 軸：指向通過格林威治天文子午線的座標軸方向。

Y 軸：與 X 軸、Z 軸呈右手正交系統之方向。

表 3-1 WGS84 座標系統重要參數

參數與說明 數值(單位)

長軸半徑(a) 6378137 (m)

短軸半徑(b) 6356752.314( m)

二分帶係數(J2) 1.082630×10^-9

地球自轉角速度(ωE) 7.292115×10^-11 (rad/sec) 地球重力常數(μ) 3.986005×108 (m³/sec²)

WGS84(

�𝑿𝑿 𝒀𝒀𝒁𝒁� = �

(𝑁𝑁 + ℎ) cos 𝜑𝜑 cos λ (𝑁𝑁 + ℎ) cos 𝜑𝜑 sin λ

�^𝑏𝑏_𝑎𝑎²₂𝑁𝑁 + ℎ� sin φ � (3.1) 其中：

𝑁𝑁 =_{�𝑎𝑎2cos2φ+𝑏𝑏}^𝑎𝑎2 _2sin2φ (3.2) 直角座標(X,Y,Z)轉換成 WGS84(φ, λ, h)橢球座標

�𝝋𝝋 𝝀𝝀𝒉𝒉� =

⎣⎢

⎢⎢

⎡tan^{−1 𝑧𝑧+𝑒𝑒}_{𝑝𝑝−𝑒𝑒}^′22_{𝑎𝑎𝑐𝑐𝑐𝑐𝑠𝑠}^{𝑏𝑏𝑠𝑠𝑠𝑠𝑠𝑠33}_𝜃𝜃^𝜃𝜃 tan^{−1 𝑌𝑌}_𝑋𝑋

𝑝𝑝

cos φ− 𝑁𝑁 ⎦⎥⎥⎥⎤

(3.3)

其中：

𝑝𝑝 = �𝑋𝑋²+ 𝑌𝑌² 𝑒𝑒² =𝑎𝑎² − 𝑏𝑏²

𝑎𝑎² 𝑒𝑒^′2= 𝑎𝑎²− 𝑏𝑏²

𝑏𝑏²

WGS84 直角座標(X,Y,Z)與本地座標系(E,N,U)之轉換關係

此二座標系統之旋轉矩陣 R 包含有圍繞 Z 軸旋轉一個λ 角度，再繞 Y 軸旋轉 一個(90-φ)角度，最後再繞 Z 軸旋轉 90 度。其中 λ 為經度角，φ 為緯度角。可以 得到：

[𝑹𝑹] = � −𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑠𝑠𝑠𝑠 0

−𝑠𝑠𝑠𝑠𝑠𝑠𝜑𝜑𝑐𝑐𝑐𝑐𝑠𝑠𝑠𝑠 −𝑠𝑠𝑠𝑠𝑠𝑠𝜑𝜑𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑠𝑠𝜑𝜑

𝑐𝑐𝑐𝑐𝑠𝑠𝜑𝜑𝑐𝑐𝑐𝑐𝑠𝑠𝑠𝑠 𝑐𝑐𝑐𝑐𝑠𝑠𝜑𝜑𝑠𝑠𝑠𝑠𝑠𝑠𝑠𝑠 𝑠𝑠𝑠𝑠𝑠𝑠𝜑𝜑� (3.4) 因此，WGS84 直角坐標和本地直角坐標的轉換為：

�𝒙𝒙_𝑳𝑳 𝒚𝒚_𝑳𝑳 𝒛𝒛𝑳𝑳

� = [𝑅𝑅] �𝑿𝑿_𝒖𝒖 − 𝑿𝑿_𝒓𝒓 𝒀𝒀𝒖𝒖 − 𝒀𝒀𝒓𝒓

𝒁𝒁𝒖𝒖 − 𝒁𝒁𝒓𝒓

� (3.5)

[xL

, y

, z

[xr

, y

, z

[xu

, y

, z

實驗中參考站的天線已固定在 WGS84 直角坐標系下的[ -3025192.198934 m , 4928782.543095 m, 2681060.943255 m ]，實驗的定位結果皆是以參考站天線位置作 為本地座標原點的計算結果。

3.2 差分定位技術

3.2.1 虛擬距離(Pseudo-range)觀測量模型

虛擬距離觀測量是利用衛星和接收機分別產生的相同電碼，經過相關性分析，

得到電波傳遞所花費的時間差，乘上光速後可得到衛星和接收機之間的虛擬距 離。

訊號傳遞時，除了衛星和接收機所具有的時表誤差外，當載波訊號通過地球 大氣的電離層和對流層，亦會造成延遲誤差，再加上其他未模型化的隨機誤差，

因此利用此觀測量得到的距離並非衛星和接收機之間的真實距離，稱為虛擬距 離。

虛擬距離觀測量的模型為：

𝑃𝑃_𝑢𝑢^𝑘𝑘 = 𝑟𝑟_𝑢𝑢^𝑘𝑘+ 𝑐𝑐[δ𝑡𝑡_𝑢𝑢− 𝛿𝛿𝑡𝑡^𝑘𝑘] + 𝐼𝐼_𝑢𝑢^𝑘𝑘+ 𝑇𝑇_𝑢𝑢^𝑘𝑘+ 𝜀𝜀_{𝑃𝑃,𝑢𝑢}^𝑘𝑘 (3.6) 其中：

𝑃𝑃𝑢𝑢𝑘𝑘：從衛星 k 到接收機 u 的虛擬距離(m) 𝑟𝑟_𝑢𝑢^𝑘𝑘：衛星 k 到接收機 u 的真實距離(m) c：光速(m/sec)

δ𝑡𝑡𝑢𝑢：接收機 u 之時表誤差(sec) 𝛿𝛿𝑡𝑡^𝑘𝑘：衛星 k 之時表誤差(sec) 𝐼𝐼_𝑢𝑢^𝑘𝑘：電離層之延遲誤差(m) 𝑇𝑇𝑢𝑢𝑘𝑘：對流層之延遲誤差(m)

𝜀𝜀_{𝑃𝑃,𝑢𝑢}^𝑘𝑘 ：系統隨機誤差

3.2.2 載波相位(Carrier-phase)觀測量模型

載波相位觀測量是由接收機所接收的衛星訊號之載波相位，與接收機自行產 生的參考相位之差，其模型如下表示：

𝜙𝜙𝑢𝑢𝑘𝑘 = 𝑠𝑠⁻¹�𝑟𝑟𝑢𝑢𝑠𝑠− 𝐼𝐼𝜙𝜙+ 𝑇𝑇𝜙𝜙� +^𝑐𝑐_𝜆𝜆[𝛿𝛿𝑡𝑡𝑢𝑢− 𝛿𝛿𝑡𝑡^𝑠𝑠] + 𝑁𝑁𝑢𝑢𝑘𝑘+ 𝜀𝜀𝜙𝜙 (3.7) 亦可將左右同乘載波波長，使方程式的單位變成長度單位：

Φ𝑢𝑢𝑘𝑘 = 𝑠𝑠 ⋅ 𝜙𝜙𝑢𝑢𝑘𝑘 = 𝑟𝑟𝑢𝑢𝑘𝑘− 𝐼𝐼𝑢𝑢𝑘𝑘+ 𝑇𝑇𝑢𝑢𝑘𝑘+ 𝑐𝑐[𝛿𝛿𝑡𝑡𝑢𝑢 − 𝛿𝛿𝑡𝑡^𝑘𝑘] + 𝑠𝑠𝑁𝑁𝑢𝑢𝑘𝑘+ 𝜀𝜀_{𝜙𝜙,𝑢𝑢}^𝑘𝑘 (3.8) 其中：

Φ_𝑢𝑢^𝑘𝑘：衛星 k 至接收機 u 的載波相位觀測量(m) 𝑁𝑁𝑢𝑢𝑘𝑘：整數周波未定值(Integer Ambiguity) 𝑠𝑠：L1 載波波長(≈ 0.1903m)

3.2.3 差分定位

虛擬距離和載波相位二種觀測量皆含有系統性誤差，因此可以採用相對定位 的方式增加定位精度。相對定位使用二部以上的 GPS 接收機，其中一部放置於已 知座標位置的點位上，稱為參考站，另一部放置於待測座標的點位上，稱為移動 站，若能測得參考站與移動站之間的基線向量，即可得知移動站之座標位置。

依不同的目的，可將基線向量組合成一次差分、二次差分和三次差分，本節 就 GPS 的虛擬距離和載波相位的觀測量分別介紹一次差分和二次差分。

地面一次差分( Single difference between receivers )

在同一時刻 t，參考站 r 和移動站 u 的接收機對同一顆衛星 k 的觀測量作相減 求得差值，即為地面一次差分，如圖(3-2)。

圖 3-2 地面一次差分示意圖

由於同一顆衛星對於此二接收機的觀測量而言，衛星的時表誤差項𝛿𝛿𝑡𝑡^𝑘𝑘的效應 幾乎相同，故經過差分可將之消除，因此載波相位與虛擬距離的差分觀測值之方 程式可表示為：

Φ_{𝑢𝑢𝑢𝑢}^𝑘𝑘 = Φ_𝑢𝑢^𝑘𝑘− Φ_𝑢𝑢^𝑘𝑘 = 𝑟𝑟_{𝑢𝑢𝑢𝑢}^𝑘𝑘 − 𝐼𝐼_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝑇𝑇_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝑐𝑐 ⋅ 𝛿𝛿𝑡𝑡_{𝑢𝑢𝑢𝑢}+ 𝑠𝑠𝑁𝑁_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝜀𝜀_{𝜙𝜙,𝑢𝑢𝑢𝑢}^𝑘𝑘 (3.9) 𝑃𝑃_{𝑢𝑢𝑢𝑢}^𝑘𝑘 = 𝑃𝑃_𝑢𝑢^𝑘𝑘− P_𝑢𝑢^𝑘𝑘 = 𝑟𝑟_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝐼𝐼_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝑇𝑇_{𝑢𝑢𝑢𝑢}^𝑘𝑘 + 𝑐𝑐 ⋅ 𝛿𝛿𝑡𝑡_{𝑢𝑢𝑢𝑢}+ 𝜀𝜀_{𝑃𝑃,𝑢𝑢𝑢𝑢}^𝑘𝑘 (3.10)

空中一次差分( Single difference between satellites )

一接收機 u 在同一時刻 t 接收到不同衛星 k 和 l 的觀測量，將二組觀測量彼此 相減求差值，即可得到空中一次差分值，如圖(3-3)。

圖 3-3 空中一次差分示意圖

和地面一次差分相同，由於同一接收機對於二衛星之間的時表誤差效應δ𝑡𝑡_𝑢𝑢 幾 乎相同，因此差分值可消除接收機的時表誤差項，方程式表示如下：

Φ_𝑢𝑢^{𝑘𝑘𝑘𝑘} = 𝜙𝜙_𝑢𝑢^𝑘𝑘− 𝜙𝜙_𝑢𝑢^𝑘𝑘 = 𝑟𝑟_𝑢𝑢^{𝑘𝑘𝑘𝑘}− 𝐼𝐼_𝑢𝑢^{𝑘𝑘𝑘𝑘}+ 𝑇𝑇_𝑢𝑢^{𝑘𝑘𝑘𝑘}+ 𝑐𝑐 ⋅ 𝛿𝛿𝑡𝑡^{𝑘𝑘𝑘𝑘}+ 𝑠𝑠𝑁𝑁_𝑢𝑢^{𝑘𝑘𝑘𝑘} + 𝜀𝜀_{𝜙𝜙,𝑢𝑢}^{𝑘𝑘𝑘𝑘} (3.11) 𝑃𝑃_𝑢𝑢^{𝑘𝑘𝑘𝑘} = 𝑃𝑃_𝑢𝑢^𝑘𝑘− 𝑃𝑃_𝑢𝑢^𝑘𝑘= 𝑟𝑟_𝑢𝑢^{𝑘𝑘𝑘𝑘} + 𝐼𝐼_𝑢𝑢^{𝑘𝑘𝑘𝑘}+ 𝑇𝑇_𝑢𝑢^{𝑘𝑘𝑘𝑘}+ 𝑐𝑐 ⋅ 𝛿𝛿𝑡𝑡^{𝑘𝑘𝑘𝑘} + 𝜀𝜀_{𝑃𝑃,𝑢𝑢}^{𝑘𝑘𝑘𝑘} (3.12)

衛星 k

接收機 u 接收機 r

衛星 k 衛星 l

接收機 u

二次差分( Double difference )

可以結合上述的地面一次差分和空中一次差分觀測值，得到二次差分值，如 此便可同時消除衛星和接收機的時表誤差，如圖(3-4)。

圖 3-4 二次差分示意圖

除了時表誤差，如果假設參考站跟移動站之間的基線長度在 10 公里以下，對 流層和電離層的延遲誤差對於二接收機之間也可視為相似，在差分值中極小可以 忽略，因此組成二次差分值的量測方程式表示如下：

Φ𝑢𝑢𝑢𝑢𝑘𝑘𝑘𝑘 = 𝑟𝑟𝑢𝑢𝑢𝑢𝑘𝑘𝑘𝑘+ 𝑠𝑠𝑁𝑁𝑢𝑢𝑢𝑢𝑘𝑘𝑘𝑘 + 𝜀𝜀_{𝜙𝜙,𝑢𝑢𝑢𝑢}^{𝑘𝑘𝑘𝑘} (3.13)

P_{𝑢𝑢𝑢𝑢}^{𝑘𝑘𝑘𝑘} = 𝑟𝑟_{𝑢𝑢𝑢𝑢}^{𝑘𝑘𝑘𝑘}+ 𝜀𝜀_{𝑃𝑃,𝑢𝑢𝑢𝑢}^{𝑘𝑘𝑘𝑘} (3.14)

利用相對定位的差分技術，可以有效地將模型中的各項誤差消除，唯有剩下 整數周波未定值 N，下一章即討論 RTK(real-time kinematic)演算法，以解算出此未 知項，達到利用載波相位精確定位的目的。

衛星 k

衛星 l

接收機 u

接收機 r

第四章 即時動態定位演算設計

4.1 LAMBDA 方法

為求得載波相位觀測方程式中的整數周波位定值，本論文使用在 Delft University of Technology 工作的 Prof. Teunissen 所提出之 LAMBDA 方法

(Least-squares AMBiguity Decorrelation Adjustment)作為整數周波未定值搜尋的工 具。[1][2][4][5][7][16]

由於經過二次差分的觀測方程式具有高相關性，因此若要求得正確的整數周 波值則必須處理此相關性的影響。LAMBDA 方法的搜尋原理，即是利用初步預估 的周波值實數解之協方差矩陣(covariance matrix)進行降低相關性的計算，以建立簡 單的搜尋空間，再進行整數搜尋。

求解的步驟可分為：

(1)利用廣義最小平方法(Generalized Least Squares, GLS)求取周波未定值的實數解 𝑎𝑎�以及此實數解的協方差矩陣𝑸𝑸𝒂𝒂�。

(2)利用矩陣的對角化分解和相關演算法可求得一整數轉移矩陣𝐙𝐙，將𝒂𝒂�和𝑸𝑸_𝒂𝒂�轉換成 具有低相關性的𝒛𝒛�和𝑸𝑸𝒛𝒛�。

(3)利用連續條件最小平方(sequential conditional least-squares)的概念，搜尋轉換過 的實數解𝑧𝑧̂，得到整數解𝑧𝑧̌後，再反轉換成原方程式之最佳整數解𝑎𝑎�。

4.1.1 求取周波未定值實數解

若接收機 u 和 r 同時收到 n+1 顆衛星訊號，則觀測值方程組可如下表示：

𝒚𝒚 =

⎝

⎜⎜

⎛ Φ_{𝑢𝑢𝑢𝑢}^1𝑘𝑘 Φ⋮_{𝑢𝑢𝑢𝑢}^{𝑘𝑘𝑘𝑘} 𝑃𝑃𝑢𝑢𝑢𝑢1𝑘𝑘

P_{𝑢𝑢𝑢𝑢}⋮^{𝑘𝑘𝑘𝑘}⎠

⎟⎟

⎞

2n×1

= � 𝑰𝑰^{𝒏𝒏×𝒏𝒏} 𝑮𝑮_{𝒏𝒏×𝟑𝟑} 𝑶𝑶_{𝒏𝒏×𝒏𝒏} 𝑮𝑮_{𝒏𝒏×𝟑𝟑}�

⎝

⎜⎜

⎛ 𝑁𝑁𝑢𝑢𝑢𝑢1𝑘𝑘

𝑁𝑁⋮𝑢𝑢𝑢𝑢𝑘𝑘𝑘𝑘

𝛿𝛿𝛿𝛿𝛿𝛿𝛿𝛿 𝛿𝛿𝑧𝑧 ⎠

⎟⎟

⎞

(𝑠𝑠+3)×1

= 𝑨𝑨𝒂𝒂 (4.1)

其中，G 為各個衛星視線(line-of-sight)向量組合而成的幾何矩陣：

𝑮𝑮𝑠𝑠×3 =

⎣⎢

⎢⎡−𝟏𝟏^{𝑢𝑢1𝑘𝑘}

−𝟏𝟏_𝑢𝑢^2𝑘𝑘

−𝟏𝟏⋮_𝑢𝑢^{𝑘𝑘𝑘𝑘}⎦⎥⎥⎤

𝑠𝑠×3

(4.2)

𝟏𝟏𝑢𝑢𝑘𝑘𝑘𝑘 = 𝟏𝟏𝑢𝑢𝑘𝑘− 𝟏𝟏𝑢𝑢𝑘𝑘為接收機 r 分別指向衛星 k 和衛星 l 的單位方向向量之差。

利用廣義最小平方法，可以估計出周波未定值和基線向量的實數解與協方差 矩陣：

𝒂𝒂� = (𝑨𝑨^𝑇𝑇𝑾𝑾𝑨𝑨)⁻¹𝑨𝑨^𝑇𝑇𝑾𝑾𝒚𝒚 (4.3) 𝑸𝑸𝒂𝒂� = (𝑨𝑨^𝑇𝑇𝑾𝑾𝑨𝑨)⁻¹𝑨𝑨^𝑇𝑇𝑾𝑾𝑹𝑹𝑾𝑾𝑨𝑨(𝑨𝑨^𝑇𝑇𝑾𝑾𝑨𝑨)⁻¹ (4.4) R 為觀測值 y 之協方差矩陣，W 為權重矩陣，假設載波相位和虛擬距離觀測量之

間互不相關，依照誤差傳遞定律，R 和 W 表示成如下：

𝑹𝑹 = �𝑪𝑪^𝜱𝜱 𝑶𝑶

𝑶𝑶 𝑪𝑪𝑷𝑷� = 𝑾𝑾⁻¹ (4.5)

其中

𝑪𝑪_Φ = σ_Φ² �

4 22 4 ⋯ 2

⋮ ⋱ ⋱ ⋮

2 ⋯ ⋱ 2 2 4

�，𝑪𝑪_P = σ_P²�

4 22 4 ⋯ 2

⋮ ⋱ ⋱ ⋮

2 ⋯ ⋱ 2 2 4

� (4.6)

σ_Φ²和σ_Φ²分別是電碼觀測量和載波觀測量之誤差變異量，其值在此設定為 1.0 和 0.06 公尺²。

4.1.2 低相關化轉換

欲將上節計算所得之實數解進行去相關性化的轉換，可先將𝑄𝑄𝑎𝑎�進行對角化分 解，將𝑄𝑄_𝑎𝑎�分解成一個下三角矩陣 L 與一個對角矩陣 D 的組合：

𝑸𝑸_𝒂𝒂�= 𝑳𝑳 𝑫𝑫 𝑳𝑳^𝑇𝑇 因此，去相關化轉換可視為對角化轉換：

𝑸𝑸_𝒛𝒛� = 𝑳𝑳⁻¹ 𝑸𝑸_𝒂𝒂� 𝑳𝑳^−𝑇𝑇 = 𝑫𝑫 𝒛𝒛� = 𝑳𝑳⁻¹ 𝒂𝒂�

但因為所求的周波未定值需為整數解，因此轉換矩陣必須是一個整數矩陣，故

LAMBDA 法將 Generalized Least-squares 所求得的實數協方差矩陣𝑸𝑸_𝑎𝑎�作對角分解 後，使用連續條件最小平方(sequential conditional least-squares)的概念，求得整數矩 陣 Z，使得Z^𝑇𝑇~𝐿𝐿⁻¹，則：

𝑸𝑸_𝒛𝒛�= 𝒁𝒁^𝑇𝑇 𝑸𝑸_𝒂𝒂� 𝒁𝒁^−𝑇𝑇~𝑫𝑫

連續條件最小平方的概念，是將每項實數值依次地消除其它項實數所造成的 相關變化量。其做法為：保留最後一個周波值的估計𝑎𝑎�𝑠𝑠不變，將倒數第二個周波 值𝑎𝑎�_𝑠𝑠−1進行去相關化，以𝑎𝑎�_𝑠𝑠修正為整數值𝑎𝑎�_𝑠𝑠作為條件，消除𝑎𝑎�_𝑠𝑠修正後對𝑎𝑎�_𝑠𝑠−1造成 的相關性變動。再對倒數第三項周波值𝑎𝑎�𝑠𝑠−2進行去相關化，以𝑎𝑎�𝑠𝑠−1和𝑎𝑎�𝑠𝑠修正為整 數值𝑎𝑎�_𝑠𝑠−1和𝑎𝑎�_𝑠𝑠作為條件，消除後二項周波值造成的相關性。依次由末項往首項進 行調整，最後可推導成：

𝑎𝑎�_𝑠𝑠 = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟(𝑎𝑎�_𝑠𝑠)

𝑎𝑎�_𝑠𝑠−1= 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟�𝑎𝑎�_{𝑠𝑠−1|𝑠𝑠}� = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟 �𝑎𝑎�_𝑠𝑠−1−σ𝑎𝑎�_𝑛𝑛−1𝑎𝑎�_𝑛𝑛

𝜎𝜎_{𝑎𝑎�𝑛𝑛}² (𝑎𝑎�_𝑠𝑠 − 𝑎𝑎�_𝑠𝑠)�

⋮

𝑎𝑎�_𝑗𝑗 = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟�𝑎𝑎�_𝑗𝑗|J� = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟 �𝑎𝑎�_𝑗𝑗− � σ𝑎𝑎�_𝑗𝑗𝑎𝑎�_{𝑖𝑖|𝐼𝐼}

𝜎𝜎𝑎𝑎�_{𝑖𝑖|𝐼𝐼}2 �𝑎𝑎�_{𝑠𝑠|𝐼𝐼}− 𝑎𝑎�_𝑠𝑠�

𝑠𝑠 𝑠𝑠=𝑗𝑗+1

� (4.7)

⋮

𝑎𝑎�1 = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟�𝑎𝑎�1|2,3…𝑠𝑠� = 𝑟𝑟𝑐𝑐𝑟𝑟𝑠𝑠𝑟𝑟 �𝑎𝑎�1− �σ_{𝑎𝑎�1𝑎𝑎�𝑖𝑖|𝐼𝐼}

𝜎𝜎𝑎𝑎�_{𝑖𝑖|𝐼𝐼}2 �𝑎𝑎�𝑠𝑠|𝐼𝐼− 𝑎𝑎�𝑠𝑠�

𝑠𝑠 𝑠𝑠=2

�

式(4.7)為此修正式的通式。𝑎𝑎�𝑗𝑗|𝐽𝐽為第 i 項周波值，以最後 i-1 項是整數值作為修正條 件，將之間的相關性消除後的修正估計值，其中的 J 代表第 j+1, j+2,…,n 項周波值 的集合，I 代表地 i+1,i+2,…,n 項周波值的集合，N 則代表所有周波值的集合，σ_{𝑎𝑎�𝑗𝑗𝑎𝑎�𝑖𝑖|𝐼𝐼}

則是與𝑎𝑎�_{𝑠𝑠|𝐼𝐼}對應的共變數。

依照上述方法，周波未定值實數解與整數解之差�𝑎𝑎�_𝑗𝑗− 𝑎𝑎�_𝑗𝑗�可以用修正實數解 𝑎𝑎�_𝑗𝑗|J取代為：

�𝑎𝑎�_𝑗𝑗 − 𝑎𝑎�_𝑗𝑗� = �𝑎𝑎�_{𝑗𝑗|𝐽𝐽}− 𝑎𝑎_𝑗𝑗� + ∑ ^𝜎𝜎𝑎𝑎�𝑗𝑗𝑎𝑎�𝑖𝑖|𝐼𝐼

𝜎𝜎_{𝑎𝑎�𝑖𝑖|𝐼𝐼}² �𝑎𝑎�_{𝑠𝑠|𝐼𝐼}− 𝑎𝑎_𝑠𝑠�

𝑠𝑠𝑠𝑠=𝑗𝑗+1 (4.8)

若以矩陣形式表示，則為：

(𝒂𝒂� − 𝒂𝒂�) = 𝑳𝑳^′ �𝒂𝒂�_{𝑗𝑗|𝐽𝐽}− 𝒂𝒂�� (4.9) 𝑸𝑸𝑎𝑎� = 𝐸𝐸[(𝒂𝒂� − 𝒂𝒂�)(𝒂𝒂� − 𝒂𝒂�)^𝑇𝑇] = 𝑳𝑳^′�𝒂𝒂�𝑗𝑗|𝐽𝐽− 𝒂𝒂���𝒂𝒂�𝑗𝑗|𝐽𝐽− 𝒂𝒂��^𝑇𝑇𝑳𝑳^′𝑇𝑇 = 𝑳𝑳^′ 𝑫𝑫^′ 𝑳𝑳^′𝑇𝑇 (4.10) 其中，

𝑫𝑫^′= 𝑟𝑟𝑠𝑠𝑎𝑎𝑑𝑑�σ𝑎𝑎�_{1|2,3…𝑛𝑛}2 ⋯ σ𝑎𝑎�_{𝑛𝑛−1|𝑛𝑛}2 𝜎𝜎_{𝑎𝑎�𝑛𝑛}²� (4.11)

𝑳𝑳^′=

⎣⎢

⎢⎢

⎢⎡ 1

𝜎𝜎_{𝑎𝑎�1𝑎𝑎�1|𝑁𝑁}𝜎𝜎_{𝑎𝑎�1|𝑁𝑁}² 1

⋮ ⋱

𝜎𝜎_{𝑎𝑎�1𝑎𝑎�𝑛𝑛−1|𝑛𝑛}𝜎𝜎_{𝑎𝑎�𝑛𝑛−1|𝑛𝑛}² … 𝜎𝜎_{𝑎𝑎�1𝑎𝑎�𝑛𝑛}𝜎𝜎_{𝑎𝑎�𝑛𝑛}² …

𝜎𝜎_{𝑎𝑎�𝑛𝑛−2𝑎𝑎�𝑛𝑛−1|𝑛𝑛}1 𝜎𝜎_{𝑎𝑎�𝑛𝑛−1|𝑛𝑛}² 1

𝜎𝜎_{𝑎𝑎�𝑛𝑛−2𝑎𝑎�𝑛𝑛}𝜎𝜎_{𝑎𝑎�𝑛𝑛}² 𝜎𝜎_{𝑎𝑎�𝑛𝑛−1𝑎𝑎�𝑛𝑛}𝜎𝜎_{𝑎𝑎�𝑛𝑛}² 1 ⎦⎥⎥⎥⎥⎤

(4.12)

而整數轉換矩陣 Z 便以此修正過之實數轉換矩陣𝑳𝑳^′為基礎，搭配行列互換等手法，

使 Z^T近似於 L^-1，且行列式大小為 1，相關演算法如附錄所示。

低相關化的轉移計算可以使周波值的誤差橢球座標變得更接近正球狀，進而 提高整數搜尋的效率以及精度，如圖(4-1)所示：

圖 4-1 去相關化前後座標空間比較示意圖

求得將𝒂𝒂�和𝑸𝑸𝑎𝑎�代入轉換矩陣 Z，得到一低相關性的實數解和協方差矩陣：

𝒛𝒛� = 𝒁𝒁^𝑇𝑇 𝒂𝒂� (4.13)

𝑸𝑸𝑧𝑧̂ = 𝒁𝒁^𝑇𝑇 𝑸𝑸𝑎𝑎� 𝒁𝒁 (4.14) 再以轉換後的數值執行整數搜尋，求得轉換後的周波未定值整數解。

4.1.3整數搜尋

搜尋的動作是利用轉換計算後所得之𝒛𝒛�與𝑸𝑸_𝑧𝑧̂組成下式，將搜尋所得之整數格點 𝒛𝒛代入式中判斷是否為所求之最佳解：

𝐹𝐹(𝒛𝒛) = (𝒛𝒛� − 𝒛𝒛)^𝑇𝑇 𝑸𝑸_𝑧𝑧̂⁻¹ (𝒛𝒛� − 𝒛𝒛) ≤ 𝜒𝜒² (4.15) 其中，𝜒𝜒²為判斷式之邊界，需要自行設定大小。

透過對角化分解，上式可改寫成：

(𝒛𝒛� − 𝒛𝒛)^𝑇𝑇 𝑸𝑸𝑧𝑧̂−1 (𝒛𝒛� − 𝒛𝒛) = (𝒛𝒛� − 𝒛𝒛)^𝑇𝑇 𝑳𝑳⁻¹ 𝑫𝑫⁻¹ 𝑳𝑳^−𝑇𝑇 (𝒛𝒛� − 𝒛𝒛) 利用連續條件最小平方，由式(4.8)與(4.9)，最後可得：

𝐹𝐹(𝒛𝒛) = ∑ ^{�𝑧𝑧𝑖𝑖}_𝜎𝜎^{−𝑧𝑧̂𝑖𝑖|𝐼𝐼�2}

𝑧𝑧�𝑖𝑖|𝐼𝐼2

𝑠𝑠𝑠𝑠=1 ≤ χ² (4.16)

此橢球的搜尋範圍是依靠式(4.15)中的χ²來控制的，若能取得一個適當的橢球 範圍，可使整數搜尋的效率提高，而本論文的搜尋策略是收縮橢球搜尋法(shrinking ellipsoid technique)，初始時先將χ²設定為無限大，隨著整數解的搜尋持續縮小橢球 邊界，直到邊界內部之整數格點符合所需。

假設整數搜尋的需要搜尋 p 組最佳整數解，記其解為 z¹，z²…z^p。第一組 z¹ 為依照前述的連續條件最小平方的概念，從末項元素開始依序消除相關性得到一 組整數向量，即是整數最佳解 z¹；其餘 p-1 組解為將向量第一元素分別替換成第二、

第三…第 p 接近整數解所得的整數向量。所以，在第一步我們可得到 p 組整數解，

進行排序使得𝐹𝐹(𝒛𝒛¹) ≤ 𝐹𝐹(𝒛𝒛²) ≤ ⋯ ≤ 𝐹𝐹(𝒛𝒛^𝑝𝑝)，此時若𝐹𝐹(𝒛𝒛^𝑝𝑝) ≤ χ²，則令χ² = 𝐹𝐹(𝒛𝒛^𝑝𝑝)，

使橢球邊界收縮，且確定新橢球中必包含 p 組整數解。

第二步，將第二項元素設定為第二接近的整數值，再次利用連續條件最小平 方法計算第一項的整數解，若其中有一組整數解在橢球邊界之內，則重新將𝐹𝐹(𝑧𝑧)的 大小排序，並將χ²設定為𝐹𝐹(𝒛𝒛^𝑝𝑝)。重複上述步驟，直到橢球縮小到空間中剩下 p 組 解為止。

當得到整數解𝑧𝑧̌，則可以利用矩陣 Z 反轉換成原始的座標空間𝒂𝒂�：

𝒂𝒂� = (𝒁𝒁^𝑇𝑇)⁻¹ 𝒛𝒛� (4.17)

4.2 解算位置修正

載具行進為連續性的運動，因此其定位結果為平滑且極少跳動。在使用 LAMBDA 方法求得整數周波未定值，解算載波相位觀測量之基線向量後，利用卡 曼濾波器(Kalman Filter)修正基線向量使其定位更符合載具需求[2][3][8][9]。

4.2.1 位置解算

當周波未定值已知，由式(3.13)，可解算基線向量：

𝚽𝚽_{𝒅𝒅𝒅𝒅}= 𝒓𝒓_{𝒅𝒅𝒅𝒅}+ 𝑠𝑠_𝐿𝐿1∙ 𝑵𝑵_{𝒅𝒅𝒅𝒅}+ 𝜺𝜺_{𝒅𝒅𝒅𝒅} (3.13) 其中，𝒓𝒓𝒅𝒅𝒅𝒅可表示成𝑮𝑮 ∙ 𝛅𝛅𝒙𝒙，𝑮𝑮即為式(4.2)中的衛星視線單位矩陣，𝛅𝛅𝒙𝒙為 WGS84 座 標下的基線向量( x , y , z )，因此可得單一時刻下由 GPS 觀測量所得之相對定位：

𝛅𝛅𝒙𝒙 = (𝑮𝑮^𝑇𝑇 𝑮𝑮)⁻¹ (𝑮𝑮^𝑇𝑇 𝒓𝒓𝒅𝒅𝒅𝒅) = (𝑮𝑮^𝑇𝑇 𝑮𝑮)⁻¹ [𝑮𝑮^𝑇𝑇 (𝚽𝚽𝒅𝒅𝒅𝒅− 𝑠𝑠𝐿𝐿1 ∙ 𝑵𝑵𝒅𝒅𝒅𝒅)] (4.18)

4.2.2 卡曼濾波器遞迴程序

卡曼濾波器是一種可以用來處理系統和量測的雜訊問題，擁有遞迴特性的演 算法。只要根據當前的觀測量和前一時刻的狀態估計，不需要保存過去的觀測數 值，能使估測的系統逼近真實狀態。卡曼濾波器的方程式包含狀態(state)方程式和 量測(measurement)方程式：

𝑿𝑿𝑘𝑘+1 = 𝚽𝚽𝑘𝑘𝑿𝑿𝑘𝑘+ 𝛚𝛚𝑘𝑘 (4.19) 𝒁𝒁_𝑘𝑘 = 𝑯𝑯_𝑘𝑘𝑿𝑿_𝑘𝑘+ 𝛖𝛖_𝑘𝑘 (4.20) 其中，

𝑿𝑿_𝑘𝑘：時刻 k 的狀態向量 𝚽𝚽𝑘𝑘：時刻 k 的狀態轉移矩陣 𝒁𝒁_𝑘𝑘：時刻 k 的量測向量 𝑯𝑯𝑘𝑘：時刻 k 的量測矩陣

𝛚𝛚_𝑘𝑘：系統誤差，為高斯白雜訊，平均值 E[ω]=0；協方差矩陣 E[ωω^T]=Q。

𝛖𝛖_𝑘𝑘：量測誤差，為高斯白雜訊，平均值 E[ν]=0；協方差矩陣 E[νν^T]=R。

設定好狀態和量測方程式的各項參數(𝚽𝚽、H、Q、R)後，卡曼濾波器的遞迴運 算程序如下：

(1) 設定初始狀態：

𝑿𝑿�₀(+) = 𝐸𝐸[𝑿𝑿₀] (4.21)

𝑷𝑷0(+) = 𝐸𝐸[(𝑿𝑿�0− 𝑿𝑿0)(𝑿𝑿�0− 𝑿𝑿0)^𝑇𝑇] (4.22) (2) 初步預估下一時刻的狀態向量和協方差矩陣：

𝑿𝑿�𝑘𝑘(−) = 𝚽𝚽𝑘𝑘−1𝑿𝑿�𝑘𝑘−1(+) (4.23) 𝑷𝑷𝑘𝑘(−) = 𝚽𝚽𝑘𝑘−1𝑷𝑷𝑘𝑘−1(+)𝚽𝚽𝑘𝑘−1𝑇𝑇+ 𝑸𝑸𝑘𝑘−1 (4.24) (3) 計算卡曼增益矩陣(Kalman Gain)：

𝑲𝑲_𝑘𝑘 = 𝑷𝑷_𝑘𝑘(−)𝑯𝑯𝑘𝑘𝑇𝑇�𝑯𝑯_𝑘𝑘𝑷𝑷_𝑘𝑘(−)𝑯𝑯𝑘𝑘𝑇𝑇+ 𝑹𝑹_𝑘𝑘�⁻¹ (4.25) (4) 更新狀態向量和協方差矩陣：

𝑿𝑿�_𝑘𝑘(+) = 𝑿𝑿�_𝑘𝑘(−) + 𝑲𝑲_𝑘𝑘�𝒁𝒁_𝑘𝑘− 𝑯𝑯_𝑘𝑘𝑿𝑿�_𝑘𝑘(−)� (4.26) 𝐏𝐏_𝑘𝑘(+) = (𝑰𝑰 − 𝑲𝑲_𝑘𝑘𝑯𝑯_𝑘𝑘)𝐏𝐏_𝑘𝑘(−) (4.27) (5) 回到步驟(2)

式(4.23)和式(4.24)中的(−)表示未更新的參數預估值；式(4.26)和式(4.27)的(+) 為加入量測更新的參數預估。

4.2.3 狀態方程式

為能描述載具移動的情況，假設載具為一個以等加速度的狀態移動之物體，

在加速度項有雜訊 w(t)，將離散時間的狀態方程式表示為：

𝐗𝐗_𝑘𝑘+1 = 𝚽𝚽_𝑘𝑘𝑿𝑿_𝑘𝑘+ 𝝎𝝎_𝑘𝑘

⎝

⎜⎜

⎜⎜

⎜⎛ 𝑝𝑝𝑥𝑥

𝑝𝑝_𝑦𝑦 𝑝𝑝_𝑧𝑧 𝑣𝑣_𝑥𝑥 𝑣𝑣_𝑦𝑦 𝑣𝑣_𝑧𝑧 𝑎𝑎_𝑥𝑥 𝑎𝑎𝑦𝑦

𝑎𝑎𝑧𝑧⎠

⎟⎟

⎟⎟

⎟⎞

𝑘𝑘+1

= �𝑰𝑰_3×3 ∆𝑡𝑡 ∙ 𝑰𝑰_3×3 ¹₂∙ ∆𝑡𝑡²∙ 𝑰𝑰_3×3 𝑶𝑶3×3 𝑰𝑰3×3 ∆𝑡𝑡 ∙ 𝑰𝑰3×3

𝑶𝑶3×3 𝑶𝑶3×3 𝑰𝑰3×3

�

⎝

⎜⎜

⎜⎜

⎜⎛ 𝑝𝑝𝑥𝑥

𝑝𝑝_𝑦𝑦 𝑝𝑝_𝑧𝑧 𝑣𝑣_𝑥𝑥 𝑣𝑣_𝑦𝑦 𝑣𝑣_𝑧𝑧 𝑎𝑎_𝑥𝑥 𝑎𝑎𝑦𝑦

𝑎𝑎𝑧𝑧⎠

⎟⎟

⎟⎟

⎟⎞

𝑘𝑘

+ 𝝎𝝎_𝑘𝑘 (4.28)

其中，𝑝𝑝表示位置項、𝑣𝑣為速度項、𝑎𝑎為加速度項。

系統誤差𝝎𝝎𝑘𝑘之協方差矩陣Ｑ

𝑘𝑘，經過離散化後可表示為：

Ｑ𝑘𝑘 = 𝐸𝐸[𝝎𝝎_𝑘𝑘𝝎𝝎_𝑘𝑘^𝑇𝑇] = 𝜎𝜎_𝑎𝑎²

⎣⎢

⎢⎢

⎡^∆𝑡𝑡₂₀⁵∙ 𝑰𝑰3×3 ∆𝑡𝑡⁴

8 ∙ 𝑰𝑰3×3 ∆𝑡𝑡³ 6 ∙ 𝑰𝑰3×3

∆𝑡𝑡⁴

8 ∙ 𝑰𝑰_3×3 ^∆𝑡𝑡₃³∙ 𝑰𝑰_3×3 ^∆𝑡𝑡₂²∙ 𝑰𝑰_3×3

∆𝑡𝑡³

6 ∙ 𝑰𝑰3×3 ∆𝑡𝑡²

2 ∙ 𝑰𝑰3×3 𝑰𝑰3×3 ⎦⎥⎥⎥⎤

(4.29)

𝜎𝜎_𝑎𝑎²為在連續時間下系統誤差的加速度項之常態白噪音變異數；∆𝑡𝑡為時刻 k 到時刻 k+1 的時間差。

4.2.4 量測方程式

利用都卜勒頻移觀測量(Doppler shift measurement)，可計算得接收機的移動速 度，其觀測量公式如下：

𝑠𝑠_𝐿𝐿1 ∙ 𝐃𝐃_𝑢𝑢^𝑘𝑘+ 𝟏𝟏_𝑢𝑢^𝑘𝑘∙ 𝒗𝒗^𝑘𝑘= 𝟏𝟏_𝑢𝑢^𝑘𝑘∙ 𝒗𝒗_𝑢𝑢− 𝜷𝜷 + 𝜺𝜺 (4.30) 其中：

𝐃𝐃_𝑢𝑢^𝑘𝑘：接收機 u 收到衛星 k 的都卜勒頻移觀測值，單位是赫茲 𝟏𝟏𝑢𝑢𝑘𝑘：接收機 u 至衛星 k 的單位方向向量

𝒗𝒗^𝑘𝑘：衛星 k 的移動速度，可由星曆資料換算得到 𝒗𝒗_𝑢𝑢：接收機 u 的移動速度

𝜷𝜷：衛星與接收機的時表誤差 𝜺𝜺：未模型化之誤差

都卜勒頻移觀測量經過二次差分後，衛星速度與誤差項可消除，因此將都卜 勒頻移觀測量加入量測向量後，量測方程式為：

𝒁𝒁_𝑘𝑘 = 𝑯𝑯_𝑘𝑘𝑿𝑿_𝑘𝑘+ 𝒗𝒗_𝑘𝑘

�𝚽𝚽^{𝒅𝒅𝒅𝒅}− 𝑠𝑠_𝐿𝐿1∙ 𝑵𝑵_{𝒅𝒅𝒅𝒅} 𝑠𝑠_𝐿𝐿1∙ 𝑫𝑫_{𝒅𝒅𝒅𝒅} �

2𝑠𝑠×1 = �Ｇ

𝑠𝑠×3 Ｏ

𝑠𝑠×3 Ｏ

𝑠𝑠×3

Ｏ𝑠𝑠×3 Ｇ

𝑠𝑠×3 Ｏ

𝑠𝑠×3

�

⎝

⎜⎜

⎜⎜

⎜⎛ 𝑝𝑝𝑥𝑥

𝑝𝑝𝑦𝑦

𝑝𝑝_𝑧𝑧 𝑣𝑣_𝑥𝑥 𝑣𝑣_𝑦𝑦 𝑣𝑣_𝑧𝑧 𝑎𝑎_𝑥𝑥 𝑎𝑎𝑦𝑦

𝑎𝑎𝑧𝑧⎠

⎟⎟

⎟⎟

⎟⎞

+ 𝒗𝒗_𝑘𝑘 (4.31)

其中：

𝚽𝚽_{𝒅𝒅𝒅𝒅}：載波相位觀測量的二次差分值

𝑵𝑵𝒅𝒅𝒅𝒅：在 4.1 節由 LAMBDA 方法搜尋得到之二次差分整數周波未定值 𝑫𝑫_{𝑑𝑑𝑑𝑑}：都卜勒頻移觀測量的二次差分值

G：由式(4.2)得到的衛星幾何矩陣。

量測誤差𝑣𝑣_𝑘𝑘的協方差矩陣𝑹𝑹_𝑘𝑘可表示為：

𝑹𝑹_𝑘𝑘 = 𝐸𝐸[𝒗𝒗_𝑘𝑘𝒗𝒗_𝑘𝑘^𝑇𝑇] = �𝑪𝑪^Φ 𝑂𝑂 𝑂𝑂 𝑪𝑪_𝑃𝑃�

2𝑠𝑠×2𝑠𝑠

4.2.5 強健型卡曼濾波器(Robust Kalman Filter)[8]

量測方程式表示為：

𝒁𝒁_𝑘𝑘 = 𝑯𝑯_𝑘𝑘𝑿𝑿_𝑘𝑘+ 𝛖𝛖_𝑘𝑘 (4.20) 令殘值(residual)為𝒆𝒆𝒌𝒌,𝒊𝒊 = 𝒁𝒁𝑘𝑘,𝑠𝑠 − 𝒉𝒉𝑘𝑘,𝑠𝑠 ∙ 𝑿𝑿𝑘𝑘，可將方程式改寫成：

∑ 𝑓𝑓(^𝑠𝑠_𝑠𝑠 𝒁𝒁𝑘𝑘,𝑠𝑠 − 𝒉𝒉𝑘𝑘,𝑠𝑠 ⋅ 𝑿𝑿𝑘𝑘) = ∑ 𝑓𝑓(𝒆𝒆^𝑠𝑠_𝑠𝑠 𝑘𝑘,𝑠𝑠) (4.32) 其中：

𝑓𝑓(∙)為一純量，代表所有殘值(residuals)的加權總和 i：表示觀測數目

n：在時刻 k 觀測量的維度

𝒁𝒁𝑘𝑘,𝑠𝑠：在時刻 k 時𝒁𝒁𝑘𝑘的第 i 個元素

𝒉𝒉_{𝑘𝑘,𝑠𝑠}：𝑯𝑯_𝑘𝑘的第 i 列向量

為使總和的𝑓𝑓(∙)最小，將(4.32)對𝑿𝑿𝑘𝑘偏微分，得：

∑ 𝜑𝜑(^𝑠𝑠_𝑠𝑠 𝒁𝒁_{𝑘𝑘,𝑠𝑠} − 𝒉𝒉_𝑠𝑠⋅ 𝑿𝑿�_𝑘𝑘)ℎ_𝑠𝑠 = ∑ 𝜑𝜑(𝒆𝒆�^𝑠𝑠_{𝑠𝑠 𝑠𝑠}) = 0 (4.33) 其中𝜑𝜑 = 𝑓𝑓′(∙)，上式可寫成：

∑ 𝒉𝒉𝑠𝑠𝒆𝒆�𝑠𝑠𝜑𝜑(𝒆𝒆�_𝑖𝑖) 𝒆𝒆�_𝑖𝑖

𝑠𝑠𝑠𝑠 = 0 (4.34)

令^{𝜑𝜑(𝒆𝒆�𝑖𝑖)}

𝒆𝒆�_𝑖𝑖 = 𝐷𝐷(𝒆𝒆�𝑠𝑠)，上式可改寫成：

∑ 𝒉𝒉^𝑠𝑠_{𝑠𝑠 𝑠𝑠}𝒆𝒆�_𝑠𝑠𝐷𝐷(𝒆𝒆�_𝑠𝑠)= 0 (4.35) 其中𝐷𝐷(𝒆𝒆�𝑠𝑠)可用來調整殘值的權重。

在利用 LAMBDA 法求周波未定值整數解時，如果因為當時刻下的虛擬距離觀 測量誤差過大或其他因素使得整數搜尋之最佳解品質不佳，單純使用 LAMBDA 法 來解算位置會產生過大的跳動，不利於我們載具運動的控制，因此我們希望能有 一個平滑的定位結果。

為使載具位置不會因量測值的差距過大造成定位跳動，因此將𝐷𝐷(𝒆𝒆�_𝑠𝑠)設為：

𝐷𝐷(𝒆𝒆�_𝑠𝑠) = � 1 𝑠𝑠𝑓𝑓 |𝒆𝒆�𝑠𝑠| < 𝑐𝑐 10 𝑠𝑠𝑓𝑓 𝑐𝑐 ≤ |𝒆𝒆�𝑠𝑠| < 𝑟𝑟

100 𝑠𝑠𝑓𝑓 |𝒆𝒆�_𝑠𝑠| ≥ 𝑟𝑟 � (4.36) 其中 c 與 d 為常數閥值。當殘值過大的時候，利用上式重新調整卡曼濾波器中狀 態方程式與量測方程式之間的加權分配。

根據載具的動態模型並結合(4.35)，將卡曼濾波器重新表示成具有權重之強健 統計數值(robust statistics)，並將量測更新流程的算式改為[6]：

𝑲𝑲_𝑘𝑘 = 𝑷𝑷_𝑘𝑘(−)𝑯𝑯𝑘𝑘𝑇𝑇�𝑯𝑯_𝑘𝑘𝑷𝑷_𝑘𝑘(−)𝑯𝑯𝑘𝑘𝑇𝑇+ 𝐷𝐷(𝒆𝒆�_𝑠𝑠)𝑹𝑹𝑘𝑘𝐷𝐷(𝒆𝒆�_𝑠𝑠)�⁻¹ (4.37) 𝑿𝑿�_𝑘𝑘(+) = 𝑿𝑿�𝑘𝑘(−) + 𝑲𝑲𝑘𝑘𝐷𝐷(𝒆𝒆�_𝑠𝑠) �𝒁𝒁𝑘𝑘− 𝑯𝑯_𝑘𝑘𝑿𝑿�_𝑘𝑘(−)� (4.38) 𝐏𝐏𝑘𝑘(+) = (𝑰𝑰 − 𝑲𝑲𝑘𝑘𝑯𝑯𝑘𝑘)𝐏𝐏𝑘𝑘(−) (4.27)

4.3 定位演算法流程

整合本章各節所述的演算步驟，即時動態定位演算法之解算流程可整理為下 圖(4-2)：

圖 4-2 定位演算程序流程圖

在載具系統的控制單元呼叫系統中的定位程式時，便會進行上圖的演算流程，

解算成功後便會輸出解算位置。

定位程序開始

參考站觀測值 移動站觀測值

二次差分

GLS 計算 周波值實數解

LAMBDA 計算 周波值整數解

卡曼濾波器修正定位

結束定位程序，

等待下次衛星資料更新 接收衛星訊號

衛星顆數 ≥4

No Yes

第五章 障礙偵測系統

載具的測障系統由[12]設計，其中包含雙眼視覺感測與超音波感測。而[14]利 用此二系統的資訊整合成可因應障礙物變化而反應的避障系統，見第六章。

5.1 雙眼視覺系統

本研究使用的雙眼視覺系統採用 Videre Design 公司之 STH-DCSG 雙眼相機。

STH-DCSG 為數位式立體影像鏡頭，IEEE1394 介面，相機擷取的影像解析度為 640x480，畫面傳輸速率為 30fps。Videre Design 提供了 SRI Stereo Engine 的程式開 發介面(Application Programming Interface, API)，使用者可以透過自行編撰的程式 碼呼叫函式庫中測距的工具函式來測量障礙物的距離，也可搭配使用者依照環境 需求所設計的輔助程式，如自動曝光模組，來使測距功能達到最佳的效果[14]。

此系統將載具前方六公尺、左右各五公尺的 6x10 長方形範圍劃分成 60 個 1 公尺 x1 公尺的網格。當程式呼叫測距函式時，函式擷取拍攝畫面，透過視差影像 計算出每個像素在相機座標中的位置。而將計算出位置的像素放置在各自的網格 中，當某格網格中的像素數量超過一定數目，且像素的平均高度超過一閥值，即 定義該網格位置為障礙物。如圖所示。

圖 5-1 左眼影像[14] 圖 5-2 視差圖[14]

圖 5-3 網格示意圖[14]

5.2 超音波系統

5.2.1 超音波測距原理

音波傳遞至物體表面會反射回音，因此可透過計算音波發射和接收回音的時 間差計算障礙物至感測器之間的距離。超音波為振動頻率超過人耳能感知的聲波 (16~20kHz)，應用在各種感測上，其中低頻的音波(20kHz~數百 kHz)多用於測距。

此系統所使用的 Devantech 公司 SRF05 超音波感測器，為雙頭式感測器，在 同側有一發射器和一接收器。發射器利用壓電逆效應，在壓電晶片上施加脈衝電 壓使壓電材料震動產生超音波；接收器收到介質的壓縮，則利用壓電正效應將空 氣振動轉換為電壓訊號。

SRF05 感測器的量測範圍為 1cm~300cm，由發射器發射超音波，至接收器接 收到反射的回音之經過時間，即可推算出感測器與反射面之距離。如下圖(5-4)與 下式(5.1)所示。

圖 5-4 超音波感測示意圖 α

D

D =^{𝑣𝑣𝑠𝑠×𝑡𝑡}₂ ^𝑠𝑠cos 𝛼𝛼 (5.1) 其中，

D：超音波傳遞距離的一半(m) 𝑣𝑣𝑠𝑠：超音波傳遞速率(m sec⁄ ) 𝑡𝑡_𝑠𝑠：超音波傳遞時間(sec)

當式(5.1)中的 D 遠大於發射端和接收端之間的距離，cos 𝛼𝛼可視為趨近於 1。

聲波的傳遞速率𝑣𝑣𝑠𝑠會隨溫度(T)而變，在攝氏 0 度為 331.31m/sec，關係式如下：

𝑣𝑣_𝑠𝑠 = 331.31 × �1 +_273.15^𝑇𝑇 (5.2)

5.2.2 感測器配置

為提供載具四周完整的障礙物資訊，並讓各個超音波感測器不會互相衝突，

因此在載具上訂製一外框，將六顆超音波感測器依圖放置於外框上。

圖 5-5 超音波感測器配置圖

由於 SRF05 感測器的輸入觸發訊號需要10μs 的 TTL 訊號，回傳的輸出訊號 也是 TTL 訊號，因此使用 FPGA 開發板，設計一 VHDL 程式來收發 TTL 訊號，

透過 RS232 傳輸介面將資料傳輸至控制單元。而為避免訊號相互干擾，TTL 訊號 間隔為 22ms。如下圖：

U1 U2 U4 U3

U5

U6

圖 5-6 FPGA 對超音波感測器提供之觸發訊號[10]

利用上述的測距原理和資訊傳輸的方法，可以持續地用超音波取得載具四周 的近距離障礙物的位置。

第六章 路徑規劃設計

無人載具的偵測障礙感測器有雙眼相機與超音波感測器，因此針對二種感測 器的特性分別使用不同的避障策略，由[13]規劃策略流程進行整合。

6.1 快速探索隨機樹

雙眼相機的視覺範圍內能捕捉到的障礙物數量會較多且分布較為複雜，因此 需要一整體性的避障策略，而此系統使用的規劃演算為快速探索隨機樹

(Rapidly-Exploring Random Tree, RRT)。RRT 是一種漸進式隨機取樣探索的路徑規 劃演算法，適用於複雜環境的路徑規劃[13]。

6.1.1 隨機樹的建構

隨機樹有二種成員：節點(node)和邊(edge)，節點是已經加入資料樹中的隨機 取樣點，邊為二節點間的連線。

快速探索隨機樹的建構，首先從初始點建立隨機樹，接著在可活動的空間中 隨機取樣一點 Xrand，使該點與隨機樹中最接近的節點 Xnear連接，連接的方法有二 種：(一)點對點連接或是(二)點對邊連接，以連接的長度最小、且不會碰觸到障礙 物空間的連接方式為優先選擇。連結方式的示意圖如下。

圖 6-1 隨機樹連接方式(一)[13]

Xinit X_init Xrand

Xnear

(a) (b)

圖 6-2 隨機樹連接方式(二)[13]

若隨機取樣點 Xrand無法與隨機樹連結，則重新取樣隨機點。重複此流程，直 至隨機樹擴張到已經和終點 X_goal連接為止。演算法如下：

圖 6-3 RRT 流程 Rapidly-exploring Random Tree：

1. Initialize RRT at starting point 2. do while path not found

3. Generate random point

4. Find the shortest connection 5. if no collision

6. Connect 7. end if 8. end do while

Xnear

X_near X_rand

X_rand Xinit

Xinit

(a)

(b)

6.1.2 路徑縮短

RRT 建構出來的隨機路徑，可能會有繞遠路或過於曲折的情形發生，因此需 要再進行路徑縮短的步驟：在規劃出來的路徑中隨機取二節點並連線，若連線間 不會接觸到障礙物空間，則將此連線加入路徑取代舊的連結；若新連線會碰觸到 障礙物空間，則維持舊的路線。重複數次此步驟即可縮短 RRT 規畫的路徑。演算 法如下：

圖 6-4 路徑縮短流程

執行路徑縮短的動作結束後，再檢查路徑中兩兩相鄰的節點，若二節點的距 離太短則移除其中一點，以減少過多的路徑雜點。

6.2 超音波避障規則庫

超音波感測器的偵測結果較單純，感測距離也較短，避障動作需要即時反應，

故使用規則庫系統來設計超音波避障系統。

Path Shortening：

1. Read RRT path

2. for times of shortening

3. Randaomly select two points on path 4. Connect

5. if no collision 6. Add the shortcut 7. Update path 8. end if

9. end for

6.2.1 規則庫介紹

規則庫系統(Rule-based System)是一種從已知事實推論新事件的系統。其運作 形式為：若 A 成立，則執行 B，A 稱作前提(Antecedent)，B 稱作結果(Consequence)。

規則庫系統包含三個元件：推理引擎(Inference Engine)、知識庫(Knowledge Base)、事件資料庫(Fact Database)。知識庫為一規則資料庫，包含了事先建立的規 則，將得知的事實能經過推理引擎得到結論。事實資料庫包含傳給知識庫中的規 則使用條件。推理引擎是結合規則和事實的控制器。

若出現複數個規則能應用在同一件事實的情況，則需要使用額外的選擇機制 決定要套用的規則，確保系統一次只會採取一個動作。

6.2.2 避障原理

因為設置了 6 個超音波感測器來偵測環境，總共會有 2⁶=64 種感測事實，會使 得規則庫的設計過於複雜且運算時間會增加，故避障系統將六個感測器整合成三 種方向，作為規則庫的輸入變數。

圖 6-5 超音波感測器方向整合示意圖

如圖(6-3)所示，U1、U2 整合為右方：U3、U4 整合為前方；U5、U6 整合為 左方。如此可將 64 種感測事實簡化歸納成七種，而將障礙物距離的安全閥值設為 50 公分。列舉針對此七種障礙物偵測的情況及相對應的避障動作如下[11]：

1. 僅右方有障礙物：只有 U1、U2 感測到障礙，使載具線速度較大，角速度較

前方 (U3, U4)

右方 (U1, U2) 左方

(U5,

U6)

小，使其輕微左轉避開障礙物。

2. 僅左方有障礙物：只有 U5、U6 感測到障礙，使載具線速度較大，角速度較 小，使其輕微右轉避開障礙物。

3. 僅前方有障礙物：只有 U3、U4 感測到障礙，使載具線速度較小，角速度較 大，使其緩慢前進並避開障礙物。

4. 右、前方有障礙物：U1~U4 感測到障礙，使載具線速度較小，角速度較大，

使其左轉避開障礙物。

5. 前、左方有障礙物：U3~U6 感測到障礙，使載具線速度較小，角速度較大，

使其右轉避開障礙物。

6. 左、右方有障礙物：U1、U2、U5、U6 感測到障礙，使載具線速度較大，角 速度為零，使其直走脫離障礙物。

7. 三方向皆有障礙物：U1~U6 皆感測到障礙，由於本實驗並不考慮載具後退之 情形，故使線速度與角速度為零，停止實驗。

上述規則以表格整理如下：

表 6-1 超音波感測結果與動作對應表

(U1, U2)右方 (U3, U4)前方 (U5, U6)左方

(U1, U2)右方

ν=0.1 ω=0.15

ν=0.05 ω=0.25

ν=0.15 ω=0.0

(U3, U4)前方

ν=0.05 ω=-0.25

ν=0.05 ω=-0.25

(U5, U6)左方

ν=0.1 ω=-0.15

上表中ω 為正代表載具的左轉方向，ω 為負是載具的右轉方向。此表即為規

則庫系統的知識庫。

而為使載具在運行即時避障時能保持「遠離障礙物」的行為，測距的安全閥