基于平面運動約束的移動機器人位姿估計

,,,

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

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

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

1 觀測矩陣獲取

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

(1)

圖1 攝像機模型Fig.1 The model of camera

(2)

其第i+1個攝像機坐標系下特征點的坐標與歸一化攝像機坐標之間的關系為

(3)

當移動機器人在平面運動時，對于每個點，由地平面約束(GPC)可知攝像機坐標的Z軸不變，即

以第i個坐標為參考坐標，可得

(4)

(5)

由式(2)和GPC約束可得

(6)

(7)

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

(8)

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

(9)

2 觀測矩陣分解

2.1 觀測矩陣分解步驟

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

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

(10)

(11)

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

(12)

2.2 光束平差法

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

(13)

3 實驗結(jié)果分析

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

圖2 實驗裝置Fig.2 Experimental setup

圖3 平面特征點像素坐標Fig.3 Pixel coordinates of planar feature points

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

圖4 移動機器人運動軌跡Fig.4 The mobile robot motion trajectories

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

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

圖7 立體場景下圖像位姿估計誤差Fig.7 The error of pose estimation with stereo scene

4 結(jié) 論

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

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

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

[2] 楊東勇,馮形松.采用形態(tài)學算子的運動目標檢測跟蹤方法研究及實現(xiàn)[J].浙江工業(yè)大學學報,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] 孫建輝，劉坤.基于序列圖像特征識別實時反饋伺服跟蹤研究[J].浙江工業(yè)大學學報，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è)大學學報,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.