視覺(jué)與微慣導(dǎo)系統(tǒng)中多傳感器信息融合導(dǎo)航方法研究

王海，劉峰寰

沈陽(yáng)理工大學(xué)

針對(duì)視覺(jué)與微慣導(dǎo)系統(tǒng)在無(wú)人機(jī)等領(lǐng)域應(yīng)用為研究背景，研究了在視覺(jué)微慣導(dǎo)系統(tǒng)中基于多傳感器信息融合的姿態(tài)測(cè)量方法。測(cè)試結(jié)果為視覺(jué)與微慣性傳感器姿態(tài)測(cè)量方法融合，以融合測(cè)姿結(jié)果作為觀測(cè)數(shù)據(jù)，隨后通過(guò)卡爾曼濾波器進(jìn)一步準(zhǔn)確估計(jì)載體姿態(tài)。采用研制的視覺(jué)與微慣導(dǎo)組件搭建了實(shí)驗(yàn)平臺(tái)，并進(jìn)行載體姿態(tài)測(cè)量實(shí)驗(yàn)，最后簡(jiǎn)單驗(yàn)證了融合算法的有效性。

同步定位與地圖構(gòu)建（SLAM）越來(lái)越廣泛地應(yīng)用到地面移動(dòng)機(jī)器人、水下機(jī)器人、無(wú)人機(jī)乃至行星探測(cè)等領(lǐng)域，是當(dāng)前工業(yè)界和學(xué)術(shù)界備受關(guān)注的熱門(mén)研究方向。SLAM技術(shù)是指某種特定的載體，搭載了一種或多種不同的傳感器，在環(huán)境先驗(yàn)未知的情況下，在運(yùn)動(dòng)過(guò)程中實(shí)現(xiàn)自身的定位以及周圍環(huán)境的建圖。

導(dǎo)航的前提就是定位與建圖，在不同應(yīng)用場(chǎng)景下實(shí)現(xiàn)定位與建圖，需要引入不同的傳感器。激光雷達(dá)和視覺(jué)（Visual）是兩大類根據(jù)所用傳感器不同而流行的SLAM主要方法。基于激光雷達(dá)的SLAM方法能夠獲取高精度的地圖，但存在價(jià)格昂貴、空間分辨率低等缺點(diǎn)。基于視覺(jué)的SLAM方法依賴的傳感器具有體積小、功耗低、場(chǎng)最辨識(shí)能力強(qiáng)及感知紋理信息豐富等優(yōu)點(diǎn)（視覺(jué)SLAM技術(shù)的進(jìn)展與應(yīng)用），但對(duì)于環(huán)境紋理特征不明顯及圖像采集幀率很快的情況，其運(yùn)動(dòng)估計(jì)能力會(huì)降低。

低成本視覺(jué)微慣導(dǎo)集成模塊的設(shè)計(jì)

在特殊環(huán)境下，實(shí)驗(yàn)室自主設(shè)計(jì)了一套完整的視覺(jué)與微慣導(dǎo)融合系統(tǒng)的位姿檢測(cè)，包括數(shù)字信號(hào)開(kāi)發(fā)板（Raspberry Pi 4B）、 電 源 模 塊、 攝 像 頭 模 塊、GY-85模塊等。攝像頭模塊使用手機(jī)自帶攝像頭及單目攝像頭，MPU6050模塊通過(guò)慣導(dǎo)解算得到六自由度的位姿信息。利用擴(kuò)展卡爾曼濾波算法對(duì)這兩個(gè)通道的姿態(tài)結(jié)果進(jìn)行融合，并得到姿態(tài)估計(jì)結(jié)果。

多傳感器姿態(tài)測(cè)量方法

視覺(jué)傳感器以及陀螺儀、加速度計(jì)、磁阻傳感器共同構(gòu)成了視覺(jué)與微慣導(dǎo)融合系統(tǒng)。四元數(shù)和歐拉角方法可通過(guò)陀螺儀輸出完成姿態(tài)更新，求解載體的橫滾、俯仰和偏航角度。解析加速度計(jì)的輸出并進(jìn)行計(jì)算，以獲得俯仰和滾轉(zhuǎn)角的兩個(gè)主要情況。在紋理不豐富或高動(dòng)態(tài)視覺(jué)丟失的情況下，在單目相機(jī)初始化時(shí)，由于單目初始化只能得到深度的初值，無(wú)法得到絕對(duì)尺度信息，只能取到所有特征點(diǎn)的深度中值。不同微慣性器件有著不同噪聲的頻率特點(diǎn)，視覺(jué)與微慣導(dǎo)在不同環(huán)境有著各自的優(yōu)勢(shì)，多重?cái)?shù)據(jù)通過(guò)互補(bǔ)濾波器的融合，提升了載體姿態(tài)測(cè)量的精準(zhǔn)度。

多傳感器信息融合與濾波算法

通過(guò)視覺(jué)與微慣導(dǎo)解算均可獲得位姿信息，視覺(jué)方法依賴的傳感器具有體積小、功耗低、場(chǎng)最辨識(shí)能力強(qiáng)及感知紋理信息豐富等優(yōu)點(diǎn)，但對(duì)于環(huán)境紋理特征不明顯及圖像采集幀率很快的情況，其運(yùn)動(dòng)估計(jì)能力會(huì)下降。微慣導(dǎo)解算短時(shí)、定位精度高、自主性強(qiáng)，同時(shí)可以完全基于加速度計(jì)與陀螺儀完成位姿解算，在紋理特征不明顯或快速運(yùn)動(dòng)的情況下可以達(dá)到良好的測(cè)量效果。通過(guò)卡爾曼濾波實(shí)現(xiàn)兩者的信息融合，一方面，相機(jī)在運(yùn)動(dòng)過(guò)程中可以確定出位姿以及能夠?yàn)橹車h(huán)境建立出3D模型；另一方面，由于微慣導(dǎo)通過(guò)解算可以直接獲得自身的運(yùn)動(dòng)信息，與視覺(jué)融合時(shí)能夠?yàn)槠涮峁┲亓Ψ较颍瑫r(shí)也使橫滾與俯仰角可觀，能夠達(dá)到更精確及魯棒的運(yùn)動(dòng)估計(jì)。

圖1 系統(tǒng)框圖。

特征提取、匹配、篩選

OFAST相比較其他算法，擁有尺度的旋轉(zhuǎn)和描述。類似計(jì)算重心指向更亮的區(qū)域，計(jì)算角度、圖像梯度。搭建圖像金字塔并在金字塔不同層的每一層分別提取一個(gè)FAST，可以得到多種尺度下的FAST特征。在這種情況下，可以認(rèn)為FAST具有一定程度上的尺度不變性。根據(jù)特征點(diǎn)的方向計(jì)算旋轉(zhuǎn)后的BRIEF描述子，使其具有旋轉(zhuǎn)不變性。

特征匹配是對(duì)兩幀圖像間的描述子進(jìn)行匹配，提供給后續(xù)位姿估計(jì)。假設(shè)分別在圖像1、2中提取到相同個(gè)數(shù)的ORB特征點(diǎn)，然后計(jì)算對(duì)應(yīng)的BRIEF描述子。ORB的特征匹配常用暴力匹配法進(jìn)行，即對(duì)第一個(gè)圖中的一個(gè)特征點(diǎn)與第二個(gè)圖中的每一個(gè)特征點(diǎn)測(cè)量BRIEF描述子的漢明距離，對(duì)這些漢明距離進(jìn)行排序，設(shè)定小于一定閾值的點(diǎn)對(duì)作為匹配點(diǎn)。

圖2 經(jīng)過(guò)篩選后的匹配。

暴力匹配的操作簡(jiǎn)單，但是結(jié)果會(huì)存在大量誤匹配，誤匹配影響著后續(xù)的位姿估計(jì)的速度以及精度。使用暴力匹配，然后根據(jù)距離應(yīng)相等或相近的正確匹配點(diǎn)對(duì)進(jìn)行提純，提高匹配的準(zhǔn)確度，以減少運(yùn)行時(shí)間及提高相機(jī)運(yùn)動(dòng)解算的精度；再使用Lowe’s算法，獲取第一個(gè)圖中有誤匹配的關(guān)鍵點(diǎn)，并找到第二個(gè)圖中在暴力匹配時(shí)得到的誤匹配的兩個(gè)關(guān)鍵點(diǎn)中歐式距離最近的那個(gè)，如果最近的關(guān)鍵點(diǎn)的歐式距離與另外一個(gè)關(guān)鍵點(diǎn)的歐式距離之比小于預(yù)設(shè)的閾值，那么接受這一對(duì)匹配點(diǎn)。最后利用篩選后的特征點(diǎn)對(duì)構(gòu)建非線性優(yōu)化問(wèn)題，求解變換矩陣。

位姿估計(jì)

單目視覺(jué)里程計(jì)中，常通過(guò)經(jīng)過(guò)篩選后的匹配并根據(jù)對(duì)極幾何以及本質(zhì)矩陣等理論，估計(jì)出粗略的運(yùn)動(dòng)。為了提高這種粗略運(yùn)動(dòng)估計(jì)的準(zhǔn)確性，基于將優(yōu)化問(wèn)題表現(xiàn)成圖的形式的理論進(jìn)行幀間運(yùn)動(dòng)估計(jì)，在幀間運(yùn)動(dòng)估計(jì)問(wèn)題中，可將不同時(shí)刻的相機(jī)位姿和路標(biāo)點(diǎn)視為頂點(diǎn)，不同位姿之間的關(guān)系當(dāng)成邊。對(duì)于幀間的運(yùn)動(dòng)估計(jì)和尺度未知的特征點(diǎn)位置解算問(wèn)題，可基于圖優(yōu)化理論進(jìn)行解算。

圖3 運(yùn)行過(guò)程。

假設(shè)幀間運(yùn)動(dòng)R，t，相機(jī)內(nèi)參矩陣為K，特征點(diǎn)的世界坐標(biāo)為Xi，兩相機(jī)坐標(biāo)系下的相機(jī)坐標(biāo)分別為Ci1、Ci2，對(duì)應(yīng)的深度值分別為S1、S2，相機(jī)1的位姿為單位矩陣，根據(jù)投影關(guān)系，有：

然而由于噪聲影響，投影關(guān)系不能準(zhǔn)確成立，用二范數(shù)量化每一個(gè)特征點(diǎn)存在的誤差，并對(duì)所有特征點(diǎn)的誤差項(xiàng)進(jìn)行求和，可得目標(biāo)優(yōu)化問(wèn)題如下。

也稱最小化重投影誤差問(wèn)題，為非線性、非凸的優(yōu)化問(wèn)題。實(shí)際的求解過(guò)程中是調(diào)整每個(gè)Xi，使得每個(gè)誤差項(xiàng)都盡量地小，因此也可稱為捆集調(diào)整（bundle adjustment，BA）。

圖4 軌跡圖。

求解兩幀之間的運(yùn)動(dòng)，需設(shè)置相機(jī)位姿結(jié)點(diǎn)、特征點(diǎn)的空間位置結(jié)點(diǎn)，以及重投影誤差邊。優(yōu)化求解前，需要選擇求解器和迭代算法：①選擇線性方程求解器；②選擇塊求解器；③選擇列文伯格-馬夸爾特（Levenburg-Marquadt，LM）迭代算法。

為解算各幀的估計(jì)位姿，幀間運(yùn)動(dòng)估計(jì)過(guò)后，根據(jù)數(shù)據(jù)集中的連續(xù)幀，首先固定第一幀的位姿，利用估計(jì)出的運(yùn)動(dòng)進(jìn)而推算出接下來(lái)各幀的估計(jì)位姿。

圖5 方向示意圖。

實(shí)驗(yàn)結(jié)果

本文試驗(yàn)采用了kitti數(shù)據(jù)集進(jìn)行測(cè)試和評(píng)估，它包含了市區(qū)、鄉(xiāng)村和高速公路等場(chǎng)景采集的真實(shí)圖像數(shù)據(jù)，每張圖像中最多達(dá)15輛車和30個(gè)行人，還有各種程度的遮擋與截?cái)唷Ｒ?0Hz的頻率采樣及同步。

結(jié)論

本文以無(wú)人機(jī)在無(wú)衛(wèi)星條件下的位姿估計(jì)為背景，設(shè)計(jì)了一個(gè)多傳感器組合定位系統(tǒng)，其中包括單目視覺(jué)傳感器、IMU，實(shí)現(xiàn)了視覺(jué)與慣導(dǎo)的融合定位。提出了基于最小二乘法實(shí)現(xiàn)單目視覺(jué)尺度的最優(yōu)估計(jì); 基于擴(kuò)展卡爾曼濾波實(shí)現(xiàn)視覺(jué)慣導(dǎo)融合定位。

本文設(shè)計(jì)實(shí)現(xiàn)的系統(tǒng)還可進(jìn)一步完善，包括增加傳感器的融合數(shù)量以及采集精度可以獲得更高的精度，以及減少視覺(jué)丟失的情況。