第五章 實驗結果
5.5 EKF-SLAM 定位以及地圖接合 機器人方型軌跡運動
圖 5.13 具路徑控制器直線來回實驗之運動軌跡
圖 5.14 機器人角度響應圖
5.5 EKF-SLAM 定位以及地圖接合 機器人方型軌跡運動
為了解決 EKF 定位系統在較大的環境運行時，隨著特徵點數量增加而隨之 提高的運算成本，本論文藉由地圖接合的方式，將特徵點地圖以機器人路徑分界 的方式分為數個區間，藉此驗證其降低定位系統所使用的特徵點數量，當區域地 圖建立完成後以地圖融合演算法維持整體環境資訊，修正區域間的特徵點差異並 修正機器人狀態資訊。
本實驗主要的目的，在於驗證 EKF 定位系統加入地圖接合演算法後在定位 精度上的結果，並以無使用地圖接合演算法之 EKF SLAM 繞行此實驗環境一圈，
比較兩者在即時運算能力上的差異。本實驗機器人所運行的實驗環境為 6mX10m
-20 -15 -10 -5 0 5 10 15 20
0 50 100 150 200 250 300 350
EKF SLAM
cm
cm
的實驗室，機器人由原點(0,0)出發，繞行 4.8mX1.8m 的方形路徑，機器人繞行 第一圈時為建立地圖的階段，完成整個環境的地圖建構後，以建立好之特徵點地 圖資訊作為環境資訊，持續修正機器人狀態並更新特徵點區域地圖，機器人重複 繞行 4 圈，機器人於此實驗環境共繞行 5 圈。在實驗進行中，當機器人處於建構 地圖的狀態時，為了降低因機器人移動對環境特徵取樣的影響，機器人最高的運 動速度限制在 10 cm/sec；當機器人進入使用地圖資訊的階段後，其最高運動速 度則設定為 20 cm/sec。
圖 5.15 為機器人運行於 4.8mX1.8m 的運行路徑繞行五圈所得到的結果，其 結果包括估測之機器人移動軌跡以及所建立的特徵點地圖資訊，這些資訊與實際 環境的配置圖結合，圖中藍色點為帶有距離資訊之特徵點，其不為定位系統所使 用，而紅色點則為定位系統之輸入，藍色線段為機器人位置估測之軌跡線。
機器人運行的運動方式，主要是以 4 個設定於環境中的座標作為機器人移動 的主要依據，這四個環境座標分別為(0,100)、(480,100)、(480,-80)和(0,-80)四個 二維座標點，機器人的運動控制上是利用運動控制器以機器人朝向角與目標點的 角度差做為輸入修正機器人的前進方向。當機器人繞行第一圈時為建立地圖的階 段，依本論文所設計之同時定位與建立地圖之演算法以及地圖接合演算法建立環 境資訊，以本實驗所行徑的路徑，依 4.1 節所敘述的區域地圖路徑邊界將整
圖 5.15 實驗室方形路徑實驗結果與實際環境對照圖
個環境分為數個部分，此實驗會產生 6 個區域地圖(圖 5.16(a)~(f))，且第 6 個地 圖依 4.3 節重合區域偵測的判定，判斷其為重合事件發生之重合區域。最後以地 圖融合演算法完成整個實驗室環境的建構，當重合事件發生後，系統依機器人當 前位置與區域地圖的路徑範圍的關係決定定位系統所使用的區域地圖，因此其後
-800 -700 -600 -500 -400 -300 -200 -100 0
-100 -50 0 50 100 150 200 250
(b)
-5000 -400 -300 -200 -100 0 100
50 100 150 200 250 300
(a)
-700 -600 -500 -400 -300 -200 -100 0 100
-400 -350 -300 -250 -200 -150 -100 -50 0 50
(d)
-800 -700 -600 -500 -400 -300 -200 -100 0
-400 -300 -200 -100 0 100 200 300
(c)
圖 5.16 實驗室方形路徑實驗之區域地圖
-200 -150 -100 -50 0 50 100 150 200 250
-250 -200 -150 -100 -50 0 50 100
(e)
-200 -150 -100 -50 0 50 100 150 200 250
-300 -200 -100 0 100 200 300
(f)
便以對應的區域地圖修正機器人狀態並修正特徵點三維座標資訊。依此方式做機 器人的定位與導航，整個實驗繞行 5 圈後完成本實驗。
圖 5.17 記錄實驗室方形路徑實驗下每個定位演算步驟花費時間之結果，每 一個定位演算步驟從感測器取得影像開始，經過 SURF 作特徵點擷取、特徵點三 維座標的計算，直到整個定位系統修正機器人狀態與更新地圖資訊，圖 5.18、圖 5.19 以及圖 5.20，將整個運算時間之結果分為建構地圖時之運算部分，與重複路 徑時定位系統所耗費之時間兩部分來檢視此實驗結果。
圖 5.18 顯示的是建立地圖階段每個定位演算步驟所花費之時間，一個區域 地圖開始與結束範圍內之運算時間類似於一濂波狀的結果。在本論文中機器人實 行定位系統之時機，是以每次機器人移動距離約 8 公分運算一次，而其後隨定位 系統所花費時間改變而影響在兩間隔間機器人移動的距離，且在建立地圖的階段 限制應用於定位系統的特徵點增加量，在本實驗中令定位系統演算每次最多擴 增 7 個特徵點資訊，如此使定位演算在短間隔內持續修正，並減緩矩陣複雜度之 增長，以區域１為例，區域內定位系統一共修正 46 次，系統狀態向量使用 322 個特徵資訊，因定位系統特徵點在每次步驟的增加數量為固定使得定位系統運算 時間與步驟次序的關係圖呈現穩定的成長，而非起伏不定之波狀圖形。根據建構 區域地圖時定位系統所耗費的運算時間，當器人重複運行該區域地圖時其定位演
圖 5.17 方形路徑實驗之運算時間
0 100 200 300 400 500 600 700 800 900 1000
0 2 4 6 8 10 12 14 16 18 20
step
s
算之時間約略將與建立地圖時之最大運算時間相等，這是因為機器人在區域內所 使用的系統狀態維度是固定大小。由圖 5.18 中可以得知，耗費運算時間最多者 為區域一，其運算時間為 1.19 秒。附帶一提的是，圖 5.18 在 step 為 1 時的運算 時間明顯高於往後數個步驟，是因為 step1 是機器人初始化之階段，機器人需啟 動感測裝置等而使運算時間高於接續的數個步驟。
圖 5.19 的部分與圖 5.18 的差異在於顯示地圖融合演算之運算時間，圖 5.18 中突出之脈衝所在即為地圖融合演算之位置，地圖融合演算使用的時機為區域結 束與開始之交界，而區域地圖１(𝐿1)因其為初始地圖亦為世界地圖資訊，故並無 使用地圖融合演算，當區域地圖二(𝐿2)完成建構，地圖接合演算法將𝐿1以及𝐿2的 資訊結合，這個步驟所花費的時間為 5.86s，其餘部分𝐿2以及𝐿3的資訊結合花費 了 3.8s，𝐿3以及𝐿4的資訊結合使用了 3.56s，𝐿4以及𝐿5的資訊結合使用了 11.39s，
最後區域 6 為重合區域，其地圖資訊與𝐿5與𝐿1的資訊結合，故其運算時間最久，
其時間為 18.37 秒，地圖接合所使用的區域地圖和演算時間的關係如表 5.6。
圖 5.20 顯示機器人在方形路徑重複運行時之運算時間，依表 5.5 區域地圖建 立之結果可知區域 1 最大運算時間為 1191ms、區域 2 為 621ms、區域 3 為 531ms，
區域 4 為 455ms、區域 5 為 521ms 而區域 6 為 562ms，當機器人重複運行時，定
圖 5.18 方形路徑實驗之地圖建構運算時間(不包括地圖融合)
表 5.5 建構地圖階段區域最大運算時間 Map order Spend time
𝑳𝟏 1191ms 𝐿2 621ms 𝐿3 531ms 𝐿4 455ms 𝐿5 521ms 𝐿6 562ms Average 646ms
圖 5.19 方形路徑實驗地圖建構之運算時間(包括地圖融合) 表 5.6 建構地圖階段地圖接合運算時間
Join maps order Spend time 𝑳𝟏and 𝑳𝟐 5.86s
𝐿2and𝐿3 3.8s 𝐿3and𝐿4 3.56s 𝐿4and𝐿5 11.39s 𝐿1、𝐿5 and 𝐿6 18.37s
位系統便依選擇之區域使用該區域之地圖資料庫，而運算時間則與建立該區域之 最大運算時間相近，然圖 5.20 中有數個區間帶有劇烈起伏之情況，此劇烈之震 盪變化代表其間有數個步驟發生無法比對到定位系統所需特徵點之情形。對應於 機器人實際運行時之情況下，其通常發生在區域間重疊區域，依 4.4 節中之地圖 選擇判斷區域轉換之發生，但區域轉換出現前區域無法比對到相同特徵，而下一 區域比對成功之總數低於閥值，使區域沒有正確的轉換而導致之結果，且當機器 人無正常使用定位演算法，機器人在最小移動距離 8cm 後便會再次啟動定位修 正，因此有大幅且劇烈之變化。
表 5.7 為本實驗所得到之機器人位置估測結果，此數據是採機器人接近起點 (0,0)時，當機器人停止運行後所記錄下來的資訊，其目的在於驗證機器人以本論 文設計之地圖接合所達成之定位精度，同時機器人繞行相同環境 5 圈，除第一圈 為建立環境資訊外，其餘圈數皆是以建立之地圖資訊修正機器人狀態資訊，代表 機器人能以資料庫訊息重複性的進行定位演算。統計十次的實驗結果，得到機器 人的平均誤差資訊，其中 X 方向的平均誤差為 2.96 公分，Ｙ方向的平均誤差為 3.56 公分，平均角度差為 1.17 度，而最大誤差的各分項分別為，X 方向最大誤 差為 6.44 公分，Y 方向最大誤差為 8.39 公分，角度上的最大誤差則為 3.25 度，
圖 5.21 以直條圖的方式呈現機器人實際與估測之狀態資訊的差異。
圖 5.20 方形路徑實驗之重複路徑運算時間
300 400 500 600 700 800 900 1000
0 0.5 1 1.5 2 2.5 3
step
s
表 5.7 實驗室方形路徑實驗之機器人位置估測結果
Ground truth EKF Estimation Error x(cm) y(cm) (°) x(cm) y(cm) (°) x(cm) y(cm) (°)
Experiment 1 0.07 5.96 90.60 1.04 0.27 89.83 -0.98 5.69 0.77 Experiment 2 12.49 34.32 91.70 6.05 28.72 91.74 6.44 5.60 -0.04 Experiment 3 13.35 12.35 96.00 12.63 8.50 92.76 0.72 3.85 3.25
Experiment4 8.20 24.14 93.40 3.70 27.19 92.78 4.50 -3.05 0.62 Experiment5 11.19 -2.22- -94.10 6.62 -4.05 91.37 4.57 1.83 2.73 Experiment6 4.42 5.03 91.50 4.76 5.91 90.32 -0.341 -0.883 1.176 Experiment7 11.42 7.13 91.50 8.93 2.78 91.00 2.49 4.35 0.50 Experiment8 17.66 13.43 92.30 11.22 13.56 91.85 6.44 -0.13 0.45 Experiment9 -5.21 5.424 93.3 -5.47 -2.96 91.88 0.26 8.39 1.43 Experiment10 18.43 13.00 97.00 15.61 14.853 96.274 2.83 -1.85 0.73
(a)二維座標誤差
(b)角度差
圖 5.21 實驗室方形路徑實驗之誤差直條圖
0 5 10
1 2 3 4 5 6 7 8 9 10
cm X error
Y error
0 1 2 3 4
1 2 3 4 5 6 7 8 9 10
degree
Angle error
為了展示地圖接合演算所帶來的好處，另外實作以無地圖接合演算在整個實 驗環境中單以 EKF 完成地圖建構，其機器人行進的路徑與特徵點地圖建立的結 果如圖 5.22。藍色線段的部分是 EKF 估測機器人位置所得到的移動軌跡，紅色 點為定位系統所使用的特徵點，藍色點則為帶有距離資訊的特徵點，但不做為定 位系統所使用，與使用地圖接合演算的實驗結果圖 5.15 比較，機器人從原點(0,0) 的地方出發，直到繞行一圈後完成地圖的建構。無地圖接合演算的機器人運行到 路徑後段時，由於每次移動所需的間隔提高，所以取樣的頻率下降，環境中用於 定位系統的特徵點數量也較圖 5.15 的使用數量來的少。另外，機器人的移動軌 跡也因為取樣間隔的間隔較長，而難以使機器人運動軌跡符合所設定的路徑。
圖 5.23 則是機器人以無地圖融合演算在建構地圖的過程中每次步驟所花費 的運算時間，此結果與圖 5.18 比較，在完成地圖建構所使用的步驟次數上，由 於兩個實驗皆是在相同的實際環境下運行，因為兩者每次定位演算所花費的時間 不同所以所使用的步驟總數是不相同的。以無地圖融合演算建構地圖，機器人每 次運算時間隨逐步增加，當機器人繞行實驗路徑一圈，最後一次定位系統演算的 時間為 7.32 秒，以兩者的比較可知藉由地圖接合之做法，可以有效提高機器人
圖 5.23 則是機器人以無地圖融合演算在建構地圖的過程中每次步驟所花費 的運算時間，此結果與圖 5.18 比較，在完成地圖建構所使用的步驟次數上，由 於兩個實驗皆是在相同的實際環境下運行，因為兩者每次定位演算所花費的時間 不同所以所使用的步驟總數是不相同的。以無地圖融合演算建構地圖，機器人每 次運算時間隨逐步增加，當機器人繞行實驗路徑一圈，最後一次定位系統演算的 時間為 7.32 秒，以兩者的比較可知藉由地圖接合之做法，可以有效提高機器人