基于視覺傳感器及IMU的多傳感器融合車輛高精度實(shí)時(shí)定位系統(tǒng)研究與設(shè)計(jì)

佘依函，蘭建平

（湖北汽車工業(yè)學(xué)院汽車工程師學(xué)院，湖北十堰，442002）

自2019年開始的第四次工業(yè)革命標(biāo)志著5G物聯(lián)網(wǎng)+人工智能+自動(dòng)駕駛汽車產(chǎn)業(yè)的飛速發(fā)展[1]。在此大環(huán)境下，自動(dòng)駕駛汽車必將成為大眾出行的主流交通工具[2]。構(gòu)建其自動(dòng)駕駛感知體系的組合導(dǎo)航定位技術(shù)逐漸成為研究的關(guān)鍵方面。若將此技術(shù)應(yīng)用于自動(dòng)駕駛汽車就會(huì)面臨在復(fù)雜環(huán)境，僅依靠組合導(dǎo)航定位技術(shù)來實(shí)現(xiàn)車輛實(shí)時(shí)定位會(huì)受到外界環(huán)境以及設(shè)備局限性這兩大問題，為解決此問題可以通過多傳感器融合技術(shù)來實(shí)現(xiàn)。

在實(shí)際環(huán)境中，車輛行駛在較為復(fù)雜的環(huán)境中，組合導(dǎo)航系統(tǒng)的信號(hào)會(huì)受到不同程度的影響，在組合導(dǎo)航系統(tǒng)信號(hào)受到較大影響時(shí)，車輛將會(huì)通過將IMU與視覺傳感器等多傳感器所傳輸回的信息進(jìn)行處理與融合[3]，減少視覺傳感器的投影誤差與IMU的時(shí)間誤差，通過算法優(yōu)化對(duì)車輛進(jìn)行高精度定位。實(shí)現(xiàn)車輛在復(fù)雜道路行駛過程中的穩(wěn)定性與安全性。

針對(duì)組合導(dǎo)航系統(tǒng)在室內(nèi)、隧道或信號(hào)較弱區(qū)域無法獲得地球坐標(biāo)系，而VINS-Mono只能獲取車體坐標(biāo)系的問題[4]。本文提出一種組合導(dǎo)航系統(tǒng)和VINS-Mono融合的定位算法，在組合導(dǎo)航系統(tǒng)初始化后可以實(shí)時(shí)得到車體的地球坐標(biāo)系，利用VINS-Mono解決在室內(nèi)或隧道內(nèi)、組合導(dǎo)航系統(tǒng)弱信號(hào)和組合導(dǎo)航系統(tǒng)掉幀、斷幀的問題，利用組合導(dǎo)航系統(tǒng)為VINS-Mono提供世界坐標(biāo)系。

此項(xiàng)研究使車輛不僅僅只依靠于組合導(dǎo)航系統(tǒng)定位的方式來確定車輛的大致位置，可以通過IMU與視覺傳感器數(shù)據(jù)融合的方式來對(duì)車輛進(jìn)行高精度定位。

1 實(shí)驗(yàn)平臺(tái)及系統(tǒng)框架

1.1 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)平臺(tái)以湖北汽車工業(yè)學(xué)院的無人駕駛方程式賽車為主，將組合導(dǎo)航安裝至車輛電池箱上方，攝像頭安裝至車輛正前方。使用工控機(jī)對(duì)組合導(dǎo)航定位系統(tǒng)與攝像頭發(fā)布的數(shù)據(jù)消息進(jìn)行訂閱和處理。實(shí)驗(yàn)使用了邁普時(shí)空M39組合導(dǎo)航系統(tǒng)、basler相機(jī)(分辨率為1920×1200，單幀采集時(shí)間40ms)。測(cè)試使用的計(jì)算機(jī)配置為 Intel i9-10900X 3.70GHz CPU、64G 內(nèi)存、GeForce RTX 2070s GPU、16G顯存。在實(shí)驗(yàn)過程中組合導(dǎo)航系統(tǒng)所采集的數(shù)據(jù)經(jīng)其內(nèi)部解算后輸出經(jīng)緯度、航向角、加速度。視覺部分的幀速為20Hz，單幀中跟蹤的功能點(diǎn)數(shù)量為300。當(dāng)特性點(diǎn)的跟蹤丟失時(shí)，將提取新的功能點(diǎn)。當(dāng)前幀和上一關(guān)鍵幀同時(shí)滿足以下兩個(gè)條件時(shí)，將創(chuàng)建一個(gè)新的可視關(guān)鍵幀：距離大于0.5m，公共顯示點(diǎn)數(shù)小于125，或平均平行度大于25像素。無人駕駛方程式賽車實(shí)驗(yàn)平臺(tái)如圖1所示。

圖1 實(shí)驗(yàn)平臺(tái)

1.2 系統(tǒng)框架

本系統(tǒng)通過組合導(dǎo)航系統(tǒng)與攝像頭的數(shù)據(jù)融合，提高了車輛高精度實(shí)時(shí)定位的目的。高精度實(shí)時(shí)定位系統(tǒng)的核心來源于多傳感器融合技術(shù)，即把不同傳感器所采集的數(shù)據(jù)，通過各個(gè)傳感器的預(yù)處理與解算，在融合過程中本文采用劃窗濾波的濾波方式對(duì)傳感器所傳回的數(shù)據(jù)進(jìn)行濾波處理，從而達(dá)到提高定位精度的目的。

系統(tǒng)框架可分為組合導(dǎo)航定位系統(tǒng)與視覺傳感器單元。組合導(dǎo)航定位系統(tǒng)采用RTK差分系統(tǒng)的GNSS定位、慣性測(cè)量單元、導(dǎo)航信息處理單元來確定自動(dòng)駕駛汽車的大致位置，攝像頭采用光流發(fā)進(jìn)行特征利采用光流法進(jìn)行特征點(diǎn)跟蹤，特征點(diǎn)深度估計(jì)有兩種方法：利用點(diǎn)云投影計(jì)算特征點(diǎn)深度，對(duì)穩(wěn)定跟蹤超過一定幀數(shù)的特征點(diǎn)通過三角化計(jì)算深度[5]。二者系統(tǒng)框架如圖2所示。

圖2 系統(tǒng)框架

2 組合導(dǎo)航系統(tǒng)

組合導(dǎo)航系統(tǒng)由IMU、GNSS、兩個(gè)主要部分組成，能在GPS信號(hào)良好的狀態(tài)下準(zhǔn)確定位自動(dòng)駕駛車輛的位置。此部分通過分析組合導(dǎo)航的定位原理來探究組合導(dǎo)航定位系統(tǒng)準(zhǔn)確定位自動(dòng)駕駛車輛位置的利弊[6]。

2.1 IMU測(cè)量原理

2.1.1 坐標(biāo)轉(zhuǎn)換

坐標(biāo)變換的方法即為通過坐標(biāo)變換矩陣將矢量在兩投影坐標(biāo)系之間進(jìn)行轉(zhuǎn)換[7]，表達(dá)式為式(1)。

式中: ΔB, ΔL同一點(diǎn)位在兩個(gè)坐標(biāo)系下的緯度差、經(jīng)度差, 單位為弧度, Δa, Δf橢球長(zhǎng)半軸差(單位米)、扁率差(無量綱), ΔX, ΔY,ΔZ平移參數(shù), 單位為米,εx,εy,εz旋轉(zhuǎn)參數(shù), 單位為弧度,m尺度參數(shù)(無量綱)。

2.1.2 IMU定位原理

通過接收傳感器得到的線性加速度和角速度，輸出位姿[8]，具體步驟如下。

⑴求旋轉(zhuǎn)

設(shè)t0-k位姿的旋轉(zhuǎn)用旋轉(zhuǎn)矩陣為R1，t0位姿的旋轉(zhuǎn)用旋轉(zhuǎn)矩陣為R2，若進(jìn)行一次旋轉(zhuǎn)，可表示為式(2)：

式中：R2為旋轉(zhuǎn)后角度矩陣，R1為旋轉(zhuǎn)前角度矩陣，R12為旋轉(zhuǎn)參數(shù)矩陣。

四元數(shù)計(jì)算為式(3)：

⑵求平移

設(shè)t0-k位姿的平移為x0-k，t0位姿的平移為x0-k，若進(jìn)行一次平移，可表示為式(4)：

⑶角速度計(jì)算

設(shè)t0-k角速度為w1，t0角速度為w2，偏移量為bg，則平均角速度為式(5)：

⑷加速度計(jì)算

設(shè)t0-k局部坐標(biāo)系的加速度表示為a0-k,加速度偏移為b0-k,重力加速度為g。t0在世界坐標(biāo)系下的加速度為a0：

2.2 雙天線GNSS測(cè)量原理

雙天線GNSS測(cè)向的基本原理是使用兩個(gè)GNSS接收機(jī)，天線位于載波上，載波相位差分技術(shù)可用于獲得由兩個(gè)天線所在點(diǎn)組成的矢量，即基矢量。然后根據(jù)幾何關(guān)系確定驅(qū)動(dòng)角和俯仰角[9]。

圖3 GNSS測(cè)量矢量圖

式中a表示航向角，e表示俯仰角。

可以看出，在確定雙天線的GNSS方向時(shí)，方向角的精度取決于計(jì)算出的基本扇區(qū)的精度。

3 VINS-Mono

3.1 視覺前端

對(duì)于每個(gè)新圖像，現(xiàn)有特征由KLT稀疏光流算法[10]。與此同時(shí)，新的角落檢測(cè)特征以保持最小數(shù)量(100-300)每個(gè)圖像中的特征。探測(cè)器強(qiáng)制執(zhí)行制服通過設(shè)置像素的最小間隔來進(jìn)行特征分布在兩個(gè)相鄰的特征之間。二維(2-D)特征首先不失真，然后投影到單位球體通過異常值拒絕后。執(zhí)行異常值拒絕使用RANSAC和基本矩陣模型。在此步驟中還將選擇關(guān)鍵幀。兩個(gè)標(biāo)準(zhǔn)用于關(guān)鍵幀選擇。第一個(gè)是平均視差除了上一個(gè)關(guān)鍵幀之外。如果平均視差跟蹤的特征介于當(dāng)前幀和最新幀之間關(guān)鍵幀超過某個(gè)閾值時(shí)，將幀視為新幀關(guān)鍵幀[11]。不僅平移，旋轉(zhuǎn)也可以造成視差。但是，不能在中對(duì)要素進(jìn)行三角剖分僅旋轉(zhuǎn)運(yùn)動(dòng)。為了避免這種情況，使用短期策略集成陀螺儀測(cè)量以補(bǔ)償旋轉(zhuǎn)計(jì)算視差時(shí)。此旋轉(zhuǎn)補(bǔ)償僅用于關(guān)鍵幀選擇，不涉及在VINS公式中的旋轉(zhuǎn)計(jì)算中。為此,即使陀螺儀包含大噪聲或有偏差，它也會(huì)僅會(huì)導(dǎo)致次優(yōu)關(guān)鍵幀選擇結(jié)果，并且將不直接影響估算質(zhì)量。另一個(gè)標(biāo)準(zhǔn)是跟蹤質(zhì)量。如果跟蹤特征的數(shù)量低于在特定閾值下，將此幀視為新的關(guān)鍵幀[12]。這標(biāo)準(zhǔn)是避免特征軌跡完全丟失。

3.2 系統(tǒng)初始化

單目緊密耦合VIO是一個(gè)高度非線性系，。由于單目攝像機(jī)無法直接觀察刻度，如果沒有良好的初始值，很難將兩個(gè)測(cè)量結(jié)果直接集成。假設(shè)一個(gè)固定的初始狀態(tài)來啟動(dòng)單目VIN估計(jì)器。然而，此假設(shè)是不恰當(dāng)?shù)模驗(yàn)樵趯?shí)踐中經(jīng)常會(huì)發(fā)現(xiàn)初始化。如果IMU結(jié)果被嚴(yán)重扭曲破壞，情況會(huì)變得更加復(fù)雜。事實(shí)上，初始化通常是單目攝像頭中最敏感的步驟，需要穩(wěn)健的初始化過程來確保系統(tǒng)的適用性[13]。

VINS-Mono使用松耦合傳感器融合方法來獲得初始值[14]。僅視覺SLAM或SFM，有良好的初始化性能。在大多數(shù)情況下，視覺系統(tǒng)可以通過從相對(duì)運(yùn)動(dòng)方法(如八點(diǎn)或五點(diǎn)算法或齊次矩陣)導(dǎo)出初始值來引導(dǎo)自身。通過將測(cè)量的IMU預(yù)積分與視覺SFM結(jié)果進(jìn)行匹配，我們可以大致恢復(fù)比例、重力、速度甚至偏移量。

第一個(gè)相機(jī)幀 (·)c0被設(shè)置為參考幀的SFM。所有幀的姿勢(shì)和特征位置相對(duì)于表示。假設(shè)相機(jī)和IMU之間存在外部參數(shù)的粗略測(cè)量，即可將姿態(tài)從C幀轉(zhuǎn)換為B幀(IMU)。

在細(xì)化重力矢量后，我們可以得到旋轉(zhuǎn) qc0W在世界框架和通過將重力旋轉(zhuǎn)到z軸，相機(jī)幀c0。那么我們將所有變量從參考系 (·)c0旋轉(zhuǎn)到世界幀(·)w。身體幀速度也將旋轉(zhuǎn)為世界框架。來自可視SFM的平移組件將按公制單位縮放。此時(shí)，初始化程序完成，所有這些度量值都將被輸入到緊密耦合的單目VIO。

3.3 單目緊密耦合VIO重定位

滑動(dòng)窗口和邊緣化計(jì)劃限制了計(jì)算復(fù)雜度，但也會(huì)引入累積系統(tǒng)的漂移。為了消除漂移，一個(gè)緊密耦合的重新校準(zhǔn)模塊與單目光學(xué)系統(tǒng)無縫集成提出了VIO[15]。重新定位過程從一個(gè)循環(huán)檢測(cè)模塊開始，該模塊識(shí)別已被定位的位置參觀。然后在循環(huán)結(jié)束日期和當(dāng)前幀之間建立特征級(jí)連接。這些特征通信緊密集成在單目VIO中模塊，產(chǎn)生無漂移狀態(tài)估計(jì)，最小計(jì)算多個(gè)特征的多個(gè)觀測(cè)直接用于重新定位，從而提高了精度和精度更好的狀態(tài)估計(jì)平滑度。

4 多傳感器數(shù)據(jù)融合

4.1 從GPS信號(hào)強(qiáng)環(huán)境至GPS信號(hào)弱環(huán)境

在k0時(shí)刻突然失去GPS的情況下，本文所設(shè)計(jì)的程序會(huì)記錄GPS信號(hào)消失前所接收到的最后一次發(fā)送的經(jīng)緯度為v0，r0。同時(shí)記錄在k0時(shí)刻通過VINS-Mono得出的全局三維位置(x0，y0，z0)，車輛在k1時(shí)刻運(yùn)動(dòng)至(x1，y1，z1)(k1-k0無線趨近于零)通過計(jì)算x1-x0、y1-y0、z1-z0得出 Δx、、 Δy、Δz。即得出全局三維位置與經(jīng)緯度變化量的公式：

隨機(jī)取多組GPS信號(hào)存在時(shí)經(jīng)緯度與全局三維位置信息，通過最小二乘法計(jì)算出A、B、C、D、E。

在無法獲得經(jīng)緯度時(shí)通過VINS-Mono得出的全局三維位置帶入公式中即可計(jì)算出經(jīng)緯度的變化量，將經(jīng)緯度的變化量累加至v0，r0。即可得出v1，r1。通過在GPS信號(hào)消失階段至GPS信號(hào)恢復(fù)階段使用VINS-Mono所得出的全局三維位置對(duì)經(jīng)緯度進(jìn)行校準(zhǔn)從而使車輛的軌跡不會(huì)丟失。

4.2 從GPS信號(hào)弱環(huán)境至GPS信號(hào)強(qiáng)環(huán)境

若車輛初始k0存在于一個(gè)GPS信號(hào)弱環(huán)境下，本文所設(shè)計(jì)的程序會(huì)讀取前一次經(jīng)全局三維位置與經(jīng)緯度變化量的公式得出的經(jīng)緯度作為本次車輛的v0，r0。同時(shí)讀取前一次通過過VINS-Mono得出的全局三維位置作為此次k0時(shí)刻的(x0，y0，z0)進(jìn)行上述從GPS信號(hào)強(qiáng)環(huán)境至GPS信號(hào)弱環(huán)境經(jīng)緯度的計(jì)算方法進(jìn)行本次計(jì)算，在kt時(shí)刻GPS信號(hào)良好時(shí)將所計(jì)算經(jīng)緯度于GNSS所得經(jīng)緯度進(jìn)行比較，計(jì)算誤差的同時(shí)對(duì)此程序進(jìn)行優(yōu)化從而縮小誤差，經(jīng)過n次迭代得出最優(yōu)運(yùn)算模型。

5 結(jié)論

本文針對(duì)基于組合導(dǎo)航系統(tǒng)定位精度不足的問題，提出了基于組合導(dǎo)航系統(tǒng)與視覺傳感器融合定位方法實(shí)現(xiàn)自動(dòng)駕駛車輛高精度實(shí)時(shí)定位功能。保證其能在復(fù)雜的自然環(huán)境下穩(wěn)定工作。實(shí)驗(yàn)結(jié)果表明，該方法能有效融合組合導(dǎo)航系統(tǒng)和VINS-Mono的數(shù)據(jù)信息，得到車體的地球坐標(biāo)系。