基于圖優化的室外無人車多圖層地圖構建*

牛國臣，馮 寧，王 瑜

（中國民航大學機器人研究所，天津 300300）

前言

在過去的幾十年里，同時定位與建圖（SLAM）的應用領域受到了越來越多的關注，尤其是在無人駕駛領域。而地圖構建是實現無人駕駛的基本前提，目前典型的地圖主要有4 種：拓撲地圖、柵格地圖、點云地圖、語義地圖。

不同類型的地圖在無人駕駛自主導航方面各有優缺點。拓撲地圖和柵格地圖易于路徑規劃。Wang 等提出一種雙層路徑規劃算法，分別在拓撲層和路由層進行路徑規劃。Jubair 等在柵格地圖中尋找源節點到單個目標節點的最短路徑。Sepehrifar 和Asaduzzaman 等解決了柵格地圖中多目標點的最短路徑搜索問題。但由于拓撲和柵格地圖缺乏足夠的3D特征信息，往往難以用于定位。

點云地圖容易獲得足夠的特征信息，但在室外場景下，地圖的數據量很大且存在動態物體，會給定位的準確性和實時性帶來很大挑戰。近年來，由于激光雷達傳感器具有測量精度高、可靠性好、不易受光照變化影響等優點，激光SLAM 在無人駕駛領域的應用越來越廣泛。Zhang 等提出一種低漂移實時的雷達定位和建圖方法（LOAM），通過計算每個點的曲率來提取特征點，考慮到系統的實時性，采取高頻里程計和低頻建圖相結合的方法，但由于缺少后端優化，導致所建地圖準確性較差。Ji 等在LOAM 的基礎上，加入GPS 約束來消除部分累積誤差。Liu等利用地平面約束減小匹配誤差。Holder等提出一種基于實時位姿圖的SLAM 系統。最近，Le等在后端提出一個優化問題對LIDAR運動進行補償，相比以前的工作，后端處理方法取得了更好的性能，但是不能實時運行。

語義地圖可以為人機交互提供環境語義信息，有效提升對環境的理解能力，但語義信息提取復雜。Chen 等采用RangeNet++的方法提取語義信息，濾除建圖過程中的動態物體。Bavle 等提出一種視覺語義SLAM 方法。Erik 等利用語義信息進行定位，節省了存儲空間，但是不適用于單一環境。Qin等提出了一個基于視覺的定位解決方案，利用魯棒的語義特征來輔助車輛在停車場中導航，但是只適用于停車場。而用于無人駕駛的高精度地圖需要昂貴的測繪系統和人工標注等復雜的后處理步驟，成本昂貴。

為解決上述問題，有必要將不同類型的地圖組合到一起。Bücken 等提出一種結合柵格和拓撲的組合地圖用來進行室內導航，但在室外環境下，無法進行精確定位。Liu 等提出一種融合拓撲地圖和柵格地圖的方法，解決了機器人在未知環境中的自動探索問題，但其選定的候選目標點是二維，缺乏三維空間信息。Galindo 等利用語義信息標記拓撲地圖，進行導航。Richter 和Yu 等提出語義柵格地圖。Cailhol 等結合語義和拓撲地圖，提高了人機交互能力。Erik 等構建點云語義地圖，打破傳統點云匹配定位方法，利用環境語義信息定位，但實現路徑規劃比較困難。

基于上述討論，本文中提出一種適用于無人駕駛的室外多圖層地圖。區別于傳統單一地圖，該地圖包含有拓撲、點云、柵格和動態數據的多層環境信息。動態層實時點云信息與點云地圖匹配，并融合拓撲地圖的絕對位置信息，實現精確、實時且魯棒的無人車定位；拓撲和柵格地圖以及對動態層檢測的實時障礙物數據，為無人車的路徑規劃和避障等功能提供豐富的環境信息。為驗證提出的地圖，在實際環境下進行實驗。實驗表明，該地圖在大規模的園區環境中也能很好地工作。

1 系統概述

1.1 多圖層地圖構建

本文主要面向園區內L4 級別的無人駕駛，園區環境具有車速較低、車流量較少、障礙物較少且多為靜態障礙物的特點。實驗平臺為改裝長城歐拉電動汽車，如圖1 所示，配備有北科天繪的16 線激光雷達，華測的組合導航和毫米波雷達等傳感器。地圖體系結構如圖2 所示，由軌跡層、靜態層和動態層組成。組合導航路徑點構建軌跡層，激光SLAM 生成點云和柵格地圖，共同構建靜態層，動態層包含激光雷達的實時點云。

圖1 實驗平臺

圖2 地圖體系結構

1.2 基于多圖層地圖的定位和障礙物檢測

基于多圖層地圖的定位方法如圖3（a）所示，根據軌跡層的GNSS 位置信息給定無人車初始位置，并實現靜態層點云地圖的分塊動態加載，然后將動態層的實時點云信息與局部點云地圖做NDT匹配定位。當點云定位偏差超過閾值時，將使用GNSS 位置信息進行重定位，輸出無人車精確位姿。

圖3（b）顯示障礙物的檢測方法，根據動態層的實時信息，結合定位輸出的無人車位姿實現障礙物的實時檢測。將檢測的障礙物同靜態層柵格地圖結合，為無人車的局部路徑規劃和避障提供環境障礙物信息。

圖3 定位和障礙物檢測

2 多圖層地圖構建

2.1 軌跡層

軌跡層包含了由GNSS 路徑點組成的拓撲地圖，為無人車的全局路徑規劃提供信息。根據GNSS位置信息給定無人車的初始位置，并實現點云地圖的分塊動態加載。

通過將組合導航原始數據的經緯度信息轉換到WGS-84 坐標系下，計算每個坐標相對于初始坐標的位置，得到運動軌跡，并采用g2o 庫生成拓撲地圖。

2.2 靜態層

2.2.1 點云地圖

根據激光SLAM 輸出得到最優軌跡，將其對應的點云幀拼接生成三維點云地圖，因此位姿的精確度影響地圖的質量。實驗平臺所使用的傳感器是16 線激光雷達，數據較為稀疏，所以后端采用圖優化方法，通過將地平面約束、GNSS約束作為一元邊，聚類特征作為閉環約束加入到圖中，在后端優化得到精確位姿。

（1）圖優化

圖由“頂點”和“邊”組成，“頂點”為車輛位姿，“邊”為位姿間的約束。圖的結構如圖4所示。

圖4 圖優化結構

（2）地平面約束

地面點云是對地面車輛位姿估計的良好約束。設定無人車前進方向為軸，軸指向左側，軸指向上方，通過將三維點云降到二維平面，可以根據激光雷達投影到地面的射線中前后兩點的坡度是否大于事先設定的坡度閾值來判斷點是否為地面點。

為輸入點云，對任一點，∈，有

式中：x、y為點在LIDAR 坐標系下的坐標位置；r為點到LIDAR的水平距離。

按照水平距離排序，計算同一射線上相鄰兩點的坡度：

式中：z為點的垂直高度；d為點和+ 1點的坡度；為LIDAR的安裝高度。

將式（2）的坡度與所設定的閾值作比較，當d＜時，將點歸為地面點。

完成地平面的檢測后，根據時刻的車輛位姿，估計地平面：

式中：[n，n，n，]=[0，0，1，0]為初始時刻平面；T為時刻的車輛位姿；′、′、′、′為時刻車輛位姿估計的地平面′。

采用最小參數法，定義平面參數表達式：

式中：arctan(n/n)為平面方位角；arctan(n/||)為平面仰角；為截距的長度。

則位姿節點和地平面節點之間的誤差為

（3）GNSS約束

將位姿節點和GNSS 數據的時間戳對齊，GNSS位置可作為先驗位置信息，成為車輛位姿節點的一元邊。計算兩者之間的誤差：

式中：T為時刻的車輛位姿；G為GNSS數據生成的節點。

（4）閉環約束

完成聚類特征提取后，基于點云聚類特征閉環得到當前幀點云與局部地圖的粗匹配，利用NDT 得到當前點云和局部地圖間的精確變換。最后將閉環得到的變換矩陣，作為約束加入到后端進行優化位姿。

2.2.2 二維柵格地圖

二維柵格地圖用占用柵格的形式存儲環境中靜態障礙物信息，為無人駕駛的路徑規劃提供信息。柵格地圖由濾除地平面后的點云地圖向水平平面投影得到。

柵格狀態有占用、空閑、未知（即初始化）3 種狀態，柵格的初始狀態= 0。用（空閑）、（占據）表示對柵格的觀測。

式中：為柵格測量值；(= 0)為柵格空閑的概率；(= 1)為柵格占據的概率。

更新柵格狀態：

或

式中和分別為更新前后柵格的狀態。

2.3 動態層

動態層接收RTK 和LIDAR 傳感器的實時數據。首先對LIDAR 進行運動補償，去除掃描過程中由于車輛移動造成的運動畸變，再利用體素濾波去除噪點和地面點，得到處理過的當前幀點云數據。

根據處理后的實時感知信息，進行NDT 匹配定位和障礙物檢測，得到的障礙物信息可用于無人車避障。為實現局部路徑規劃等功能，需最后將障礙物信息同靜態層柵格地圖結合，為無人車自主導航提供環境信息。

2.4 多圖層地圖

將各層地圖轉換到世界坐標系進行組合，完成多圖層地圖的構建。如圖5 所示，GNSS 拓撲地圖提供絕對位置信息，二維柵格地圖存儲環境靜態障礙物信息，兩者為無人車的路徑規劃提供信息，點云地圖提供環境幾何結構信息，與動態層點云匹配為無人車提供相對定位信息，動態環境信息層提供了實時環境信息，組合構成了完整的環境感知系統。

圖5 多圖層地圖構建

3 基于多圖層地圖的定位和障礙物檢測

3.1 點云地圖處理

室外點云地圖的數據量非常大，加載全局地圖耗時很長，因此在實際定位時，將當前幀點云與局部地圖進行配準。本文對點云地圖進行分割，按照方形區域劃分并分塊存儲，設置劃分區域的邊長為30 m，定位時根據軌跡層GNSS 信息實現點云地圖分塊動態加載，提高定位效率。

3.2 定位

在實際應用場景中定位時，由于環境變化的影響，例如車輛及行人的移動、樹木的稀疏程度等，會導致較大匹配誤差，選擇NDT 來解決上述問題，可以在很大程度上消除這種不確定性，具體步驟如下。

（1）根據軌跡層的GNSS 位置信息，給定無人車的初始位置，并動態加載分塊點云地圖。

（2）動態層實時點云與點云地圖進行NDT 匹配定位。

NDT 首先將參考點云（即先驗地圖）網格化，基于網格內的點計算其概率密度：

式中：為正態分布的均值；為協方差矩陣；=[，…，x]，對3D點云，= 3。

NDT配準：定義當前幀點云=[，，]，變換參數，空間轉換函數(，)表示使用對點進行變換。求出使得概率密度之和最大的變換參數。

非線性優化：使用牛頓法進行優化，更新姿態。

（3）為進一步保證定位精度，設定NDT 匹配定位得分_＞100 時，用軌跡層提供的GNSS位置信息進行重定位。

3.3 障礙物檢測

動態層接收傳感器的實時點云信息，在對點云進行障礙物檢測時，由于點云密度不均等原因可能會導致誤檢等問題，本文中采用DBSCAN 算法檢測障礙物。輸出的障礙物信息，通過與靜態層柵格地圖信息結合，為無人車自主導航提供豐富的環境信息。檢測步驟如下：

（1）設定點云分割閾值，在LIDAR 坐標系下按長方形區域劃分車體周圍點云，正方向上取30 m，負方向取10 m，正負方向各取10 m；

（2）將LIDAR獲取的點云數據去除地面點；

（3）密度聚類確定障礙物的點云簇；

（4）利用最小面積矩形包絡法構建障礙物的外接框，輸出環境中的障礙物信息。

4 實驗結果與分析

采用實際數據對提出的地圖進行評估。首先創建實際環境中的多圖層地圖，然后基于該地圖實現實時定位和障礙物檢測，最后對結果進行分析。

4.1 環境地圖創建

本文中分別對多圖層地圖的各個圖層構建進行了實驗驗證，最后將各個圖層轉換到世界坐標系，構建出完整地圖并進行分析。

圖6 顯示了實驗環境和行駛路線，A 點為起點，車輛沿A-B-C-D-E-F-A 路線行駛，總路線長約700 m，雙向車道，道路平坦寬廣，道路寬約6 m，有減速帶。

圖6 實驗環境與行駛路線

4.1.1 軌跡層

如圖7 所示，綠線表示無人車的行駛軌跡，由組合導航的路徑點組成，A 為起點，B～F 代表拐點。實驗平臺配備的高精度的組合導航，精度約在2～3 cm，構建的GNSS 拓撲地圖精度達到了厘米級，能夠應用于真實環境。

圖7 GNSS拓撲地圖

4.1.2 靜態層

（1）三維點云地圖

為了不丟失特征信息，且有更好的建圖效果，在實車建圖實驗時采用了后端圖優化的方法。圖8（a）表示生成的點云地圖，圖8（b）和圖8（c）分別表示局部細節。從地圖來看，沒有丟失環境細節信息，誤匹配較少，沒有出現模糊不清的場景。

圖8 點云地圖效果示意

（2）二維柵格地圖

通過將濾除地平面后的點云地圖向地面投影，實現柵格地圖的創建。為保證地圖的存儲效率，設置柵格地圖分辨率為0.2 m ，柵格占據和空閑的概率分別設置為0.3和0.7。圖9（a）為柵格地圖，圖中黑色部分指的是障礙物占據區域，環境邊界清晰，能很好的描述環境中的靜態障礙物。圖9（b）和圖9（c）是柵格地圖的細節，為更好展示柵格地圖構建效果，圖中還顯示了原始點云信息（彩色為點云）。

圖9 柵格地圖效果示意

4.1.3 動態層

傳感器獲取實時環境信息，圖10（a）為A路口處的當前幀點云，在地圖中顯示如圖10（b）所示，紅色為當前幀點云。

圖10 動態環境信息

4.1.4 多圖層地圖

通過將各個圖層轉換到同一坐標系，構建完整的環境地圖。如圖11 所示，彩色為點云地圖，柵格地圖在點云下方，藍色軌跡為GNSS路徑。

圖11 多圖層地圖

4.2 基于多圖層地圖的定位和障礙物檢測

4.2.1 定位

基于多圖層地圖進行定位，效果如圖12 所示，黃線為匹配定位得到的軌跡，藍線為真實軌跡。可以明顯看出，定位軌跡和真實軌跡基本重合，沒有明顯的偏差，定位精確。

圖12 定位效果示意圖

為驗證定位的準確性，將定位得到的軌跡與真實軌跡作對比，并計算誤差，進行定量分析，如圖13所示。

圖13（a）左列顯示了定位得到的軌跡與真實軌跡的對比圖，右列展示了兩者對比的細節圖。圖13（b）、圖13（c）顯示了兩個軌跡間、方向上的誤差，誤差均值分別為0.095、0.180 m。圖13（d）展示了軌跡間的距離誤差，距離誤差均值為0.222 m，定位精度較高。結果表明，本文提出的多圖層地圖能夠用于室外實時定位。

圖13 定位誤差分析

4.2.2 障礙物檢測

對動態層的實時信息進行障礙物檢測，檢測效果如圖14 所示，左列為實際環境圖，右列為檢測結果。圖14（a）是靜態障礙物場景，左圖紅框區域為檢測到的靜態障礙物。圖14（b）為動態障礙物場景，紅框區域為動態物體。結果表明檢測較準確，能用于無人車避障等，且檢測到的實時障礙物信息可以同柵格地圖結合，為無人車的路徑規劃提供信息。

圖14 障礙物檢測效果

5 結論

本文中提出一種基于圖優化的室外多圖層地圖，經實驗驗證得到以下結論：

（1）區別于傳統單一地圖，該地圖由軌跡層、靜態層和動態層構成，包含了拓撲、點云、柵格和動態數據的多種環境信息，為無人車自主導航提供了充足的信息。

（2）基于該地圖可以實現精確、實時且魯棒的無人車定位，在實際實驗中，距離誤差均值可達到0.222 m 以下。

（3）在地圖的動態層準確完成了實時障礙物檢測，與柵格地圖信息相結合，為無人車自主導航提供了精準的環境障礙物信息。

加入語義信息提供人機交互功能將是今后進一步研究的方向。