Prediction step

當機器人移動時，其狀態會依運動模型，(22)式而改變，而在 SLAM 中，移 動模型的預測而改變；其他元素則全為零，代表機器人的移動不會影響 Landmark 的狀態，其狀態均保留與前一刻相同。

x_t-1之偏導數：

sin sin sin Covariance matrix M_t，此刻新增的移動雜訊之 Covariance matrix R t可由(34)式轉 換式得到：

( )

由(25)式，包含 Noise 之完整的 Measurement model 表示如下式：

⎟⎟

σr^=0.41m、σ_θ ^=3.1ْ、σ_φ^=6.4ْ，此數值即代表 sensor 的 noise，並將之用在 Kalman

⎟⎟

表 3-1 EKF SLAM 演算法之步驟流程 EKF SLAM Algorithm ( μ_t−1 ,Σ_t−1 ,u ,_t z_t ⁾ Prediction Step：

1.

第四章 參考點轉換

4.1 參考點比對

在使用 Laser scanner 之定位與地圖建立的研究中，scan-matching 是個常用的 方法，利用兩組雷射資訊的比對以估測出與 reference scan 的差異，藉此判定與 reference scan相對位置關係，整個地圖就是以一連串的 reference scan 所組成，

利用機器人即時的雷射掃描資訊與 reference scan 比對，即可得知機器人在地圖 中的位置。[28]即是利用 Laser scanner 與 scan-matching 的概念來完成定位與地圖 建立的功能，將 current scan 與 reference scan 做比對，可得到與參考點的位置差 異，地圖中建立了數個已知位置 reference scan，便能利用這些參考點所掃描到的 雷射資訊，來決定出機器人的正確位置。

在[6]中亦是使用 scan-matching 的概念，不同的是使用的感測器資訊不是 Laser 的距離資訊而是攝影機的影像資訊，利用擷取到的影像完成 visual scan-matching，並影像畫面做為視覺參考點。機器人以即時影像與地圖中的參考 點影像比對，即可判定出目前影像中所看到的特徵點與地圖中特徵點的對應關 係，以此對地圖中的特徵點位置作更新的動作並定位出機器人本身的位置。

在基本的 SLAM 演算法中，當感測器觀測到一個特徵點時，要以此點之資 訊與地圖中所有的特徵點做比對，以找出現在觀測到的這個特徵點是地圖上的哪 個特徵點，才能更新此點的位置資訊，若不存在於地圖中，則視為新的特徵點加 入地圖中。在 visual scan-matching 的方法中則不同，擷取到的影像特徵點不需與 地圖中所有的特徵點比對以找出對應關係，利用在地圖上已建立的視覺參考點資 訊，特徵點只需與參考點影像比對即能找出現在觀測到的這個特徵點是地圖上的 哪個特徵點，增加了特徵點與地圖對應的速度。

4.2 參考點轉換演算法

本研究之主要目的就是希望能夠藉由全向式攝影機豐富的影像資訊及廣闊 的視野，幫助機器人在室內走廊的環境下完成自我定位的功能，配合自主移動的

行為，讓機器人能移動至目的地，完成機器人在室內走廊環境下的巡航功能。在 第三章中已說明了利用 EKF SLAM 來達成機器人自我定位的方法，由於使用的 感測器是攝影機，對於偵測辨識環境中的 Landmark 需要兩張影像方可比對出相 同的特徵點；在本文中所使用的比對策略為以起始點影像為參考點，機器人移動 後所擷取到的畫面均與起始點影像作比較，以此方法定位出機器人自身的位置並 可建立出起始點周遭環境特徵點之地圖。

但此方法並不能讓機器人走出較遠的距離，因為機器人移動的愈遠，與起始 點影像所能比對到的特徵點愈少，因此若要讓機器人走出較長的距離，且要能比 對出足夠的特徵點來定位，則不能再以起點之單一參考點影像來做為比對的基 準，勢必要改變參考點的影像。為了解決上述問題，本論文設計一參考點建立及 轉換的流程。當機器人移動到新地點時則建立此處的參考點影像；每到一個新的 區域時，重新設定參考點影像，並將先前的參考點資訊存入資料庫中，讓各區域 都各有做為基準的參考點影像，機器人即可持續行走更長的距離，並依據各參考 點影像比對出特徵點來定位出機器人的位置。判斷參考點建立的時機決定於移動 的距離及特徵點的數目，距離是一個最大的因素，因此若機器人移動超過設定的 距離時，則重新設定此時的位置為新的參考點；除了距離之外特徵點的數目也是 判斷的標準之一，當比對成功的特徵點愈來愈少時，就表示當前的參考點影像已 不敷使用，亦是要替換參考點的時機之一。

在替換參考點的同時，首先要先將原本的參考點區域相關資訊存入資料庫，

如參考點影像、特徵點資訊、環境地圖(特徵點位置)、各個特徵點之 Covariance…

等，並且將新舊兩個參考點影像作比較，將共有的特徵點及其 Covariance 相關資 料留下，作為新參考點區域已知的環境特徵點，也因此新舊兩區域有了共同的連 繫，不會互不相關。

在[6]中因為使用的攝影機視角有限，故當旋轉超過一定角度時，會無法與 原參考點影像成功比對，要建立新的參考點，例如在回到已走過的地點時，因為 與之前建立的參考點影像視角不同無法比對，而再次建立參考點，相同的特徵點

圖 4-1 參考點影像負責區域示意圖

會因為不同的視角而重覆定義成不同的點。在本論文中，因為使用的全向式攝影 機擁有 360 度視角，即使機器人旋轉了 180 度，在影像平面中仍能辨識出是同一 區域，故在機器人回頭走向原本已走過的路徑時，仍可在影像中辨識出之已經估 測好位置的特徵點，此種情況下不需執行新參考點建立的步驟，而是將之前舊的 參考點資訊取出才是最好的選擇。這個情況就好像圖 4-1，每個參考點影像都有 其負責的區域範圍，在各區域(Ref01、Ref02、Ref03、Ref04)內均以各區的參考 點影像作比較來定位出機器人的位置，到達了新的地點則建立新的參考點，回到 了走過的地方，則從資料庫中取回舊有的資料幫助機器人定位。

以距離參考點設定位置的遠近來判別是否進入舊有的參考點區域，當要進入 舊的參考點區域時，則將原本的參考點區域資料存入資料庫，以備以後使用，並 從資料庫中找出舊有的參考點資料，以舊有的參考點影像為基準，持續的運行機 器人定位的演算法。如此一來不僅可以減少參考點建立的數量，更可以降低同一 特徵點被重覆定義的情況發生，降低地圖與定位系統的複雜度。

圖 4-2 即參考點轉換之流程圖，其中 Δs 代表參考點區域範圍的最大半徑，

N代表特徵點比對成功數目下限。首先以移動的距離及特徵點比對數目作為判斷 的基準，當距離大於設定的上限值或是特徵點數目小於設定的下限，則要開始進 入參考點轉換的步驟；搜尋資料庫中距離機器人此時位置最近的參考點，判斷機 器人是否在那參考點的區域範圍之內：若是，則代表進入了舊有的參考點區域，

並取出資料庫中舊有的資訊來使用；若否，則表示機器人此時已進入了一個新的 Ref 01 Ref 02 Ref 03 Ref 04

地點，將原本的資訊存入資料庫後，建立新的參考點，並開始建立出新的參考點 區域環境地圖。

圖 4-2 參考點轉換策略之流程圖

使用此參考點建立及轉換的流程，當機器人移動到新地點時則建立此處的參 考點影像，回程時則從資料庫中取回之前已存入的參考點資訊，如此將環境中不 同地點的全向式影像特徵整合，讓機器人不僅能建立出環境特徵地圖，並可在大 範圍環境中持續重覆行走，達成機器人長距離導航的功能。

圖 4-3 為系統資料庫之結構示意圖，主要以各個參考點影像(Ref_01、

Ref_02…)為群組，每個參考點下包含其擁有的特徵點資訊，如影像特徵、位置 資訊…等，每個參考點間亦具有共同的特徵點，在前一個參考點區域中所得到的 估測值亦能被下一個參考點所使用。以下步驟為參考點轉換與資料庫存取之流 程。

圖 4-3 資料庫之結構示意圖

Step1：

即時影像與參考點影像比對，使用 EKF-SLAM 完成對機器人位置與特徵點 的估測。

Step2：

判斷機器人此刻與參考點的距離是否超過參考點區域最大半徑，若否，代表 機器人仍在同一參考點區域內，回到 Step1 繼續下一個影像 frame 的運算。若是，

代表機器人已離開參考點區域，即開始轉換參考點的步驟。

Step3：

搜尋 database 中區域半徑內是否有其他的參考點，若是，則將原參考點影像 中的特徵點資訊存入並更新 database，並將參考點轉換成已搜尋到的舊有參考 點。若否，將原參考點影像中的特徵點資訊存入並更新 database，此刻的即時影 像則新增存入 database，視為一個新的參考點，與原參考點之特徵點對應關係，

即為兩個參考點所共有的特徵，並在 database 中標記。切換參考點後再次回到 Step1繼續下一個影像 frame 的運算。

第五章 實驗結果

經過上述章節的演算法設計描述後，在本章中設計了幾個實驗來驗證各章節 中所述演算法之性能。最後以一個機器人室內導航的情境展示實驗來驗證本論文 所設計之方法能實現機器人室內導航的功能。

在 5.2 節中將介紹實驗用之機器人，包括了機器人本體、運算核心、及攝影 機系統，並說明機器人的硬體控制架構。5.3 節將介紹環境特徵偵測之實驗，此 實驗主要用以驗證第二章中所述之演算法的可行性，利用不同位置、不同角度所 拍攝下之影像畫面，偵測、辨識出相同的特徵點，確定機器人在移動或是旋轉的 情況下，影像處理系統仍能偵測出足夠的特徵點供定位演算法使用。5.4 節將介 紹 EKF-SLAM 之初步測試實驗，此實驗主要是用以驗證第三章所述之 EKF 演算 法之正確性，利用手動遙控機器人移動的實驗，比較 Odometer 與 EKF-SLAM 演

在 5.2 節中將介紹實驗用之機器人，包括了機器人本體、運算核心、及攝影 機系統，並說明機器人的硬體控制架構。5.3 節將介紹環境特徵偵測之實驗，此 實驗主要用以驗證第二章中所述之演算法的可行性，利用不同位置、不同角度所 拍攝下之影像畫面，偵測、辨識出相同的特徵點，確定機器人在移動或是旋轉的 情況下，影像處理系統仍能偵測出足夠的特徵點供定位演算法使用。5.4 節將介 紹 EKF-SLAM 之初步測試實驗，此實驗主要是用以驗證第三章所述之 EKF 演算 法之正確性，利用手動遙控機器人移動的實驗，比較 Odometer 與 EKF-SLAM 演