史鵬程，葉 勤，張紹明，鄧海峰

1. 自然資源部城市國土資源監(jiān)測與仿真重點實驗室，廣東 深圳 518034； 2. 同濟大學測繪與地理信息學院，上海 200092； 3. 武漢大學計算機學院，湖北 武漢 430072； 4. 上海華測導航技術股份有限公司，上海 201702

伴隨人工智能技術快速發(fā)展，自動駕駛不僅改變了人們的出行方式，在物流、餐飲、醫(yī)療等領域發(fā)揮著重要作用，對于推動智慧化城市進程具有重要意義[1-3]。目前自動駕駛分為L0—L5共6個級別，L3級以上才能實現(xiàn)無須人工干預的無人駕駛。L1—L4可在限定條件下運行，無人駕駛場景分為：包括礦山、機場、港口及園區(qū)等低速封閉場景，包括高速公路等高速半封閉場景，以及包括城市中道路等的高速開放場景。無人駕駛對于定位暫無具體嚴格要求，但目前各個研究機構都在致力于進行高精度定位欲實現(xiàn)厘米級定位效果。

同時定位與地圖建圖(simultaneous localization and mapping，SLAM)[4]是自動駕駛與機器人導航定位的關鍵技術。目前根據(jù)傳感器類型主要分為視覺和激光兩大類。視覺SLAM以ORB-SLAM[5]和LSD-SLAM[6]為代表，但是這類方法受限于視覺傳感器的量測范圍，不適用于本文中的大尺度場景。激光SLAM以LOAM[7]和LeGO-LOAM[8]為代表，量測范圍廣、精度高，抗干擾能力強，是目前較為穩(wěn)定和有效的自動駕駛方案。定位初始化是機器人導航定位SLAM中的重要環(huán)節(jié)，即當傳感器初始位于某場景中，需要通過定位初始化確定其在當前場景下的初始位置，為后續(xù)導航定位提供初始信息，保證平臺定位工作正常進行[9]。在機器人的連續(xù)定位過程中，若由于信號遮擋、傳感器故障、目標特征缺失等某些不確定性原因造成機器人定位失效，即機器人的追蹤丟失，可再次啟動定位初始化來恢復機器人的實時位姿，即機器人的綁架恢復[10]。

在室內環(huán)境中，基于GNSS信號[11]、第3方開源地圖[12]等輔助信號源定位方案效果不佳，全局初始定位的挑戰(zhàn)性更大。為簡化定位算法，一些方法采用視覺標志[13-14]、QR碼[15]、Wi-Fi信號[16]、天花板[17]等人工輔助外源信號方案來進行定位，但是視覺標志方案易受到場景中移動目標遮擋影響，Wi-Fi定位通常無法解決朝向等問題。一些方法根據(jù)場景中存在的部分幾何特征如線特征[18-19]、角點[20-21]、平面[22-23]、停車場特殊結構[24]等實現(xiàn)定位，但這些方法當場景紋理特征較弱時定位效果欠佳。一些方法利用特殊環(huán)境條件如磁場[25]、門牌號[26]等實現(xiàn)定位，但是對于場景有著嚴格要求，方法適用性有待提高。一些研究[27-28]中采用蒙特卡洛結合神經(jīng)網(wǎng)絡的方法可在全局范圍內搜索出最優(yōu)全局位姿，但在這種方法中一定程度上受限于模型訓練效果，并需要數(shù)據(jù)清洗工作，前期工作量較大。部分多傳感器融合方案如相機與LiDAR[29]、IMU與LiDAR[30]等，雖然能增加數(shù)據(jù)豐富度，但多源數(shù)據(jù)的融合增加了算法挑戰(zhàn)性。

綜上關于定位初始化的相關研究，仍存在部分待優(yōu)化問題：①對于大尺度室內場景的定位初始化應用研究相對較少，其效率與準確度有待提高；②在GNSS信號不佳的室內場景下，利用人工標志的方法，需要提前布設，同時易在遮擋時失效，不依賴于外源信息的定位方案有待研究；③基于特征的定位初始化方法依賴于特征提取效果，穩(wěn)健的特征提取方法有待研究。大尺度室內環(huán)境更有利于機器人導航及自動駕駛產(chǎn)業(yè)落地推廣，但此類場景占地面積大、地物特征信息較少，光照條件不佳等問題對定位算法挑戰(zhàn)性較大。為此，本文提出一種在大尺度室內場景下基于特征模式的機器人定位初始化方法。通過對室內場景特征分析，探索場景中一些穩(wěn)定的人工構筑物結構及結構組合，并將其定義為特征模式，提高特征提取準確度，進一步改善室內機器人全局定位初始化性能。

1 特征模式提取

圖1為本文室內場景下基于特征模式的定位初始化流程。

圖1 定位初始化流程Fig.1 Flow chart of localization initialization

1.1 特征模式類別

室內場景中存在的一些穩(wěn)定的人工構筑物如墻壁、柱體等，這些構筑物位置發(fā)生變化的概率極低，同時作為場景中的顯著標志，可標示場景的總體布局。將這些穩(wěn)健結構以及結構組合定義為特征模式?；谖墨I[31]的研究，本文增加了對稱結構模式。如圖2為定義的(a)—(e) 5類特征模式。根據(jù)結構的幾何顯著性將特征模式分為3個級別，即圖2(a)為1級結構，圖2(b)—圖2(d)為2級結構，圖2(e)為3級結構。圖2中第1行為實際場景結構，第2行為結構示意圖，第3行為簡化后的特征模式表達形式。

圖2 特征模式Fig.2 Feature pattern

1.2 實時幀特征模式提取

多線激光雷達是自動駕駛中常用的傳感器，具有距離分辨率高、抗干擾能力強等優(yōu)點。其數(shù)據(jù)具有如下特征：①組織形式，一系列具有三維坐標信息的點；②幾何形態(tài)，呈現(xiàn)多條線狀分布；③空間分布，不規(guī)則離散分布，不同掃描線點云之間距離差不同；④空間視場角，水平視角為360°，豎直方向存在一定視角限制。

1.2.1 預處理

在獲取實時點云S后，需對其預處理為后續(xù)特征提取做準備。主要包括以下步驟。

(1) 直通濾波：通過兩個高度閾值(H1,H2)濾除部分干擾點云，得到濾波點云SF。

(1)

(2)

利用奇異值分解依次得出點云3個正交方向向量(k,l,m)，令k、l所成平面與XOY面對齊，實現(xiàn)校正。

(3) 點云投影：將校正后點云正射投影至XOY平面，得到投影點云SJ。

1.2.2 一級結構提取

室內場景中最為顯著的結構為多段墻體結構?；诿烙^和布局考慮，室內墻體通常會在方向上保持一致性。因此，采用一種循環(huán)分割的策略來提取場景中線狀分布的分段墻體結構。主要包括以下步驟。

(1) 直線分割。如式(3)所示

(3)

(4)

(5)

式中，i為當前分割索引，se為1時停止分割，為0時繼續(xù)分割；Nlp為百分比閾值，ci為第i次分割累積分割次數(shù)，Cm為分割次數(shù)閾值。

(6)

(7)

式中，ec為點云聚類的有效性；Nlc為點云數(shù)量閾值；l(·)為計算點云長度，采用最小外接立方體的對角線長度近似，Llc為長度閾值。

圖3 截斷誤差Fig.3 Truncation error

圖4 線段擬合Fig.4 Line fitting

(4) 干擾線段剔除。如圖4(b)在直線分割時，由于參數(shù)設置等問題，某點云線段可能會被分割為雙線段結構(L1,L2)。這些雙線段結具有如下特征：①距離較近;②方向近似平行；③彼此重疊度較高?；谝陨咸卣鳎瑢ふ页鲭p線段結構，并保留其中較長線段得到L3。

1.2.3 二級結構提取

二級結構中包含平行、對稱和垂直結構(圖2(b)—圖2(d))。相比于單線段結構，二級結構(線段對)在特征描述時更加穩(wěn)健，具有更多屬性，在后續(xù)匹配階段效率更高。其提取主要包括以下步驟。

(1) 確定線段對。單線段結構兩兩組合為線段對集合Lp={(Li,Lj)i≠j,i,j∈nL}，nL為L數(shù)量。

(2) 夾角計算。設定平行和垂直余弦閾值Cp、Cv篩選候選平行和垂直線段對Lpp、Lpv。

(3) 距離計算。式(8)為計算近似平行線段對的距離；式(9)為根據(jù)線段距離對平行線段對分類

(8)

(9)

(4) 重疊度計算。如圖5(a)在線段對中計算兩線段重疊率o12o21,圖5(b)為無重疊線段對距離dn計算方式。

圖5 重疊度計算Fig.5 The calculation of overlapping degree

在式(10)中對候選平行和對稱結構篩選得到AL和PL

(10)

(5) 垂足計算。根據(jù)式(11)計算兩線段的垂足點(x,y)

(11)

式中a1、b1、c1、a2、b2、c2為線段的直線參數(shù)。

1.2.4 3級結構提取

3級結構中定義了圓弧結構(圖2(e))。AS對應的是場景中圓柱等柱面結構。其提取采用一種結合量化分析的序列化提取方法，主要包含以下4個步驟。

(12)

式中，Ix(·)索引函數(shù)，可保留三維點索引。

(4) 弧弦分析。如圖6在圓弧點云中尋找3點關鍵幾何點q1q2q3，其中S為激光雷達，q2為圓弧中點，是當前圓弧點云中距離S最近的點。q2q3為兩圓弧端點，是圓弧上q2兩側距S最遠的點，q1q2q3近似構成一個等腰三角形即式(13)中弧弦約束，在幾何形態(tài)上不滿足該弧弦條件的圓弧結構將會被剔除掉，最后得到AS。

圖6 弧弦分析Fig.6 Chord analysis

(13)

式中，d(A,B)表示AB的距離；a(A,B,C)表示ABC所形成的角度；Dch是距離誤差閾值；Ach是角度誤差閾值。

1.3 點云地圖特征模式提取

相比于單幀激光雷達數(shù)據(jù)，地圖點云數(shù)據(jù)較為稠密，在實時數(shù)據(jù)中適用的特征模式提取方法不適用于點云地圖。因此為提高點云地圖M上特征模式提取準確度，提出一種半自動化方法提取地圖特征模式。點云地圖特征模式提取流程如下(圖7)。

(1) 地圖拼接。采用激光SLAM算法LOAM計算LiDAR數(shù)據(jù)幀間相對位姿，通過選取關鍵幀Sk的方式進行地圖數(shù)據(jù)拼接，完成了地圖點云的初步拼接。

圖7 地圖數(shù)據(jù)處理流程Fig.7 Map data processing

(2) 地圖預處理。采用與文獻[32]相同的預處理方法，即分別使用離群點去除，統(tǒng)計濾波等濾除部分噪聲點云，并用直通濾波濾除部分干擾結構。

(3) 第1次地圖裁剪。采用CloudCompare軟件的編輯裁剪功能，手動勾選部分拼接偏差點，通過目視檢查場景結構的整體精度控制地圖數(shù)據(jù)修剪過程。

(4) 地圖校正投影。采用1.2.1節(jié)中點云校正的方法對修剪后的地圖數(shù)據(jù)進行校正， 并投影到XOY平面內，得到投影點云MJ。

(5) 第2次地圖裁剪。手動修剪MJ，裁剪出每一特征模式對應點云聚類。

(6) 特征模式提取。根據(jù)模式類別采用Ransac在對應點云聚類中擬合特征模式。

2 地圖數(shù)據(jù)管理

為提高定位初始化的效率，本文提出一種高效的地圖數(shù)據(jù)管理方案。以文本的方式對地圖相關數(shù)進行存儲與管理，同一場景下只需完一次地圖數(shù)據(jù)處理并離線保存，避免了重復煩瑣的地圖數(shù)據(jù)操作。僅在場景發(fā)生變化更新時，重復一次地圖數(shù)據(jù)處理流程，更新地圖數(shù)據(jù)即可。如圖8為地圖數(shù)據(jù)存儲格式，其中主要包括以下兩方面。

(1) 地圖特征模式：根據(jù)特征模式初始化方式，對各類特征模式單獨采用離線文本方式進行存儲。其存儲格式如圖8中L、AL、PL、VL、AS等特征模式數(shù)據(jù)存儲格式。

圖8 地圖數(shù)據(jù)管理格式Fig.8 Management format of map data

(2) 地圖格網(wǎng)：根據(jù)格網(wǎng)分辨率r和投影點云MJ生成二值化格網(wǎng)地圖(圖9)。如圖8中MG行記錄格網(wǎng)非0元素位置，1為行號，從2開始依次為列號。

圖9 地圖網(wǎng)格Fig.9 Map grid

3 位姿求解

由于室內場景地面起伏波動較小，6自由度(degree of freedom，DOF)的位姿可退化為3自由度(tx、ty、θ)即可完成室內的定位與建圖任務。其中tx和ty分別為X軸和Y軸方向平移量，θ則為繞著Z軸旋轉角度量。通過構建對應各特征模式的誤差方程組合及匹配與配準策略，實現(xiàn)定位初始化，主要包含以下步驟。

(2) 誤差方程構建。采用兩類誤差方程，即點到點距離與點到線段距離。在各類特征模中，構建適應其幾何屬性的多個誤差方程組合。

(3) 位姿參數(shù)求解。主要包括以下步驟：①位姿參數(shù)初始化為0；②采用L-M梯度下降方法對誤差方程進行優(yōu)化，推導最終得到關于位姿增量x的線性方程如式(14)所示

(H+λDTD)Δx=g

(14)

式中，H=J(x)TJ(x)，g=-J(x)Tf(x)，f(x)為誤差方程，D取對角矩陣即JTJ的對角元素平方根。采用QR分解計算出Δx的值；③計算ρ

(15)

若ρ>0.75，令μ=2μ,若ρ<0.25，令μ=0.5μ，其中μ為優(yōu)化半徑，若ρ大于0.5，則xk+1=xk+Δxk；④若Δxk足夠小，則停止，否則返回步驟②。

⑤解算出當前匹配元素所確定的位姿Ti=(txi,tyi,θi)。若當前匹配元素存在角度不確定時，按照式(16)更新θ角度作為初始值

(16)

再次進行求解，得到Ti+1=(txi+1,tyi+1,θi+1)，計算出當前特征模式匹配對所得到的所有位姿集合{Ti}k，k為當前特征模式匹配對索引。

(17)

(18)

(5) 遍歷所有候選特征模式匹配對，重復步驟(1)—步驟(4)，得到匹配位姿集合序列，{Ti}0,{Ti}1…{Ti}m。在所有初始化成功的匹配位姿中，選出得分最高者作為最終的初始化匹配位姿。

4 試 驗

4.1 試驗數(shù)據(jù)

圖10(a)—圖10(c)選取走廊、大廳、地下停車場3種代表性的大尺度室內結構化場景進行試驗。圖10(a)為室內走廊，占地約750 m2，包含多段墻壁、柱子、和圍欄扶手等。圖10(b)是室內大廳，占地約800 m2，場景中包含多個圓形柱子、展臺桌、墻壁、盆栽和長樓梯等，整個場景相對空曠。作為挑戰(zhàn)性之一，以上兩個場景都存在部分玻璃結構，對數(shù)據(jù)獲取存在一定影響。圖10(c)為地下停車場，占地約3000 m2，除包含多段墻壁、柱子、警示標志，還包括眾多車輛(干擾)等。停車場挑戰(zhàn)性相對較大，光照條件不佳同時場景中存在大量相似的車體等結構，對于墻壁等穩(wěn)健結構的提取存在一定的干擾。

試驗采用RoboSense16線激光雷達如圖10(d)所示。將激光雷達一段時間內采集的點云數(shù)據(jù)保存，并進行離線全局定位初始化試驗。試驗中建圖與定位使用同一組數(shù)據(jù)，但由于地圖數(shù)據(jù)在拼接過程中采用關鍵幀策略，同時地圖數(shù)據(jù)在處理過程中通過采樣、濾波、裁剪等操作，完全不同于實時幀數(shù)據(jù)處理流程，因此可使用同組數(shù)據(jù)在全局范圍內測試本文定位方法。試驗中采用激光SLAM算法LOAM計算匹配位姿作為相對真值。LOAM具有0.55%的平移精度，0.001 3 deg/m的角度精度，是目前KITTI數(shù)據(jù)中排名前三的SLAM算法。當計算位姿與相對真值誤差在2 m認為初始化成功，因為在大尺度室內場景中2 m左右初始位置誤差可在后續(xù)定位與建圖過程中逐步降低，不影響整體自動駕駛性能。試驗環(huán)境為Intel Core i7-5500U CPU @2.40 GHz 2.39 GHz和8.0 GB of RAM。

4.2 試驗結果

由于單線段結構L匹配效率較低，實際場景應用性不強，因此，試驗中未統(tǒng)計基于L結構的初始化結果。圖11在3個場景中分別選取某幀數(shù)據(jù)進行初始化效果顯示，圖11(a)為地圖中提取的特征模式，并用不同顏色表示。圖11(b)為定位初始化匹配效果的側視圖。黃色為地圖點云，紅色為初始定位前單幀激光雷達點云，綠色為初始定位后，利用匹配位姿將單幀點云轉入地圖坐標下的點云。圖11為了提高顯示效果，對地圖數(shù)據(jù)進行降采樣顯示。在初始定位后，單幀點云與地圖點云目視匹配效果較好，可有效確定傳感器在全局地圖中的初始位姿。

圖10 試驗場景與設備Fig.10 Experimental scene and equipment

圖11 定位初始化效果Fig.11 The effect of localization initialization

在3個場景中各取3000、548、1015幀數(shù)據(jù)進行全局定位初始化測試，見表1。并統(tǒng)計了包括地圖模式數(shù)、定位初始化準確率、召回率、一次成功初始化時間、定位誤差均值等各指標。由于文中初始定位位姿為3自由度位姿，統(tǒng)計的初始定位誤差為水平定位精度。

(1) 場景1(走廊)。走廊場景中，相互垂直的墻壁、門窗、柱子等所形成的對稱、圓弧模式較為顯著。地圖特征模式數(shù)量分別為：PL-41、VL-53、AL-14、AS-8。在定位準確性方面，VL準確率達到94%以上，AS和AL分別達到76%和69%左右，PL相對較低。各類特征模式在完成一次成功初始化時所需時間分別為245.419 ms、457.3 ms、177.796 ms、870.941 ms，其中PL的定位誤差相對較大達到0.28 m，其他特征模式的定位誤差均在0.03 m以內。

(2) 場景2(大廳)。與走廊場景相似，大廳中的特征模式均較為顯著。地圖特征模式數(shù)量分別為：PL-23、VL-47、AL-11、AS-11。各類模式的定位初始化準確率均達到了85%以上，平均初始化速度分別為209.908 ms、382.1 ms、82.638 ms、874.508 ms，定位誤差分別為0.048 5 m、0.028 3 m、0.026 4 m、0.057 6 m均保持在0.06 m以內。各類特征模式在該場景下的定位初始化效果良好。

(3) 場景3(停車場)。停車場是挑戰(zhàn)性較大的一個場景，地圖特征模式數(shù)量分別為：PL-358、VL-562、AL-37。場景地圖中未出現(xiàn)AS模式，因此未對AS進行定位初始化統(tǒng)計。由于該場景中存在多個車輛結構，造成地圖中的特征模式相對較多，定位初始化效率相對較低。VL與AL的準確率在62%和66%左右，但定位誤差均在0.04 m以內，PL的準確率在34%左右，定位誤差在0.136 7 m左右。相比于前兩個場景，整體定位準確率稍有下降，但垂直和對稱模式仍具有良好的定位精度。

表1 全局定位初始化試驗統(tǒng)計

4.3 討論與分析

試驗結果表明，本文方法可有效完成室內大尺度場景的定位初始化，且速度較快。試驗選取的典型的室內大場景具備一定的挑戰(zhàn)性，如長走廊和大廳玻璃結構等，場景空曠特征信息不足，地下停車場光照不佳，存在車輛等大型動態(tài)障礙物。目前實際的機器人導航或自動駕駛中初始定位方法通常采用室外GNSS、室內WIFI、人工標志等輔助信號源或2D LiDAR、相機等傳感器，這些方法在信號不佳、標志遮擋時效果欠佳、同時輔助信號源的設置需要較多人工干預，自動化程度有待提高，在典型室內場景中定位效果不佳。

在定位召回率方面，所選取的室內試驗場景占地面積較大，某些數(shù)據(jù)存在遮擋或在某些位置不存在某些特定特征模式，如在走廊場景中，試驗中激光雷達曾在電梯附近小空間內運行一段時間，此處位置特征模式不足，類似情況也存在于大廳和停車場場景，因此造成初始定位試驗的整體召回率較低。在實際機器人自主導航或自動駕駛時，可以避開像電梯附近這種人流量大、空間小的位置，選擇視野開闊位置進行定位初始化，若定位初始化失敗時，可適當移動當前傳感器位置，重新定位即可。

本文方法召回率雖然不高，但定位誤差相對較低，可以較好地完成定位初始化任務。由于地圖數(shù)據(jù)為單獨離線處理，因此在處理時可有選擇地剔除部分對初始化意義不大的特征數(shù)據(jù)，以提高整體定位速度。如試驗場景3中由于車輛信息的存在導致提取的地圖模式(干擾特征)較其他場景多，因此在試驗中，本文在地圖特征模式提取中有選擇地刪除掉部分(干擾)特征模式，以提高定位初始化的效率。同時在實際場景中，還可通過特征模式間相互組合的方式以提高定位初始化的召回率。

5 結 論

本文提出一種大尺度室內環(huán)境下基于特征模式的室內機器人定位初始化方法。在分析室內場景常見結構類別與特性基礎上，提出了特征模式的概念及多線激光雷達點云中特征模式提取的方法，并對其分級管理，采用輕量級的表達方式顯著提高特征表達效率。本文的半自動化地圖特征模式提取流程，保證了地圖數(shù)據(jù)準確度，并設計一種高效的地圖數(shù)據(jù)管理方案實現(xiàn)對地圖數(shù)據(jù)的高效管理，提高了全局初始定位效率。針對各類特征模式構建自適應匹配與配準策略，并利用地圖網(wǎng)格作為初始化評價指標，實現(xiàn)室內場景下傳感器的快速、準確的定位初始化，為室內機器人的快速初始定位提供一種新方法。為驗證本文方法的有效性，選取了3類代表性場景進行定位初始化試驗，并對試驗結果進行評價分析。本文方法不局限于16線LiDAR數(shù)據(jù)，可應用于自動駕駛中其他線數(shù)的多線激光雷達數(shù)據(jù)。本文全局定位初始化性能基本滿足實際室內自動駕駛性能需求，可應用于其初始定位模塊中。在進一步的研究工作中，將在更多類型場景中探索典型地物特征，擴充特征模式庫。并研究融合IMU等傳感器的實時定位系統(tǒng)構建，以應對更加復雜的室內大場景下定位工作。