訾 燁 任明武

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

近年來，隨著信息工業(yè)的不斷進(jìn)步，無人駕駛得到了密切關(guān)注和極大發(fā)展。在無人駕駛領(lǐng)域中，一個(gè)標(biāo)識(shí)準(zhǔn)確且包含豐富信息的地圖是十分重要的，它能夠幫助無人車定位和配準(zhǔn)，并作為后續(xù)路徑規(guī)劃的基礎(chǔ)。目前使用最廣泛的地圖構(gòu)建、匹配、定位與導(dǎo)航技術(shù)是基于全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）與慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）相結(jié)合、并輔以動(dòng)態(tài)實(shí)時(shí)差分方法（Real-time kinematic，RTK）進(jìn)行誤差修正的技術(shù)［1］。但是，由于以GNSS技術(shù)為基礎(chǔ)的導(dǎo)航只精確到米級(jí)，且極易受到地形及衛(wèi)星信號(hào)的影響，人們渴望構(gòu)建一種更精準(zhǔn)、更穩(wěn)定且包含更豐富語義信息的高精度地圖。

高精度地圖是指精細(xì)化定義的地圖，其精度需要達(dá)到分米級(jí)才能夠區(qū)分各個(gè)車道。而精細(xì)化定義，則需要格式化存儲(chǔ)交通場(chǎng)景中的各種交通要素，包括傳統(tǒng)地圖的道路網(wǎng)數(shù)據(jù)、車道線和交通標(biāo)志等數(shù)據(jù)?；诟呔鹊貓D的匹配定位作為近年來無人駕駛領(lǐng)域研究的熱門問題，已經(jīng)積累了一定經(jīng)驗(yàn)，提出了多種用于高精度地圖匹配定位的算法。其大致可以分為基于地圖特征的匹配定位和基于掃描的匹配定位兩種?；诘貓D特征的匹配通常需要向地圖中添加特定的語義特征信息，本質(zhì)上仍然是概率統(tǒng)計(jì)，且在極大程度依賴GNSS所提供的米級(jí)精度的數(shù)據(jù)。為了解決這一問題，基于掃描的匹配定位方法越來越得到重視?；趻呙璧钠ヅ涠ㄎ环椒ㄒ蟾呔鹊貓D中包含構(gòu)建地圖時(shí)的點(diǎn)云數(shù)據(jù)，通過比較當(dāng)前掃描點(diǎn)云和地圖點(diǎn)云確定車輛相對(duì)于地圖的姿態(tài)。掃描匹配通過點(diǎn)的匹配來完成定位，其中最經(jīng)典的算法是迭代就近點(diǎn)算法（Iterative Closest Point，ICP）［2］。但是，ICP算法有運(yùn)算時(shí)間長(zhǎng)、對(duì)初始姿態(tài)要求高、容易陷入局部最優(yōu)等缺點(diǎn)。為解決這一問題，2003年，P.Biber和W.Strasser等提出了正態(tài)分布變換匹配算法（Normal Distributions Transform，NDT）［3］。由于NDT算法不利用對(duì)應(yīng)點(diǎn)的特征進(jìn)行計(jì)算，所以其運(yùn)算速度比ICP快，且不會(huì)陷入局部最優(yōu)。

同時(shí)，由于NDT算法會(huì)輸出在地圖的不同點(diǎn)處的匹配準(zhǔn)確率，該匹配準(zhǔn)確率在一定程度上會(huì)影響基于該地圖的路徑規(guī)劃。因此，要想提高基于高精度地圖的路徑規(guī)劃的置信度，必須在路徑規(guī)劃中考慮到匹配與定位準(zhǔn)確率的問題。傳統(tǒng)的路徑規(guī)劃算法可以分為圖搜索法、人工勢(shì)場(chǎng)法、隨機(jī)地圖法和智能化算法四類。每一類方法自成體系，都各有其優(yōu)缺點(diǎn)?？紤]到代價(jià)函數(shù)的靈活性和應(yīng)用場(chǎng)景的廣泛性，以A*算法［4］和蟻群算法［5］為代表的智能化算法得到了越來越多的重視，逐漸成為主流使用的路徑規(guī)劃算法。

本文擬采用致密的激光雷達(dá)點(diǎn)云數(shù)據(jù)作為高精度地圖的數(shù)據(jù)來源；然后使用NDT算法進(jìn)行匹配與定位，并輸出匹配準(zhǔn)確率；最后將匹配準(zhǔn)確率與局部路徑規(guī)劃算法的代價(jià)函數(shù)相結(jié)合，完成局部路徑規(guī)劃。

2 算法描述

2.1 NDT算法

2003年，P.Biber和W.Strasser等提出了一種范圍掃描的替代表示形式，即為正態(tài)分布變換（Nor?mal Distributions Transform，NDT）［3］，該分布在本地模擬測(cè)量點(diǎn)的概率。轉(zhuǎn)換的結(jié)果是分段的連續(xù)且可微的概率密度，可以使用牛頓算法將其用于匹配另一次掃描。2006年，Takashi Tsubouchi等提出了一種將2D-NDT掃描匹配方法擴(kuò)展到3D掃描匹配的方法，首次提出了3D-NDT的概念并對(duì)其進(jìn)行了改進(jìn)以加快處理時(shí)間［6］。2010年，Todor Stoyanov等在3D-NDT的基礎(chǔ)上提出了一種路徑規(guī)劃的新穎方法，該方法修改了著名的波前計(jì)劃器，以使用3D-NDT作為地圖表示的基礎(chǔ)，并使用室內(nèi)和室外數(shù)據(jù)集進(jìn)行評(píng)估［7］。2013年，Jari Saarinen等提出了一種新穎的3D映射方法-正態(tài)分布變換占用圖（NDT-OM），來解決現(xiàn)實(shí)世界地圖的挑戰(zhàn)［8］。

NDT算法的核心思想是把二維或三維點(diǎn)云數(shù)據(jù)集轉(zhuǎn)化為連續(xù)可微的、多維變量的正態(tài)分布函數(shù)。NDT算法首先把參考點(diǎn)云（reference scan）所在的空間劃分為指定均勻大小或形狀的網(wǎng)格，并分別計(jì)算每個(gè)網(wǎng)格的正態(tài)分布參數(shù)均值μ和協(xié)方差矩陣∑：

得到每個(gè)點(diǎn)的概率分布后，就可以根據(jù)正態(tài)分布參數(shù)計(jì)算每個(gè)點(diǎn)的概率密度，繼而得到NDT配準(zhǔn)得分（score），判斷配準(zhǔn)準(zhǔn)確率。

2.2 A*算法

A*算法是一種運(yùn)用最廣泛的啟發(fā)式搜索算法。A*算法的基本理論是計(jì)算當(dāng)前節(jié)點(diǎn)可能到達(dá)的每個(gè)相鄰子節(jié)點(diǎn)的評(píng)估值。當(dāng)路徑搜索過程擴(kuò)展到某節(jié)點(diǎn)時(shí)，需要將該節(jié)點(diǎn)與所有已擴(kuò)展的節(jié)點(diǎn)進(jìn)行比較。如果該節(jié)點(diǎn)是擴(kuò)展節(jié)點(diǎn)，則表明已找到到達(dá)該節(jié)點(diǎn)的新路徑；如果新路徑使該節(jié)點(diǎn)評(píng)估值較小，則必須修改該節(jié)點(diǎn)使它指向新路徑的新父節(jié)點(diǎn)。最后，該算法選擇最小評(píng)估節(jié)點(diǎn)以按順序展開并將其放入封閉表中。

算法的基本步驟描述如下。

1）將地圖轉(zhuǎn)化為柵格化的網(wǎng)格表示；

2）首先設(shè)置open列表和closed列表，open列表用于記錄所有被考慮用來尋找最短路徑的節(jié)點(diǎn)，closed列表用于記錄不會(huì)再被考慮的節(jié)點(diǎn)；

3）計(jì)算各個(gè)節(jié)點(diǎn)i的評(píng)估函數(shù)f(i)=g(i)+h(i)，其中g(shù)(i)代表從起點(diǎn)到當(dāng)前點(diǎn)i的移動(dòng)量，h(i)表示從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的移動(dòng)量估算值，該估算值是不確定的，有可能受到地圖上其他因素的影響；

4）將起點(diǎn)s加入open列表，并查看所有與其相鄰的節(jié)點(diǎn)，選擇其中可走的（walkable）或可到達(dá)的（reachable）節(jié)點(diǎn)也加入open列表；

5）選擇open列表中的評(píng)估函數(shù)值f(i)最小的節(jié)點(diǎn)，將其從open表中刪除，加入closed表，并檢查其相鄰的所有節(jié)點(diǎn)。若有節(jié)點(diǎn)滿足在closed表中是不可走的unwalkable，則忽略它，否則轉(zhuǎn)6）；

6）若該節(jié)點(diǎn)不在open表中，則將它加入open表，將當(dāng)前節(jié)點(diǎn)設(shè)置為它的父親節(jié)點(diǎn)，并計(jì)算f(i)，否則轉(zhuǎn)7）；

7）若該節(jié)點(diǎn)已在open表中，則檢查這條路徑是否是更好的路徑（若其g(i)更小，則代表這條新路徑更優(yōu)），若是則將其父親節(jié)點(diǎn)設(shè)置為當(dāng)前節(jié)點(diǎn)，并重新計(jì)算其g(i)；

8）重復(fù)步驟5）～8），直至將終點(diǎn)加入open表或open表已空。前者輸出最優(yōu)路徑，后者表示沒有最優(yōu)路徑。

3 基于NDT算法的地圖匹配及同步路徑規(guī)劃

基于NDT算法的地圖匹配及同步路徑規(guī)劃算法的步驟如下圖所示。具體步驟可描述為：1）首先將處理過的高精度地圖網(wǎng)格化；2）使用NDT算法完成地圖匹配與定位；3）最后基于地圖匹配輸出的匹配準(zhǔn)確率，使用A*算法進(jìn)行局部路徑規(guī)劃。

圖1 本文改進(jìn)算法流程圖

3.1 高精度地圖網(wǎng)格化

網(wǎng)格數(shù)據(jù)是以規(guī)則的陣列來表示空間地物或現(xiàn)象分布的數(shù)據(jù)組織。每個(gè)網(wǎng)格代表一個(gè)至多個(gè)像素，并包含能夠表示該像素的數(shù)據(jù)類型或數(shù)量。一般來說，網(wǎng)格劃分大小越大，包含的像素個(gè)數(shù)越多，表達(dá)的語義信息越少；網(wǎng)格劃分大小越小，其包含的像素個(gè)數(shù)越少，表達(dá)的語義信息越多［9］。因此，在定義網(wǎng)格大小時(shí)，應(yīng)當(dāng)平衡每個(gè)網(wǎng)格包含的信息數(shù)量和信息精確度的關(guān)系，選擇合適的網(wǎng)格大小。

本文使用八叉樹（Octomap）的格式進(jìn)行地圖的存儲(chǔ)。首先使用三維激光掃描儀獲取環(huán)境的三維點(diǎn)云地圖，然后將其轉(zhuǎn)換為八叉樹數(shù)據(jù)結(jié)構(gòu)。八叉樹是一種樹形的數(shù)據(jù)存儲(chǔ)結(jié)構(gòu)，除了空樹外，八叉樹中的每個(gè)節(jié)點(diǎn)都有八個(gè)或零個(gè)子節(jié)點(diǎn)。八叉樹的格式存儲(chǔ)方便，節(jié)省空間，且有利于使用二分法進(jìn)行查找。對(duì)三維八叉樹地圖進(jìn)行任意一個(gè)方向的投影，即可得到二維占據(jù)柵格地圖［10］。

3.2 基于NDT算法的地圖匹配

本文使用基于NDT的點(diǎn)云匹配算法完成地圖的匹配與定位。NDT算法中尤其需要重視的是四個(gè)參數(shù)的設(shè)置，分別是最小轉(zhuǎn)換差異（Transforma?tion Epsilon）、搜索的最大步長(zhǎng)（Step Size）、網(wǎng)格結(jié)構(gòu)的分辨率（Resolution）、匹配迭代的最大次數(shù)（Maximum Iterations）［11］。

NDT網(wǎng)格結(jié)構(gòu)的分辨率即為均分的網(wǎng)格大小，網(wǎng)格的大小設(shè)置應(yīng)合理，能夠幫助計(jì)算和優(yōu)化網(wǎng)格內(nèi)每一點(diǎn)的存在概率。網(wǎng)格精度一般以為數(shù)據(jù)精度的3～5倍為宜，在本文中網(wǎng)格大小設(shè)置為5cm*5cm大小。

NDT算法使用More-Thuente線搜索用以確定搜索的最大步長(zhǎng)，該方法試圖使用包圍以及二次和三次插值找到滿足下降條件和曲率條件的一個(gè)點(diǎn)，用來確定最大步長(zhǎng)值范圍內(nèi)的最佳步長(zhǎng)。最小變換差異參數(shù)一般用作算法的終止條件，分別從長(zhǎng)度和弧度定義了變換矩陣的最小許可遞增量，一旦遞增量減小到這個(gè)臨界值以下，那么配準(zhǔn)將終止［12～13］。

算法的基本步驟描述如下：

1）首先，將參考點(diǎn)云網(wǎng)格化，并計(jì)算每個(gè)網(wǎng)格的多維正態(tài)分布參數(shù)均值和協(xié)方差矩陣∑；

2）初始化變換參數(shù)p=(tx.ty，φ)T；

3）通過變換T將待配準(zhǔn)的點(diǎn)云轉(zhuǎn)換到參考點(diǎn)云的網(wǎng)格中：其中，表示待配準(zhǔn)的點(diǎn)云網(wǎng)格內(nèi)所有掃描

點(diǎn)，變換T可描述為

5）NDT的配準(zhǔn)得分（score）為每個(gè)網(wǎng)絡(luò)計(jì)算出的概率密度之和：

6）使用Hessian矩陣和牛頓迭代法對(duì)score(p)求最小值，利用如下函數(shù)：

其中g(shù)是score(p)的轉(zhuǎn)置梯度，H是score(p)的Hessian矩陣。

7）重復(fù)步驟3）～6），直至達(dá)到收斂條件為準(zhǔn)。

結(jié)束匹配后，可以輸出NDT的配準(zhǔn)得分值（score）對(duì)應(yīng)的匹配準(zhǔn)確率作為輸出，引導(dǎo)后續(xù)的路徑規(guī)劃。

3.3 基于匹配誤差的同步路徑規(guī)劃

本文使用A*算法進(jìn)行基于匹配誤差的局部路徑規(guī)劃。A*是一種應(yīng)用十分廣泛的啟發(fā)式搜索算法，它依賴啟發(fā)信息構(gòu)造啟發(fā)函數(shù)，并在規(guī)劃路徑時(shí)對(duì)所訪問或待訪問的節(jié)點(diǎn)進(jìn)行代價(jià)評(píng)價(jià)，選擇其中代價(jià)最小的、同時(shí)可滿足要求的節(jié)點(diǎn)進(jìn)行路徑規(guī)劃［14］。

由于在高精度地圖的不同點(diǎn)處的匹配誤差也有所不同，將匹配誤差作為計(jì)算代價(jià)的一部分是很有必要的［15］。如果該點(diǎn)的匹配誤差大，則認(rèn)為該點(diǎn)附近區(qū)域的局部地圖置信度較低，該區(qū)域內(nèi)的障礙物對(duì)路徑規(guī)劃帶來的風(fēng)險(xiǎn)就大；反之，若該點(diǎn)匹配誤差小，對(duì)該區(qū)域內(nèi)的障礙物的描述和判斷也更加準(zhǔn)確。因此，本文將NDT匹配算法輸出的匹配準(zhǔn)確率作為A*算法代價(jià)函數(shù)的輸入，輔助路徑規(guī)劃代價(jià)評(píng)估。

在A*算法中，標(biāo)準(zhǔn)的啟發(fā)函數(shù)使用歐氏距離，從u點(diǎn)到v點(diǎn)的歐式距離應(yīng)表示為

則評(píng)估函數(shù)中從u點(diǎn)移動(dòng)到v點(diǎn)的實(shí)際移動(dòng)距離G(u，v)應(yīng)為從u點(diǎn)到v點(diǎn)的距離D(u，v)與v點(diǎn)的權(quán)重w(v)的乘積，表示為

為使用NDT匹配算法輸出的匹配準(zhǔn)確率來指導(dǎo)路徑規(guī)劃，本文將該值引入A*代價(jià)函數(shù)中的某點(diǎn)的權(quán)重值w(i)，i點(diǎn)處準(zhǔn)確率越高，說明原始點(diǎn)云和待配準(zhǔn)點(diǎn)云越相似，配準(zhǔn)效果越好，經(jīng)過i點(diǎn)的代價(jià)越小。因此，w(i)與該點(diǎn)的匹配準(zhǔn)確率p之間的關(guān)系可表示為

因此，本文中使用的A*代價(jià)函數(shù)可表示為

4 仿真實(shí)驗(yàn)結(jié)果與誤差分析

為了驗(yàn)證NDT算法對(duì)于高精度地圖的匹配的適用程度，并同時(shí)評(píng)估使用地圖匹配誤差指導(dǎo)的局部路徑規(guī)劃算法的有效性，本文分別對(duì)地圖匹配算法和路徑規(guī)劃算法進(jìn)行了仿真實(shí)驗(yàn)和評(píng)估。

4.1 基于NDT的高精度地圖匹配

本文使用致密的高精度點(diǎn)云數(shù)據(jù)作為高精度地圖的數(shù)據(jù)源，將全局地圖記為scan1（如圖2所示），并將其轉(zhuǎn)化為用八叉樹結(jié)構(gòu)表示的三維八叉樹地圖，向地面投影，去除噪點(diǎn)后得到俯視的二維網(wǎng)格圖（如圖3）。在序列的局部點(diǎn)云圖像中任取5幀（分別記為scan2-1至2-5，以scan2-3為代表表示為圖4）與全局地圖進(jìn)行匹配，從而確定局部地圖在全局地圖中的位置，進(jìn)而確定該區(qū)域在二維網(wǎng)格圖中的位置（如圖5）。

圖2 全局點(diǎn)云圖像scan1

圖3 scan1轉(zhuǎn)化的全局二維網(wǎng)格圖像

圖4 局部點(diǎn)云圖像scan2-3

圖5 scan2-3對(duì)應(yīng)的二維網(wǎng)格區(qū)域

圖6 中列出了實(shí)驗(yàn)所選取的每幀對(duì)應(yīng)的匹配準(zhǔn)確率，可以看出，在使用同一套NDT算法參數(shù)的情況下，不同場(chǎng)景的匹配準(zhǔn)確率是不同的。

圖6 匹配準(zhǔn)確率變化展示

4.2 基于地圖匹配誤差的局部路徑規(guī)劃

本文分別對(duì)基礎(chǔ)的A*算法和使用地圖匹配誤差進(jìn)行改進(jìn)的A*算法在簡(jiǎn)化的scan2-3對(duì)應(yīng)二維網(wǎng)格圖中進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果分別如圖7（a）和（b）所示。

圖7 原始A*算法與本文的改進(jìn)A*算法實(shí)驗(yàn)結(jié)果圖

表1 中列出了A*與改進(jìn)A*兩種算法在仿真實(shí)驗(yàn)中的性能指標(biāo)衡量結(jié)果，該表列出的三項(xiàng)指標(biāo)為50次實(shí)驗(yàn)的平均值。在表中，運(yùn)行時(shí)間表示算法在地圖中遍歷尋找最優(yōu)路徑的時(shí)間、危險(xiǎn)網(wǎng)格數(shù)表示障礙物周圍危險(xiǎn)系數(shù)大于0.5（代價(jià)w大于2）的網(wǎng)格個(gè)數(shù)、潛在危險(xiǎn)程度表示規(guī)劃出的路徑經(jīng)過危險(xiǎn)網(wǎng)格的個(gè)數(shù)。

表1 A*與本文改進(jìn)A*的結(jié)果評(píng)價(jià)

從圖7（a）、（b）及表2中數(shù)據(jù)可以看出，使用地圖匹配誤差進(jìn)行改進(jìn)的A*算法在基本不增加遍歷時(shí)間的條件下，明顯降低了規(guī)劃出路徑的潛在危險(xiǎn)程度，使得無人車在行駛過程中能夠更加穩(wěn)定。

改進(jìn)算法對(duì)障礙物周邊網(wǎng)格危險(xiǎn)程度的判斷主要基于地圖匹配誤差的準(zhǔn)確率。若準(zhǔn)確率越高，匹配誤差越小，障礙物周邊網(wǎng)格的危險(xiǎn)程度越小。圖8（a）為匹配準(zhǔn)確率0.8331的局部地圖，圖8（b）為匹配準(zhǔn)確率0.7594的局部地圖，可以看出，算法在圖8（a）中障礙物輻射的危險(xiǎn)范圍明顯小于圖8（b），在（a）中無人車經(jīng)過障礙物周邊網(wǎng)格的代價(jià)也低于（b）。

圖8 不同匹配誤差下的局部路徑規(guī)劃情況

5 結(jié)語

高精度地圖作為一種表達(dá)道路信息的更為精確、包含語音信息更豐富的形式，在當(dāng)今的無人駕駛領(lǐng)域起到越來越重要的作用。因此，研究基于高精度地圖的地圖匹配及同步路徑規(guī)劃也是有其重要意義的。本文提出了一種將基于高精度地圖的匹配誤差引入路徑規(guī)劃算法的路徑規(guī)劃方法。實(shí)驗(yàn)證明，這種方法在保證路徑規(guī)劃效率的同時(shí)，也使得全局路徑規(guī)劃更加合理，避開更多的障礙，付出更少的代價(jià)。