結 論

5

Besl 與 Mckay[4]於 1992 年所提出的迭代最近點演算法(Iterative Closest Point Algorithm, 簡稱 ICP 演算法)是目前最常被拿來使用做幾何相似度對齊的演算法，

6

7

8

法則是使用兩點直線距離的方式搜尋，且定義X



x ii^, ^{1 ~} Nx



為在M 中所尋找 到的對應點，如(2-1)所示，即可找出 data shape 每個點在 model shape 中所找到的 匹配點配對關係，最後再將這些配對關係去計算幾何轉換資訊。

由於 ICP 演算法屬於剛體轉換(rigid body transformation)，包含了旋轉矩陣與 平移矩陣，在上一節中我們透過計算距離的方式得到一組點與點之間的配對關係，

9

接著運用(2-5)式計算X 與P 之間的交叉共變異矩陣(cross-covariance matrix)



x p ^：

10

由此可知，ICP 演算法式不斷的尋找在第 n 次疊代後所更新的 data shape 與 model shape 之間尋找距離最近點的對應關係，進而縮小 data shape 與 model shape 之間的均方距離。 model shape，綠色為 data shape，設定完門檻值後，接著如圖 2-2(b)開始找最近點 為匹配點，接著計算幾何轉換資訊與更新座標如圖 2-2(c)，最後計算兩集合的最 小均方誤差，如果未達門檻值，則重複以上步驟直到如圖 2-2(d)兩集合疊合為止。

11

圖 2-2(a)兩集合初始相對位置

圖 2-2(b)尋找匹配點

圖 2-2(c)計算幾何轉換資訊並更新座標

12

圖 2-2(d)再尋找一次匹配點

圖 2-2(e)完成對齊

圖 2-2 ICP 演算法對齊示意圖

2.3. 標準 ICP 演算法

ICP 演算法最大的優點在於，可以用來處理不同型態的幾何對齊，如：點、

直線、平面、曲線、曲面等，且不需要經過物體表面特徵擷取的過程，直接求點 與點之間的最短距離，然而 ICP 演算本身也有缺點，在尋找匹配點的時候，可能 會因為離散點資訊或是雜訊點導致 ICP 演算法尋找到不恰當的匹配點，導致兩集 合對齊效果不佳，而 Segal、Haehnal 及 Thrun[21] 和 Zhang[22]文中提出一標準 ICP 演算法，建議在尋找匹配點時，設置一個尋找匹配點的門檻值，如果 data shape 在 model shape 中所搜尋到的配對點距離大於門檻值，則表示 data shape 該點在該 次迭代沒有成功匹配到恰當的配對點，篩選方式如(2-10)式，圖 2-3 為增加匹配點 門檻值的細部計算流程，其原理是將配對距離過大者認為是配對錯誤而不去計算，

13

14

不給予配對，這樣可以大幅降低配對到不理想的點，進而提高對齊的效果。

圖 2-4(a)原始 ICP 尋找匹配點方式

圖 2-4(b)標準 ICP 尋找匹配點方式

圖 2-4 原始 ICP 與標準 ICP 尋找匹配點的不同處

2.4. 加權式 ICP

自從 Mckay 提出 ICP 演算法後，後人為了使 ICP 演算法的對齊效果更加可靠 強健，故 Godin[23]提出了加權式 ICP 演算法，透過給予每個即將對齊的集合點 一個權重值，進而改善 ICP 演算法對齊結果，常見的權重有兩個，一個是常數權 重，另一個如(2-11)式，如果兩集合之間所對應到的配對點距離越長，則所得到 的權重越小，相對的兩集合之間對應到的配對點距離越短，則其權重值越高，經

15 Kennedy 跟 Russell Eberhart 於 1995 年所提出，是一種群體智慧的方法，起因是 出於對鳥群或魚群的群聚互動特性所引發之社會行為的觀察，藉由群聚移動的模

16

決定粒子數量以及決定演化次數，並使用隨機的方式給予每一個粒子初始位置與 速度，接著將初始化過後的粒子一一帶入其適應方程式計算出適應值，以作為判 斷每一個粒子的好壞，計算完適應值後，接著找出每個粒子在目前為止所找到的 最佳解，而這個最佳解稱為 Pbest，下一步找出所有粒子中，目前為止所搜尋到 的整體最佳解，此最佳解稱為 Gbest，最後將得到的 Pbest 與 Gbest 帶入(2-13)式 更新粒子速度，(2-13)式中^w 為慣性權重因子，c₁與c₂為加速度常數，建議值為

17

計算適應值

尋找粒子最佳解

尋找全域最佳解

更新粒子 速度與位置

收斂門檻?

結束 NO

Yes 初始化

Pbest

Gbest

圖 2-5 PSO 演算法流程圖

18

19

入S_R ，使得S_R 資訊內容如(3-1)式，這樣可以讓S_R 資訊越來越豐富，其目的是為 了增加座標的分佈密集程度，越密集的地方表示越可確認該處為障礙物，即可以 有效克服因距離雷射測距儀較遠的資料量比較稀疏所造成的區域最佳解的結果，

同時也可以有效降低雜訊點的干擾，進而讓對其效果更加精準。

圖 3-1 雷射掃描示意圖

20

圖 3-2 只針對資料密集的點做對齊

1 2

{ , , , }

R

 S S S

n

S

(3-1)

3.2. 尋找重疊的資訊點

由於機器人的移動，使得雷射每次掃描所接收的每筆環境地圖資訊都有些許 不相同，經過 ICP 對齊後的 data shape，可以知道此時的機器人在全域地圖中所 朝向的方向以及的可視範圍如圖 3-3，而圖 3-3 中黃色的點為全域地圖 model shape，

橙色點是機器人位置，而紅色點為機器人當下位置的地圖資訊 data shape，從圖

21

22

圖 3-3 機器人可視範圍示意圖

圖 3-4 非重疊資訊示意圖

23

圖 3-5 橘色線將資訊分成兩部分

接著下一步將剔除這些非重疊的資訊，剔除的方式是使用不等式的概念，首 先將

S

_n^的9 0資訊座標

(

x_{9 0}

,

y_{9 0}

)

帶入已經求得的直線方程式L ，此時可以求得一 個值為V ，接著再將全域地圖中的一個點

(

x_{m i}

,

y_{m i}

)

帶入L，其值為V_m，最後將V 與

Vm 相乘，如計算結果大於等於零則表示

( , )

m i m i

x y 與機器人目前方向相同，則將

(

x_{m i}

,

y_{m i}

)

保留下來並放入共同可視集合S_{R C}，如計算結果小於零，則表示

(

x_{m i}

,

y_{m i}

)

並 不在於機器人的可視範圍內，不會將此點保留下來，圖 3-6 為尋找兩集合重疊資 訊的流程，經由此演算法後即可將兩集合重疊資訊與非重疊區域分割兩部分，並 獲得共同可視集合S_{R C}，後面將用來幫助增加建立地圖的精準度。

24

25

對不恰當的點所造成的影響，但是當這兩個方式結合時，反而會產生一個新問 題。

假設環境地圖如圖 3-7 所示，機器人搭載一雷射測距儀掃描角度1 8 0，此時 機器人所蒐集的全域地圖資訊如圖 3-8 綠色線段，當機器人移動到圖 3-9 的位置 後，因使用全域地圖的方式，此時機器人的地圖資訊如圖 3-10，而搭配篩選與機 器人共同可視的全域地圖後，此時 model shape 則為圖 3-11 紅色線段所示，只留 下全域地圖與當下雷射測距儀的重疊資訊，但當機器人移動到如圖 3-12 的時候，

此時使用篩選機制後會發生如圖 3-13 的狀況，圖 3-13 左邊的紅色地圖資訊因篩 選機制保留了下來，但實際上雷射測距儀根本看不到該地方的資訊，此時這些資 訊會導致 ICP 演算法尋找到不適當的配對點。

圖 3-7 機器人初始狀態與環境

26

圖 3-8 機器人剛開始掃描的資訊狀況

圖 3-9 機器人移動到此位置方向

27

圖 3-10 機器人 reference data 中存放的資訊

圖 3-11 紅色為使用篩選機制所找到的資訊

28

圖 3-12 機器人移動到此位置與方向

圖 3-13 紅色為使用篩選機制所找到的資訊

故本節提出一個方法，將全域地圖資存取方式改變成只存取機器人當下的前 面幾筆掃描資訊，其目的是為了避免如圖 3-13 左邊掃描的資訊並不是當下雷射測 距儀所能看見的地方，所以我們可以將(3-1)式改寫成(3-3)式，即表示每次 ICP 對

29

30

31

避免匹配到兩集合之間的非重疊資訊，找出重疊資訊S_{R C}後，將S_{R C} 與S_n重新使用 一次標準 ICP 對齊兩集合得到S_n^，此時判斷n 是否為a 的倍數，如果為a 的倍數，

則將S_n^放入全域地圖S_R，否則不會將資訊存入S_R，將資訊存入S_R 的目的是為了 增加座標的分佈密集程度，越密集的地方表示越可確認該處為障礙物，而加入a 的 目的是為了使得權重在增強型 ICP 上可以增加其精確度，對於後續對齊可大幅提 升其整體地圖的對齊準確度，接著判斷S_R 資料筆數是否大於

k

，如果大於

k

，則 將從S_R 移除最早一筆資訊S_{n a k}^，避免因機器人移動到不同環境使得S_n配對到當 下雷射所掃描不到的配對點，接著不斷的重複以上步驟直到沒下一筆資訊為止，

圖 3-15 為使用增強型 ICP 演算法建立環境地圖的流程圖，流程圖中橘色方塊內 部為增強型 ICP 主要流程。

32

33

34

圖 4-1(a)兩集合初始相對位置圖

圖 4-1 (b)兩集合成功對齊圖 圖 4-1 兩集合成功對齊示意圖

35

圖 4-2(a)另外兩集合初始相對位置圖

圖 4-2(b)無法完全對齊圖 圖 4-2 兩集合對其失敗示意圖

36

37

圖 4-4 PSO 初始化後各個粒子的旋轉位移示意圖

圖 4-5 PSO 演化數代後每個粒子的旋轉位移示意圖

38

4.2. 使用 PSO 調整增強型 ICP 演算法建立環境地圖

在上一章中，提到使用部份全域地圖當 model shape，並搭配篩選機制找出 model shape 與 data shape 之間的重疊資訊成為增強型 ICP，避免雜訊干擾以及演 算法找到不恰當的配對點，接下來在本節中，我們將上一節使用 PSO 尋找到的旋 轉位移矩陣T_{p b}用來更新S_n得到S_n^，求得S_n^的目的是為了先讓兩集合有初步的對 齊，不但可以有效降低S_R 與S_n兩集合之間的落差，並可以增加增強型 ICP 演算 法對齊精確度。

使用 PSO 調整增強型 ICP 演算法建立環境地圖流程如圖 4-6 所示，首先定義 全域地圖為S_R ，新掃描資訊為S_n，

k

為保存雷射掃描數量閥值，當接收到新資訊 時先使用 PSO 演算法尋找一個旋轉位移矩陣T_{p b}，並將T_{p b}用來更新S_n為S_n^，做為 兩集合初步對齊，接著將S_R 與S_n^做增強型 ICP 對齊得到S_n^，並且將S_n^放入全 域地圖S_R，增加座標的分布密度，下一步則判斷S_R 資料筆數是否大於

k

，如果大 於

k

，則將從S_R 移除一筆資訊S_{n a k}^，最後不斷的重複以上步驟直到沒有下一筆 掃描資訊，環境地圖即可建立完成。

39

40

第十三章 實驗結果

在本節中，將會介紹本論文所使用的實驗設備、實驗環境以及實驗結果與分 析，本論文主要使用自走車機器人搭載著一個雷射區域掃描儀來建立環境地圖，

利用雷射掃描儀接收環境資訊，加上自走車的移動，進而將欲建立的環境資訊蒐 集下來，接者使用本論文所提出的 PSO 調整增強型 ICP 演算法，將每個掃描下 來的地圖資訊一一對齊起來成為完整的全域地圖。

5.1. 實驗設備與環境

本論文主要使用由 Adept MobileRobots 公司所開發的 P3-DX 自走車[26]，如 圖 5-1 搭載由 SICK 公司開發的 LSM-100 雷射掃描儀[27]，如圖 5-2 所示，自走 車的規格表如表 5-1 所示，雷射規格如表 5-2 所示，圖 5-3 為雷射掃瞄範圍示意

本論文主要使用由 Adept MobileRobots 公司所開發的 P3-DX 自走車[26]，如 圖 5-1 搭載由 SICK 公司開發的 LSM-100 雷射掃描儀[27]，如圖 5-2 所示，自走 車的規格表如表 5-1 所示，雷射規格如表 5-2 所示，圖 5-3 為雷射掃瞄範圍示意

在文檔中 使用PSO調整之增強型ICP演算法於未知環境地圖之建立 (頁 12-82)