特徵點於環境的三維座標資訊

本論文以視覺感測裝置實現機器人定位並同時建立地圖，以影像中的特徵點 作為環境的參考資訊，特徵點於環境中的三維座標由感測裝置取得影像開始到計

0 2 4 6 8 10 12

60 120 180 240 300 360 420 480 540 600 660 720

cm

cm

Yc error

表 2. 3 X 方向實際距離與平均誤差之數值 X 方向 (cm) 平均誤差(cm) X 方向 (cm) 平均誤差(cm)

-87 0.72 0 0.53 -78 0.36 9 0.76 -69 0.21 18 0.91 -60 0.29 33 0.27 -51 0.29 42 0.31 -42 0.31 51 0.23 -27 0.23 60 0.23 -18 0.18 69 0.60 -9 0.35 78 0.97

圖 2.14 特徵點Ｘ方向量測位置與平均誤差關係 表 2. 4 Y 方向實際距離與平均誤差之數值 Y 方向 (cm) 平均誤差(cm) Y 方向 (cm) 平均誤差(cm)

60 1.31 210 1.44 90 1.01 240 1.47 120 0.97 270 1.69 150 1.10 300 1.43 180 1.05 330 2.14

算出其座標資訊的流程如圖 2.17 所示，影像從感測裝置取得，透過 SURF 演算 法擷取影像特徵點，這些影像特徵點與資料庫進行比對而能得知影像特徵中與資 料庫相同的特徵點，（在這裡影像特徵與資料庫比對的結果是將影像中的特徵點 分為與資料庫不相同以及與資料庫相同的兩個部分），影像中每個特徵點皆有其

-5 -4 -3 -2 -1 0 1 2 3 4 5

-87 -78 -69 -60 -51 -42 -27 -18 -9 0 9 18 33 42 51 60 69 78

cm

cm

Xc error

圖 2.15 特徵點 Y 方向量測位置與平均誤差關係 表 2. 5 Z 方向實際距離與平均誤差之數值

Z 方向實際數值(cm) 平均誤差 Z 方向實際數值(cm) 平均誤差

18 0.32 -27 0.27

9 0.18 -36 0.31

0 0.08 -45 0.31

-9 0.15 -54 0.48

-18 0.20 -63 0.61

圖 2.16 特徵點 Z 方向量測位置與平均誤差關係

對應的影像平面座標，假設當次影像有 All_t 個特徵點且其對應的影像座標可用 [Im_1,row Im_1,col… Im_{All_t,row} Im_{All_t,col}]表示，接著，由 Kinect 所取得的深度資訊，

藉由 OpenNI 將影像平面與深度資訊結合，使得每個影像座標都對應一個深度資 訊，這個深度資訊以 OpenNI 轉換為 Kinect 座標系下的三維座標，於是可以得到

-5 -4 -3 -2 -1 0 1 2 3 4 5

60 90 120 150 180 210 240 270 300 330

cm

cm

Yc error

-5 -4 -3 -2 -1 0 1 2 3 4 5

18 9 0 -9 -18 -27 -36 -45 -54 -63

cm

cm

Zc error

圖 2.17 影像特徵點三維座標之計算流程圖

當次影像上特徵點在 Kinect 座標系下的三維座標資訊，但因為感測裝置有其能 感測的範圍限制，假設在感測範圍內的影像特徵點的總數為ｎ_t，則這些特徵點 在 Kinect 座標系下的三維座標以[D_1,x D_1,y D_1,z… D_{n_t,x} D_{n_t,y} D_{n_t,z}]表示，接著，

由上節所敘述的方式透過漸進曲線降低感測器量測數值的誤差，修正後的三維座 標資訊為[d_i,x d_i,y d_i,z](以其中第 i 個特徵點為例)，最後，結合機器人狀態資訊 [R_x R_y R_θ]，轉換特徵點的座標至世界座標系，完成特徵點三維座標的計算。

在計算特徵點於世界座標的三維資訊時，根據本論文所使用之機器人移動平

台而必頇對特徵點在機器人座標系下之資訊添加一修正項；因實現本論文之機器 人為實驗室自製的二輪差動式移動平台，其以雙獨立驅動輪為前置，一輔助輪為 後，而 Kinect 則架設於機器人正中心的位置(圖 2.18 中R₀的位置)，然機器人運 動是控制雙獨立驅動輪做速度控制的結果，而雙輪之架構偏機器人前方，而非整 體機器人的中心位置，也就是非 Kinect 所架設之中心位置，為了特徵點轉換座 標系之一致性，而應以輪軸中心(圖 2.18 中𝐶𝑂𝑅的位置)為機器人中心，因此在轉 換 Kinect 座標系下的特徵點三維座標到世界座標的步驟，對每個 Y 方向的座標 資訊皆會扣除一個位移距離D_w0如圖 2.18，假設當前計算影像中第ｉ個特徵點在 環境中的三維座標，則該特徵點在 Kinect 座標系下的 Y 方向座標資訊修正如式 2.9。

wo y i y

i d D

dˆ_,  _, 

(2.9) 得到機器人座標系下的特徵點三維座標後，透過座標轉換的關係式，結合機

器人在世界座標狀態資訊，透過式 2.10 便可得到特徵點位於世界座標的三維座 標[𝑚_𝑖,𝑥 𝑚_𝑖,𝑦 𝑚_𝑖,𝑧]

圖 2.18 座標系特徵點資訊修正之示意圖



 







 





 



 







 





y i

x i y

x y i

x i

d d R

R m

m

, ,

, ,

cos ˆ sin

sin cos









(2.10)

其中𝝓指的是取樣影像與深度資訊時的機器人朝向角與世界座標縱軸的夾 角，因為我們是以機器人初始位置的機器人朝向角為世界座標縱軸 Y，以其水平 垂直方向為橫軸 X，假設當前機器人於世界座標之朝向角為R_θ，則定義𝝓：

  R_

2 (2.11) 機器人狀態向量以[R_x R_y R_θ]代表機器人在世界座標二維座標位置和朝向角，

R_H為 Kinect 感測裝置裝設在機器人的高度，因此實際特徵點於世界座標的高度 為：

m_i,z R_H d_i,z

(2.12)