垂直高度估測

計算出特徵點的水平位置之後，即以此為基礎估測出特徵點對於地圖之高 度，如圖 3-6 之側視圖，其中攝影機之高度 h 已知，相對距離 r 也已估測出來，

若攝影機對特徵點的垂直視角φ也能得知的話，特徵點高度 z 就能以(16)式計算 出來。

( )

φ ^φ_φ

tan tan

tan r z h r

z

h− = ⇒ = − (16) 在全向式影像平面上，距離鏡心愈遠的位置，代表相對於鏡心的視角愈大，本論 文用校正實驗來近似出影像平面上距鏡心的距離(Pixel)對於垂直視角之對應關 係。圖 3-7 即為校正實驗之影像，在牆面上放置 300(15*20)個已知高度的格子點 做為樣本，並將攝影機對每個格子點之垂直視角與在影像平面中相對於鏡心之距 離記錄下來(如圖 3-8)，在圖 3-8 中將各個記錄值以一條二階曲線近似出來，此二 階函數即為影像平面上距鏡心的距離(Pixel)對於垂直視角之轉換公式，如(17)

圖 3-7 視角校正之實驗影像

圖 3-8 視角-Pixel 實驗結果及近似關係圖 式，其中φ為垂直視角(ْ )，x 為影像平面上距鏡心的距離(Pixel)。

6305 . 2 7262 . 0 0013 .

0 ² + +

−

= x x

φ ⁽¹⁷⁾

利用特徵點在影像平面上的位置距離代入轉換公式，即可得知攝影機對此特 徵點的垂直視角，將 Ф 套入(16)式中，就可以求出我們對特徵點估測的另一個資

degree

訊，高度 z。圖 3-9 為特徵點高度估測之實驗，Y 軸為水平座標，Z 軸為垂直座 標，此結果顯示一牆面上之特徵點位置分佈，其中淺色塊代表牆面上較可能存在 特徵點的區塊，如門、佈告欄等，圖中特徵點大都依這些區塊所分佈，利影像中 的各個特徵點找出實際環境中正確對應的位置，並與估測結果做比較，Z 方向之 平均誤差為 0.355m。

圖 3-9 特徵點高度估測結果 3.3 EKF-SLAM 演算法

3.3.1 系統設計

Kalman filter是一種利用系統模型與系統狀態量測值，對系統的狀態作最佳 估測的方法；利用系統的狀態方程式預測當時的狀態值，使用感測器測量系統狀 態之量測值，而狀態預測及感測器量測值都有其不確定性存在，Kalman filter 就 是依據兩者資訊之可靠程度，以最佳比例融合兩種數值，得到系統之最佳化狀態 估測值。

本研究之目的即是要對機器人姿態及環境地圖的狀態作估測，使用全向式攝 影機為系統量測裝置。因為本系統之模型為非線性系統，因此使用 Extended Kalman filter來處理非線性之系統，以達成系統狀態之最佳估測。本系統之系統 狀態包含了機器人本身的姿態及環境地圖的位置資訊。其狀態向量(State vector)yt 如(18)式所示；xt 代表機器人的姿態，包含有水平位置座標(x,y)、朝向 角度 θ，m 代表環境地圖資訊也就是所有 Landmark 的位置資訊，每一個 Landmark 以(m , _n,x m , _n,y m )表示其位置資訊，下標 n 代表第 n 個 Landmark，下標 x,y,z_n,z 分別為平面座標(x,y)及高度 z。

Y Z

(m)

State vector：

T

⎟⎟⎠⎞

⎜⎜⎝⎛

= m y_t x^t

(

x y θ m1,x m1,y m1,z ... mn,x mn,y mn,z

)

= (18)

不同於一般僅估測機器人狀態之定位設計，在 SLAM 的演算法中亦要對所 有的 Landmark 作估測：除了用感測器量測環境資訊以定位出機器人的位置外，

也要同時建立環境地圖，這是 SLAM 的主要概念。因此在 State vector 中也要增 加 Landmark 的資訊，達到同時估測機器人姿態與環境 Landmark 的功能。

圖 3-10 為 EKF-SLAM 之設計概念，主要分為兩個階段：首先在 Prediction step 中利用機器人的 Motion model 預測出系統此時的狀態，接著在 Update step 中整 合預測出的系統狀態及感測器量測出的測量值，計算出 Kalman gain 以更新對系 統狀態的估測，更新後的狀態資訊再回傳到 Prediction step，作為下一時刻狀態 預測依據；整個系統即是在這兩個階段中循環運作，完成機器人定位及地圖建立 的功能。

圖 3-10 EKF SLAM設計概念圖 3.3.2 系統模型

運動模型：

由於本研究是使用速度控制來操作機器人之運動，因此使用基於速度之機器

人移動模型(Velocity motion model)。機器人的速度命令主要有兩個：線速度 (Linear velocity) υ及角速度(Angular velocity) ω，速度控制模型即是以此兩種速度 為輸入之模型。假設機器人在 Δt 時間內是以固定速度沿一圓弧形軌跡移動，此 圓弧之半徑為 r。圖 3-11 為運動模型之示意圖，(x,y)為機器人所在位置座標、θ 為其朝向角，箭號代表其移動之圓弧方向，(xc,y_c)則為此圓之圓心，已知在半徑 r之圓弧軌跡上移動之速度關係如(19)式：

r

v=ω⋅ ⁽¹⁹⁾ 因此可得在已知速度的條件下機器人的移動半徑

ω

r = v (20)

由(20)式及示意圖 3-11 可推得圓心座標 ：

⎪⎩

⎪⎨

⎧

+

=

−

= ω θ ω θ

cos sin y v

y x v x

c c

(21)

若機器人在(t-1)時刻之姿態 xt-1=(x, y, θ)，且保持移動速度經過 Δt 時間後，其理 想之姿態 xt=(x’, y’, θ’)可表示成：

圖 3-11 機器人運動模型表示圖

( )

⎟⎟

在本研究中使用的感測器為全向式攝影機，因此量測模型(Measurement model)即是基於全向式攝影機器可量測的內容為依據來設定。量測模型包含了觀 測 之 量 測 向 量 (Observed measurement vector) 及 預 測 之 量 測 向 量 (Predicted measurement vector)，Observed measurement vector 即是感測器測量到的狀態值向 量，將其定義為z ： _tⁱ

Observed measurement vector：

⎟⎟ 向式影像平面上，Landmark 所出現的方向即是實際相對於機器人所在的方向，

因此利用 Landmark 在影像平面上對於鏡心的角度即可得知θ_tⁱ⁽如圖 3-12)；φ_tⁱ為 對於 Landmark 的垂直視角(如圖 3-6 之ψ)，利用圖 3-9 中之垂直視角轉換公式，

以及影像平面中 Landmark 相距鏡心的距離即可求得。

Predicted measurement vector不同於 Observed measurement vector 由感測器 所量測到的狀態值，而是由機器人所預測出來的預測值。基於前一時刻所估測出 的 Landmark 位置(mx, m_y, m_z)及預測出之機器人位置(x,y,θ)，算出預測之量測向 量。圖 3-12 為機器人及 Landmark 之水平位置示意圖，可推得預測之 Landmark 相對距離rˆ 及角度_tⁱ θ^ˆ_tⁱ，參考圖 3-7 機器人及 Landmark 之垂直位置示意圖，利用

已知固定之鏡心高度 h 可推得對 Landmark 預測之垂直視角φ^ˆ_tⁱ，Predicted measurement vector如下(27)式：

⎟⎟

在 SLAM 中，Landmark 的狀態也包含在系統狀態之內，故系統狀態向量(state vector)y_t中有 3N+3 個元素，前三個為機器人的姿態資訊，之後為 N 個 Landmark

Landmark (m_i,x, m_i,y, m_i,z)

(x,y,θ) Mobile robot

3N+3維矩陣。

3.3.3.1 Prediction step

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

x_t-1之偏導數：

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

( )

3.3.3.2 Update step

由(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 需要兩張影像方可比對出相 同的特徵點；在本文中所使用的比對策略為以起始點影像為參考點，機器人移動 後所擷取到的畫面均與起始點影像作比較，以此方法定位出機器人自身的位置並 可建立出起始點周遭環境特徵點之地圖。

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

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

在文檔中 基於全向式影像之機器人同步定位與環境地圖建立 (頁 36-0)