基於多感測器融合的穩健精確車輛定位
編者序：本文是百度阿波羅計劃開源的無人車定位模塊使用的演算法，自適應的融合了GNSS、激光雷達和IMU數據，在多種複雜場景實現了5-10cm的高精度定位，能夠適用於完全自動駕駛應用。百度的演算法經過了大量場景大量數據的驗證，穩健性和擴展性都非常不錯。
推薦指數
一、引言
車輛定位是自動駕駛的一項基礎任務。全球導航衛星系統（global navigationsatellite system，GNSS）精度約10m左右，受制於衛星軌道和時鐘誤差、對流層和電離層延遲的影響，這些誤差可通過參考站的觀測進行矯正。基於載波相位的差分GNSS技術，也叫實時運動（Real time kinematic，RTK），能夠提供厘米級的定位精度。RTK最大優勢在於能夠全天候工作，缺點在於嚴重受信號遮擋、多徑效應的影響，因為它依賴於精確的載波相位定位技術。激光雷達LIDAR在精確定位中非常有用，缺點在於在極端天氣環境和道路結構下失效，儘管已有解決這些缺點的工作進展，如在小雨和小雪下的定位。進一步的講，就應用場景而言，LIDAR和RTK是兩種互補的感測器，LiDAR在3D紋理特徵環境工作很好，而RTK在開放環境工作很好。IMU包括陀螺儀和加速度計，通過航位推測（dead reckoning，DK）連續計算位置、姿態和速度，對干擾和欺騙免疫，但存在積分漂移問題。
每種感測器有自己獨特特點和工作條件，我們提出了一種適用於複雜城市和高速場景自動駕駛的穩健、精確多感測器融合定位系統。更準確的說，我們自適應融合基於Lidar、RTK和IMU等不同感測器的不同定位方法。系統感測器配置如下圖所示。該系統可為無人車其它模塊提供穩定、精確的定位服務，具備在複雜場景中的自動駕駛能力，如市中心、隧道、樹蔭下、停車場和高速公路。我們使用60km的數據驗證了該演算法的有效性。
多感測器融合框架如下圖所示。本文貢獻主要有：（1）提出了一種自適應融合不同感測器的車輛定位聯合框架，有效地利用了它們的優點，並通過有效的不確定性估計來屏蔽系統在各種場景中的失效；（2）提出了一種自適應聯合強度和高度線的Lidar定位方法，在施工道路等挑戰性比較大的場景下仍能獲得穩健、精確定位結果，超過了以前方法；（3）一種經過日常擁擠城市街區嚴格測試過的車輛定位系統，使得無人車可在城市中心、高速、隧道等各種有挑戰性的場景能夠全自動駕駛。
二、相關工作
多感測器融合併不是一個新思想，然而融合多種感測器以使得整個系統精確、穩健、能適用於各種場景，是一項非常有挑戰性的工作。A Soloviev和Y Gao等人使用一個2D激光雷達和GNSS、IMU構建GNSS/LiDAR/IMU導航系統，應用場景因LiDAR定位模塊需要依賴於牆等特定特徵而受限。IMU輔助LiDAR也能實現無人車定位，使用LiDAR提供位置測量，使用IMU預測Scan間的增量運動，從而提高點雲匹配精度。這些方法僅依賴於LiDAR，但RTK是與LiDAR完美互補的一種感測器，RTK在開放環境或結構化道路中起非常重要的作用。
基於LiDAR的車輛定位近年來非常流行，R和W提出了一種基於粒子濾波的多級匹配定位系統，K和S使用ICP演算法對齊實時點雲和參考地圖實現定位。然而，ICP對初始值非常敏感，在高速公路等開放環境中因沒有充足3D特徵而失效。J和S提出了一種接近本文的基於LiDAR強度的定位演算法，與僅僅使用3D幾何點雲方法相比，LiDAR強度提供了很有價值的、更豐富的紋理信息。我們在多個方面改進了該方法，包括一種新的圖像對齊步驟用以改良車頭朝向估計；一個包括強度和高度的聯合代價函數以獲得穩健結果；一種新的估計狀態協方差矩陣的方法。本文提出的方法是針對HDL-64、HDL-32E、VLP-16等多線LiDAR的，LiDAR的零售價會降很快，因為在寫本文時就有40多家該領域的公司。也有一些文獻試圖使用2D或低端LiDAR實現該功能。
三、LiDAR地圖生成
本文基於LiDAR的定位模塊依賴於提前生成的地圖，本部分目的是獲得環境的一種網格表示，每個網格存儲激光反射強度和高度的統計值。有的文獻使用描述強度均值和方差的單個高斯分布表示每個網格，有的則擴展為使用描述強度和高度的混合高斯模型GMM。本文使用單個高斯分布建模，但是同時包括了強度和高度，如下圖所示。我們發現，在代價函數中通過自適應加權方法聯合強度和高度測量，能夠顯著提高定位精度和穩健性。下圖（a）為3.3km3.1km的強度值，（b）為局部放大圖，（c）為高度渲染圖。
四、實驗
實驗平台為本文開頭那輛車，運動軌跡的ground truth通過離線方法獲取，以便定量分析。在GNSS信號良好的開放空間，NovAtel的Inertial Explore等基於後處理的GNSS/INS方法能夠提高足夠精確的運動軌跡；在複雜城市道路等GNSS信號微弱的場景，我們將其建模為經典的地圖重建問題，綜合使用NovAtel IE後處理、LIDAR SLAM、閉環檢測和全局位姿圖優化等技術。本文僅僅給出了隧道、地下停車場等demo視頻片段的定量分析結果。將整個60km的測試數據集分為3個場景：（1）48.1km的正常道路，YF-1到YF-5包含了城市、鄉村和交通擁堵；（2）10.4km的弱GNSS道路，HBY-1、DS-1包括了高樓和樹木下的窄路；（3）2.1km沒有GNSS的道路，DT-1包括隧道。
4.1 定量分析
對比演算法為Levinson等提出的State-of-the-art基於強度的定位方法。為了簡要證明不同感測器的貢獻，測試結果分兩種模式：（1）LiDAR+IMU的2系統；（2）LIDAR+GNSS+IMU的3系統，正常和微弱GNSS道路的定量分析如下表所示。可見相對於對比方法，本文方法的定位精度和穩健性都有大幅度提高。2系統和3系統都工作很好，證明我們的系統並不依賴某個感測器而是自適應融合了感測器輸入。橫向和縱向誤差如下圖所示。
4.2 定性分析
對於沒有GNSS的道路，由於缺乏groundtruth而不能定量分析，我們給出了本文定位結果和NovAtel的GNSS RTK/IMU位姿的定性比較，之所以沒有給出對比方法的結果，是由於它在GNSSRTK/IMU位姿不足夠穩定足夠平滑時失效。
下圖給出了一些性能分析：（a）RTK、LiDAR和融合系統定位都很好的情況；（b）LiDAR因地圖過時而失效，融合系統則在RTK的輔助下定位良好；（c）RTK因信號遮擋而定位不好，LiDAR工作良好；（d）和（e）：擁擠的人車場景本文系統定位良好；（f）僅依賴強度的LiDAR定位因新建的路和牆等明顯環境變化而失效，但自適應融合高度後則定位良好。
4.3LiDAR詳細分析
為了證明LiDAR定位各子模塊的有效性，給出了下表中2系統不同方法的定位精度，並增加了建圖後道路發生改變的數據YF-6。Intensity為僅使用強度的基準方法；Heading增加了車頭方向估計步驟，在低級IMU中非常有用；FixedAlt在匹配時融入和高度信息，使得精度有了大幅度提升；AdapAlt使用自適應權值融入高度。YF-4結果表明車頭角度優化對激光雷達定位很重要；YF-6結果表明自適應權值策略是得系統對環境變化更穩健，並且定位誤差最小。
4.4 實時性分析
我們的系統包括LiDAR、GNSS和SINS三個模塊，分別為10Hz、5Hz和200Hz；可採用兩種部署方式，一種是僅僅用單個CPU，另一種是採用一個CPU和一個FPGA。在第一種部署中，我們通過在車頭角度估計中使用較小的histogram濾波器大小和降採樣來降低運算量。這兩種部署都可以提供較高的定位精度，但是較大的histogram濾波器大小能夠在濾波器漂移時增加濾波器收斂的概率。GNSS和SINS模塊僅僅佔用20%的CPU。
英文名稱：Robustand Precise Vehicle Localization based on Multi-sensor Fusion in Diverse CityScenes
本公眾號專註於提供計算機視覺、機器學習、深度學習、智能感測器技術等人工智慧、機器人、無人車相關領域省時的知識轉述，敬請各位看官關注、評論、轉發、分享。
關注該公眾號，一起創造奇蹟。
