HectorSLAM論文解析・代碼重寫（2）

這篇文章爲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流程分如下幾個部分：

1. motion prediction（運動預測）
2. scan matching（掃描匹配）
3. map update（地圖更新）
4. 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的取值。

 

越近的點權重越大，具體公式爲：

至此咱們已經得到了一個能夠求偏導的地圖，具體的求偏導方法爲如下：

 

3.map update（地圖更新）

在求解出機器人姿態之後，咱們已經可以根據如今姿態更新地圖了，

更新地圖具體方法爲，對於地圖的網格的原始數據（raw data）：

• 激光束的頂點（hit），存在障礙物的可能性變高，因此增長一個權重
• 激光束上的其餘點（miss），存在障礙物的可能性變小，因此減小一個權重
• 激光束的始點（original point），存在障礙物的可能性變小，因此減小一個權重

根據如下公式能夠求出障礙物存在的機率：

$M=\dfrac {e^{x}} {1+e^{x}}$

其中x表明網格的原始數據的值，M表明障礙物存在的機率，這個函數是形以下圖：

咱們能夠看網格的原始數據的值越大，越接近1，反之越接近0，很完美的實現的上文中對M的定義

M表明某座標爲障礙物的機率。１爲100%爲障礙物，０爲100%空閒區域

 

 最後更新姿態（並無須要細說的地方），至此基本的SLAM的框架已經創建好了。

 

既然理論咱們已經清楚，那麼從下一篇文章開始，咱們就用python，手把手把上面的內容用代碼實現。