張曉龍，江 昆，孫 愷，劉茂林，邵振雷，肖鵬川，楊殿閣

（1.清華大學車輛與運載學院，汽車安全與節(jié)能國家重點實驗室，北京100084；2.上海禾賽科技股份有限公司，上海201702）

前言

針對激光測距的理論研究早在20世紀60年代開始，在2000年前后迎來快速發(fā)展，相關(guān)產(chǎn)品在測繪領(lǐng)域得到廣泛應(yīng)用［1］。近年來，激光雷達（light detection and ranging，LiDAR）已成為自動駕駛中重要的車載傳感器。相比于視覺傳感器，激光雷達不受環(huán)境光照變化影響，可以直接獲得周圍道路環(huán)境的高精度三維點云信息，因此在高精度地圖構(gòu)建、環(huán)境感知和定位等任務(wù)中得到廣泛應(yīng)用［2］。根據(jù)光束掃描實現(xiàn)方式的不同，激光雷達可分為機械式激光雷達（mechanical LiDAR）和固態(tài)激光雷達（solid?state LiDAR）兩大類［3］。

激光雷達通過測量激光的飛行時間（time of flight，ToF）獲得被測物的距離ρ；通過旋轉(zhuǎn)編碼器（rotary encoder）確定激光的方位角（azimuth angle）φ，從而獲得球坐標系下的測量量M=[ρ φ]，如圖1所示。激光的俯仰角（elevation angle）θ通過激光發(fā)射器的安裝角度確定，屬于激光雷達內(nèi)部參數(shù)（intrinsic，后文簡稱內(nèi)參）的一部分，將在后文詳述。之后通過內(nèi)參模型進行解算，獲得被測物在激光雷達中心空間直角坐標系下的點坐標PL=[x y z]T。

圖1 球坐標系示意圖

正確的激光雷達內(nèi)參建模具有重要意義：首先，這保證了獲得的測量點坐標能夠準確反映實際物理環(huán)境；其次，這也為后續(xù)利用激光雷達進行定位和建圖提供了準確的點云數(shù)據(jù)。在實踐中發(fā)現(xiàn)，不準確的激光雷達內(nèi)參建模和標定會導致獲得的點云平面邊緣不平整［4］、平面厚度增加［5-6］。

目前，針對激光雷達內(nèi)參的建模主要針對于機械式激光雷達，通常依賴于機械式激光雷達的物理結(jié)構(gòu)以及工作過程［7］。一類內(nèi)參模型認為距離測量量ρ和方位角測量量φ是相對于每個激光發(fā)射器獲得的，對于每條激光線束都定義了多個參數(shù)，通過解算測量量M=[ρ φ]獲得點云坐標，如式（1）所示：

式中：s為尺度因子，用于修正與距離相關(guān)的測距誤差［8］；δρ為對于距離測量的修正量，δθ為俯仰角（可理解為相對于水平面的修正量），δφ為對于方位角的修正量；h和v分別為激光發(fā)射器相對于激光雷達中心在水平和垂直方向上的偏移量。

這樣的內(nèi)參模型將所有參數(shù)都定義在激光線束上，對于每條激光線束使用6個參數(shù)[δρ δθ δφs h v]進行描述，在使用前須對這些參數(shù)進行標定。模型參數(shù)較多，在實際應(yīng)用中為內(nèi)參標定帶來了不便。

上述內(nèi)參模型得到了廣泛應(yīng)用，但在實際使用中，也常常舍去部分模型參數(shù)，使得上述模型發(fā)生退化。根據(jù)模型中6個參數(shù)的有無，對于不同文獻中的內(nèi)參建模方法進行整理匯總，結(jié)果如表1所示。

表1 內(nèi)參建模方法整理

Pandar XT機械式激光雷達由于收發(fā)時所有激光線束均通過相同的發(fā)射透鏡和接收透鏡中心，具有結(jié)構(gòu)和光路上的對稱性，因此可以帶來內(nèi)參建模和標定上的簡化。本文提出一種針對該型機械式激光雷達內(nèi)參建模和標定方法；并考慮其偏心結(jié)構(gòu)對于點云的影響，提出角度修正和距離修正兩種方法對于獲得點云進行修正；通過仿真模擬兩種模型的修正效果，并通過實際點云進行驗證；最后在位姿估計算法中驗證修正算法的有效性。

1 內(nèi)參建模

由于不同型號機械式激光雷達間存在結(jié)構(gòu)差異，本章從Pandar XT激光雷達的機械結(jié)構(gòu)與工作原理出發(fā)，建立內(nèi)參模型，并給出各個參數(shù)的定義和標定方式，進而提出兩種點云修正方法，后文將沿用本章中參數(shù)的符號和定義。

1.1 機械結(jié)構(gòu)與工作原理

Pandar XT機械式激光雷達機械結(jié)構(gòu)如圖2所示。

圖2 Pandar XT機械式激光雷達結(jié)構(gòu)示意圖

圖中：O為激光雷達坐標系原點，S為發(fā)射透鏡光心，二者等高共面，之間有一恒定偏移量；T為測量點位置；Laser為激光發(fā)射器；APD為雪崩光電二極管（avalanche photon diode，APD）。

每條激光線束從激光發(fā)射器發(fā)出后經(jīng)過光路折疊反射鏡兩次反射，從發(fā)射透鏡光心S發(fā)出；在目標表面發(fā)生漫反射后經(jīng)過接收透鏡返回激光雷達，再經(jīng)過光路折疊反射鏡兩次反射，在雪崩光電二極管處進行光電信號轉(zhuǎn)換，完成飛行時間測量，進而獲得距離測量值d；通過旋轉(zhuǎn)編碼器獲得激光雷達的方位角ω。之后通過內(nèi)參模型對于獲得的測量量M=[dω]進行解算，獲得激光雷達坐標系下的點坐標PL，該過程可以用一個抽象函數(shù)f(?)表示：

激光雷達坐標系為一右手系，采用“左-后-上”方式定義；方位角ω相對于y軸正方向定義，俯視圖中順時針方向為正，如圖3所示。

圖3 激光雷達坐標系及方位角示意圖

1.2 內(nèi)參模型與參數(shù)標定

由于所有激光線束在收發(fā)時均通過相同的發(fā)射透鏡和接收透鏡中心，該型機械式激光雷達具有結(jié)構(gòu)和光路上的對稱性，因此可以帶來內(nèi)參建模和標定上的簡化。定義如下模型參數(shù)：

（1）發(fā)射透鏡光心S相對于激光雷達中心O的偏移量b和h，如圖2所示；

（2）發(fā)射透鏡光心S處球坐標系下每條激光線束的方位角修正量α和俯仰角β。

可以看出，第1項與激光雷達自身結(jié)構(gòu)相關(guān)，對于每條激光線束只須標定第2項中的兩個參數(shù)。通過引入激光雷達結(jié)構(gòu)參數(shù)減少了需要標定的參數(shù)量，簡化了標定流程。在標定過程中，偏移量b和h通過激光雷達CAD模型獲得；方位角修正量α和俯仰角β相對于發(fā)射透鏡光心S標定，距離測量值d相對于激光雷達中心O標定。

在激光雷達驅(qū)動中，目前直接基于球坐標系下的激光發(fā)射角以及距離測量來解算點的坐標，如式（3）所示。

但是，由于激光雷達中心O和發(fā)射透鏡光心S在空間上不重合，獲得的距離測量d=|OT|≠|(zhì)ST|，如圖4所示。因此在采用上述方法進行坐標解算時會帶來誤差，造成獲得的點云存在畸變，需要對于點云進行修正。

圖4 測量示意圖

1.3 修正方法建模

考慮通過兩種方法進行修正：一方面，考慮將發(fā)射透鏡光心S下的方位角修正量α和俯仰角β修正到激光雷達中心O下的方位角修正量α′和俯仰角β′，利用在該處標定的距離測量值d解算坐標，該方法稱為角度修正；另一方面，考慮將激光雷達中心O下的距離測量值d修正到發(fā)射透鏡光心S下的距離測量值d′，利用在該處標定的方位角修正量α和俯仰角β解算坐標，該方法稱為距離修正。

1.3.1 角度修正模型

首先對方位角修正量α進行修正，如圖5所示。

圖5 方位角修正示意圖

圖中T′為測量點T在水平面上的投影。

在俯視圖中ΔSOT′應(yīng)用正弦定理，有

可得：

由于β′≈β，進行如下近似：

之后對俯仰角β進行修正，如圖6所示。

圖6 俯仰角修正示意圖

圖中O′為將激光雷達中心O向平面STT′投影獲得的垂足。依據(jù)俯仰角定義，有∠TST′=β，近似∠TO′T′≈β′。

在ΔSO′T中應(yīng)用正弦定理，有

由 于x=d?cosβ′?sinα′≈d?cosβ?sinα，進行如下近似：

通過式（5）和式（6）可以獲得激光雷達中心O下的方位角修正量α′和俯仰角β′：

可得角度修正模型為

1.3.2 距離修正模型

在任何旋轉(zhuǎn)角度下的激光雷達坐標系中，T點坐標均有如下兩種表達方式：

其中：d=|OT|；d′=|ST|

由于|OT|2=x2+y2+z2，聯(lián)立可得關(guān)于d′的一元二次方程：d′為該方程的一個實根，令B=cosβ?(h?cosα-b?sinα)>0，可得

可得距離修正模型為

2 激光雷達位姿估計算法

考慮到上述修正模型的效果難以通過點云直接進行定量驗證和分析，因此考慮定量評估修正前后點云對于激光雷達位姿估計算法的影響，進而驗證本文提出方法的有效性。

2.1 即時定位與地圖構(gòu)建技術(shù)簡介

即時定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）技術(shù)在高精度地圖構(gòu)建過程中得到廣泛應(yīng)用［17-18］。一個經(jīng)典的SLAM系統(tǒng)主要由前端、回環(huán)檢測、后端構(gòu)成［19］，其系統(tǒng)框圖如圖7所示。

圖7 SLAM算法框圖

圖中，前端負責接收并預處理來自傳感器的數(shù)據(jù)，并進行特征提取和數(shù)據(jù)關(guān)聯(lián)，估計幀間相對位姿，但是這一步獲得的估計結(jié)果存在噪聲，經(jīng)過累加會造成軌跡漂移，進而造成地圖的結(jié)構(gòu)性錯誤?；丨h(huán)檢測負責計算當前場景與歷史場景的相似度，進而判斷自身是否返回到之前經(jīng)過的地點，為后端提供新的約束，進而修正地圖的結(jié)構(gòu)性錯誤。后端負責接收位姿估計結(jié)果，并對位姿進行最大后驗概率（maximum a posteriori，MAP）估計，在高斯噪聲假設(shè)下該問題可以轉(zhuǎn)化為最小二乘問題，通過非線性優(yōu)化的方式進行求解［20］。建圖結(jié)果分為度量地圖（metric map）和拓撲地圖（topologi?cal map），其中度量地圖又可以分為稀疏地圖和稠密地圖［19］。

2.2 LeGO?LOAM位姿估計算法

2014年，Zhang等人提出激光里程計和建圖（LiDAR odometry and mapping，LOAM）算法［21-22］，將激光SLAM問題分解為高頻低精度的前端激光里程計和低頻高精度的后端地圖構(gòu)建，較好地兼顧了算法的實時性和精度。LOAM算法通過定義的點的曲率值大小將點云中的點分為邊緣點和平面點兩類，如式（15）所示。

2018年，在LOAM算法的基礎(chǔ)上，Shan等人提出LeGO?LOAM算法［23］，算法框圖如圖8所示。

圖8 LeGO?LOAM算法框圖

LeGO?LOAM算法在前端添加了分割聚類模塊，用于提取點云中的地面點，并對非地面點進行聚類，提高了處理速度和特征提取精度，同時增強了對于非結(jié)構(gòu)化環(huán)境的魯棒性。沿用了LOAM算法的特征提取和誤差函數(shù)定義方式，在幀間匹配過程中引入地面分割結(jié)果，提高了特征匹配的效率和準確性，采用雙步列文伯格?馬夸爾特（Levenberg?Marquardt，L?M）算法估計6自由度位姿。在后端地圖構(gòu)建模塊中采用因子圖優(yōu)化位姿估計結(jié)果。

2.3 點云誤差對于SLAM算法的影響分析

在實踐中發(fā)現(xiàn)，LeGO?LOAM算法存在較為嚴重的垂向位姿估計漂移。通過上述算法框圖可以看出，垂向位姿是通過將當前時刻的地面平面點與上一時刻的所有平面點進行匹配獲得的。匹配過程中的對應(yīng)關(guān)系通過如圖9中所示的方式確定。

圖9 平面點匹配示意圖

對于當前幀中的一個地面平面點，在上一幀中尋找同一線束上與之距離最近的兩個平面點以及相鄰線束上與之距離最近的一個平面點。這3個平面點可以構(gòu)成一個局部小平面，由此確定點到面的對應(yīng)關(guān)系。通過上述方式確定當前幀中所有地面平面點的對應(yīng)關(guān)系，將點到面距離之和作為代價函數(shù)進行優(yōu)化，進而獲得位姿估計結(jié)果。

通過定性分析可知，在點云坐標存在誤差的情況下，對應(yīng)局部小平面的位置和法線方向?qū)l(fā)生變化，如圖10所示，會影響代價函數(shù)計算，進而影響位姿估計結(jié)果。因此，對于原始點云進行修正，減少點云坐標誤差，提高地面點云質(zhì)量，對于LeGO?LOAM算法垂向位姿估計精度有改善作用。

圖10 畸變點云影響示意圖

3 仿真驗證

搭建仿真模型對上述修正模型進行驗證，使用Pandar XT機械式激光雷達內(nèi)參進行仿真，其中h=30.94 mm、b=13 mm；在32條線束中等間隔地選取線束，獲得其中8條線束參數(shù)，如表2所示。

表2 線束參數(shù)選擇

表中俯仰角β向上為正，方位角修正量α正方向與方位角ω相同。

對于每條線束生成0.5~50 m的距離測量值d，按照角度修正和距離修正兩種方式對測量點坐標進行修正，對修正前后結(jié)果進行比較。

3.1 角度修正模型

采用角度修正模型對方位角修正量α和俯仰角β進行修正，效果如圖11和圖12所示。

圖11 方位角修正量變化Δα

圖12 俯仰角變化Δβ

可以看出：隨著距離增大，對于方位角修正量α的修正效果快速減小，在20 m外修正效果變化不大；從修正效果上來看，不同線束間的修正量大小基本相同。

可以看出：隨著距離增大，對于俯仰角β的修正效果快速減小，在20 m外修正效果變化不大；從修正效果上來看，對于大俯仰角線束的修正效果更加明顯，且修正后點云具有整體向xOy平面靠近的趨勢。

3.2 距離修正模型

采用距離修正模型對于距離測量值d進行修正，效果如圖13所示。

圖13 距離測量值變化Δd

可以看出：隨著距離增大，對于距離測量值d的修正效果快速減小，在5 m外修正效果變化不大；從修正效果上來看，對于大俯仰角線束的修正效果更加明顯，且修正后點云具有整體向原點靠近的趨勢。

4 實驗

實驗中使用的原始點云均通過式（3）中的方法解算獲得，未經(jīng)修正。

4.1 單幀點云修正效果

在標定間中放置多個平面標定板，使用激光雷達采集單幀點云數(shù)據(jù)，手動選出屬于同一標定板的點云，如圖14所示。

圖14 標定板點云

分別對于原始標定板點云進行角度修正和距離修正，利用M-估計量樣本一致性（M?estimator sample consensus，MSAC）算法［24］擬合各個標定板的平面參數(shù)，之后分別計算3種條件下各個平面上的點到擬合平面的平均距離誤差，將其作為平面質(zhì)量的評價指標；并計算激光雷達到標定板的距離，結(jié)果如表3所示。

表3 標定板平面誤差統(tǒng)計結(jié)果

可以看出，在使用修正方法后，點云中平面點的質(zhì)量較修正前得到了提高。

4.2 對激光里程計的影響分析

利用采集車采集3段不同場景中的點云序列，利用GPS/IMU設(shè)備獲得車輛位姿并轉(zhuǎn)換到激光雷達坐標系下作為位姿真值，其基本信息如表4所示。

表4 點云序列基本信息

由于實際采集點云的xy坐標存在較大運動畸變，考慮僅對于點云的z坐標進行修正。使用原始點云、角度修正點云和距離修正點云運行LeGO?LOAM算法，并定量評估激光里程計的精度，結(jié)果如圖15所示。

圖15 里程計和建圖結(jié)果

在評價指標上，選擇絕對位姿誤差（absolute pose error，APE）和相對位姿誤差（relative pose er?ror，RPE），計算公式為

按照式（16）計算絕對位姿誤差，結(jié)果如表5所示。

表5 絕對位姿誤差統(tǒng)計結(jié)果

可以看出，使用修正后的點云獲得的絕對位姿誤差的平移和旋轉(zhuǎn)部分均小于原始點云組，說明對于原始點云進行修正可以提高位姿估計精度。

絕對位姿誤差反映了單個位姿估計結(jié)果與位姿真值之間的差距，受到位姿估計結(jié)果漂移的影響較大；相對位姿誤差反映了一段時間或路程下的位姿估計增量與位姿真值增量之間的差距，用于評估里程計精度更加合適。對于相對位姿誤差，采用KITTI數(shù)據(jù)集的方式進行處理［25］，選擇軌跡中所有長度為100，200，…，600 m的位姿片段按照式（17）分別計算原始點云、角度修正和距離修正條件下在不同距離上的相對位姿誤差，這樣做是為了通過長距離來消除GPS/IMU設(shè)備在單一位置處定位誤差對于里程計精度評估結(jié)果的影響。相對位姿誤差結(jié)果整理如圖16所示，將平移部分誤差和旋轉(zhuǎn)部分誤差分別列出，其中左側(cè)縱坐標對應(yīng)的條形圖表示原始點云結(jié)果，右側(cè)縱坐標對應(yīng)的折線圖表示角度修正和距離修正后結(jié)果的變化情況。

圖16 相對位姿誤差

之后，將每個相對位姿誤差相對于片段長度歸一化后再取平均，獲得里程計的平移部分誤差以及旋轉(zhuǎn)部分誤差，計算公式為

式中：Etrans代表平移部分誤差，%；Erot代表旋轉(zhuǎn)部分誤差，（°）/m。

按照式（18）和式（19）計算出里程計的平移部分誤差和旋轉(zhuǎn)部分誤差，結(jié)果如表6所示。

表6 里程計誤差統(tǒng)計結(jié)果

可以看出，使用修正后的點云獲得的平移和旋轉(zhuǎn)誤差均小于原始點云組，說明對于原始點云進行修正對于減小激光里程計誤差有改善作用。

最后，考慮到修正算法需要對點云中每個點進行處理，會帶來計算資源的額外消耗和計算時間的增加，因此分析修正算法對實時性的影響。實驗中點云修正發(fā)生在LeGO?LOAM算法的特征提取模塊中，因此分別統(tǒng)計該模塊在使用原始點云、角度修正和距離修正方式下的單幀計算時間，結(jié)果如圖17所示。

圖17 單幀計算時間統(tǒng)計

分別計算3種條件下單幀計算時間的最小值、最大值和平均值，結(jié)果如表7所示。

表7 修正算法對于實時性的影響

可以看出：使用角度修正算法的平均單幀計算時間最長，距離修正算法次之；但是從平均值來看兩種修正算法導致單幀處理時間的增加均小于0.01 s，因此可以認為修正算法對于實時性的影響不大。

5 結(jié)論

本文中根據(jù)Pandar XT機械式激光雷達的結(jié)構(gòu)和工作原理，提出一種內(nèi)參建模方法，通過引入激光雷達結(jié)構(gòu)參數(shù)減少了模型參數(shù)量?？紤]激光雷達偏心結(jié)構(gòu)對點云的影響，本文中提出角度修正和距離修正兩種點云修正方法。通過仿真模擬兩種方法的修正效果，并通過實際采集點云進行驗證。最后通過LeGO-LOAM位姿估計算法進行驗證，證明使用修正后的點云對于減小位姿估計誤差和里程計誤差有改善作用。

從實際應(yīng)用角度，部分激光SLAM算法需要激光雷達的俯仰角β作為算法參數(shù)輸入。因此，為方便使用，應(yīng)盡量采用距離修正方式以保證出廠前標定的俯仰角β不變。對于大尺寸激光雷達（b和h數(shù)值較大）、大俯仰角（β數(shù)值較大）、近距離測距（例如激光雷達斜裝在車頂）的情況下，偏心結(jié)構(gòu)帶來的影響愈加不能忽視，修正模型帶來的影響將更加顯著。

后續(xù)工作中將考慮將該內(nèi)參模型推廣到具有類似結(jié)構(gòu)的機械式激光雷達型號中，并對于其他來源的誤差進行建模；并設(shè)計針對內(nèi)參的自動化標定方法。