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

韓 沖，蘇 濤，謝基榕

(中國(guó)船舶科學(xué)研究中心 深海載人裝備國(guó)家重點(diǎn)實(shí)驗(yàn)室，江蘇 無(wú)錫 214082)

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

韓 沖，蘇 濤，謝基榕

(中國(guó)船舶科學(xué)研究中心 深海載人裝備國(guó)家重點(diǎn)實(shí)驗(yàn)室，江蘇 無(wú)錫 214082)

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

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

0 引言

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

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

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

1 問(wèn)題描述

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

圖1 水下機(jī)器人自動(dòng)回收示意圖

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

圖2 攝像機(jī)成像過(guò)程各坐標(biāo)系

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

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

2.1 攝像機(jī)標(biāo)定

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

2.2 目標(biāo)點(diǎn)識(shí)別

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

圖3 水下機(jī)器人攝像機(jī)成像

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

2.3 攝像機(jī)定位

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

(1)

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

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

(2)

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

(3)

此時(shí)，-RTt表示攝像機(jī)坐標(biāo)系坐標(biāo)原先在世界坐標(biāo)系中的坐標(biāo)，即攝像機(jī)的光心位置。

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

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

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

(4)

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

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

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

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

sinα=-MWMW’/OWMW=-R13

(5)

即：

α=-arcsinR13

(6)

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

(7)

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

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

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

圖5 攝像機(jī)位姿確定算法流程

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

3.1 CATIA攝像機(jī)拍攝模型

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

圖6 攝像機(jī)回收裝置模型

利用OpenCV可以對(duì)光源標(biāo)記進(jìn)行識(shí)別定位，分別表示出輪廓、重心和中心位置，重心坐標(biāo)可以作為光源標(biāo)記點(diǎn)的像素坐標(biāo)，中心位置可以作為四個(gè)標(biāo)記點(diǎn)的位置分辨依據(jù)，整個(gè)識(shí)別過(guò)程如圖7所示。

圖7 光源標(biāo)記點(diǎn)識(shí)別定位過(guò)程

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

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

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

表1 攝像機(jī)的內(nèi)參數(shù)坐標(biāo)

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

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

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

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

圖8 位姿信息偏差對(duì)比示意圖

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

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

5 結(jié)論

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

[1] 李 曄, 姜言清, 張國(guó)成, 等.考慮幾何約束的AUV回收路徑規(guī)劃[J].機(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水下對(duì)接的雙目視覺(jué)測(cè)距方法[J].計(jì)算機(jī)測(cè)量與控制,2008,16(10): 1460-1462.

[4] 嚴(yán)衛(wèi)生,高 智,楊小龍,等.面向AUV自主回收的單目視覺(jué)定位算法[J].電子設(shè)計(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ī)與二維激光測(cè)距儀外參數(shù)最小解標(biāo)定新算法[J].自動(dòng)化學(xué)報(bào),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問(wèn)題的線性求解算法[J].軟件學(xué)報(bào),2003,14(3):682-688.

[10] 宋 濤, 褚光宇, 侯培國(guó),等.基于質(zhì)心點(diǎn)優(yōu)化的魚(yú)眼攝像機(jī)標(biāo)定[J].光子學(xué)報(bào),2016,(5):100-105.

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

[16] 施生達(dá).潛艇操縱性[M].北京:國(guó)防工業(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-)，男，江蘇徐州人，碩士，工程師，主要從事圖像識(shí)別、計(jì)算機(jī)視覺(jué)方向的研究。

1671-4598(2017)12-0219-05

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

TP391

A