牛國(guó)臣，王瑜

（中國(guó)民航大學(xué) 機(jī)器人研究所，天津300300）

無人車沿著特定路線行駛，實(shí)現(xiàn)如機(jī)場(chǎng)擺渡車、觀光車，在工廠或封閉園區(qū)巡邏、貨物搬運(yùn)［1］、衛(wèi)生清掃以及安保監(jiān)測(cè)［2］等功能，已經(jīng)成為未來的發(fā)展趨勢(shì)。定位問題是無人駕駛中的關(guān)鍵問題［3］。最常見的定位方法是基于全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS），其中全球定位系統(tǒng)（Global Positioning System，GPS）創(chuàng)立時(shí)間較早，應(yīng)用最為廣泛，使用基于載波相位的差分GPS技術(shù)RTK定位精度可達(dá)到分米級(jí)［4］。但是在實(shí)際應(yīng)用中，樹木、樓房建筑物等高大物體的遮擋均會(huì)對(duì)GPS信號(hào)的強(qiáng)度產(chǎn)生影響，導(dǎo)致定位失效。為了克服信號(hào)不足的問題，常用卡爾曼濾波器將GPS與慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）耦合［5-6］，但是高精度慣性導(dǎo)航元件較為昂貴，且存在累積誤差。近年來結(jié)合高精度地圖（High Definition Map，HD Map）［7］的 無 人 車 匹 配 定 位 發(fā) 展 較 為 迅 速［8］，但HD Map的構(gòu)建需要昂貴的測(cè)繪系統(tǒng)和人工標(biāo)注等復(fù)雜的后處理步驟［9］，成本十分昂貴，不適用于特定場(chǎng)景下無人車的大規(guī)模應(yīng)用，構(gòu)建一個(gè)低成本、可靠且在復(fù)雜環(huán)境下魯棒的定位與建圖系統(tǒng)已成為無人車產(chǎn)業(yè)級(jí)應(yīng)用的需求。

為了解決上述問題，機(jī)器人導(dǎo)航領(lǐng)域的關(guān)鍵技術(shù)之一，同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）技術(shù)被用于解決無人駕駛的定位問題［10-11］。其中，基于激光雷達(dá)的三維激光定位與建圖（SLAM）方法不受光照影響，數(shù)據(jù)量相對(duì)較少，創(chuàng)建的地圖精度更高，在無人車中應(yīng)用更為廣泛。Zhang和Singh［12］提出了一種基于激光雷達(dá)的實(shí)時(shí)定位和地圖（LiDAR Odometry and Mapping，LOAM）構(gòu)建方法，該方法通過激光雷達(dá)來獲取點(diǎn)云圖，并使用非線性最小二乘優(yōu)化方法來優(yōu)化角點(diǎn)和平面點(diǎn)的距離，同步實(shí)現(xiàn)了高頻率的里程計(jì)和低頻率的建圖，實(shí)時(shí)性較好，但由于缺少后端優(yōu)化，累積誤差導(dǎo)致所建點(diǎn)云地圖欠準(zhǔn) 確，全 局 一 致 性 低。Shan和 Englot［13］在LOAM框架基礎(chǔ)上，提出了改進(jìn)方案Lego-LOAM，使用迭代最近點(diǎn)（Iterative Closest Point，ICP）算法實(shí)現(xiàn)了閉環(huán)檢測(cè)和全局優(yōu)化的建圖功能，利用高層次幾何特征提高了空間結(jié)構(gòu)的描述效率，但該方法所建地圖為較稀疏的特征點(diǎn)地圖，難以用于定位。Ji等［14］提出了一種改進(jìn)LOAM 方案，在LOAM的框架中加入了GPS約束，該方法在局部估計(jì)中獲得了較高的準(zhǔn)確性，GPS先驗(yàn)信息可消除部分激光里程計(jì)帶來的累積誤差，但點(diǎn)云地圖的“重影”問題明顯。Deschaud［15］提出了一種掃描到模型（Scan to Model）的匹配方法，通過最小化當(dāng)前點(diǎn)到以隱式移動(dòng)最小二乘（Implicit Moving Least Squares，IMLS）表示的表面之間的距離來完成位姿估計(jì)，實(shí)現(xiàn)了穩(wěn)定準(zhǔn)確的點(diǎn)云匹配，但計(jì)算量大，難以用于實(shí)際系統(tǒng)。Chen等［16］提出了一種基于語義特征的改進(jìn)LOAM 方案，用于森林中樹木模型的檢測(cè)，但該方法僅對(duì)環(huán)境中樹木的語義特征進(jìn)行提取，對(duì)于無人車應(yīng)用場(chǎng)景來說，單一的環(huán)境特征遠(yuǎn)遠(yuǎn)不夠，并且還需要考慮大規(guī)模點(diǎn)云語義分割帶來的計(jì)算量問題。

綜上，以LOAM為首的激光里程計(jì)作為應(yīng)用程度最為廣泛的激光SLAM 主流方案，仍存在大規(guī)模場(chǎng)景中無法消除累積誤差、所建的點(diǎn)云地圖無法用于定位的問題。而Lego-LOAM 等增加了閉環(huán)約束的改進(jìn)方案，使系統(tǒng)增加對(duì)歷史觀測(cè)信息的判斷、保存和校正，提高軌跡的全局一致性，構(gòu)建較精確的全局地圖，提高地圖精度。增加了GNSS以及地平面等先驗(yàn)環(huán)境信息的方案，為SLAM 系統(tǒng)后端增添約束，可顯著優(yōu)化激光里程計(jì)的誤差，提高姿態(tài)精度。在實(shí)際應(yīng)用中，無論是姿態(tài)精度還是地圖精度均會(huì)影響無人車的定位效果。目前雖然有一些研究提出了較為完整的SLAM方案，但都處于研究階段，未考慮無人車的實(shí)際應(yīng)用場(chǎng)景。

基于此，本文設(shè)計(jì)了一個(gè)低成本、高精度的無人車定位與建圖系統(tǒng)，為滿足實(shí)時(shí)性，提出一種基于主成分分析（PCA）算法進(jìn)行點(diǎn)云特征提取與匹配的激光里程計(jì)。針對(duì)點(diǎn)云地圖的“重影”和激光里程計(jì)的累積誤差問題，從因子圖優(yōu)化角度構(gòu)建了一個(gè)完整的三維激光SLAM 系統(tǒng)，將GNSS（以GPS為例）及地平面等先驗(yàn)信息和基于聚類特征的閉環(huán)檢測(cè)使用圖優(yōu)化框架融合到SLAM系統(tǒng)中，并使用KITTI數(shù)據(jù)集測(cè)試了該系統(tǒng)的性能。

1 系統(tǒng)概述

本文所提出的算法框架如圖1所示。首先，對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行運(yùn)動(dòng)補(bǔ)償、體素濾波和分割等預(yù)處理操作；其次，在前端實(shí)現(xiàn)一個(gè)基于特征匹配的激光里程計(jì)，將地平面約束、GPS約束作為一元邊，聚類特征作為閉環(huán)約束加入到圖中，在后端進(jìn)行位姿圖優(yōu)化，得到最優(yōu)位姿，增量顯示軌跡并存儲(chǔ)相應(yīng)幀的點(diǎn)云數(shù)據(jù)構(gòu)建點(diǎn)云地圖。

圖1 系統(tǒng)整體結(jié)構(gòu)Fig.1 Overall structure of system

2）IMU坐標(biāo)系I，以IMU重心為坐標(biāo)原點(diǎn)，分別沿IMU水平軸和縱向軸設(shè)置X軸和Y軸，Z軸垂直于XY平面。

2 SLAM 方案

在本節(jié)中將介紹激光SLAM 系統(tǒng)的各個(gè)子模塊，包括點(diǎn)云預(yù)處理、激光里程計(jì)、多約束因子圖和圖優(yōu)化等4個(gè)模塊，并對(duì)其理論內(nèi)容進(jìn)行推導(dǎo)。

2.1 點(diǎn)云預(yù)處理

根據(jù)LiDAR的掃描方法，按空間和時(shí)間順序記錄這些點(diǎn)以進(jìn)行遍歷。由于在LiDAR的掃描過程中，車輛已經(jīng)移動(dòng)，為此引入IMU 位姿信息［12］對(duì)LiDAR運(yùn)動(dòng)進(jìn)行補(bǔ)償，得到去除點(diǎn)云畸變的當(dāng)前幀。激光點(diǎn)云信息量巨大，在進(jìn)行幀間匹配之前，將輸入點(diǎn)云S經(jīng)過體素濾波去除噪點(diǎn)和地面點(diǎn)，對(duì)剩下的點(diǎn)云進(jìn)行歐氏聚類，濾除動(dòng)態(tài)物體等無效點(diǎn)。

本文提出一種輕量級(jí)地平面分割方法。設(shè)定無人車前進(jìn)的方向?yàn)槭澜缱鴺?biāo)系X軸的正方向，將點(diǎn)云的三維空間XYZ降到二維平面XY，根據(jù)激光雷達(dá)投影到地面的射線中前后兩點(diǎn)的坡度是否大于事先設(shè)定的坡度閾值來判斷點(diǎn)是否為地面點(diǎn)。

激光雷達(dá)安裝在車輛頂部并與地面平行，雷達(dá)由下至上分布多個(gè)激光器，發(fā)出一系列放射狀激光束，這些激光束在平地上即表現(xiàn)為一條射線，如圖2所示。

圖2 地平面分割方法示意Fig.2 Schematic diagram of ground plane segmentation method

因雷達(dá)安裝高度固定，對(duì)于平坦的地面，可以根據(jù)LiDAR返回的坐標(biāo)值得到點(diǎn)到LiDAR的距離，對(duì)于任一點(diǎn)i，i∈S有

式中：xi和yi為i點(diǎn)在LiDAR坐標(biāo)系L下的坐標(biāo)位置；θi為點(diǎn)相對(duì)于車頭正方向（X方向）的夾角；ri為i點(diǎn)到LiDAR的水平距離。

將同一射線上的點(diǎn)按照水平距離ri排序，計(jì)算同一條射線上相鄰兩點(diǎn)的坡度。

式中：zi為i點(diǎn)的垂直高度；ai，i+1為i點(diǎn)和i+1點(diǎn)之間的坡度；h為L(zhǎng)iDAR的安裝高度。

為了避免地面不平或是LiDAR并未完全平行于地面安裝等微小因素引起的檢測(cè)誤差，定義一個(gè)閾值threguler，將式（2）結(jié)果與此閾值作比較，當(dāng)ai，i+1＜threguler即將i點(diǎn)歸為地面點(diǎn)。

對(duì)去除地面點(diǎn)后的點(diǎn)云進(jìn)行歐氏聚類，為了達(dá)到更好的聚類效果，在以LiDAR為圓心的不同距離區(qū)域內(nèi)使用不同的聚類半徑閾值，效果如圖3所示。

圖3 點(diǎn)云預(yù)處理效果Fig.3 Point cloud pretreatment effect

場(chǎng)景中的運(yùn)動(dòng)物體嚴(yán)重影響建圖過程的準(zhǔn)確性，但是從掃描中刪除所有動(dòng)態(tài)對(duì)象，需要場(chǎng)景的高水平語義信息，對(duì)點(diǎn)云進(jìn)行語義分割耗時(shí)較長(zhǎng)，不適用于實(shí)時(shí)系統(tǒng)。本文通過刪除小型物體來替代動(dòng)態(tài)物體刪除，從場(chǎng)景中丟棄那些尺寸被認(rèn)為可能是動(dòng)態(tài)物體的對(duì)象。將可能是行人、汽車、公交車或者貨車等小的點(diǎn)類去除，移除聚類邊界框X方向上小于10m，Y方向上小于5 m，Z方向上小于4m的點(diǎn)類，保留足夠的大型基礎(chǔ)設(shè)施信息，如墻壁、柵欄、立面和樹木（高度超過4 m），確保激光雷達(dá)里程計(jì)特征匹配過程中角點(diǎn)和邊緣等特征信息足夠。

2.2 激光里程計(jì)

LOAM根據(jù)激光雷達(dá)的特性，按照掃描線的空間順序和時(shí)間順序提取特征點(diǎn)。在本文中對(duì)其簡(jiǎn)化，引入主成分分析計(jì)算點(diǎn)云主方向，以滿足計(jì)算的實(shí)時(shí)性和準(zhǔn)確性要求。當(dāng)前幀點(diǎn)云表示為

式中：xi、yi和zi，i∈（1，2，…，n）為經(jīng)過點(diǎn)云坐標(biāo)系到世界坐標(biāo)系的坐標(biāo)轉(zhuǎn)換后的點(diǎn)云坐標(biāo)。

將點(diǎn)云分別投影到X軸、Y軸和Z軸上，計(jì)算協(xié)方差矩陣為

式中：cov為協(xié)方差算子，對(duì)每個(gè)點(diǎn)分別計(jì)算其x坐標(biāo)、y坐標(biāo)以及z坐標(biāo)之間的協(xié)方差。例如，cov（x，y）為x坐 標(biāo) 與y坐 標(biāo) 之 間 的 方 差，當(dāng)cov（x，y）＞0時(shí)，x和y正相關(guān)，cov（x，y）=0時(shí)，x和y相互獨(dú)立。協(xié)方差由式（5）計(jì)算：

式中：xi和yi為點(diǎn)云坐標(biāo)；n為當(dāng)前幀點(diǎn)云中點(diǎn)的個(gè)數(shù)。

計(jì)算協(xié)方差矩陣C的特征值和特征向量，根據(jù)特征值將特征向量從大到小排列，組成特征矩陣U。

設(shè)點(diǎn)i為在tk處獲得的特征矩陣Uk中的點(diǎn)，R點(diǎn)為i周圍10個(gè)連續(xù)點(diǎn)的集合，通過圍繞點(diǎn)i的10個(gè)矢量之和的范數(shù)的大小來評(píng)估點(diǎn)i附近局部表面的光滑度。定義光滑度c為

根據(jù)點(diǎn)的時(shí)間戳將Uk分為4個(gè)部分，并根據(jù)光滑度c的值對(duì)每個(gè)部分進(jìn)行排序，選擇值最大的2個(gè)點(diǎn)作為拐角點(diǎn)，值最小的4個(gè)點(diǎn)為平面點(diǎn)。

在進(jìn)行幀間匹配時(shí)，同一幀內(nèi)的2個(gè)連續(xù)角點(diǎn)可以形成一條邊緣線，3個(gè)平面點(diǎn)可以形成一個(gè)平面。利用幀間變換矩陣T將k+1幀的點(diǎn)變換回第k幀，則屬于k+1幀的角點(diǎn)和平面點(diǎn)分別到屬于k幀的邊緣線以及平面的距離應(yīng)該為0，設(shè)該距離的向量形式為d，T的初值可由IMU提供的車輛位姿得到。求d的最優(yōu)值即是一個(gè)典型的非線性最小二乘問題。使用Levenberg-Marquardt（L-M）優(yōu)化方法最小化d，得到最優(yōu)的T：

2.3 多約束因子圖

因子圖由“頂點(diǎn)”和“邊”組成［17］，具體到SLAM問題中，“頂點(diǎn)”為車輛的位姿，“邊”為位姿之間的約束［18］。為了解決長(zhǎng)距離漂移問題，本文提出設(shè)有一種包含地平面約束、GPS約束、點(diǎn)云聚類特征閉環(huán)約束和前端里程計(jì)約束的多約束因子圖，如圖4所示。圖中：Gi為由GPS數(shù)據(jù)生成的固定節(jié)點(diǎn)；Ti為三維位姿變換SE（3）中的車輛位姿節(jié)點(diǎn)；π0為地平面節(jié)點(diǎn)。

圖4 因子圖結(jié)構(gòu)Fig.4 Structure of factor graph

2.3.1 地平面約束

在2.1節(jié)點(diǎn)云預(yù)處理中已完成地平面的檢測(cè)，將地面局部建模為π=［nx，ny，nz，a］T參數(shù)化的平面［19］，nx、ny和nz為平面的法線，a為原點(diǎn)到平面的距離，對(duì)于平面上的任一點(diǎn)xi、yi、zi，有xinx+yiny+xinz+a=0。

為了計(jì)算車輛姿態(tài)和地平面之間的誤差，需要將車輛的姿態(tài)節(jié)點(diǎn)與固定的地平面節(jié)點(diǎn)相連，先進(jìn)行坐標(biāo)轉(zhuǎn)換，用初始時(shí)刻的地平面和車輛姿態(tài)，得到由車輛姿態(tài)估計(jì)的地平面，即

采用最小參數(shù)法定義平面參數(shù)表達(dá)式為

式中：arctan（ny/nx）為平面方位角；arctan（nz/n）為平面仰角；a為截距，表示原點(diǎn)到該平面的距離。則位姿節(jié)點(diǎn)和地平面節(jié)點(diǎn)之間的誤差被定義為

式中：πt為t時(shí)刻檢測(cè)到的地平面。

2.3.2 GPS約束

為了便于優(yōu)化，首先將GPS數(shù)據(jù)轉(zhuǎn)換為UTM坐標(biāo)，然后將每個(gè)GPS數(shù)據(jù)與位姿節(jié)點(diǎn)相關(guān)聯(lián)［20］，位姿節(jié)點(diǎn)與GPS數(shù)據(jù)的時(shí)間戳對(duì)齊，則GPS位置即可作為先驗(yàn)位置信息，成為位姿節(jié)點(diǎn)的一元邊緣。位姿節(jié)點(diǎn)的平移矢量與GPS位置之間的誤差定義為

式中：Tt為t時(shí)刻的車輛位姿；Gi為GPS數(shù)據(jù)生成的固定節(jié)點(diǎn)。

2.3.3 點(diǎn)云聚類特征閉環(huán)約束

在2.1節(jié)中已得到一系列點(diǎn)云聚類，分別進(jìn)行幾何特征和直方圖特征提取，用于隨后的標(biāo)識(shí)和分類［21-22］。提取的點(diǎn)云段特征由特征向量f=［f1f2… fn］表示，每個(gè)元素代表局部或全局描述方法。通過計(jì)算點(diǎn)云段中的點(diǎn)與其質(zhì)心之間的差，可以獲得點(diǎn)云段的3D結(jié)構(gòu)張量及其特征值λ1、λ2和λ3。參照SegMatch方法［23］計(jì)算點(diǎn)云的線性度（Lλ）、平面度（Pλ）、散射度（Sλ）、全方差（Aλ）、各向異性（Oλ）、特征熵（Eλ）和曲率變化度（Cλ），特征描述符如下

式中：zi為歸一化的特征值。

直方圖特征包含隨機(jī)兩點(diǎn)的距離統(tǒng)計(jì)、隨機(jī)三點(diǎn)組成的三角形面積統(tǒng)計(jì)以及隨機(jī)三點(diǎn)組成的一個(gè)角的角度統(tǒng)計(jì)等3種。

在完成聚類特征提取后，先基于點(diǎn)云聚類閉環(huán)匹配得到當(dāng)前幀與地圖的粗匹配，再采用掃描到地圖（Scan to Map）模型［24］，以當(dāng)前時(shí)刻點(diǎn)云幀為中心，按照時(shí)間向前和向后各索引數(shù)幀點(diǎn)云組成局部地圖，利用正態(tài)分布變換（Normal Distribution Transform，NDT）得到當(dāng)前點(diǎn)云和局部地圖間的精確變換。

為了提高閉環(huán)檢測(cè)的效率，避免頻繁檢測(cè)造成計(jì)算量過大問題，設(shè)置閉環(huán)檢測(cè)幀的最小時(shí)間差，濾除時(shí)間間隔較近的匹配。最終將閉環(huán)得到的變換矩陣，作為位姿約束輸入后端進(jìn)行優(yōu)化。

2.4 圖優(yōu)化

本文采用g2o優(yōu)化庫，對(duì)最終建立的位姿圖進(jìn)行優(yōu)化［25］。當(dāng)優(yōu)化了整個(gè)LiDAR運(yùn)動(dòng)的位姿圖后，將位姿節(jié)點(diǎn)對(duì)應(yīng)的三維點(diǎn)云幀進(jìn)行拼接，從而得到全局一致的軌跡和地圖。

3 實(shí)驗(yàn)分析

為了驗(yàn)證本文方法的可行性，選擇無人駕駛數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)。目前KITTI數(shù)據(jù)集是評(píng)估SLAM 的最受歡迎的數(shù)據(jù)集，為本文的實(shí)驗(yàn)評(píng)估提供了真實(shí)的數(shù)據(jù)。實(shí)驗(yàn)借助Linux下的機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）架構(gòu)，在具有2.20 GHz×4的i5-5200 CPU和12 GB內(nèi)存的計(jì)算機(jī)上運(yùn)行。

3.1 定位精度

以KITTI數(shù)據(jù)集提供的真值作為參考，將SLAM 軌跡與真值進(jìn)行對(duì)比，通過計(jì)算位姿軌跡與真實(shí)軌跡之間的相對(duì)位姿誤差（Relative Pose Error，RPE）和絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）來評(píng)估本文方法的定位精度。

3.1.1 準(zhǔn)確性分析

RPE是指在固定長(zhǎng)度的間隔內(nèi)，估算位姿和真實(shí)位姿之間的差值，相當(dāng)于直接測(cè)量激光里程計(jì)的誤差。為了體現(xiàn)本系統(tǒng)的定位效果，以KITTI數(shù)據(jù)集作為輸入數(shù)據(jù)，采用經(jīng)典的LOAM 方法和本文方法，分別在短距離運(yùn)動(dòng)場(chǎng)景（1 392m）和長(zhǎng)距離運(yùn)動(dòng)場(chǎng)景（3 714m）2種模式下進(jìn)行實(shí)驗(yàn)，將里程計(jì)數(shù)據(jù)存儲(chǔ)并解算，每隔50 m 統(tǒng)計(jì)一次RPE，以此評(píng)價(jià)姿態(tài)精度。表1中列出了不同運(yùn)動(dòng)場(chǎng)景的數(shù)據(jù)幀數(shù)、軌跡長(zhǎng)度以及RPE的詳細(xì)值，包括最大值、平均值、中值以及均方根值（Root Mean Square Error，RMSE）等。

表1 相對(duì)位姿誤差對(duì)比Table 1 Comparison of Relative Pose Errors（RPE）

可以看到，無論是在短距離還是長(zhǎng)距離運(yùn)動(dòng)環(huán)境下，本文方法的RPE要明顯低于LOAM 方法，經(jīng)典的LOAM 方法在短距離運(yùn)動(dòng)場(chǎng)景下RMSE為0.39m，但是在長(zhǎng)距離運(yùn)動(dòng)環(huán)境下，里程計(jì)精度降低，而本文方法在長(zhǎng)距離實(shí)驗(yàn)測(cè)試中，RPE的RMSE值可保持在1.5m以下，適用于無人車的應(yīng)用場(chǎng)景。圖5為本文方法和LOAM方法在長(zhǎng)距離運(yùn)動(dòng)環(huán)境下，RPE值和點(diǎn)云序列之間的關(guān)系，為了對(duì)比更為明顯，LOAM 方法用虛線表示，本文方法用實(shí)線表示。

通過比較可以看出，本文方法對(duì)應(yīng)的曲線更加緩和，相對(duì)來說跳變較少，雖然在第12次計(jì)算時(shí)出現(xiàn)了較大誤差值，但系統(tǒng)的圖約束消除了部分漂移，隨后誤差變化逐漸趨于平穩(wěn)。與本文方法相比，隨著車輛運(yùn)動(dòng)距離的增加，LOAM 方法的累積誤差逐漸增加，曲線增幅逐漸變大，這可說明本文方法在無人車較長(zhǎng)距離運(yùn)動(dòng)環(huán)境中的可靠性更高、穩(wěn)定性更好。

圖5 相對(duì)位姿誤差和點(diǎn)云序列曲線Fig.5 Curves of RPE and point cloud sequence

3.1.2 一致性分析

對(duì)于SLAM系統(tǒng)一致性的評(píng)估，本文引入絕對(duì)軌跡誤差（ATE）指標(biāo)，根據(jù)時(shí)間戳對(duì)齊位姿的真實(shí)值和SLAM系統(tǒng)的估計(jì)值，計(jì)算每對(duì)位姿之間的差值，最后對(duì)ATE進(jìn)行評(píng)價(jià)。這里同時(shí)考慮旋轉(zhuǎn)和平移帶來的影響。表2中列出了ATE的詳細(xì)值，包括最大值、平均值、中值以及RMSE等。

圖6為使用本文方法和LOAM 方法得到的ATE值和點(diǎn)云序列之間的關(guān)系。LOAM 沒有GPS、地平面以及點(diǎn)云聚類特征等約束信息，雖然在開始時(shí)軌跡誤差可維持在10 m以下，但在運(yùn)行一段時(shí)間后，因?yàn)槔鄯e誤差，位姿發(fā)生偏移，并且這種偏移隨著車輛運(yùn)動(dòng)軌跡的增長(zhǎng)而遞增；本文方法由于加入了先驗(yàn)和閉環(huán)等信息，使得車輛在運(yùn)動(dòng)過程中時(shí)時(shí)修正位姿的偏移量，從而可以保證車輛運(yùn)動(dòng)軌跡更貼近真實(shí)軌跡。

表2 絕對(duì)軌跡誤差對(duì)比Table 2 Comparison of Absolute Trajectory Errors（ATE）

圖7為使用本文方法得到的車輛運(yùn)動(dòng)軌跡與參考軌跡的對(duì)比，其中SLAM 系統(tǒng)的估計(jì)位姿越靠近真實(shí)位姿即表示誤差越小。從表1中已知使用經(jīng)典的LOAM 方法只適合于車輛運(yùn)動(dòng)距離較小時(shí)，隨著運(yùn)動(dòng)時(shí)間增長(zhǎng)，里程計(jì)帶來的漂移誤差隨著LiDAR的運(yùn)動(dòng)逐漸增加，使位姿嚴(yán)重偏離真實(shí)軌跡。與LOAM相比，本文方法所得到的軌跡與真實(shí)軌跡基本一致，全局一致性高，可以進(jìn)一步提高無人車定位與建圖的精度。

圖6 絕對(duì)軌跡誤差和點(diǎn)云序列的變化曲線Fig.6 Change curves of ATE and point cloud sequence

圖7 SLAM系統(tǒng)得到的軌跡與參考軌跡對(duì)比Fig.7 Comparison between trajectory obtained by SLAM system and reference trajectory

3.2 建圖效果

為了驗(yàn)證本文方法的大規(guī)模建圖效果，將通過SLAM方法得到一系列連續(xù)位姿形成的位姿軌跡對(duì)應(yīng)的點(diǎn)云幀轉(zhuǎn)換為世界坐標(biāo)系，拼接生成點(diǎn)云地圖，如圖8所示，基于點(diǎn)云的三維地圖可用于后期無人車定位。

在實(shí)驗(yàn)中，圖8（a）、（b）分別為使用LOAM方法和本文方法得到的建圖場(chǎng)景俯視圖，圖中A、B、C均為點(diǎn)云地圖局部細(xì)節(jié)效果?？梢钥吹?，由于缺乏閉環(huán)檢測(cè)和后端圖優(yōu)化，圖8（a）中地圖出現(xiàn)了較多的重影和模糊的現(xiàn)象，如圖中框選中區(qū)域所示，點(diǎn)云幀之間誤差較大，漂移嚴(yán)重甚至導(dǎo)致地圖構(gòu)建失敗，在框選中C部分，點(diǎn)云幀間匹配丟失，所建地圖未閉合。本文方法所建地圖與真實(shí)環(huán)境基本吻合，圖8（b）中地圖封閉且完整，邊界清晰，無明顯重影，效果較好。

圖8 點(diǎn)云地圖Fig.8 A point cloud map

4 結(jié) 論

本文提出一種基于多約束因子圖優(yōu)化的三維激光SLAM方案，經(jīng)實(shí)驗(yàn)驗(yàn)證可得到以下結(jié)論：

1）基于PCA算法和點(diǎn)云特征匹配的激光里程計(jì)可以有效應(yīng)對(duì)無人車室外環(huán)境定位，該匹配方法簡(jiǎn)單且魯棒，實(shí)時(shí)性好。

2）加入地平面、GPS數(shù)據(jù)等先驗(yàn)信息以及點(diǎn)云聚類特征閉環(huán)約束的多約束因子圖，降低了前端里程計(jì)的累積誤差，解決了構(gòu)建地圖時(shí)的重影問題，同時(shí)提高了定位精度。

3）以三維激光SLAM 的代表性方法LOAM與本文方法進(jìn)行對(duì)比驗(yàn)證，實(shí)驗(yàn)證明，本文方法在長(zhǎng)距離實(shí)驗(yàn)測(cè)試中，RPE可達(dá)到1.5 m以下，適用于無人車的應(yīng)用場(chǎng)景。

下一步計(jì)劃將此系統(tǒng)用到無人車實(shí)際系統(tǒng)中，探索復(fù)雜環(huán)境下高精度的無人車定位與建圖方法。