這篇文章為HectorSLAM系列的以下部分

HectorSLAM的整體邏輯

激光匹配

地圖構造

地圖更新

500行代碼重寫一個LidarSLAM

測試數據的准備，和測試數據讀取模塊的編寫

GUI編寫

地圖模塊的編寫

核心模塊的編寫

主循環

匹配算法

參考以下論文

A flexible and scalable SLAM system with full 3D motion estimation
https://www.researchgate.net/publication/228852006_A_flexible_and_scalable_SLAM_system_with_full_3D_motion_estimation

首先繪制一張以掃描匹配為核心的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代表某坐標為障礙物的概率。１為100%為障礙物，０為100%空閑區域

在傳感器沒有噪音，地圖已知且沒有誤差的的情況下，可以求解以上方程，得到未知量ξ（姿態）。直觀上來說就是調整姿態使得所有的激光都正確的測量到障礙物。但現實的情況卻沒有怎么完美，傳感器有噪音，地圖有誤差，且部分未知。所以我們轉而求解以下最小二乘：

$argmin_{\xi}\sum _{i=0}^{n}\left[ 1-M\left( S_{i}\left( \xi \right) \right) \right]^2$

這個式子的求解方法，在論文的公式（９）～（１３）有推導過程。這里雖然不推導（論文已經足夠詳細），但是仍然建議自己推導一次加深理解。

根據下面的方程我們似乎已經可以通過掃描匹配的方法解出機器人姿態了。

原版的Hector中，為了避免對姿態的求解陷入局部最優解，用了幾種不同的分辨率表示地圖。首先在分辨率較低的地圖中求解姿態，然后逐漸帶入較高分辨率的地圖中。

２.地圖構造

但為什么只是似乎呢？因為求解以上（12）（13）是需要求M的偏導的，我們知道求解偏導的是需要連續空間的，但Hector使用的格子地圖（grid map）存在於離散的空間中。所以，首先我們需要一些方法把我們的離散的格子地圖，轉換為可以求導的連續地圖。論文的【IV. 2D SLAM】介紹了一種用Bilinear filtering的解決辦法：

這個方法直觀的理解：比如下圖中我們要求點P_m的值時，由於格子地圖是離散的，所以並沒有辦法直接取得。我們轉而根據距離P_m最近的4個點P₀₀,P₁₀,P₁₀P₁₁，估計出P_m的取值。