1

背景

此次的優(yōu)化針對(duì)的場(chǎng)景是室內(nèi)小場(chǎng)景、室內(nèi)大場(chǎng)景和室外場(chǎng)景下運(yùn)行3D SLAM時(shí)，z方向的誤差累積、以及十萬平米（≈400*400）建圖時(shí)回環(huán)檢測(cè)效率低下的問題，機(jī)器上使用的主要傳感器3D LiDAR有velodyne16線（或ouster16線），IMU使用的是阿路比-LPMS-IG1。那么在進(jìn)行優(yōu)化前，進(jìn)行普通的3D SLAM能得到什么樣“離譜“的結(jié)果呢？有如此一幅點(diǎn)云地圖（見圖1上）在俯瞰視角下彷佛一切”歲月靜好“，但在正視視角下其尾巴都翹上了天（見圖1下）：

圖1：開環(huán)室內(nèi)20x260米走廊場(chǎng)景（俯視圖：上，正視圖：下）

由圖1，我們可以看到在SLAM過程中x、y、yaw的狀態(tài)量估計(jì)還是基本準(zhǔn)確的，在點(diǎn)云地圖上的邊界沒有出現(xiàn)明顯的結(jié)構(gòu)性問題，但是其在建圖開始（始于地圖最右側(cè)）后，隨著roll、pitch、z的誤差嚴(yán)重積累，在跑完這260+米的走廊后終點(diǎn)位置（地圖最左側(cè)）的z坐標(biāo)與起始點(diǎn)位置的z坐標(biāo)偏差了8.57米（實(shí)際偏差<2m）。再看另一個(gè)現(xiàn)象在一個(gè)廠區(qū)室外130x240的回環(huán)區(qū)域下進(jìn)行3D SLAM，得出了如圖2所示的點(diǎn)云地圖：

圖2：閉環(huán)室外130x240米廠區(qū)場(chǎng)景（俯視圖：上，正視圖：下）

由圖2，還是可以得出SLAM過程中狀態(tài)量估計(jì)基本準(zhǔn)確的狀態(tài)為x、y、yaw，而roll、pitch、z的誤差積累問題嚴(yán)重，在上圖中長(zhǎng)邊（240米）左、右關(guān)鍵幀的位姿上z坐標(biāo)間的差值有9.88米（實(shí)際偏差<2m）。

針對(duì)上面兩個(gè)例子中發(fā)現(xiàn)的問題，本人將簡(jiǎn)單分析其誤差的產(chǎn)生的原因，并分享根據(jù)誤差產(chǎn)生原因的物理依據(jù)進(jìn)行優(yōu)化設(shè)計(jì)和實(shí)現(xiàn)思路。

2

軌跡z方向飄移原因分析

在SLAM建圖過程中軌跡某個(gè)狀態(tài)量的不準(zhǔn)確、在某個(gè)狀態(tài)量上的誤差積累本質(zhì)上都是算法設(shè)計(jì)時(shí)的假設(shè)與實(shí)際情況不符，且在進(jìn)行SLAM問題建模和解決方案設(shè)計(jì)時(shí)沒有考慮到假設(shè)與真實(shí)情況的不一致性而導(dǎo)致的解算誤差。而在點(diǎn)云配準(zhǔn)的階段，經(jīng)典點(diǎn)云配準(zhǔn)算法ICP及其變體針對(duì)了各種環(huán)境、場(chǎng)景上出現(xiàn)的問題進(jìn)行了各種各樣的算法設(shè)計(jì)以想要高效且魯棒的解決所有使用上的問題，但是z軸方向的飄移其實(shí)是由于z軸、roll和pitch這三個(gè)機(jī)器人狀態(tài)量在一定程度上不可觀測(cè)且難以評(píng)價(jià)和消除，雖然機(jī)器上使用了IMU和LiDAR傳感器進(jìn)行慣性信息與環(huán)境信息的輸入，但軌跡中z方向的誤差來源還是主要集中在LiDAR的數(shù)據(jù)。

2.1、LiDAR靜態(tài)tf標(biāo)定引入的誤差

一般來說機(jī)器人出廠后都需要進(jìn)行一次詳細(xì)地標(biāo)定工作，但即使再詳盡的標(biāo)定工作多多少少都還是會(huì)留有一些誤差存在，另外隨著機(jī)器的使用機(jī)器上實(shí)際的LiDAR靜態(tài)tf也會(huì)因?yàn)闄C(jī)器的損耗逐漸與最初標(biāo)定的數(shù)值發(fā)生偏差。以上圖1中場(chǎng)景點(diǎn)云數(shù)據(jù)為例，在獲取到一幀激光點(diǎn)云數(shù)據(jù)后直接進(jìn)行靜態(tài)tf的變換，將其從LiDAR坐標(biāo)系轉(zhuǎn)移到車體坐標(biāo)系后可獲得俯視圖和正視圖如圖3所示：

圖3：室內(nèi)工廠單幀點(diǎn)云數(shù)據(jù)（俯視圖：上，正視圖：下）

由于該機(jī)器是剛出廠且完成了標(biāo)定，可以由圖3看到當(dāng)機(jī)器人靜止水平時(shí)LiDAR返回的點(diǎn)云數(shù)據(jù)經(jīng)過tf變換后的roll、pitch誤差還是較小的。但是在另外一臺(tái)使用年限較長(zhǎng)、經(jīng)過多次維修的機(jī)器人上做相同的實(shí)驗(yàn)，roll、pitch的誤差就變得肉眼可見的大了（見圖4）。將上述兩幀點(diǎn)云分別與對(duì)應(yīng)的以LiDAR坐標(biāo)系的點(diǎn)云進(jìn)行ICP匹配以獲取靜態(tài)tf的真值，可以發(fā)現(xiàn)圖3中roll、pitch的誤差分別是0.011和0.008弧度，而圖4中的roll、pitch誤差分別是0.035和0.017弧度。

在上述分析的基礎(chǔ)上，若是進(jìn)行純LiDAR的SLAM，一般情況下第一幀關(guān)鍵幀的位姿就是（0，0，0，0，0，0），而由于靜態(tài)tf的誤差存在其真實(shí)情況的關(guān)鍵幀并不完全水平，即使在后續(xù)的SLAM軌跡構(gòu)件中沒有其他問題發(fā)生，在開環(huán)情況下z軸的累積誤差就是：

z軸的累積誤差=(實(shí)際行走距離)×sin?(初始俯仰角誤差)

圖4：室外單幀點(diǎn)云數(shù)據(jù)（俯視圖：上，正視圖：下）

2.2、LiDAR豎直方向分辨率及入射角引入的誤差

首先從3D激光雷達(dá)的豎直方向分辨率開始，以常見的機(jī)械式激光雷達(dá)為例，即使是128線的3D激光雷達(dá)，其豎直方向上的分辨率也是小于水平方向的分辨率的，更何況16線的3D激光雷達(dá)。以Velodyne-VLP-16為例，

由于較低的豎直方向分辨率和豎直視場(chǎng)，以至于每一幀激光數(shù)據(jù)能檢測(cè)到地面的點(diǎn)云十分有限，且地圖特征遠(yuǎn)遠(yuǎn)不如水平視場(chǎng)探測(cè)到的角點(diǎn)、平面特征，例子可見圖3和圖4中的俯視圖，其中藍(lán)色標(biāo)記的點(diǎn)云即為激光打到地面上的點(diǎn)云。

其次，由于機(jī)器人上3D激光雷達(dá)普遍的安裝方式（雷達(dá)水平安裝在機(jī)器人正上方）導(dǎo)致在SLAM構(gòu)建軌跡的過程中激光打到地面上的入射角很大（遠(yuǎn)大于水平視場(chǎng)的垂直入射角），雷達(dá)入射角越大其產(chǎn)生的光斑也就越大，絕對(duì)精度和重復(fù)精度就越差。

上述的結(jié)論，除了可以通過各個(gè)雷達(dá)技術(shù)論文和技術(shù)參數(shù)手冊(cè)獲取，還有一個(gè)廣而知之的論證途徑就是LOAM中針對(duì)激光點(diǎn)云數(shù)據(jù)的預(yù)處理中定義了兩類異常點(diǎn)并進(jìn)行了剔除，其中一類就是平行點(diǎn)（定義圖示見圖5），平行點(diǎn)指的就是圖5中的B點(diǎn)，就是激光的射線幾乎和物體的平面平行了，而LOAM中剔除這類點(diǎn)的原因有兩個(gè)：

1、 激光的數(shù)據(jù)會(huì)不準(zhǔn)，射線被拉長(zhǎng)

2、這種點(diǎn)被視為特征點(diǎn)后會(huì)非常不穩(wěn)定，下一幀可能就沒了，無法進(jìn)行下一步的點(diǎn)云配準(zhǔn)，例如圖5中的LiDAR原點(diǎn)再向左移一點(diǎn)，那么B點(diǎn)就消失了，與之形成對(duì)比的就是A點(diǎn)，極短時(shí)間內(nèi)不會(huì)消失。

圖5：LOAM中平行點(diǎn)定義圖示

由此，若是在SLAM系統(tǒng)中的配準(zhǔn)使用的是點(diǎn)云層面的處理，而不是基于特征的匹配，則入射角很大處的地面點(diǎn)云由于絕對(duì)精度和重復(fù)精度差，就很有可能影響點(diǎn)云配準(zhǔn)時(shí)roll、pitch的觀測(cè)估算了。

綜上，LiDAR豎直方向分辨率及入射角對(duì)SLAM過程中軌跡z軸的影響在于兩個(gè)方面，第一個(gè)就是由于豎直分辨率低導(dǎo)致地面點(diǎn)云及其特征少，特別是降采樣后可能都沒剩下多少可以用來點(diǎn)云配準(zhǔn)了，第二個(gè)就是激光雷達(dá)打到地面上的點(diǎn)入射角通常較大，其點(diǎn)云精度差且不穩(wěn)定導(dǎo)致在點(diǎn)云配準(zhǔn)時(shí)出現(xiàn)誤差。

3

SLAM軌跡z方向飄移問題優(yōu)化思路

以上的分析都是常規(guī)過程中可量化且較為統(tǒng)一的誤差來源分析，而其他的原因比如退化場(chǎng)景下配準(zhǔn)算法失效引起的誤差、配準(zhǔn)算法本身性能導(dǎo)致的誤差還有機(jī)器運(yùn)算資源高壓條件下SLAM運(yùn)算阻塞引發(fā)的位姿滯后誤差等不在本文中進(jìn)行優(yōu)化的討論。下文中針對(duì)SLAM過程中軌跡z軸飄移的優(yōu)化思路還是針對(duì)上文中的靜態(tài)tf標(biāo)定誤差、LiDAR豎直方向低分辨率及大入射角帶來的誤差。

3.1、靜態(tài)tf標(biāo)定引入的誤差優(yōu)化

激光雷達(dá)坐標(biāo)系與車體中心坐標(biāo)系的靜態(tài)tf，首先會(huì)在SLAM開始階段引入一個(gè)初始位姿誤差，因?yàn)闄C(jī)器人在每一次開始SLAM時(shí)不一定是完全水平的，那么為了解決這個(gè)問題就需要利用IMU的數(shù)據(jù)進(jìn)行重力方向估計(jì)和對(duì)每一個(gè)關(guān)鍵幀位姿進(jìn)行重力方向校準(zhǔn)，這部分的工作可以參考LIO-SAM（局部因子圖緊耦合的LIO）和VINS-Fusion（滑窗優(yōu)化下的VIO）中針對(duì)IMU的高頻慣性數(shù)據(jù)的處理。

圖6：VINS中的滑窗優(yōu)化示意圖

以LIO-SAM為例，可以考慮將SLAM前端模塊進(jìn)行基于IMU的緊耦合局部因子圖的聯(lián)合優(yōu)化，與此同時(shí)完成針對(duì)IMU數(shù)據(jù)的初始化（外參及bias的初始估計(jì)）、IMU重力估計(jì)模塊和IMU預(yù)積分功能，局部因子圖的構(gòu)建主要節(jié)點(diǎn)還是基于點(diǎn)云配準(zhǔn)后的關(guān)鍵幀，但此時(shí)關(guān)鍵幀之間會(huì)再多上一個(gè)重力方向校準(zhǔn)的約束和IMU預(yù)積分計(jì)算出的慣性變換約束，以盡可能地減少靜態(tài)tf和點(diǎn)云配準(zhǔn)時(shí)帶來的誤差，特別是IMU重力估計(jì)模塊，可以避免純LiDAR SLAM在roll、pitch的完全不可觀測(cè)問題，在很大程度上優(yōu)化z軸的飄移問題。

圖7：LIO-SAM的緊耦合LIO系統(tǒng)框圖

3.2、豎直方向低分辨率及大入射角帶來的誤差優(yōu)化

豎直方向低分辨率及大入射角帶來的誤差來源根據(jù)上文中的分析，可以歸結(jié)為兩個(gè)：第一，由于豎直分辨率低導(dǎo)致地面點(diǎn)云及其特征少，第二，激光雷達(dá)打到地面上的點(diǎn)入射角通常較大，其點(diǎn)云精度差且不穩(wěn)定。一言以蔽之，3D激光雷達(dá)獲取到的地面信息較少且獲取到的點(diǎn)云信息較差導(dǎo)致的觀測(cè)校正不夠理想，誤差由此被引入到SLAM軌跡中且難以在后續(xù)構(gòu)建中被消除。

既然點(diǎn)云的信息又少又差，我們避免直接使用點(diǎn)云信息進(jìn)行配準(zhǔn)操作不就行了嘛。還是先以LIO-SAM為例，在其系統(tǒng)的激光回調(diào)中完成了以下關(guān)鍵步驟：

（1）將當(dāng)前幀位姿初始化并隨后提取當(dāng)前幀的局部角點(diǎn)、平面點(diǎn)云集合，加入到局部map；

（2）將當(dāng)前激光幀角點(diǎn)、平面點(diǎn)集合降采樣后進(jìn)行scan-to-map匹配，以優(yōu)化當(dāng)前幀的位姿；

（3）若當(dāng)前幀為關(guān)鍵幀則執(zhí)行因子圖優(yōu)化，更新因子圖中所有變量節(jié)點(diǎn)的位姿由此獲得優(yōu)化后的SLAM軌跡?？梢钥吹剑琇IO-SAM的系統(tǒng)中沒有進(jìn)行基于點(diǎn)云的配準(zhǔn)，而是通過提取平面點(diǎn)進(jìn)行基于特征的匹配優(yōu)化計(jì)算，而且特征匹配時(shí)要求當(dāng)前幀特征點(diǎn)數(shù)量需要足夠多且reference匹配的點(diǎn)數(shù)也足夠多才執(zhí)行優(yōu)化。

我們不妨根據(jù)上文中平面特征的特征匹配過程進(jìn)一步擴(kuò)展，平面點(diǎn)云特征不再單單作為用以前端關(guān)鍵幀位姿觀測(cè)更新的一部分，而是將其擴(kuò)展成一個(gè)地面的約束（Ground Constrain）以避免z軸的飄移問題，而添加了地面約束后即是嘗試?yán)玫孛嫣卣髯鳛榧僭O(shè)條件，在優(yōu)化時(shí)考慮地面約束帶來的殘差以影響優(yōu)化結(jié)構(gòu)，為了避免添加地面約束對(duì)上下坡場(chǎng)景的不友好，可以對(duì)地面約束進(jìn)行打分計(jì)算，只要地面特征足夠“明顯“和地面約束足夠可靠，才在對(duì)應(yīng)的關(guān)鍵幀處添加地面約束的殘差塊。

另一個(gè)延申就是可以針對(duì)每一幀點(diǎn)云數(shù)據(jù)進(jìn)行地面點(diǎn)的分割和標(biāo)記，而這個(gè)分割后的結(jié)果可以不再像上文一樣單獨(dú)使用在前端的配準(zhǔn)模塊中，而是進(jìn)一步地被利用在回環(huán)檢測(cè)模塊中以保證地面特征被充分利用。在這里分享兩篇對(duì)我啟發(fā)較大的有關(guān)地面點(diǎn)云分割的技術(shù)論文和開源代碼，

3.3、萬能誤差消除術(shù)

萬能誤差消除術(shù)其一：添加和利用好GNSS、Landmarks等外部的絕對(duì)位置信息，唯一需要注意的就是當(dāng)GNSS數(shù)據(jù)或者Landmark信息被機(jī)器人獲取時(shí)，需要自動(dòng)地判斷出該絕對(duì)位置信息是否可靠，避免因?yàn)槭褂美?、離群點(diǎn)位置信息而帶來更嚴(yán)重的誤差?？蓞⒖嘉恼骆溄樱篽ttps://zhuanlan.zhihu.com/p/603099637。

萬能誤差消除術(shù)其二：回環(huán)檢測(cè)?；丨h(huán)檢測(cè)是所有SLAM問題的解決方案里必不可少關(guān)鍵模塊，比如經(jīng)典的Scan-Context，基于多分辨率地圖的分支定界+相關(guān)性匹配，以及利用語義信息的各種回環(huán)檢測(cè)算法，只要在SLAM過程中找到正確的回環(huán)，基本上就可以消除90%的誤差了，但是怎么保證回環(huán)檢測(cè)的效率、準(zhǔn)確度又是一個(gè)研究方向了。

4

優(yōu)化結(jié)果展示

使用優(yōu)化后的算法模塊在相同的數(shù)據(jù)集和硬件環(huán)境下獲得了與圖1和圖2例子進(jìn)行對(duì)比的圖10開環(huán)室內(nèi)工廠環(huán)境和圖8閉環(huán)室外廠區(qū)環(huán)境結(jié)果，其中優(yōu)化后的結(jié)果（見圖8）SLAM軌跡圖中長(zhǎng)邊（240米）中最左和最右關(guān)鍵幀的位姿上z坐標(biāo)間的差值從之前的9.88米優(yōu)化成1.82m，圖10的軌跡結(jié)果則從原先260米長(zhǎng)度下累積8.57米誤差，到現(xiàn)在300米累積0.89米的誤差，以上兩個(gè)對(duì)比實(shí)例都說明了上文中的優(yōu)化思路是可行的。

圖8：閉環(huán)室外廠區(qū)130x240米（俯視圖：上，正視圖：下）

圖9：閉環(huán)室內(nèi)+室外廠區(qū)205x240米（俯視圖：上，正視圖：下）

圖10：室內(nèi)廠區(qū)120x300米（俯視圖：上，正視圖：下）

除開大場(chǎng)景、長(zhǎng)軌跡的SLAM構(gòu)建，還使用優(yōu)化后的SLAM算法模塊在小范圍的地庫及室內(nèi)房間場(chǎng)景進(jìn)行了魯棒性測(cè)試，地庫場(chǎng)景的有效高度區(qū)間相較于工廠與室外環(huán)境會(huì)更加小，同時(shí)存在更多的動(dòng)態(tài)障礙物（移動(dòng)的車輛和行人），室內(nèi)房間場(chǎng)景由于空間更加狹窄，3D激光雷達(dá)獲取到的地面特征幾乎沒有，由此也就更加依賴IMU的重力估計(jì)和校準(zhǔn)功能，同時(shí)還需要避免錯(cuò)誤的地面約束影響SLAM軌跡發(fā)生偏移。圖11和圖12分別對(duì)應(yīng)優(yōu)化后的SLAM算法模塊下地庫、室內(nèi)房間場(chǎng)景下的結(jié)果。

圖11：室內(nèi)地庫50x70米（俯視圖：上，正視圖：下）

圖12：室內(nèi)房間22x35米（俯視圖：上，正視圖：下）

5

總結(jié)

在純LiDAR SLAM的算法模塊中添加IMU慣性里程計(jì)約束、IMU重力估計(jì)及校準(zhǔn)，并針對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行地面點(diǎn)云的分割和針對(duì)地面點(diǎn)云進(jìn)行平面特征匹配，可在一定程度上解決LO在z、roll、pitch這幾個(gè)機(jī)器人狀態(tài)量輕微不可觀的問題。值得說明的一點(diǎn)是，在本人的優(yōu)化工作中，先是嘗試在SLAM算法模塊的前端使用局部因子圖框架，利用LiDAR觀測(cè)（地面點(diǎn)云在此階段也被利用到點(diǎn)云配準(zhǔn)算法中以進(jìn)一步提升z、roll、pitch的觀測(cè)精度）得到的關(guān)鍵幀及幀間IMU慣性里程計(jì)約束，獲取一個(gè)優(yōu)化后LIO關(guān)鍵幀并輸入到后端中，這樣的處理在回環(huán)檢測(cè)觸發(fā)之前，前端LIO中還是會(huì)積累一定程度的z軸飄移（但比沒有添加地面點(diǎn)云優(yōu)化配準(zhǔn)和imu約束前要好一些）。而最有效的優(yōu)化處理則是在后端的優(yōu)化問題構(gòu)造時(shí)，針對(duì)每一個(gè)關(guān)鍵幀添加必要的IMU重力校準(zhǔn)殘差項(xiàng)和地面約束殘差項(xiàng)，如此一來才能真正意義上的解決z軸飄移的問題。

在后端優(yōu)化的改造工作中，難點(diǎn)在于如何保證添加的IMU重力校準(zhǔn)殘差項(xiàng)和地面約束殘差項(xiàng)是精準(zhǔn)和可靠的，在改造初期由于經(jīng)常引入較差的IMU重力校準(zhǔn)約束和地面約束而出現(xiàn)關(guān)鍵幀位姿出現(xiàn)大跳變和SLAM軌跡失敗的問題，地面約束的約束質(zhì)量確認(rèn)可以通過地面點(diǎn)云分割質(zhì)量和地面特征匹配得分的高低來進(jìn)行把控，而要保證IMU重力校準(zhǔn)精度，一方面需要加強(qiáng)針對(duì)IMU數(shù)據(jù)的使用問題建模和解決方案設(shè)計(jì)，添加重力校準(zhǔn)約束前再進(jìn)一步進(jìn)行完整性分析以最后把控約束質(zhì)量（這部分的內(nèi)容后續(xù)再加更輸出文章）。

審核編輯：劉清