介紹
Slam：同步定位與建圖，就是在定位的同時，建立環(huán)境地圖。
主要思路是根據(jù)運動學(xué)模型計算位姿，并通過傳感得到的環(huán)境信息，對估計位姿調(diào)整優(yōu)化，從而得到準(zhǔn)確位姿，根據(jù)定位及感知數(shù)據(jù)繪制地圖。
下圖為slam主流框架：
- 傳感器感知在視覺SLAM中主要為傳感信息的讀取和預(yù)處理。
- 前端里程計(Radar/Visual Odometry)。特征點匹配及運動估計。
- 后端優(yōu)化(Optimization)。后端接受不同時刻里程計測量位姿，以及回環(huán)檢測的信息，對估計軌跡及環(huán)境信息進(jìn)行全局優(yōu)化。
- 回環(huán)檢測(Loop Closing)?；丨h(huán)檢測判斷機(jī)器人是否到達(dá)過先前的位置。
- 建圖(Mapping)。根據(jù)估計的環(huán)境信息及機(jī)器人運動軌跡，建立對應(yīng)的地圖。
前端：主要是提取處理傳感器得到的數(shù)據(jù)，比如，激光的點云，視覺的圖像，結(jié)合傳感器數(shù)學(xué)模型及機(jī)器人運動學(xué)模型，推算得到機(jī)器人的位姿及熱環(huán)境特征點的相對位置。
后端：由于模型偏差，會導(dǎo)致誤差累積，因此，需要后端做一次全局的優(yōu)化
回環(huán)檢測：就是閉環(huán)檢測，檢測機(jī)器人是否到達(dá)原始位置，從而結(jié)合后端算法，對機(jī)器人的運動軌跡及地圖進(jìn)行全局的優(yōu)化和調(diào)整。
前端差異
前端完成的主要任務(wù)包括：特征點提取，特征點匹配，機(jī)器人運動位姿估計。
一般前端運動估計都是與里程計結(jié)合的，如IMU或輪式里程計等。視覺匹配也算是里程計。通過里程計的運動學(xué)模型得到估計位姿，但是由于里程計模型的誤差(靜差)以及測量誤差等，會導(dǎo)致累計誤差產(chǎn)生，因此需要通過環(huán)境反饋的信息對位姿估計值進(jìn)行修正優(yōu)化。
環(huán)境特征：
- 視覺利用圖像來解析位姿：特征點法和光流法均對圖像特征點進(jìn)行操作，大多使用角點(圖像像素變化最快的點)，特征點法為特征點建立描述信息(描述子)，用來與其他幀匹配特征點。光流法則利用灰度一致性來匹配特征點，直接法則直接用像素塊進(jìn)行操作，使用像素塊的移動解析出相機(jī)的位姿變化，直接法相對特征點法對相機(jī)硬件要求較高(需要較好的曝光及較高的幀率)，但計算量相對減少，一般會與特征點結(jié)合使用來保證精度。詳細(xì)請看特征點法&光流法&直接法
- 激光利用得到的三維數(shù)據(jù)點來解析位姿：激光束打到實際位置返回來的激光束，得到的點。雷達(dá)收到的是各束光返回的距離信息。
提取特征點差異對比：
- 視覺得到的是像素特征點，需要通過前后幀圖像對比計算得到實際的環(huán)境特征點的位置，因此會導(dǎo)致
- 激光得到是環(huán)境信息相對于激光的極坐標(biāo)下的坐標(biāo)。但是得到的環(huán)境信息量比較少，會加大點云配準(zhǔn)的難度。
匹配算法對比：
- 視覺前端：利用幀間圖像差估計運動，稱視覺里程計
代表算法 ：特征點法，光流法，直接法
整體思路：就是根據(jù)前后幀得到的圖像來估計機(jī)器人的運動變換
特征點法：通過提取圖像特征點，采用描述子來匹配前后幀圖像的特征點，計算得到特征點的環(huán)境深度及估計機(jī)器人位姿。
光流法：假設(shè)灰度信息不變，對比前后幀圖像，得到同一特征點在圖像上的移動。從而得到得到特征匹配，位姿估計算法同特征點法。特征點法與光流法是特征點的匹配方法不同。一個利用圖像描述子，一個通過像素移動匹配特征點。
直接法：基于直接法，利用得到的像素移動，來直接估計機(jī)器人運動。 - 激光：激光一般與其他里程計結(jié)合使用。那么其他里程計就是激光slam的前端。
單獨的激光slam(不含其他里程計) 也是利用前后幀特征點(點云)的變化來估計運動。
激光由于環(huán)境信息量少，雖然知道具體特征點精確相對位置，但是幀間匹配由于信息量比較少，兩幀點云的匹配相對視覺有難度，因此常與其他里程計結(jié)合。
代表算法：ICP算法。ICP是3D-3D的一種求解位姿算法，多用于已知環(huán)境點的三維位姿，因此在激光slam中使用較多。ICP原理
ICP算法其實是一種求解運動的思路：迭代優(yōu)化，因為由于激光點云特殊，點云匹配和運動估計構(gòu)成了雞生蛋，蛋生雞的問題，因此出現(xiàn)點云匹配和運動估計交叉迭代的求解思路，就是ICP。
ICP是一種交叉迭代優(yōu)化，即分為兩步優(yōu)化，優(yōu)化點云配準(zhǔn)與優(yōu)化運動估計。
演化算法：NICP NDT等算法。 后期演化算法求解思路一致，只是約束方程不同，約束方程的思路不同。
NICP加入了點所在曲面的法向量信息，增加點云配準(zhǔn)的可靠性。NICP詳細(xì)介紹
NDT算法是加入障礙物信息(柵格地圖占用率)來進(jìn)行匹配。NDT詳細(xì)介紹
目前激光使用點到線點到面的優(yōu)化算法求解運動估計(代表算法LOAM)。
后端優(yōu)化
用來修正估計位姿的累計誤差，前端多是以單點建立約束方程，而后端則使用批量點或者新的傳感器信息來求解，后端的計算量更大，所以一般降低幀率，或只在關(guān)鍵上處理，但是更精確。后端更詳細(xì)整理請看后端優(yōu)化算法
優(yōu)化算法思路：激光和視覺的優(yōu)化方法是類似的。通過后驗來評估先驗的準(zhǔn)確性。
簡單說，由A推出B，那么通過檢驗B的存在與否也能推測出A的存在與否。—來自十四講
代表算法：拓展卡爾曼濾波器(EKF)，粒子濾波器(PF)，最小二乘法-圖優(yōu)化
KF: 應(yīng)用于線性系統(tǒng)?；诟咚乖肼暦植?，計算權(quán)重(乘子)，將估計位姿代入觀測模型得到的觀測值與真實觀測值的差，乘以得到的乘子，就是位姿估計的誤差修正。
EKF：就是將運動方程微分線性化(運動方程的一階泰勒展開)，在瞬時就相當(dāng)于線性方程，從而使用KF算法。缺點：有求逆過程，計算量隨特征點數(shù)多少指數(shù)級上升。
PF：是使用粒子來模擬機(jī)器人，不同粒子所攜帶的位置信息有不同的可信度，通過得到的觀測值來衡量粒子攜帶位置信息的可信度，并不斷更新粒子?？尚哦茸罡叩牧Ｗ泳痛頇C(jī)器人的運動過程種的位置情況。缺點：粒子耗散問題
最小二乘法：使用最優(yōu)化方法(最速下降法;GN;LM等)迭代改變估計位姿，使估計位姿代入觀測模型得到的觀測值與真實觀測值的范數(shù)最小化。從而得到優(yōu)化估計位姿。
圖優(yōu)化：基于最小二乘法，將多個時刻機(jī)器人的位姿同時進(jìn)行優(yōu)化。
回環(huán)檢測
用來判斷機(jī)器人是否到達(dá)之前位置
算法思路：有基于里程計結(jié)合的，就是根據(jù)機(jī)器人位姿大致估計有沒有到回環(huán)。
因為不知道什么時候能是回環(huán)，實時比對太耗計算量，隨機(jī)幀則可能錯過回環(huán)?，F(xiàn)在常用局部回環(huán)，
視覺：使用圖像相似性來匹配回環(huán)
激光：點云信息少，回環(huán)不好建立。Cartographer的回環(huán)檢測方法。通過匹配submap來匹配回環(huán)。
匹配代表算法：Bow(詞袋模型)
詞袋模型，找到一個字典，通過這個字典，得到每幀點云或者圖像基于這個字典的表示。通過對比這個表示得到圖像或者點云差異。
就好比判斷人臉，可以使用眼睛，鼻子，嘴作為字典，查看圖像是否包含這些元素，來判斷是不是人臉圖像。
字典生成：多層k-means聚類方法k-means介紹，就是使用不同特征分類，將點集進(jìn)行分類，從而字典就使用kd-tree來表述。遍歷到葉子節(jié)點的路徑，就是點集基于該字典的描述。
視覺/激光優(yōu)劣勢對比
硬件上: 相機(jī)相比雷達(dá)安裝方便，雷達(dá)貴
視覺：語義豐富，但是計算量大，對環(huán)境依賴性強(qiáng)，對光照條件要求高。
激光：相對更精確，但是雷達(dá)貴，探測范圍有限，另外缺乏語義信息，回環(huán)檢測難度大。
在環(huán)境場景多樣性差的環(huán)境中，如結(jié)構(gòu)一致的走廊或隧道等等結(jié)構(gòu)化的場景中，會出現(xiàn)退化的問題，此時，激光相對于視覺(信息量大)，更容易出現(xiàn)退化的問題。SLAM中狀態(tài)估計優(yōu)化退化問題。
比如大平面，視覺和激光掃過去，得到每幀的特征點相類似，得到幀間匹配的特征點距離幾乎均為0，從而無法采用優(yōu)化法得到位姿變換估計。這就是退化問題。
位置求解中，可將約束函數(shù)線性化后，求解線性方程狀態(tài)矩陣的特征值，特征值小于閾值的方向，則表明此方向上，位置求解存在退化，即位置估計在該方向上的不準(zhǔn)確度大。如下圖b在藍(lán)色解在橙色方向有退化現(xiàn)象，橙色則正常。
激光slam及視覺slam其實流程相似，在特征匹配上差別：
視覺slam：提取圖像特征點，特征點匹配通過匹配描述子確定。
激光slam：處理點云數(shù)據(jù)，通過ICP(交叉迭代)，NDT(概率模型)一類的方法匹配。參見點云配準(zhǔn)及運動估計算法
開源算法
視覺slam開源算法
ORB算法：描述子做了取舍折中，特征匹配上做了提速。
ORB2開源地址
稠密建圖：個人覺得對于位姿估計的精確度影響一般，但是會拖延運算速率。主要優(yōu)點，人機(jī)交互好一些。
直接法：運算速度快，但是有灰度不變的假設(shè)，匹配上精確度會變差。
VINS mono是香港大學(xué)開源的算法，主要是基于單目 IMU緊耦合，實時性及準(zhǔn)確度都高，但是靜止時會出現(xiàn)漂移，初始化時要充分移動相機(jī)，獲得足夠的匹配信息(單目需要解析深度)。
VIO：視覺慣性里程計，在視覺SLAM中增加IMU傳感器后，按照融合方式分為松耦合和緊耦合。松耦合是分別單獨利用視覺和IMU傳感器估計位姿，最后將求得的兩個狀態(tài)融合；緊耦合是將視覺和IMU的測量信息統(tǒng)一起來，構(gòu)建運動方程和觀測方程來估計位姿。
tagslam基于外部marker的視覺定位
https://github.com/berndpfrommer/tagslam_root
激光slam開源算法
gmapping技術(shù)特點
gmapping代碼地址
s
l
a
m
_
g
m
a
p
p
i
n
g
slam\_gmapping
slam_gmapping 是
o
p
e
n
s
l
a
m
_
g
a
m
p
p
i
n
g
openslam\_gampping
openslam_gampping 在ros下的二次封裝
openslam_gampping代碼地址
基于粒子濾波，類似AMCL方法加上了同步建圖功能。
運動估計，依賴?yán)锍逃嫈?shù)據(jù)，并添加粒子高斯噪聲
運動優(yōu)化，點云匹配，根據(jù)匹配值，更新粒子的可信度，可信度最高的就是優(yōu)化后的運動估計。
無回環(huán)
缺點：不適合用于大型場合(每個粒子都包含一個地圖，大型場合，會造成計算的爆發(fā)增長)以及噪聲比較多的場合(會出現(xiàn)粒子耗散問題)。
cartographer
paper：《Real-Time Loop Closure in 2D LIDAR SLAM》
官網(wǎng)介紹
cartographer開源代碼地址
cartographer開源地址
運動優(yōu)化，采用圖優(yōu)化方法，就是最小化估計位姿帶入觀測方程得到的估計觀測值與實際觀測值的差距。得到運動優(yōu)化。
建圖：采用子圖匹配回環(huán)。分支界定法加速。
LOAM技術(shù)特點：
paper: 《Lidar Odometry and Mapping in Real-time》
LOAM介紹
LOAM地址
A_LOAM代碼地址
LOAM_notes代碼地址
注：文中通過hukuyo，裝到電機(jī)上完成三維點云掃描，一次完整得掃描為一次sweep。一個sweep包含多個scan。雷達(dá)每次在一個位姿狀態(tài)發(fā)射光束，這是scan，完成一個完整旋轉(zhuǎn)，是sweep。
運動估計：高頻粗略估計，根據(jù)IMU里程計或者運動連續(xù)性(下一時刻)。利用線性插值，對各個角度得激光束對應(yīng)得機(jī)器人姿態(tài)角進(jìn)行估計。
點云匹配：將點分為邊線(edge)點和平面(planar)點，邊線用兩點確定，平面用三點確定。
將k時刻雷達(dá)坐標(biāo)系下點云
P
k
P_k
Pk?，根據(jù)估計位姿，映射到k 1時刻的雷達(dá)坐標(biāo)下
P
k
￣
\overline{P_k}
Pk??，在k 1時刻雷達(dá)坐標(biāo)系下點云
P
k
1
P_{k 1}
Pk 1?，找到
P
k
￣
\overline{P_k}
Pk??最鄰近點，線找兩個，平面找三個點。點云使用kdtree結(jié)構(gòu)存儲，用來尋找最近鄰的點。
運動優(yōu)化：低頻，這步計算量大
利用匹配好的點，建立約束方程，最小化點到線距離或點到面距離。得到優(yōu)化的位姿。
建圖：輸出點云地圖。
Lego-LOAM
相比LOAM加入點云聚類，去噪點，以及使用GTSAM作為后端的回環(huán)檢測
地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
LIO-SAM
優(yōu)化包含LiDAR里程計因子，IMU預(yù)積分因子，GPS因子和回環(huán)因子來得到機(jī)器人的全局一致的位姿。
加入慣性融合，為'Lego-LOAM'的后續(xù)版本。
其他開源代碼地址參考
當(dāng)前的開源SLAM方案
GitHub 上優(yōu)秀的開源SLAM repo
知識點
激光雷達(dá)的參數(shù)定義
分辨率：激光雷達(dá)輸出的圖像也被稱為“點云”圖像，相鄰兩個點之間的夾角就是角分辨率。
幀率：一幅點云圖像代表一幀，對應(yīng)到激光雷達(dá)內(nèi)部就是電機(jī)旋轉(zhuǎn)一圈完成一次掃描。幀率即代表一秒鐘內(nèi)激光雷達(dá)電機(jī)旋轉(zhuǎn)的圈數(shù)，也就是每秒鐘完成一圈掃描的次數(shù)，完成多少次掃描。
FOV :Field of view，雷達(dá)每scan能夠覆蓋的角度。
多線激光雷達(dá)：就是通過多個激光發(fā)射器在垂直方向上的分布，通過電機(jī)的旋轉(zhuǎn)形成多條線束的掃描。
https://www.sohu.com/a/302268573_473608
深度攝像頭會不會取代雷達(dá)
深度相機(jī)可以說是相機(jī)結(jié)合雷達(dá)技術(shù)的一個變種，但是相機(jī)主動式測距采用的光源，沒有脈沖雷達(dá)的傳輸距離遠(yuǎn)，且由于波長與普通光源更接近，因為也更容易受干擾。
視覺導(dǎo)航是否會替代激光導(dǎo)航原理技術(shù)詳解
深度攝像頭，相當(dāng)于目前已經(jīng)商用化、最便宜、性能比較差的“激光雷達(dá)”。攝像頭怎么成了激光雷達(dá)了，大家的共同點是通過測距實現(xiàn)三維空間感知。雖然叫攝像頭，其實也是測距傳感器，都屬于主動光源測距傳感器（發(fā)光，接收回光測量距離）。包括結(jié)構(gòu)光和相位TOF兩種原理。
可測深度攝像頭TOF Camera 原理
結(jié)構(gòu)光
Light Coding（編碼光）的結(jié)構(gòu)光技術(shù)，當(dāng)光投射到物體表面時，不可避免將出現(xiàn)畸變。因此，通過檢測畸變光線，便能夠獲得關(guān)于物體的3D信息。而后通過算法分析處理，最終便能夠獲得一張完整的3D結(jié)構(gòu)圖。
Light Coding技術(shù)雖然也是結(jié)構(gòu)光的一種，但其投射的是“激光散斑”。散斑具有高度的隨機(jī)性，并且距離不同，散斑的形狀也不同。這也就相當(dāng)于，當(dāng)光源發(fā)射出散斑后，整個空間都已經(jīng)被標(biāo)記。因此，當(dāng)有物體進(jìn)入這個空間的時候，通過記錄散斑的變化，就能夠監(jiān)測到物體的空間位置。
TOF
同結(jié)構(gòu)光不同，ToF（Time of flight 飛行時間）技術(shù)首先會發(fā)射光脈沖，當(dāng)光脈沖遇見障礙物時，必然會出現(xiàn)反射。距離不同，反射運動時間也不同。通過記錄光脈沖的反射運動時間，推算出光脈沖發(fā)射器同目標(biāo)物體的距離，并以此生成一張目標(biāo)物體的3D信息圖。
激光的距離測試也常采用TOF技術(shù)
ITOF(indirect time of flight)，間接測量光飛行時間，大部分的間接測量方案都是采用了測相位偏移的方法，即發(fā)射的正弦波與接收的正弦波之間的相位差，先算相位，再算時間，進(jìn)而到距離。在iToF中，發(fā)射的并非一個光脈沖，而是調(diào)制過的光。接收到的反射調(diào)制光和發(fā)射的調(diào)制光之間存在一個相位差，通過檢測該相位差就能測量出飛行時間，從而估計出距離。目前iToF的主要應(yīng)用場景都是測距距離在數(shù)米范圍的應(yīng)用（例如手機(jī)等）
DTOF(direct time of flight)， 直接測量飛行時間解算距離，DTOF向目標(biāo)發(fā)射一個光脈沖信號，直接測量光子從出發(fā)到返回的用時；ITOF發(fā)射一連串經(jīng)過調(diào)制的光波，通過檢測來回光波間的相位差測量飛行時間。dtof在很多關(guān)鍵性能方面對itof都有絕對優(yōu)勢，但其技術(shù)壁壘比較高。DTOF多使用激光雷達(dá)，多用于車載LiDAR。脈沖激光，比雷達(dá)利用的無線電波波長更短、頻率更高，可以得到分辨率更高的圖像，精度可達(dá)厘米甚至毫米級別。
激光/視覺點云區(qū)別
根據(jù)激光測量原理得到的點云，包括三維坐標(biāo)（XYZ）和 激光反射強(qiáng)度（Intensity）。
根據(jù)攝影測量原理得到的點云，包括三維坐標(biāo)（XYZ）和 顏色信息（RGB）
|
