- 一、概念介紹
- 1.1、激光雷達傳感器
- 1.2、Lidar數學模型
- 1.3、運動畸變
- 二、畸變去除
- 2.1、純估計（ICP）
- 2.2、里程計輔助法(外部傳感器輔助)
一、概念介紹
1.1、激光雷達傳感器
三角測距，例如：類似於雙目測距原理，對於lidar，A是激光發射器，B是接收器
飛速時間差，沒啥好說
三角測距 飛行時間(TOF)
- 中近距離精度較高(測距范圍受基線距離限制) 測距范圍廣
- 價格便宜 （400-3000） 測距精度高
- 遠距離精度較差 抗干擾能力強
- 易受干擾 價格昂貴(幾萬級別)
- 一般在室內使用 室內室外皆可
室內場景中，測距范圍小於15m的時候，可以選用左邊三角測距類型雷達；
如果是室外場景，選用TOF雷達（300線雷達 約等於 深度相機）
三角測距原理：
已知基線L(線段AB)、alpha、beta(這三個參數都是標定好的)，求解d，過於簡單，不多BB
TOF測距原理：
略
1.2、激光雷達數學模型
前言：
我們知道目前的激光SLAM算法主要分為兩種：基於濾波的SLAM和基於圖優化的SLAM。而基於濾波的SLAM問題又是基於貝葉斯濾波的框架。因此我們不僅要問為什么需要使用貝葉斯來做濾波？
概念：什么是濾波？
濾波，即濾除噪聲。我們知道在估計狀態的過程中，控制引入運動噪聲和觀測引入傳感器測量噪聲。而濾波的實現是通過控制和觀測獲得的對同一狀態不同的冗余信息（控制的預測信息和觀測的測量信息）來減小位姿狀態的不確定度，從而濾除控制和觀測過程中的噪聲。例如：若控制和觀測的噪聲都服從高斯分別，那么控制和觀測得到的對狀態估計也都是服從高斯分布。將兩個高斯估計相乘得到后驗分別的不確定度就會下降，從而達到濾波的效果。
原因：為什么用貝葉斯做濾波？
首先，從邏輯上講，機器人SLAM的過程是：先控制機器人到達一個位姿，然后再進行觀測。但是問題出現在了我們是由控制數據和觀測數據來求機器人位姿，這就出現了一個逆向的過程即：我們是先有位姿再得到觀測數據卻變成了先得到觀測數據再來求位姿的過程。通俗來講就是我們需要由結果（觀測數據）來反推原因（位姿）；因此這里就引入了貝葉斯公式，貝葉斯公式的存在就是為了求解這種由結果反推原因的問題；不懂的小伙伴可以看一下貝葉斯公式的由來。
這里參考：https://blog.csdn.net/liuyanpeng12333/article/details/81003340
1.2.1 光束模型(beam model)
激光的觀測值的概率分布公式：
上式中，xt表示基線位置（好像不太對），m表示地圖，zt表示觀測到的距離值；p(zt | xt, m):已知地圖，雷達基線位置，得到zt值的概率。（可參考書籍《概率機器人》）注：等式右邊是連乘，我猜應該是為了求解極大似然，求解方法應該就是取負對數，轉為求極小值。（概率可理解為：什么樣的位姿xt，地圖m才能導致現在的觀測值是zt）
上述的距離觀測概率值p(zt | xt, m)的分布有如下四種形式：
對上圖的解釋：
- a、常規測距的概率分布
- b、有遮擋物的時候的概率分布（講課真尼瑪水，幾百的課程，也就這B樣了）
- c、忽略不計
- d、忽略不計
p(zt | xt, m)也就是置信度得分（ score(zt) ），表示的是：當前這一束激光測距與地圖重合程度，其實就是目標函數，作評價、優化用。
光束模型(beam model)缺點：
- 期望值的計算需要用raytracing,每一個位姿需要進行N次raytracing,N為一幀激光的激光束數量（計算量大，raytracing：光線追蹤）。
- 在非結構化環境中(clutter),位姿微小的改變會造成期望值的巨大變化,從而導致得分進行突變。
- （假如機器人在t時刻，雷達的一幀點雲中的某一束激光剛好觀測到空間中的一個點，score = 1；在t+1時刻，機器人動了一點點，很可能沒“看到”剛才那個點，這時候score = 0）
混合模型p(zt | xt, m)示意圖
1.2.2 似然場模型(likelihood model)（勞資是沒聽懂講的沙JB玩意）
- 對圖像(如下右圖，應該是深度圖)進行高斯平滑,在任何環境中期望值對於位姿都是平滑的。
- 得分的計算不需要經過raytracking,直接通過查表即可得到,計算量低
- 同時適合結構化環境和非結構環境中
1.2、激光雷達運動畸變
雷達一般有運動畸變不變性的假設，如果你的雷達有50Hz，這種假設是很逼真的，但是如果只有5-10Hz，這種假設前提帶來的誤差就比較大，也就是運動畸變很大。
如上圖(b)中，藍色地圖就是運動畸變導致的跑偏，原因有：
- 一幀數據激光點數據不是瞬時獲得
- 激光測量時伴隨着機器人的運動（所以嚴格意義上：只要機器人拖着雷達動，雷帶采集一貞數據中的每一個點，都對應不同的機器人位姿；如果雷達幀率足夠高，機器人運動幅度足夠小，可以近似認為所有點對應同一個位姿，很多算法也是這樣假設的）
- 激光幀率較低時,機器人的運動不能忽略
下一章節，說說怎么去除運動畸變
二、激光雷達運動畸變去除
2.1 基於純估計法的畸變去除
這里用ICP算法去除畸變。
ICP算法懶得BB。
簡單介紹下EM算法(馬爾科夫）：
例如，我們要求解優化變量A、B，步驟：
- 1、固定A，優化B，直到損失函數最小化，
- 2、固定B，優化A，直到損失函數最小化，
- 不斷重復過程1、2，直到收斂為止。
ICP算法可以看作EM算法的一個特例，在ICP算法中，R|t相當於A， 兩幀點雲中3D點對匹配關系相當於B；上圖中，藍色、紅色兩條線是一個ICP配准的過程。
ICP算法用在去除運動畸變的缺陷有：
- 沒有考慮激光的運動畸變
- 當前激光數據的是錯誤的，則會導致算法誤差很大(這好像是廢話)
這里介紹ICP的變種算法VICP(velocity ICP)算法，沒有忽略雷達的運動，但是假定雷達是勻速直線運動（比假定不動好一點）
上述機器人速度公式中，對於齊次矩陣T少寫了一個ln(李群 -> 李代數)，T應該表示：當前幀 到 world 的變換，Vi是一個向量。 假定雷達采集一幀數據過程中，采樣周期是Δt，所以有上述時刻表。
如何依據時刻 i 的位姿 Ti，得到時刻(ti - (n - j) Δt) 的位姿 T(ti - (n - j) Δt) ，如下公式，這兩個時刻的時間差為 (n - j) *Δt，這里自然底數e表示：李代數 -> 李群。
所以可得運動矯正公式：
Note：速度Vi帶了負號，因為當前時刻到歷史時刻，方向是反的。
VICP算法流程和效果圖如下圖：
在 1.2、激光雷達運動畸變中，有個ICP矯正后的，通過對比，可以發現：VICP還是具有更好的效果的。
2.2、里程計輔助法(外部傳感器輔助)
用IMU進行位姿估計，而不是雷達，IMU幀率足夠高！
當然也可以使用其他傳感器輔助去除Lidar運動畸變，例如：論速計
輪速計效果好一點。
任意時間點的位姿如果不能直接得到，可以通過插值得到，如下公式。
還可以進行二次多項式差值，懶得寫了，簡單，不多BB。
嚴格意義上，一幀點雲中，每個點都應一個獨特的機器人位姿，其對應的位姿都可以通過上述插值得到，這樣就得到一幀矯正后的點雲。
