雖然以 FastSLAM 為基礎的演算法已有相當良好的結果，但即便如此，以 FastSLAM 為基礎的改良型演算法仍有幾個重大缺點，尤其是隨著機器人搜索範 圍逐漸增加，所儲存的環境資訊隨之增加，因此在比對環境資訊以及感測器資訊 時，比對時間大幅度提高，將導致運算成本的提高。由於現實生活環境皆為非稀 疏地標的環境，地標的數目增加非常迅速，因此導致比對系統極為耗時而且精準 度也不足以應付定位與建圖。為了解決執行時間的問題，同時也保有精確度，因 此本論文擬發展一種改良型 SLAM 演算法，簡述如下：
FastSLAM2.0 計算耗時的問題主要是因為在更新機器人位置時，粒子所儲存的地 標資訊必須與感測器偵測範圍內所有偵測到的環境資訊進行比對，並更新粒子狀 態中的環境資訊(
[ ]s tm, 和u[ ]s t,m )，不論這組資料的比對好壞，隨著地標數目的增加，計算時間不但大量增加，誤差的累積也相當可觀，造成定位與建圖的精確度下降。
因此，我們提出一種新的架構，其作法如上文所示，在狀態預測階段，為了維持 程式的執行速度，我們採取跟 FastSLAM1.0 一樣的做法，只考慮控制輸入
u
t以預 測機器人的位置，避免如 FastSLAM2.0 使用到錯誤的環境資訊更新粒子狀態，定 位出錯誤的粒子位置，進而影響建圖的結果。接著，使用特徵提取的方法找出具 有特徵的環境資訊進行資料關聯步驟的運算，求得環境資訊與感測器資訊的似然 性。如果該時刻有多筆似然性高於門檻值，則會取似然性最高的預測結果，進行 地標的更新。之後透過三個似然性最高的三個的地標進行粒子狀態預測。相對於 FastSLAM2.0，此方法可以有效降低粒子狀態的計算時間，並能更精準預測出粒 子之位置。根據上述作法，所提出的改良型 SLAM 架構中，每個時刻 t 只會對粒 子的狀態更新一次，如此就能省下大量的計算時間，不會因為地標數目逐漸增多，不會因為錯誤的環境資訊使得粒子更新至錯誤的位置，且不會因為更新次數過於 頻繁，耗費大量的計算時間。詳細步驟說明如下：
24
1、 狀態預測(Prediction)
首先與 FastSLAM 1.0 相同，根據控制命令加以估測每個粒子
m
的狀態：25
26
圖 3- 1 轉角特徵提取演算法 (2)曲率特徵
一般室內環境常具有弧形的物體，因此可以透過曲率計算，建立具有曲率特 徵的地標，使地圖建置更加完善。但曲率計算需使用三次微分，運算成本非常高 昂，因此目前有許多演算法利用一些簡化的方程式模擬曲率計算的表示法，達到 近似的效果，例如：B-Splines 曲線[33]為包含 Bezier 曲線的通用數學表示法，除 了有 Bezier 曲線的優點，同時也具備獨有的特性，例如局部控制的能力以及不改 變曲線階數下增加曲線的控制點等。在[34]中，Junhua Z.比較各種計算曲率的演 算法，並透過實驗去模擬各個曲率演算法之特性，在兩種尺度不同的情形下，均 能獲得良好的轉角、內凹、或是平滑曲線的地標特徵。但上述方法皆有運算量龐 大的問題，因此本計畫擬參考上述之轉角特徵提取方式，透過當前感測資訊及機 器人位置進行運算，偵測得到如圖 3- 2(a)之綠色位置。如欲判斷其曲率特徵，(1) 可利用每個綠色位置
P
以及相鄰位置(P
r,P
l)分別計算出向量R
與L
，並以(2)藉 由內積計算得到兩向量之夾角
，最後利用數筆夾角之平均除以其中最大的夾角，即可以輕易計算出曲率大小
C
，成功偵測出具有曲率特徵的地標圖 3- 2(b)。(P P)r
R
(P P)lL
(3-9)(a)轉角特徵提取演算法 (b)轉角特徵提取演算法模擬結果
27
3、 資料關聯(data association)
就由預測步驟估測出當時刻機器人的位置後，利用粒子狀態與該粒子中之地