基于激光雷達(dá)的停車(chē)場(chǎng)車(chē)輛定位算法

周 蘇，李偉嘉，郭軍華

（1.同濟(jì)大學(xué)汽車(chē)學(xué)院，上海 201804；2.同濟(jì)大學(xué)中德學(xué)部，上海 201804）

汽車(chē)定位是汽車(chē)自動(dòng)駕駛的核心技術(shù)之一，也是實(shí)現(xiàn)環(huán)境感知、路徑規(guī)劃與控制等后續(xù)功能的基礎(chǔ)。目前汽車(chē)定位采用的傳感器主要有激光雷達(dá)、毫米波雷達(dá)、慣性測(cè)量單元（IMU）和攝像頭。在室外空曠無(wú)遮蔽的道路環(huán)境下，基于GPS+IMU的定位導(dǎo)航方法已經(jīng)可以保證較高的精度［1］。L4以上級(jí)別的自動(dòng)駕駛要求厘米級(jí)定位精度，目前最有前景的技術(shù)方案是基于GNSS（global navigation satellite system）+IMU+lidar/camera+高精地圖的融合定位。例如，谷歌的激光雷達(dá)定位方案、斯坦福大學(xué)的概率地圖點(diǎn)云匹配定位方案［2］和特斯拉的視覺(jué)定位方案。無(wú)論是基于激光雷達(dá)的還是基于視覺(jué)的定位系統(tǒng)，其基本思路都是基于環(huán)境要素的匹配對(duì)汽車(chē)進(jìn)行定位，即事先利用采集車(chē)采集高精地圖數(shù)據(jù)，然后用實(shí)時(shí)掃描特征（激光或視覺(jué)）與事先制作的高精地圖匹配，最后獲取車(chē)輛的位置估計(jì)。因此，即使在室內(nèi)環(huán)境如隧道、停車(chē)場(chǎng)等情況下，車(chē)輛無(wú)法獲取可靠的GPS信息，也可以采用其他傳感器信號(hào)進(jìn)行定位。毫米波雷達(dá)無(wú)法得到大量的環(huán)境點(diǎn)云信息，目前主要在ADAS（advanced driver assistant system）中用于距離探測(cè)［3］?；跀z像頭的視覺(jué)SLAM（simultaneous localization and mapping）算法目前在一些室內(nèi)機(jī)器人中有所應(yīng)用，但是其累積誤差較大，會(huì)發(fā)生定位漂移。在較大的環(huán)境尺度如停車(chē)場(chǎng)內(nèi)，即使增加閉環(huán)檢測(cè)措施，漂移情況也十分嚴(yán)重，視覺(jué)SLAM算法用作車(chē)輛主要定位方案的可靠性較低。激光雷達(dá)可以獲取大量的環(huán)境點(diǎn)云信息，已經(jīng)越來(lái)越多地被用于三維環(huán)境重構(gòu)和機(jī)器人定位以及無(wú)人駕駛建圖定位等領(lǐng)域［4］。

激光雷達(dá)的自主定位可分為建圖和定位2個(gè)步驟。較早的研發(fā)中建圖主要通過(guò)卡爾曼濾波器等方法實(shí)現(xiàn)，較近的研發(fā)中已經(jīng)逐步采用基于非線性?xún)?yōu)化的SLAM算法，即同時(shí)定位與建圖的方法。目前較為流行的激光雷達(dá)SLAM算法開(kāi)源框架主要有ROS（robot operating system）中集成的Gmapping、Karto-SLAM、德國(guó)達(dá)姆施塔特工業(yè)大學(xué)提出的Hector-SLAM和谷歌開(kāi)發(fā)的Cartographer-SLAM等。為了在較大尺度的室內(nèi)環(huán)境中研究激光雷達(dá)SLAM，以某一100 m×50 m的測(cè)試停車(chē)場(chǎng)為例，基于Cartographer框架開(kāi)展相關(guān)的優(yōu)化工作。

根據(jù)SLAM算法得到環(huán)境地圖后，在車(chē)輛運(yùn)行過(guò)程中就可以實(shí)現(xiàn)實(shí)時(shí)車(chē)輛定位。車(chē)輛定位算法的實(shí)質(zhì)是一個(gè)狀態(tài)估計(jì)問(wèn)題，一般分為狀態(tài)預(yù)測(cè)與修正2個(gè)步驟。首先根據(jù)狀態(tài)轉(zhuǎn)移方程預(yù)測(cè)狀態(tài)的當(dāng)前估計(jì)值，然后根據(jù)當(dāng)前的觀測(cè)模型修正狀態(tài)的估計(jì)值，狀態(tài)估計(jì)就是不斷地重復(fù)狀態(tài)預(yù)測(cè)與修正的過(guò)程?？柭鼮V波器和粒子濾波器是狀態(tài)估計(jì)常用的2種算法。本研究中采用粒子濾波器進(jìn)行車(chē)輛位姿的估計(jì)，得到車(chē)輛位姿的最大似然估計(jì)［5］。然后，結(jié)合KLD（Kullback-Leibler divergence）采樣方法，動(dòng)態(tài)控制采樣粒子樣本數(shù)。

1 SLAM算法構(gòu)建地圖

1.1 概率占柵格地圖模型

選擇概率占柵格地圖模型［6］用于車(chē)輛定位。占柵格地圖實(shí)際上是環(huán)境信息的一種離散描述，即根據(jù)分辨率將地圖劃分為小柵格，地圖M就是所有柵格mp的集合。如果用1或0表示柵格是被占據(jù)的或未被占據(jù)的，那么ρ(mp=1)（簡(jiǎn)寫(xiě)為ρ(mp)）就代表著這個(gè)柵格被占據(jù)的概率。地圖M表示如下：

概率占柵格地圖M的取值被限制在[ρmin，ρmax]之間，表示柵格被占據(jù)的概率。每個(gè)柵格在作為圖片顯示時(shí)，也是一個(gè)像素。當(dāng)每一幀激光雷達(dá)掃描數(shù)據(jù)被插入到概率柵格中時(shí)，都有一組表示命中與一組表示未命中的點(diǎn)集被計(jì)算。如果命中，就將最接近激光雷達(dá)掃描點(diǎn)的柵格加入到命中點(diǎn)集中。如果未命中，就將激光雷達(dá)掃描的原點(diǎn)與命中點(diǎn)連線，將連線上所經(jīng)過(guò)的柵格都加入到未命中點(diǎn)集中。如圖1所示，命中點(diǎn)所在的柵格用打叉的灰色格子表示，未命中點(diǎn)所在的柵格用不打叉的灰色格子表示。白色柵格表示未知，即尚未被探測(cè)到的區(qū)域。

圖1 概率占柵格地圖模型［6］Fig.1 Probability grid map model[6]

網(wǎng)格中每個(gè)柵格被占據(jù)的概率ρ(mp)不是由一次掃描決定的，而是由經(jīng)過(guò)該點(diǎn)的多次掃描共同決定的。對(duì)于之前已經(jīng)觀察過(guò)的柵格，如果處于命中或者未命中的點(diǎn)集中，將會(huì)被分配到一個(gè)占據(jù)概率ρ。對(duì)一個(gè)之前已經(jīng)觀察過(guò)的柵格，可通過(guò)下式更新它的命中率與被占據(jù)概率［7］：

式中：odds表示命中率函數(shù)，odds-1表示命中率函數(shù)的反函數(shù)；ρ表示柵格被占據(jù)的概率；Mold、Mnew分別表示更新前、更新后的地圖；h表示命中率；clamp表示極值限定函數(shù)，可以將函數(shù)結(jié)果限定在給定的最小值與最大值的區(qū)間中。

1.2 激光雷達(dá)前端點(diǎn)云匹配

SLAM通常分為前端和后端，前端主要充當(dāng)里程計(jì)的角色，通過(guò)2幀激光雷達(dá)數(shù)據(jù)之間的相對(duì)變換確定車(chē)輛的位姿。通過(guò)旋轉(zhuǎn)和平移將2幀激光雷達(dá)數(shù)據(jù)進(jìn)行最大限度的重疊，就可以得到車(chē)輛在2幀數(shù)據(jù)之間的相對(duì)位姿變化。

現(xiàn)在的激光雷達(dá)SLAM一般不直接進(jìn)行2幀數(shù)據(jù)之間的點(diǎn)云匹配，這是因?yàn)橹苯狱c(diǎn)云匹配計(jì)算量太大、耗時(shí)太長(zhǎng)，而且匹配結(jié)果的誤差較大、漂移嚴(yán)重，不利于后端的位姿優(yōu)化。因此，一般使用激光雷達(dá)數(shù)據(jù)與當(dāng)前構(gòu)建的子地圖Submap進(jìn)行匹配［7］。激光雷達(dá)位姿的變換可用下式表示：

式中：Tξ為坐標(biāo)系的變換矩陣；Rξ為旋轉(zhuǎn)矩陣；tξ為平移矩陣；p為當(dāng)前掃描數(shù)據(jù)中各個(gè)點(diǎn)云的坐標(biāo)；ξ為激光雷達(dá)坐標(biāo)系與子圖坐標(biāo)系之間的相對(duì)位姿。激光雷達(dá)數(shù)據(jù)與子圖之間的匹配可以通過(guò)構(gòu)造一個(gè)非線性最小二乘問(wèn)題得到解決，相應(yīng)的目標(biāo)函數(shù)如下所示：

式中：K為一次掃描數(shù)據(jù)的點(diǎn)集總數(shù)，k為點(diǎn)集K中的第k個(gè)掃描點(diǎn)；h k為第k個(gè)掃描點(diǎn)的空間坐標(biāo)；Msmooth為經(jīng)過(guò)插值處理的概率占柵格地圖。概率占柵格地圖本來(lái)是離散的，為了保證式（6）在進(jìn)行迭代計(jì)算時(shí)能夠穩(wěn)定且收斂，需要通過(guò)插值處理來(lái)生成光滑的概率地圖。選用雙三次插值方法進(jìn)行處理，產(chǎn)生的地圖更加平滑且穩(wěn)定，同時(shí)插值后可能出現(xiàn)概率小于0或者大于1的情況，但這種情況不會(huì)產(chǎn)生錯(cuò)誤也不會(huì)影響計(jì)算。高斯-牛頓迭代法可以求解式（6）描述的非線性最小二乘問(wèn)題［8］，同時(shí)已有很多開(kāi)源求解器如Ceres、G2O等可供選用。Ceres庫(kù)提供了基于模板元的自動(dòng)求導(dǎo)和運(yùn)行時(shí)的數(shù)值求導(dǎo)，只需構(gòu)建代價(jià)函數(shù)即可，而G2O庫(kù)只提供運(yùn)行時(shí)數(shù)值求導(dǎo)的一種方式，同時(shí)需要構(gòu)建誤差圖模型。因此，采用Ceres庫(kù)［9］進(jìn)行求解。

某次激光雷達(dá)數(shù)據(jù)與子圖間的匹配如圖2所示。左側(cè)為匹配前的點(diǎn)云初始位置，即粗配準(zhǔn)位姿，右側(cè)為經(jīng)過(guò)式（6）迭代計(jì)算后的點(diǎn)云位置?？梢杂^察到，經(jīng)過(guò)高斯-牛頓迭代法求解式（6）后得到的位姿更為精確，這是因?yàn)榧す饫走_(dá)點(diǎn)云數(shù)據(jù)與子圖有著更多的重合，如圖2中用圓圈標(biāo)出部分。迭代過(guò)程中的車(chē)輛位姿（ξx，ξy，ξθ）變化如表1所示。經(jīng)過(guò)約11次迭代后收斂至新位姿，可見(jiàn)通過(guò)高斯-牛頓迭代法可以快速對(duì)車(chē)輛位姿進(jìn)行精配準(zhǔn)。

圖2 激光雷達(dá)數(shù)據(jù)與子圖匹配示意圖Fig.2 Schematic diagram of lidar data matching with submap

表1 位姿隨迭代次數(shù)變化Tab.1 Pose changes with the number of iterations

圖3為通過(guò)精配準(zhǔn)后導(dǎo)出的停車(chē)場(chǎng)局部三維點(diǎn)云地圖。由于使用的是ibeo LUX 4L激光雷達(dá)，在垂直于地面方向上只有4條線束，因此無(wú)法采集到較高位置的信息，只能獲得二維平面地圖信息?？梢钥吹?，點(diǎn)云地圖中的車(chē)輛與墻壁受限于激光雷達(dá)線束數(shù)量，只有很低的高度，因此采用二維占柵格地圖配合粒子濾波器用于車(chē)輛定位。

圖3 停車(chē)場(chǎng)局部點(diǎn)云配準(zhǔn)地圖Fig.3 Local point cloud registration map of parking lot

1.3 基于圖優(yōu)化理論的后端優(yōu)化

基于圖優(yōu)化理論的位姿調(diào)整（在很多文獻(xiàn)中也被稱(chēng)為閉環(huán)檢測(cè)）是激光雷達(dá)SLAM的核心內(nèi)容。后端優(yōu)化用于修正SLAM過(guò)程產(chǎn)生的累積誤差，提高車(chē)輛定位和地圖構(gòu)建的準(zhǔn)確度。和前端匹配類(lèi)似，后端優(yōu)化也需要構(gòu)造非線性最小二乘問(wèn)題，如下所示：

式中：e為誤差函數(shù)。計(jì)算中要用到式（5）旋轉(zhuǎn)矩陣Rξ的逆變換。

圖4為一次閉環(huán)行駛后未經(jīng)過(guò)圖優(yōu)化調(diào)整直接得到的停車(chē)場(chǎng)占柵格地圖。雖然停車(chē)場(chǎng)的局部匹配信息較為精確，但是SLAM過(guò)程中有累積誤差，導(dǎo)致地圖在較大尺度上漂移嚴(yán)重。圖5為根據(jù)式（7）進(jìn)行全局位姿調(diào)整后得到的停車(chē)場(chǎng)占柵格地圖。從圖5可以發(fā)現(xiàn)，累積的全局誤差被很大程度地消除了。

1.4 占柵格地圖與建筑地圖對(duì)比

通過(guò)前端的點(diǎn)云匹配以及后端的位姿優(yōu)化就可以繪制出環(huán)境地圖，但繪制出的環(huán)境地圖并不能直接用于車(chē)輛定位，因?yàn)榈貓D還存在少許特征缺失、尺度偏移等現(xiàn)象。同時(shí)，靜態(tài)地圖不應(yīng)該含有環(huán)境的動(dòng)態(tài)信息，如停車(chē)場(chǎng)中的車(chē)輛信息。因此，需要通過(guò)多次數(shù)據(jù)采集來(lái)繪制出大量信息完整的地圖，再將地圖拼接融合，并且剔除環(huán)境的動(dòng)態(tài)信息。圖6為單次SLAM所繪制的停車(chē)場(chǎng)概率占柵格平面地圖，可以看到一些環(huán)境信息被車(chē)輛所遮蔽。

圖6 一次SLAM后的停車(chē)場(chǎng)占柵格地圖Fig.6 Grid map of parking lot after one SLAM

通過(guò)多次數(shù)據(jù)采集并進(jìn)行信息融合，剔除停車(chē)場(chǎng)的車(chē)輛信息，補(bǔ)全每次局部采集缺失的信息后，最終得到的停車(chē)場(chǎng)占柵格地圖如圖7所示。與圖8對(duì)比后可以發(fā)現(xiàn)，經(jīng)過(guò)融合調(diào)整后的地圖能夠精確反映停車(chē)場(chǎng)的環(huán)境信息。

圖7 停車(chē)場(chǎng)概率占柵格地圖Fig.7 Probability grid map of parking lot

圖8 停車(chē)場(chǎng)的建筑設(shè)計(jì)地圖Fig.8 Architectural design map of parking lot

2 車(chē)輛在線定位

車(chē)輛的在線定位通過(guò)蒙特卡洛定位方法予以實(shí)現(xiàn)。首先，通過(guò)運(yùn)動(dòng)模型確定車(chē)輛當(dāng)前時(shí)刻的先驗(yàn)位姿分布；然后，在先驗(yàn)位姿周?chē)断麓罅康膸?quán)重隨機(jī)粒子，粒子的權(quán)重代表著該位置與地圖的匹配程度，匹配程度越高的粒子就越有可能在重采樣過(guò)程中被選入后驗(yàn)概率分布的粒子集；最后，通過(guò)重采樣過(guò)程得到車(chē)輛位姿的后驗(yàn)概率分布。

2.1 粒子濾波器框架設(shè)計(jì)

粒子濾波器是通過(guò)迭代方式不斷更新車(chē)輛位姿的后驗(yàn)概率分布，以逐漸消除誤差，最終達(dá)到精確定位車(chē)輛的目的。主要步驟為：①初始化粒子群，生成N個(gè)帶權(quán)重的粒子，每一個(gè)粒子代表一種可能的車(chē)輛位姿狀態(tài)；②模擬粒子運(yùn)動(dòng)，通過(guò)里程計(jì)或者激光雷達(dá)里程計(jì)給出車(chē)輛位姿的先驗(yàn)估計(jì)；③計(jì)算粒子權(quán)重，通過(guò)粒子與概率占柵格地圖的匹配程度給予每一個(gè)粒子一個(gè)權(quán)重，權(quán)重越大，表明該粒子越有可能代表真實(shí)的車(chē)輛位姿狀態(tài)；④根據(jù)粒子權(quán)重重采樣，權(quán)重越大的粒子就越有機(jī)會(huì)被重采樣選中，之后便可以得到當(dāng)前時(shí)刻車(chē)輛位姿的后驗(yàn)概率分布。不斷地重復(fù)②～④就可以對(duì)車(chē)輛進(jìn)行在線定位。粒子濾波器流程如圖9所示。

圖9 粒子濾波器流程Fig.9 Flow chart of particle filter

粒子濾波器定位的核心就是通過(guò)大量帶權(quán)重的粒子模擬車(chē)輛運(yùn)動(dòng)，每個(gè)粒子都表示一個(gè)可能的車(chē)輛位姿。圖10描述了粒子狀態(tài)隨迭代次數(shù)的變化。剛開(kāi)始定位時(shí)，初始位姿來(lái)自于GPS信息，此時(shí)車(chē)輛已經(jīng)進(jìn)入停車(chē)場(chǎng)，衛(wèi)星信號(hào)遮蔽嚴(yán)重，定位結(jié)果不可靠。因此，假定粒子的分布是以初始位姿為均值（0，0）、方差為0.5 m2的二維高斯分布，并以此狀態(tài)開(kāi)始粒子濾波器的迭代，如圖10a所示。在經(jīng)過(guò)100次迭代后，粒子的朝向已經(jīng)較為統(tǒng)一，而粒子的位置分布還較為分散，如圖10b所示。在經(jīng)過(guò)200次迭代后，粒子基本穩(wěn)定在位姿均值附近0.5 m范圍內(nèi)，在經(jīng)過(guò)300次迭代后，粒子方向基本一致，位置也更為聚攏，如圖10c、d所示。由圖10可見(jiàn)，在車(chē)輛運(yùn)動(dòng)過(guò)程中，通過(guò)對(duì)環(huán)境的觀測(cè)，不斷地更新粒子權(quán)重以及重采樣高權(quán)重粒子，以逼近車(chē)輛的最大后驗(yàn)估計(jì)位姿。

圖10 粒子濾波過(guò)程中粒子狀態(tài)變化Fig.10 Changes of particle state during particle filtering

2.2 粒子權(quán)重的計(jì)算

粒子的權(quán)重可以通過(guò)計(jì)算各個(gè)激光雷達(dá)掃描點(diǎn)與概率占柵格地圖M中處于占據(jù)狀態(tài)的柵格之間的距離和來(lái)確定。距離和越小，說(shuō)明該粒子越能反映真實(shí)的車(chē)輛位姿，因此權(quán)重就越大；反之，距離和越大，粒子的權(quán)重也就越小［10］。指數(shù)函數(shù)可以描述上述關(guān)系，如下所示：

式中：M表示原始的概率占柵格地圖；Mdis表示由距被占據(jù)狀態(tài)柵格最近距離組成的二維地圖矩陣；Tdis表示激光點(diǎn)云的坐標(biāo)變換矩陣；Dmax表示Mdis地圖矩陣中的距離上限；x q表示第q個(gè)粒子掃描得到的所有激光雷達(dá)掃描點(diǎn)的集合；x q，k表示第q個(gè)粒子中的第k個(gè)掃描點(diǎn)；N表示x q中掃描點(diǎn)的總個(gè)數(shù)；w q表示第q個(gè)粒子的最終權(quán)重。

圖11中，左側(cè)為粒子濾波器在線定位時(shí)激光雷達(dá)與占柵格地圖匹配圖，右側(cè)為以車(chē)輛估計(jì)位姿為中心的4 m×4 m范圍內(nèi)權(quán)重的分布結(jié)果。權(quán)重是重采樣過(guò)程的重要依據(jù)，也是粒子濾波器的核心步驟。權(quán)重表征該位置為車(chē)輛真實(shí)位置的可能性大小，因此權(quán)重的分布與車(chē)輛周?chē)沫h(huán)境有著緊密的聯(lián)系。圖11a中，高權(quán)重區(qū)域集中在垂直于車(chē)輛前進(jìn)的方向上，車(chē)輛在該方向上移動(dòng)時(shí)車(chē)輛兩側(cè)的點(diǎn)云始終與地圖中的墻壁重合，因此在該方向上計(jì)算出的粒子權(quán)重也較其他地方更大。圖11b中，高權(quán)重區(qū)域集中在平行于車(chē)輛前進(jìn)的方向上，車(chē)輛進(jìn)入狹長(zhǎng)的廊道時(shí)，沿著車(chē)輛前進(jìn)的方向均能保證較高的點(diǎn)云重合度，因此該區(qū)域粒子權(quán)重也更大。圖11c中，權(quán)重的分布呈一單峰狀，表明車(chē)輛在該位置時(shí)環(huán)境特征明顯，利于粒子濾波器定位。

圖11 不同位置粒子的權(quán)重分布Fig.11 Weight distribution of particles at different positions

2.3 重采樣過(guò)程

KLD采樣是蒙特卡洛定位的一個(gè)變種，能夠動(dòng)態(tài)地隨時(shí)間改變采樣樣本的粒子數(shù)［11］。引入KLD采樣可以在保證定位精度的前提下，盡量減少粒子數(shù)量，進(jìn)而提高運(yùn)算效率。通過(guò)采樣的近似質(zhì)量的統(tǒng)計(jì)界限來(lái)確定粒子數(shù)量。具體地，對(duì)于每次迭代，KLD采樣以概率(1-δ)確定樣本數(shù)量，使得真實(shí)的后驗(yàn)分布與基于采樣的近似分布之間的誤差小于ε。KLD采樣的抽樣公式由下式給出：

式中：SX表示抽樣樣本；l表示樣本編號(hào)；ε、δ表示誤差的界；z1-δ表示上分位數(shù)為(1-δ)的標(biāo)準(zhǔn)正態(tài)分布。與傳統(tǒng)的粒子濾波器不同的是，KLD采樣將一個(gè)加權(quán)后的采樣集合作為輸入，因此上一時(shí)刻的樣本沒(méi)有被重采樣。另外，將統(tǒng)計(jì)誤差的界限限定在δ與ε之間。

測(cè)試中ε取值為0.05，δ取值為0.2。圖12為粒子濾波過(guò)程中無(wú)KLD采樣與有KLD采樣下粒子數(shù)變化對(duì)比。在KLD采樣的作用下，粒子數(shù)隨著迭代次數(shù)增加而迅速下降，之后逐漸穩(wěn)定在600個(gè)粒子左右。傳統(tǒng)粒子濾波器（無(wú)KLD采樣）的粒子數(shù)不變，始終固定在設(shè)定的最大粒子數(shù)上限2 500個(gè)。由此可見(jiàn)，KLD采樣讓粒子濾波器能夠自適應(yīng)地調(diào)節(jié)粒子數(shù)大小，極大地提高了計(jì)算效率，降低了計(jì)算資源占用，在嵌入式系統(tǒng)上也可以應(yīng)用。

圖12 有無(wú)KLD采樣粒子數(shù)變化Fig.12 Particle number change with and without KLD sampling

3 實(shí)車(chē)測(cè)試

3.1 ROS節(jié)點(diǎn)流圖

在ROS中，數(shù)據(jù)的基本計(jì)算單元叫做節(jié)點(diǎn)。節(jié)點(diǎn)可以訂閱消息，消息就是基本的數(shù)據(jù)流，經(jīng)過(guò)計(jì)算之后再將消息發(fā)布出去［12］。多個(gè)節(jié)點(diǎn)之間可以互相通信，這也是ROS的方便之處，可以清晰地區(qū)分?jǐn)?shù)據(jù)流關(guān)系。圖13為ROS節(jié)點(diǎn)圖。圖13中，amcl節(jié)點(diǎn)是粒子濾波器的運(yùn)行節(jié)點(diǎn)，它訂閱初始位姿消息initialpose、激光雷達(dá)數(shù)據(jù)ibeolux以及tf變換消息消息。amcl_pose為最終得到的車(chē)輛定位位姿，particlecloud為粒子濾波時(shí)的粒子點(diǎn)云位姿數(shù)據(jù)。tf節(jié)點(diǎn)主要用來(lái)處理ROS中的坐標(biāo)系變換，tf_bc用來(lái)播報(bào)坐標(biāo)轉(zhuǎn)換關(guān)系。pointcloud_to_laserscan節(jié)點(diǎn)用來(lái)將三維的激光雷達(dá)點(diǎn)云數(shù)據(jù)轉(zhuǎn)化為二維平面的激光雷達(dá)點(diǎn)云數(shù)據(jù)，point_cloud為激光雷達(dá)原始數(shù)據(jù)，as_tx為其命名空間，ibeolux/scan為轉(zhuǎn)換后的二維激光雷達(dá)點(diǎn)云數(shù)據(jù)。cartographer_node為前文提到的谷歌開(kāi)源SLAM框架；map_server節(jié)點(diǎn)用于發(fā)布地圖數(shù)據(jù)map；playbag節(jié)點(diǎn)用于發(fā)布離線測(cè)試時(shí)錄制的激光雷達(dá)數(shù)據(jù)，在線測(cè)試時(shí)無(wú)需此節(jié)點(diǎn)，可以直接從車(chē)輛上讀取數(shù)據(jù)；intialpose為車(chē)輛的初始位姿，一般在進(jìn)入停車(chē)場(chǎng)前由GNSS+IMU的融合定位結(jié)果給出；n_rviz節(jié)點(diǎn)用于上位機(jī)的可視化監(jiān)控。

圖13 ROS節(jié)點(diǎn)數(shù)據(jù)流Fig.13 Data flow of ROS node

3.2 定位精度分析

定位精度包括位置精度和角度精度。由于真實(shí)的車(chē)輛位姿難以獲取，因此無(wú)法進(jìn)行比較。粒子濾波器在提供定位的估計(jì)位姿之外，還提供了一組位姿的協(xié)方差矩陣。協(xié)方差矩陣表征了數(shù)據(jù)信息的不確定度，因此以位置信息和角度信息的不確定度作為參考信息來(lái)表征粒子濾波器的定位精度。此外，還可以通過(guò)激光雷達(dá)點(diǎn)云與地圖的重合情況對(duì)定位精度有一個(gè)大致的判斷。圖14為局部的定位效果圖。

圖14 粒子濾波器定位匹配圖Fig.14 Matching map of positioning for particle filter

圖14 中黑色箭頭為車(chē)輛在當(dāng)前時(shí)刻的定位位姿?；疑c(diǎn)集為激光雷達(dá)的點(diǎn)云數(shù)據(jù)。黑色邊框?yàn)橥＼?chē)場(chǎng)的概率占柵格地圖?？梢钥吹剑c(diǎn)云數(shù)據(jù)與概率占柵格地圖能夠很好地重合，從一定程度上表明了定位的精確性。

對(duì)于定位精度的定量分析，采用協(xié)方差分析方法分析定位位置與角度的標(biāo)準(zhǔn)差，如圖15、圖16所示。圖15中，在開(kāi)始階段粒子濾波器產(chǎn)生的位置不確定度標(biāo)準(zhǔn)差σp較大，這是由于已知的初始位姿并不是完全精確的，而是存在誤差。粒子濾波器需要經(jīng)過(guò)一定次數(shù)的迭代才能逐漸趨于真實(shí)位姿，在經(jīng)過(guò)約60次迭代后，σp逐漸穩(wěn)定在5.0 cm以下，平均標(biāo)準(zhǔn)差為2.3 cm，3σ為6.9 cm。根據(jù)統(tǒng)計(jì)學(xué)的3σ原則［13］，可以認(rèn)為估計(jì)位姿與真實(shí)車(chē)輛位姿之間的誤差不超過(guò)6.9 cm。圖16中，車(chē)輛定位角度不確定度的平均標(biāo)準(zhǔn)差σθ約為0.01 rad，3σθ為0.03 rad，即1.8°。同樣根據(jù)3σ原則，車(chē)輛定位角度的偏差不超過(guò)1.8°。

圖15 定位位置標(biāo)準(zhǔn)差和迭代次數(shù)的關(guān)系Fig.15 Relationship between position standard deviation and the number of iterations

圖16 定位角度標(biāo)準(zhǔn)差和迭代次數(shù)的關(guān)系Fig.16 Relationship between angle standard deviation and the number of iterations

3.3 算法實(shí)時(shí)性分析

算法的實(shí)時(shí)性直接代表著算法的效率。為保證算法的效率，首先需要設(shè)定粒子濾波器的更新策略。粒子濾波器狀態(tài)的更新，并不是以時(shí)間為結(jié)算單位。如果設(shè)定時(shí)間更新，當(dāng)車(chē)輛移動(dòng)較慢或者車(chē)輛停止時(shí)，就會(huì)造成嚴(yán)重的計(jì)算資源浪費(fèi)。因此，通過(guò)設(shè)置車(chē)輛的位置變化最小值，來(lái)判斷是否進(jìn)行粒子濾波器的迭代更新。當(dāng)車(chē)輛的先驗(yàn)位置變化超過(guò)程序設(shè)定的最小值min_d時(shí)，便進(jìn)行一次粒子濾波的重采樣過(guò)程，更新車(chē)輛的位姿。

實(shí)驗(yàn)過(guò)程中，車(chē)輛在停車(chē)場(chǎng)中的平均行駛速度為1 m·s-1。當(dāng)?shù)钚【嚯xmin_d設(shè)置為20 cm時(shí)，粒子濾波器在理想狀態(tài)下應(yīng)為每秒輸出5次最佳估計(jì)位姿；當(dāng)最小迭代距離min_d設(shè)置為5 cm時(shí)，應(yīng)為每秒輸出20次最佳估計(jì)位姿。由于計(jì)算需要時(shí)間，因此實(shí)際的位姿更新頻率一般都會(huì)低于理論頻率。

表2展示了車(chē)輛速度v為1 m·s-1、迭代最小距離min_d分別為20、15、5 cm時(shí)理論位姿更新頻率與實(shí)際位姿更新頻率的關(guān)系。所有計(jì)算均基于一臺(tái)6核CPU/內(nèi)存16G的i7 8750H電腦，其中操作系統(tǒng)是Ubuntu 16.04Lts版本，ROS為Kinetic。測(cè)試車(chē)輛為一臺(tái)改裝過(guò)的帕薩特，搭載6個(gè)ibeo LUX 4L激光雷達(dá)。

表2 位姿更新率Tab.2 Renewal rate of pose

由表2可知，粒子濾波器定位的實(shí)際更新頻率與理論更新頻率相差很小，能滿足車(chē)輛定位實(shí)時(shí)性的要求。實(shí)際汽車(chē)自主定位中，激光雷達(dá)的定位位姿還要與其他高頻的定位位姿，如視覺(jué)里程計(jì)、輪速里程計(jì)等融合，位姿刷新頻率還會(huì)得到進(jìn)一步提高。

4 結(jié)語(yǔ)

以自動(dòng)駕駛中基于激光雷達(dá)的高精度定位為目標(biāo)，在GPS信號(hào)缺失的室內(nèi)環(huán)境下，通過(guò)激光雷達(dá)進(jìn)行車(chē)輛高精度定位。從激光雷達(dá)SLAM入手對(duì)環(huán)境進(jìn)行建圖，將激光雷達(dá)的點(diǎn)云匹配與后端圖優(yōu)化轉(zhuǎn)化為非線性?xún)?yōu)化問(wèn)題，通過(guò)高斯-牛頓迭代法進(jìn)行求解。在得到精確的概率占柵格地圖后，通過(guò)粒子濾波器對(duì)車(chē)輛進(jìn)行定位，同時(shí)結(jié)合KLD采樣等方法優(yōu)化粒子濾波器中的粒子采樣數(shù)量，實(shí)現(xiàn)了粒子采樣數(shù)量的動(dòng)態(tài)管理，進(jìn)而縮減了計(jì)算時(shí)間。從實(shí)車(chē)測(cè)試結(jié)果可以看到，車(chē)輛定位的位置誤差以及角度誤差能夠得到很好的控制，算法的計(jì)算復(fù)雜度較低，在一臺(tái)普通配置的筆記本上也能夠?qū)崿F(xiàn)低延遲的實(shí)時(shí)定位。在保證定位精度的同時(shí)，實(shí)現(xiàn)了在線定位的實(shí)時(shí)性要求。該定位方法具有較強(qiáng)的魯棒性，在行車(chē)過(guò)程中出現(xiàn)打滑等現(xiàn)象而導(dǎo)致誤差的增加，粒子濾波器也能夠很好地在后續(xù)的迭代過(guò)程中消除誤差。

作者貢獻(xiàn)說(shuō)明：

周 蘇：擬定研究方向和內(nèi)容，提供指導(dǎo)與規(guī)劃項(xiàng)目進(jìn)度，論文修改及定稿。

李偉嘉：進(jìn)行算法研究、實(shí)驗(yàn)測(cè)試、數(shù)據(jù)采集、論文撰寫(xiě)。

郭軍華：提供咨詢(xún)，幫助知識(shí)框架梳理，指導(dǎo)論文撰寫(xiě)，修改論文。