SLAM建圖構(gòu)建算法概述
SLAM 是一個(gè)龐大的理論體系,，涉及傳感器處理,、貝葉斯濾波,、非線性優(yōu)化、地圖的表示,、圖形處理等各方面的相關(guān)理論,。早期 SLAM 的基本理論是以擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）為代表的濾波方法為主,。
如下圖所示表示了一種典型的 SLAM 系統(tǒng)建圖方法,，系統(tǒng)主要由前端（front-end）和后端（back-end）兩個(gè)部分組成，前端根據(jù)傳感器觀測到的環(huán)境信息估計(jì)車輛的位姿,，然后將車輛的位姿信息傳遞給后端進(jìn)行優(yōu)化估計(jì),，得到具有全局一致性的地圖，最后得到整體的 SLAM 估計(jì),。SLAM 的前端算法包括了特征提取和數(shù)據(jù)關(guān)聯(lián)兩部分,。特征不但可以用來確定機(jī)器人的位置，而且可以用來進(jìn)行閉環(huán)檢測,，從而構(gòu)建具有全局一致性的環(huán)境地圖,。因此特征提取是 SLAM 研究中非常重要的一個(gè)環(huán)節(jié)。
目前在 SLAM 的研究中,，前端主要是以濾波的方法獲得車輛位姿的更新信息,，后端則大多是基于優(yōu)化的方法對(duì)機(jī)器人全局的位姿和地標(biāo)信息進(jìn)行優(yōu)化。基于視覺感知的SLAM基礎(chǔ)建圖構(gòu)建在SLAM算法構(gòu)建中,，前視攝像頭/激光雷達(dá)化身“智能采集終端”,，眾包生產(chǎn)“動(dòng)態(tài)高精地圖數(shù)據(jù)”，動(dòng)態(tài)高精地圖的本質(zhì)是實(shí)時(shí)交通數(shù)據(jù)的時(shí)空載體,，眾包數(shù)據(jù)是高頻更新的核心,。主要通過如下幾個(gè)步驟進(jìn)行實(shí)時(shí)建圖：
1）實(shí)時(shí)構(gòu)建高精度地圖：即基于視覺或激光點(diǎn)云數(shù)據(jù)建圖，并與云端融合冷啟動(dòng)從0-1構(gòu)建全路網(wǎng)高精地圖提升高精地圖“廣度”,。
2）更新高精地圖元素：通過要素關(guān)聯(lián)與變化檢測發(fā)現(xiàn)地圖元素的變化并實(shí)時(shí)更新,，以確保高精地圖的“鮮度”；
3）自建 “ 記憶地圖 ”：通過自建局部記憶地圖支持記憶泊車,、園區(qū)記憶式自動(dòng)駕駛“記憶地圖”提升駕駛體驗(yàn),；
4）上報(bào)事件型動(dòng)態(tài)信息：通過事件型動(dòng)態(tài)信息采集與上報(bào)（施工、擁堵,、事故等）構(gòu)建高精地圖“動(dòng)態(tài)信息層”,。
如上圖表示了一種典型的視覺SLAM技術(shù)方案框圖，其中整個(gè)智能駕駛攝像頭（包含前視,、側(cè)視,、環(huán)視以及后視攝像頭等）形成SLAM的輸入數(shù)據(jù)端，使用單目相機(jī)的 SLAM 稱為單目視覺 SLAM,，單目相機(jī)使用一個(gè)攝像頭進(jìn)行移動(dòng)機(jī)器人的定位,，由于單目攝像頭只能獲取周圍環(huán)境的二維信息,，無法給出物體的深度信息，因此會(huì)帶來尺度不確定的問題,。使用雙目相機(jī)的 SLAM 稱為雙目視覺 SLAM,，也稱為立體視覺 SLAM，它可以通過比較兩個(gè)攝像頭圖像的差異判斷物體的遠(yuǎn)近,，獲得物體的深度信息,。通過中央處理器對(duì)輸入圖像進(jìn)行感知、分割,、檢測,、跟蹤等操作，輸出給導(dǎo)航網(wǎng)絡(luò)端進(jìn)行語義建圖及匹配定位,，同時(shí)通過目標(biāo)識(shí)別形成相應(yīng)的ADAS系統(tǒng)目標(biāo)屬性,。最后分兩方面進(jìn)行輸出：其一是通過太網(wǎng)連接高精地圖盒子，其接收到相應(yīng)的局部地圖建圖結(jié)果后,，更新地圖底圖,。其二是通過輸出給MCU（微處理器單元）進(jìn)行融合定位及車輛控制。這里需要注意的是MCU通過CAN網(wǎng)絡(luò)也接收了諸如GNSS,、IMU等相關(guān)信息,，并輸入給AI芯片進(jìn)行建圖優(yōu)化和確認(rèn)。同時(shí),，先進(jìn)的SLAM方案通過云端多源 V2X 感知部署,，打造安全出行解決方案。一方面,，通過創(chuàng)新模式與路方合作,，利用既有道路信息基礎(chǔ)能力，有效整合路側(cè)信息,，通過個(gè)體級(jí)車輛的交通場景感知算法,，實(shí)施云端 V2X 信息處理，通過導(dǎo)航地圖優(yōu)勢,，為實(shí)時(shí)建圖提供統(tǒng)一入口,。另一方面，由于車路協(xié)同系統(tǒng)中將產(chǎn)生大量數(shù)據(jù)采集和處理,，為進(jìn)一步挖掘相關(guān)數(shù)據(jù)價(jià)值,，提升智慧出行效率?；诖髷?shù)據(jù)和人工智能能力,，可以對(duì)相關(guān)數(shù)據(jù)進(jìn)行人工智能模型訓(xùn)練，從而進(jìn)一步提高建圖能力,，優(yōu)化建圖精度,。如下圖是一種典型的將云端數(shù)據(jù)納入到建圖過程中的例子,。
基于激光雷達(dá)的SLAM基礎(chǔ)建圖構(gòu)建除了如上所述的視覺SLAM建圖以外，還包括了激光雷達(dá)數(shù)據(jù)建圖,。激光雷達(dá)雖然比相機(jī)要貴很多，但因其測量精度高,，測量性能穩(wěn)定,，目前在工業(yè)中應(yīng)用更加廣泛。激光雷達(dá)不能直接給出車輛的位置變化,，但它能提供周圍物體的距離信息,，可以用于車輛的實(shí)時(shí)定位、避障等場合,。
早期的激光雷達(dá)主要是利用三角測距的原理進(jìn)行測距,，后來發(fā)展出基于飛行時(shí)間法（Time Of Flight，TOF）測距原理的激光雷達(dá),。TOF 測距原理比較簡單,，通過記錄激光從發(fā)射出去到被接收所經(jīng)歷的時(shí)間可以計(jì)算出激光所走過的距離，從而得到激光雷達(dá)周圍環(huán)境的距離信息,。針對(duì)激光雷達(dá)和相機(jī),，目前都已經(jīng)有很多非常知名的開源算法的實(shí)現(xiàn)。在激光 SLAM 領(lǐng)域目前的很多開源算法都是使用 2D 激光雷達(dá)實(shí)現(xiàn) SLAM,。如谷歌公司在 2016 年開源的Cartographer SLAM 算法是一個(gè)跨平臺(tái)的激光 SLAM 方案,，能夠同時(shí)實(shí)現(xiàn) 2D 激光和 3D 激光的實(shí)時(shí) SLAM。在視覺 SLAM 領(lǐng)域也出現(xiàn)了許多優(yōu)秀的開源方案,。如Davison 等人在 2007 年提出的單目視覺 SLAM 系統(tǒng) MonoSLAM 是第一個(gè)實(shí)時(shí)的單目 SLAM,，它以擴(kuò)展卡爾曼濾波為基礎(chǔ)，把相機(jī)的當(dāng)前狀態(tài)和所有路標(biāo)的信息作為狀態(tài)量來更新均值和協(xié)方差信息,。SLAM 的理論在國外發(fā)展的比較早,，理論比較成熟，國內(nèi)在這方面起步比較晚,，大部分研究都集中在 SLAM 的工程應(yīng)用上,，重點(diǎn)解決工程應(yīng)用中的一些實(shí)際問題。SLAM 的前端算法包括了特征提取和數(shù)據(jù)關(guān)聯(lián)兩部分,。特征不但可以用來確定機(jī)器人的位置,，而且可以用來進(jìn)行閉環(huán)檢測，從而構(gòu)建具有全局一致性的環(huán)境地圖,。因此特征提取是 SLAM 研究中非常重要的一個(gè)環(huán)節(jié),。激光雷達(dá)掃描后會(huì)獲得一組由環(huán)境中的物體反射回來的點(diǎn)云，對(duì)于激光雷達(dá)的掃描點(diǎn)云,，常用的特征有點(diǎn),、線段,、平面等。點(diǎn)特征是日常環(huán)境中常見的一種特征,，通常會(huì)從環(huán)境中提取角點(diǎn),、交叉點(diǎn)或墻的端點(diǎn)等一些特殊的點(diǎn)作為特征點(diǎn)。線段特征是室內(nèi)移動(dòng)機(jī)器人定位,、導(dǎo)航時(shí)廣泛使用的一種特征,，它在結(jié)構(gòu)化環(huán)境中容易獲取，特征之間也容易建立約束,，對(duì)激光雷達(dá)的數(shù)據(jù)關(guān)聯(lián)和閉環(huán)檢測來說都是一種非常好的特征,。面特征主要應(yīng)用于室外環(huán)境中 3D 激光雷達(dá)掃描匹配，如無人車行駛在城市環(huán)境中可以利用 3D 激光雷達(dá)提取到大量的曲面和平面特征來定位,。由于線段特征具有容易識(shí)別,、魯棒性好等優(yōu)點(diǎn)，在 2D 激光 SLAM 中應(yīng)用比較廣泛,。下面將對(duì)激光雷達(dá)點(diǎn)云建圖中一種基本的線段擬合防范進(jìn)行簡單說明,。
如上圖表示激光雷達(dá)掃描得到的點(diǎn)云離散數(shù)據(jù)，針對(duì)線段特征提取的主流方法主要是采用分割-合并方法（Spilt-and-Merge,，SM）,，且許多線段特征的提取方法都是分割-合并方法的變形。分割-合并的基本過程就是對(duì)點(diǎn)云進(jìn)行聚類,，聚類過程中分別設(shè)定一定的閾值進(jìn)行分割-合并-分割-合并依次循環(huán)的過程,，從而實(shí)現(xiàn)線段的有效提取。在分割階段,，首先從給定的點(diǎn)集中擬合出一條直線,。然后找出點(diǎn)集中到擬合直線距離最遠(yuǎn)的點(diǎn)，計(jì)算該點(diǎn)到擬合直線的距離作為擬合誤差,，如果擬合誤差大于設(shè)定的距離閾值,，則從點(diǎn)集中按照某種規(guī)則找出一個(gè)點(diǎn)，將直線從該點(diǎn)處進(jìn)行分割,。不斷重復(fù)這個(gè)過程直到擬合誤差小于設(shè)定的距離閾值則不再分割,。在合并階段，如果相鄰的兩條線段的擬合誤差小于距離閾值,，就將這兩條相鄰線段合并成一條線段,。總結(jié)激光SLAM發(fā)展至今已經(jīng)有比較豐富的理論研究成果，激光測距的原理比較簡單,，測量精度高,，測量距離比較遠(yuǎn)，穩(wěn)定性、可靠性也都比較高,，但其成本較大,，目前激光雷達(dá)有向低成本發(fā)展的趨勢。但在實(shí)際的應(yīng)用中仍然存在定位精度不夠,、地圖構(gòu)建不準(zhǔn)確等問題,，給機(jī)器人執(zhí)行相關(guān)任務(wù)帶來了一定的障礙。與激光雷達(dá)相比,，相機(jī)的成本要低很多,，且能提供更多的信息，也更能滿足人們對(duì)機(jī)器人的想象,。但目前相機(jī)仍存在著測量范圍小、測量噪聲大,、易受日光干擾等問題,。視覺定位與建圖帶來的駕駛功能提升使得視覺定位在單車視覺感知局限得以解決。