轉自：https://blog.csdn.net/hltt3838/article/details/109261334
固態激光雷達的一段視頻：https://v.qq.com/x/page/a0787ajele3.html
注意：本人實驗室買的是Velodyne VLP-16激光雷和 LOAM 論文中作者用的不一樣，在介紹論文之前先介紹一下激光雷達的工作原路，這樣更容易理解激光雷達的工作過程，其實物圖如下圖1所示：
圖1 Velodyne VLP-16激光雷 實物圖
簡介：
- Velodyne PUCK（VLP-16）16通道
- TOF測量距離，最遠100m，精度3cm
- 垂直分辨率2°
- 水平分辨率600、1200RPM對應0.2°、0.4°
- 旋轉速率5-20Hz
- 最大點雲數據量 360/0.4161200/60=288000（30萬）
相關概念
謝謝Robots.https://blog.csdn.net/i_robots/article/details/108331306#comments_13621877
首先明確作者論文中用的二維激光雷達只是水平方向旋轉掃描，此時一次水平掃描完成的數據為一次scan；三維激光雷達（多線雷達，激光束在垂直方向按照時間間隔依次發射）先垂直掃描再水平旋轉實現三維掃描，此時一次三維掃描稱為一次scan。雖然二維激光和三維激光都有一次scan的術語，但是對應的含義只是說明一次完整的掃描過程，實際表示的數據並不一樣。原論文作者使用二維激光實現三維激光，需要添加機械結構實現垂直掃描，這樣就會導致多個scan才是我們需要的三維掃描，那么這多個scan稱為一次sweep。也就是，這里的一次sweep對應於三維激光中的一次scan，二維激光的一次scan不等於三維激光的一次scan
疑問：16線激光雷達是同時發射的還是間隔發射？
答：
一、Velodyne VLP-16激光雷達工作原理
VLP-16型號雷達具有在垂直方向上 16線 的激光束，激光雷達在采集三維數據時，每一步的旋轉（旋轉頻率可設定）可在空間上采集16個點的三維數據。在垂直方向上的視角范圍為-15度到+15度，每個激光束對應的角度分別為如下圖所示：
圖2 Velodyne VLP-16 ID和原理圖
16線 的激光束在每一時刻都會測量出對應的距離，而且每個激光束的角度也是知道的，那么怎么把 角度和距離 信息轉化為三維坐標XYZ值，如下圖所示：
其中R的值為激光雷達測得的與障礙物的直線距離，其中垂直角度ω可通過查表方法獲得，每個激光束對應的角度ω是固定的本文第一張圖所示，而α是旋轉角度值。已知旋轉角度α、垂直角度ω和R 通過圖中所述的公式即可求得XYZ值，這便是3D激光雷達測量環境三維信心的原理。
注意：
vlp16的坐標系為 右前上，並且掃描時 順時針旋轉，通過安裝可以將坐標系變換為 前左上，因此我們在數據處理時只需要將 前左上 坐標系的點雲數據轉換為 左上前 坐標系即可。因為，在LOAM中，使用的坐標系是 左上前 坐標系，也就是X指向左，Y指向上，Z指向前。
圖3 Velodyne VLP-16 坐標系
二、各部分論文分析
2.1、 摘要
目標：
使用一個三維空間中運動的 兩軸單線激光雷達 來構建 實時激光里程計並建圖
難點：
點雲不是相同時間獲得的，因為點雲中的點隨着激光雷達運動會產生運動畸變，也就是點雲中的點會相對實際環境中的物品表面上的點存在位置上的誤差。這種運動畸變會造成點雲在匹配時發生錯誤，從而不能正確獲得兩幀點雲的相對位置關系也就無法獲得正確的里程計信息。
解決算法：
一方面執行高頻率的里程計但是低精度的運動估計用於 定位；另一方面算法在比定位低一個數量級的頻率執行匹配和注冊點雲信息，實現建圖和校正里程計
2.2、 Introduction
激光雷達的優勢: 測距頻率高、准確，並測量產生相對固定的誤差且與距離無關
激光雷達的缺點: 定位和建圖的復雜度、運動畸變問題，也就是 低漂移和低計算量 問題
解決方法：
1、特征點匹配
匹配問題分為scan-to-scan、scan-to-map、map-to-map。scan-to-scan匹配：優點是計算量小速度快，缺點是誤差累計大，長距離誤差累計后地圖就無法看了; map-to-map的匹配：優點是精度高，誤差累計小；缺點就是計算量大，實時性壓力大。scan-to-map的匹配居中。LOAM的優點就是很好地利用力匹配的優缺點，首先，雖然scan-to-scan匹配精度差，但是我們可以只是使用它做一個獲取粗的里程計，用獲取的結果用於去除勻速運動造成的運動畸變，由於scan-to-scan的計算量較小因此我們可以高頻執行。其次，有了里程計校正后的點雲數據，接下來我們就可以做一個map-to-map的匹配了。但是map-to-map存在計算量大的問題，因此 我們可以讓其執行的頻率降低。這樣的高低頻率結合就保證了計算量的同時又兼具了精度。
2、特征點的選取
特征點提取方法：特征向量、直方圖、旋轉圖片等，方法好不好，好但是計算量大。 作者提出了新穎的方法就是提取邊緣點和平面點作為特征點來使用，這種提取的方法就很簡單只需要計算一個點前后五個點就可以得到該點的曲率，計算量就減少了很多
圖4 問題 和 改正結果
2.3、 相關工作
激光的轉速 相對 激光雷達本體運動的速度 高很多的話就可以忽略由於運動造成的 運動畸變，那么直接使用標准的ICP就可以完成兩幀點雲匹配。實際情況下：需要有 速度估計 來去除運動造成的運動畸變。如果激光雷達的旋轉速度相對較慢，則會產生非常嚴重的 運動畸變。特別是對兩軸激光雷達，其中一個軸的速度相對較慢。通常使用其他的傳感器獲得運動速度用於去除運動畸變。例如可以使用視覺里程計和IMU進行插值去除運動畸變，當使用多傳感器時可以使用卡爾曼進行濾波。這樣就可以獲得實時的地圖用於機器人導航時的路徑規划和避障。如果沒有其它傳感器輔助，那么運動估計和畸變校正就是一個難題。常用的方法是使用激光點雲的強度構建圖像，根據兩幀數據間圖像的位移估計地面車輛的速度，車輛是基於 勻速運動 模型進行計算 （也是需要改進的地方，畢竟勻速太理想了）。
所謂的畸變，如下圖所示：
圖5 激光雷達運動畸變圖
畸變描述：單線激光雷達為例。激光雷達沒旋轉一周返回一幀數據，.如上圖左邊所示我們使用單線激光對環境進行觀測。激光雷達靜止的話激光返回的點雲應該如上圖中右邊圖中的 紅色 點雲，但是激光在獲取一幀點雲的過程中時不斷運動的，這就造成 一幀點雲數據中越早獲得的點相對其對應的真實位置偏移量越大，形成上圖右邊的藍色線。上圖中激光雷達 朝向障礙物運動，旋向為逆時針，這樣藍色點雲右邊點在激光雷達運動方向上的畸變就要大於左邊點的偏移量。
解釋：是因為，先接受的點，距離遠，由於運動產生的誤差大；而近的點也會有誤差，但是相對遠距離來說小一點，因為激光的速度是不變的，發射激光的位置和接受的位置不在一個地方，你可以用一個極限的思想去考慮這個問題，不知道本人理解的對不對！
2.4、表示和任務描述
任務：使用激光雷達獲得的點雲數據進行運動估計並構建遍歷過環境的地圖
注意：局部坐標系中，x軸指向左邊，y軸指向上，z軸指向前（這里需要注意和通常傳感器安裝的方向不一樣，全局坐標系在初始時刻和局部坐標系相同
解決問題：給定點雲計算每次掃描的運動並使用點雲構建地圖
2.5、系統概括
SLAM分為兩個部分：10Hz的激光里程計和1Hz的建圖
激光里程計使用基於特征點進行相鄰點雲幀數據匹配，
激光建圖使用基於特征點進行校正后輸入點雲數據和地圖拼接。
程序流程描述：
圖6 系統軟件結構圖
解釋：
A、特征點提取
使用的特征點和對應的特征區域有兩類：
- 使用
邊緣點以及邊緣點所在邊緣線， - 使用
平面點以及平面點所在平面。
怎么對邊緣點和平面點進行分類呢？設S是第k 幀點雲數據中點i 對應X(k,i)_L附近的一個連續點集合。定義一個“距離值”c 來評估局部曲面的平滑度，用於邊緣特征點、平面特征點和普通點的分類：
k次sweep的點雲數據 測量的位置（XYZ）， 哈哈，可惜的是程序和這個公式不是對應的，你們可以看看
- 掃描中的點根據c 值進行排序，然后用最大c 值（即邊緣點）和最小c 值（即平面點）選擇特征點。
- 一次掃描分為四個均等的區域。每個區域中最多可提供2個邊緣點和4個平面點。
- 不能位於與激光束大致平行的曲面片上
- 不能位於遮擋區域的邊界上
綜上所述：
從最大c 值開始選擇特征點作為邊緣點，從最小c 值開始選擇平面點，每個區域中邊緣點不超過2個，平面點不超過4個，選中點周圍的點不能被選擇 且 不能選擇位於與激光束大致平行的曲面上的點 以及位於遮擋區域的邊界上的點
圖7 遮擋
圖8 平面點和邊緣點（紅色為平面特征點，黃色為邊緣特征點）
B、特征點匹配關聯
Pk表示第k次sweep的點雲數據, Pk開始的時間戳為tk ，結束的時間戳為t k + 1，經過畸變校正之后將Pk 重投影到t k + 1 得到P ˉ k 。估計激光雷達運動時，使用的是重投影點雲數據(經過畸變校正)P ˉ k 和新的點雲數據（未經過畸變校正）P k + 1 , 他們代表的線如下圖所示：
圖10 Finding an edge line as the correspondence for an edge point in E˜k+1 (a), and a planar patch as the correspondence for a planar point in H˜k+1 (b). In both (a) and (b), j is the closest point to the feature point, found in P¯k. The orange colored lines represent the same scan of j, and the blue colored lines are the two consecutive scans. To fifind the edge line correspondence in (a), we fifind another point, l, on the blue colored lines, and the correspondence is represented as (j, l). To fifind the planar patch correspondence in (b), we fifind another two points, l and m, on the orange and blue colored lines, respectively. The correspondence is (j, l, m).
有了點到線和點到面的對應關系，接下來就要求點到線和點到面的 距離。首先求點到線段的距離，公式如下圖所示：
1、點到線的理解圖
圖11 點到線的理解圖
接下來求平面點到對應平面的距離，距離公式如下圖所示：
公式的分子包括兩部分，上邊是獲得一個向量，下邊也是獲得一個向量，但兩個向量叉乘再取模就表示的求下邊得到三角形面積上面表示立方體的高，兩者相乘就表示立方體體積。而分母表示立方體底面三角形的面條，得到的高就是平面點到平面的距離。直觀上的理解如下圖所示:
2、 點到面的理解圖
圖12 點到面的理解圖
C、運動估計
為了獲得這一幀數據中的點和上一幀數據中點的對應關系，我們使用一個旋轉矩陣R和一個平移量T表示。
這里需要把時間點對齊，才能求解上面的點到線和點到面的距離d ！
由於旋轉矩陣求導非常復雜，所以用羅德里傑斯公式將旋轉矩陣R展開為:
這樣就很容易對旋轉矩陣進行求導。
到此我們有了點到線和面的距離，並獲得用於優化的誤差函數：
這是一個非線性優化問題，可以通過 GN 求解，直到d最小化
f中的每一行代表一個特征點，接下就要求解雅克比矩陣，最后使用LM的方法進行優化。
優化的過程中是對每一次迭代都尋找一次最近鄰的點。
此時如果當前幀計算完成，則將當前的P k + 1 P_{k+1}Pk+1點雲則在t k + 2 t_{k+2}tk+2時刻 轉換到激光世界坐標系下。
激光雷達里程計具體的流程
圖13激光雷達里程計具體的流程
2.6、建圖
累計一定幀數目的點雲數據才會進行建圖，頻率較低，但是精度很高。目的是將點雲數據融入到世界地圖中，精確估計激光在世界坐標系的位姿。與里程計相比，這里構建邊緣線，平面時使用的點數大大增加，因此需要采用其他方法得到線向量，法向量。
因為里程計使用的點數較少，因此對於線，使用兩個點就可以確定這個線向量，但是對於建圖部分使用的點數較多，不能直接得到線向量，對於這個問題如何結果，作者使用了協方差矩陣。
根據數學知識，可以得到一個區域的三維點坐標分布與這些點三維坐標形成的協方差矩陣是存在一定關系的。對應的話就是
對於在近似一條線上的點，它們的協方差矩陣中特征值存在兩小一大（λ 1 > > λ 2 > λ 3 ），其中λ 1 對應的特征向量就是這些點所在的線向量。對於在近似平面的點，它們的協方差矩陣中特征值存在兩大一小（λ 1 > λ 2 > > λ 3 ），其中λ 3 對應的特征向量就是這些點所在平面的法向量。
LOAM的優點：
將里程計和建圖分隔開，一個高頻低精(處理每次幀數據)，一個低頻高精(累積一定次數)
整理框架是串行結構，將整個問題逐步划分為多個層次。
可實時建圖的開源3D Lidar SLAM
LOAM的缺點：
點雲特征處理可進一步改善：運動的人等物體回環檢測
假設勻速運動模型
三、參考資料
https://www.zhihu.com/people/li-shuai-xin-36/posts
https://blog.csdn.net/i_robots/article/details/108331306