基于单目视觉的相机位姿解算

王 鹏，赵汗青，王江峰，刘红彬，陈 伟

(1.装甲兵工程学院 控制科学与工程，北京 100072；2.装甲兵工程学院 科研部，北京 100072)

基于单目视觉的相机位姿解算

王 鹏1，赵汗青2，王江峰2，刘红彬2，陈 伟2

(1.装甲兵工程学院 控制科学与工程，北京 100072；2.装甲兵工程学院 科研部，北京 100072)

机器人在工作环境中利用传感器感知信息来实现可靠的定位，这是自主移动机器人的基本功能之一。GPS等卫星定位系统在室内信号较弱且有偏差，而视觉定位技术可以通过图像特征匹配进行位姿估计，因而被广泛应用。文中选取结构简单、成本低且易实现单目视觉定位的技术作为研究对象，以屋顶上打的结构光点作为观测数据来源，设计利用匹配特征点之间的关系，解算单目相机位姿的系统。通过对机器人所获取的数据进行实验分析表明，文中所设计的系统对于单目视觉机器人定位是有效的。

单目视觉；顶视；位姿解算；自主移动机器人

随着计算机技术与人工智能技术的发展，在工厂、生活当中出现了许多种类的自主移动机器人[1-3]。自主移动机器人视觉定位是近些来发展起来的一种定位方法，该方法在实时数字摄影测量、机器人运动导航、飞行器主动视觉定位等领域具有重要的使用价值，也是目前较为流行的研究课题[4]。

因此, 研究者开始关注基于视觉原理的相机空间定位方法。文献[5]对室内定位技术进行了综述, 介绍目前主要的定位算法，以及各种算法衍生出来的各种定位技术[5]。文献[6]利用场景中的三垂线和点特征构建道路结构特征, 并在此基础上提出一个基于道路结构特征的单目视觉定位算法[6]。文献[7]提出使用单个天花板视觉传感器的快速和鲁棒的技术，所提出的算法适用于需要非常高的定位精度的系统[7]。为实现快速、鲁棒性高的自主移动机器人定位，本文使用单目相机，采取顶视方案，利用已经匹配好的特征点信息，解算相机位姿，最后根据实验证明方法的有效性。

1 单目相机位姿解算

实验平台如图1所示，相机采用顶视模型，在屋顶打红色激光以增加特征点的选取与匹配。在两幅二维图像中，利用对极约束得到同一场景两幅图像之间的几何关系，它独立于场景结构，只与摄像机的内、外部参数有关[8]。其一，借助射影几何和齐次坐标等数学工具，可以描述两幅图像间的对极几何关系，计算出旋转和平移矩阵；其二，利用三角化得到特征点的三维坐标，之后的图像就可利用PNP问题的求解方法解算相机位姿。

图1 实验平台

2 基于对极几何求解单目相机位姿

2.1 对极约束原理

如图2所示，这是对极几何约束的示意图。其中，O1和O2分别是是相机两个时刻的中心，空间一点P在左边图片的投影为P1， 在右边图片的投影为P2。连接O1与P1，O2与P2，在三维空间会交于点P。O1，O2，P确定的平面为极平面，极平面与左右图像的像平面所交得到相交线l1和l2称为极线，连接O1与O2交左右图像的像平面于点e1,e2。

图2 对极几何约束

以左边图像为世界坐标系，设P的空间坐标为

P=[X,Y,Z]T

根据针孔相机模型，可得

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

(1)

其中，K为相机内参，R，t为相机的旋转，平移矩阵。在齐次坐标下，上式可写为

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

(2)

像素点坐标归一化得

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

(3)

代入式(2)得

x2=Rx1+t

(4)

(5)

将式(2)代入，可得

(6)

两式是对极约束数学表达，做进一步简化

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

可得

(7)

其中，E为本质矩阵；F为基础矩阵[9]。所以，根据配对点可以求出E或者F，进而求出R，t。

可以看出本文实验，特征点有可能都会落在同一个平面，这需要利用单应矩阵进行运动估计[10]

(8)

其中，n为平面的法向量；d为摄像机到平面距离

(9)

上式等号在非零因子下成立，通常乘以一个非零因子使得h9=1。整理有

(10)

(11)

可以看出，一组匹配点可以构造两个约束，而单应矩阵的自由度为8，所以最少通过4对匹配特征点求解。

2.2 求解单目相机位姿

从式(7)可以得出，平移与旋转各有3个自由度，由于尺度等价性，所以本质矩阵只有5个自由度，最少可以用5对点求解。如果只利用本质矩阵的线性质，最少需要利用8对点可以求解。在本文实验中，特征点有可能都会落在同一个平面，这样需要利用单应矩阵进行运动估计。以本质矩阵为例恢复相机位姿，基本矩阵与单应矩阵恢复相机位姿与本质矩阵类似，都会有4个解。

设E的SVD分解是

E=U∑VT

(12)

其中，U和V为正交矩阵，Σ为奇异值矩阵。根据E的性质，可以得到Σ=diag(σ，σ，0)。在SVD分解中，对于任何E都有两个t，R与之对应。因此，如图3所示，一共有4组解。只有第1个解，点P在两个相机下具有正深度，符合要求[11]。

图3 本质矩阵4个解的几何示意

分解单应矩阵时有4组解，可以得到法向量n根据特征点深度为正，可以排除两个。因为场景平面与相机平行，理论上法向量n=1T。代入即可得到正确解。本文实验会出现特征点共面情况，因此需要同时利用本质矩阵与单应矩阵求解相机位姿，选择重投影误差较小的作为最终选择。

3 PNP问题

PNP问题是利用透视投影得到的n个象点，求解摄像时刻物体相对于相机的位姿。当分别求得两个不同摄像时刻下，物体相对于相机的位姿, 则可以求得这两个时刻间物体相对于相机的运动。在两张图像中，其中一张特征点的三维位置已知，最少只需要3对就可以估计相机运动。本文实验利用三角化进行深度估计，得到一张特征点的三维位置，进而转换为PNP问题进行求解。文献[12]中，对于透视投影来说，要使得 PNP 问题有确定的有限数目解，最少需要3组控制点[12]。P3P问题最多有4个解且解的上限可以达到[13]。对于P4P问题,当4个控制点共面时有惟一解[14]，当4个控制点不共面时，最多可能有4个解且解的上限可以达到[15]，P5P问题最多可能有两个解,且解的上限可以达到[16]，当N≥6时，可以用直接线性变换法。

3.1 直接线性变换法

如图2，点P的齐次坐标为[X,Y,Z,1]T。在左图像对应的特征点x1=(u1,v1,1)T，定义矩阵T=[R|t]，可以得出

(13)

可以得到

(14)

每一组特征点可以得到两个约束，矩阵T有12个变量，因此最少通过6对匹配点求解T。当匹配点>6个，利用最小二乘求解。以上t和R是通过上述原理计算出的相机运动。

3.2 P3P

P3P仅使用3对匹配点即可求出相机位姿。利用匹配好的3对点，已知一幅图像3个点的世界坐标，3个点记为A，B，C。另一幅图像3个点的图像坐标，记为a，b，c。匹配点的几何关系如图4所示。

图4 特征点几何关系

记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)

因为点a，b，c图像坐标已知，所以cos〈a,b〉,cos〈b,c〉,cos〈a,c〉已知。AB、BC、AC可以由点A，B，C得到。所以，式(15)为二元二次方程。该方程最多可以有4个解，利用一对验证点D，d，找出合适的解。最终得到点A，B，C在另一幅图像坐标系下的三维坐标，然后根据3D点对匹配计算相机的位姿R，t。

4 实验

为了验证算法获得相机位姿的有效性，本文利用如图1所示的实验平台，在室内获取了图像。经过相机标定获得相机参数，如图5对图像进行特征点的提取与匹配，获得匹配点对的信息。相机的内参矩阵K为

图5 特征点匹配

4.1 对极几何求解相机位姿

利用已经标定的内参矩阵，匹配好的特征点的信息。根据上述原理，可以得到相机的基本矩阵，本质矩阵以及单应矩阵，求出平移和旋转矩阵

其中，E与F相差相机内参矩阵。从E，F和H都可以分解运动，因为H需要假设特征点位于平面，本实验无法保证特征点位于同一平面，所以利用E来分解运动

在单目SLAM中，将轨迹和地图同时缩放任意倍数，得到的地图是一致的。E具有尺度等价性，分解得到的t，R也有一个尺度等价性。因此，通常将t进行归一化，尺度为1。以此次的t为单位1，计算相机运动和特征点的三维信息。

4.2 P3P求解相机位姿

利用三角化，可以得到特征点的空间三维信息，通过上述原理计算t，R

可以看出两次计算中R相差无几，t不同是因为引入了深度信息所导致的。P3P实验过程中存在一些问题，P3P只利用3个点的信息，当多余3组点时，信息利用率低。特征点受噪声影响或匹配错误，则无法计算。在视觉SLAM里，通常使用P3P等方法估计相机位姿，而后构建最小二乘优化问题进行优化。

5 结束语

本文利用在屋顶打红色激光点，降低了特征点的选取与匹配。前期工作对图像进行预处理，提取ORB特征点作为实验数据。本文根据实验平台的情况，设计解算相机位姿的系统，利用最少的匹配点对，求解相机位姿。实验表明，对于此次实验模型来说，以上两种方式都可以计算出相机位姿。也验证了在此场景下，匹配特征点较少时，系统仍可以解算相机位姿。系统同时根据本质矩阵与单应矩阵求解相机位姿，选择重投影误差较小的作为最终选择，解决了特征点在同一平面本质矩阵无法解算相机位姿的问题。本文存在的不足之处是未考虑噪声、匹配错误的影响，下一步需要对计算结果进行优化处理，使结果更加接近真值，提高系统的鲁棒性。另外相机的内参作为已知给出，标定时会出现误差，下一步将相机内参加入优化，直接计算相机的位姿。

[1] 武嘉琪.计算机人工智能技术的发展与应用研究[J].信息与电脑,2016(7):103-104.

[2] 王彦娴.人工智能在计算机网络技术中的应用研究[J].信息技术与信息化,2015(8):84-85.

[3] 田良.人工智能技术在计算机中的发展和应用[J].信息通信,2015(1):156-158.

[4] 王鑫.基于单目视觉的目标物定位算法研究[D].哈尔滨:哈尔滨工业大学,2016.

[5] 赵锐,钟榜.室内定位技术及应用综述[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] 吴昊.基础矩阵估计方法研究[D].兰州:兰州大学,2015.

[9] 吴福朝.计算机视觉中的数学方法[M].北京:科学出版社,2008.

[10] 丁夏清,杜卓洋,陆逸卿,等.基于混合势场的移动机器人视觉轨迹规划[J].浙江大学学报,2016,50(7):1298-1306.

[11] 王文斌,刘桂华,刘先勇,等.本质矩阵五点算法伪解的两种剔除策略[J].光电工程,2010(8):46-52.

[12] 关凯.基于标识的室内视觉定位算法研究[D].哈尔滨:哈尔滨工业大学,2016.

[13] 杨述强.基于单目视觉的场景三维重建与飞行器位姿求解关键技术研究[D]. 长沙:国防科学技术大学,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

空军“十二五”条件建设项目(2017216WX08)

王鹏(1992-)，男，硕士研究生。研究方向：单目视觉定位。赵汗青(1961-)，男，博士，高级工程师。研究方向：自主车环境感知，任务规划。

10.16180/j.cnki.issn1007-7820.2017.12.020

TN911.73；TP391.41

A

1007-7820(2017)12-075-04