HectorSLAM論文解析?代碼重寫（2）
這篇文章為HectorSLAM系列的以下部分
- HectorSLAM的整體邏輯
- 激光匹配
- 地圖構造
- 地圖更新
- 500行代碼重寫一個LidarSLAM
- 測試數據的準備，和測試數據讀取模塊的編寫
- GUI編寫
- 地圖模塊的編寫
- 核心模塊的編寫
- 主循環
- 匹配算法
首先繪制一張以掃描匹配為核心的SLAM（例如Hector,Karto_slam,Cartographer..）大概的流程圖：
基本上以這一類SLAM流程分以下幾個部分：
- motion prediction（運動預測）
- scan matching（掃描匹配）
- map update（地圖更新）
- pose update（姿態更新）
因為原版的Hector並沒有使用車輪odometry。所以運動預測部分，僅為根據前一個狀態的位置和速度的預測。所以我們直接從跳過motion prediction，下面從２.scan matching（掃描匹配）開始談起。
１.scan matching（掃描匹配）
所謂匹配就是把從傳感器得到的數據和已經知道的地圖進行匹配。我們首先假定周圍環境是靜態的，在靜態的環境中，傳感器數據應該和已知的地圖情報應該高度一致才對。所以在沒有噪音的情況下以下方程應該成立：
$\sum _{i=0}^{n}\left[ 1-M\left( S_{i}\left( \xi \right) \right) \right] = 0$
其中$\xi=\left(p_x,p_y,\psi\right)$代表機器人的姿態(x坐標，y坐標，角度)
Si代表，i號激光束，在姿態下ξ的照射到的障礙物坐標。
M代表某坐標為障礙物的概率。１為１００％為障礙物，０為１００％
在傳感器沒有噪音，地圖情已知且沒有誤差的的情況下，可以求解以上方程，得到未知量ξ（姿態）。直觀上來說就是調整姿態使得所有的激光都正確的測量到障礙物。但現實的情況卻沒有怎麽完美，傳感器有噪音，地圖有誤差，且部分未知。所以我們轉而求解以下最小二乘法：
$argmin_{\xi}\sum _{i=0}^{n}\left[ 1-M\left( S_{i}\left( \xi \right) \right) \right]^2$
這個公式的就求解方法，在論文的公式（９）～（１３）有推導過程。這裏雖然不推導（論文已經足夠詳細），但是仍然建議自己推導一次加深理解。
根據下面的方程我們似乎已經可以通過掃描匹配解出機器人姿態了。
２.地圖構造
我們似乎可以解出機器人姿態了。。。其實我們還不行的。。。因為求解以上（１２）（１３）是需要求M的偏導的，我們知道求解偏導的是需要連續空間的，但Hector使用的格子地圖（grid map）存在於離散的空間中。
所以，首先我們需要一些方法把我們的離散的格子地圖，轉換為可以求導的連續地圖。
HectorSLAM論文解析?代碼重寫（2）