韓 沖，蘇 濤，謝基榕

(中國船舶科學研究中心 深海載人裝備國家重點實驗室，江蘇 無錫 214082)

基于OpenCV的水下機器人單目定位技術(shù)研究與仿真

韓 沖，蘇 濤，謝基榕

(中國船舶科學研究中心 深海載人裝備國家重點實驗室，江蘇 無錫 214082)

針對水下機器人自主回收對接時的定位問題，提出了一種基于OpenCV的單目定位技術(shù)研究與仿真方法;首先確定回收裝置在回收側(cè)的光源標記點坐標信息；然后借助OpenCV算法庫，通過對水下機器人自帶攝像機的標定，得出反應(yīng)攝像機固有信息的內(nèi)參數(shù)，通過攝像機對回收裝置光源標記點的識別，得出光源標記點在圖像上的像素坐標，結(jié)合其世界坐標，得出反映回收裝置在攝像機坐標系下位置和姿態(tài)信息關(guān)系的平移向量和旋轉(zhuǎn)向量，進而確定水下機器人在回收裝置坐標系下的位置和姿態(tài)信息；最后運用CATIA軟件對攝像機拍攝模型進行建模和仿真，結(jié)果表明，所提方法不僅能快速地獲得水下機器人的位置和姿態(tài)信息，而且定位精度高，滿足水下機器人在自主回收對接時的設(shè)計要求。

OpenCV；水下機器人；單目視覺；位置和姿態(tài)；CATIA建模

0 引言

深?？臻g站國家重大專項的設(shè)立，標志著我國進入了深海開發(fā)的新時代，其中，水下機器人作為深海空間站勘探和開采深海資源的主要工具，發(fā)揮著不可或缺的作用。在水下機器人開發(fā)應(yīng)用過程中，合理有效的回收對接方案將直接影響到水下機器人的作業(yè)范圍、隱蔽性和回收釋放時間，于是自主回收對接技術(shù)[1]逐漸成為水下機器人研究的前沿和關(guān)鍵技術(shù)。自主回收對接的難點和前提在于對水下機器人的精確定位，即獲知水下機器人本體相對于回收對接目標的位置和姿態(tài)信息。

水下機器人的回收對接對遠端的定位要求并不高，近距離的導(dǎo)航定位精度才是最終影響回收對接的關(guān)鍵。近距離使用的導(dǎo)航定位傳感器一般包括聲學傳感器、光學傳感器和視覺傳感器，視覺傳感器以其具有分辨率高和幾乎不存在盲區(qū)等優(yōu)點成為廣大專家學者在研究水下機器人回收對接定位時的選擇。韓國科技高等研究所的Myung-Hwan Oh和Jun-Ho Oh利用ISIMI進行的視覺伺服算法[2]設(shè)計了一種視覺引導(dǎo)對接方法，實現(xiàn)了水下機器人的精確定位，但是其過分依賴于復(fù)雜的算法，導(dǎo)致實時性較差；哈爾濱工程大學的施小威等[3]針對一種具有圓形對接導(dǎo)航標志的坐落式對接平臺，提出了一種基于紋理控制的金字塔互相關(guān)快速雙目視覺測距的方法，此方法有較高的精度和實時性，但是卻不適用于姿態(tài)信息的求解；西北工業(yè)大學的嚴衛(wèi)生等[4]針對自主水下航行器(AUV)的不同姿態(tài)，分別給出對應(yīng)的平移定位與旋轉(zhuǎn)定位的單目定位方法，但是并沒有指出AUV不同姿態(tài)的具體判定依據(jù)。

基于上述局限，結(jié)合實際應(yīng)用場合，本文提出了一種基于計算機視覺的水下機器人單目定位方法，在位置信息和姿態(tài)信息的確定上均有較高的精度，而且對OpenCV計算機視覺庫的應(yīng)用，算法的復(fù)雜度得到了極大的簡化，從而保證了算法具有較高的實時性。最后通過CATIA軟件建立了水下相機拍攝模型，仿真結(jié)果表明，該方法定位精度高，實時性好，從而為進一步的水池實驗奠定了理論基礎(chǔ)。

1 問題描述

水下機器人(即潛器)在自動回收對接過程中，如圖1所示， P1、P2、P3、P4為設(shè)置在回收裝置進門側(cè)頂點處的4個光源標記點。潛器在靠近回收裝置過程中，回收裝置包括光源標記點在攝像機平面中成像，根據(jù)攝像機針孔成像原理[5]，標記點在世界坐標系下的坐標和其在像素坐標系下的坐標可以由以攝像機像素焦距、光學中心為主的內(nèi)參數(shù)和以旋轉(zhuǎn)向量、平移向量為主的外參數(shù)[6]來表征，從而確定潛器相對于回收裝置的位置和姿態(tài)信息。

圖1 水下機器人自動回收示意圖

若要獲得目標的位置和姿態(tài)信息，就必須知道攝像機的圖像坐標系與各坐標系的相互轉(zhuǎn)換模型。攝像機成像時的坐標系有4個，均是右手坐標系；由xw，yw和zw構(gòu)成的世界坐標系；xc，yc和zc構(gòu)成的攝像機坐標系；由px和py構(gòu)成的圖像坐標系；由u和v構(gòu)成的像素坐標系。 軸和圖像坐標平面垂直，oc為相機光心，M為物點，m為像點，如圖2所示。

圖2 攝像機成像過程各坐標系

2 水下機器人位置和姿態(tài)信息獲取

OpenCV的calib3d模塊主要用于攝像機標定和場景的三維重建[7]，其中包含的庫函數(shù)可以方便地對攝像機進行內(nèi)參的標定和外參的獲取。對于設(shè)定好的攝像機，其內(nèi)部參數(shù)是固定的，通過實時地獲取不同成像平面下攝像機的外部參數(shù)[8]，便可以實現(xiàn)對目標的實時位姿信息獲取。定位過程中，由4個目標點共面且光心不在此平面時必然有唯一解[9]，首先對回收裝置的光源標記點進行識別，得到其像素坐標，結(jié)合標記點的世界坐標，運用OpenCV內(nèi)置函數(shù)，解算出實時的旋轉(zhuǎn)和平移向量，也就獲得了回收裝置相對于潛器的實時位置和姿態(tài)信息，文中攝像機坐標系等價為潛器本身的坐標系。

2.1 攝像機標定

利用OpenCV進行攝像機標定時，需要拍攝至少10幅以上各個角度和平面的棋盤圖片[10]，利用cv2.findChessboardCorners()函數(shù)可以得到棋盤格每個棋盤亞像素級角點的像素坐標，借助cv2.calibrateCamera()攝像機標定函數(shù)，可以得到攝像機的內(nèi)參信息，該函數(shù)返回攝像機矩陣，畸變系數(shù)，旋轉(zhuǎn)和平移向量。

2.2 目標點識別

水下機器人在靠近回收裝置時，攝像機實時捕捉回收裝置的光源標記點，進行圖像預(yù)處理，包括高斯濾波，去白噪聲。如圖3所示，對目標點的識別也就是獲取光源標記點中心的像素坐標。

圖3 水下機器人攝像機成像

為了提高計算效率，在目標識別時，運用檢測光源輪廓的方法[11]，OpenCV函數(shù)庫提供cv2.findContours()函數(shù)對輪廓進行檢測，返回光源輪廓的像素坐標。由于圖像的旋轉(zhuǎn)不確定性[12]，在實際應(yīng)用中還需對目標點進行排序，以便將像素坐標點和世界坐標點相對應(yīng)。

2.3 攝像機定位

在得到了光源標記點的像素坐標后運用OpenCV中提供的cv2.solvePnPRansac()函數(shù)進行攝像機定位，返回回收裝置平面坐標系到默認潛器攝像機平面坐標系的旋轉(zhuǎn)向量和平移向量，而只有標準的潛器坐標系在標準世界坐標系的姿態(tài)才能直觀反映出潛器的姿態(tài)，故默認潛器坐標系需要左乘一姿態(tài)矩陣轉(zhuǎn)換為標準潛器坐標系，變換過程為：

(1)

2.3.1 根據(jù)平移向量計算攝像機光心位置

通過OpenCV算法函數(shù)和必要的坐標軸轉(zhuǎn)換，得到了攝像機平面坐標系到回收裝置平面坐標系的平移向量，它的物理含義為世界坐標系坐標原點在攝像機坐標系下的坐標，以下公式表示了這種轉(zhuǎn)換關(guān)系：

(2)

由于旋轉(zhuǎn)矩陣各元素兩兩正交，自由度和秩均是3，所以可以得出：

(3)

此時，-RTt表示攝像機坐標系坐標原先在世界坐標系中的坐標，即攝像機的光心位置。

2.3.2 根據(jù)平移向量計算攝像機平面姿態(tài)

由于OpenCV中，返回的外參數(shù)在物理含義上不能直觀地反映姿態(tài)信息，所以先將得到的旋轉(zhuǎn)向量轉(zhuǎn)化為可以直觀表示物體旋轉(zhuǎn)的歐拉角[13]形式。這個過程可以通過羅得里格斯變換[14]獲得。OpenCV里的羅得里格斯函數(shù)解算出來的矩陣是：

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

(4)

式中,旋轉(zhuǎn)矩陣R表征世界坐標系依次沿xw、yw和zw軸逆時針旋轉(zhuǎn)角度θ、φ和φ后與攝像機坐標系姿態(tài)相同。

上述旋轉(zhuǎn)順序下的歐拉角可以直觀地反映出潛器的姿態(tài)，分別為偏航角β、俯仰角α、滾轉(zhuǎn)角γ[15]，即船舶領(lǐng)域中的艏向、縱傾和橫傾[16]。根據(jù)縱傾的定義，潛器前進方向-ocxc軸與世界坐標系水平面-owxwyw平面的夾角即為潛器的縱傾角，攝像機坐標系下前進方向xc軸一點，其在世界坐標系下的坐標為MW，然后投影變換如圖4所示。

圖4 潛器姿態(tài)角求解

結(jié)合旋轉(zhuǎn)矩陣的定義得出潛器的縱傾表達式：

sinα=-MWMW’/OWMW=-R13

(5)

即：

α=-arcsinR13

(6)

同理可以求出艏向和橫傾的表達式：

(7)

此時求出縱傾、艏向和橫傾的物理意義即為回收裝置所在的標準世界坐標系下，潛器攝像機坐標系的姿態(tài)，實際運算時，潛器的攝像機坐標系到潛器的本體坐標系也有一個變換，這里把兩個坐標系等價為一個坐標系。

2.4 攝像機位置和姿態(tài)獲取算法軟件流程

攝像機位姿確定是根據(jù)其捕捉的單幀圖像實時計算獲取的，軟件流程如圖5所示。

圖5 攝像機位姿確定算法流程

3 仿真與結(jié)果分析

3.1 CATIA攝像機拍攝模型

本文借助CATIA三維建模軟件，建立了水下機器人攝像機和回收裝置的拍攝模型，如圖6所示。P1、P2、P3、P4分別為回收裝置進門側(cè)頂點上的光源標記點，V2為攝像機的觀測點，回收裝置的中心點為三維坐標原點。

圖6 攝像機回收裝置模型

利用OpenCV可以對光源標記進行識別定位，分別表示出輪廓、重心和中心位置，重心坐標可以作為光源標記點的像素坐標，中心位置可以作為四個標記點的位置分辨依據(jù)，整個識別過程如圖7所示。

圖7 光源標記點識別定位過程

攝像機的內(nèi)參信息是給定的，本次實驗折算后的攝像機的內(nèi)參信息見表1。

3.2 數(shù)據(jù)獲取與分析

根據(jù)目標點的像素坐標、世界坐標以及相機的內(nèi)參數(shù)，借助OpenCV，計算出50組攝像機在回收裝置坐標系的位置和

表1 攝像機的內(nèi)參數(shù)坐標

表2 攝像機在世界坐標系下的位姿對比信息/(位置/毫米；姿態(tài)/度)

姿態(tài)信息，然后與CATIA真實的位姿對比，得出OpenCV單目視覺定位出的計算位姿與真實位姿信息及誤差，以位置-姿態(tài)的形式給出，位置表示x軸、y軸和z軸的距離信息，姿態(tài)表示縱傾、艏向和橫傾的角度信息，其中，偏差為計算值與理論值之差，偏差率為偏差的模與理論值的比值，選取部分數(shù)據(jù)見表2。

由表2可知，由于潛器攝像機坐標系和回收裝置世界坐標系的設(shè)定，且潛器總是面對著回收裝置進行拍攝，所以潛器前進方向(x軸)的坐標在世界坐標系下均是負值；本文的研究方法在位置和姿態(tài)的確定上均有較高的精度，位置最大誤差率不超過6%，姿態(tài)最大誤差率不超過10%，滿足回收對接設(shè)計要求。

圖8為不同實驗序號下的位置和姿態(tài)誤差對比圖，由圖可知，六自由度位置和姿態(tài)求取中，其誤差隨著潛器與回收裝置

圖8 位姿信息偏差對比示意圖

距離的增大有增大的趨勢，符合機器視覺的光學特性，同時與實際經(jīng)驗分析相吻合。

考慮造成偏差的因素，主要有：第一：光源點像素坐標的獲取受環(huán)境和識別算法等因素的影響存在一定誤差，這會對后面的位置和姿態(tài)計算造成影響；第二：在計算攝像機平面在世界坐標系下的位置和姿態(tài)信息時，不同的求解算法對結(jié)果也會有一定的影響，同時算法本身也有一定的誤差。

5 結(jié)論

本文提出了一種基于OpenCV的單目視覺定位方法,實現(xiàn)了水下機器人自主回收對接時位置和姿態(tài)信息的獲取。并巧妙地借助CATIA軟件對攝像機拍攝模型進行建模，通過數(shù)據(jù)的獲取與分析確定了該方法的可行性。該方法簡單直觀，快速準確地解決了水下機器人自主回收對接時的定位問題，具有較高的工程應(yīng)用價值。

[1] 李 曄, 姜言清, 張國成, 等.考慮幾何約束的AUV回收路徑規(guī)劃[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] 嚴衛(wèi)生,高 智,楊小龍,等.面向AUV自主回收的單目視覺定位算法[J].電子設(shè)計工程,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] 胡釗政, 趙 斌, 李 娜, 等.基于虛擬三面體的攝像機與二維激光測距儀外參數(shù)最小解標定新算法[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] 宋 濤, 褚光宇, 侯培國,等.基于質(zhì)心點優(yōu)化的魚眼攝像機標定[J].光子學報,2016,(5):100-105.

[11] 王 森, 伍 星, 劉 韜, 等.基于反對稱雙正交小波變換的多尺度歸一化分割方法[J].計算機輔助設(shè)計與圖形學學報,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參數(shù)的視覺相對位姿確定方法[J].宇航學報,2010,31(3):753-757.

[16] 施生達.潛艇操縱性[M].北京:國防工業(yè)出版社,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