夏華佳 章紅平 陳德忠 李 團(tuán)

（武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心 武漢 430079）

0 引 言

近年來，全場景環(huán)境下的高可用、高可靠、高精度的完備導(dǎo)航參數(shù)定位需求日益增長，傳統(tǒng)GNSS/INS組合導(dǎo)航在GNSS信號(hào)遮擋區(qū)域，INS的導(dǎo)航誤差迅速累積，定位結(jié)果精度快速下降[1]。視覺慣性里程計(jì)（VIO）是利用圖像和慣導(dǎo)信息，融合計(jì)算獲取定位導(dǎo)航信息的熱點(diǎn)技術(shù)，能有效解決GNSS失效情況下導(dǎo)航系統(tǒng)誤差快速發(fā)散的問題，因此被廣泛研究與應(yīng)用。根據(jù)其融合框架，可以分為基于濾波器的VIO和基于優(yōu)化的VIO[2]。前者核心思想為慣導(dǎo)測量載體的運(yùn)動(dòng)數(shù)據(jù)用于狀態(tài)傳播，相機(jī)測量的信息數(shù)據(jù)用于狀態(tài)更新估計(jì)。而后者需要處理在軌跡上所有的測量，估計(jì)和平滑載體狀態(tài)，由于這類方法擁有迭代的運(yùn)算機(jī)制，能夠修正歷史的狀態(tài)估計(jì)，因此可達(dá)到較高的運(yùn)動(dòng)估計(jì)精度。但基于優(yōu)化的方法計(jì)算量較大，算法設(shè)計(jì)更復(fù)雜，無法在一些算力有限的平臺(tái)上運(yùn)行。而基于濾波器的VIO算法可同時(shí)兼顧計(jì)算效率和精度，且大多數(shù)平臺(tái)上均能獲取準(zhǔn)確一致的定位結(jié)果[3]。

基于濾波的VIO又分為松耦合[4]、緊耦合[5]這2種模式。松耦合模式是將視覺運(yùn)動(dòng)估計(jì)的位置、速度、姿態(tài)變化的結(jié)果作為觀測量構(gòu)建觀測方程，與慣性導(dǎo)航系統(tǒng)方程進(jìn)行融合解算；緊耦合模式則是直接將相機(jī)觀測到的圖像特征信息作為觀測量，構(gòu)建觀測方程對慣導(dǎo)誤差進(jìn)行修正。緊耦合算法中具有代表性的是MSCKF算法，采用了1個(gè)滑動(dòng)窗口維護(hù)多個(gè)時(shí)刻的相機(jī)位姿，以多幀圖像共視到同1個(gè)特征點(diǎn)為出發(fā)點(diǎn)，特征點(diǎn)與圖像之間的約束關(guān)系構(gòu)建觀測方程，估計(jì)并輸出最新時(shí)刻的位姿[6]。該方法直接使用了圖像特征點(diǎn)原始觀測值，因此視覺粗差對VIO解算精度影響極大。為了減少視覺粗差，需要提高VIO數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性，即特征點(diǎn)跟蹤匹配的準(zhǔn)確度。

目前，特征點(diǎn)跟蹤匹配算法大多采用基于描述符特征點(diǎn)匹配，或光流法匹配[7]。描述符的特征點(diǎn)匹配方法，描述了特征點(diǎn)與周圍像素分布特征，可將其作為目標(biāo)匹配的依據(jù)；光流法則是依據(jù)同一特征點(diǎn)灰度不變原理，尋找該點(diǎn)在下一幀圖像中可能出現(xiàn)的位置，從而實(shí)現(xiàn)匹配。在Liu等[8]的研究中改善了特征點(diǎn)的局部描述符，采用融合全局環(huán)境描述符，使得特征信息具備一定的空間描述能力；在成怡等[9]的成果中提出了基于Hamming距離和余弦相似度結(jié)合的描述符匹配方法，降低了多相似區(qū)域圖像特征點(diǎn)的誤匹配率；而鄒斌等[10]的研究中則采用區(qū)域分塊對圖像進(jìn)行預(yù)處理，挑選最佳匹配子塊預(yù)期得到最佳匹配。以上3種方法均屬于對描述符表達(dá)能力及匹配策略進(jìn)行改進(jìn)，能夠獲取較為準(zhǔn)確數(shù)據(jù)關(guān)聯(lián)，但匹配過程計(jì)算復(fù)雜，實(shí)時(shí)性較差?；诠饬鞯姆椒ㄖ?，王亭亭等[11]采用前后雙向光流方法提高了特征點(diǎn)的跟蹤精度，該方法不需要計(jì)算特征點(diǎn)描述符，節(jié)約了描述符暴力匹配時(shí)間，但在實(shí)際環(huán)境中由于光照、載體動(dòng)態(tài)等因素引入大量錯(cuò)誤匹配點(diǎn)，導(dǎo)致特征點(diǎn)跟蹤匹配的準(zhǔn)確度改善有限。

針對濾波框架下的視覺慣性里程計(jì)定位精度易受特征點(diǎn)異常匹配值影響的問題，光流法具備較高的計(jì)算速度，但假設(shè)條件過于嚴(yán)苛導(dǎo)致跟蹤匹配不準(zhǔn)確；基于描述符的特征點(diǎn)匹配在準(zhǔn)確性上存在一定的優(yōu)勢，但描述符計(jì)算和匹配過程復(fù)雜。因此，本文結(jié)合了快速角點(diǎn)檢測（FAST）、旋轉(zhuǎn)二值描述符（rotated BRIEF）和Lucas-Kanade（LK）光流算法，提出了rBRIEF描述符輔助光流跟蹤匹配的特征點(diǎn)數(shù)據(jù)關(guān)聯(lián)方法，在保留計(jì)算效率的同時(shí)，可有效剔除光流異常匹配點(diǎn)對，提高特征點(diǎn)跟蹤匹配的準(zhǔn)確度，進(jìn)一步提升MSCKF算法的定位性能。

1 特征點(diǎn)數(shù)據(jù)關(guān)聯(lián)方法

基于特征點(diǎn)的數(shù)據(jù)關(guān)聯(lián)主要是完成序列圖像中相鄰2幀圖像之間特征點(diǎn)的匹配，本文的視覺數(shù)據(jù)處理方法見圖1，主要包括FAST特征點(diǎn)提取、LK光流跟蹤匹配以及基于rBRIEF描述符的特征匹配點(diǎn)對優(yōu)選。

圖1 基于特征點(diǎn)的數(shù)據(jù)關(guān)聯(lián)方案Fig.1 Data association scheme based on feature points

1.1 FAST角點(diǎn)檢測

FAST角點(diǎn)檢測算法檢測圖像中較為特別的點(diǎn)，基本思想是1個(gè)像素的灰度大小與其鄰域內(nèi)足夠多像素點(diǎn)灰度差異達(dá)到某種標(biāo)準(zhǔn)時(shí)，則認(rèn)為該像素為角點(diǎn)。

以圖像內(nèi)任意1個(gè)像素作為候選特征點(diǎn)，建立以該像素點(diǎn)為中心，半徑為3、周長為16像素的圓形檢測模板。如圖2所示，順時(shí)針方向?qū)A上的像素點(diǎn)進(jìn)行編號(hào)，若圓上有連續(xù)N（一般取值為9）個(gè)像素點(diǎn)灰度值都比圓心灰度加上設(shè)立的閾值大，或者都比圓心灰度減去設(shè)立的閾值小，則將該點(diǎn)視為目標(biāo)點(diǎn)。

圖2 FAST角點(diǎn)檢測模板Fig.2 FAST corner detection template

為了避免檢測到的角點(diǎn)過于集中在1個(gè)區(qū)域，導(dǎo)致角點(diǎn)的分布性不好，一般做法是對圖像進(jìn)行網(wǎng)格化，在網(wǎng)格化后的所有小圖像塊內(nèi)進(jìn)行非極大值抑制操作，只保留Shi-Tomas響應(yīng)值最大的角點(diǎn)作為該圖像提取到的特征點(diǎn)。

1.2 LK光流跟蹤匹配

光流是像素在2幅序列圖像中運(yùn)動(dòng)的表現(xiàn)方式，通過估計(jì)像素的位移完成特征點(diǎn)的匹配，一般分為稠密光流和稀疏光流。前者指的是計(jì)算圖像中所有像素的運(yùn)動(dòng)，后者則計(jì)算部分像素運(yùn)動(dòng)。稀疏光流以LK光流法為代表。

LK光流基于以下3個(gè)假設(shè)：像素由1幅圖像流向另外1幅圖像，像素的灰度保持不變；相鄰2幀圖像之間運(yùn)動(dòng)變化不大；像素及其鄰域像素具有相似的運(yùn)動(dòng)[12]。假設(shè)圖像I中1個(gè)包含紋理信息的特征窗口W=(-wx,wx)×(-wy,wy)，中心點(diǎn)坐標(biāo)u=(ux,uy)，當(dāng)圖像發(fā)生變化時(shí)，LK光流跟蹤的目的是在圖像J中找到另1個(gè)點(diǎn)v=(ux+dx,uy+dy)，使得2處窗口內(nèi)像素點(diǎn)的灰度值近似相等，定義像素窗口的灰度誤差函數(shù)。

此時(shí)，LK光流跟蹤的任務(wù)求解合適的d，使得誤差函數(shù)ε(d)達(dá)到最小。其中，I(x,y)與J(x,y)分別代表對應(yīng)圖像坐標(biāo)(x,y)處的灰度值。運(yùn)動(dòng)量d=(dx,dy)為計(jì)算光流，也稱為像素點(diǎn)u的偏移量。式（1）描述的灰度誤差函數(shù)，關(guān)于變量dx，dy求解雅可比矩陣。

在2幀圖像之間為小運(yùn)動(dòng)的前提下，式（2）中，在d=0處對J(x+dx,y+dy)進(jìn)行泰勒展開，再令灰度函數(shù)關(guān)于偏移量的雅可比矩陣為0求解光流，進(jìn)而得到匹配關(guān)系。

LK光流法假設(shè)2幀圖像之間運(yùn)動(dòng)不大，但當(dāng)這個(gè)假設(shè)不成立時(shí)，跟蹤匹配會(huì)出現(xiàn)較大的誤差。為了解決這個(gè)問題，通常使用基于金字塔分層的LK光流[13]。對原尺寸圖像進(jìn)行若干層級(jí)的縮放，即可將處于金字塔底端像素的大運(yùn)動(dòng)在金字塔頂端被視為小運(yùn)動(dòng)，滿足了LK光流的計(jì)算條件。

將金字塔最頂層Lm光流初始值傳遞值記為，通過最小化灰度誤差函數(shù)，計(jì)算的得到Lm層的殘余光流，則最終計(jì)算光流為，假設(shè)金字塔圖像縮放因子為0.5，利用和可以估計(jì)次1層光流的初始傳遞值為

依次遞推，即可計(jì)算原始尺寸大小的圖像光流位移d。

1.3 rBRIEF描述符精確匹配

FAST特征點(diǎn)經(jīng)過金字塔分層的LK光流跟蹤后，得到相鄰2幀圖像特征點(diǎn)匹配結(jié)果。在實(shí)際情況中，圖像不可避免地受到光照、噪聲以及運(yùn)動(dòng)狀態(tài)等因素的影響，光流計(jì)算假設(shè)并不成立，導(dǎo)致跟蹤匹配結(jié)果不準(zhǔn)確，即使是基于金字塔分層的光流法改善也很有限。特征點(diǎn)描述符在一定程度上可以反應(yīng)特征點(diǎn)以及特征點(diǎn)周圍像素的分布特征，以此作為約束條件進(jìn)一步濾除部分匹配點(diǎn)，可提高光流跟蹤匹配的準(zhǔn)確性。

特征描述符可分為2種形式，梯度直方圖描述符和二進(jìn)制描述符。本文選擇二進(jìn)制rBRIEF描述符[14]作為異常匹配判斷依據(jù)的原因?yàn)椋孩傧鄬μ荻戎狈綀D描述符，例如SIFT，SURF特征，在計(jì)算和對比匹配時(shí)均需要進(jìn)行多種復(fù)雜且耗時(shí)的操作，而rBRIEF描述符作為1種二進(jìn)制字符向量，其描述向量以及相似度判斷均可以較高效地計(jì)算；②常規(guī)車載運(yùn)動(dòng)連續(xù)圖像之間的旋轉(zhuǎn)非常輕微，而rBRIEF描述符具備一定的旋轉(zhuǎn)不變性，足夠滿足該場景下的應(yīng)用需求。

rBRIEF描述符主要步驟包括FAST特征點(diǎn)方向計(jì)算以及進(jìn)行描述的二進(jìn)制編碼。對FAST特征點(diǎn)采用灰度質(zhì)心法計(jì)算方向：以特征點(diǎn)為中心，半徑為r的小圖像塊為例，定義圖像塊的矩為

式中：(x,y)為定義圓域內(nèi)的像素坐標(biāo)；I(x,y)為像素點(diǎn)(x,y)處的灰度值。則該圓域圖像的質(zhì)心有

特征點(diǎn)的方向可表示為

為了使得描述子具備一定的旋轉(zhuǎn)不變性，rBRIEF描述符根據(jù)上一步計(jì)算得到的FAST特征點(diǎn)方向，以該特征點(diǎn)為原點(diǎn)，x軸正半軸方向?yàn)樘卣鞣较?，y軸垂直x軸建立坐標(biāo)系，見圖3，在該坐標(biāo)系下以特定模式選取n對點(diǎn)對，按照一定的準(zhǔn)則對比這n對點(diǎn)對的灰度大小，定義比較準(zhǔn)則τ見式（7）。

圖3 rBRIEF描述符旋轉(zhuǎn)不變性Fig.3 Rotation invariance of rBRIEF descriptor

式中：I(a)為點(diǎn)a處的灰度值。將這n對點(diǎn)對的灰度大小比較結(jié)果按位編碼即可得到該特征點(diǎn)的rBRIEF描述符。

對光流跟蹤后所有特征點(diǎn)匹配對，重復(fù)以上操作計(jì)算rBRIEF描述符，通過Hamming距離對每一對匹配點(diǎn)對的描述符進(jìn)行相似度比較，對沒有通過的特征點(diǎn)匹配對進(jìn)行濾除，最終再利用RANSAC算法[15]進(jìn)行幾何校驗(yàn)，選取算法結(jié)果內(nèi)點(diǎn)作為最終的匹配結(jié)果。

2 MSCKF視覺慣性里程計(jì)算法

采用本文提出的rBRIEF描述符輔助光流跟蹤匹配的數(shù)據(jù)關(guān)聯(lián)方法進(jìn)行圖像特征點(diǎn)跟蹤匹配，進(jìn)一步基于MSCKF濾波器對視覺、慣導(dǎo)數(shù)據(jù)進(jìn)行融合解算，獲取運(yùn)動(dòng)載體的位姿信息。設(shè)計(jì)視覺慣性里程計(jì)定位算法見圖4。

圖4 MSCKF視覺慣性里程計(jì)算法框圖Fig.4 MSCKF-based visual-inertial odometer algorithm

利用GNSS/INS松耦合提供位置、速度、姿態(tài)信息，完成VIO的初始化。在融合解算過程中，INS作為主要的傳感器進(jìn)行航位推算，視覺作為輔助手段對導(dǎo)航誤差進(jìn)行修正。MSCKF算法維護(hù)了1個(gè)包含時(shí)間序列上連續(xù)的位姿信息窗口，因此，待估計(jì)參數(shù)包括當(dāng)前時(shí)刻慣性測量單元（inertial measurement unit，IMU）狀態(tài)誤差以及滑動(dòng)窗口內(nèi)相機(jī)狀態(tài)誤差，定義單個(gè)相機(jī)狀態(tài)誤差為

當(dāng)處理下1幀圖像時(shí)，MSCKF完成現(xiàn)有誤差參數(shù)的狀態(tài)傳播。

式中：Φk,k-1為k-1時(shí)刻到k時(shí)刻的系統(tǒng)轉(zhuǎn)移矩陣，其值由狀態(tài)參數(shù)的動(dòng)力學(xué)模型來決定；和Pk-1分別為k時(shí)刻狀態(tài)參數(shù)預(yù)測方差陣和k-1時(shí)刻狀態(tài)參數(shù)方差陣；Qk-1為系統(tǒng)噪聲方差陣，由狀態(tài)參數(shù)的動(dòng)力學(xué)模型精度確定。

同時(shí)對該時(shí)刻IMU位置、旋轉(zhuǎn)誤差項(xiàng)進(jìn)行增廣，以及增廣后誤差參數(shù)的方差協(xié)方差陣的更新。

濾波觀測更新是建立在多幀圖像共視到同1個(gè)特征點(diǎn)基礎(chǔ)之上。當(dāng)連續(xù)被跟蹤到的特征點(diǎn)從當(dāng)前相機(jī)視野中消失，或窗口內(nèi)包含歷史IMU位姿個(gè)數(shù)達(dá)最大需要移除部分狀態(tài)時(shí)，不再被跟蹤到的特征點(diǎn)以及被移除狀態(tài)時(shí)刻相機(jī)視野中所有的特征點(diǎn)均被用于構(gòu)建觀測方程。此時(shí)，滑動(dòng)窗口內(nèi)多個(gè)與相機(jī)固連的IMU位姿受特征點(diǎn)fi(i=1,2,…,m)約束。假設(shè)特征點(diǎn)fj被連續(xù)的n幀圖像觀測到，這n幀圖像對應(yīng)的相機(jī)位姿可記為，采用逆深度參數(shù)化的估計(jì)方法恢復(fù)該特征點(diǎn)的地固系e坐標(biāo)，重投影至第i幀圖像對應(yīng)的相機(jī)坐標(biāo)系Ci，投影后的坐標(biāo)可表示為

根據(jù)相機(jī)模型，式表示的特征點(diǎn)在相機(jī)坐標(biāo)系下坐標(biāo)，經(jīng)過歸一化投影，在Ci對應(yīng)的像素平面坐標(biāo)表示為

式中：fx和fy分別為像素平面x軸、y軸上的歸一化焦距（標(biāo)定已知）；為圖像噪聲向量。為了方便描述，在第i幀圖像中，特征點(diǎn)fj像素觀測值記為，根據(jù)式（15）和式（16）計(jì)算得到fj像素估計(jì)值記為，像素觀測值與估計(jì)值做差即構(gòu)成了測量殘差。

為了建立測量殘差與待估計(jì)量關(guān)系，將上式線性化可得

將式（15）擴(kuò)展至滑動(dòng)窗口內(nèi)所有觀測到該特征點(diǎn)的狀態(tài)，得到該特征點(diǎn)構(gòu)建的觀測方程。

視覺輔助INS解算需要對δx進(jìn)行估計(jì)，然而由于觀測方程中特征點(diǎn)坐標(biāo)誤差項(xiàng)的存在，需要對觀測方程進(jìn)行解耦操作，目的是消除待估計(jì)狀態(tài)量與特征點(diǎn)3D位置坐標(biāo)誤差之間的相關(guān)性，使得解耦后的觀測方程滿足卡爾曼濾波更新的要求。式投影至的左零空間。

一般而言，在MSCKF某更新時(shí)刻，存在多個(gè)滿足條件的特征點(diǎn)用于構(gòu)建觀測方程，每個(gè)特征點(diǎn)對窗口內(nèi)多個(gè)狀態(tài)形成約束，構(gòu)建式（20）類型的觀測方程，堆疊所有點(diǎn)構(gòu)建的觀測方程完成導(dǎo)航狀態(tài)誤差量的參數(shù)估計(jì)。

3 實(shí)驗(yàn)與分析

本節(jié)實(shí)驗(yàn)分別從特征匹配效果和定位精度2個(gè)方面，對本文方案進(jìn)行評(píng)價(jià)。數(shù)據(jù)采集設(shè)備中，視覺/慣性數(shù)據(jù)采集硬件包括Basler acA1600-20gm型工業(yè)相機(jī)、普通MEMS級(jí)別慣導(dǎo)M40（內(nèi)置慣導(dǎo)型號(hào)為ADIS16460）。此外，POS320作為1款高精度導(dǎo)航級(jí)光纖慣導(dǎo)，與GNSS載波相位差分后處理（PPK）進(jìn)行組合濾波和反向平滑處理，可為車載定位提供參考真值。2款慣導(dǎo)及Basler acA1600-20gm相機(jī)的主要性能參數(shù)見表1～2。

表1 慣導(dǎo)設(shè)備參數(shù)Tab.1 Equipment parameters of inertial-navigation

表2 Basler acA1600-20gm相機(jī)參數(shù)Tab.2 Parameters of Basler acA1600-20gm camera

3.1 特征點(diǎn)匹配效果分析

分別采用原始金字塔分層的LK光流跟蹤匹配方法和本文提出的加rBRIEF描述符優(yōu)化光流跟蹤匹配方法，對實(shí)測車載視覺圖像提取到的FAST特征點(diǎn)進(jìn)行了跟蹤匹配。圖5描述了上述2類方法處理后特征點(diǎn)匹配點(diǎn)對數(shù)量情況。

圖5中，藍(lán)色曲線表示每幀圖像均可提取到足夠多的FAST特征點(diǎn)，其個(gè)數(shù)均在150個(gè)以上；光流跟蹤后，絕大多數(shù)特征點(diǎn)均被視為跟蹤匹配成功，綠色曲線則對應(yīng)著匹配點(diǎn)對的數(shù)量情況；而結(jié)合rBRIEF描述符對光流跟蹤匹配結(jié)果作濾除后，留存的匹配點(diǎn)對數(shù)量進(jìn)一步減少，如紅色曲線所示。這是因?yàn)閳D像灰度本質(zhì)上是由光照強(qiáng)度、圖像拍攝角度等因素綜合決定，而光流法在求解特征點(diǎn)運(yùn)動(dòng)偏移量時(shí)，采用運(yùn)動(dòng)參數(shù)擬合灰度誤差函數(shù)，這使得只有在小運(yùn)動(dòng)、光照不變等假設(shè)前提下，求解過程中擬合才不會(huì)引入較大的模型誤差。圖6給出了本次測試中載體行駛的速度以及姿態(tài)信息。

圖5 特征點(diǎn)跟蹤匹配點(diǎn)對數(shù)量圖Fig.5 Quantity of feature points tracking matching-point pairs

圖6 載體導(dǎo)航狀態(tài)Fig.6 Navigation status of the carrier

當(dāng)載體處于平穩(wěn)行駛狀態(tài)時(shí)，rBRIEF描述符剔除部分特征不穩(wěn)定的匹配點(diǎn)對，特征數(shù)據(jù)優(yōu)化后留存的特征匹配點(diǎn)對在100以上；而當(dāng)載體出現(xiàn)較大機(jī)動(dòng)時(shí)，例如轉(zhuǎn)彎時(shí)刻，圖5中紅色曲線總會(huì)伴隨1個(gè)尖峰跳躍，特征匹配點(diǎn)對數(shù)量銳減至100以下，這與上述光流跟蹤適用情況原理分析一致。

抽取其中具有代表性圖像進(jìn)行視覺效果分析，圖7中給出轉(zhuǎn)向場景下基于rBRIEF描述符濾除異常匹配點(diǎn)對前后的特征匹配圖。如載體表面輪廓、標(biāo)識(shí)牌、建筑表面以及路面等處，利用rBRIEF描述符對像素及其鄰域像素分布特征進(jìn)行描述、比對，有效地對這些錯(cuò)誤匹配點(diǎn)對進(jìn)行濾除。

圖7 rBRIEF描述符濾除異常匹配前后對比Fig.7 Comparison before and after rBRIEF descriptor filtering exception matching

3.2 定位算法性能分析

從VIO定位精度方面對本文提出的視覺特征點(diǎn)匹配優(yōu)化方案進(jìn)行檢驗(yàn)評(píng)估。對比算法采用基于原始金字塔LK光流跟蹤匹配的MSCKF，簡記為VIO1.0，基于本文提出的rBRIEF描述符優(yōu)化特征點(diǎn)跟蹤匹配方案的MSCKF在本實(shí)驗(yàn)中簡記為VIO2.0。2種方法的解算軌跡以及典型定位誤差曲線見圖8～9，表3～4給出了相關(guān)定位誤差的統(tǒng)計(jì)結(jié)果。

圖8為視覺慣性組合導(dǎo)航解算軌跡與真值軌跡對比，由于視覺輔助定位缺乏絕對外部觀測，導(dǎo)致VIO解算存在累計(jì)誤差，總體呈現(xiàn)發(fā)散趨勢。VIO2.0采用rBRIEF描述符對光流跟蹤匹配結(jié)果進(jìn)行精化，視覺數(shù)據(jù)關(guān)聯(lián)準(zhǔn)確度更高，定位累計(jì)誤差較小。因此，相較于VIO1.0，VIO2.0軌跡跟真值軌跡更加貼合。

圖8 真值與不同算法解算軌跡圖Fig.8 Solving trajectory of the true value and different algorithms

圖9描述了2種算法解算定位結(jié)果與參考真值對比后的位置、姿態(tài)誤差曲線。由圖9可知：基于本文視覺特征匹配優(yōu)化方案的MSCKF算法，定位誤差更貼近0誤差標(biāo)準(zhǔn)線，這與圖8分析結(jié)果一致。結(jié)合圖6所示的載體行駛姿態(tài)變化曲線，每當(dāng)載體在轉(zhuǎn)彎時(shí)，北向、東向位置誤差改善的尤為明顯，航向誤差發(fā)散速度也更緩慢。

圖9 2種算法解算定位結(jié)果誤差曲線圖Fig.9 Two algorithms solving the error curves of positioning results

表3與表4分別統(tǒng)計(jì)了2種算法解算位姿的最大誤差值（MAX）和均方根誤差（RMS）。376 s內(nèi)，VIO1.0定位結(jié)果北-東-高3個(gè)方向最大發(fā)散誤差為7.74 m，16.80 m，2.63 m，3D位置最大發(fā)散誤差為16.90 m，考慮到載體在該段時(shí)間內(nèi)行駛距離超過2 km，定位誤差漂移約為0.84%；而VIO2.0定位結(jié)果在這3個(gè)方向最大發(fā)散誤差5.51 m，7.17 m，2.43 m，3D位置最大發(fā)散誤差為7.47 m，定位誤差漂移優(yōu)于0.38%。2種方案的RMS值在垂向方向相當(dāng)，而在平面方向本文方法具有明顯的改善，尤其是東方向改善達(dá)到56.5%。姿態(tài)結(jié)果與位置結(jié)果類似，VIO2.0方案的3個(gè)方向最大誤差值均優(yōu)于VIO1.0，YAW方向的改善達(dá)到45.2%，進(jìn)一步說明本文方法的有效性。

表3 VIO1.0解算誤差統(tǒng)計(jì)表Tab.3 Statistics of VIO1.0 solution errors

表4 VIO2.0解算誤差統(tǒng)計(jì)表Tab.4 Statistics of VIO2.0 solution errors

本實(shí)驗(yàn)共采集7 191張序列影像，對圖像處理運(yùn)算時(shí)間進(jìn)行分析。使用基于rBRIEF描述符輔助光流跟蹤匹配的數(shù)據(jù)關(guān)聯(lián)方法在計(jì)算機(jī)上進(jìn)行處理，各步驟處理時(shí)間如下：FAST特征點(diǎn)提取時(shí)間花費(fèi)195.82 s，3層金字塔光流跟蹤時(shí)間花費(fèi)22.00 s，描述符濾除異常匹配點(diǎn)時(shí)間花費(fèi)64.90 s，平均處理單幀圖像時(shí)間約為39 ms。傳統(tǒng)金字塔光流跟蹤平均處理單幀圖像時(shí)間約為30ms，使用ORBSLAM2中基于描述符匹配方法處理單幀圖像時(shí)間約為68 ms。本文提出的算法相較于傳統(tǒng)金字塔光流跟蹤，單幀圖像處理時(shí)間增加9 ms，但能夠改善54.7%的位置漂移；而相較于ORBSLAM 2中基于描述符特征點(diǎn)匹配方法，本文提出的方法處理時(shí)間縮短了42%。

4 結(jié)束語

基于MSCKF的視覺慣性里程計(jì)定位方法，具有計(jì)算效率高、可移植性強(qiáng)等特點(diǎn)，但其定位精度嚴(yán)重依賴于視覺特征點(diǎn)跟蹤匹配的準(zhǔn)確性。本文提出了1種rBRIEF描述符輔助光流跟蹤匹配的數(shù)據(jù)關(guān)聯(lián)方法，該方法利用描述符具備描述特征點(diǎn)及其周圍像素分布信息的能力，計(jì)算每對匹配點(diǎn)的描述符，根據(jù)對描述符的相似度判斷是否濾除該對點(diǎn)，從而提升特征點(diǎn)匹配的準(zhǔn)確度。實(shí)驗(yàn)表明：當(dāng)載體存在較大的機(jī)動(dòng)時(shí)，該方法能有效地剔除異常匹配點(diǎn)；在實(shí)測載體行駛距離超過2 km的大尺度場景下，基于描述符輔助光流跟蹤匹配的MSCKF算法整體位姿均要優(yōu)于原始光流跟蹤的MSCKF，尤其是位置漂移率優(yōu)于0.38%，改善約為54.7%。同時(shí)，該算法具有圖像處理速度快的優(yōu)點(diǎn)，與ORBSLAM2[17]圖像特征匹配算法相比，單幀圖像處理時(shí)間縮減42%。