熊華川

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

1 前言

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

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

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

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

2 多傳感器融合算法

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

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

本文的整體框架分為3個部分：

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

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

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

2.1 初始化

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

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

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

2.2 運動模型

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時刻名義位置狀態(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時刻誤差位置狀態(tài)量、誤差速度狀態(tài)量、角度誤差狀態(tài)量、誤差加速度零偏狀態(tài)量、誤差角速度零偏狀態(tài)量、誤差重力加速度狀態(tài)量；ηv、ηθ、ηba、ηbw為隨機噪聲；Δt為IMU采樣時間。

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

2.3 觀測模型

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

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

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

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

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

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

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

2.4 校正矩陣

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

圖2 傳感器輸出對比

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

3 試驗驗證

3.1 試驗設(shè)備

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

圖3 試驗用智能車

3.2 試驗數(shù)據(jù)

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

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

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

圖5 先驗地圖

3.3 試驗結(jié)果分析

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

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

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

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

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

圖7 軌跡對比

圖8 特殊路段場景

圖9 GNSS狀態(tài)

4 結(jié)束語

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

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