基于先驗(yàn)地圖/GNSS/IMU融合的自動(dòng)駕駛車輛定位系統(tǒng)

熊華川

（重慶交通大學(xué)，重慶 400074）

1 前言

精確的定位是自動(dòng)駕駛車輛實(shí)現(xiàn)自主導(dǎo)航的前提[1]。僅依靠單一傳感器的傳統(tǒng)定位方式無(wú)法克服復(fù)雜的環(huán)境條件，如全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）在隧道、建筑遮擋條件下極易失效，不能滿足定位的穩(wěn)定性需求。針對(duì)復(fù)雜多變的環(huán)境，將多個(gè)傳感器的數(shù)據(jù)進(jìn)行融合實(shí)現(xiàn)定位是目前主流的定位方式。

其 中 ，GNSS/慣 性 測(cè) 量 單 元（Inertial Measurement Unit，IMU）融合定位是目前較流行的定位方案[2]。李慶成等[3]利用IMU 進(jìn)行位姿、速度等狀態(tài)預(yù)測(cè)，再利用擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）算法融合GNSS 和車速傳感器數(shù)據(jù)來(lái)修正預(yù)測(cè)狀態(tài)值，從而為礦用車提供精準(zhǔn)的定位數(shù)據(jù)。Musavi 等[4]同樣通過(guò)融合GNSS 和IMU數(shù)據(jù)實(shí)現(xiàn)精確定位，不同的是采用的融合算法為自適應(yīng)神經(jīng)觀測(cè)器。李鵬等[5]在融合GNSS 和IMU 數(shù)據(jù)的基礎(chǔ)上，提出丟棄惡化的觀測(cè)量策略，以降低觀測(cè)量精度較差時(shí)系統(tǒng)所受影響。因此，GNSS 與IMU 融合的定位算法均利用IMU 通過(guò)航位推算技術(shù)進(jìn)行位姿估計(jì)，IMU 長(zhǎng)時(shí)間積分會(huì)產(chǎn)生累積誤差[6]，需通過(guò)GNSS 修正預(yù)測(cè)值。但當(dāng)GNSS 失效或者受環(huán)境影響變得低效時(shí)，反而會(huì)降低定位系統(tǒng)的精度。

目前，利用激光雷達(dá)點(diǎn)云與先驗(yàn)地圖匹配定位的方法同樣是研究熱點(diǎn)。可以通過(guò)實(shí)時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping,SLAM）方法來(lái)構(gòu)建先驗(yàn)地圖，再利用激光雷達(dá)掃描得到的點(diǎn)云與先驗(yàn)地圖進(jìn)行匹配獲取位姿。基于迭代最近點(diǎn)匹配（Iterative Closest Point，ICP）算法是其中最常見(jiàn)的方法[7-8]，但該方法適用于少量點(diǎn)云的匹配運(yùn)算。Droeschel 等[9]提出通過(guò)構(gòu)建柵格局部地圖與點(diǎn)云進(jìn)行匹配，并通過(guò)分層優(yōu)化的方式優(yōu)化匹配結(jié)果。以上方法都是在點(diǎn)云特征的基礎(chǔ)上進(jìn)行計(jì)算優(yōu)化，當(dāng)出現(xiàn)大規(guī)模點(diǎn)云，以及兩幀之間數(shù)據(jù)變化劇烈等情況時(shí)，點(diǎn)云配準(zhǔn)均出現(xiàn)耗時(shí)長(zhǎng)、準(zhǔn)確率低的現(xiàn)象。因此，僅依靠激光雷達(dá)的地圖匹配方法不能滿足定位系統(tǒng)的要求。

針對(duì)以上問(wèn)題，本文提出利用誤差卡爾曼濾波（Error State Kalman Filter，ESKF）算法融合GNSS、IMU 和先驗(yàn)地圖點(diǎn)云數(shù)據(jù)，再通過(guò)引入矩陣校正方法，避免GNSS 失效或先驗(yàn)地圖匹配延遲等情況下系統(tǒng)無(wú)法獲取有效觀測(cè)值以修正位姿的問(wèn)題，使系統(tǒng)保持穩(wěn)定和高精度狀態(tài)。

2 多傳感器融合算法

本文算法的整體框架如圖1所示。本文采用的數(shù)據(jù)融合算法為ESKF 算法[10]，相較于EKF 算法，其具有以下優(yōu)勢(shì)[11]：ESKF 主要針對(duì)誤差狀態(tài)量進(jìn)行處理，誤差狀態(tài)量通常較小，因此計(jì)算量小，且總是在原點(diǎn)附近，可避免奇異值，保證線性化的合理性和有效性；在旋轉(zhuǎn)處理方面，ESKF 可以用三維變量來(lái)表示旋轉(zhuǎn)增量。

圖1 多傳感器融合定位系統(tǒng)框架

本文的整體框架分為3個(gè)部分：

a. 數(shù)據(jù)預(yù)處理。接收到IMU 原始數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)后，根據(jù)IMU 推算出車輛在激光雷達(dá)幀內(nèi)的位姿變化，進(jìn)而完成點(diǎn)云去畸變。

b.激光與點(diǎn)云地圖匹配。通過(guò)SLAM 算法構(gòu)建先驗(yàn)地圖，并將GNSS 信息作為先驗(yàn)地圖的初始坐標(biāo)，完成對(duì)多傳感器輸出位姿的坐標(biāo)系統(tǒng)一，再利用正態(tài)分布變換（Normal-Distributions Transform，NDT）算法將每一幀去畸變的點(diǎn)云與先驗(yàn)地圖進(jìn)行匹配輸出位姿。

c.濾波。利用ESKF 算法融合IMU 推算出的狀態(tài)值、激光雷達(dá)與先驗(yàn)地圖匹配輸出的位姿以及GNSS 輸出的位姿。通過(guò)觀測(cè)信息修正IMU 推算出的狀態(tài)值。

2.1 初始化

本文所使用的IMU 由陀螺儀和加速度計(jì)組成。根據(jù)二者特性可以對(duì)IMU 測(cè)量的加速度和角速度進(jìn)行數(shù)學(xué)建模：

式中，am、wm分別為IMU 加速度、角速度的測(cè)量值；at、wt分別為IMU 加速度、角速度的真實(shí)值；ba、bw分別為加速度零偏和角速度零偏；ηa、ηw分別為加速度、角速度測(cè)量噪聲；CNI為IMU 坐標(biāo)系到慣性坐標(biāo)系的變換矩陣；g為重力加速度。

在初始化階段，對(duì)IMU 的加速度零偏和角速度零偏進(jìn)行估計(jì)。具體而言，當(dāng)車輛靜止時(shí)，除加速度計(jì)的Z向加速度與重力加速度相等外，IMU 的數(shù)值均應(yīng)為零，因此車輛靜止時(shí)IMU 的加速度和角速度測(cè)量結(jié)果即可認(rèn)為是IMU 的零偏值[12]。隨后，使用激光雷達(dá)匹配先驗(yàn)地圖輸出的位姿作為基準(zhǔn)，對(duì)初始位姿進(jìn)行估計(jì)。這種方法使得系統(tǒng)能夠在任何地點(diǎn)進(jìn)行初始化，無(wú)需考慮GNSS的狀態(tài)。

2.2 運(yùn)動(dòng)模型

ESKF 將系統(tǒng)狀態(tài)分為不考慮噪聲的名義狀態(tài)和誤差狀態(tài)。二者相加即為真值狀態(tài)：

式中，Xt為系統(tǒng)真值狀態(tài)；Xn為系統(tǒng)名義狀態(tài)；δX為系統(tǒng)誤差狀態(tài)；p、δp分別為名義位置狀態(tài)量、誤差位置狀態(tài)量；v、δv分別為名義速度狀態(tài)量、誤差速度狀態(tài)量；q、δθ分別為名義姿態(tài)狀態(tài)量、角度誤差狀態(tài)量；δg為誤差重力加速度狀態(tài)量；δbw、δba分別為誤差角速度零偏狀態(tài)量、誤差加速度零偏狀態(tài)量。

根據(jù)IMU 航位推算算法，可以得到名義狀態(tài)的離散形式：

式中，pk、vk、qk、ba(k)、bw(k)、gk分別為k時(shí)刻名義位置狀態(tài)量、名義速度狀態(tài)量、名義姿態(tài)狀態(tài)量、加速度零偏狀態(tài)量、角速度零偏狀態(tài)量、重力加速度狀態(tài)量。

根據(jù)名義狀態(tài)與誤差狀態(tài)的關(guān)系可以推算出誤差狀態(tài)的離散形式：

式中，δpk、δvk、δθk、δba(k)、δbw(k)、δgk分別為k時(shí)刻誤差位置狀態(tài)量、誤差速度狀態(tài)量、角度誤差狀態(tài)量、誤差加速度零偏狀態(tài)量、誤差角速度零偏狀態(tài)量、誤差重力加速度狀態(tài)量；ηv、ηθ、ηba、ηbw為隨機(jī)噪聲；Δt為IMU采樣時(shí)間。

根據(jù)系統(tǒng)誤差狀態(tài)方程，可以得到ESKF 預(yù)測(cè)方程和預(yù)測(cè)的協(xié)方差矩陣：

2.3 觀測(cè)模型

本文的觀測(cè)數(shù)據(jù)分別由GNSS 和激光雷達(dá)與先驗(yàn)地圖提供。二者的觀測(cè)模型構(gòu)建為：

式中，YGNSS、YLiDAR為GNSS 觀測(cè)量、雷達(dá)匹配觀測(cè)量；h()、f()分別為GNSS 觀測(cè)方程、雷達(dá)匹配觀測(cè)方程；V、R分別為GNSS 觀測(cè)量的協(xié)方差矩陣、雷達(dá)匹配觀測(cè)量的協(xié)方差矩陣。

根據(jù)二者的輸出為位姿，可以將觀測(cè)模型統(tǒng)一寫作：

式中，Y為觀測(cè)量；F為觀測(cè)方程系數(shù)矩陣；Pobs為觀測(cè)量的協(xié)方差矩陣；H為觀測(cè)方程對(duì)誤差狀態(tài)的雅可比矩陣。

根據(jù)ESKF 的修正公式，可以分別得到對(duì)誤差狀態(tài)的修正值和協(xié)方差矩陣：

式中，K為卡爾曼增益；Xcertify(k+1)為(k+1)時(shí)刻修正后的狀態(tài)量；Xn(k+1)為(k+1)時(shí)刻名義狀態(tài)量；⊕表示廣義相加。

隨后，通過(guò)將修正后的誤差量與名義狀態(tài)量進(jìn)行廣義相加即可得到最終的系統(tǒng)真值狀態(tài)量。這一過(guò)程能夠有效減小預(yù)測(cè)值的誤差，提高系統(tǒng)的精度。

2.4 校正矩陣

GNSS 信號(hào)在高樓、隧道等環(huán)境下極易受到干擾，導(dǎo)致定位精度下降甚至失效，因此不能作為觀測(cè)值使用。激光雷達(dá)雖然可以克服環(huán)境的影響，但其匹配過(guò)程耗時(shí)較長(zhǎng)，無(wú)法滿足車輛對(duì)實(shí)時(shí)定位的需求。另一方面，IMU 的采樣頻率遠(yuǎn)高于GNSS 和激光雷達(dá)的采樣頻率，不同頻率下的觀測(cè)值同樣不能實(shí)時(shí)更新，如圖2所示。針對(duì)以上問(wèn)題，本文提出一種矩陣校正方法，以在GNSS 失效或激光雷達(dá)匹配延遲的情況下近似估計(jì)系統(tǒng)的狀態(tài)，避免系統(tǒng)因累積誤差快速發(fā)散。具體地，利用修正位姿與預(yù)測(cè)位姿之間的相對(duì)位姿作為校正矩陣，當(dāng)下一幀沒(méi)有合適的觀測(cè)值時(shí)，則利用校正矩陣修正系統(tǒng)位姿，以彌補(bǔ)GNSS 和激光雷達(dá)的局限性，提高系統(tǒng)的實(shí)時(shí)性和定位精度：

圖2 傳感器輸出對(duì)比

式中，ΔC為校正矩陣；Ccertify、Cpredict分別為位姿的修正矩陣和預(yù)測(cè)位姿。

3 試驗(yàn)驗(yàn)證

3.1 試驗(yàn)設(shè)備

本文試驗(yàn)由基于吉利博瑞改裝的智能車實(shí)現(xiàn)，如圖3所示。智能車搭載的試驗(yàn)設(shè)備有：車頂前、后放置的2根GNSS蘑菇頭天線，車頂中央處3顆16線激光雷達(dá)，后備箱處的GNSS信號(hào)接收機(jī)和IMU設(shè)備。

圖3 試驗(yàn)用智能車

3.2 試驗(yàn)數(shù)據(jù)

為驗(yàn)證本文算法的有效性，首先通過(guò)試驗(yàn)車在校園內(nèi)進(jìn)行GNSS、IMU 和激光雷達(dá)數(shù)據(jù)采集，采集路線如圖4所示，分別通過(guò)空曠路段、遮擋和隧道路段，來(lái)驗(yàn)證當(dāng)GNSS 失效或低效狀態(tài)下系統(tǒng)狀態(tài)的穩(wěn)定性。

圖4 數(shù)據(jù)采集路線

根據(jù)采集的激光雷達(dá)數(shù)據(jù)，通過(guò)SLAM 算法構(gòu)建定位系統(tǒng)中的先驗(yàn)地圖，如圖5所示。

圖5 先驗(yàn)地圖

3.3 試驗(yàn)結(jié)果分析

為驗(yàn)證定位系統(tǒng)的有效性，根據(jù)采集的數(shù)據(jù)分別進(jìn)行IMU航位推算、GNSS/IMU組合定位和先驗(yàn)地圖/GNSS/IMU組合定位的算法驗(yàn)證以及結(jié)果對(duì)比。

如圖6 所示為IMU 進(jìn)行航位推算的結(jié)果，可見(jiàn)IMU 在積分時(shí)速度隨著誤差累積而發(fā)散，導(dǎo)致最終的軌跡和以四元數(shù)[qw,qx,qy,qz]T表示的位姿均隨之發(fā)散，由式（5）可知，發(fā)散的速度成指數(shù)級(jí)增長(zhǎng)，因此需要其他數(shù)據(jù)來(lái)抑制IMU的誤差。

圖6 IMU航位推算軌跡、速度和位姿

分別利用GNSS/IMU組合定位算法和先驗(yàn)地圖/GNSS/IMU 組合定位算法生成定位軌跡，如圖7 所示。其中包括樹林遮擋、隧道等影響GNSS 狀態(tài)的路段，如圖8 所示。圖9 所示為整條軌跡中GNSS 狀態(tài)變化曲線，其中GNSS狀態(tài)定義如表1所示。由圖9 可以看出，經(jīng)過(guò)特殊路段時(shí)，GNSS 的狀態(tài)都不為4，根據(jù)表1，GNSS 其處于精度較差甚至失效狀態(tài)。從圖7中可知，在空曠路段，兩類算法輸出的軌跡幾乎重合，且平滑性好。差別體現(xiàn)在GNSS 低效處，即圖7中特殊路段處，GNSS/IMU 組合算法明顯較先驗(yàn)地圖/GNSS/IMU 組合算法具有更大的誤差偏移。并且值得注意的是，當(dāng)GNSS 失效后，軌跡并沒(méi)有立即如圖6一樣發(fā)散，而是隨著GNSS失效后的時(shí)間推移而逐漸發(fā)散，這是校正矩陣作用的結(jié)果，校正矩陣可有效延緩系統(tǒng)狀態(tài)的發(fā)散。

表1 GNSS狀態(tài)定義

圖7 軌跡對(duì)比

圖8 特殊路段場(chǎng)景

圖9 GNSS狀態(tài)

4 結(jié)束語(yǔ)

本文通過(guò)ESKF 算法融合GNSS/IMU/先驗(yàn)地圖構(gòu)建自動(dòng)駕駛車輛實(shí)時(shí)定位系統(tǒng)，并引入校正矩陣提高了系統(tǒng)的魯棒性。試驗(yàn)結(jié)果表明，在GNSS 低效甚至失效的情況下，先驗(yàn)地圖/GNSS/IMU 組合算法能夠很好地抑制系統(tǒng)因誤差累積而發(fā)散，從而為自動(dòng)駕駛車輛提供穩(wěn)定、精準(zhǔn)的定位結(jié)果。

但是，本文提出的方法依賴于高精地圖，高精地圖的制作和維護(hù)將限制其應(yīng)用。下一步的改進(jìn)方向應(yīng)是從高精點(diǎn)云地圖中提取道路交通信息，以構(gòu)建矢量地圖。矢量地圖的維護(hù)成本遠(yuǎn)小于高精點(diǎn)云地圖的維護(hù)成本，且道路交通信息不會(huì)輕易改變。