一種基于LiDAR-IMU-GNSS 系統(tǒng)同步進(jìn)行車(chē)輛定位和路旁桿狀物清查的方法

袁 超，潘文波，陳志偉，黃文宇，李源征宇，楊振宇

（中車(chē)株洲電力機(jī)車(chē)研究所有限公司，湖南 株洲 412001）

0 引言

路旁一般情況下主要存在著樹(shù)木、路燈等桿狀物，它們作為參照物在城市、城際和郊區(qū)道路環(huán)境和安全分析中發(fā)揮著重要作用［1］，但也給自動(dòng)駕駛帶來(lái)挑戰(zhàn)。樹(shù)木可能干擾衛(wèi)星定位信號(hào)并阻礙傳感器的可視性，導(dǎo)致出現(xiàn)定位精度下降、潛在的遺漏或錯(cuò)誤檢測(cè)等問(wèn)題，甚至造成安全事故［2］。自動(dòng)駕駛系統(tǒng)依賴對(duì)周?chē)h(huán)境的全面理解以有效感知并應(yīng)對(duì)復(fù)雜的道路情況，其中路旁桿狀物作為重要的路邊特征對(duì)車(chē)輛定位和安全分析具有重要影響。因此，開(kāi)發(fā)一種可同時(shí)實(shí)現(xiàn)定位、高精地圖創(chuàng)建以及路邊桿狀物識(shí)別和分類(lèi)的有效方法對(duì)于提高駕駛安全至關(guān)重要。

傳統(tǒng)測(cè)繪方式耗時(shí)、流程煩瑣且設(shè)備昂貴。因此，航空影像、光檢測(cè)與測(cè)距點(diǎn)云等遙感數(shù)據(jù)成為創(chuàng)建路邊桿狀物清單的重要資源［3］。移動(dòng)激光掃描（mobile laser scanning，MLS）系統(tǒng)由于能夠安全高效地獲取準(zhǔn)確的三維信息，因此在道路環(huán)境建模中變得越來(lái)越重要［4］。MLS系統(tǒng)通過(guò)收集激光脈沖的坐標(biāo)來(lái)生成密集的點(diǎn)云，其速率可達(dá)每秒一百萬(wàn)個(gè)點(diǎn)，可見(jiàn)MLS 系統(tǒng)在提取道路環(huán)境（包括樹(shù)木、交通標(biāo)志和其他特征）信息方面具有強(qiáng)大的能力［5-6］。MLS 系統(tǒng)由高精度的全球?qū)Ш叫l(wèi)星系統(tǒng)（global navigation satellite system，GNSS）、慣性測(cè)量單 元（inertial measurement unit，IMU）和測(cè)繪級(jí)激光掃描儀組成。雖然MLS 系統(tǒng)能夠?qū)崿F(xiàn)高精度的三維地圖創(chuàng)建，但高昂的成本使其難以被廣泛應(yīng)用。此外，MLS 系統(tǒng)的地圖創(chuàng)建和感知效率較低，難以滿足自動(dòng)駕駛的實(shí)時(shí)性需求。

雖然同步定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）技術(shù)對(duì)于車(chē)輛定位和實(shí)時(shí)路旁桿狀物清單創(chuàng)建具有潛力，但目前仍缺乏能夠?qū)崿F(xiàn)同時(shí)進(jìn)行車(chē)輛定位和路邊桿狀物清單創(chuàng)建的方法。主要的技術(shù)挑戰(zhàn)如下：

1）長(zhǎng)距離的樹(shù)木遮擋會(huì)阻礙GNSS信號(hào)，顯著降低衛(wèi)星導(dǎo)航的精度。慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）依賴IMU進(jìn)行車(chē)輛定位，而IMU累計(jì)誤差大。

2）單獨(dú)運(yùn)行SLAM 算法和路旁桿狀物提取算法會(huì)導(dǎo)致較高的計(jì)算成本。現(xiàn)有的SLAM 特征提取算法和路旁桿狀物特征提取算法存在明顯差異，很難采用統(tǒng)一的算法來(lái)同時(shí)實(shí)現(xiàn)車(chē)輛定位和實(shí)時(shí)路旁桿狀物清單創(chuàng)建。

3）在道路條件復(fù)雜的地區(qū)，利用共享SLAM特征信息的桿狀物檢測(cè)算法容易受到環(huán)境干擾而產(chǎn)生誤報(bào)。

基于桿狀物結(jié)構(gòu)識(shí)別、樹(shù)木清單創(chuàng)建方法和SLAM 算法，本文提出了一個(gè)準(zhǔn)確而穩(wěn)健的車(chē)輛定位和路旁桿狀物清單創(chuàng)建系統(tǒng)，其具有以下特點(diǎn)：

1）引入適用于長(zhǎng)距離遮擋場(chǎng)景定位和地圖構(gòu)建的方案。該方案通過(guò)滑動(dòng)窗口形式，緊耦合了激光雷達(dá)（LiDAR）、IMU和GNSS，減少了遮擋和退化場(chǎng)景中的誤差積累，提高了系統(tǒng)的準(zhǔn)確性。

2）采用兩階段的方法來(lái)最小化全局地圖誤差，通過(guò)GNSS輔助來(lái)修正累積的地圖誤差。

3）提出了一種桿狀物檢測(cè)方法，利用SLAM中的共享特征提取算法，從而最大化特征信息的利用。該算法減少了系統(tǒng)的計(jì)算成本，實(shí)現(xiàn)了同時(shí)定位和桿狀物檢測(cè)。

4）引入了一種利用方位角特征信息進(jìn)一步減少誤報(bào)的方法。

本文所提系統(tǒng)在城市和郊區(qū)進(jìn)行了廣泛的測(cè)試和評(píng)估。結(jié)果表明，該系統(tǒng)具有較好的準(zhǔn)確性和穩(wěn)健性，能夠有效處理各種場(chǎng)景中的定位和桿狀物清單創(chuàng)建任務(wù)。

1 研究現(xiàn)狀

目前，使用MLS 自動(dòng)提取燈桿、電線桿等形狀單一的桿狀物的信息已經(jīng)有了快速、高效的解決方案，且準(zhǔn)確率較高［5-6］。然而，樹(shù)木的形狀較為復(fù)雜，需要區(qū)分樹(shù)干與樹(shù)冠。在過(guò)去的十年中，關(guān)于從點(diǎn)云數(shù)據(jù)中進(jìn)行個(gè)體樹(shù)木分割也受到了重視，例如從密集的點(diǎn)云信息中獲取樹(shù)木高度、樹(shù)干直徑、胸徑等屬性信息［7-11］并基于點(diǎn)云數(shù)據(jù)進(jìn)行3D目標(biāo)檢測(cè)［12］。此外，將相機(jī)與MLS點(diǎn)云結(jié)合起來(lái)添加圖像信息的研究也受到了廣泛的關(guān)注。例如，將全景圖像與MLS點(diǎn)云融合，在分割過(guò)程中添加來(lái)自圖像的顏色信息作為樹(shù)木識(shí)別的行動(dòng)準(zhǔn)則［13］。類(lèi)似的方法包括將多光譜圖像信息與MLS點(diǎn)云融合以進(jìn)行樹(shù)木識(shí)別［14-16］。然而，這些依賴MLS 系統(tǒng)的方法需要昂貴的高精度GNSS/IMU定位設(shè)備和測(cè)量級(jí)激光掃描儀，限制了它們的大規(guī)模部署應(yīng)用。此外，基于高精度點(diǎn)云數(shù)據(jù)進(jìn)行自動(dòng)樹(shù)木識(shí)別和標(biāo)定的方法實(shí)時(shí)性能較差。

激光雷達(dá)以其對(duì)光照變化的抗干擾性和精確測(cè)距能力在物體感知和導(dǎo)航領(lǐng)域得到了廣泛的應(yīng)用［17］。激光雷達(dá)傳感器在各種機(jī)器人應(yīng)用中越來(lái)越常見(jiàn)，如自動(dòng)駕駛車(chē)輛［18］、無(wú)人機(jī)［19］等。文獻(xiàn)［20］將SLAM 姿態(tài)估計(jì)轉(zhuǎn)化為最小二乘優(yōu)化問(wèn)題，利用泰勒展開(kāi)將非線性目標(biāo)函數(shù)線性化，并通過(guò)梯度下降、高斯-牛頓或列文伯格-馬夸爾特法等方法進(jìn)行求解。最為經(jīng)典的3D激光雷達(dá)SLAM 算法——實(shí)時(shí)激光里程計(jì)和建圖（lidar odometry and mapping in real-time，LOAM）根據(jù)曲率從每幀中提取角點(diǎn)和平面點(diǎn)，并基于角點(diǎn)構(gòu)建特征線，基于平面點(diǎn)構(gòu)建特征平面?；趫?zhí)行點(diǎn)云配準(zhǔn)并使用最小二乘優(yōu)化方法解決姿態(tài)，LOAM通過(guò)低頻率建圖和高頻率定位確保了準(zhǔn)確的定位和良好的建圖結(jié)果。然而，該方法缺乏回環(huán)檢測(cè)模塊［21］，無(wú)法修正累積誤差且不能進(jìn)行重定位。文獻(xiàn)［22］改進(jìn)了LOAM，對(duì)點(diǎn)云進(jìn)行聚類(lèi)和分割，成功地將地面點(diǎn)與其他點(diǎn)分離。其基于聚類(lèi)方法，過(guò)濾掉了不可靠的點(diǎn)云，提高了特征點(diǎn)的質(zhì)量；還提出了一種兩步優(yōu)化方法來(lái)加速姿態(tài)估計(jì)和收斂速度；此外，還引入了一種基于歐幾里得距離的回環(huán)檢測(cè)方法，以消除累積誤差。這種方法比LOAM 更高效，更適合在自動(dòng)駕駛系統(tǒng)中部署，同時(shí)在系統(tǒng)完整性方面也超過(guò)了LOAM［22］。2021 年，文獻(xiàn)［23］提出了一種利用激光雷達(dá)點(diǎn)云中的幾何和強(qiáng)度信息的新型SLAM 解決方案，其設(shè)計(jì)了一個(gè)基于強(qiáng)度信息的前端里程計(jì)估計(jì)和一個(gè)基于強(qiáng)度信息的后端優(yōu)化的方法，該方法優(yōu)于僅依賴幾何信息的SLAM方法。文獻(xiàn)［24］介紹了一種名為R3LIVE的新型激光雷達(dá)-慣性-視覺(jué)傳感器融合框架，其利用激光雷達(dá)、慣性和視覺(jué)傳感器的測(cè)量來(lái)進(jìn)行強(qiáng)大而準(zhǔn)確的狀態(tài)估計(jì)。該框架還整合了視覺(jué)慣性傳感器的數(shù)據(jù)并渲染地圖紋理。文獻(xiàn)［25］提出了一種實(shí)時(shí)、準(zhǔn)確、穩(wěn)健的激光雷達(dá)SLAM 方法，其將非重復(fù)掃描激光雷達(dá)、IMU、輪式里程計(jì)和GNSS 緊密耦合，用于姿態(tài)估計(jì)和同步全局地圖生成。盡管當(dāng)前基于激光雷達(dá)的SLAM 算法在許多場(chǎng)景中已經(jīng)呈現(xiàn)出足夠的精度和穩(wěn)健性，但在退化和大規(guī)模環(huán)境中仍面臨挑戰(zhàn)。此外，上述提到的SLAM方法中的特征提取算法并未考慮到桿狀物檢測(cè)和識(shí)別的要求，使得同時(shí)實(shí)現(xiàn)車(chē)輛定位和路旁桿狀物清單創(chuàng)建變得困難。

2 系統(tǒng)介紹

本文提出了一種可以同時(shí)實(shí)現(xiàn)車(chē)輛定位和道路旁桿狀物清單生成的多傳感器融合方案，其能夠在長(zhǎng)距離樹(shù)木遮擋衛(wèi)星信號(hào)和光照條件不理想等場(chǎng)景下穩(wěn)定運(yùn)行。在選擇定位、建圖和感知傳感器時(shí)，常用的選項(xiàng)包括GNSS、IMU、激光雷達(dá)和相機(jī)等。然而，相機(jī)易受光照影響，而GNSS+IMU在長(zhǎng)距離遮擋場(chǎng)景下會(huì)累積顯著誤差。因此，本文主要采用激光雷達(dá)和組合慣導(dǎo)作為主要傳感器。車(chē)輛定位和路旁桿狀物清單創(chuàng)建系統(tǒng)的工作流程如圖1所示。

圖1 車(chē)輛定位和路旁桿狀物清單創(chuàng)建系統(tǒng)概述Fig.1 Overview of vehicle localization and roadside pole-shaped object inventory creation system

在本文提出的系統(tǒng)中，首先通過(guò)將激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)與組合慣導(dǎo)中的測(cè)量值相結(jié)合，對(duì)激光點(diǎn)云進(jìn)行運(yùn)動(dòng)補(bǔ)償和畸變?nèi)コ忍幚?，再?gòu)狞c(diǎn)云中提取邊緣特征、平面特征等信息。接著，提取的特征點(diǎn)被分別應(yīng)用于桿狀物檢測(cè)模塊和迭代卡爾曼濾波模塊的計(jì)算中。桿狀物檢測(cè)模塊對(duì)樹(shù)干特征進(jìn)行聚類(lèi)和檢測(cè)，并將屬于樹(shù)木特征點(diǎn)的屬性信息發(fā)送到全局地圖，以增加這些特征點(diǎn)的樹(shù)木屬性信息。迭代卡爾曼濾波模塊利用特征點(diǎn)和局部地圖構(gòu)建殘差方程，并結(jié)合IMU預(yù)積分對(duì)位姿狀態(tài)進(jìn)行更新。如果誤差狀態(tài)收斂，則輸出位姿信息；否則，繼續(xù)迭代。然后，根據(jù)迭代卡爾曼濾波模塊輸出的位姿信息來(lái)更新特征點(diǎn)并將其添加到全局地圖中。最后，在全局地圖中根據(jù)樹(shù)木特征點(diǎn)的方位信息進(jìn)一步過(guò)濾樹(shù)冠的誤報(bào)，并通過(guò)GNSS-RTK 信息或回環(huán)檢測(cè)進(jìn)一步優(yōu)化關(guān)鍵幀的位姿。

2.1 特征提取

本節(jié)對(duì)激光里程計(jì)特征提取模塊進(jìn)行介紹，主要針對(duì)點(diǎn)云中面特征和線特征候選點(diǎn)篩選展開(kāi)描述。

2.1.1 候選點(diǎn)計(jì)算

激光雷達(dá)感知周?chē)h(huán)境信息，形成三維點(diǎn)云，一幀點(diǎn)云往往包含上萬(wàn)至數(shù)十萬(wàn)的點(diǎn)，如果全部用于計(jì)算，會(huì)極大消耗計(jì)算資源，且實(shí)時(shí)性也無(wú)法得到滿足。如圖2 所示，空間中存在大量的特征點(diǎn)云，如面點(diǎn)和線點(diǎn)，通過(guò)對(duì)該類(lèi)點(diǎn)云進(jìn)行提取，可以極大地減少計(jì)算過(guò)程中使用點(diǎn)的個(gè)數(shù)，從而節(jié)約計(jì)算時(shí)間［21-22］。

圖2 激光雷達(dá)點(diǎn)云Fig.2 LiDAR point cloud

現(xiàn)有的特征點(diǎn)計(jì)算算法，如 LOAM 和 Lego-LOAM 等，通過(guò)計(jì)算同一線激光點(diǎn)中相鄰點(diǎn)的空間距離，得出該點(diǎn)的線曲率，并以該線曲率作為分類(lèi)特征點(diǎn)的標(biāo)準(zhǔn)。該方法可以有效地通過(guò)計(jì)算得到面點(diǎn)和線點(diǎn)，能很好地檢測(cè)到路燈和電線桿，但是特征提取方法不適用于樹(shù)木檢測(cè)，且當(dāng)激光入射角度和距離變化時(shí)，特征提取將變得不穩(wěn)定。例如，由于現(xiàn)有的SLAM的特征提取方法在樹(shù)葉密集情況下容易將樹(shù)葉點(diǎn)云作為無(wú)效特征點(diǎn)濾除，導(dǎo)致后續(xù)基于特征點(diǎn)的樹(shù)冠部分點(diǎn)丟失。當(dāng)激光雷達(dá)入射角正對(duì)樹(shù)葉且距離適中時(shí)，不僅有部分激光點(diǎn)云被表層樹(shù)葉反射，還有相當(dāng)一部分穿過(guò)表層樹(shù)葉，被后面的樹(shù)葉或樹(shù)枝反射回來(lái)。在這種情況下，可以較好地提取出角點(diǎn)。當(dāng)入射距離比較近且樹(shù)葉茂密時(shí)，可能連續(xù)出現(xiàn)在同一線束上多個(gè)點(diǎn)被表層樹(shù)葉反射的情況，計(jì)算得到的線曲率將明顯變小，甚至?xí)`判成面點(diǎn)。因此，本文將重點(diǎn)介紹如何解決樹(shù)木檢測(cè)的問(wèn)題。

本文通過(guò)自適應(yīng)空間幾何候選特征計(jì)算方式，對(duì)面點(diǎn)候選集合P1和線點(diǎn)候選集合L1進(jìn)行初步篩選。

記一幀完整點(diǎn)云為Q，待判斷點(diǎn)為qi。首先對(duì)點(diǎn)云進(jìn)行預(yù)處理操作，去除無(wú)效點(diǎn)后，根據(jù)點(diǎn)云中各點(diǎn)的行列索引，將點(diǎn)云映射至序列圖上，如圖3所示。點(diǎn)云中各個(gè)有效點(diǎn)都將被映射到序列圖上，以方便后續(xù)查詢操作。若點(diǎn)qi的行屬性為m、列屬性為n，則其將被映射到序列圖中第m行、第n列，索引符號(hào)被記作indexm，n。

圖3 點(diǎn)云序列映射圖Fig.3 Point cloud sequence mapping

通過(guò)序列圖對(duì)點(diǎn)進(jìn)行索引，得到點(diǎn)pi。對(duì)點(diǎn)pi進(jìn)行特征初步判斷時(shí)，不再通過(guò)選取固定個(gè)數(shù)領(lǐng)域點(diǎn)的方式計(jì)算曲率，而是根據(jù)距離閾值 Δd對(duì)周?chē)c(diǎn)進(jìn)行篩選，并對(duì)與pi距離為 Δd的同一條線上的點(diǎn)pi～pi-m和pi～pi+n構(gòu)建特征判斷函數(shù)，具體如下：

式中：ci——候選點(diǎn)判斷值；pi+j，pi-j——pi點(diǎn)前j點(diǎn)和后j點(diǎn)。

Nj計(jì)算公式如下：

計(jì)算得到ci后，對(duì)ci作如下判斷，將其分類(lèi)為P1和L1：

式中：Δc——平面特征點(diǎn)的閾值。

2.1.2 特征點(diǎn)篩選

根據(jù)式（1）～式（3）計(jì)算，得到P1和L1；進(jìn)一步對(duì)L1進(jìn)行篩選，得到篩選后的線點(diǎn)候選集合L2，并建立體素柵格，對(duì)密集點(diǎn)云進(jìn)行采樣，完成面特征點(diǎn)集合P和線特征點(diǎn)集合L的篩選。

針對(duì)L1構(gòu)建分類(lèi)，如圖4所示。

圖4 線點(diǎn)候選集合分類(lèi)篩選Fig.4 Classification and filtering of candidate line feature point cloud sets

記L1中線點(diǎn)pli，其周?chē)鷄×b鄰域內(nèi)線特征點(diǎn)個(gè)數(shù)記為k，線特征點(diǎn)鄰域內(nèi)同類(lèi)點(diǎn)個(gè)數(shù)的閾值為ΔK，則根據(jù)式（4）對(duì)pli篩選：

圖4中，橘色索引表示當(dāng)前待篩選點(diǎn)pli，對(duì)其鄰域 5 × 3 格內(nèi)的點(diǎn)進(jìn)行查詢，比如索引為index4，4的點(diǎn)，其周?chē)樵兊?6 個(gè)同類(lèi)點(diǎn)；而索引為（index4，n-1）的待篩選點(diǎn)，其周?chē)鷥H一個(gè)同類(lèi)點(diǎn)。設(shè)置ΔK=5，并代入式（4），則刪除（index4，n-1）索引點(diǎn)，并在L2中加入index4，4索引點(diǎn)。

對(duì)P1和L2進(jìn)行體素網(wǎng)格采樣，首先根據(jù)各點(diǎn)坐標(biāo)（x，y，z）計(jì)算各點(diǎn)所處體素柵格坐標(biāo)（coordinateX、coordinateY、coordinateZ），假設(shè)體素柵格長(zhǎng)、寬、高分別為l、w、h，則

一個(gè)體素柵格中可能存在多個(gè)特征點(diǎn)，這些聚集的特征點(diǎn)如果全部被用于位姿解算，不僅不會(huì)提升定位精度，反而會(huì)加大時(shí)間消耗。若某一體素網(wǎng)格中，點(diǎn)云簇過(guò)于聚集，則對(duì)其進(jìn)行體素采樣，每個(gè)體素網(wǎng)格保留單個(gè)特征點(diǎn)，用于位姿解算過(guò)程。通過(guò)體素采樣的方式，能夠解決點(diǎn)云簇過(guò)于聚集的問(wèn)題，最終得到特征點(diǎn)集合P和L。

2.2 桿狀物檢測(cè)

為了確保車(chē)輛自動(dòng)駕駛的安全性，需要實(shí)時(shí)檢測(cè)桿狀物的信息。路燈桿形狀單一，易被檢測(cè)；而樹(shù)木形狀復(fù)雜且需檢測(cè)出樹(shù)干部分。因此，本文首次提出直接利用SLAM中的特征點(diǎn)進(jìn)行樹(shù)木檢測(cè)，以提高檢測(cè)效率。大部分樹(shù)木特征點(diǎn)云位于特征點(diǎn)集合L中，因此本文對(duì)集合L中的特征點(diǎn)進(jìn)行聚類(lèi)和提取樹(shù)干特征。在提取樹(shù)干之前，根據(jù)車(chē)輛姿態(tài)和雷達(dá)的安裝高度，直接濾除地面以下和接近地面的特征點(diǎn)，這樣可以進(jìn)一步減輕計(jì)算負(fù)擔(dān)；此外，這些接近地面的點(diǎn)通常代表灌木叢和路旁設(shè)備，可能在樹(shù)干提取步驟中引起錯(cuò)誤提取的問(wèn)題。

樹(shù)干可被定義為從根部或靠近地面的地平線開(kāi)始向上延伸到一個(gè)或多個(gè)不確定點(diǎn)的樹(shù)木的主干。樹(shù)干的提取分為兩個(gè)步驟，即聚類(lèi)和樹(shù)干識(shí)別。

基于密度的聚類(lèi)（DBSCAN）方法將具有相似距離和角度的特征點(diǎn)分組到同一個(gè)聚類(lèi)中。針對(duì)樹(shù)干特征提取，本文對(duì)DBSCAN算法進(jìn)行了改進(jìn)，主要體現(xiàn)在搜索方向權(quán)重的調(diào)整和樹(shù)干聚類(lèi)的優(yōu)化兩個(gè)方面。首先，考慮到樹(shù)干主要向上（即z方向）生長(zhǎng)，雷達(dá)在z方向上測(cè)量的點(diǎn)之間的距離較大，而在x和y方向上較為密集，因此，在x、y和z方向上賦予不同的權(quán)重ωx、ωy和ωz。改進(jìn)后的聚類(lèi)算法中的搜索距離如式（6）所示。

式中：Dij——目標(biāo)i與目標(biāo)j的搜索距離；xi、yi、zi——目標(biāo)i的坐標(biāo)；xj、yj、zj——目標(biāo)j的坐標(biāo)。

調(diào)整權(quán)重系數(shù)，使得ωx=ωy＞ωz，此時(shí)Dij受x和y方向上距離的影響將大于z方向，這也符合樹(shù)干現(xiàn)狀特性及反射點(diǎn)在x、y方向和z方向上的差異情況。

為了滿足實(shí)時(shí)性的需求，需要快速聚類(lèi)出樹(shù)干。首先選擇高度約為1.5 m的特征點(diǎn)作為DBSCAN聚類(lèi)的初始點(diǎn)。由于樹(shù)干部分的點(diǎn)云比樹(shù)冠部分更密集，特別是在x和y方向上的距離差異較大，因此可以根據(jù)樹(shù)冠和樹(shù)干之間特征點(diǎn)的差異將樹(shù)干單獨(dú)聚類(lèi)出來(lái)。

樹(shù)干反射的雷達(dá)點(diǎn)具有獨(dú)特的形狀特征，可以將其與其他目標(biāo)區(qū)分開(kāi)。因此，本文系統(tǒng)主要依靠聚類(lèi)后的形狀估計(jì)來(lái)綜合判斷目標(biāo)是否為樹(shù)干，即對(duì)一組聚類(lèi)點(diǎn)進(jìn)行形狀估計(jì)，從而得到一個(gè)矩形框，通過(guò)矩形的高寬比來(lái)判斷目標(biāo)是否為樹(shù)干；同時(shí)，將樹(shù)干上方一定范圍內(nèi)的角特征點(diǎn)視為樹(shù)冠點(diǎn)云。由于樹(shù)冠點(diǎn)云較為稀疏，在地圖構(gòu)建的過(guò)程中，本文系統(tǒng)通過(guò)疊加多幀特征點(diǎn)來(lái)進(jìn)一步識(shí)別樹(shù)冠。

2.3 前端里程計(jì)

本文所提地圖構(gòu)建算法利用信號(hào)質(zhì)量良好的歷史GNSS-RTK 定位信息構(gòu)建關(guān)鍵幀局部地圖，以豐富點(diǎn)云地圖歷史信息。前端里程計(jì)利用位姿優(yōu)化初值將當(dāng)前提取的幀特征點(diǎn)投影至局部地圖，并進(jìn)行最近點(diǎn)查詢。若最近點(diǎn)距離大于預(yù)先設(shè)定的距離閾值，則該特征點(diǎn)為離群點(diǎn)，無(wú)法構(gòu)建重投影誤差；反之，構(gòu)建點(diǎn)云重投影代價(jià)函數(shù)（點(diǎn)-面距離、點(diǎn)-點(diǎn)距離），利用非線性最小二乘法迭代優(yōu)化位姿參數(shù)。最后，根據(jù)位姿更新量進(jìn)行關(guān)鍵幀診斷，若為關(guān)鍵幀，則更新局部地圖。本文對(duì)局部地圖采用滑動(dòng)窗口法，在保證實(shí)時(shí)動(dòng)態(tài)更新的同時(shí)，達(dá)到減少數(shù)據(jù)量和計(jì)算量的目的。點(diǎn)-面重投影誤差rpi，f、點(diǎn)-點(diǎn)重投影誤差rpj，s如式（7）所示。點(diǎn)-面重投影誤差和點(diǎn)-點(diǎn)重投影誤差組成的優(yōu)化目標(biāo)函數(shù)f(R，t)如式（8）所示。

式中：rpk，f——平面特征點(diǎn)重投影誤差；pk——當(dāng)前幀激光點(diǎn)云平面特征點(diǎn)；R——待優(yōu)化的旋轉(zhuǎn)矩陣；t——待優(yōu)化平移向量；pk，m——pk在局部地圖中的最近點(diǎn)；nk——局部地圖平面點(diǎn)pk，m所在平面單位法向量；pj——當(dāng)前幀激光點(diǎn)云邊線特征點(diǎn)；pj，m——pj在局部地圖中的最近點(diǎn)；Pf——當(dāng)前幀激光點(diǎn)云中所有平面特征點(diǎn)的集合；Ps——當(dāng)前幀激光點(diǎn)云中所有邊線特征點(diǎn)的集合。

2.4 后端優(yōu)化

為減小激光SLAM 前端位姿估計(jì)累計(jì)誤差，SLAM 后端融合GNSS 觀測(cè)約束和前端里程計(jì)約束，構(gòu)建位姿圖優(yōu)化代價(jià)函數(shù)，迭代優(yōu)化以減小前端累計(jì)位姿估計(jì)誤差，提升定位精度。如圖5 所示，變換矩陣TGs為在GNSS-RTK 信號(hào)丟失區(qū)段之前由GNSS-RTK定位模式輸出的東北天坐標(biāo)系下車(chē)輛位姿約束；變換矩陣Ti，i∈{0，1，2，…，k}為GNSS-RTK 信號(hào)丟失區(qū)段內(nèi)待優(yōu)化車(chē)輛位姿；變換矩陣TGe為GNSS-RTK信號(hào)恢復(fù)后由GNSS-RTK定位模式輸出的東北天坐標(biāo)系下車(chē)輛位姿約束；變換矩陣為激光SLAM前端里程計(jì)輸出的相鄰兩幀位姿約束信息。待優(yōu)化位姿目標(biāo)函數(shù)如式（9）所示，其由位姿約束rT0、rTi，Ti+1、rTk組成。

圖5 后端位姿優(yōu)化圖Fig.5 Diagram of pose optimization at rear end

式中：(·)?——李代數(shù)到李群的映射。

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

為了評(píng)估所提出方法的性能，在城市和郊區(qū)道路環(huán)境中進(jìn)行了廣泛的測(cè)試。

3.1 實(shí)驗(yàn)設(shè)備

本系統(tǒng)選擇的LiDAR 型號(hào)是RS-Ruby-80 線激光雷達(dá)，其最大測(cè)距范圍為200 m；所使用的組合慣性導(dǎo)航系統(tǒng)（INS）是華測(cè)CGI-610，其提供（0.1+1×10-6D）m（D為INS 與定位基站的距離，單位為m）的GNSSRTK定位精度；采用SPAN-ISA-100C系統(tǒng)作為評(píng)估本文系統(tǒng)定位性能的基準(zhǔn)參考；車(chē)載計(jì)算機(jī)配備了Intel i7-6820HQ 處理器，主頻2.7 GHz，以及16 GB 的內(nèi)存。此外，所有算法都是使用C++實(shí)現(xiàn)，并在Ubuntu Linux上使用機(jī)器人操作系統(tǒng)（robot operating system，ROS）進(jìn)行執(zhí)行。測(cè)試平臺(tái)車(chē)輛和傳感器的安裝布置如圖6所示。

圖6 自動(dòng)駕駛平臺(tái)車(chē)輛和傳感器安裝Fig.6 Installation of sensors on autonomous driving platform vehicles

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

在城市和郊區(qū)道路環(huán)境下的自動(dòng)駕駛車(chē)輛平臺(tái)上進(jìn)行了同步實(shí)現(xiàn)車(chē)輛定位和路旁桿狀物清單創(chuàng)建實(shí)驗(yàn)，實(shí)驗(yàn)涵蓋了某市中心和郊區(qū)高速公路上的多個(gè)場(chǎng)景。自動(dòng)駕駛汽車(chē)運(yùn)行了2 300 s，行駛距離12.95 km。圖7 顯示了原始點(diǎn)云和桿狀物檢測(cè)的結(jié)果，可見(jiàn)本文系統(tǒng)能夠清晰地提取桿狀物。

圖7 城市道路數(shù)據(jù)集的原始點(diǎn)云和桿狀物提取結(jié)果Fig.7 Extraction results of raw point clouds and pole-shaped objects from urban road dataset

x、y、z方向的軌跡曲線如圖8所示?；疑摼€表示SPAN-ISA-100C 設(shè)備提供的真實(shí)軌跡，藍(lán)色曲線表示算法輸出的關(guān)鍵幀軌跡?？梢钥闯?，軌跡誤差在水平和垂直方向上都相對(duì)較小，并且表現(xiàn)出與真實(shí)情況相似的趨勢(shì)。

圖8 x、y、z 方向上的真實(shí)軌跡和關(guān)鍵幀軌跡Fig.8 Real trajectories and key frame trajectories in the x,y,and z directions

為了定量評(píng)價(jià)算法精度，選擇軌跡的絕對(duì)位姿誤差（APE）作為評(píng)價(jià)指標(biāo)。僅考慮位置誤差，而忽略方向誤差，從而得到以米為單位的 APE 值，結(jié)果如圖9 所示。圖9統(tǒng)計(jì)結(jié)果表明，APE的最大值為0.223 m，最小值為0.001 m，平均值（mean）為0.027 m，均方根誤差（RMSE）為0.035 m 。與高精度、高成本的定位設(shè)備SPAN-ISA-100C 設(shè)備提供的地面真實(shí)軌跡作對(duì)比，該算法的平均誤差小于3 cm。這些定量分析結(jié)果表明，該算法具有較高的軌跡精度。

圖9 APE 統(tǒng)計(jì)結(jié)果Fig.9 APE statistical results

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

為同步實(shí)現(xiàn)車(chē)輛定位和對(duì)道路環(huán)境的感知，本文提出了一種利用LiDAR-IMU-GNSS 融合系統(tǒng)同時(shí)實(shí)現(xiàn)車(chē)輛定位和路旁桿狀物絕對(duì)位置清單創(chuàng)建的方法。其通過(guò)緊耦合LiDAR、IMU和GNSS信息，實(shí)現(xiàn)了在遮擋環(huán)境下準(zhǔn)確的姿態(tài)估計(jì)；基于滑動(dòng)窗口的優(yōu)化融合定位算法，通過(guò)融合多傳感器數(shù)據(jù)提高了系統(tǒng)的魯棒性。此外，本文提出了一種與SLAM共享特征提取的樹(shù)木檢測(cè)方法，減輕了計(jì)算負(fù)載，使得系統(tǒng)能夠?qū)崟r(shí)地進(jìn)行定位建圖和樹(shù)干檢測(cè)。通過(guò)在城市和郊區(qū)的各種道路場(chǎng)景中進(jìn)行廣泛測(cè)試、評(píng)估，本文所提系統(tǒng)展示了厘米級(jí)定位精度，同時(shí)實(shí)現(xiàn)了實(shí)時(shí)自動(dòng)創(chuàng)建路側(cè)桿狀物清單?？傮w來(lái)說(shuō)，本文所提的融合LiDAR-IMU-GNSS系統(tǒng)為同時(shí)實(shí)現(xiàn)車(chē)輛定位和路旁桿狀物清單構(gòu)建提供了一種解決方案，有助于提高駕駛安全性和對(duì)道路環(huán)境的理解。但本文未剔除道路中運(yùn)動(dòng)的車(chē)輛和行人等因素，后續(xù)研究中需要在前端在線過(guò)濾動(dòng)態(tài)物體，以進(jìn)一步提高定位精度。