,,,

(1.浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023; 2.南開大學(xué) 計(jì)算機(jī)與控制工程學(xué)院，天津 300071)

自主移動(dòng)機(jī)器人的一個(gè)基本功能是可以自我確定自身在環(huán)境中的位置[1].視覺傳感器信息量大，能完成很多智能化任務(wù)[2]，如位姿估計(jì)[3]、視覺伺服控制[4-5]、同步定位及地圖構(gòu)建(SLAM)[6]等，準(zhǔn)確的位姿估計(jì)是視覺伺服控制和SLAM的基礎(chǔ).在利用視覺傳感器測(cè)量移動(dòng)機(jī)器人位姿時(shí)，通常把視覺傳感器固定在移動(dòng)機(jī)器人上，移動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)，通過攝像機(jī)拍攝的二維圖像的像素信息來(lái)得到移動(dòng)機(jī)器人自身的位置和姿態(tài)，簡(jiǎn)稱位姿.由于單目攝像機(jī)成本低廉且無(wú)需解決立體匹配的難題，因此受到廣泛的研究和應(yīng)用[1].但單目攝像機(jī)面臨的最主要問題是圖像中缺乏深度信息,也就是尺度的模糊性.在這種情況下，通常利用攝像機(jī)模型，根據(jù)兩幅或多幅圖像確定移動(dòng)機(jī)器人的相對(duì)位姿，即實(shí)現(xiàn)從二維圖像中獲取移動(dòng)機(jī)器人的三維位姿信息.

近年來(lái)，通過運(yùn)動(dòng)拍攝圖片得到的二維信息來(lái)獲得運(yùn)動(dòng)信息和場(chǎng)景結(jié)構(gòu)信息的SFM(Structure from motion)[7]算法得到了較大的發(fā)展.關(guān)于SFM的研究開始于20 世紀(jì)80 年代，其基本思想是根據(jù)單目圖像序列的二維信息直接恢復(fù)出攝像機(jī)運(yùn)動(dòng)的位姿信息和三維場(chǎng)景結(jié)構(gòu)信息.其中主要有幾種不同的SFM方法，如因式分解法[8-10]、多視圖幾何方法[11]和基于模型的方法[12]等.基于因式分解的SFM算法最早由Tomashi等提出，并應(yīng)用于正射投影[8].其主要思想是通過秩約束(Rank constraints)來(lái)表示數(shù)據(jù)中存在的幾何不變量，將觀測(cè)矩陣(由圖像中一組特征跟蹤點(diǎn)的坐標(biāo)組成)通過因式分解得出攝像機(jī)的運(yùn)動(dòng)參數(shù)和特征點(diǎn)結(jié)構(gòu)信息.這種方法相比其他SFM方法，如基于模型的SFM方法和多視圖幾何方法，基于因式分解的SFM方法不依賴具體模型，對(duì)場(chǎng)景無(wú)特殊要求，不依賴某些特殊的特征和圖像，且同時(shí)考慮所有圖像中的數(shù)據(jù)，因而有較強(qiáng)的抗噪能力.而實(shí)際情況中，平面運(yùn)動(dòng)的情況很普遍，增加平面運(yùn)動(dòng)約束，可降低觀測(cè)矩陣的秩，簡(jiǎn)化了觀測(cè)矩陣的分解，目前很多因式分解方法結(jié)合地平面約束進(jìn)行計(jì)算[13-15]，但大多數(shù)方法采用線性不等式或反復(fù)迭代來(lái)分解觀測(cè)矩陣，很多情況下只能保證局部最優(yōu)而忽略了全局最優(yōu).文獻(xiàn)[16]用消失點(diǎn)、消失線和平面約束來(lái)構(gòu)建觀測(cè)矩陣，只需要進(jìn)行奇異值分解、最小二乘來(lái)求解運(yùn)動(dòng)矩陣和恢復(fù)三維結(jié)構(gòu)，其采用所有相同特征點(diǎn)進(jìn)行運(yùn)算，可以保證全局最優(yōu)且計(jì)算效率高.但該方法在室內(nèi)環(huán)境或者不存在平行線的室外環(huán)境，對(duì)消失線和消失點(diǎn)的提取困難，且很難獲得正確的約束矩陣，使得算法經(jīng)常失效.針對(duì)以上方法的不足，筆者提出一種奇異值分解與非線性約束相結(jié)合的因式分解方法，通過特征點(diǎn)、攝像機(jī)標(biāo)定內(nèi)參和地平面約束構(gòu)建觀測(cè)矩陣并分解，將分解得到的位姿通過光束平差法[17-18]進(jìn)行優(yōu)化，保證結(jié)果的準(zhǔn)確性.

1 觀測(cè)矩陣獲取

則由攝像機(jī)內(nèi)參數(shù)模型可得

(1)

圖1 攝像機(jī)模型Fig.1 The model of camera

(2)

其第i+1個(gè)攝像機(jī)坐標(biāo)系下特征點(diǎn)的坐標(biāo)與歸一化攝像機(jī)坐標(biāo)之間的關(guān)系為

(3)

當(dāng)移動(dòng)機(jī)器人在平面運(yùn)動(dòng)時(shí)，對(duì)于每個(gè)點(diǎn)，由地平面約束(GPC)可知攝像機(jī)坐標(biāo)的Z軸不變，即

以第i個(gè)坐標(biāo)為參考坐標(biāo)，可得

(4)

(5)

由式(2)和GPC約束可得

(6)

(7)

將式(6，7)代入式(5)得

(8)

由式(8)可以組成觀測(cè)矩陣W，即

(9)

2 觀測(cè)矩陣分解

2.1 觀測(cè)矩陣分解步驟

1) 將觀測(cè)矩陣進(jìn)行奇異值(SVD)分解(W=U·D·VΤ)，為避免后面出現(xiàn)最小二乘的病態(tài)問題，使運(yùn)動(dòng)矩陣M=U，形狀矩陣S=D·VΤ.

2) 為了使W服從秩為3的約束，選取U的前三列，D的前三行前三列(特征值最大的三個(gè)元素)，VΤ的前三行構(gòu)建新的觀測(cè)矩陣W為

(10)

(11)

式中Ri(i∈[1,m])為每幀相對(duì)于第一幀的旋轉(zhuǎn)矩陣，因其與自身轉(zhuǎn)置的乘積為單位陣，故必滿足

(12)

2.2 光束平差法

光束平差法的最終目的時(shí)為了減少測(cè)量圖像的特征點(diǎn)和反計(jì)算特征點(diǎn)之間的重投影誤差.最小化誤差算法使用的是最小二乘算法，為了優(yōu)化移動(dòng)機(jī)器人的計(jì)算位姿，建立函數(shù)

(13)

3 實(shí)驗(yàn)結(jié)果分析

對(duì)于筆者提出的改進(jìn)型因式分解方法，本節(jié)進(jìn)行實(shí)驗(yàn)結(jié)果分析和驗(yàn)證.如圖2所示，采用Pioneer3-DX移動(dòng)機(jī)器人進(jìn)行實(shí)驗(yàn)，其上安裝有SV400型攝像機(jī)，移動(dòng)機(jī)器人行走時(shí)拍攝圖片.實(shí)驗(yàn)中先固定一幀圖像為參考幀，隨后機(jī)器人從任意一點(diǎn)出發(fā)，向參考幀的方向移動(dòng)，每秒拍攝50 幀圖像，如圖1所示.在用OpenCV對(duì)攝像機(jī)進(jìn)行標(biāo)定后，通過基于SIFT的特征點(diǎn)提取和匹配，選取在每一幀中都出現(xiàn)的特征點(diǎn)，并提取其像素值，平面特征點(diǎn)像素坐標(biāo)隨移動(dòng)機(jī)器人運(yùn)動(dòng)變化，如圖3所示.星形對(duì)應(yīng)的坐標(biāo)是參考幀提取的像素值(圖1中移動(dòng)機(jī)器人在參考位置拍攝的圖像中特征點(diǎn)Oj對(duì)應(yīng)的像素坐標(biāo))，圓形對(duì)應(yīng)的是移動(dòng)機(jī)器人初始位姿拍攝的圖像特征點(diǎn)，軌跡對(duì)應(yīng)的坐標(biāo)是移動(dòng)機(jī)器人行走時(shí)所拍攝圖片的像素坐標(biāo)(圖1中移動(dòng)機(jī)器人在不同位姿拍攝圖像中Oj的像素坐標(biāo)).

圖2 實(shí)驗(yàn)裝置Fig.2 Experimental setup

圖3 平面特征點(diǎn)像素坐標(biāo)Fig.3 Pixel coordinates of planar feature points

如圖4所示，移動(dòng)機(jī)器人實(shí)際運(yùn)動(dòng)軌跡為圖4中曲線，三角形方向?yàn)闄C(jī)器人運(yùn)動(dòng)方向，圖4中散點(diǎn)為筆者算法計(jì)算求得的移動(dòng)機(jī)器人位姿，可知計(jì)算求得的移動(dòng)機(jī)器人位姿基本符合實(shí)際位姿.筆者算法在平面場(chǎng)景和立體場(chǎng)景下的實(shí)驗(yàn)結(jié)效果基本一致，如圖5～7所示.在兩種情況下分別對(duì)實(shí)驗(yàn)誤差進(jìn)行計(jì)算：1) 由圖5，6可知,平面場(chǎng)景下，當(dāng)少量圖像幀數(shù)參與計(jì)算時(shí)，移動(dòng)機(jī)器人位姿估計(jì)準(zhǔn)確，而當(dāng)大量圖像參與計(jì)算時(shí)，旋轉(zhuǎn)矩陣基本正確，而平移矩陣由于噪聲影響誤差較大，因而要及時(shí)切換參考幀，每次用低于50 幀的圖像參與計(jì)算.故在圖2的立體場(chǎng)景下，選用21 幀圖像;2) 由圖7可知,實(shí)驗(yàn)誤差較小，位姿結(jié)果計(jì)算準(zhǔn)確.

圖4 移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡Fig.4 The mobile robot motion trajectories

圖5 10 幀圖像位姿估計(jì)誤差Fig.5 The error of pose estimation with 10 frames

圖6 100 幀圖像位姿估計(jì)誤差Fig.6 The error of pose estimation with 100 frames

圖7 立體場(chǎng)景下圖像位姿估計(jì)誤差Fig.7 The error of pose estimation with stereo scene

4 結(jié) 論

針對(duì)大部分位姿估計(jì)方法只適用于平面場(chǎng)景的問題，提出了一種同時(shí)適用于平面場(chǎng)景和立體場(chǎng)景的位姿估計(jì)方法.該方法結(jié)合奇異值分解和光束平差法，最少用三幅圖像就能計(jì)算移動(dòng)機(jī)器人當(dāng)前的位姿.且采用地平面約束，降低了觀測(cè)矩陣的秩，從而簡(jiǎn)化了分解步驟且采用SVD進(jìn)行觀測(cè)矩陣分解，避免了使用迭代優(yōu)化來(lái)分解觀測(cè)矩陣，提高了計(jì)算效率.實(shí)驗(yàn)結(jié)果表明：當(dāng)參與計(jì)算圖像幀數(shù)少時(shí)，算法精確度高，能準(zhǔn)確進(jìn)行移動(dòng)機(jī)器人位姿估計(jì)，可應(yīng)用于視覺里程計(jì)或視覺伺服控制的研究，故移動(dòng)機(jī)器人移動(dòng)的時(shí)候需要不停轉(zhuǎn)換關(guān)鍵幀，如何快速的選擇關(guān)鍵幀也是一個(gè)關(guān)鍵問題.且由相機(jī)或攝像機(jī)拍攝的二維圖像,不可避免地會(huì)因?yàn)槲矬w的自身遮擋或者其他原因的遮擋造成一部分區(qū)域無(wú)法呈現(xiàn)，因此,每一幅圖像總有一部分特征點(diǎn)會(huì)缺失且會(huì)有一些圖像上的特征點(diǎn)由于不能出現(xiàn)在每一幀導(dǎo)致不能被利用，對(duì)于求解缺失點(diǎn)，可進(jìn)行缺失點(diǎn)的辨識(shí)，如利用重投影和可見點(diǎn)構(gòu)造一個(gè)目標(biāo)函數(shù)再利用L-M或牛頓法進(jìn)行求解.

本文得到浙江工業(yè)大學(xué)控制科學(xué)與工程學(xué)科開放基金(20140808，20150710)的資助.

[1] 劉璽,方勇純,張雪波.基于單應(yīng)矩陣的位姿估計(jì)方法及應(yīng)用[C]//程代展,李川.第27屆中國(guó)控制會(huì)議論文集.北京：北京航空航天大學(xué)出版社，2008.

[2] 楊東勇,馮形松.采用形態(tài)學(xué)算子的運(yùn)動(dòng)目標(biāo)檢測(cè)跟蹤方法研究及實(shí)現(xiàn)[J].浙江工業(yè)大學(xué)學(xué)報(bào),2010,38(2):149-154.

[3] RAMBACH J R, TEWARI A, PAGANI A, et al. Learning to fuse: a deep learning approach to visual-inertial camera pose estimation[C]//Proceedings of IEEE International Symposium on Mixed and Augmented Reality.Piscataway: IEEE, 2016:71-76.

[4] 孫建輝，劉坤.基于序列圖像特征識(shí)別實(shí)時(shí)反饋伺服跟蹤研究[J].浙江工業(yè)大學(xué)學(xué)報(bào)，2015,43(2):185-189.

[5] KESHMIRI M, XIE W F, MOHEBBI A. Augmented image based visual servoing of a manipulator using acceleration command[J]. IEEE transactions on industrial electronics, 2014, 61(10):5444-5452.

[6] GALVEZLOPEZ D, SALAS M, TARDOS J D, et al. Real-time monocular object SLAM[J]. Robotics & autonomous systems, 2016(75):435-449.

[7] 管秋,金俊杰,張劍華,等.基于最優(yōu)RANSAC算法的非增加式多視圖三維重建[J].浙江工業(yè)大學(xué)學(xué)報(bào),2015,43(5):473-478.

[8] TOMASI C, KANADE T. Shape and motion from image streams under orthography: a factorization method[J]. International journal of computer vision, 1992, 9(2):137-154.

[9] DAI Yuchao, LI Hongdong, HE Mingyi. Projective multiview structure and motion from element-wise factorization[J]. IEEE transactions on pattern analysis and machine intelligence, 2013, 35(9): 2238-2251.

[10] WANG G, ZELEK J S, WU Q M J, et al. Robust rank-4 affine factorization for structure from motion[C]//Proceedings of IEEE Workshop on Applications of Computer Vision. Piscataway: IEEE, 2013:180-185.

[11] LI H. Multi-view structure computation without explicitly estimating motion[C]//Proceedings of IEEE Conference on Computer Vision and Pattern Recognition. Piscataway: IEEE, 2010:2777-2784.

[12] LU G. Image-based indoor localization system based on 3D SfM model[C]//Proceedings of the International Society for Optics and Photonics on IS&T/SPIE Electronic Imaging. San Francisco, USA: SPIE, 2014.

[13] OLIENSIS J. Structure from linear or planar motions[C]//1996 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Piscataway: IEEE, 1996:335-342.

[14] VIDAL R, OLIENSIS J. Structure from planar motions with small baselines[C]//Proceeding of European Conference on Computer Vision. Berlin Heidelberg: Springer, 2002:383-398.

[15] TAN T N, BAKER K D, SULLIVAN G D. 3D structure and motion estimation from 2D image sequences[J]. Image & vision computing, 1993, 11(4):203-210.

[16] LI J, CHELLAPPA R. Structure from planar motion[J]. IEEE transactions on signal processing society, 2006, 15(11):3466-3477.

[17] KONOLIGE K, AGRAWAL M. FrameSLAM: from bundle adjustment to real-time visual mapping[J]. IEEE transactions on robotics, 2008, 24(5):1066-1077.

[18] MOURAGNON E, LHUILLIER M, DHOME M, et al. Generic and real-time structure from motion using local bundle adjustment[J]. Image & vision computing, 2009, 27(8):1178-1193.