鹿珂珂，王雅平，王 超，吳俊峰

（海軍航空大學(xué)，山東 煙臺(tái) 264001）

0 引言

近幾年發(fā)展最快的新科技，當(dāng)屬無(wú)人機(jī)的強(qiáng)勢(shì)崛起，軍用無(wú)人機(jī)在監(jiān)視與偵察、特定目標(biāo)追蹤、戰(zhàn)斗毀傷評(píng)估、火力支援等作戰(zhàn)任務(wù)中發(fā)揮了越來(lái)越重要的作用［1］。在民用領(lǐng)域，無(wú)人機(jī)也正展現(xiàn)出越來(lái)越豐富的可能性，航拍、植保，替代電力工人巡線等。為了達(dá)到穩(wěn)定、魯棒的飛行效果，無(wú)人機(jī)通常需要一種精確且實(shí)時(shí)的位姿狀態(tài)估計(jì)方法。最常用的導(dǎo)航方式為衛(wèi)星導(dǎo)航，但是只能進(jìn)行大范圍粗略地估計(jì)，同時(shí)衛(wèi)星信號(hào)易受干擾和欺騙。此外，在室內(nèi)等衛(wèi)星導(dǎo)航信號(hào)失效的場(chǎng)景下，無(wú)人機(jī)無(wú)法實(shí)時(shí)定位，僅使用單目相機(jī)的方法，由于其體積小、成本低和硬件設(shè)置簡(jiǎn)單,而獲得了社會(huì)的極大關(guān)注［2-6］。當(dāng)利用單目視覺(jué)為無(wú)人機(jī)進(jìn)行定位時(shí)，需要進(jìn)行初始化，通過(guò)初始化求解得到位姿，然后作為后端優(yōu)化的初始值，能夠有效地使后端的非線性優(yōu)化收斂，這在實(shí)際中得到了有效的驗(yàn)證［7-10］。而視覺(jué)里程計(jì)的實(shí)現(xiàn)通常有3 種方法［11-12］：使用兩組二維像素坐標(biāo)使用三角約束求解；利用三維點(diǎn)和它們?cè)谙鄼C(jī)的投影位置時(shí)，利用多點(diǎn)透視成像（perspective-n-point，PnP）求解；利用兩組三維點(diǎn)時(shí)，使用光束平差法求解。然而，上述方法不同程度存在耗時(shí)較長(zhǎng)和位姿估計(jì)不準(zhǔn)確的問(wèn)題。因此，在上述研究基礎(chǔ)上，提出基于多視圖幾何中的基本原理，組合利用滑窗方法、三角約束、PnP 和光束平差法，快速準(zhǔn)確地實(shí)現(xiàn)無(wú)人機(jī)位姿估計(jì)初始化的方法，并通過(guò)數(shù)據(jù)集驗(yàn)證了純視覺(jué)初始化效果。實(shí)驗(yàn)結(jié)果顯示，本文提出的方法在運(yùn)動(dòng)充分的情況下，僅需2～3 s時(shí)間，即可在15幀的活動(dòng)窗口內(nèi)實(shí)現(xiàn)快速準(zhǔn)確的位姿估計(jì)初始化，在EuRoC MAV 數(shù)據(jù)集［13］的實(shí)驗(yàn)結(jié)果表明，本文提出的算法在初始化時(shí)間和精度上都優(yōu)于ORB-SLAM3［14］和S-MSCKF［15］算法，位姿精度在對(duì)齊后，可以有效加快后端非線性優(yōu)化的收斂速度?；谠摮跏蓟惴ǖ囊曈X(jué)里程計(jì)，取得了較為理想的實(shí)驗(yàn)效果。

1 基于多視圖幾何的初始化算法流程

基于多視圖幾何的定位初始化算法流程組合運(yùn)用了滑窗方法、三角約束、PnP 和光束平差法，具體流程如圖1所示。基于在利用單目視覺(jué)進(jìn)行無(wú)人機(jī)位姿估計(jì)過(guò)程中，隨著捕獲視覺(jué)幀的增多，路標(biāo)點(diǎn)也會(huì)增多，全局優(yōu)化的計(jì)算效率會(huì)不斷下降，因此，算法首先使用滑動(dòng)窗口方法進(jìn)行數(shù)據(jù)預(yù)處理，將優(yōu)化計(jì)算的視覺(jué)幀限制在一定的數(shù)量。由于無(wú)人機(jī)載處理器性能有限，處理所有圖像幀時(shí)，實(shí)時(shí)性較差，而且無(wú)人機(jī)懸停時(shí)，會(huì)產(chǎn)生大量信息冗余的圖像幀，從而導(dǎo)致很多無(wú)用的優(yōu)化計(jì)算，甚至使優(yōu)化問(wèn)題退化，因此，從一系列普通幀中選出關(guān)鍵幀作為局部幀的代表，記錄局部信息。初始化算法在視覺(jué)前端得到的圖像幀已經(jīng)填滿滑動(dòng)窗口時(shí)啟動(dòng)，并且通過(guò)無(wú)人機(jī)載IMU 進(jìn)行判斷，保證有充分的運(yùn)動(dòng)激勵(lì)。由于純視覺(jué)無(wú)法得知世界坐標(biāo)系的信息，算法通過(guò)檢查當(dāng)前幀和滑動(dòng)窗口中的所有幀對(duì)應(yīng)的特征點(diǎn)，找到具有穩(wěn)定跟蹤特征點(diǎn)和足夠視差的參考幀，利用對(duì)極約束求得當(dāng)前幀相對(duì)參考幀的位姿。隨后，利用特征點(diǎn)在參考幀和當(dāng)前幀上的坐標(biāo)，以及相對(duì)位姿矩陣，三角化得到特征點(diǎn)在空間中的位置，即特征點(diǎn)的深度。在已知多個(gè)特征點(diǎn)坐標(biāo)和這些點(diǎn)的像素坐標(biāo)情況下，使用PnP 求解當(dāng)前幀和參考幀之間的圖像幀位姿，隨后使用遞推迭代上述過(guò)程，求得所有空間點(diǎn)位置和圖像幀相對(duì)于參考幀的位姿，最后利用BA 算法將空間點(diǎn)位置作為優(yōu)化變量，進(jìn)行聯(lián)合優(yōu)化，得到初始化窗口中所有圖像幀的對(duì)應(yīng)位姿，完成初始化過(guò)程。

圖1 基于多視圖幾何的單目視覺(jué)定位初始化流程圖Fig.1 Flow chart of initialization of monocular visual positioning based on multi-view geometry

2 基于多視圖幾何的初始化算法實(shí)現(xiàn)

2.1 圖像預(yù)處理

真實(shí)世界中的物體在相機(jī)坐標(biāo)系中的坐標(biāo)位置，投影到拍攝圖像上的像素坐標(biāo)可以使用針孔相機(jī)模型，這一映射關(guān)系需要使用相機(jī)內(nèi)參。相機(jī)透鏡的成像過(guò)程還會(huì)引入畸變，一般需要使用畸變參數(shù)進(jìn)行校正。相機(jī)的內(nèi)參和畸變參數(shù)可以使用張正友標(biāo)定方法得到。

式中，［u,v］T為空間點(diǎn)在成像平面上的像素坐標(biāo)；PT=［X,Y,Z］T為空間點(diǎn)在相機(jī)坐標(biāo)系中的三維坐標(biāo)，矩陣K為相機(jī)的內(nèi)參矩陣。

式中，［x,y］T為空間點(diǎn)在歸一化平面上的坐標(biāo)；r為歸一化平面上空間點(diǎn)到坐標(biāo)原點(diǎn)的距離，k1,k2,k3，p1,p2為相機(jī)畸變參數(shù)。為了方便從圖像中提取特征點(diǎn)，可以對(duì)圖像進(jìn)行直方圖均衡化處理，改變圖像的對(duì)比度。

2.2 特征點(diǎn)選取與跟蹤

對(duì)每一新的圖像幀使用KLT 稀疏光流算法進(jìn)行特征跟蹤［16］，對(duì)于匹配成功的特征點(diǎn)，使用隨機(jī)采樣一致性算法（random sample consensus，RANSAC）計(jì)算基礎(chǔ)矩陣進(jìn)行誤匹配的篩選［17］。誤匹配篩選完成后，將特征點(diǎn)進(jìn)行畸變矯正，并投影到單位球上。同時(shí)檢測(cè)新的特征點(diǎn)，使每一幀圖像中具有最小數(shù)目的特征點(diǎn)［18］。在檢測(cè)新的特征點(diǎn)時(shí)，通過(guò)設(shè)置相鄰特征之間的最小間隔像素，使特征分布符合正態(tài)分布。

選擇關(guān)鍵幀考慮兩個(gè)方面：關(guān)鍵幀特征點(diǎn)數(shù)量要充足、特征點(diǎn)分布要盡量均勻；關(guān)鍵幀之間既存在約束，又有盡量少的信息冗余。因此，在當(dāng)前幀和上一關(guān)鍵幀跟蹤特征點(diǎn)的平均視差超出設(shè)定的閾值時(shí)，就將當(dāng)前幀設(shè)為關(guān)鍵幀，此外，如果當(dāng)前幀跟蹤到的特征點(diǎn)數(shù)目低于某一閾值時(shí)，為防止特征跟蹤的完全丟失，將當(dāng)前幀設(shè)為關(guān)鍵幀。需要注意的是，相機(jī)旋轉(zhuǎn)和平移都會(huì)產(chǎn)生視差，而相機(jī)純旋轉(zhuǎn)時(shí)，特征點(diǎn)將無(wú)法被三角化。

從滑動(dòng)窗口內(nèi)的第1幀開(kāi)始計(jì)算每一幀和當(dāng)前幀的匹配特征點(diǎn)，尋找與當(dāng)前幀共視點(diǎn)數(shù)較多、且視差量較大的作為參考幀。在滑動(dòng)窗口內(nèi)，記錄所有能夠被關(guān)鍵幀觀測(cè)到的特征點(diǎn)，而對(duì)于每個(gè)特征點(diǎn)，同時(shí)記錄能夠觀測(cè)到該特征點(diǎn)的圖像幀序列，以及特征點(diǎn)在對(duì)應(yīng)圖像幀中的相關(guān)信息。

2.3 對(duì)極約束和三角化

如圖2 所示，場(chǎng)景中空間點(diǎn)在不同視角下像點(diǎn)之間的約束關(guān)系稱為對(duì)極約束，基礎(chǔ)矩陣則是這種約束關(guān)系的代數(shù)表示。假設(shè)參考幀I與當(dāng)前幀C具有N對(duì)匹配好的特征點(diǎn)，基礎(chǔ)矩陣F=K-Tt∧RK-1是3×3的矩陣，而且使用的是齊次坐標(biāo)系，因此，僅用8 個(gè)匹配的特征點(diǎn)即可求解F，這種方法稱為8 點(diǎn)法。一般情況下，匹配特征點(diǎn)數(shù)N遠(yuǎn)大于8，可以采用最小二乘法求解基礎(chǔ)矩陣F，但這要求特征點(diǎn)的匹配是正確的，而實(shí)際情況中，往往存在外點(diǎn)，采用RANSAC算法來(lái)解決。

圖2 對(duì)極約束示意圖Fig.2 Schematic diagram of epipolar constraints

2.4 PnP（Perspective-n-Point）

已知M個(gè)特征點(diǎn)在參考幀I坐標(biāo)系中的位置，如圖3 所示，找到其中與I+1 幀的N個(gè)共視點(diǎn)在I+1幀上的像素坐標(biāo)，可以采用PnP 算法求取I+1 幀相對(duì)I幀的位姿。以此類推，可以得到I幀到當(dāng)前幀C之間所有幀的位姿估計(jì)，以及第1幀到第I-1幀的所有位姿。在此基礎(chǔ)上，三角化得到所有共視點(diǎn)的空間位置。

圖3 三角化示意圖Fig.3 Schematic diagram of triangulation

2.5 BA（bundle adjustment）光束平差法

在使用遞推迭代方法求得所有空間點(diǎn)位置和圖像幀相對(duì)于參考幀的位姿之后，固定住I幀的位置和姿態(tài)，固定住最后一幀C的位置。利用BA（bundle adjustment）光束平差法算法，將所有的相機(jī)位姿和空間點(diǎn)位置作為優(yōu)化變量，進(jìn)行聯(lián)合優(yōu)化。

如下頁(yè)圖4所示，令點(diǎn)Pj為位姿Ti的相機(jī)拍攝到的圖像歸一化坐標(biāo)系上的坐標(biāo)為，其重投影后的圖像歸一化坐標(biāo)系下的坐標(biāo)為，其中，是為了在計(jì)算時(shí)能不受相機(jī)內(nèi)參影響，k和k'是將齊次坐標(biāo)轉(zhuǎn)換為非齊次坐標(biāo)的常數(shù)項(xiàng)，可以得到該重投影誤差為：eij=uij-vij。

圖4 重投影誤差示意圖Fig.4 Schematic diagram of re-projection errors

BA就是將所有的重投影誤差的和最小化：

其中，當(dāng)點(diǎn)Pj在相機(jī)Ti中有投影時(shí)σij=1，否則為σij=0。

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

在EuRoC MAV數(shù)據(jù)集上，使用筆記本電腦進(jìn)行測(cè)試，電腦型號(hào)為L(zhǎng)enovo Thinkpad E14，擁有Intel酷睿i7 10510U 1.8 GHz，8 G 內(nèi)存。其中，滑動(dòng)窗口大小選擇為10 幀，對(duì)于不同數(shù)據(jù)集進(jìn)行測(cè)試，關(guān)鍵幀的選擇如圖5 所示，圖中標(biāo)識(shí)的點(diǎn)對(duì)應(yīng)于圖像的特征點(diǎn)，不同顏色深度顯示了特征點(diǎn)被跟蹤的次數(shù)。由于使用IMU 對(duì)圖像關(guān)鍵幀進(jìn)行了選擇，保證了關(guān)鍵幀圖像之間具有充分的運(yùn)動(dòng)激勵(lì)，使后續(xù)算法可以更為魯棒。

圖5 MH_05數(shù)據(jù)集上的關(guān)鍵幀選取Fig.5 Key frame selection on the MH_05 dataset

在EuRoC 所有數(shù)據(jù)集上進(jìn)行初始化，完成的初始化首幀時(shí)間統(tǒng)計(jì)表格如表1 所示，表中使用Unix系統(tǒng)時(shí)，單位為s，從表中可以看到，本文算法完成初始化時(shí)間與ORBSLAM3 相近，在MH01、MH02、MH04、V101 和V202 這5 個(gè)數(shù)據(jù)集上完成初始化得到位姿的圖像幀比數(shù)據(jù)集給出的真值時(shí)間還要早。

表1 位姿估計(jì)完成初始化首幀時(shí)間Table 1 The first frame time when the pose estimation is initialized

可以與真值進(jìn)行對(duì)比的數(shù)據(jù)集初始化結(jié)果誤差圖如圖6 所示，圖示軌跡是在對(duì)齊定位結(jié)果的情況下繪制，可見(jiàn)初始化結(jié)果與真值基本一致。誤差統(tǒng)計(jì)結(jié)果如表2所示，其中，同時(shí)考慮了旋轉(zhuǎn)和位移得到的相對(duì)位姿誤差估計(jì)結(jié)果，單位無(wú)量綱，從表中可以看到，本文設(shè)計(jì)的方法行之有效，能夠?yàn)楹蠖说姆蔷€性優(yōu)化，獲取較為準(zhǔn)確的初始值。

表2 位姿估計(jì)初始化誤差統(tǒng)計(jì)Table 2 Pose estimation initialization error statistics

圖6 位姿初始化軌跡誤差圖Fig.6 Pose initialization trajectory error diagram

作為對(duì)比，選取V102數(shù)據(jù)集，跟ORB和SMSCKF算法進(jìn)行尺度對(duì)齊和修正之后的誤差統(tǒng)計(jì)結(jié)果如表3所示，需要注意的是，對(duì)應(yīng)結(jié)果是分別截取了不同算法下對(duì)應(yīng)時(shí)間的結(jié)果。

表3 不同算法初始化誤差對(duì)比Table 3 Comparison of initialization errors of different algorithms

繪制V102 數(shù)據(jù)集下不同算法的軌跡圖，如圖7和圖8 所示，可見(jiàn)本文算法結(jié)果相比其他算法精度要更高。從圖8 中可見(jiàn)，本文算法在敏感到有充分運(yùn)動(dòng)激勵(lì)時(shí)才進(jìn)行初始化，這樣能夠保證關(guān)鍵幀之間有足夠的視差，因此，得到相對(duì)更為準(zhǔn)確的初始化結(jié)果。

圖7 V102數(shù)據(jù)集初始化定位軌跡圖Fig.7 Initialization positioning trajectory diagram of V102 data set

圖8 V102數(shù)據(jù)集三軸軌跡圖Fig.8 Three-axis trajectory diagram of V102 dataset

圖9 是在數(shù)據(jù)集基于上述初始化過(guò)程，使用純視覺(jué)定位的結(jié)果。其中，視覺(jué)定位結(jié)果未使用回環(huán)檢測(cè)，由于單目視覺(jué)軌跡的估計(jì)，無(wú)法獲得尺度真值，因此，對(duì)圖中的軌跡與真值進(jìn)行了對(duì)齊，可以看出是在本文提出視覺(jué)初始化的基礎(chǔ)上實(shí)現(xiàn)的視覺(jué)定位，能夠有效跟蹤真實(shí)軌跡。

圖9 V102數(shù)據(jù)集視覺(jué)定位結(jié)果Fig.9 Visual positioning results of 9V102 data set

4 結(jié)論

本文提出了一種無(wú)人機(jī)載視覺(jué)定位系統(tǒng)魯棒的初始化方法，該方法基于多視圖幾何原理，通過(guò)滑動(dòng)窗口限制實(shí)時(shí)運(yùn)算量，綜合運(yùn)用三角化、PnP 和BA 優(yōu)化，得到所有圖像幀相對(duì)參考幀的相對(duì)位姿，完成單目視覺(jué)的初始化，實(shí)驗(yàn)結(jié)果顯示，本文提出的方法快速準(zhǔn)確，為快速啟動(dòng)無(wú)人機(jī)視覺(jué)或者視覺(jué)慣性組合導(dǎo)航提供初始值。單目視覺(jué)定位算法存在尺度的問(wèn)題，后續(xù)實(shí)際應(yīng)用過(guò)程中，需要進(jìn)一步融合IMU 的數(shù)據(jù)，實(shí)現(xiàn)更為完備的初始化結(jié)果和定位效果。