一種基于高精度地圖匹配誤差的路徑規劃方法*

訾 燁 任明武

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

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

高精度地圖是指精細化定義的地圖，其精度需要達到分米級才能夠區分各個車道。而精細化定義，則需要格式化存儲交通場景中的各種交通要素，包括傳統地圖的道路網數據、車道線和交通標志等數據。基于高精度地圖的匹配定位作為近年來無人駕駛領域研究的熱門問題，已經積累了一定經驗，提出了多種用于高精度地圖匹配定位的算法。其大致可以分為基于地圖特征的匹配定位和基于掃描的匹配定位兩種。基于地圖特征的匹配通常需要向地圖中添加特定的語義特征信息，本質上仍然是概率統計，且在極大程度依賴GNSS所提供的米級精度的數據。為了解決這一問題，基于掃描的匹配定位方法越來越得到重視。基于掃描的匹配定位方法要求高精度地圖中包含構建地圖時的點云數據，通過比較當前掃描點云和地圖點云確定車輛相對于地圖的姿態。掃描匹配通過點的匹配來完成定位，其中最經典的算法是迭代就近點算法（Iterative Closest Point，ICP）［2］。但是，ICP算法有運算時間長、對初始姿態要求高、容易陷入局部最優等缺點。為解決這一問題，2003年，P.Biber和W.Strasser等提出了正態分布變換匹配算法（Normal Distributions Transform，NDT）［3］。由于NDT算法不利用對應點的特征進行計算，所以其運算速度比ICP快，且不會陷入局部最優。

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

本文擬采用致密的激光雷達點云數據作為高精度地圖的數據來源；然后使用NDT算法進行匹配與定位，并輸出匹配準確率；最后將匹配準確率與局部路徑規劃算法的代價函數相結合，完成局部路徑規劃。

2 算法描述

2.1 NDT算法

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

NDT算法的核心思想是把二維或三維點云數據集轉化為連續可微的、多維變量的正態分布函數。NDT算法首先把參考點云（reference scan）所在的空間劃分為指定均勻大小或形狀的網格，并分別計算每個網格的正態分布參數均值μ和協方差矩陣∑：

得到每個點的概率分布后，就可以根據正態分布參數計算每個點的概率密度，繼而得到NDT配準得分（score），判斷配準準確率。

2.2 A*算法

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

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

1）將地圖轉化為柵格化的網格表示；

2）首先設置open列表和closed列表，open列表用于記錄所有被考慮用來尋找最短路徑的節點，closed列表用于記錄不會再被考慮的節點；

3）計算各個節點i的評估函數f(i)=g(i)+h(i)，其中g(i)代表從起點到當前點i的移動量，h(i)表示從當前點到目標點的移動量估算值，該估算值是不確定的，有可能受到地圖上其他因素的影響；

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

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

6）若該節點不在open表中，則將它加入open表，將當前節點設置為它的父親節點，并計算f(i)，否則轉7）；

7）若該節點已在open表中，則檢查這條路徑是否是更好的路徑（若其g(i)更小，則代表這條新路徑更優），若是則將其父親節點設置為當前節點，并重新計算其g(i)；

8）重復步驟5）～8），直至將終點加入open表或open表已空。前者輸出最優路徑，后者表示沒有最優路徑。

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

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

圖1 本文改進算法流程圖

3.1 高精度地圖網格化

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

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

3.2 基于NDT算法的地圖匹配

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

NDT網格結構的分辨率即為均分的網格大小，網格的大小設置應合理，能夠幫助計算和優化網格內每一點的存在概率。網格精度一般以為數據精度的3～5倍為宜，在本文中網格大小設置為5cm*5cm大小。

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

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

1）首先，將參考點云網格化，并計算每個網格的多維正態分布參數均值和協方差矩陣∑；

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

3）通過變換T將待配準的點云轉換到參考點云的網格中：其中，表示待配準的點云網格內所有掃描

點，變換T可描述為

5）NDT的配準得分（score）為每個網絡計算出的概率密度之和：

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

其中g是score(p)的轉置梯度，H是score(p)的Hessian矩陣。

7）重復步驟3）～6），直至達到收斂條件為準。

結束匹配后，可以輸出NDT的配準得分值（score）對應的匹配準確率作為輸出，引導后續的路徑規劃。

3.3 基于匹配誤差的同步路徑規劃

本文使用A*算法進行基于匹配誤差的局部路徑規劃。A*是一種應用十分廣泛的啟發式搜索算法，它依賴啟發信息構造啟發函數，并在規劃路徑時對所訪問或待訪問的節點進行代價評價，選擇其中代價最小的、同時可滿足要求的節點進行路徑規劃［14］。

由于在高精度地圖的不同點處的匹配誤差也有所不同，將匹配誤差作為計算代價的一部分是很有必要的［15］。如果該點的匹配誤差大，則認為該點附近區域的局部地圖置信度較低，該區域內的障礙物對路徑規劃帶來的風險就大；反之，若該點匹配誤差小，對該區域內的障礙物的描述和判斷也更加準確。因此，本文將NDT匹配算法輸出的匹配準確率作為A*算法代價函數的輸入，輔助路徑規劃代價評估。

在A*算法中，標準的啟發函數使用歐氏距離，從u點到v點的歐式距離應表示為

則評估函數中從u點移動到v點的實際移動距離G(u，v)應為從u點到v點的距離D(u，v)與v點的權重w(v)的乘積，表示為

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

因此，本文中使用的A*代價函數可表示為

4 仿真實驗結果與誤差分析

為了驗證NDT算法對于高精度地圖的匹配的適用程度，并同時評估使用地圖匹配誤差指導的局部路徑規劃算法的有效性，本文分別對地圖匹配算法和路徑規劃算法進行了仿真實驗和評估。

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

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

圖2 全局點云圖像scan1

圖3 scan1轉化的全局二維網格圖像

圖4 局部點云圖像scan2-3

圖5 scan2-3對應的二維網格區域

圖6 中列出了實驗所選取的每幀對應的匹配準確率，可以看出，在使用同一套NDT算法參數的情況下，不同場景的匹配準確率是不同的。

圖6 匹配準確率變化展示

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

本文分別對基礎的A*算法和使用地圖匹配誤差進行改進的A*算法在簡化的scan2-3對應二維網格圖中進行仿真實驗，實驗結果分別如圖7（a）和（b）所示。

圖7 原始A*算法與本文的改進A*算法實驗結果圖

表1 中列出了A*與改進A*兩種算法在仿真實驗中的性能指標衡量結果，該表列出的三項指標為50次實驗的平均值。在表中，運行時間表示算法在地圖中遍歷尋找最優路徑的時間、危險網格數表示障礙物周圍危險系數大于0.5（代價w大于2）的網格個數、潛在危險程度表示規劃出的路徑經過危險網格的個數。

表1 A*與本文改進A*的結果評價

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

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

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

5 結語

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