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

趙雨楠，季 陽(yáng)，郭元江，李 昂，趙 豪

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

0 引言

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

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

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

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

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

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

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

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

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

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

(1)

(2)

1.2 地磁匹配模塊

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

圖2 基于序列趨勢(shì)的匹配定位方案

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

(3)

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

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

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

1)特征提取

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

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

(4)

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

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

2)兩級(jí)LM匹配

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

圖3 兩級(jí)LM匹配方案

1.4 信息融合模塊

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

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

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

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

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

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

(5)

b) 運(yùn)動(dòng)約束因子

車(chē)輛行駛時(shí)，側(cè)向速度理應(yīng)為0，以此運(yùn)動(dòng)約束建立相對(duì)應(yīng)的因子，即

(6)

(7)

c) 地磁匹配因子

(8)

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

(9)

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

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

(10)

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

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

2.1 試驗(yàn)條件

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

(a) 原理樣機(jī) (b) 跑車(chē)試驗(yàn)

圖6 定位軌跡曲線(xiàn)

2.2 試驗(yàn)結(jié)果

1)精度驗(yàn)證

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

圖7 定位誤差曲線(xiàn)

2)可靠性驗(yàn)證

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

圖8 定位誤差曲線(xiàn)(部分路段缺乏點(diǎn)云地圖)

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

3 結(jié)論

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

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

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