slam算法說明：
SLAM 算法最早出現在 SmithSelf 和 Cheeseman 的論文中,他 們 運用擴展卡爾曼濾波對狀態空間中的機器人位姿和特征 ( 路標 ) 位置進行同 時 估 計 。 計 算 復 雜 度 高 一 直 是 SLAM 的 難 題 之 一 ，在 基 於 特 征 的SLAM 算法中,標准 EKF SLAM 方法的計算復雜度與環境特征個 數 呈二次方關系,這使得它只能在一般不超過上百個特征的較小范圍內應用。 因此,研究一種計算量可隨地圖大小進行縮放的 SLAM 算法成為一個公共難題。由於 SLAM 問題針對未知且不確定的環境,一般使用概率方法來描述該問題。 SLAM 的常用概率解法有兩類, EKF 算法 和 粒 子 濾 波 算法。 對於非線性系統的 SLAM 問題 EKF 算法和快速 SLAM(FastSLAM)算法是比較典型的兩種算法 。 粒子濾波方法對模型進行直接近似,它不要 求 控 制 向 量 和 觀 測 值 滿 足 高 斯 分 布 假 設 將 粒 子 濾 波 方 法 應 用 到高維 SLAM 問題,計算量非常大, FastSLAM 算法 把 聯 合SLAM狀 態 分成運動部分和條件地圖部分以縮小采樣空間 ,機器人的位姿用不同權值的粒子 表 示 ,地 圖 由 獨 立 的 高 斯 分 布 解 析 表 示 ,機 器 人 位 姿 狀 態 的遞歸估計采用粒子濾波方法,地圖狀態的遞歸估計采用 EKF 方法。
主要算法：
（1）、EKF 的 SLAM 算法
傳統的 SLAM 算法需要將不同時間、不同地點的感知信息進行匹配和聯合 ,存 在 局 部 數 據 之 間 的 關 聯 問 題 ,也 存 在 局 部 數 據 和 全 局 數據的關聯與匹配問題 [3] 。隨着 SLAM 問題研究的深入, 人們發 現 EKF 方 法 的 瓶 頸 在 於 其計算復雜性 。 對於其最優濾波,無論對濾波方程如何整理和計算,其計算復雜度都至少與地圖中特征個數的平方成正比 ,難以滿足構建大規模地圖和實時性的要求。 對於大規模地圖, EKF SLAM 算法難以避免出現不一致現象。 一致性 (consistency) 是指算法能得到無偏估計且能正確地給 出 估 計 方 差 ,近 幾 年 ,國 外 許 多 學 者 在 這 方 面 已 經 進 行 了 研究 , 分析了模型誤差、線性化誤差等因素對一致性的影響。 這些研究表明, EKF 算法並不適合大規模的 SLAM 。基於擴展卡爾曼濾波的 SLAM 算法是一種經典方法,雖然該方法計算復雜度高且無法處理“閉環”問題,但其收斂性是目前所有算法中最好的,因此至今仍在陸地、水下及空中等場合得到廣泛的應用。
（2）、FAST SLAM
與基於 Kalman 濾波的 SLAM 不同, RBPF SLAM 采用粒子 來 表 示機器人的位姿,而環境特征的估計依然利用 EKF 解析計算。 這種將狀態分解為采樣部分和解析部分的粒子濾波方法一般稱為 RBPF 。 由於這 種 分 解 , RBPF SLAM 的 計 算 復 雜 度 為 O(NM) ,其 中 N , M 分 別 為 所用粒子的數目及特征個數,如給定粒子數 N , RBPF SLAM 的 計 算 復 雜度與特征個數 M 成線性關系,而傳統 EKF SLAM 的復雜度為 O(M) ,因此 RBPF SLAM 也被稱為 FAST SLAM , 這種算法降低了計算復雜度 ,是 SLAM 的一種常用算法 。FAST SLAM 算 法 是 同 時 定 位 與 地 圖 創 建 領 域 的 一 類 重 要 方 法 ,UPF-IEKF FastSLAM 2.0 算法采用 UPF 估計機器人的路徑,地圖估計則采用 IEKF 算法 UPF 算法使粒子向后驗概率高的區域運動,提高了估 計 精 度 同 時 降 低 計 算 復 雜 度 ; IEKF 算 法 通 過 迭 代 觀 測 更 新 過 程 來提高估計精度。 地圖估計和路徑估計是相互關聯的 ,路徑和地圖估計的精度主要取決於路徑估計的方法,采用 UKF 估計地圖時,雖然估計精度有所提高,計算量卻高於 EKF 算法。 綜合考慮估計精度和計算復雜度, 認為 “ UPF-IEKF ” 是一種更合理的 FastSLAM 算法濾波架構,UPF-IEKF FastSLAM 2.0 ( n=2 )算 法 是 FastSLAM 2.0 系 列 算 法 中 最 理想的算法。