鐳射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）地圖生成：
每個粒子都攜帶一個路徑地圖，整個過程下來，我們選取最優的粒子，即可獲得規劃區域的柵格地圖。
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)）演算法。
鐳射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）地圖生成：
每個粒子都攜帶一個路徑地圖，整個過程下來，我們選取最優的粒子，即可獲得規劃區域的柵格地圖。
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)）演算法。