基于圖優(yōu)化的慣性/地磁/激光雷達(dá)復(fù)合定位技術(shù)研究

趙雨楠，季 陽，郭元江，李 昂，趙 豪

(北京自動化控制設(shè)備研究所，北京 100074)

0 引言

自動駕駛技術(shù)的不斷發(fā)展對車輛定位導(dǎo)航的精度與可靠性提出了更高的需求，復(fù)雜環(huán)境下的米級定位已成為車輛安全行駛的前提。

目前，自動駕駛的主流是基于多源信息融合的定位方法。動態(tài)實時差分全球定位系統(tǒng)(Real-Time Kinematic difference Global Positioning System，RTK-GPS)技術(shù)是目前自動駕駛中重要的定位手段之一，在空曠無遮擋的區(qū)域能夠?qū)崿F(xiàn)對車輛的米級甚至更高精度的定位，但其局限性在于城市路況復(fù)雜、建筑物或隧道遮擋信號、信號多徑效應(yīng)等因素的影響，會出現(xiàn)精度下降甚至失鎖的情況。慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)以其自主性、連續(xù)性和全導(dǎo)航參量的優(yōu)點在各類車輛中應(yīng)用廣泛，但是INS的定位誤差隨時間累積，必須引入其他輔助手段對其進(jìn)行校正。地磁匹配方法不依賴高品質(zhì)的位姿先驗信息，即可實現(xiàn)十米級定位，其誤差不累積，是一種有效的修正手段。激光雷達(dá)點云匹配定位技術(shù)的定位精度可達(dá)亞米級，是自動駕駛領(lǐng)域的主流定位方法之一。然而，這種匹配對先驗位姿信息品質(zhì)要求較高，一方面點云匹配采用非線性優(yōu)化方法，若無先驗易陷入局部最優(yōu)；另一方面點云特征數(shù)據(jù)量大、難以快速遍歷，因此，若無先驗位姿信息，該方法難以在大范圍環(huán)境中準(zhǔn)確確定位置，且在類幾何環(huán)境中易失效。

自動駕駛主流的導(dǎo)航方案為衛(wèi)星/慣性/激光雷達(dá)匹配組合方案，在衛(wèi)星拒止條件下，僅依靠慣性/激光雷達(dá)組合，難以適應(yīng)點云匹配失效的情況，存在點云匹配一旦失效，先驗位姿信息會快速發(fā)散，導(dǎo)致后面再難匹配上的問題。

因此，為了解決衛(wèi)星失鎖條件下衛(wèi)星/慣性/激光雷達(dá)匹配組合方案在點云匹配失效后難以恢復(fù)的問題，本文在該方案的基礎(chǔ)上，引入地磁匹配與運動約束信息，提出了一種基于圖優(yōu)化的慣性/地磁/激光雷達(dá)復(fù)合定位方案。在衛(wèi)星拒止環(huán)境下，在點云正常匹配時，實現(xiàn)米級定位精度；在點云匹配失效時，依靠慣性/運動約束/地磁匹配實現(xiàn)高精度保持。一方面繼續(xù)為車輛提供高精度、高可靠的位置信息，另一方面為點云再次匹配提供良好的先驗。

1 慣性/地磁/激光雷達(dá)復(fù)合定位方案

在衛(wèi)星拒止條件下，針對衛(wèi)星/慣性/激光雷達(dá)匹配組合方案的精度、可靠性有限的問題，本文設(shè)計了基于圖優(yōu)化的慣性/運動約束/地磁/激光雷達(dá)復(fù)合定位方案，如圖1所示。

圖1 基于圖優(yōu)化的慣性/地磁/激光雷達(dá)復(fù)合定位方案

本文方案主要包括慣性導(dǎo)航、地磁匹配、激光點云匹配、運動約束、信息融合5個模塊組成。慣性導(dǎo)航模塊完成預(yù)積分解算，結(jié)合側(cè)向零速約束,抑制誤差發(fā)散速度;在此基礎(chǔ)上，引入地磁匹配模塊,完成基于序列趨勢的匹配定位，為激光雷達(dá)點云匹配提供持續(xù)可用的先驗信息；激光雷達(dá)點云匹配模塊完成點云預(yù)處理與匹配，可以得到米級定位信息；在信息融合模塊完成慣導(dǎo)、運動約束、地磁、激光雷達(dá)信息的融合。通過增加運動約束與地磁匹配，提升了點云匹配失效時的精度保持能力，其中地磁匹配雖然匹配率有限，但是一旦完成匹配，可以進(jìn)行絕對位置修正，有效延長對點云匹配失效的最大容忍距離。

1.1 慣性導(dǎo)航模塊

慣性導(dǎo)航模塊主要完成預(yù)積分解算。慣性測量單元可自主高頻率測量運動信息，一般輸出頻率為數(shù)百赫茲，遠(yuǎn)超過導(dǎo)航系統(tǒng)的輸出頻率。因此，將慣性測量值在載體系下直接進(jìn)行預(yù)積分，預(yù)積分結(jié)果用于后續(xù)信息融合模塊。

(1)

(2)

1.2 地磁匹配模塊

地磁匹配模塊采用基于序列趨勢的匹配定位方案，如圖2所示，主要通過一維有限空間的地磁匹配技術(shù)實現(xiàn)10m級導(dǎo)航精度。

圖2 基于序列趨勢的匹配定位方案

一方面，建立的地磁基準(zhǔn)庫為沿道路軌跡的一維地磁強度參考點，相較于傳統(tǒng)的二維地磁地圖存儲量小、空間分辨率高；另一方面，基于地磁異常特征序列，將實時采集的序列與地磁基準(zhǔn)庫中的序列進(jìn)行相似度計算，如式(3)，完成趨勢匹配，并采用綜合置信度分析方法，提升地磁匹配準(zhǔn)確性。

(3)

式中，()為采集的地磁序列；()為地磁基準(zhǔn)庫中的序列，序列長度為。

1.3 激光雷達(dá)高精點云匹配模塊

考慮到激光雷達(dá)采集點云的數(shù)據(jù)量大，如果直接采用非線性最小二乘方法，會帶來迭代次數(shù)多、運算量大等問題。因此，本文設(shè)計了一種基于線面特征的兩級LM匹配算法，在保證匹配精度的同時，可以大幅提升匹配的實時性。

1)特征提取

為了避免直接對大量的點云數(shù)據(jù)進(jìn)行匹配計算，同時減小點云分布對后續(xù)匹配定位的影響，對點云信息進(jìn)行特征提取，并等量篩選不同方向的特征。

首先，根據(jù)環(huán)境中物體的特點，將特征分為線特征與面特征。根據(jù)點云與其臨近點(≠)的關(guān)系，對的曲率進(jìn)行提取。

(4)

式中，、分別為激光雷達(dá)到與的距離；為的集合，為的同線相鄰點云。當(dāng)大于門限1，則認(rèn)為其為線特征；當(dāng)小于門限2，則認(rèn)為其為面特征。

其次，將360°的視角分割為六部分，在每個方向分別選擇等量的最優(yōu)線特征與面特征，作為后續(xù)進(jìn)行匹配的特征。

2)兩級LM匹配

考慮到不同方向的位置、姿態(tài)信息的收斂速度不同，根據(jù)所提取特征的特點，設(shè)計了一種兩級LM匹配方案，如圖3所示。一方面可以降低迭代次數(shù)，提升匹配實時性；另一方面，考慮到激光雷達(dá)線數(shù)限制，俯仰角、高度精度有限，兩級LM匹配方案更便于拓展輔助信息源的水平姿態(tài)與高度信息的輔助。

圖3 兩級LM匹配方案

1.4 信息融合模塊

考慮到傳感信息的多源性與異步特征，以及兼容低精度慣導(dǎo)的非線性誤差特性，傳統(tǒng)的基于線性高斯的拓展卡爾曼濾波(Extended State Kalman Filter，ESKF)信息融合方法難以滿足需求。因此，搭建更為靈活的基于因子圖優(yōu)化的信息融合架構(gòu)。信息融合的核心是因子圖構(gòu)建與因子圖優(yōu)化。

1)因子圖構(gòu)建

因子圖構(gòu)建主要是因子圖的節(jié)點與因子配置。節(jié)點是待求解的導(dǎo)航信息，本方案采用時刻的位置與姿態(tài)為節(jié)點；因子表示節(jié)點間的空間約束，本方案的因子包括慣導(dǎo)預(yù)積分因子、地磁匹配因子、運動約束因子與激光雷達(dá)匹配因子。慣性/地磁/激光雷達(dá)復(fù)合定位方案的因子圖如圖4所示。

圖4 慣性/地磁/激光雷達(dá)復(fù)合定位方案的因子圖示意

a) 慣導(dǎo)預(yù)積分因子

慣導(dǎo)預(yù)積分因子與前后2個節(jié)點相關(guān)，其誤差函數(shù)可以寫作

(5)

b) 運動約束因子

車輛行駛時，側(cè)向速度理應(yīng)為0，以此運動約束建立相對應(yīng)的因子，即

(6)

(7)

c) 地磁匹配因子

(8)

d) 激光雷達(dá)匹配因子

(9)

2)因子圖優(yōu)化

完成慣性/地磁/激光雷達(dá)復(fù)合定位方案的節(jié)點與因子配置后，需要根據(jù)該系統(tǒng)的節(jié)點與因子構(gòu)造優(yōu)化的目標(biāo)函數(shù)，并推導(dǎo)雅可比矩陣，以最終完成優(yōu)化。第個節(jié)點優(yōu)化的目標(biāo)函數(shù)()可分為4個部分

(10)

至此，可采用Gauss-Newton法或Levenberg-Marquardt法求解式(10)，重復(fù)迭代修正節(jié)點，直到目標(biāo)函數(shù)取最小值，此時節(jié)點得到優(yōu)化調(diào)整，可以估計出車輛的位置與姿態(tài)信息。在構(gòu)造式(10)時，可根據(jù)該節(jié)點是否有對應(yīng)因子靈活增減目標(biāo)函數(shù)，易于融合異步多源的信息。

2 試驗結(jié)果與分析

2.1 試驗條件

為了驗證算法的有效性，搭建慣性/地磁/激光雷達(dá)試驗平臺，完成跑車試驗(圖5)。跑車采用某型號INS，陀螺零偏為0.01(°)/h，加表零偏為30μ；地磁傳感器選用PNI公司的TCM-XB三軸磁強計，重復(fù)性0.1μT；激光雷達(dá)采用速騰聚創(chuàng)RS-LiDAR-16。試驗以某高精度慣導(dǎo)/千尋衛(wèi)星組合導(dǎo)航系統(tǒng)的輸出為基準(zhǔn)。在北京市豐臺區(qū)長青路進(jìn)行驗證，全程約7km，耗時10min，平均速度約40km/h，整段路程路況復(fù)雜，包括直行路段、十字路口、丁字路口等，跑車軌跡如圖6所示。

(a) 原理樣機 (b) 跑車試驗

圖6 定位軌跡曲線

2.2 試驗結(jié)果

1)精度驗證

圖7所示為跑車的位置誤差曲線，可以看出，采用了基于圖優(yōu)化的慣性/地磁/激光雷達(dá)復(fù)合定位技術(shù)，定位誤差均方根為0.48m，最大值為1.24m。其中需要說明的是，由于試驗樣機輸出的信息與基準(zhǔn)信息采樣周期不一致，所以在車輛高速行駛時兩者信息不同步，于是出現(xiàn)誤差曲線的毛刺噪聲。

圖7 定位誤差曲線

2)可靠性驗證

考慮到高精度基準(zhǔn)點云地圖的覆蓋性，在實際中可能存在部分路段缺乏點云地圖的情況(圖8)，因此，去掉部分路段的點云地圖再次進(jìn)行試驗。

圖8 定位誤差曲線(部分路段缺乏點云地圖)

根據(jù)圖8可以看出，在行駛97～343s期間，點云地圖缺失，點云匹配失效，慣性/運動約束/地磁可以完成精度保持，定位誤差均方根為6.48m，最大值為10.29m；當(dāng)成功加載可用的點云地圖時(343s后)，可以順利完成點云匹配，定位誤差均方根為0.48m，最大值為1.19m。這表明，本文提出的方案可以避免衛(wèi)星拒止環(huán)境下，衛(wèi)星/慣性/激光雷達(dá)點云匹配方法存在的點云匹配一旦失效后再難匹配上的問題，具有較好的魯棒性與可靠性。

3 結(jié)論

1)為了解決衛(wèi)星失鎖條件下車輛的高精度定位問題，本文基于慣性導(dǎo)航、運動約束、激光點云匹配方法的互補優(yōu)勢，引入地磁匹配，設(shè)計了一種基于圖優(yōu)化的慣性/地磁/激光雷達(dá)復(fù)合定位方案。

2)試驗結(jié)果表明，本文提出的復(fù)合定位方案具有高精度、高可靠性的優(yōu)點。一方面，在衛(wèi)星拒止條件下，本文提出的方案定位精度可達(dá)0.48m (RMS)，為車輛自動駕駛提供良好的定位信息；另一方面，在點云匹配失效時，慣性/運動約束/地磁可以完成精度保持，為后續(xù)點云匹配提供持續(xù)可用的先驗信息，直到下次點云匹配成功。

3)后續(xù)，還需要對本文所提出的方案進(jìn)行復(fù)雜場景下的試驗驗證，尤其是高速公路等特征少、幾何相似的場景，以驗證環(huán)境特征對導(dǎo)航精度的影響。