智慧型服務機器人之視覺追蹤及辨識應用於感測器融合理論 (III)
本計畫的目標為發展智慧型服務機器人創新關鍵科技，並針對遠距教學機器人進行 開發，使教師可在遠端電腦並透過網路來操控機器人進行遠距教學，藉由此服務型機器 人，可大大改善城鄉間的教學品質差距，並使相當多的學童受惠，此機器人關鍵技術，基於影像之同步定位與地圖建置(Vision Based SLAM)
Localization and SLAM
發展完善的機器人運動規劃與控制系統，使機器人能透過此一系統，進行即時的運動規劃，以完全自主性的方式在動態環境中自由移動，完成所要求的任務。其中機器人系統架構以運動規劃與同步定位與建構環境地圖（Simultaneous Localization and Mapping）以及路徑規劃與自動導航之演算法為核心，將涵蓋感測(sensing)，感知(Perception)，智能(Intelligence)，控制(Control)，機構(Mechanism)，驅動(Actuation)等方面。
Fig 1.A.1. 運動規劃所涵蓋的技術範圍
由於本計畫機器人型態包含輪型平台主要目標在建立行動式機器人各種前瞻運動規劃理論，其中包含輪型機器人差速協調控制、輪型機器人即時避障機制、輪型機器人即時路徑規劃系統、動態環境同步定位與地圖建構（SLAM）、多機器人協調控制系統。此外為了使機器人感知系統更為完整，在本計畫中配合影像系統作為輔助，提高感知系統的準確度，以達到更為完善的即時運動規劃系統。
本計畫依照系統模組的方式進行分類，則可以分為協調控制系統、同步定位與環境地圖建構系統、路徑規劃與自動導航系統、影像分析與控制系統、感知系統、機器人社群智慧系統。目前規劃是依此模組化架構發展各項模組系統的前瞻技術，最後再將各模組系統整合成完整的運動規劃系統。
Fig 1.A.2. 運動規劃系統的模組化架構
目前實現SLAM 的方式主要分成三種：Topological, Grid-Based, Feature-Based methods。在Topological的方式，是將整個環境當作一張圖，利用辨識各個物體或是環境的方式在環境中達到定位的功能，雖然透過這種方式可以做到大尺寸環境的定位，但實際上要做到辨識環境則是相當困難的，因此也較少人使用。
Grid-Based 的方式，也同樣被廣泛使用來處理定位與建構地圖的問題，一個典型的處理方式，就是將環境分成全域地圖（global map）與區域地圖( local map )，全域地圖是用來儲存所有環境的資訊，而區域地圖則是用來更新全域地圖的資訊，透過偵測區域地圖在全域地圖的相對位置，就可以達到機器人定位的目的。但由於需要一直儲存區域地圖的資訊因此Grid-Based 需要相當大的計算量以及儲存空間。相較於Topological 和Grid-Based 兩種方式，透過特徵擷取來達到定位與建構地圖顯得更直覺得多，可以經由影像來獲取邊緣、菱角、角落、線等特徵資訊，甚至也可以利用顏色、形狀、長度等特徵來瞭解環境，一個特徵地圖（Feature-Based Map）通常可以利用下式表達，其中i f 是指特徵， N 則是指地圖中特徵的數目。
地圖比對(Map-Matching)
利用機器人的感應器所建出的局部地圖和全域地圖做比較，以全域地圖為基準來修正目前機器人的位置。利用比對局部地圖和全域地圖的關係來做位置上面的估測，但其計算量相對來說較大，而且其不確定性也遠大於利用地標來做估測；不過其可以更快速的完成地圖的建立，只是此種方法必須周圍環境具有顯的形狀特徵才可以完成，例如直角，利用此形狀特性來做比對與結合。其缺點在於環境中做融合之特徵不易被擷取，資料融合所憑藉的是機器人所偵測到的局部地圖資訊中，尋找出如直角等有明顯特徵的圖案，再與全域座標做特徵的比對，當環境處於類似走廊的情況，便無法做有效的融合、定位，此方法之受周圍環境限制太大，並且其不確定性也遠大於利用地標來做估測的結果。
SLAM演算法
目前用來做為多機器人定位的演算法中，主要有三角定位法、Kalman Filter、MCL(Monte Carlo Localization)，Particle Filter、 Mixture MCL、Grid-Based Markov 定位法。三角定位法提供簡單、快速、幾何形狀的位置更新，但其所得到的位置資訊不能將感測器及機器人的誤差以機率方式涵蓋進去，因此機器人的誤差及定位後所在位置的機率無法有效的比較。
利用統計學演算法來對機器人其位置估測的不確定性作數學式的表示，以此不確定性拿來當做機器人定位之效能指標是目前機器人定位領域被公認的，如Kalman Filter、MCL、Mixture MCL、Grid-Based Markov 等定位演算法都是利用機率理論來建構的。這些定位演算法都具有相同的特性，那就是以多筆資料融合來得到最佳位置的估測，融合的資料越多，其估測值也經將更加準確。
Kalman Filter 的原理在於利用現在的狀態資訊加上下一秒的輸入，推斷出下一時刻的輸出，因此我們可以發現在使用Kalman Filter 演算法的時候，我們必須有現在機器人位置的狀態。Kalman Filter 利用高斯分佈(Gaussian Distribution)，在經過其遞迴的演算法則來對外在感測之誤差做有效的估測與融合。
Kalman Filter 是個精確又簡單的方法。不過此方法必須假設加入融合的感測器模型誤差分佈(Error Distribution)接近高斯分佈且必須為線性的。卡曼濾波器的優點在於融合、收斂速度快，若其誤差模型為高斯分佈則其估測出來的結果準確度將會相當高；缺點就如同上面所說，必須其誤差模型必須為高斯分佈，否則估測出來的結果將會缺乏準確度。
因為多機器人系統與SLAM 領域的堀起，許多研究者提出了Markov 定位演算法，其主要原理也是利用機率的觀念估測機器人間的位置，此法允許機器人在全域的地圖中來估測彼此之間的位置，它將區域地圖與全域地圖做從頭到尾的比對，找出機器人最有可能所在的位置，不需要起始位置，也不怕定位失敗，但是可以想像計算量將非常巨大，特別是一開始的時候。為了降低機器人在定位過程的時間消耗，MCL 定位演算法隨之產生，MCL(Monte Carlo Localization)並不是把區域地圖與全域地圖做完全的比對， MCL 的重點在相信度函數(Believe Function)，其用來做估測及修正的方式在於不斷的取樣(Sampling)與重新取樣(Re-Sampling)，在全域地圖上定義一定數量的取樣點(Sample)或稱粒子(Particle)，將每個取樣點賦予一個機率值，當這些取樣點越收斂表示機器人的位置越有可能在那個地方，它確實降低了Markov 定位演算法的計算量，其計算的時間皆隨著其地圖的維度，大小，分割精確度決定；換句話說，其計算量的大小取決於分佈粒子的取樣數，因此較難滿足即時性(Real-Time)的要求。不過MCL 的優點在於能接受具有不同或多種的感測器模型的誤差分佈(Error Distribution)，不限於高斯分佈而已。
由Kalman Filter 與MCL 的定位方法比較中，我們發現MCL 定位在大地圖的時候比Kalman Filter 定位來的穩定(Stable)與強健(Robust)但是，從另一方面來看，Kalman Filter 定位卻比MCL 定位來的精確，只要其輸入的感測模型資訊越接近高斯分佈(Gaussian Distribution)時，其效能及精準度都會大大提高，且Kalman Filter 收斂速度比MCL 快，因此比MCL 更適合運用在即時的需求上。雖然說Kalman Filter演算法被提出的時間較早，探討的也較深入，但因為這兩種定位演算法各有其專長，因此運用KalmanFilter 定位演算法的人還是不少，也提出了許多的改良方法EX(:Extended Kalman filter(EKF)，Unscented Kalman Filter(UKF)‧‧‧等)。總結來說，Kalman Filter 較常被運用在小地圖的定位上，而MCL 定位演算法較常被運用在大地圖的定位。
如果SLAM 的問題是線性而且為Gaussian，則Kalman Filter 可以保證其結果為收斂錯誤! 找不到參照來源。且為最佳化之結果。然而實際上SLAM 的問題卻很少為線性的，因此Extended Kalman filter(EKF)才發展出來用以解決非線性的定位問題。Xt 代表機器人在時間為t 時的位置資訊，ut+1 為控制機器人往t+1 的時間行動的命令，融合Xt 和ut+1可以得到Xt+1 的位置，再跟機器人得到對環境的觀測資訊zt+1 做比較，就可以準確的定位出機器人的所在位置。如此一直疊代下去就可以得到每個時刻機器人的所在位置了。
圖4、Formulation of SLAM
0為SLAM的算式，藉由前一秒的位置與這一秒的命令推算出這一秒的位置，並且與感應器所觀察到的資料做比對，得到這一秒精準的機器人位置。0為Durrant-Whyte所說明的SLAM中所要解決的問題：因為無法得知機器人及環境Landmark確實的位置，所以要藉由Sensor來測量環境的資訊，以此估測出機器人的位置及環境地圖。例如當機器人位於Xk-1時，觀測到mi等兩個Landmark，將兩個沒見過的Landmark建立在地圖上後，亦估測出機器人本身的相對位置。因為機器人移動的距離及對Landmark的觀測值不可能是完全準確的，因此Landmark 及機器人位置均有一不確定性(Uncertainty) 存在；在Xk時，同時觀測的到mi、mj…等三個Landmark，除了將之前沒見過的mj等兩個Landmark加入地圖後，因為mi是第二次觀測到了，不僅更新了mi的位置，更能縮小其位置所在的不確定性，增加Landmark的正確度，同時依據觀測到的Landmark，也能再次估測出機器人的位置；以此循環，機器人便能完成同時定位且建立環境地圖的功能。
圖5、SLAM 演算法概念示意圖
在SLAM 中，解決機器人位置與地圖不確定性的機率問題是主要的議題之一，Kalman filter是蠻好的方法，但其缺點為必須使用在線性系統下，因此基於feature-based的Extended Kalman filter則是常用的解決方法，EKF具有收斂速度快、運算精度高、穩定性佳等優點，其演算法較為直接且容易實現，並且可用於非線性系統中，可應用在即時系統，有效克服數據的誤差，保持系統狀態估測的正確性。
我們將利用EKF做為解決雜訊融合資料的方法，並尋求EKF的改良版本Unscented Kalman Filter(UKF)，來達到比EKF效率更好，精準度更高的結果。藉由視覺判斷出Landmark的位置，並使用超音波與視覺資料的Covariance Intersection，達到同時SLAM與避障的效果。Unscented Kalman Filter(UKF)是根據EKF修改後所得到改良版的演算法，EKF須利用到泰勒展開式的一階微分得到計算所需要的係數，因此必須反覆的計算Jacobians，而UKF是藉由線性還原得到一個高斯分佈的平均值，因此不用每次預測和更新時都要重新計算Jacobian，總結來說，UKF和EKF都有快速收斂的高效率，但在泰勒展開式的第一項和第二項為係數時，UKF會比EKF更加來的線性，預測精確度也會較高如0 所示，我們可以看到UKF的預測誤差範圍幾乎完全覆蓋在正確預測誤差範圍上而EFK的預測誤差範圍則是偏了一小部分，且UKF少了反覆計算Jacobians的步驟，相對的來說演算法也就更加簡單。