王雅儀，余 萌，朱 鋒

（武漢大學(xué) 測(cè)繪學(xué)院，武漢 430079）

0 引言

自動(dòng)駕駛是1個(gè)集精準(zhǔn)定位、環(huán)境感知、決策控制等多功能于一體的復(fù)雜系統(tǒng)，而定位又是這些功能發(fā)揮作用的先決條件。當(dāng)前的定位定姿技術(shù)眾多，但均存在缺陷，如衛(wèi)星導(dǎo)航容易在信號(hào)遮擋的環(huán)境下失效；高精度的慣導(dǎo)設(shè)備成本極高、且定位誤差隨著時(shí)間增加而快速發(fā)散；視覺(jué)導(dǎo)航易受光照和視角變化的影響，基于無(wú)線信號(hào)即無(wú)線保真（wireless fidelity, WiFi）、藍(lán)牙、發(fā)光二極管（light emitting diode, LED）等的定位方案，須預(yù)先布置使用場(chǎng)景[1]。文獻(xiàn)[2-3]指出：激光雷達(dá)（light detection and ranging, LiDAR）具有測(cè)量精度高、時(shí)間和空間分辨率精細(xì)、無(wú)需事先布置場(chǎng)景、能夠快速響應(yīng)環(huán)境變化、可融合多傳感器等優(yōu)點(diǎn)，與基于視覺(jué)同步定位和制圖（simultaneous location and mapping, SLAM）技術(shù)相比，激光SLAM自主定位更加安全、穩(wěn)健，成為了國(guó)內(nèi)外研究的熱點(diǎn)。

激光SLAM框架，一般分為前端掃描匹配、后端優(yōu)化、回環(huán)檢測(cè)、地圖構(gòu)建等4個(gè)關(guān)鍵模塊。前端掃描匹配是SLAM最核心的功能，實(shí)現(xiàn)前后兩幀點(diǎn)云的相對(duì)位姿恢復(fù)，也被稱為前端里程計(jì)。前端里程計(jì)的主流匹配算法有：迭代最臨近點(diǎn)法（iterative closest point, ICP）及其變種、相關(guān)性掃描匹配法（correlation scan match, CSM）、正態(tài)分布變換（normal distribution transformation, NDT）、基于特征的匹配等等。ICP是由文獻(xiàn)[4]在1992年提出的，迭代轉(zhuǎn)換矩陣使得待匹配的兩幀點(diǎn)云歐氏距離最小化，但是傳統(tǒng)的ICP需要對(duì)全點(diǎn)云進(jìn)行優(yōu)化，存儲(chǔ)和計(jì)算消耗大；基于特征的匹配，通過(guò)抽象出3維點(diǎn)云的幾何信息，能更好地輔助前端進(jìn)行點(diǎn)云匹配，在相同時(shí)間下準(zhǔn)確度更高，是當(dāng)下的研究熱點(diǎn)。

2009年，文獻(xiàn)[5]首次在點(diǎn)特征直方圖（point feature histograms, PFH）的基礎(chǔ)上，提出了更加穩(wěn)定的快速點(diǎn)特征直方圖（fast point feature histograms,FPFH），類似于視覺(jué)特征子的描述符一樣，描述了3維（3D）點(diǎn)云集中1個(gè)點(diǎn)周圍的局部幾何特征，通過(guò)直方圖的相似度確定同名點(diǎn)對(duì)，再進(jìn)行基于同名點(diǎn)的ICP配準(zhǔn)；文獻(xiàn)[6]提出利用建筑物邊界約束進(jìn)行點(diǎn)云數(shù)據(jù)配準(zhǔn)，通過(guò)探測(cè)建筑物垂直外墻和地面相交處的邊角點(diǎn)，預(yù)先構(gòu)建出地圖，然后通過(guò)ICP實(shí)現(xiàn)地圖匹配來(lái)校正車輛位置；文獻(xiàn)[7]采用分開(kāi)—合并框架提取點(diǎn)線特征，還融合了不同觀測(cè)值的置信度；文獻(xiàn)[8]采用“點(diǎn)－切平面”的特征對(duì)重疊點(diǎn)云進(jìn)行配準(zhǔn)；2016年，文獻(xiàn)[9]提出的激光雷達(dá)里程計(jì)與地圖構(gòu)建（LiDAR odometry and mapping, LOAM）成為3維激光SLAM最流行的點(diǎn)云匹配方法，它基于線面特征進(jìn)行了點(diǎn)云特征提取，并將激光雷達(dá)里程計(jì)的輸出和點(diǎn)云子地圖進(jìn)行特征匹配，對(duì)里程計(jì)恢復(fù)的位姿進(jìn)行優(yōu)化，但系統(tǒng)實(shí)時(shí)性差；基于LOAM的基本思想，列戈-洛姆（LeGO-LOAM）[10]引入了基于歷史位置的最近鄰搜索做閉環(huán)檢測(cè)，采用兩步優(yōu)化的方式降低設(shè)計(jì)矩陣的維數(shù)，可以滿足系統(tǒng)實(shí)時(shí)性的要求。

相對(duì)于點(diǎn)線面特征，更加抽象的語(yǔ)義特征能如實(shí)地反映3維場(chǎng)景，提供正確的特征匹配。蘇黎世聯(lián)邦理工大學(xué)開(kāi)發(fā)的LeGO-LOAM軟件[11]，則基于分割算法進(jìn)行回環(huán)檢測(cè)，首先從3D點(diǎn)云中聚類并標(biāo)注分割物，將它們用于匹配已經(jīng)走過(guò)的地方和使用幾何驗(yàn)證對(duì)比的方法選出回環(huán)檢測(cè)的候選點(diǎn)云。文獻(xiàn)[12]提出的塞格·馬特赫（SegMatch），采用漫水填充方法消除錯(cuò)誤的類別標(biāo)簽，識(shí)別動(dòng)態(tài)物體并剔除，構(gòu)建帶有語(yǔ)義約束的ICP模型，提高了定位的穩(wěn)定性。

本文采用1種線面特征相結(jié)合的3維激光SLAM算法，通過(guò)基于特征的ICP變種算法，對(duì)在不同時(shí)刻掃描的重疊點(diǎn)云進(jìn)行配準(zhǔn)，恢復(fù)行駛車輛的6個(gè)自由度位姿；為提高解算精度，提出全球衛(wèi)星導(dǎo)航系統(tǒng)（global satellite navigation system,GNSS）數(shù)據(jù)融合激光SLAM的方案。

1 算法理論及實(shí)現(xiàn)

3維激光SLAM的任務(wù)是估計(jì)載體自身的位姿，同時(shí)建立周圍的環(huán)境地圖。因?yàn)榧す饫走_(dá)可以準(zhǔn)確測(cè)量障礙點(diǎn)的角度與距離，環(huán)境地圖通過(guò)對(duì)正確的位姿進(jìn)行點(diǎn)云疊加即可獲得，可見(jiàn)激光SLAM的關(guān)鍵在于提高位姿估計(jì)的精度。本文提出的SLAM基本框架可以分為：預(yù)處理、前端里程計(jì)和后端優(yōu)化3個(gè)模塊，如圖1所示，圖1中的ICP是指基于特征的迭代最臨近點(diǎn)法，實(shí)驗(yàn)中采用的激光點(diǎn)云采樣率設(shè)為10 Hz，而GNSS接收機(jī)位置先驗(yàn)信息則以低頻1 Hz的速度輸入系統(tǒng)，最終的位姿估計(jì)的頻率與點(diǎn)云采樣率一致，在GNSS接收機(jī)良好的場(chǎng)景下可以用GNSS接收機(jī)輔助構(gòu)建點(diǎn)云地圖。

1.1 預(yù)處理

預(yù)處理分為點(diǎn)云分割和特征提取2個(gè)步驟，點(diǎn)云分割能提高特征提取的速度和精度。對(duì)原始點(diǎn)云進(jìn)行聚類，去除冗余點(diǎn)，僅留下包含結(jié)構(gòu)化特征的分割點(diǎn)云，包括地面點(diǎn)和大型物體上的點(diǎn)（如樹(shù)干）。采用基于密度的聚類方法，以某一點(diǎn)為啟發(fā)點(diǎn)向四周探索，計(jì)算相鄰方位角、相鄰掃描線上鄰點(diǎn)的距離和角度，足夠靠近的一群點(diǎn)云組成1個(gè)簇，只有點(diǎn)云簇含30個(gè)點(diǎn)以上才被認(rèn)為是候選點(diǎn)云。接著，從分割點(diǎn)云中再提取對(duì)應(yīng)的線特征點(diǎn)和面特征點(diǎn)。

圖1 本文提出的3維激光SLAM算法流程

本文采用文獻(xiàn)[9]提出的線面特征提取方法，定義激光點(diǎn)的局部平滑度為

式中：n為選取與點(diǎn)iP同一激光掃描線的鄰點(diǎn)的個(gè)數(shù)；ic為點(diǎn)iP與其相鄰2n個(gè)激光點(diǎn)形成的單位向量之和的模。

當(dāng)激光束打在“面”上，iP與其鄰點(diǎn)分布在近似一條直線上，因此這些鄰點(diǎn)與iP構(gòu)成的向量相互抵消，ic值較小。當(dāng)激光束打在“線”上，鄰點(diǎn)與iP形成的向量無(wú)法相互抵消，ic值較大。

為獲取空間均勻分布的特征，將每根掃描線等分為6段，在段內(nèi)依據(jù)激光點(diǎn)的局部平滑度進(jìn)行排序，每一段選取平滑度大于0.1的最大2個(gè)點(diǎn)為線特征點(diǎn)，選取平滑度小于0.1的最小4個(gè)點(diǎn)為面特征點(diǎn)。特征提取的效果如圖2所示。

圖2 線面特征提取示意圖

原始激光點(diǎn)云數(shù)約為11.7萬(wàn)個(gè)，提取特征后，線特征點(diǎn)數(shù)量為667個(gè)，面特征點(diǎn)數(shù)量為845個(gè)，總特征點(diǎn)數(shù)為1 512個(gè)，待估觀測(cè)數(shù)據(jù)大大減少，并且如圖2所示，提取出的線特征點(diǎn)主要聚集在樹(shù)干、路沿、車輛邊緣、房屋角點(diǎn)等處，而面特征點(diǎn)聚集在地面，能如實(shí)反映3維環(huán)境的結(jié)構(gòu)化特征。

1.2 前端里程計(jì)

里程計(jì)主要負(fù)責(zé)每幀點(diǎn)云的位姿估計(jì)，當(dāng)結(jié)構(gòu)化線面特征輸入系統(tǒng)后，采用畸變校正和掃描匹配并行的算法，恢復(fù)出相鄰幀的位姿關(guān)系。

考慮到常用的激光雷達(dá)都是旋轉(zhuǎn)掃描的工作方式，車輛在激光雷達(dá)一幀掃描的周期內(nèi)，會(huì)行駛過(guò)一段距離，導(dǎo)致一幀內(nèi)的所有激光點(diǎn)并不是處于同一個(gè)局部坐標(biāo)系，經(jīng)過(guò)畸變校正可以恢復(fù)到同一局部坐標(biāo)系下。點(diǎn)云的去畸變需要已知幀間運(yùn)動(dòng)量，而求解幀間運(yùn)動(dòng)量又依賴于去畸變后掃描匹配，因此可以用位姿插值的辦法將畸變校正和掃描匹配放在1個(gè)算法中同時(shí)處理。假設(shè)掃描周期內(nèi)汽車的運(yùn)動(dòng)是平滑的，基于勻速運(yùn)動(dòng)模型，不同時(shí)刻對(duì)應(yīng)的位姿可以插值表示，設(shè)在第k幀期間，車體的運(yùn)動(dòng)量等價(jià)于幀尾時(shí)刻的局部坐標(biāo)系{Lk+1}相對(duì)于幀頭時(shí)刻的局部坐標(biāo)系{Lk}的旋轉(zhuǎn)和平移，用Tk表示，Tk=[θx,θy,θz,t x,t y,tz]T。

如果已知某點(diǎn)在一幀掃描內(nèi)的相對(duì)時(shí)間，記為s，那么該點(diǎn)對(duì)應(yīng){Lk}的旋轉(zhuǎn)和平移量為（以繞Y軸旋轉(zhuǎn)和平移為例）

在對(duì)每幀點(diǎn)云進(jìn)行特征提取操作后，本文采用PCL庫(kù)中的FLANN匹配器快速查找前后兩幀點(diǎn)云的匹配特征點(diǎn)對(duì)，線特征點(diǎn)只和線特征點(diǎn)進(jìn)行最近鄰匹配，面特征點(diǎn)只和面特征點(diǎn)進(jìn)行最近鄰匹配。對(duì)于線特征點(diǎn)而言，雖然最近鄰點(diǎn)和它可能不是同一個(gè)點(diǎn)，但是它們?cè)谕粭l結(jié)構(gòu)線上的可能性很大，因此在目標(biāo)點(diǎn)云中找到最近鄰點(diǎn)和次近鄰點(diǎn)，觀測(cè)方程就變成了最小化點(diǎn)到線/面的距離之和的函數(shù)，相比于傳統(tǒng)全點(diǎn)云ICP，線面-ICP能夠建立更準(zhǔn)確的觀測(cè)方程。

對(duì)于線特征點(diǎn)，其算法模型為

以同一數(shù)據(jù)測(cè)試全點(diǎn)云，ICP算法和線面特征結(jié)合的ICP算法，得到全點(diǎn)云ICP算法每一幀的計(jì)算時(shí)間平均為3.183 s，而改進(jìn)后的ICP算法只需要0.292 s，這主要是因?yàn)樘卣魈崛p小了匹配點(diǎn)對(duì)的數(shù)量，提高了匹配速率；另外，考慮到用最小二采估計(jì)位姿速度慢，本文采用分層優(yōu)化的方法[10]減小設(shè)計(jì)矩陣求逆的維度，如圖3所示，在同等精度下可以減少35%的計(jì)算時(shí)間。

圖3 分層優(yōu)化算法

1.3 后端優(yōu)化

前端里程計(jì)每一步的解算都存在誤差，隨著航位推算的距離增加，全局位姿的誤差也在累積，根據(jù)位姿拼接起來(lái)的地圖也會(huì)發(fā)生畸變。在SLAM領(lǐng)域，又提出了后端優(yōu)化的關(guān)鍵技術(shù)，對(duì)位姿進(jìn)行優(yōu)化，得到全局一致的軌跡和地圖。始終假設(shè)后一時(shí)刻的估計(jì)較前一時(shí)刻帶有更多的誤差，因此對(duì)已經(jīng)構(gòu)建的地圖的信任程度遠(yuǎn)高于相鄰幀點(diǎn)云配準(zhǔn)后的運(yùn)動(dòng)估計(jì)。

本文通過(guò)幀間距離和角度的關(guān)系來(lái)劃分關(guān)鍵幀，距離相隔1 m或者角度轉(zhuǎn)過(guò)30°就向后端輸入1個(gè)關(guān)鍵幀；一旦前端判斷當(dāng)前幀為關(guān)鍵幀后，從后端的關(guān)鍵幀庫(kù)中查詢?cè)诋?dāng)前位置80 m范圍內(nèi)的關(guān)鍵幀軌跡點(diǎn)；并將對(duì)應(yīng)的點(diǎn)云疊加、構(gòu)建局部地圖，進(jìn)行基于地圖的線面-ICP匹配；最后將優(yōu)化后的位姿更新到關(guān)鍵幀點(diǎn)云庫(kù)和軌跡庫(kù)中。

如果系統(tǒng)中還有GNSS數(shù)據(jù)融合，考慮到GNSS 數(shù)據(jù)具有不發(fā)散的良好特性，可以用來(lái)抑制激光SLAM的長(zhǎng)時(shí)性漂移，保證了SLAM軌跡漂移在1個(gè)可控范圍之內(nèi)。將車輛的初始時(shí)間和位置和GNSS 時(shí)間和坐標(biāo)系對(duì)準(zhǔn)后，設(shè)定每1 Hz得到對(duì)應(yīng)的GNSS先驗(yàn)數(shù)據(jù)。系統(tǒng)首先進(jìn)行3維激光點(diǎn)云配準(zhǔn)，然后融合GNSS先驗(yàn)位置數(shù)據(jù)和對(duì)應(yīng)位姿點(diǎn)，經(jīng)過(guò)GNSS融合后的位姿估計(jì)才被存儲(chǔ)到關(guān)鍵幀點(diǎn)云庫(kù)和軌跡庫(kù)中?；谑剑?）的觀測(cè)模型，觀測(cè)值為源點(diǎn)云到最近鄰目標(biāo)點(diǎn)云形成的線/面的歐氏距離，可以引入1個(gè)附加先驗(yàn)信息的最小二乘估計(jì)器，假設(shè)觀測(cè)方程線性化后得到系數(shù)矩陣為B，觀測(cè)值為L(zhǎng)，觀測(cè)值權(quán)陣為P，則附加先驗(yàn)信息的最小二乘法方程為

式中：待估的6自由度的位姿(R t)是當(dāng)前幀相對(duì)于全局坐標(biāo)系的旋轉(zhuǎn)、平移量；N、W的值由公式N=BTPB，W=BTPL求得；PG是具有先驗(yàn)信息的平移量t的權(quán)陣，根據(jù)經(jīng)驗(yàn)設(shè)定；LG表示t的GNSS先驗(yàn)值減去前端ICP位姿估計(jì)值的差。

2 實(shí)驗(yàn)與結(jié)果分析

為了對(duì)激光SLAM系統(tǒng)的車載導(dǎo)航性能進(jìn)行測(cè)試和評(píng)估，采用不同道路環(huán)境的數(shù)據(jù)對(duì)算法的整體定位定姿性能以及GNSS融合對(duì)定位結(jié)果的影響進(jìn)行分析。本文測(cè)試了卡爾斯魯厄工學(xué)院和豐田技術(shù)學(xué)院聯(lián)合采集的開(kāi)源數(shù)據(jù)集基蒂（KITTI）[13]，其采集環(huán)境包括公路、城市及鄉(xiāng)村。采集設(shè)備有一套OXTS RT3003的GNSS與慣性測(cè)量系統(tǒng)（inertial measurement unit, IMU）組合導(dǎo)航系統(tǒng)和一臺(tái)Velodyne 64線激光雷達(dá)。激光雷達(dá)以10 Hz的頻率進(jìn)行旋轉(zhuǎn)掃描，測(cè)距精度0.02 m（＜1δ），水平視角360°，水平分辨率0.09°，豎直視角為26.8 °；涵蓋三類環(huán)境，“公路”道路寬、車速快；“城市”道路兩旁是建筑物，“鄉(xiāng)村”道路兩旁是大量植被。其實(shí)驗(yàn)設(shè)備、采集環(huán)境如圖4所示。

2.1 整體精度分析

把GNSS/INS的解算結(jié)果視為參考真值，和純激光SLAM實(shí)時(shí)輸出的定位定姿結(jié)果進(jìn)行誤差分析，水平軌跡誤差的大小由灰度深淺來(lái)表示，灰度越深代表偏差越大，如圖5所示。

圖4 實(shí)驗(yàn)裝備與采集環(huán)境

KITTI共有11組數(shù)據(jù)，但是由于公路場(chǎng)景下車速達(dá)到80 km/h，點(diǎn)云變化速度快，特征提取稀疏，SLAM位姿誤差隨時(shí)間很快地發(fā)散了，在圖5中沒(méi)有展示出來(lái)。由圖5可知，大部分的軌跡和參考真值重合度高，但是仍明顯存在著累計(jì)誤差隨行駛距離增大而增大的情況。這里采用平均相對(duì)誤差進(jìn)行精度分析，每行駛100 m計(jì)算1次誤差，得到整體的定位定姿精度如表1所示。

圖5 整體軌跡誤差結(jié)果

表1 KITTI數(shù)據(jù)描述以及整體解算精度

由表1可知，本文給出的線面特征結(jié)合的純激光SLAM在車速20～50 km/h的結(jié)構(gòu)化環(huán)境下，能較好地維持車輛的位姿，整體定位精度約為1.98%，即行駛100 m，偏離真值1.98 m，角度誤差為0.005 1(°)/m。分析幾個(gè)相對(duì)誤差較大的軌跡，如序號(hào)4、序號(hào)8的軌跡，明顯是誤差隨著行駛距離的增加而累積，所以激光雷達(dá)必須融合其他具備絕對(duì)定位能力的傳感器數(shù)據(jù)，才能抑制自身長(zhǎng)時(shí)性漂移；對(duì)序號(hào)7的異常軌跡分析發(fā)現(xiàn)，車輛經(jīng)過(guò)了多次快速轉(zhuǎn)彎、環(huán)境退化（大量相似及不穩(wěn)定特征干擾導(dǎo)致）的場(chǎng)景，在平面上產(chǎn)生了嚴(yán)重漂移。

2.2 后端優(yōu)化對(duì)定位定姿結(jié)果的影響

后端優(yōu)化將當(dāng)前關(guān)鍵幀和距離80 m以內(nèi)的歷史關(guān)鍵幀組成的地圖關(guān)聯(lián)起來(lái)，用構(gòu)建好的地圖對(duì)當(dāng)前幀的位姿進(jìn)行精化。為了具體分析后端地圖優(yōu)化模塊對(duì)整體定位定姿精度的影響，以序列6數(shù)據(jù)為例，這是1組城市道路的數(shù)據(jù)，全長(zhǎng)700 m左右。分析不加地圖優(yōu)化和加入地圖優(yōu)化后，對(duì)比兩種模式下估計(jì)出來(lái)的軌跡和定位定姿結(jié)果的誤差序列，結(jié)果如圖6至圖8所示。

圖6 后端優(yōu)化對(duì)整體精度的影響

圖7 后端優(yōu)化對(duì)位置估計(jì)的影響

圖8 后端優(yōu)化對(duì)姿態(tài)估計(jì)的影響

由圖6可以看出：沒(méi)有進(jìn)行后端優(yōu)化的軌跡尺度漂移明顯，各方向的位置、角度逐漸偏離真值。反之，成功的地圖優(yōu)化可使得起點(diǎn)和終點(diǎn)高度重合，最大偏差顯著降低。本文采用均方根誤差（root mean square error, RMSE）作為評(píng)價(jià)標(biāo)準(zhǔn)，來(lái)定量地分析優(yōu)化前后相對(duì)精度變化，其結(jié)果如表2所示。

表2 后端優(yōu)化前后RMSE

水平定位精度提升至1～2 m，角度精度約為2°，提高了66.7%的姿態(tài)精度，說(shuō)明優(yōu)化后整體精度有了顯著提高，驗(yàn)證了后端優(yōu)化的有效性。在沒(méi)有其他傳感器的融合輔助下，成功的后端優(yōu)化可以在一定程度上抑制激光SLAM自身的長(zhǎng)時(shí)性漂移。

2.3 GNSS融合對(duì)定位定姿結(jié)果的影響

圖9 GNSS融合對(duì)整體精度的影響

為進(jìn)一步分析多傳感器融合對(duì)3維激光SLAM位姿估計(jì)的影響，在后端優(yōu)化中引入GNSS位置先驗(yàn)數(shù)據(jù)，采用有先驗(yàn)約束的整體平差法進(jìn)行數(shù)據(jù)融合，這是1種松組合的方法，便于多傳感器的拓展。將車輛的初始時(shí)間和位置與 GNSS 時(shí)間和坐標(biāo)系對(duì)準(zhǔn)后，參考真值轉(zhuǎn)換為GNSS約束條件，模擬實(shí)時(shí)GNSS約束輸入。以長(zhǎng)路程的序列1、序列7的數(shù)據(jù)為例，GNSS融合后估計(jì)出來(lái)的軌跡和建圖效果如圖9所示。

由圖9可知，融合了GNSS數(shù)據(jù)的計(jì)算軌跡和參考真值軌跡的重合度更高：當(dāng)有GNSS數(shù)據(jù)約束時(shí)，可抑制激光雷達(dá)里程計(jì)的漂移發(fā)散；沒(méi)有GNSS數(shù)據(jù)約束時(shí)，激光雷達(dá)里程計(jì)依賴自身航位推算進(jìn)行位姿估計(jì)與建圖。

表3分析了車輛行駛中水平方向上的定位精度。

表3 KITTI數(shù)據(jù)定位結(jié)果分析

由表3可知，通過(guò)精度對(duì)比，GNSS/激光SLAM的融合算法明顯優(yōu)于純SLAM位姿優(yōu)化算法。該定位方法不存在由里程計(jì)引起的累積誤差，在30 km/h左右的速度下，表現(xiàn)出良好的定位性能，適用于在GNSS條件良好下的點(diǎn)云地圖構(gòu)建。定位精度提高至25 cm左右時(shí)，和純GNSS定位精度相當(dāng)，但融合算法能輸出更高頻率的位姿結(jié)果，有助于生成點(diǎn)云密度更高、一致性良好的點(diǎn)云地圖。

3 結(jié)束語(yǔ)

本文針對(duì)傳統(tǒng)全點(diǎn)云ICP算法存儲(chǔ)和運(yùn)算消耗大、點(diǎn)云誤匹配的問(wèn)題，提出了1種線面特征結(jié)合的點(diǎn)云精配準(zhǔn)算法。通過(guò)對(duì)獲取的點(diǎn)云進(jìn)行分割和特征提取，再對(duì)結(jié)構(gòu)化的線、面特征點(diǎn)云進(jìn)行精配準(zhǔn)，并在精配準(zhǔn)的過(guò)程中，同時(shí)實(shí)驗(yàn)姿點(diǎn)云運(yùn)動(dòng)的畸變校正，最后通過(guò)KITTI測(cè)試，對(duì)算法的可行性和有效性進(jìn)行了評(píng)估，驗(yàn)證了該算法的整體定位相對(duì)精度約為1.98%，定姿相對(duì)精度約為0.005 1(°)/m；后端優(yōu)化在一定程度上可抑制激光SLAM自身的長(zhǎng)時(shí)性漂移；當(dāng)有GNSS約束時(shí)，軌跡無(wú)發(fā)散，定位精度約為25 cm，達(dá)到分米級(jí)定位精度。