融合IMU去除運動模糊的改進光流匹配算法

摘要：為進一步提高視覺SLAM中的光流匹配精度和速度，提出一種融合慣性測量單元（inertial measurement unit，IMU）去除運動模糊的改進光流匹配算法。該算法首先利用IMU運動信息計算的點擴散函數去除運動模糊，提高特征點匹配率；其次在LK（Lucas-Kanade）光流的基礎上引入梯度誤差，并使用圖像梯度L1范數作為正則項模擬稀疏噪聲，構建代價函數；然后利用IMU預測特征點位置作為該算法初始值，并加入BB（Barzilar-Borwein）步長改進原有的高斯牛頓算法，提高計算速度。實驗表明，通過兩幀之間的比較，該算法的效率和精度均優于LK光流法；然后將該算法集成到VINS-Mono框架，在數據集EuRoC上的結果顯示，該算法提高了原有框架的定位精度和魯棒性。

關鍵詞：光流法； 前端視覺里程計； 運動模糊； 多傳感器融合

中圖分類號：TP242文獻標志碼：A

文章編號：1001-3695（2022）10-047-3174-05

doi：10.19734/j.issn.1001-3695.2022.02.0075

Feature tracking algorithm for removing motion blur by improving optical flow incorporating IMU

Luan Xiaozhena， Wei Guoliangb， Cai Jiea

（a.College of Science， b.Business School， University of Shanghai for Science amp; Technology， Shanghai 200093， China）

Abstract：In order to improve the accuracy and efficiency of the feature point matching， this paper proposed a novel feature point matching algorithm in terms of vision and inertial measurement unit （IMU） fusion. Firstly， the algorithm calculated the point diffusion function by using the motion information of IMU to remove motion blur， and improved the feature point matching rate. Secondly， based on LK optical flow method， this paper introduced gradient error and used L1 parametric to simulate sparse noise. Furthermore， the feature point position by using IMU was the initial value of the algorithm， and then this paper used BB step to improve the efficiency of the algorithm. Finally， the comparison experiments show that the efficiency and accuracy of the algorithm are better than the LK optical flow method. Especially， the algorithm improves the localization accuracy and robustness of the VINS-Mono framework on the dataset EuRoC.

Key words：optical flow; visual odometry; motion blur; multi-sensor fusion

0引言

近年來，視覺同時定位與建圖（visual simultaneous localization and mapping，V-SLAM）［1］在機器人領域有著重要作用，如物流配送、虛擬現實（virtual reality，VR）［2］、智能家居等。雖然單目相機成本較低、使用方便，但是僅僅依靠相機難以估計環境的尺度。因此在V-SLAM中，為了解決尺度不可觀問題，眾多學者更傾向于多傳感器融合技術。由于慣性測量單元能夠在隔絕外部環境的影響下測量本體的角速度和加速度，從而使單目視覺的公制尺度可觀測，并提供魯棒和準確的幀間運動估計，所以視覺和IMU融合的SLAM技術（VI-SLAM）異軍突起［3～5］。VI-SLAM框架主要分為前端里程計、后端優化、回環檢測和建圖。前端里程計作為首要步驟，在整個系統中顯得尤其重要。在前端里程計中，不僅需要進行IMU預積分，而且需要對相機傳入的圖片進行特征點提取和匹配，進而根據匹配信息計算相鄰兩幀之間機器人的運動，并構建特征點的局部3D位置［6］。一旦發生誤匹配［7］，會導致估計不準確，因此，特征點提取與匹配也是前端里程計中的重要一環。

特征點提取與匹配主要分為特征點法［8，9］、光流法［10］和直接法［11］。特征點法在計算關鍵點、描述子以及匹配描述子時計算量過大，而光流法和直接法彌補了以上缺陷，光流法只計算關鍵點，不計算描述子，通過兩幀之間的光度不變假設，直接最小化光度誤差并進行迭代匹配，本文重點研究光流法。

在光流法中目前主要存在以下兩個問題。一方面，若物體運動過快，則在相機曝光期間，景物和攝像機之間存在相對運動，會導致像素重疊。出現圖像模糊現象，稱為運動模糊；而光流法對光度穩定和圖像的連續有較強依賴，當不滿足以上假設的時候，容易找不到跟蹤點，更糟糕的情況是追蹤到錯誤的特征點。另一方面，在光流法中使用非線性優化方法進行最小化光度誤差迭代匹配的過程中，首先，非線性算法的最優值對初值有很強的依賴性，如果沒有初值則需要進行全局搜索，計算量較大且容易造成誤匹配；其次，傳統高斯牛頓方法的步長是固定的或者直接由線搜索算法給出，如果步長取得太大，一開始會很快向最優解逼近，在收斂的最后階段，容易跳過最優解，而步長取得太小，則導致收斂速度變慢。因此，去除運動模糊和選取非線性優化算法的初始值、步長是本文研究的兩個問題。

基于以上問題，本文提出了一種融合IMU去除運動模糊的光流匹配算法。該算法通過融合IMU運動信息，計算點擴散函數去除運動模糊，之后再次融合IMU信息預測特征點位置，為光流跟蹤提供初值，進而提高特征點提取與匹配的性能。最后，將該算法集成到了完整的VI-SLAM框架中。

在VI-SLAM框架特征點提取與檢測方面，Qin等人［12］提出的VINS-Mono前端使用光流法提取關鍵點，并使用多層金字塔LK進行跟蹤，雖然整體框架是基于IMU和視覺傳感器融合，但在關鍵點提取和跟蹤中，并未將IMU信息與視覺信息融合。Guan等人［13］提出的IFPT特征點匹配算法將特征點法與IMU進行融合，通過IMU的位置和旋轉方向預測下一幀關鍵點的位置，并以其為圓心進行描述子局部搜索與匹配。OKVIS（open keyframe-based visual-inertial SLAM）［14］的3D-2D的匹配中，利用IMU預測的位姿篩選當前幀可見的特征點，去除掉當前幀已經追蹤不到的特征點，節省暴力匹配時間。OKVIS和IFPT共同點是將特征點法與IMU融合，即基于預積分理論進行預測，為待匹配的每個特征點提供新圖像中的可能位置，進而完成前端特征點的提取與匹配。不同點是，IFPT得到可能位置之后進行局部搜索，而OKVIS篩選去除當前幀已經看不到的點，然后進行全局暴力匹配。

本文算法和之前相關工作有所區別，首先從算法本身比較：a） 之前的工作［15］直接設置平移信息t=0，而本文算法充分利用了IMU的信息，加入了加速度計得到的t，實驗結果表明融合平移信息后，IMU預測的特征點位置更加準確；b） 之前的工作未將速度信息充分利用，而本文算法利用IMU預積分得到的運動信息計算點擴散函數，加入了運動模糊復原步驟，減少運動模糊導致匹配特征點失敗現象；c） 在非線性優化方法中，加入BB可變步長進行搜索，使步長大小可以自適應損失函數。其次，從框架上比較，之前的工作［15］把偏置當做狀態向量的一部分去優化，并且在初始化前，偏置求解不準確的情況下仍然預測相機位姿，導致視覺里程計的誤差一直傳遞下去，更糟糕的是若系統中沒有閉環檢測，很難消除視覺里程計產生的累計誤差。本文算法利用IMU預測的時機是在系統初始化后，此時偏置信息已經求出，減小了累計誤差。

1系統框架

該算法在VINS-Mono框架前端算法基礎上進行研究。原VINS-Mono框架前端算法使用ShiTomasi［16，17］算法檢測角點，LK光流法跟蹤匹配。在運動幅度較大、光照變化劇烈的情況下，匹配誤差較大。

為解決以上問題，提高定位精度，本文對VINS-Mono前端算法進行改進。具體實施方案如下：首先在初始化前，仍通過傳統光流法進行特征點提取，初始化完成之后，將得到的IMU加速度計和角速度計偏置代入到預積分中，融合預積分預測得到的速度、位置、旋轉方向信息，發送到前端，判斷速度是否大于閾值，如果大于閾值說明運動過快，則根據運動信息進一步對圖像進行非盲復原［18，19］;如果上一步完成或者速度小于閾值，說明運動幅度較小，為滿足實時性，則不進行圖像復原操作，直接將IMU預測得到的位置、旋轉方向與視覺信息融合，即將恢復之后的點投影到下一幀圖像上，提供給光流追蹤中高斯牛頓算法一個初始值，確定光流追蹤匹配的局部搜索位置。該算法集成到框架上，其流程如圖1所示。

2預積分模型預測特征點位置

本節首先對常用的一些符號進行定義。（·）^表示傳感器測量值，R表示3×3的旋轉矩陣，q表示旋轉的四元數表示，t表示3維平移向量，T=［R|t］表示姿態的轉換矩陣，包括旋轉矩陣和平移向量，（·）I為IMU坐標系，（·）C為相機坐標系。特別地，令相機的第一幀為參考坐標系，即旋轉為單位矩陣RC0=E，平移向量為零向量tC0=0。假設經過初始化［20，21］之后，從相機坐標系到IMU坐標系的旋轉外參Rbc、平移外參tbc、IMU陀螺儀的偏置bg、加速度計偏置ba以及在參考幀坐標系下的重力矢量gC0是已知的。

2.1基于預積分求解機器人的位置、速度、旋轉方向

只考慮陀螺儀和加速度計的偏置白噪聲以及偏置的隨機游走噪聲，則IMU模型為［22］

C0Ij=wC0Ij+bg+ngj

C0Ij=qTC0Ij （aC0Ij+gC0）+ba+naj（1）

其中：wC0Ij為陀螺儀在IMU坐標系下真實的角速度值;C0Ij為陀螺儀在IMU坐標系下測量得到的角速度值;aC0Ij為加速度計在IMU坐標系下真實的加速度值;C0Ij為加速度計在IMU坐標系下測量得到的加速度值。bg、ba的導數服從高斯分布。

bg·～Euclid Math TwoNAp（0，σbg），bg·～Euclid Math TwoNAp（0，σba）

其中：ngj、naj分別為陀螺儀、加速度計偏置的隨機游走噪聲，服從高斯分布。

ngj～Euclid Math TwoNAp（0，σg），naj～Euclid Math TwoNAp（0，σa）

在相機坐標系下，將j時刻機器人位置、速度、旋轉方向對分別求導，得

p·C0Ij=vC0Ij

v·C0Ij=aC0Ij

q·C0Ij=qC0Ij012wC0Ij

（2）

其中：pC0Ij為從相機第一幀坐標系到IMU第j幀坐標系的平移向量；vC0Ij為相機坐標系下，j時刻機器人速度，用三維向量表示；qC0Ij為從IMU第i幀坐標系到相機第一幀坐標系的旋轉矩陣，用四元數表示。p·C0Ij、v·C0Ij、q·C0Ij表示將機器人的位置、速度、旋轉方向分別對時間求導。

對式（2）進行積分，得到在相機坐標系下j時刻機器人的位置、速度、旋轉方向。

pC0Ij=pC0Ij－1+vC0IjΔt+12aΔt2

vC0Ij=vC0Ij－1+aΔt

qC0Ij=qC0Ij－1112wΔt（3）

其中：Δt為i時刻與j時刻的時間間隔，采用中值法得到j時刻的加速度a和角速度w。

a=12［qC0Ij－1（aC0Ij－1－ba）－gC0+qC0Ij（aC0Ij－ba）－gC0］

w=12［（wC0Ij－1－bg）+（wC0Ij－bg）］（4）

但是在優化位姿過程中，當前幀的速度和旋轉矩陣等會發生改變，需要重新進行積分，計算量較大，故引入預積分［22，23］：

qC0Ij=qC0IkqIkIj（5）

其中：表示四元數乘法［22］。將式（5）代入式（4）更新加速度：

a=qC0Ik12［qIkIj－1（aC0Ij－1－ba）+qIkIj（aC0Ij－ba）］－gC0

引入預積分之后運動學模型的離散形式為

pC0Ij=pC0Ij－1+vC0IjΔt－12gC0Δt2+qC0IkαIkIj

vC0Ij=vC0Ij－1－gC0Δt+qC0IkβIkIj

qC0Ij=qC0Ij－1γIkIj（6）

其中：

αIkIj=12［qIkIj－1（aC0Ij－1－ba）+qIkIj（aC0Ij－ba）］Δt2

βIkIj=12［qIkIj－1（aC0Ij－1－ba）+qIkIj（aC0Ij－ba）］Δt

γIkIj=112（（wC0Ij－1－bg）+（wC0Ij－bg））（7）

為預積分量，計算式（6）得到相機坐標系下機器人的位置、速度、旋轉方向。

2.2預測位姿以及特征點位置

結合2.1節得到的結果，計算相機坐標系下i和j兩幀之間的轉換矩陣TCjCi 。

TC0CjTCjCi=TC0Ci（8）

其中：

TC0Ci=TC0IiTIiCi，TC0Cj=TC0IjTIjCj（9）

將式（9）代入式（8）化簡，分別得到i和j兩幀之間旋轉矩陣RCjCi和平移向量tCjCi。

RCjCi=R-1ICR-1C0IjRC0IiRIC

tCjCi=R-1IC［R-1C0Ij（RC0IitIC+tC0Ii-tC0Ij）-tIC］（10）

其中：RI-1C為從IMU坐標系到相機坐標系的旋轉矩陣；RC0I-1j為從相機第一幀坐標系到IMU第j幀坐標系的旋轉矩陣；RC0Ii為從IMU第i幀坐標系到相機第一幀坐標系的旋轉矩陣；RIC為從相機坐標系到IMU坐標系的旋轉矩陣；tIC為從IMU坐標系到相機坐標系的平移向量；tC0Ii為從相機第一幀坐標系到IMU第i幀坐標系的平移向量。

將第i幀的特征點歸一化坐標pi映射到第j幀，得到

pj=RCjCip*i+tCjCi（11）

其中：pi*為pi的齊次坐標，大小為3×1的向量;pj為IMU預測得到的特征點在第j幀下的歸一化坐標。

3IMU去除運動模糊光流特征點跟蹤算法

運動模糊圖像的復原模型可以描述為一個清晰圖像經過退化函數作用之后，再加上一個噪聲項。設一幅清晰圖像f（x，y）經過退化函數卷積，則產生的退化圖像g（x，y），具體為

g（x，y）=f（x，y）*h（x，y）+n（x，y）（12）

其中：h（x，y）表示模糊算子或點擴散函數;*表示卷積;n（x，y）表示加性噪聲。

復原模型可以根據有無運動先驗分為盲復原和非盲復原。由于通過IMU得到的加速度和角速度可以確定運動信息，所以研究重點放在非盲復原模型。

3.1融合IMU先驗信息的運動模糊非盲復原模型

當兩幅圖像的時間戳，即時間間隔tij已知時，通過式（7）得到了機器人在兩幀之間的速度信息，進而利用式（11）計算特征點在兩幀的歸一化坐標（ui，vi）、（uj，vj），于是得到機器人運動模糊方向的角度和長度：

θ=arctanvj－viuj－uiL=v×tij（13）

其中：v表示機器人速度;tij表示時間間隔。通過運動模糊的方向θ和長度L，則可確定運動模糊的點擴散函數寬度b、高度h。

b=L×cos θ

h=L×sin θ（14）

點擴散函數h（x，y）由以下隱函數表示為

h（x，y）=y=［bh×x］x=0，1，…，h－1

x=［hb×y］y=0，1，…，b－1（15）

根據點擴散函數建立模糊復原模型f（x，y）［24］為

f（x，y）=argminf（x，y）‖g（x，y）－f（x，y）h（x，y）‖2（16）

其中：‖g（x，y）－f（x，y）h（x，y）‖2為圖像保真項，保證清晰圖像經過點擴散函數作用后，與模糊圖像一致。式（16）可通過快速傅里葉變換求解［19］。

3.2改進原有光流法優化求解匹配特征點

本文在原有LK光流假設兩幀圖像之間同一特征點光度不變的基礎上，首先加入梯度不變假設：

I1（x，y）=I2（x+Δx，Δy）

其充分利用特征點與相鄰像素的信息，保證正確的匹配對。其次，為了防止過擬合，使用第二幀圖像梯度L1范數模擬稀疏噪聲，構建正則化項，最終模型建立如下：

（Δx，Δy）=argmin（Δx，Δy）（‖I1（x，y）－I2（x+Δx，Δy）‖2+

α‖I1（x，y）－I2（x+Δx，Δy）‖2+β‖I2（x，y）‖1）（17）

其中：I1（x，y）表示參考幀在像素坐標（x，y）處圖像的灰度值；I2（x+Δx，Δy）表示當前幀在像素坐標（x+Δx，Δy）處圖像的灰度值；I1（x，y）表示參考幀在像素坐標（x，y）處圖像的梯度值；I2（x+Δx，Δy）表示當前幀在像素坐標（x+Δx，Δy）處圖像的梯度值；α為梯度權重系數;β為稀疏噪聲項權重系數。

在2.2節中，已經通過IMU預測得到第二幀特征點的位置，將預測位置作為高斯牛頓非線性優化算法初始值，并在原來固定步長基礎上加入BB可變步長進行迭代。

將式（17）非線性函數F（X）進行泰勒展開至二階：

F（X）=F（Xk）+F（Xk）（X－Xk）+

12！（X－Xk）THXk（X－Xk）

（18）

其中：X為優化變量;Xk為優化變量在第k步的取值;F（Xk）為非線性函數F（X）在Xk處的取值;F（Xk）為非線性函數F（X）在Xk處的梯度值;HXk為F（X）在Xk處的海塞矩陣。

對式（18）求導，令其為0，得到優化變量X=Xk-H-1Xkf（Xk），則優化方向dk為

dk=-H-1XkF（Xk）（19）

下面對步長進行求解，對于一般的二次函數優化問題

F（X）=12XTAX+bTX（20）

其中：A表示二次函數的系數矩陣;b為偏置。對式（20）求導，得一階導函數F′（X）=AX+b，則第k步迭代梯度為

F（Xk）=AXk+b（21）

將式（21）代入式（19），加入常數高斯牛頓迭代步長gk，近似迭代方向：

dk=-H-1XkF（Xk）≈-gkIF（Xk）=-（g-1kI）-1F（Xk）（22）

其中：I為單位矩陣。定義自變量的差Sk－1、梯度差Yk－1：

Sk－1=Xk－Xk－1，Yk－1=F（Xk）－F（Xk－1）（23）

對式（21）在第k與第k－1處做差：

F（Xk）－F（Xk－1）=AXk－AXk－1=ASk－1（24）

則得到擬牛頓方程：

ASk-1=（g-1kI）Sk-1=Yk-1（25）

在式（25）中令δ=g－1，則第k步步長：

g－1k=argming12‖Sk－1δ－Yk－1‖2（26）

求得式（26）的最小二乘解：

gk=STk－1Sk－1STk－1Yk－1（27）

由式（27）可以看出，gk僅與第k步自變量狀態信息和相鄰兩步函數的梯度有關，與其他變量無關。

4實驗結果與分析

本文所有實驗均在同一平臺進行，平臺主要配置為AMD Ryzen 5 3500U with Radeon Vega Gfx 四核筆記本處理器，內存為12 GB，基準頻率為2.10 GHz，Ubuntu18.04操作系統。

首先單獨對算法本身的性能進行評估，將本文算法與VINS-Mono框架中匹配算法LK光流法、ORB特征點檢測算法對比，測試在兩幀之間的匹配運行時間和匹配精度。其次，為了進一步驗證本文優化算法的準確性以及在系統框架中的穩定性，將該算法集成到VINS-Mono系統框架，與原VINS-Mono、PL-VIO［25］、VINS-Fusion［26］單目相機加IMU（Mono-IMU）、VINS-Fusion雙目相機加IMU（Stereo-IMU）進行對比，評價定位精度。

實驗選用的數據集是機器人微型飛行器（micro aerial vehicle，MAV）上收集的視覺慣性數據集EuRoC，飛行場景是蘇黎世聯邦理工學院ETH的一個機器工廠和兩個普通房間場景。為了驗證本文算法在普通場景數據下的有效性，該算法使用了三個場景不同難度的九個序列，即MH_02_easy、MH_03_medium、MH_04_difficult、V1_01_easy、V1_02_ medium、V1_03_difficult、V2_01_easy、V2_02_medium、V2_03_difficult。

4.1兩幀之間算法運行時間對比

本文算法與LK光流法同屬于光流法，在比較匹配的運行時間時，實驗控制特征點提取算法相同，均采用ShiTomasi算法的角點檢測，利用OpenCV中的函數cv：：goodFeaturesToTrack。提取算法參數：最大角點數目500，質量水平系數0.01，最小距離30，其他參數都采用cv：：goodFeaturesToTrack默認參數。將算法循環1 000次，取平均值，本文算法的平均運行時間為1.361 ms；OpenCV中LK光流法為2.892 ms，速度提高52.939%，隨機取20次實驗結果如圖2所示。

當本文算法和特征點法比較時，由于特征點法利用描述子進行匹配，所以在比較過程中，需要加入計算描述子的時間，另外，描述子與特征點檢測算法往往是一一對應的，所以最終的比較策略是將特征點提取時間加匹配時間一起進行比較。同樣將算法循環1 000次，取平均值，本文算法的平均運行時間為8.382 ms；OpenCV中ORB特征點法特征點提取與匹配平均時間為35.110 ms，速度提高76.125%。結果如圖3所示。

4.2兩幀之間匹配精度評估

如果兩幀的特征點x1和x2是正確匹配點，則應該滿足對極約束：

xT2t^21R21x1=0（28）

由于測量存在誤差，上式幾乎不可能成立，故求其最小二乘誤差最小解：

all_error=∑sizei=1error（i）TW－1error（i）（29）

其中：error（i）為觀測數據;W為協方差矩陣。當最小二乘解越小時，兩幀之間匹配程度也越高，定義匹配分數為

score=∑sizei=1（1/（error（i）+1））TW－1（1/（error（i）+1））（30）

根據以上定義的評估標準計算匹配分數，如表1所示。本文算法精度比光流法精度提高了53.295%，這得益于提出的改進優化模型式（17），即在原有光度不變假設的基礎上引入梯度誤差和噪聲稀疏項。這一對比結果充分說明了改進優化模型的合理性和有效性。另外，當與OpenCV中ORB特征點檢測算法相比時，雖然算法精度降低了13.554%，但其運行時間提高了76.125%，考慮實際情況中的實時性要求，這個精度結果也是可以接受的。

圖4～6分別為傳統高斯牛頓法優化結果、OpenCV中匹配優化結果以及本文算法匹配優化結果，展示選用的圖片為MH_02_easy序列中相鄰兩張圖片，并以左圖特征點順序為匹配對信息編號。圖4在沒有給定初始值的情況下，默認初始值旋轉矩陣為單位矩陣，平移向量為零向量。當物體運動過快，圖像模糊程度較高，旋轉位姿與默認初始值偏差較大，誤匹配現象較多。對比圖5、6可以看出，由于給定了初始值進行當前幀局部鄰域搜索匹配點，基本避免了特征點對的誤匹配，OpenCV中光流法仍然會出現誤匹配現象，如編號2，11，20，21，22，27，33，39，49等。

4.3系統定位精度評估

本節將提出的改進光流匹配算法移植到經典的VINS-Mono前端中，其目的是進一步驗證本文優化方法在VI-SLAM系統上的準確性和穩定性。為此，將改進的框架與VINS-Mono、PL-VIO、Mono-IMU、Stereo-IMU通過EVO評估工具［27］進行對比。其中，PL-VIO是在VINS-Mono前端加入點線特征改進；Mono-IMU、Stereo-IMU是VINS-Fusion在VINS-Mono基礎上加入多傳感器改進，并且是KITTI Visual Odometry 榜單中排名最靠前的開源方案及改進算法。

與文獻［12］一樣，將對齊之后絕對軌跡誤差（absolute tra-jectory error，ATE）的均方根誤差（root mean square error，RMSE）和誤差均值作為評估指標。為防止結果的偶然性，進行20次

實驗取均值對比，如表2所示。本文算法在EuRoC數據集上均優于PL-VIO框架，與VINS-Fusion性能相近，并且經

過多次實驗，VINS-Fusion在V2_03_difficult數據集上會出現偏離真實軌跡特別遠的情況。值得注意的是，由于MH_03_medium與V2_01_easy數據集在兩個主導方向x和y的陀螺儀、加速度計偏置較其他數據集平均誤差較大，不符合本文算法強依賴于整個系統運動的精確初始化這一基本假設，所以這兩個序列性能差于VINS-Mono。更具體而言，如果沒有好的初始值，優化算法將退化為無初始化的高斯牛頓法，這不僅會增加尋優時間，也會影響系統精確度。除這兩個數據集外，其他序列都有所提升，最為顯著的是V2_03_difficult序列，其具有飛行器擺動較快、圖像模糊程度較高等特點，是具有挑戰性的數據集之一，其RMSE降低18.066%，誤差均值降低28.902%。另外，圖7、8展示了本文算法與WINS-Mono在V2_03_difficult序列估計軌跡與真實軌跡比較熱力圖；圖9、10展示了V2_03_difficult序列中本文算法與VINS-Mono算法相對軌跡誤差的均方根、均值、標準差。通過這些比較，表明本文算法在相機劇烈抖動情況下魯棒性更好。

5結束語

本文提出了一種融合視覺和IMU對圖像進行去除運動模糊預處理，再改進光流法對特征點匹配的新算法。該算法充分利用IMU的運動信息，實現去除圖像運動模糊信息并為迭代匹配算法提供初始值，減小特征點對誤匹配概率。該算法是一個通用的模塊，可以移植到其他VI-SLAM框架中。通過在三個不同難度層次的數據集上進行驗證表明，該算法本身效率和精度都優于OpenCV庫函數中光流匹配算法，并且將本文算法集成到最先進的VIO框架VINS-Mono上，在位姿估計方面提高了精度。未來可以對去模糊算法作進一步研究，加入正則項去除偽影，進一步提高匹配精度。

參考文獻：

[1]Durrant-Whyte H， Bailey T. Simultaneous localization and mapping： part I［J］.IEEE Robotics amp; Automation Magazine，2006，13（2）：99-110.

［2]Pradeep V， Rhemann C， Izadi S， et al. MonoFusion： real-time 3D reconstruction of small scenes with a single Web camera［C］//Proc of IEEE International Symposium on Mixed and Augmented Reality.Piscataway，NJ：IEEE Press，2013：83-88.

［3]郭金輝，陳秀萬，王媛.視覺慣性SLAM研究進展［J］.火力與指揮控制，2021，46（1）：1-8.（Guo Jinhui， Chen Xiuwan， Wang Yuan. Research progress of visual inertia SLAM［J］.Fire Control and Command Control，2021，46（1）：1-8.）

［4]胡立華，左威健，張繼福.采用逆近鄰與影響空間的圖像特征誤匹配剔除方法［J］.計算機輔助設計與圖形學學報，2022，34（3）：449-458.（Hu Lihua， Zuo Weijian， Zhang Jifu. The image feature mismatch elimination method of inverse nearest neighbor and influence space is adopted［J］.Journal of Computer-Aided Design and Computer Graphics，2022，34（3）：449-458.）

［5]付煜，鄭爽，別桐，等.融合點線特征的視覺慣性SLAM算法［J］.計算機應用研究，2022，39（2）：349-355.（Fu Yu， Zheng Shuang， Bie Tong， et al. Visual inertial SLAM algorithm with fused point and line features［J］.Application Research of Computers，2022，39（2）：349-355.）

［6]Triggs B， Mclauchlan P F， Hartley R I， et al. Bundle adjustment：a modern synthesis［C］//Proc of International Workshop on Vision Algorithms. Berlin：Springer，1999：298-372.

［7]傘紅軍，王汪林，陳久朋，等.改進誤匹配剔除的SLAM算法［J］.軟件導刊，2021，20（8）：99-104.（San Hongjun， Wang Wanglin， Chen Jiupeng， et al. Improved SLAM algorithm for error matching elimination［J］.Software Guide，2021，20（8）：99-104.）

［8]Lowe D G. Distinctive image features from scale-invariant keypoints［J］.International Journal of Computer Vision，2004，60（2）：91-110.

［9]Rosten E， Drummond T. Machine learning for high-speed corner detection［C］//Proc of European Conference on Computer Vision.Berlin：Springer，2006：430-443.

［10]Lucas B D， Kanade T. An iterative image registration technique with an application to stereo vision［C］//Proc of the 7th International Joint Conference on Artificial intelligence.San Francisco，CA：Morgan Kaufmann Publishers Inc.，1981：674-679.

［11］Engel J， Schps T， Cremers D. LSD-SLAM： large-scale direct monocular SLAM［C］//Proc of European Conference on Computer Vision.Berlin：Springer，2014：834-849.

［12］Qin Tong， Li Peiliang， Shen Shaojie. VINS-Mono： a robust and versatile monocular visual-inertial state estimator［J］.IEEE Trans on Robotics，2018，34（4）：1004-1020.

［13］Guan Qi， Wei Guoliang， Wang Licheng， et al. A novel feature points tracking algorithm in terms of IMU-aided information fusion［J］.IEEE Trans on Industrial Informatics，2021，17（8）：5304-5313.

［14］Leutenegger S， Lynen S， Bosse M， et al. Keyframe-based visuali-nertial odometry using nonlinear optimization［J］.The International Journal of Robotics Research，2015，34（3）：314-334.

［15］Mourikis A I， Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2007：3565-3572.

［16］Davison A J. Active search for real-time vision［C］//Proc of the 10th IEEE International Conference on Computer Vision.Piscataway，NJ：IEEE Press，2005：66-73.

［17］Shi Jianbo， Tomasi C. Good features to track［C］//Proc of IEEE Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，1994：593-600.

［18］Fergus R， Singh B， Hertzmann A， et al. Removing camera shake from a single photograph［C］//Proc of ACM SIGGRAPH.New York：ACM Press，2006：787-794.

［19］Chen Liang， Fang Faming， Wang Tingting， et al. Blind image deblurring with local maximum gradient prior［C］//Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2019：1742-1750.

［20］Mur-Artal R， Tardós J D. ORB-SLAM2： an open-source slam system for monocular， stereo， and RGB-D cameras［J］.IEEE Trans on Robotics，2017，33（5）：1255-1262.

［21］Qin Tong， Shen Shaojie. Robust initialization of monocular visual inertial estimation on aerial robots［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2017：4225-4232.

［22］Lupton T， Sukkarieh S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions［J］.IEEE Trans on Robotics，2012，28（1）：61-76.

［23］Shen Shaojie， Michael N， Kumar V. Tightly-coupled monocular visual inertial fusion for autonomous flight of rotorcraft MAVs［C］//Proc of IEEE International Conference on Robotics and Automation.Pisca-taway，NJ：IEEE Press，2015：5303-5310.

［24］Chen Liang， Fang Faming， Lei Shen， et al. Enhanced sparse model for blind deblurring［C］//Proc of European Conference on Computer Vision.Berlin：Springer，2020：631-646.

［25］He Yijia， Zhao Ji， Guo Yue， et al. PL-VIO： tightly-coupled mono-cular visual-inertial odometry using point and line features［J］.Sensors，2018，18（4）：1159.

［26］Qin Tong， Pan Jie， Cao Shaozu， et al. A general optimization-based framework for local odometry estimation with multiple sensors［EB/OL］.（2019-01-11）.http：//doi.org/10.48550/arxiv.1901.03638.

［27］Grupp M. EVO： Python package for the evaluation of odometry and SLAM［J/OL］.（2017-01-01）［2022-04-11］.https：//github.com/MichaelGrupp/evo.

收稿日期：2022-02-23；

修回日期：2022-04-14

基金項目：上海市“科技創新行動計劃”國內科技合作項目（20015801100）

作者簡介：欒小珍（1997-），女，山東聊城人，碩士研究生，主要研究方向為視覺SLAM、計算機視覺；魏國亮（1973-），男（通信作者），河南新鄉人，教授，博導，博士，主要研究方向為非線性系統優化、機器視覺、隨機系統（guoliang.wei@usst.edu.cn）；蔡潔（1996-），女，江蘇徐州人，博士研究生，主要研究方向為視覺SLAM.