基于魚眼鏡頭的單目視覺定位系統

雷鵬宇 周莉玲 李志天 鄒旭東

摘? 要：隨著無人駕駛，掃地機器人的廣泛應用，基于移動機器人技術的定位導航系統研究成為了一項科研熱點。移動機器人達到自主導航的目的，需要具備精確的定位功能。視場角在定位系統中起著重要的作用，越大的視場角代表著更多的環境信息獲取，而傳統的單目相機的視場角小，在紋理較少的環境中，無法進行有效的定位。本文利用魚眼鏡頭來替換傳統的單目針孔相機，可以增加定位系統的視場角，有效利用冗余觀測信息來保證位姿估計的魯棒性和觀測目標特征的長可見性。在本文中，設計了搭載魚眼鏡頭的定位系統可以捕獲更豐富的環境特征信息，并且引入了相機畸變模型改善圖像畸變的問題，設計了基于特征點對圖像信息進行處理的系統，實現了導航定位的功能，獲得了更魯棒的位姿跟蹤效果。實驗表明，基于魚眼鏡頭的單目視覺定位系統定位性能提高，實現厘米級定位。

關鍵詞：視覺定位? 魚眼相機? SLAM? EUCM

中圖分類號：TP249? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻標識碼：A? ? ? ? ? ? ? ? ? 文章編號：1674-098X（2021）05（c）-0092-06

Monocular vision positioning system based on fisheye lens

LEI Pengyu1，2? ZHOU Liling1，2? LI Zhitian1? ZOU Xudong1*

（1.Aerospace Information Research Institute， Chinese Academy of Sciences， Beijing， 100190? China;

2.University of Chinese Academy of Sciences， Beijing， 100049? China）

Abstract： With the wide application of unmanned and sweeping robots， the research of positioning and navigation system based on mobile robot technology has become a scientific research hotspot. To achieve the purpose of autonomous navigation， mobile robot needs to have accurate positioning function. The field of view angle plays an important role in the positioning system. The larger the field of view angle represents more environmental information acquisition， while the traditional monocular camera has a small field of view angle and can not locate effectively in the environment with less texture. In this paper， fish eye lens is used to replace the traditional monocular pinhole camera， which can increase the field angle of view of the positioning system， and effectively use redundant observation information to ensure the robustness of pose estimation and the long visibility of observation target features. In this paper， a positioning system equipped with fish eye lens is designed， which can capture more abundant environmental feature information， and the camera distortion model is introduced to improve the problem of image distortion. A system for processing image information based on feature points is designed， which realizes the function of navigation and positioning and obtains a more robust pose tracking effect. Experiments show that the positioning performance of monocular vision positioning system based on fish eye lens is improved and centimeter level positioning is realized.

Key Words： Visual positioning; Fish eye camera; SLAM; EUCM

這年來，同步定位與建圖（Simultaneous Localization and Mapping）技術發展迅速，簡稱SLAM，它是指移動機器人（如無人駕駛，掃地機器人等）在處于未知環境和未知位置情況下，利用自身傳感器對周圍環境信息進行獲取，構建全局一致的環境地圖同時進行定位與導航的技術。由于相機成本低，體積小，獲取環境信息豐富等突出優勢，在SLAM技術中被廣泛研究，移動機器人在獲取環境信息的時候，相機以其成本低與采集信息豐富等特點成為了定位技術研究中廣泛使用的傳感器，對于定位導航有著非常重要的意義。目前相機主要分為單目，雙目與深度RGB-D，雙目的配置與標定都比較復雜，深度RGB-D只適用于室內定位，單目相機憑借著結構簡單，成本低的特點非常受研究者的關注，而普通的單目由于視場角小的緣故會導致采集到的信息并不完整，特別是在具有挑戰性的環境中，普通單目可能會定位失敗。普通單目針孔相機的視場角一般都在90°以內，得到的圖片畸變較小，真實度高，不需要繁瑣的標定步驟就可以直接使用。雖然針孔相機得到的圖像在算法處理工程中簡單且效率高，但是較小的視場角同樣使得攝像頭拍攝得到的內容較少，在紋理不豐富的場景下或運動模糊等原因會導致定位結果不準確甚至定位失敗，這也是所有普通單目相機存在的問題，視場角小導致采集得到的信息并不完整，而魚眼相機由于其能夠通過透鏡的不同組合而獲取包含周圍環境信息更多的圖像，對于提升定位的魯棒性和精度有著重要意義。但魚眼相機具備視場大的優勢同樣帶來了圖像畸變的問題，因此需要對圖片進行去畸變處理再進行定位與導航。

針對這些傳統定位導航方法中單目相機獲取信息的局限，本文通過加入魚眼鏡頭來加大定位系統的視場角，進而獲取更多的環境信息，并且針對魚眼相機引入的圖像畸變問題，引入了相機畸變模型EUCM（Extended Unified Camera Model），在一定程度上改善了圖像畸變導致的缺陷，實驗證明，基于魚眼鏡頭的單目視覺定位系統能夠得到更好的定位結果。

1? 魚眼相機模型

魚眼鏡頭屬于超廣角鏡頭中的一種，它一般是由十幾個不同的透鏡組合成的，在實際工程應用中的相機鏡頭，通常能夠達到140°的視角，而在一些視覺導航的應用中，相機的視角能夠達到360°，因此記錄的圖像信息更多，視覺導航系統也就可以在高動態的工況下正常工作[1-3]。在成像的過程中，透鏡的作用下導致入射光線存在不同程度的折射，投影到較小的成像平面之后，會使得魚眼鏡頭相比普通鏡頭有著更大的視野范圍，但是透鏡的加入不僅會使得光的傳播發生變化，而且在組裝過程中，透鏡和成像平面也不可能完全平行，這也會使得光線穿過透鏡投影到成像平面時的位置發生變化。前者會產生徑向畸變，后者會產生切向畸變，兩者的疊加導致圖像變形，最終使定位結果不準確[4-5]。由于魚眼鏡頭的畸變比普通相機的畸變更大，在進行圖像處理的時候，需要對圖像進行矯正，即將這種廣角下的圖片處理為平面圖片，所以，本文利用魚眼相機增加定位系統的視場角，同時引入了相機畸變模型EUCM（Extended Unified Camera Model）用來解決圖像的畸變問題。

EUCM模型適用于視場角大于180°的魚眼鏡頭，這種視角的鏡頭由于焦距短，視角大，導致光學原理產生的變形更加強烈，因此需要標定6個參數、、、、、，其中、、、為相機的內參，、為相機的畸變系數，如圖1。假設三維坐標中有一點p它的坐標為，在定位過程中需要將點p投影到相機平面上[6-8]。在EUCM模型中，投影分為兩部分，第一部分需要將三維坐標中的點投影到歸一化平面坐標，得到點的坐標如式（1）。

，其中

（1）

第二部分是將歸一化平面坐標利用式（2）投影到像素平面中。

（2）（2）

其中，u，v是像素平面的坐標。除此之外，在定位過程中，2D點到3D點的恢復也尤為重要，所以需要將像素坐標反投影到歸一化平面中，該過程也分為2個部分[9]。首先，需要將像素平面的坐標反投影到橢球面坐標中，假設橢球面坐標為xp，它的坐標值可以根據像素坐標u，v得到，如式（3）。

（3）

其中，。

然后，將得到橢球面的坐標進行歸一化就可以得到歸一化平面的坐標。

2? 系統框架

在視覺定位中，根據對圖像處理的方式不同可以分為特征點法和直接法[10]。特征點法是提取圖像中的特征之后根據一定的策略進行特征點匹配的過程，該方法得到的特征較穩定，并且其稀疏的特征特別適用于通過魚眼鏡頭所獲得的廣角、包含信息更多的圖像。特別是本文提出的導航系統具有3個線程，分別是跟蹤、建圖、閉環線程，均與特征點關聯密切。因此，本文在Raul Mur-Artal的ORB-SLAM2框架下，基于魚眼相機模型搭建了一套在寬視場角等具有挑戰性環境下使用的魯棒定位算法。該算法是基于稀疏特征點的定位方法，在此框架中，跟蹤與建圖是分別處理的，這樣可以大大地提高系統的實時性。

在該框架中，首先進行初始化。在對圖像處理之后，得到了去畸變的擴展圖像，之后需要進行特征點的提取，ORBSLAM2中使用的是ORB特征點，該特征點由FAST角點與BRIEF描述子組成[11]。當在兩幅圖像中提取到的特征點的描述子歐式距離小于一定的閾值時就證明兩者是同一個路標點。在得到一系列的匹配點之后需要進行位姿的恢復，之后通過得到的位姿進行三角化以此來獲取圖像中的地圖點，以便后續進行位姿跟蹤的過程。其中位姿的恢復需要同時計算本質矩陣E與單應矩陣H，本質矩陣適用于特征點都不在同一個平面中的情況，單應矩陣適用于平面的情況，最后從中選取得到最優模型，從矩陣中分解得到旋轉與平移。當得到位姿之后，可以對匹配點進行三角化得到三維坐標點。

跟蹤線程中，主要分為3種模式：恒速模型、關鍵幀匹配與重定位模式[12]。恒速模型使用前一幀的位姿，并通過對誤差模型的優化來獲取當前幀的位姿。關鍵幀匹配是在兩幀圖像匹配特征點不夠的情況下，將當前幀與關鍵幀進行特征點匹配，然后進行位姿估計。重定位模式是在前兩者都跟蹤失敗的情況下與地圖進行特征點匹配跟蹤，該過程中用到了DBoW3詞袋模型，該模型可以加快匹配的速度。

跟蹤線程中還包含了關鍵幀的選取，若要成為關鍵幀必須要滿足以下條件。

（1）從上次的重定位到當前幀，間隔20幀以上。

（2）本地映射處于空閑狀態，或者從插入最后一個關鍵幀到當前間隔20幀以上。

（3）當前幀至少跟蹤50個特征點。

（4）當前幀跟蹤的特征點比參考關鍵幀少90%。

在得到關鍵幀之后，可以進入局部建圖線程，該線程管理著局部地圖，并進行局部地圖中的整體優化。而且它還會創建新的地圖點，并且進行關鍵幀的管理。在建圖線程中管理著共視圖，共視圖指的是以2個關鍵幀共同觀測到的地圖點的個數為權重，當它達到一定閾值時，就在2個關鍵幀之間形成了一條邊，該邊以關鍵幀為頂點。共視圖的存在可以幫助局部地圖線程剔除關鍵幀，如果2個關鍵幀之間重復的地圖點過多則會將其中一個關鍵幀進行剔除以此來提高空間的利用率。在局部地圖線程中，除了會創建新的地圖點還會剔除地圖點，這樣可以避免誤匹配和錯誤的三角化，以此來保證地圖點的質量。當前局部地圖線程如果空閑的話則會對關鍵幀與地圖點進行整體的優化，優化的內容包括新加入的關鍵幀，具有共視關系的關鍵幀，以及所觀測到的地圖點。總而言之，局部地圖線程在提高定位精度的同時也減少了定位過程中信息的冗余。以上就是框架中跟蹤線程與局部建圖線程的主要過程。

3? 圖像的處理

魚眼相機所獲得的圖片包含的信息量更多，為了方便后續的圖像處理，本文選擇了ROI（Region of interest）選取。該方法可以將原始的魚眼圖像轉換成沒有畸變的圖像并且保證圖像區域沒有減少。處理過程分為兩部分：（1）將魚眼圖像中的坐標點根據它原始的內參以及EUCM模型反投影得到歸一化平面坐標系上的點;（2）將反投影得到的歸一化平面坐標系上的點進行投影，投影得到虛擬平面的像素坐標。該過程通過式（4）表示。

（4）

其中，uc、uv和分別為原始魚眼圖像與虛擬圖像的坐標。是從歸一化平面坐標投影到虛擬平面的過程，該過程適用的相機內參是虛擬內參，與相機光心到虛擬平面的距離與角度有關。是反投影方程，該過程使用的是EUCM模型的原始參數，目的是獲得去畸變之后的三維坐標。是虛擬圖像和原始圖像之間的旋轉矩陣[13-14]。

通過上述的投影與反投影過程，便可以通過處理魚眼鏡頭獲得的圖像得到5幅虛擬圖像，在后續的過程中不再需要對圖像進行去畸變操作。

在上述描述中，使用了ROI區域的提取，所以得到的虛擬平面的像素坐標并不是真實的像素坐標。在傳統的針孔相機模型中，重投影誤差定義在一個普遍意義的圖像平面，因此本文將相機測量殘差定義在單位球面上，該殘差適用于所有類型的相機，包括廣角、魚眼和全向相機。將該殘差建模為一個單位射線到單位球表面的聯系，殘差的定義如式（5）與式（6）。

（5）

（6）

其中，式（5）中的b1和b2是任意正交基。式6中的R和t是圖像j與圖像i之間的旋轉矩陣與位移，pj為圖像j中對應的歸一化平面坐標系中的坐標，pi圖像i對應的歸一化平面坐標系中的坐標，利用該公式便可計算得到相機的測量殘差。

4? 實驗結果

為了驗證本文所提出方案，評估使用魚眼鏡頭之后定位系統所能實現的精度，設計了如下實驗。該實驗在Ubuntu 16.04環境下運行該算法，使用的運行平臺是ROS Kinetic。測試使用的數據集為TUM數據集的Room4，通過定位誤差評估系統的運行效果。在計算的過程中，使用了Sim（3） Umeyama對準（Sim（3） Umeyama alignment）。并且使用絕對位姿誤差（Absolute Pose Error， APE）來評估該框架下的關鍵幀的軌跡以及其定位精度。由于APE比較的是平移部分的誤差，為了準確評估實驗的性能，本文同時采用了均方根誤差（Root Mean Square Error，RMSE），均值誤差（Mean Error），中值誤差（Median Error）和標準差（Standard Deviation，Std.）來評估實驗結果。

圖2是系統輸出的軌跡圖與真值的對比。其中虛線代表軌跡真值，實線代表系統輸出的軌跡，右側的彩條代表著誤差，其顏色的變化代表偏離真值的程度。從圖2中可以看出，該系統可以正常地在數據集上運行并且能得到高精度的定位結果。通過軌跡的顏色可以看出大部分軌跡的誤差保持在較低的水平，在拐角處的紋理信息特別少的地方，誤差較大，超過10cm。

圖3給出了系統運行軌跡的前100s內的APE平移部分（絕對軌跡誤差），單位為米，結果顯示絕對軌跡誤差控制在了15cm以內，整體的誤差保持在較低的范圍，經過計算，在該數據集上的RMSE（均方根誤差）達到了0.086m，定位精度達到了厘米級。使用多個參數進行評估定位系統的定位精度，對應的結果如圖所示，誤差表現良好。

5? 結語

本文針對普通單目相機獲取的環境信息較少，在一些紋理少的環境中，可能導致定位失敗的問題，提出了一種結合魚眼相機，并使用特征點法的導航定位系統。通過利用魚眼鏡頭視場角大的特點，獲取包含更多環境信息的圖片，并且使用相機畸變模型EUCM，實現魚眼鏡頭的高精度校準，保證了從魚眼鏡頭圖像到相應全景圖像的精確轉換。設計的基于特征的SLAM系統框架由初始化、特征匹配、幀跟蹤和閉環等幾個特定的策略和算法組成。該系統具有更大的視場角，能適用于更多的場景，提取更多的特征，從而使系統的運行更魯棒。最后，設計的實驗顯示，該系統可在標準CPU中實時運行，并且能夠實現高精度的導航定位，定位誤差小于10cm，均方根誤差達到了0.086m，實現了厘米級的定位。

參考文獻

[1] 趙棟.超廣角鏡頭拍攝影像的畸變糾正分析[J].城市勘測，2021（1）：135-140.

[2] 姚偉，徐駿善，汪惠芬.基于視覺導航AGV的視覺定位系統設計[J].制造業自動化，2020，42（11）：18-22.

[3] 楊宇，趙成星，張曉玲.魚眼相機的視覺標定及畸變校正[J].激光雜志，2020，41（9）：20-23.

[4] 肖智越，劉力雙，呂勇，等.基于相機移動的魚眼成像系統目標方位標定研究[J].激光雜志，2020，41（9）： 24-28.

[5] 姚奕杰.魚眼相機的標定與視頻增強場景方法的研究[D].南京：南京師范大學，2020.

[6] 陳鵬.基于室內視覺定位的輪式機器人主動建圖與導航避障研究[D].成都：電子科技大學，2020.

[7] 吳婕.基于視覺的無人機導航定位系統關鍵技術研究[D].成都：電子科技大學，2020.

[8] 李傳立，尚俊娜，李芳.單目視覺人工路標輔助INS的組合導航定位方法[J].傳感技術學報，2020，33（1）： 68-73.

[9] E. Rublee，V. Rabaud，K. Konolige，etal. Bradski. ORB： an efficient alternative to SIFT or SURF[J].IEEE International Conference on Computer Vision （ICCV），2011：2564–2571.

[10]D. G' alvez-L' opez， J. D. Tard' os.Bags of binary words for fast place recognition in image sequences[J].IEEE Transactions on Robotics， 2012，28（5）：1188–1197.

[11]朱佳文.基于視覺的機器人室內定位與導航方法研究[D].大連：大連理工大學，2019.

[12]邵澤明.視覺移動機器人自主導航關鍵技術研究[D].南京：南京航空航天大學，2009.

[13]R. Mur-Artal， J. M. M. Montiel， J. D. Tardós. ORB-SLAM： A Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics，2015，31（5）：1147-1163.

[14]J. Heikkila， O. Silven.A four-step camera calibration procedure with implicit image correction[J].Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition， San Juan，PR，USA，1997：1106-1112.