激光SLAM基本原理
基本原理
關於機器人運動控制系統架構，在《ros by example》 chapter 7一章第二節中介紹了控制機器人的5個層次，從低到高依次是：motor controllers anddrivers-> ROS base controller ->Frame-Base Motion（move_base）->Frame-Base Motion（gmapping + amcl）->Semantic Goals。總結起來如下圖所示：
可簡單的分爲三個層面，最底層，中間通信層和決策層。最底層就是機器人本身的電機驅動和控制部分，中間通信層是底層控制部分和決策層的通信通路，決策層就是負責機器人的建圖定位以及導航。本文主要研究激光SLAM(構建2D地圖和導航)，所以只探討決策層這一層的實現。我們在已有機器人最底層的前提下，採用ROS提供的Gmapping包和Navigation棧作爲機器人的決策層。
1、佔據柵格地圖基本原理
Gmapping包是在ROS裏對開源社區openslam下gmapping算法的C++實現，該算法採用一種高效的Rao-Blackwellized粒子濾波將收取到的激光測距數據最終轉換爲柵格地圖。
機器人定位與建圖通常被認爲是“雞與雞蛋”的問題，因爲這個原因纔會將這個過程命名爲SLAM（Simultaneous localization and mapping），所以即時定位與地圖構建（SLAM）是這樣一個概念：把兩方面的進程都捆綁在一個循環之中，以此支持雙方在各自進程中都求得連續解；不同進程中相互迭代的反饋對雙方的連續解有改進作用。
佔據柵格地圖的構建主要採取粒子濾波的方法，粒子濾波是目前一種可以代替高斯濾波器的廣爲流行的濾波器是非參數化濾波器。[這句話的描述不清晰]非參數化濾波器不需要滿足擴展卡爾曼濾波算法所要求的非線性濾波隨機量必須滿足高斯分佈的條件，它也不依賴於一個固定的後驗方程去估計後驗狀態，而是從後驗概率中抽取隨機狀態粒子來表達其分佈。粒子濾波就是一種非參數化濾波器的實現算法，粒子濾波的關鍵是從後驗分佈中產生一組隨機狀態樣本來表示後驗概率分佈。
粒子濾波的思想基於蒙特卡洛方法來表示概率[粒子濾波的思想是基於蒙特卡洛方法來表示概率]，可以用在任何形式的狀態空間模型上。其核心思想是通過從後驗概率（觀測方程）中抽取的隨機狀態粒子來表達其分佈，是一種循序重要性採樣法。簡單來說，粒子濾波法是指通過尋找一組在狀態空間傳播的隨機樣本對概率密度函數進行近似，以樣本均值代替積分運算(狀態方程)，從而獲得狀態最小方差分佈的過程。這裏的樣本即指粒子,當樣本數量N→∝時可以逼近任何形式的概率分佈。雖然在粒子濾波算法中，其概率分佈僅僅是真實分佈的一種近似，但由於粒子濾波是非參數化的，它解決了非線性濾波問題中隨機量必須滿足高斯分佈的缺陷，能表達相較於高斯分佈模型而言更爲廣泛的分佈，也對變量參數的非線性特性有更強的建模能力。因此，粒子濾波能夠比較精確地表達基於觀測量和控制量的後驗概率分佈，可以用於解決SLAM問題。
在粒子濾波中，後驗分佈的樣本，我們稱之爲“粒子”，每一個粒子都是在時刻t的一個狀態的實例化，這個實例化就是在t時刻的真實狀態的假設。這裏M代表粒子集中粒子的總數。在實際環境中，粒子總數M通常是一個較大的數字，在一些實現方法中，M是與時間t或者其它與狀態的概率密度相關的函數。
粒子濾波的思想是通過一組粒子來估計近似狀態概率。理論上，粒子集中一個假設狀態粒子的概率應該與貝葉斯濾波器t時刻後驗概率成一定比例關係。
粒子濾波主要步驟如下：
（1）初始化階段：
規定粒子數量，將粒子平均的分佈在規劃區域，規劃區域需要人爲或者通過特徵算法計算得出，比如人臉追蹤，初始化階段需要人爲標出圖片中人臉範圍或者使用人臉識別算法識別出人臉區域。對於SLAM來說，規劃區域一般爲用來進行定位的地圖，在初始化時，將需要設置的特定數量粒子均勻的撒滿整張地圖。
（2）轉移階段：
這個階段所做的任務就是對每個粒子根據狀態轉移方程進行狀態估計，每個粒子將會產生一個與之相對應的預測粒子。這一步同卡爾曼濾波方法相同，只是卡爾曼是對一個狀態進行狀態估計，粒子濾波是對大量樣本（每個粒子即是一個樣本）進行狀態估計。
（3）決策階段：
決策階段也稱校正階段。在這一階段中，算法需要對預測粒子進行評價，越接近於真實狀態的粒子，其權重越大，反之，與真實值相差較大的粒子，其權重越小。此步驟是爲重採樣做準備。在SLAM中權重計算方式有很多，比如機器人行走過程中，激光雷達或者深度攝像頭會返回周圍位置信息，如果這些信息與期望值相差較大，亦或者在運動中某些粒子本應該沒有碰到障礙或者邊界，然而在運算中卻到達甚至穿過了障礙點或邊界，那麼這種粒子就是壞點粒子，這樣的粒子權重也就比較低一些。
（4）重採樣階段：
根據粒子權重對粒子進行篩選，篩選過程中，既要大量保留權重大的粒子，又要有一小部分權重小的粒子；權重小的粒子有些會被淘汰，爲了保證粒子總數不變，一般會在權值較高的粒子附近加入一些新的粒子。
（5）濾波：
將重採樣後的粒子帶入狀態轉移方程得到新的預測粒子，然後將它們繼續進行上述轉移、決策、重採樣過程，經過這種循環迭代，最終絕大部分粒子會聚集在與真實值最接近的區域內，從而得到機器人準確的位置，實現定位。
（6）地圖生成：
每個粒子都攜帶一個路徑地圖，整個過程下來，我們選取最優的粒子，即可獲得規劃區域的柵格地圖。
2、導航基本原理
Navigation棧[是否爲Navigation包]是一個獲取里程計信息、傳感器數據和目標位姿並輸出安全的速度命令到運動平臺的2D導航包的集合。
(1) 定位
機器人在導航的過程中需要時刻確定自身當前的位置，Navigation 棧中使用amcl包來定位。amcl是一種概率定位系統，以2D方式對移動機器人定位，它實現了自適應（或者KLD-採樣）蒙特卡洛定位法，使用粒子濾波跟蹤機器人在已知地圖中的位姿。下面的圖片顯示用里程計和AMCL定位的不同之處，AMCL估計base結構（機器人）相當於global結構(世界地圖)TF轉換（ROS中的座標系轉換）。從本質上，這種轉換利用航位推算來處理漂移，所發佈的轉換是遠期的。
(2)路徑規劃
路徑導航部分則使用move_base包，move_base能夠獲取機器人周圍信息（如激光雷達掃描結果）並生成全局與局部的代價地圖，根據這些代價地圖可以使機器人繞開障礙物安全到達指定的位置。move_base的路徑規劃主要分爲全局規劃和局部規劃，分別採用的是A*算法和DWA（Dynamic Window Approach)）算法。