TWI572847B - An Improved Algorithm for Simultaneous Localization and Mapping - Google Patents

Description

改良型同時定位及建圖之演算法

本發明係一種同時定位及建圖之演算法，尤其是一種具高運算效率及低運算成本之改良型同時定位及建圖之演算法。

雖然以快速同時定位及建圖之演算法（Fast Simultaneous Localization and Mapping，簡稱FastSLAM）為基礎的演算法已有相當良好的結果，但即便如此，以FastSLAM為基礎的改良型演算法仍有幾個重大缺點。尤其是隨著機器人搜索範圍逐漸增加，所儲存的環境資訊隨之增加，因此在比對環境資訊以及感測器資訊時，比對時間大幅度提高，將導致運算成本的提高。

由於現實生活環境皆為非稀疏地標的環境，地標的數目增加非常迅速，因此導致比對系統極為耗時而且精準度也不足以應付定位與建圖。

因此，有必要設計一種新型的同時定位及建圖之演算法，以克服上述缺陷。

本發明的一目的在於提供一種改良型同時定位及建圖之演算法，其在狀態預測階段，為了維持程式的執行速度，只考慮控制輸入 以預測粒子的位置，避免使用錯誤的環境資訊更新粒子狀態，定位出錯誤的粒子位置，進而影響建圖的結果。

本發明的另一目的在於提供一種改良型同時定位及建圖之演算法，其使用特徵提取的方法找出具有特徵的感測器環境資訊進行資料關聯步驟的運算，求得環境資訊與感測器資訊的似然性，如果該時刻有多筆似然性高於門檻值，則會取似然性最高的預測結果，進行地標的更新。

本發明的另一目的在於提供一種改良型同時定位及建圖之演算法，其可有效降低粒子狀態的計算時間，並能更精準預測出粒子之位置。

為了達到上述目的，本發明之改良型同時定位及建圖之演算法，其包括下列步驟：a：以複數個粒子執行狀態預測；b：以感測器資訊執行特徵計算；c：資料關連性的計算；d：地標更新與新增；e：計算權重；f：是否所有地標已計算完畢，若否，則回到步驟c；g：是否所有感測器已執行完畢，若否，則回到步驟b；h：執行三角定位；i：是否所有粒子已計算完畢，若否，則回到步驟a；j：重新取樣；以及k：地圖是否建置完成，若否，則回到步驟a。

為使　貴審查委員能進一步瞭解本發明之結構、特徵及其目的，茲附以圖式及較佳具體實施例之詳細說明如後。

請一併參閱圖1至圖3，其中，圖1繪示本發明一較佳實施例之改良型同時定位及建圖之演算法之流程示意圖；圖2a繪示本發明一較佳實施例之以複數個感測器執行特徵計算步驟中使用轉角特徵提取演算法之示意圖；圖2b繪示本發明一較佳實施例之以複數個感測器執行特徵計算步驟中使用轉角特徵提取演算法之模擬結果示意圖；圖2c繪示本發明一較佳實施例之以複數個感測器執行特徵計算步驟中使用曲率特徵提取演算法之示意圖；圖2d繪示本發明一較佳實施例之以複數個感測器執行特徵計算步驟中使用曲率特徵提取演算法之模擬結果示意圖；圖3繪示本發明一較佳實施例之三角定位示意圖。

如圖所示，本發明之改良型同時定位及建圖之演算法，稱為FESLAM，其包括下列步驟：a：以複數個粒子執行狀態預測；b：以感測器資訊執行特徵計算；c：資料關連性的計算；d：地標更新與新增；e：計算權重；f：是否所有地標已計算完畢，若否，則回到步驟c；g：是否所有感測器已執行完畢，若否，則回到步驟b；h：執行三角定位；i：是否所有粒子已計算完畢，若否，則回到步驟a；j：重新取樣；以及k：地圖是否建置完成，若否，則回到步驟a。

於步驟a中，以複數個粒子執行狀態預測；其中，以複數個粒子執行狀態預測之原理如下，與FastSLAM 1.0相同，本發明係根據控制命令加以估測每個粒子 的狀態：

(3-1)

接著，使用粒子 的位置與地標 產生數筆估測的量測資訊：

(3-2)

之後，計算量測資訊模型對地標狀態的偏微分矩陣：

(3-3)

同時，計算量測資訊模型之於粒子狀態的偏微分矩陣：

(3-4)

並利用量測資訊模型對地標狀態的偏微分矩陣計算創新共變異數矩陣：

(3-5)

最後，利用以上資訊更新粒子 之狀態：

(3-6)

其中，在起始時間會給予粒子狀態，設定 、 初始值：

(3-7)

藉由初始值以及預測步驟即可得到更新後的粒子狀態：

(3-8)

於步驟b中，以感測器資訊執行特徵計算；其中，該感測器係以物體邊緣之曲率或角度執行特徵計算。

此外，步驟b進一步包括b1：是否為具有強烈特徵之感測資訊，若否，則回到步驟b之次步驟。

其原理如下：有鑑於感測器資訊數量龐大且彼此沒有關連性，造成比對效率低落，也無法準確估測地標位置，因此本發明擬透過特徵提取的方式，將環境中具有明顯特徵的地標提取出來進行比對，解決比對效率低落的問題。由於在一般室內環境中，物體邊緣及牆壁最具特徵，包含：直線、轉角、及弧度，因此可以透過計算物體邊緣的曲率或是角度加以判斷。

透過當前感測資訊及粒子位置進行運算，即可偵測得到如圖2a中的三角形地標位置。如欲獲得圓圈感測資訊所對應的轉角特徵，可利用該感測資訊及其兩邊數筆感測資訊所建立的地標，分別計算兩邊地標相對於欲判斷地標 的平均向量 與 ，再藉由內積計算得到兩平均向量之夾角 ，如果兩個平均向量的夾角越接近90度，則內積的值越近似於0，表示該感測資訊所對應的地標為一轉角，因此可藉由設定一閥值以判別某一地標是否為轉角地標，以大幅刪減不具轉角特徵資訊的地標，透過此方法即有效地提取出具有轉角特徵的環境資訊，並建立如圖2b三角形所示之轉角特徵地標。

此外，一般室內環境常具有弧形的物體，因此可以透過曲率計算，建立具有曲率特徵的地標，使地圖建置更加完善。但曲率計算需使用三次微分，運算成本非常高昂，因此目前有許多演算法利用一些簡化的方程式模擬曲率計算的表示法，達到近似的效果，例如：B-Splines曲線[請參照S.Suranthiran and S. Jayasuriya, “Effective Fusion of Distorted Multi-sensor Data,” in Proc. Intelligent Control, 2003, pp.444-449]包含Bezier曲線的通用數學表示法，除了有Bezier曲線的優點，同時也具備獨有的特性，例如局部控制的能力以及不改變曲線階數下增加曲線的控制點等。在[請參照Z. Junhua and Wangwei, “The Extration of Fractional-order Curvature Attributes and Applications in the Structure Interpreation,” in Proc. Artificial Intelligence and Computational Intelligence, 2000, pp.123-126]中，Junhua Z.比較各種計算曲率的演算法，並透過實驗去模擬各個曲率演算法之特性，在兩種尺度不同的情形下，均能獲得良好的轉角、內凹、或是平滑曲線的地標特徵。

但上述方法皆有運算量龐大的問題，因此本發明擬參考上述之轉角特徵提取方式，透過當前感測資訊及粒子位置進行運算，偵測得到如圖2c之三角形位置。

如欲判斷其曲率特徵，(1)可利用每個三角形位置 以及相鄰位置( , )分別計算出向量 與 ，並以(2)藉由內積計算得到兩向量之夾角 ，最後利用數筆夾角之平均除以其中最大的夾角，即可以輕易計算出曲率大小 ，成功偵測出具有曲率特徵的地標圖2d。由圖2c之三角形候選地標中，選取兩側地標更新以及新增的對象。

(3-9)

(3-10)

(3-11)

藉由步驟b之特徵提取演算法進行特徵判斷，如若環境特徵不夠明顯則直接跳過後續處理，直接進行下一筆雷射的特徵提取的判斷。

於步驟c中，資料關連性的計算；其中，該資料關連性的計算例如但不限於係計算其似然性 (likelihood)。

其原理如下：就由預測步驟估測出當時刻粒子的位置後，利用粒子狀態與該粒子中之地標 ，產生數筆估測量測資訊 ：

(3-12)

由於粒子狀態更新過，因此再次計算量測資訊模型之於地標的偏微分矩陣：

(3-13)

之後，計算創新共變異數矩陣：

(3-14)

再計算真實量測資訊 以及估測量測資訊 之間的似然性：

(3-15)

不斷計算上述方程式，直到真實量測資訊與粒子 中所有 個地標的似然性皆計算完畢，然後建立似然性矩陣 ：

(3-16)

(3-17)

其中， 為粒子 中第 個地標的似然性， 為判斷是否新增地標的門檻值(threshold)。接著，從似然性矩陣中找出似然性最大的地標， 為似然性最大的地標之編號，可藉此判斷該真實量測資訊 是否為已知地標。

於步驟d中，地標更新與新增；其中，其進一步包括：d1：判斷該似然性是否大於一門檻值；d2：若是，則利用環境資訊更新該粒子之狀態，若否，則至步驟d4；d3：更新地標；以及d4：新增地標至該粒子中等次步驟。

當 ：代表此筆量測資訊 與地標 有較大似然性，即表示該量測資訊為已知地標，因此透過量測資訊更新粒子 中地標 的狀態及共變異數矩陣：

(3-18)

當 ：

代表粒子 中沒有任何地標能與此筆量測資訊 具有強烈的似然性，故判斷此筆量測資訊為新地標，因此將此地標加入至粒子 的狀態中：

(3-19)

若在 時刻中具有多筆真實量測資訊，則不斷重複資料關聯以及地標更新與新增兩個步驟，直到 時刻中所有量測資訊皆完成地標更新與新增步驟。

於步驟f~g中，其中，步驟f：是否所有地標已計算完畢，若否，則回到步驟c；g：是否所有感測器已執行完畢，若否，則回到步驟b。

於步驟h中，執行三角定位；其中，本發明係透過該地標資訊以及該地標特徵進行資料關聯步驟後，會從該感測器資訊與所有地標資訊所計算出的最大相似性地標中，再取出其中相似性最大的三個地標，作為該機器人地位的三個參考點。

由於隨著粒子探索，傳統FastSLAM更新粒子的效率會越來越差，而且容易更新至錯誤的位置，因此本發明採用三角地位法，因為三角定位被廣泛用於空間定位，地圖量測，海運導航等項目，透過三個以之參考坐標，以及當下的所在位置，透過三角函數即可更新其所在位置。

本發明在透過地標資訊以及地標特徵進行資料關聯步驟後，會從感測器資訊與所有地標資訊所計算出的最大似然性地標中，再取出其中似然性最大的三個地標，作為粒子地位的三個參考點。

如圖3所示， 、 、 為參考地標。其中， 為三個地標似然性最大的地標； 為目前粒子狀態；感測器與粒子狀態的距離 、 、 。首先，藉由兩筆似然性較低的量測資訊分別與似然性最高的量測資訊之間的距離 、 ，利用餘弦定理求得夾角 、 :

(3-20)

(3-21)

接著求出 與通過 之水平線的角度 ，以及 與通過 之水平線的角度 。接著以地標 起始點，以感測器資訊為長度 ，分別以角度 、 、 ，以及 回推回粒子狀態:

(3-22)

(3-23)

最後透過誤差公式進行三角形所有線段誤差的加總，求出誤差最小值的粒子位置:

(3-24)

(3-25)

透過此方法，可以利用高信任程度之地標進行定位以取代FastSLAM2.0的更新粒子步驟，以減少其運算成本，並提高其定位精準度。

於步驟i中，是否所有粒子已計算完畢，若否，則回到步驟a。

於步驟j中，重新取樣；其中，其係以權重較高的粒子淘汰權重較低的粒子，使該粒子群往權重高的粒子集中，如此可提升估測該粒子以及地標之精確性。

當所有粒子的權重都計算完之後，則利用重新取樣步驟，以權重較高的粒子淘汰低權重的粒子，使粒子群往權重高的粒子集中，如此可提升估測粒子以及地標之精確性。

本發明所揭示之改良式同時定位及建圖演算法能有效地抑制地標快速的增加，並能透過特徵提取使得環境以及地標的比較更為精準、快速，最後透過準確的地標位置進行粒子位置的更新。因此，較習知技術之演算法具進步性。

請參閱圖4，其繪示本發明一較佳實施例之定位誤差示意圖。

如圖4所示，其為執行20次上述環境實驗後所統計出的定位誤差數據圖，橫軸為取樣時間，縱軸為定位誤差。其中，圓圈表示使用FastSLAM2.0演算法所得到的定位誤差，正方形表示使用CESLAM演算法所得到的定位誤差，三角形表示使用本發明之改良型SLAM演算法所得到的定位誤差，由圖4可以清楚發現隨著粒子的移動，傳統型FastSLAM的定位效果會逐步降低，到後來甚至於發散，而本發明所提出的改良型SLAM定位效果穩定，不容易因為隨著探索的環境增加，而導致定位效果變差。

而本發明所提出的改良型SLAM，因為可以藉由特徵來選擇是否進行計算，因此可以有效大幅度降低計算時間，而三角定位也可以快速定位出粒子的位置，進而取代傳統SLAM演算法的預測模組。

最後，本發明將此演算法應用於美商Altera公司所生產之DE2i-150開發平台，其搭配MTLC觸碰顯示面板，並以全硬體設計之方式加速演算法之效率，最後與全軟體設計之演算法進行比較，驗證演算法以全硬體設計可以達到即時運算之效果。本發明之實驗比較平台有三個，包含：在FPGA硬體平台上以全硬體設計執行、在FPGA硬體平台上以NIOS軟體執行、以及在筆電上以軟體執行。表1所示的FPGA 硬體規格，而以軟體來實現本發明之規格則如表2所示。

表1 以FPGA來實現本發明之規格： <TABLE border="1" borderColor="#000000" width="_0043"><TBODY><tr><td> FPGA系列 </td><td> Altera Cyclone IV GX </td></tr><tr><td> 裝置 </td><td> EP4CGX150DF31C7 </td></tr></TBODY></TABLE>

表2 以軟體來實現本發明之規格： <TABLE border="1" borderColor="#000000" width="_0044"><TBODY><tr><td> 處理器 </td><td> Intel® Atom™ Dual Core Processor N2600 2.3GH_Z</td></tr><tr><td> 記憶體 </td><td> DDR3 SO-DIMM SDRAM </td></tr></TBODY></TABLE>

其中，地圖大小為210 217畫素，粒子數仍為 32。粒子每次移動的距離為 10 pixel，每次旋轉的角度為90度。模擬的雷射感測器範圍為180度，1度為一個單位，故每一時刻都有181筆量測資訊，最大量測距離為200 pixel。在里程計所回傳移動距離、旋轉角度，以及雷射所收到的距離都具有高斯雜訊。為了快速計算實際路徑與估測路徑的定位誤差，本模擬中粒子皆照著設定好的路徑進行移動，每次模擬共執行70次動作，包含前進以及原地旋轉。

實驗結果如表3所示，表3清楚顯示在FPGA平台上利用軟體與硬體設計執行各項模組軟體與硬體設計之演算法之時間花費，由硬體設計之演算法通常比軟體設計之演算法快300~1000倍，足以看出硬體設計之優勢。

表3 在FPGA平台上利用軟體及全硬體設計之各模組執行時間比較結果： <TABLE border="1" borderColor="#000000" width="_0046"><TBODY><tr><td> 模組（Module） </td><td> 所花費時間（μs） </td><td></td></tr><tr><td> 硬體（HW） </td><td> 軟體（SW） </td><td> 速度提升（Speed up） </td></tr><tr><td> 啟始 </td><td> 0.2 </td><td> 54 </td><td> 270倍 </td></tr><tr><td> 預測 </td><td> 1 </td><td> 746 </td><td> 746倍 </td></tr><tr><td> 特徵擷取 </td><td> 0.5 </td><td> 500 </td><td> 1000倍 </td></tr><tr><td> 似然性計算 </td><td> 1.64 </td><td> 614 </td><td> 374倍 </td></tr><tr><td> 地標更新 </td><td> 1.24 </td><td> 465 </td><td> 375倍 </td></tr><tr><td> 狀態更新 </td><td> 5 </td><td> 40 </td><td> 8倍 </td></tr></TBODY></TABLE>

請參照圖5，其繪示本發明一較佳實施例以軟、硬體實現FESLAM演算法的累積時間比較示意圖。如圖所示，橫軸為取樣時間點，縱軸為累積的時間，單位皆為秒(s)。其中，菱形表示FESLAM演算法在FPGA平台上執行的累積時間；正方形表示FESLAM演算法在筆電(PC)上執行的累積時間；三角形表示FESLAM演算法在FPGA平台上執行且以NIOS II軟體模擬的累積時間。其中，以低成本的FPGA所設計的演算法效率已經可以比擬一般市面上的筆電(PC)執行效能，也由於全硬體設計可以利用較為低階的硬體裝置進行設計，成本得以大幅降低，因此大大提升了實用性以及可塑性。

因此，本發明之同時定位及建圖之演算法確實較習知技術具有進步性。

本發明所揭示者，乃較佳實施例之一種，舉凡局部之變更或修飾而源於本發明之技術思想而為熟習該項技藝之人所易於推知者，俱不脫本發明之專利權範疇。

綜上所陳，本發明無論就目的、手段與功效，在在顯示其迥異於習知之技術特徵，且其首先發明合於實用，亦在在符合發明之專利要件，懇請　貴審查委員明察，並祈早日賜予專利，俾嘉惠社會，實感德便。

無。

圖1為一示意圖，其繪示本發明一較佳實施例之改良型同時定位及建圖之演算法之流程示意圖。    圖2a為一示意圖，其繪示本發明一較佳實施例之以感測器資訊執行特徵計算步驟中使用轉角特徵提取演算法之示意圖。    圖2b為一示意圖，其繪示本發明一較佳實施例之以感測器資訊執行特徵計算步驟中使用轉角特徵提取演算法之模擬結果示意圖。    圖2c為一示意圖，其特徵計算步驟中使用曲率特徵提取演算法之示意圖。    圖2d為一示意圖，其繪示本發明一較佳實施例之以感測器資訊執行特徵計算步驟中使用曲率特徵提取演算法之模擬結果示意圖。    圖3為一示意圖，其繪示本發明一較佳實施例之三角定位示意圖。    圖4為一示意圖，其繪示本發明一較佳實施例之定位誤差示意圖。    圖5為一示意圖，其繪示本發明一較佳實施例以軟、硬體實現FESLAM演算法的累積時間比較示意圖。

Claims (5)

1. 一種改良型同時定位及建圖之演算法，其包括下列步驟：a：以複數個粒子執行狀態預測；b：以感測器資訊執行特徵計算，其中該感測器係以物體邊緣之曲率或角度執行特徵計算；c：資料關連性的計算；d：地標更新與新增；e：計算權重；f：是否所有地標已計算完畢，若否，則回到步驟c；g：是否所有感測器已執行完畢，若否，則回到步驟b；h：執行三角定位；i：是否所有粒子已計算完畢，若否，則回到步驟a；j：重新取樣；以及k：地圖是否建置完成，若否，則回到步驟a。
2. 如申請專利範圍第1項所述之改良型同時定位及建圖之演算法，其中於該步驟c中，該資料關連性的計算係計算其似然性。
3. 如申請專利範圍第2項所述之改良型同時定位及建圖之演算法，其中於該步驟d中，其進一步包括：d1：判斷該似然性是否大於一門檻值；d2：若是，則利用環境資訊更新該粒子之狀態，若否，則至步驟d4；d3：更新地標；以及d4：新增地標至該粒子中。
4. 如申請專利範圍第1項所述之改良型同時定位及建圖之演算法，其中於該步驟h中，係透過該地標資訊以及該地標特徵進行資料關聯步驟後，會從該感測器資訊與所有地標資訊所計算出的最大似然性地標中，再取出其中似然性最大的三個地標，作為該粒子地位的三個參考點。
5. 如申請專利範圍第1項所述之改良型同時定位及建圖之演算法，其中於該步驟j中，係以權重較高的粒子淘汰權重較低的粒子，使該粒子群往權重高的粒子集中，如此可提升估測該粒子以及地標之精確性。