基于平面运动约束的移动机器人位姿估计

,,,

(1.浙江工业大学 信息工程学院，浙江 杭州 310023; 2.南开大学 计算机与控制工程学院，天津 300071)

自主移动机器人的一个基本功能是可以自我确定自身在环境中的位置[1].视觉传感器信息量大，能完成很多智能化任务[2]，如位姿估计[3]、视觉伺服控制[4-5]、同步定位及地图构建(SLAM)[6]等，准确的位姿估计是视觉伺服控制和SLAM的基础.在利用视觉传感器测量移动机器人位姿时，通常把视觉传感器固定在移动机器人上，移动机器人运动时，通过摄像机拍摄的二维图像的像素信息来得到移动机器人自身的位置和姿态，简称位姿.由于单目摄像机成本低廉且无需解决立体匹配的难题，因此受到广泛的研究和应用[1].但单目摄像机面临的最主要问题是图像中缺乏深度信息,也就是尺度的模糊性.在这种情况下，通常利用摄像机模型，根据两幅或多幅图像确定移动机器人的相对位姿，即实现从二维图像中获取移动机器人的三维位姿信息.

近年来，通过运动拍摄图片得到的二维信息来获得运动信息和场景结构信息的SFM(Structure from motion)[7]算法得到了较大的发展.关于SFM的研究开始于20 世纪80 年代，其基本思想是根据单目图像序列的二维信息直接恢复出摄像机运动的位姿信息和三维场景结构信息.其中主要有几种不同的SFM方法，如因式分解法[8-10]、多视图几何方法[11]和基于模型的方法[12]等.基于因式分解的SFM算法最早由Tomashi等提出，并应用于正射投影[8].其主要思想是通过秩约束(Rank constraints)来表示数据中存在的几何不变量，将观测矩阵(由图像中一组特征跟踪点的坐标组成)通过因式分解得出摄像机的运动参数和特征点结构信息.这种方法相比其他SFM方法，如基于模型的SFM方法和多视图几何方法，基于因式分解的SFM方法不依赖具体模型，对场景无特殊要求，不依赖某些特殊的特征和图像，且同时考虑所有图像中的数据，因而有较强的抗噪能力.而实际情况中，平面运动的情况很普遍，增加平面运动约束，可降低观测矩阵的秩，简化了观测矩阵的分解，目前很多因式分解方法结合地平面约束进行计算[13-15]，但大多数方法采用线性不等式或反复迭代来分解观测矩阵，很多情况下只能保证局部最优而忽略了全局最优.文献[16]用消失点、消失线和平面约束来构建观测矩阵，只需要进行奇异值分解、最小二乘来求解运动矩阵和恢复三维结构，其采用所有相同特征点进行运算，可以保证全局最优且计算效率高.但该方法在室内环境或者不存在平行线的室外环境，对消失线和消失点的提取困难，且很难获得正确的约束矩阵，使得算法经常失效.针对以上方法的不足，笔者提出一种奇异值分解与非线性约束相结合的因式分解方法，通过特征点、摄像机标定内参和地平面约束构建观测矩阵并分解，将分解得到的位姿通过光束平差法[17-18]进行优化，保证结果的准确性.

1 观测矩阵获取

则由摄像机内参数模型可得

(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Τ)，为避免后面出现最小二乘的病态问题，使运动矩阵M=U，形状矩阵S=D·VΤ.

2) 为了使W服从秩为3的约束，选取U的前三列，D的前三行前三列(特征值最大的三个元素)，VΤ的前三行构建新的观测矩阵W为

(10)

(11)

式中Ri(i∈[1,m])为每帧相对于第一帧的旋转矩阵，因其与自身转置的乘积为单位阵，故必满足

(12)

2.2 光束平差法

光束平差法的最终目的时为了减少测量图像的特征点和反计算特征点之间的重投影误差.最小化误差算法使用的是最小二乘算法，为了优化移动机器人的计算位姿，建立函数

(13)

3 实验结果分析

对于笔者提出的改进型因式分解方法，本节进行实验结果分析和验证.如图2所示，采用Pioneer3-DX移动机器人进行实验，其上安装有SV400型摄像机，移动机器人行走时拍摄图片.实验中先固定一帧图像为参考帧，随后机器人从任意一点出发，向参考帧的方向移动，每秒拍摄50 帧图像，如图1所示.在用OpenCV对摄像机进行标定后，通过基于SIFT的特征点提取和匹配，选取在每一帧中都出现的特征点，并提取其像素值，平面特征点像素坐标随移动机器人运动变化，如图3所示.星形对应的坐标是参考帧提取的像素值(图1中移动机器人在参考位置拍摄的图像中特征点Oj对应的像素坐标)，圆形对应的是移动机器人初始位姿拍摄的图像特征点，轨迹对应的坐标是移动机器人行走时所拍摄图片的像素坐标(图1中移动机器人在不同位姿拍摄图像中Oj的像素坐标).

图2 实验装置Fig.2 Experimental setup

图3 平面特征点像素坐标Fig.3 Pixel coordinates of planar feature points

如图4所示，移动机器人实际运动轨迹为图4中曲线，三角形方向为机器人运动方向，图4中散点为笔者算法计算求得的移动机器人位姿，可知计算求得的移动机器人位姿基本符合实际位姿.笔者算法在平面场景和立体场景下的实验结效果基本一致，如图5～7所示.在两种情况下分别对实验误差进行计算：1) 由图5，6可知,平面场景下，当少量图像帧数参与计算时，移动机器人位姿估计准确，而当大量图像参与计算时，旋转矩阵基本正确，而平移矩阵由于噪声影响误差较大，因而要及时切换参考帧，每次用低于50 帧的图像参与计算.故在图2的立体场景下，选用21 帧图像;2) 由图7可知,实验误差较小，位姿结果计算准确.

图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 结 论

针对大部分位姿估计方法只适用于平面场景的问题，提出了一种同时适用于平面场景和立体场景的位姿估计方法.该方法结合奇异值分解和光束平差法，最少用三幅图像就能计算移动机器人当前的位姿.且采用地平面约束，降低了观测矩阵的秩，从而简化了分解步骤且采用SVD进行观测矩阵分解，避免了使用迭代优化来分解观测矩阵，提高了计算效率.实验结果表明：当参与计算图像帧数少时，算法精确度高，能准确进行移动机器人位姿估计，可应用于视觉里程计或视觉伺服控制的研究，故移动机器人移动的时候需要不停转换关键帧，如何快速的选择关键帧也是一个关键问题.且由相机或摄像机拍摄的二维图像,不可避免地会因为物体的自身遮挡或者其他原因的遮挡造成一部分区域无法呈现，因此,每一幅图像总有一部分特征点会缺失且会有一些图像上的特征点由于不能出现在每一帧导致不能被利用，对于求解缺失点，可进行缺失点的辨识，如利用重投影和可见点构造一个目标函数再利用L-M或牛顿法进行求解.

本文得到浙江工业大学控制科学与工程学科开放基金(20140808，20150710)的资助.

[1] 刘玺,方勇纯,张雪波.基于单应矩阵的位姿估计方法及应用[C]//程代展,李川.第27届中国控制会议论文集.北京：北京航空航天大学出版社，2008.

[2] 杨东勇,冯形松.采用形态学算子的运动目标检测跟踪方法研究及实现[J].浙江工业大学学报,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].浙江工业大学学报，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] 管秋,金俊杰,张剑华,等.基于最优RANSAC算法的非增加式多视图三维重建[J].浙江工业大学学报,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.