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

（海軍航空大學，山東 煙臺 264001）

0 引言

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

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

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

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

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

2.1 圖像預處理

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

式中，［u,v］T為空間點在成像平面上的像素坐標；PT=［X,Y,Z］T為空間點在相機坐標系中的三維坐標，矩陣K為相機的內參矩陣。

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

2.2 特征點選取與跟蹤

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

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

從滑動窗口內的第1幀開始計算每一幀和當前幀的匹配特征點，尋找與當前幀共視點數較多、且視差量較大的作為參考幀。在滑動窗口內，記錄所有能夠被關鍵幀觀測到的特征點，而對于每個特征點，同時記錄能夠觀測到該特征點的圖像幀序列，以及特征點在對應圖像幀中的相關信息。

2.3 對極約束和三角化

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

圖2 對極約束示意圖Fig.2 Schematic diagram of epipolar constraints

2.4 PnP（Perspective-n-Point）

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

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

2.5 BA（bundle adjustment）光束平差法

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

如下頁圖4所示，令點Pj為位姿Ti的相機拍攝到的圖像歸一化坐標系上的坐標為，其重投影后的圖像歸一化坐標系下的坐標為，其中，是為了在計算時能不受相機內參影響，k和k'是將齊次坐標轉換為非齊次坐標的常數項，可以得到該重投影誤差為：eij=uij-vij。

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

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

其中，當點Pj在相機Ti中有投影時σij=1，否則為σij=0。

3 實驗分析

在EuRoC MAV數據集上，使用筆記本電腦進行測試，電腦型號為Lenovo Thinkpad E14，擁有Intel酷睿i7 10510U 1.8 GHz，8 G 內存。其中，滑動窗口大小選擇為10 幀，對于不同數據集進行測試，關鍵幀的選擇如圖5 所示，圖中標識的點對應于圖像的特征點，不同顏色深度顯示了特征點被跟蹤的次數。由于使用IMU 對圖像關鍵幀進行了選擇，保證了關鍵幀圖像之間具有充分的運動激勵，使后續(xù)算法可以更為魯棒。

圖5 MH_05數據集上的關鍵幀選取Fig.5 Key frame selection on the MH_05 dataset

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

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

可以與真值進行對比的數據集初始化結果誤差圖如圖6 所示，圖示軌跡是在對齊定位結果的情況下繪制，可見初始化結果與真值基本一致。誤差統(tǒng)計結果如表2所示，其中，同時考慮了旋轉和位移得到的相對位姿誤差估計結果，單位無量綱，從表中可以看到，本文設計的方法行之有效，能夠為后端的非線性優(yōu)化，獲取較為準確的初始值。

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

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

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

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

繪制V102 數據集下不同算法的軌跡圖，如圖7和圖8 所示，可見本文算法結果相比其他算法精度要更高。從圖8 中可見，本文算法在敏感到有充分運動激勵時才進行初始化，這樣能夠保證關鍵幀之間有足夠的視差，因此，得到相對更為準確的初始化結果。

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

圖8 V102數據集三軸軌跡圖Fig.8 Three-axis trajectory diagram of V102 dataset

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

圖9 V102數據集視覺定位結果Fig.9 Visual positioning results of 9V102 data set

4 結論

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