基于OpenCV的水下机器人单目定位技术研究与仿真

韩 冲，苏 涛，谢基榕

(中国船舶科学研究中心 深海载人装备国家重点实验室，江苏 无锡 214082)

基于OpenCV的水下机器人单目定位技术研究与仿真

韩 冲，苏 涛，谢基榕

(中国船舶科学研究中心 深海载人装备国家重点实验室，江苏 无锡 214082)

针对水下机器人自主回收对接时的定位问题，提出了一种基于OpenCV的单目定位技术研究与仿真方法;首先确定回收装置在回收侧的光源标记点坐标信息；然后借助OpenCV算法库，通过对水下机器人自带摄像机的标定，得出反应摄像机固有信息的内参数，通过摄像机对回收装置光源标记点的识别，得出光源标记点在图像上的像素坐标，结合其世界坐标，得出反映回收装置在摄像机坐标系下位置和姿态信息关系的平移向量和旋转向量，进而确定水下机器人在回收装置坐标系下的位置和姿态信息；最后运用CATIA软件对摄像机拍摄模型进行建模和仿真，结果表明，所提方法不仅能快速地获得水下机器人的位置和姿态信息，而且定位精度高，满足水下机器人在自主回收对接时的设计要求。

OpenCV；水下机器人；单目视觉；位置和姿态；CATIA建模

0 引言

深海空间站国家重大专项的设立，标志着我国进入了深海开发的新时代，其中，水下机器人作为深海空间站勘探和开采深海资源的主要工具，发挥着不可或缺的作用。在水下机器人开发应用过程中，合理有效的回收对接方案将直接影响到水下机器人的作业范围、隐蔽性和回收释放时间，于是自主回收对接技术[1]逐渐成为水下机器人研究的前沿和关键技术。自主回收对接的难点和前提在于对水下机器人的精确定位，即获知水下机器人本体相对于回收对接目标的位置和姿态信息。

水下机器人的回收对接对远端的定位要求并不高，近距离的导航定位精度才是最终影响回收对接的关键。近距离使用的导航定位传感器一般包括声学传感器、光学传感器和视觉传感器，视觉传感器以其具有分辨率高和几乎不存在盲区等优点成为广大专家学者在研究水下机器人回收对接定位时的选择。韩国科技高等研究所的Myung-Hwan Oh和Jun-Ho Oh利用ISIMI进行的视觉伺服算法[2]设计了一种视觉引导对接方法，实现了水下机器人的精确定位，但是其过分依赖于复杂的算法，导致实时性较差；哈尔滨工程大学的施小威等[3]针对一种具有圆形对接导航标志的坐落式对接平台，提出了一种基于纹理控制的金字塔互相关快速双目视觉测距的方法，此方法有较高的精度和实时性，但是却不适用于姿态信息的求解；西北工业大学的严卫生等[4]针对自主水下航行器(AUV)的不同姿态，分别给出对应的平移定位与旋转定位的单目定位方法，但是并没有指出AUV不同姿态的具体判定依据。

基于上述局限，结合实际应用场合，本文提出了一种基于计算机视觉的水下机器人单目定位方法，在位置信息和姿态信息的确定上均有较高的精度，而且对OpenCV计算机视觉库的应用，算法的复杂度得到了极大的简化，从而保证了算法具有较高的实时性。最后通过CATIA软件建立了水下相机拍摄模型，仿真结果表明，该方法定位精度高，实时性好，从而为进一步的水池实验奠定了理论基础。

1 问题描述

水下机器人(即潜器)在自动回收对接过程中，如图1所示， P1、P2、P3、P4为设置在回收装置进门侧顶点处的4个光源标记点。潜器在靠近回收装置过程中，回收装置包括光源标记点在摄像机平面中成像，根据摄像机针孔成像原理[5]，标记点在世界坐标系下的坐标和其在像素坐标系下的坐标可以由以摄像机像素焦距、光学中心为主的内参数和以旋转向量、平移向量为主的外参数[6]来表征，从而确定潜器相对于回收装置的位置和姿态信息。

图1 水下机器人自动回收示意图

若要获得目标的位置和姿态信息，就必须知道摄像机的图像坐标系与各坐标系的相互转换模型。摄像机成像时的坐标系有4个，均是右手坐标系；由xw，yw和zw构成的世界坐标系；xc，yc和zc构成的摄像机坐标系；由px和py构成的图像坐标系；由u和v构成的像素坐标系。 轴和图像坐标平面垂直，oc为相机光心，M为物点，m为像点，如图2所示。

图2 摄像机成像过程各坐标系

2 水下机器人位置和姿态信息获取

OpenCV的calib3d模块主要用于摄像机标定和场景的三维重建[7]，其中包含的库函数可以方便地对摄像机进行内参的标定和外参的获取。对于设定好的摄像机，其内部参数是固定的，通过实时地获取不同成像平面下摄像机的外部参数[8]，便可以实现对目标的实时位姿信息获取。定位过程中，由4个目标点共面且光心不在此平面时必然有唯一解[9]，首先对回收装置的光源标记点进行识别，得到其像素坐标，结合标记点的世界坐标，运用OpenCV内置函数，解算出实时的旋转和平移向量，也就获得了回收装置相对于潜器的实时位置和姿态信息，文中摄像机坐标系等价为潜器本身的坐标系。

2.1 摄像机标定

利用OpenCV进行摄像机标定时，需要拍摄至少10幅以上各个角度和平面的棋盘图片[10]，利用cv2.findChessboardCorners()函数可以得到棋盘格每个棋盘亚像素级角点的像素坐标，借助cv2.calibrateCamera()摄像机标定函数，可以得到摄像机的内参信息，该函数返回摄像机矩阵，畸变系数，旋转和平移向量。

2.2 目标点识别

水下机器人在靠近回收装置时，摄像机实时捕捉回收装置的光源标记点，进行图像预处理，包括高斯滤波，去白噪声。如图3所示，对目标点的识别也就是获取光源标记点中心的像素坐标。

图3 水下机器人摄像机成像

为了提高计算效率，在目标识别时，运用检测光源轮廓的方法[11]，OpenCV函数库提供cv2.findContours()函数对轮廓进行检测，返回光源轮廓的像素坐标。由于图像的旋转不确定性[12]，在实际应用中还需对目标点进行排序，以便将像素坐标点和世界坐标点相对应。

2.3 摄像机定位

在得到了光源标记点的像素坐标后运用OpenCV中提供的cv2.solvePnPRansac()函数进行摄像机定位，返回回收装置平面坐标系到默认潜器摄像机平面坐标系的旋转向量和平移向量，而只有标准的潜器坐标系在标准世界坐标系的姿态才能直观反映出潜器的姿态，故默认潜器坐标系需要左乘一姿态矩阵转换为标准潜器坐标系，变换过程为：

(1)

2.3.1 根据平移向量计算摄像机光心位置

通过OpenCV算法函数和必要的坐标轴转换，得到了摄像机平面坐标系到回收装置平面坐标系的平移向量，它的物理含义为世界坐标系坐标原点在摄像机坐标系下的坐标，以下公式表示了这种转换关系：

(2)

由于旋转矩阵各元素两两正交，自由度和秩均是3，所以可以得出：

(3)

此时，-RTt表示摄像机坐标系坐标原先在世界坐标系中的坐标，即摄像机的光心位置。

2.3.2 根据平移向量计算摄像机平面姿态

由于OpenCV中，返回的外参数在物理含义上不能直观地反映姿态信息，所以先将得到的旋转向量转化为可以直观表示物体旋转的欧拉角[13]形式。这个过程可以通过罗得里格斯变换[14]获得。OpenCV里的罗得里格斯函数解算出来的矩阵是：

R=Rx(φ)Ry(φ)Rz(θ)=

(4)

式中,旋转矩阵R表征世界坐标系依次沿xw、yw和zw轴逆时针旋转角度θ、φ和φ后与摄像机坐标系姿态相同。

上述旋转顺序下的欧拉角可以直观地反映出潜器的姿态，分别为偏航角β、俯仰角α、滚转角γ[15]，即船舶领域中的艏向、纵倾和横倾[16]。根据纵倾的定义，潜器前进方向-ocxc轴与世界坐标系水平面-owxwyw平面的夹角即为潜器的纵倾角，摄像机坐标系下前进方向xc轴一点，其在世界坐标系下的坐标为MW，然后投影变换如图4所示。

图4 潜器姿态角求解

结合旋转矩阵的定义得出潜器的纵倾表达式：

sinα=-MWMW’/OWMW=-R13

(5)

即：

α=-arcsinR13

(6)

同理可以求出艏向和横倾的表达式：

(7)

此时求出纵倾、艏向和横倾的物理意义即为回收装置所在的标准世界坐标系下，潜器摄像机坐标系的姿态，实际运算时，潜器的摄像机坐标系到潜器的本体坐标系也有一个变换，这里把两个坐标系等价为一个坐标系。

2.4 摄像机位置和姿态获取算法软件流程

摄像机位姿确定是根据其捕捉的单帧图像实时计算获取的，软件流程如图5所示。

图5 摄像机位姿确定算法流程

3 仿真与结果分析

3.1 CATIA摄像机拍摄模型

本文借助CATIA三维建模软件，建立了水下机器人摄像机和回收装置的拍摄模型，如图6所示。P1、P2、P3、P4分别为回收装置进门侧顶点上的光源标记点，V2为摄像机的观测点，回收装置的中心点为三维坐标原点。

图6 摄像机回收装置模型

利用OpenCV可以对光源标记进行识别定位，分别表示出轮廓、重心和中心位置，重心坐标可以作为光源标记点的像素坐标，中心位置可以作为四个标记点的位置分辨依据，整个识别过程如图7所示。

图7 光源标记点识别定位过程

摄像机的内参信息是给定的，本次实验折算后的摄像机的内参信息见表1。

3.2 数据获取与分析

根据目标点的像素坐标、世界坐标以及相机的内参数，借助OpenCV，计算出50组摄像机在回收装置坐标系的位置和

表1 摄像机的内参数坐标

表2 摄像机在世界坐标系下的位姿对比信息/(位置/毫米；姿态/度)

姿态信息，然后与CATIA真实的位姿对比，得出OpenCV单目视觉定位出的计算位姿与真实位姿信息及误差，以位置-姿态的形式给出，位置表示x轴、y轴和z轴的距离信息，姿态表示纵倾、艏向和横倾的角度信息，其中，偏差为计算值与理论值之差，偏差率为偏差的模与理论值的比值，选取部分数据见表2。

由表2可知，由于潜器摄像机坐标系和回收装置世界坐标系的设定，且潜器总是面对着回收装置进行拍摄，所以潜器前进方向(x轴)的坐标在世界坐标系下均是负值；本文的研究方法在位置和姿态的确定上均有较高的精度，位置最大误差率不超过6%，姿态最大误差率不超过10%，满足回收对接设计要求。

图8为不同实验序号下的位置和姿态误差对比图，由图可知，六自由度位置和姿态求取中，其误差随着潜器与回收装置

图8 位姿信息偏差对比示意图

距离的增大有增大的趋势，符合机器视觉的光学特性，同时与实际经验分析相吻合。

考虑造成偏差的因素，主要有：第一：光源点像素坐标的获取受环境和识别算法等因素的影响存在一定误差，这会对后面的位置和姿态计算造成影响；第二：在计算摄像机平面在世界坐标系下的位置和姿态信息时，不同的求解算法对结果也会有一定的影响，同时算法本身也有一定的误差。

5 结论

本文提出了一种基于OpenCV的单目视觉定位方法,实现了水下机器人自主回收对接时位置和姿态信息的获取。并巧妙地借助CATIA软件对摄像机拍摄模型进行建模，通过数据的获取与分析确定了该方法的可行性。该方法简单直观，快速准确地解决了水下机器人自主回收对接时的定位问题，具有较高的工程应用价值。

[1] 李 晔, 姜言清, 张国成, 等.考虑几何约束的AUV回收路径规划[J].机器人,2015(4):478-485.

[2] Pan M L,Bong H J,Chong M L.A Docking and Control System for an Autonomous Underwater Vehicle[C].Proceedings of the MTS/IEEE OCEANS Conference,2002,1609-1614.

[3] 施小威,王晓娟.一种面向AUV水下对接的双目视觉测距方法[J].计算机测量与控制,2008,16(10): 1460-1462.

[4] 严卫生,高 智,杨小龙,等.面向AUV自主回收的单目视觉定位算法[J].电子设计工程,2014, 22(22): 174-176.

[5] Erik J. Programming Computer Vision with Python[M].Zhu W, Yuan Y, translated. Beijing: Posts and Telecom Press, 2014.

[6] 胡钊政, 赵 斌, 李 娜, 等.基于虚拟三面体的摄像机与二维激光测距仪外参数最小解标定新算法[J].自动化学报,2015,(11):1951-1960.

[7] Robert L.OpenCV2 Computer Vision Application Programming Cookbook [M].Zhang J, translated. Shanghai: Science Press, 2013:246-247.

[8] Zhao Z Q, Ye D, Zhang X,et al.Improved direct linear transformation for parameter decoupling in camera calibration[J].Algorithms,2016(2):100-105.

[9] 吴福朝,胡占义.PnP问题的线性求解算法[J].软件学报,2003,14(3):682-688.

[10] 宋 涛, 褚光宇, 侯培国,等.基于质心点优化的鱼眼摄像机标定[J].光子学报,2016,(5):100-105.

[11] 王 森, 伍 星, 刘 韬, 等.基于反对称双正交小波变换的多尺度归一化分割方法[J].计算机辅助设计与图形学学报,2016(1):106-118.

[12] Bradski G, Kaehler A. Learning OpenCV[M]. Yu S Q,Liu R Z, translated. Beijing: Tsinghua University Press, 2009:414-416.

[13] Weng J, Cohen P.Hemiou M. Camera calibration with molds and accuracy Evaluation [J]. IEEE Transaction on Pattern Analysis and Machine Intelligence, 2011, 16(15):469-476.

[14] Dai, Jian S.Euler-Rodrigues formula variations, quaternion conjugation and intrinsic connections[J]. Mechanism and Machine Theory: Dynamics of Machine Systems Gears and Power Transmissions Robots and Manipulator Systems Computer-Aided Design Methods, 2015,(1):144-152.

[15] 岳晓奎,侯小娟.基于Rodrigues参数的视觉相对位姿确定方法[J].宇航学报,2010,31(3):753-757.

[16] 施生达.潜艇操纵性[M].北京:国防工业出版社,1995.

Research and Simulation of Underwater Vehicle Monocular Positioning Technology Based on OpenCV

Han Chong, Su Tao，Xie Jirong

(State Key Laboratory of Deep-sea Manned Vehicles, China Ship Scientific Research Center, Wuxi 214082, China)

Focused on obtaining the position and attitude information for automation recovery of underwater vehicle, a research and simulation method of monocular positioning technology based on OpenCV was proposed. Firstly, the light source tag coordinate information for the recovery side of the recovery unit was determined;Then, with the help of OpenCV library, the intrinsic parameters of the camera's inherent information were obtained by calibrating the camera of the underwater vehicle. Through the recognition of the recovery markers, the translation and rotation vector which reacted position and attitude relationship of the reclaimer in the camera coordinate system were obtained. Then the position and attitude of the underwater vehicle in the world coordinate system of the reclaimer were determined. Finally, the camera shooting model was modeled and simulated by CATIA, the experimental results demonstrate that the method can obtain the position and attitude information of underwater vehicle quickly and accurately, and the positioning accuracy is high,which meets the design requirements of autonomous unterwater vehicle for recovery and docking.

OpenCV; underwater vehicle; monocular vision; CATIA modeling

2017-05-16；

2017-06-07。

韩 冲(1988-)，男，江苏徐州人，硕士，工程师，主要从事图像识别、计算机视觉方向的研究。

1671-4598(2017)12-0219-05

10.16526/j.cnki.11-4762/tp.2017.12.057

TP391

A