基于單目視覺的相機(jī)位姿解算

王 鵬，趙汗青，王江峰，劉紅彬，陳 偉

(1.裝甲兵工程學(xué)院 控制科學(xué)與工程，北京 100072；2.裝甲兵工程學(xué)院 科研部，北京 100072)

基于單目視覺的相機(jī)位姿解算

王 鵬1，趙汗青2，王江峰2，劉紅彬2，陳 偉2

(1.裝甲兵工程學(xué)院 控制科學(xué)與工程，北京 100072；2.裝甲兵工程學(xué)院 科研部，北京 100072)

機(jī)器人在工作環(huán)境中利用傳感器感知信息來實(shí)現(xiàn)可靠的定位，這是自主移動(dòng)機(jī)器人的基本功能之一。GPS等衛(wèi)星定位系統(tǒng)在室內(nèi)信號(hào)較弱且有偏差，而視覺定位技術(shù)可以通過圖像特征匹配進(jìn)行位姿估計(jì)，因而被廣泛應(yīng)用。文中選取結(jié)構(gòu)簡單、成本低且易實(shí)現(xiàn)單目視覺定位的技術(shù)作為研究對(duì)象，以屋頂上打的結(jié)構(gòu)光點(diǎn)作為觀測(cè)數(shù)據(jù)來源，設(shè)計(jì)利用匹配特征點(diǎn)之間的關(guān)系，解算單目相機(jī)位姿的系統(tǒng)。通過對(duì)機(jī)器人所獲取的數(shù)據(jù)進(jìn)行實(shí)驗(yàn)分析表明，文中所設(shè)計(jì)的系統(tǒng)對(duì)于單目視覺機(jī)器人定位是有效的。

單目視覺；頂視；位姿解算；自主移動(dòng)機(jī)器人

隨著計(jì)算機(jī)技術(shù)與人工智能技術(shù)的發(fā)展，在工廠、生活當(dāng)中出現(xiàn)了許多種類的自主移動(dòng)機(jī)器人[1-3]。自主移動(dòng)機(jī)器人視覺定位是近些來發(fā)展起來的一種定位方法，該方法在實(shí)時(shí)數(shù)字?jǐn)z影測(cè)量、機(jī)器人運(yùn)動(dòng)導(dǎo)航、飛行器主動(dòng)視覺定位等領(lǐng)域具有重要的使用價(jià)值，也是目前較為流行的研究課題[4]。

因此, 研究者開始關(guān)注基于視覺原理的相機(jī)空間定位方法。文獻(xiàn)[5]對(duì)室內(nèi)定位技術(shù)進(jìn)行了綜述, 介紹目前主要的定位算法，以及各種算法衍生出來的各種定位技術(shù)[5]。文獻(xiàn)[6]利用場(chǎng)景中的三垂線和點(diǎn)特征構(gòu)建道路結(jié)構(gòu)特征, 并在此基礎(chǔ)上提出一個(gè)基于道路結(jié)構(gòu)特征的單目視覺定位算法[6]。文獻(xiàn)[7]提出使用單個(gè)天花板視覺傳感器的快速和魯棒的技術(shù)，所提出的算法適用于需要非常高的定位精度的系統(tǒng)[7]。為實(shí)現(xiàn)快速、魯棒性高的自主移動(dòng)機(jī)器人定位，本文使用單目相機(jī)，采取頂視方案，利用已經(jīng)匹配好的特征點(diǎn)信息，解算相機(jī)位姿，最后根據(jù)實(shí)驗(yàn)證明方法的有效性。

1 單目相機(jī)位姿解算

實(shí)驗(yàn)平臺(tái)如圖1所示，相機(jī)采用頂視模型，在屋頂打紅色激光以增加特征點(diǎn)的選取與匹配。在兩幅二維圖像中，利用對(duì)極約束得到同一場(chǎng)景兩幅圖像之間的幾何關(guān)系，它獨(dú)立于場(chǎng)景結(jié)構(gòu)，只與攝像機(jī)的內(nèi)、外部參數(shù)有關(guān)[8]。其一，借助射影幾何和齊次坐標(biāo)等數(shù)學(xué)工具，可以描述兩幅圖像間的對(duì)極幾何關(guān)系，計(jì)算出旋轉(zhuǎn)和平移矩陣；其二，利用三角化得到特征點(diǎn)的三維坐標(biāo)，之后的圖像就可利用PNP問題的求解方法解算相機(jī)位姿。

圖1 實(shí)驗(yàn)平臺(tái)

2 基于對(duì)極幾何求解單目相機(jī)位姿

2.1 對(duì)極約束原理

如圖2所示，這是對(duì)極幾何約束的示意圖。其中，O1和O2分別是是相機(jī)兩個(gè)時(shí)刻的中心，空間一點(diǎn)P在左邊圖片的投影為P1， 在右邊圖片的投影為P2。連接O1與P1，O2與P2，在三維空間會(huì)交于點(diǎn)P。O1，O2，P確定的平面為極平面，極平面與左右圖像的像平面所交得到相交線l1和l2稱為極線，連接O1與O2交左右圖像的像平面于點(diǎn)e1,e2。

圖2 對(duì)極幾何約束

以左邊圖像為世界坐標(biāo)系，設(shè)P的空間坐標(biāo)為

P=[X,Y,Z]T

根據(jù)針孔相機(jī)模型，可得

s1p1=KP,s2p2=K(BP+t)

(1)

其中，K為相機(jī)內(nèi)參，R，t為相機(jī)的旋轉(zhuǎn)，平移矩陣。在齊次坐標(biāo)下，上式可寫為

p1=KP,p2=K(RP+t)

(2)

像素點(diǎn)坐標(biāo)歸一化得

x1=K-1p1,x2=K-1p2

(3)

代入式(2)得

x2=Rx1+t

(4)

(5)

將式(2)代入，可得

(6)

兩式是對(duì)極約束數(shù)學(xué)表達(dá)，做進(jìn)一步簡化

E=t＾R
F=K-1EK-1

可得

(7)

其中，E為本質(zhì)矩陣；F為基礎(chǔ)矩陣[9]。所以，根據(jù)配對(duì)點(diǎn)可以求出E或者F，進(jìn)而求出R，t。

可以看出本文實(shí)驗(yàn)，特征點(diǎn)有可能都會(huì)落在同一個(gè)平面，這需要利用單應(yīng)矩陣進(jìn)行運(yùn)動(dòng)估計(jì)[10]

(8)

其中，n為平面的法向量；d為攝像機(jī)到平面距離

(9)

上式等號(hào)在非零因子下成立，通常乘以一個(gè)非零因子使得h9=1。整理有

(10)

(11)

可以看出，一組匹配點(diǎn)可以構(gòu)造兩個(gè)約束，而單應(yīng)矩陣的自由度為8，所以最少通過4對(duì)匹配特征點(diǎn)求解。

2.2 求解單目相機(jī)位姿

從式(7)可以得出，平移與旋轉(zhuǎn)各有3個(gè)自由度，由于尺度等價(jià)性，所以本質(zhì)矩陣只有5個(gè)自由度，最少可以用5對(duì)點(diǎn)求解。如果只利用本質(zhì)矩陣的線性質(zhì)，最少需要利用8對(duì)點(diǎn)可以求解。在本文實(shí)驗(yàn)中，特征點(diǎn)有可能都會(huì)落在同一個(gè)平面，這樣需要利用單應(yīng)矩陣進(jìn)行運(yùn)動(dòng)估計(jì)。以本質(zhì)矩陣為例恢復(fù)相機(jī)位姿，基本矩陣與單應(yīng)矩陣恢復(fù)相機(jī)位姿與本質(zhì)矩陣類似，都會(huì)有4個(gè)解。

設(shè)E的SVD分解是

E=U∑VT

(12)

其中，U和V為正交矩陣，Σ為奇異值矩陣。根據(jù)E的性質(zhì)，可以得到Σ=diag(σ，σ，0)。在SVD分解中，對(duì)于任何E都有兩個(gè)t，R與之對(duì)應(yīng)。因此，如圖3所示，一共有4組解。只有第1個(gè)解，點(diǎn)P在兩個(gè)相機(jī)下具有正深度，符合要求[11]。

圖3 本質(zhì)矩陣4個(gè)解的幾何示意

分解單應(yīng)矩陣時(shí)有4組解，可以得到法向量n根據(jù)特征點(diǎn)深度為正，可以排除兩個(gè)。因?yàn)閳?chǎng)景平面與相機(jī)平行，理論上法向量n=1T。代入即可得到正確解。本文實(shí)驗(yàn)會(huì)出現(xiàn)特征點(diǎn)共面情況，因此需要同時(shí)利用本質(zhì)矩陣與單應(yīng)矩陣求解相機(jī)位姿，選擇重投影誤差較小的作為最終選擇。

3 PNP問題

PNP問題是利用透視投影得到的n個(gè)象點(diǎn)，求解攝像時(shí)刻物體相對(duì)于相機(jī)的位姿。當(dāng)分別求得兩個(gè)不同攝像時(shí)刻下，物體相對(duì)于相機(jī)的位姿, 則可以求得這兩個(gè)時(shí)刻間物體相對(duì)于相機(jī)的運(yùn)動(dòng)。在兩張圖像中，其中一張?zhí)卣鼽c(diǎn)的三維位置已知，最少只需要3對(duì)就可以估計(jì)相機(jī)運(yùn)動(dòng)。本文實(shí)驗(yàn)利用三角化進(jìn)行深度估計(jì)，得到一張?zhí)卣鼽c(diǎn)的三維位置，進(jìn)而轉(zhuǎn)換為PNP問題進(jìn)行求解。文獻(xiàn)[12]中，對(duì)于透視投影來說，要使得 PNP 問題有確定的有限數(shù)目解，最少需要3組控制點(diǎn)[12]。P3P問題最多有4個(gè)解且解的上限可以達(dá)到[13]。對(duì)于P4P問題,當(dāng)4個(gè)控制點(diǎn)共面時(shí)有惟一解[14]，當(dāng)4個(gè)控制點(diǎn)不共面時(shí)，最多可能有4個(gè)解且解的上限可以達(dá)到[15]，P5P問題最多可能有兩個(gè)解,且解的上限可以達(dá)到[16]，當(dāng)N≥6時(shí)，可以用直接線性變換法。

3.1 直接線性變換法

如圖2，點(diǎn)P的齊次坐標(biāo)為[X,Y,Z,1]T。在左圖像對(duì)應(yīng)的特征點(diǎn)x1=(u1,v1,1)T，定義矩陣T=[R|t]，可以得出

(13)

可以得到

(14)

每一組特征點(diǎn)可以得到兩個(gè)約束，矩陣T有12個(gè)變量，因此最少通過6對(duì)匹配點(diǎn)求解T。當(dāng)匹配點(diǎn)>6個(gè)，利用最小二乘求解。以上t和R是通過上述原理計(jì)算出的相機(jī)運(yùn)動(dòng)。

3.2 P3P

P3P僅使用3對(duì)匹配點(diǎn)即可求出相機(jī)位姿。利用匹配好的3對(duì)點(diǎn)，已知一幅圖像3個(gè)點(diǎn)的世界坐標(biāo)，3個(gè)點(diǎn)記為A，B，C。另一幅圖像3個(gè)點(diǎn)的圖像坐標(biāo)，記為a，b，c。匹配點(diǎn)的幾何關(guān)系如圖4所示。

圖4 特征點(diǎn)幾何關(guān)系

記x=OA/OC,y=OB/OC

x2+y2-2xy·cos〈a,b〉=AB2/OC2

(15)

x2+1-2y·cos〈b,c〉=BC2/OC2

(16)

x2+1-2x·cos〈a,c〉=AC2/OC2

(17)

因?yàn)辄c(diǎn)a，b，c圖像坐標(biāo)已知，所以cos〈a,b〉,cos〈b,c〉,cos〈a,c〉已知。AB、BC、AC可以由點(diǎn)A，B，C得到。所以，式(15)為二元二次方程。該方程最多可以有4個(gè)解，利用一對(duì)驗(yàn)證點(diǎn)D，d，找出合適的解。最終得到點(diǎn)A，B，C在另一幅圖像坐標(biāo)系下的三維坐標(biāo)，然后根據(jù)3D點(diǎn)對(duì)匹配計(jì)算相機(jī)的位姿R，t。

4 實(shí)驗(yàn)

為了驗(yàn)證算法獲得相機(jī)位姿的有效性，本文利用如圖1所示的實(shí)驗(yàn)平臺(tái)，在室內(nèi)獲取了圖像。經(jīng)過相機(jī)標(biāo)定獲得相機(jī)參數(shù)，如圖5對(duì)圖像進(jìn)行特征點(diǎn)的提取與匹配，獲得匹配點(diǎn)對(duì)的信息。相機(jī)的內(nèi)參矩陣K為

圖5 特征點(diǎn)匹配

4.1 對(duì)極幾何求解相機(jī)位姿

利用已經(jīng)標(biāo)定的內(nèi)參矩陣，匹配好的特征點(diǎn)的信息。根據(jù)上述原理，可以得到相機(jī)的基本矩陣，本質(zhì)矩陣以及單應(yīng)矩陣，求出平移和旋轉(zhuǎn)矩陣

其中，E與F相差相機(jī)內(nèi)參矩陣。從E，F(xiàn)和H都可以分解運(yùn)動(dòng)，因?yàn)镠需要假設(shè)特征點(diǎn)位于平面，本實(shí)驗(yàn)無法保證特征點(diǎn)位于同一平面，所以利用E來分解運(yùn)動(dòng)

在單目SLAM中，將軌跡和地圖同時(shí)縮放任意倍數(shù)，得到的地圖是一致的。E具有尺度等價(jià)性，分解得到的t，R也有一個(gè)尺度等價(jià)性。因此，通常將t進(jìn)行歸一化，尺度為1。以此次的t為單位1，計(jì)算相機(jī)運(yùn)動(dòng)和特征點(diǎn)的三維信息。

4.2 P3P求解相機(jī)位姿

利用三角化，可以得到特征點(diǎn)的空間三維信息，通過上述原理計(jì)算t，R

可以看出兩次計(jì)算中R相差無幾，t不同是因?yàn)橐肓松疃刃畔⑺鶎?dǎo)致的。P3P實(shí)驗(yàn)過程中存在一些問題，P3P只利用3個(gè)點(diǎn)的信息，當(dāng)多余3組點(diǎn)時(shí)，信息利用率低。特征點(diǎn)受噪聲影響或匹配錯(cuò)誤，則無法計(jì)算。在視覺SLAM里，通常使用P3P等方法估計(jì)相機(jī)位姿，而后構(gòu)建最小二乘優(yōu)化問題進(jìn)行優(yōu)化。

5 結(jié)束語

本文利用在屋頂打紅色激光點(diǎn)，降低了特征點(diǎn)的選取與匹配。前期工作對(duì)圖像進(jìn)行預(yù)處理，提取ORB特征點(diǎn)作為實(shí)驗(yàn)數(shù)據(jù)。本文根據(jù)實(shí)驗(yàn)平臺(tái)的情況，設(shè)計(jì)解算相機(jī)位姿的系統(tǒng)，利用最少的匹配點(diǎn)對(duì)，求解相機(jī)位姿。實(shí)驗(yàn)表明，對(duì)于此次實(shí)驗(yàn)?zāi)Ｐ蛠碚f，以上兩種方式都可以計(jì)算出相機(jī)位姿。也驗(yàn)證了在此場(chǎng)景下，匹配特征點(diǎn)較少時(shí)，系統(tǒng)仍可以解算相機(jī)位姿。系統(tǒng)同時(shí)根據(jù)本質(zhì)矩陣與單應(yīng)矩陣求解相機(jī)位姿，選擇重投影誤差較小的作為最終選擇，解決了特征點(diǎn)在同一平面本質(zhì)矩陣無法解算相機(jī)位姿的問題。本文存在的不足之處是未考慮噪聲、匹配錯(cuò)誤的影響，下一步需要對(duì)計(jì)算結(jié)果進(jìn)行優(yōu)化處理，使結(jié)果更加接近真值，提高系統(tǒng)的魯棒性。另外相機(jī)的內(nèi)參作為已知給出，標(biāo)定時(shí)會(huì)出現(xiàn)誤差，下一步將相機(jī)內(nèi)參加入優(yōu)化，直接計(jì)算相機(jī)的位姿。

[1] 武嘉琪.計(jì)算機(jī)人工智能技術(shù)的發(fā)展與應(yīng)用研究[J].信息與電腦,2016(7):103-104.

[2] 王彥嫻.人工智能在計(jì)算機(jī)網(wǎng)絡(luò)技術(shù)中的應(yīng)用研究[J].信息技術(shù)與信息化,2015(8):84-85.

[3] 田良.人工智能技術(shù)在計(jì)算機(jī)中的發(fā)展和應(yīng)用[J].信息通信,2015(1):156-158.

[4] 王鑫.基于單目視覺的目標(biāo)物定位算法研究[D].哈爾濱:哈爾濱工業(yè)大學(xué),2016.

[5] 趙銳,鐘榜.室內(nèi)定位技術(shù)及應(yīng)用綜述[J].電子科技,2014,27(3):154-157.

[6] Jeong W,Lee K M.CV-SLAM:A new ceiling vision-based SLAM technique[C].UT,USA:IEEE/RSJ International Conference on Intelligent Robots and Systems,2005.

[7] Seo-Yeo Hwang,Park J T,Song J B. Autonomous navigation of a mobile robot using an upward-looking camera and sonar sensors[C].Korea:IEEE Workshop on Advanced Robotics and its Social Impacts,2010.

[8] 吳昊.基礎(chǔ)矩陣估計(jì)方法研究[D].蘭州:蘭州大學(xué),2015.

[9] 吳福朝.計(jì)算機(jī)視覺中的數(shù)學(xué)方法[M].北京:科學(xué)出版社,2008.

[10] 丁夏清,杜卓洋,陸逸卿,等.基于混合勢(shì)場(chǎng)的移動(dòng)機(jī)器人視覺軌跡規(guī)劃[J].浙江大學(xué)學(xué)報(bào),2016,50(7):1298-1306.

[11] 王文斌,劉桂華,劉先勇,等.本質(zhì)矩陣五點(diǎn)算法偽解的兩種剔除策略[J].光電工程,2010(8):46-52.

[12] 關(guān)凱.基于標(biāo)識(shí)的室內(nèi)視覺定位算法研究[D].哈爾濱:哈爾濱工業(yè)大學(xué),2016.

[13] 楊述強(qiáng).基于單目視覺的場(chǎng)景三維重建與飛行器位姿求解關(guān)鍵技術(shù)研究[D]. 長沙:國防科學(xué)技術(shù)大學(xué),2014.

[14] Hu ZY,Lei C,Wu F C.A short note on P4P problem[J].Chinese Journal of Automation,2001,27(6):770-776.

[15] Hu Z Y,Wu F C.A note on the number of solutions of the non-coplanar P4P problem[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2002,24(4):550-555.

[16] Wu F C,Hu Z Y.A study on the P5P problem[J].Journal of Software,2001, 12(5):768-775.

Based on Monocular Vision Camera Pose Computation

WANG Peng1,ZHAO Hanqing2,WANG Jiangfeng2,LIU Hongbin2,CHEN Wei2

(1.Department of Control Science and Engineering,The Academy of Armored Forces Engineering,Beijing 100072,China;2. Research Department,The Academy of Armored Forces Engineering,Beijing 100072,China)

The robot working in unknown environment, using sensors information, realize the reliable positioning is autonomous mobile robot is one of the most basic, the most important function of. Indoors, such as GPS satellite positioning system (GPS) signal is weak and deviation; Visual positioning technology and pose estimation through image feature matching is widely used. Selection system has simple structure, low cost and easy realization of monocular vision. Select on the roof structure light as observation data sources .Built using matching feature points with geometric relationship and principle of perspective projection, calculating system of monocular camera pose. Through the experiment, the results show that this method for top monocular vision robot localization is an effective method.

monocular vision;upward-looking camera;position calculation;autonomous mobile robot

2017- 04- 14

空軍“十二五”條件建設(shè)項(xiàng)目(2017216WX08)

王鵬(1992-)，男，碩士研究生。研究方向：單目視覺定位。趙汗青(1961-)，男，博士，高級(jí)工程師。研究方向：自主車環(huán)境感知，任務(wù)規(guī)劃。

10.16180/j.cnki.issn1007-7820.2017.12.020

TN911.73；TP391.41

A

1007-7820(2017)12-075-04