基于魚眼鏡頭的單目視覺定位系統(tǒng)

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

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

關(guān)鍵詞：視覺定位? 魚眼相機(jī)? SLAM? EUCM

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

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

1? 魚眼相機(jī)模型

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

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

，其中

（1）

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

（2）（2）

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

（3）

其中，。

然后，將得到橢球面的坐標(biāo)進(jìn)行歸一化就可以得到歸一化平面的坐標(biāo)。

2? 系統(tǒng)框架

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

在該框架中，首先進(jìn)行初始化。在對(duì)圖像處理之后，得到了去畸變的擴(kuò)展圖像，之后需要進(jìn)行特征點(diǎn)的提取，ORBSLAM2中使用的是ORB特征點(diǎn)，該特征點(diǎn)由FAST角點(diǎn)與BRIEF描述子組成[11]。當(dāng)在兩幅圖像中提取到的特征點(diǎn)的描述子歐式距離小于一定的閾值時(shí)就證明兩者是同一個(gè)路標(biāo)點(diǎn)。在得到一系列的匹配點(diǎn)之后需要進(jìn)行位姿的恢復(fù)，之后通過得到的位姿進(jìn)行三角化以此來獲取圖像中的地圖點(diǎn)，以便后續(xù)進(jìn)行位姿跟蹤的過程。其中位姿的恢復(fù)需要同時(shí)計(jì)算本質(zhì)矩陣E與單應(yīng)矩陣H，本質(zhì)矩陣適用于特征點(diǎn)都不在同一個(gè)平面中的情況，單應(yīng)矩陣適用于平面的情況，最后從中選取得到最優(yōu)模型，從矩陣中分解得到旋轉(zhuǎn)與平移。當(dāng)?shù)玫轿蛔酥螅梢詫?duì)匹配點(diǎn)進(jìn)行三角化得到三維坐標(biāo)點(diǎn)。

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

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

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

（2）本地映射處于空閑狀態(tài)，或者從插入最后一個(gè)關(guān)鍵幀到當(dāng)前間隔20幀以上。

（3）當(dāng)前幀至少跟蹤50個(gè)特征點(diǎn)。

（4）當(dāng)前幀跟蹤的特征點(diǎn)比參考關(guān)鍵幀少90%。

在得到關(guān)鍵幀之后，可以進(jìn)入局部建圖線程，該線程管理著局部地圖，并進(jìn)行局部地圖中的整體優(yōu)化。而且它還會(huì)創(chuàng)建新的地圖點(diǎn)，并且進(jìn)行關(guān)鍵幀的管理。在建圖線程中管理著共視圖，共視圖指的是以2個(gè)關(guān)鍵幀共同觀測到的地圖點(diǎn)的個(gè)數(shù)為權(quán)重，當(dāng)它達(dá)到一定閾值時(shí)，就在2個(gè)關(guān)鍵幀之間形成了一條邊，該邊以關(guān)鍵幀為頂點(diǎn)。共視圖的存在可以幫助局部地圖線程剔除關(guān)鍵幀，如果2個(gè)關(guān)鍵幀之間重復(fù)的地圖點(diǎn)過多則會(huì)將其中一個(gè)關(guān)鍵幀進(jìn)行剔除以此來提高空間的利用率。在局部地圖線程中，除了會(huì)創(chuàng)建新的地圖點(diǎn)還會(huì)剔除地圖點(diǎn)，這樣可以避免誤匹配和錯(cuò)誤的三角化，以此來保證地圖點(diǎn)的質(zhì)量。當(dāng)前局部地圖線程如果空閑的話則會(huì)對(duì)關(guān)鍵幀與地圖點(diǎn)進(jìn)行整體的優(yōu)化，優(yōu)化的內(nèi)容包括新加入的關(guān)鍵幀，具有共視關(guān)系的關(guān)鍵幀，以及所觀測到的地圖點(diǎn)?？偠灾植康貓D線程在提高定位精度的同時(shí)也減少了定位過程中信息的冗余。以上就是框架中跟蹤線程與局部建圖線程的主要過程。

3? 圖像的處理

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

（4）

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

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

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

（5）

（6）

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

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

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

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

圖3給出了系統(tǒng)運(yùn)行軌跡的前100s內(nèi)的APE平移部分（絕對(duì)軌跡誤差），單位為米，結(jié)果顯示絕對(duì)軌跡誤差控制在了15cm以內(nèi)，整體的誤差保持在較低的范圍，經(jīng)過計(jì)算，在該數(shù)據(jù)集上的RMSE（均方根誤差）達(dá)到了0.086m，定位精度達(dá)到了厘米級(jí)。使用多個(gè)參數(shù)進(jìn)行評(píng)估定位系統(tǒng)的定位精度，對(duì)應(yīng)的結(jié)果如圖所示，誤差表現(xiàn)良好。

5? 結(jié)語

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

參考文獻(xiàn)

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

[2] 姚偉，徐駿善，汪惠芬.基于視覺導(dǎo)航AGV的視覺定位系統(tǒng)設(shè)計(jì)[J].制造業(yè)自動(dòng)化，2020，42（11）：18-22.

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

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

[5] 姚奕杰.魚眼相機(jī)的標(biāo)定與視頻增強(qiáng)場景方法的研究[D].南京：南京師范大學(xué)，2020.

[6] 陳鵬.基于室內(nèi)視覺定位的輪式機(jī)器人主動(dòng)建圖與導(dǎo)航避障研究[D].成都：電子科技大學(xué)，2020.

[7] 吳婕.基于視覺的無人機(jī)導(dǎo)航定位系統(tǒng)關(guān)鍵技術(shù)研究[D].成都：電子科技大學(xué)，2020.

[8] 李傳立，尚俊娜，李芳.單目視覺人工路標(biāo)輔助INS的組合導(dǎo)航定位方法[J].傳感技術(shù)學(xué)報(bào)，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]朱佳文.基于視覺的機(jī)器人室內(nèi)定位與導(dǎo)航方法研究[D].大連：大連理工大學(xué)，2019.

[12]邵澤明.視覺移動(dòng)機(jī)器人自主導(dǎo)航關(guān)鍵技術(shù)研究[D].南京：南京航空航天大學(xué)，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.