嚴(yán)衛(wèi)生，高 智 ，楊小龍，王衛(wèi)國(guó)

（西北工業(yè)大學(xué) 航 海學(xué)院，陜西 西 安 7 10072）

自主水下航行器在海洋研究、生態(tài)監(jiān)測(cè)、軍事偵察等領(lǐng)域有著重要而廣泛的應(yīng)用。受體積和質(zhì)量的限制，AUV攜帶的能源十分有限，對(duì)于長(zhǎng)時(shí)間作業(yè)的AUV，必須通過(guò)支持平臺(tái)對(duì)AUV進(jìn)行布放和回收，以補(bǔ)充能源、傳遞信息和維護(hù)保障。利用水面船或空中進(jìn)行布放回收，無(wú)法保證回收平臺(tái)的隱蔽性，而且易受水面波浪的干擾，在復(fù)雜海況下無(wú)法對(duì)AUV進(jìn)行回收。因此，在水下設(shè)置回收站，實(shí)施AUV的自主回收回收成為目前AUV研究的一個(gè)重要方向[1]。

俄羅斯海洋科學(xué)技術(shù)研究所對(duì)回收系統(tǒng)進(jìn)行了研究設(shè)計(jì)，1995年他們利用聲學(xué)和電磁信息進(jìn)行了回收系統(tǒng)的設(shè)計(jì)，隨后有利用聲學(xué)和視頻信息對(duì)回收系統(tǒng)進(jìn)行了進(jìn)一步的設(shè)計(jì)[2]。韓國(guó)的A.A.Kushnerik等人[3]利用視覺(jué)輔助方法進(jìn)行了AUV回收的研究。他們根據(jù)AUV的回收結(jié)構(gòu)設(shè)計(jì)了一個(gè)矩形標(biāo)志，用AUV自身安裝的攝像機(jī)對(duì)其成像，然后對(duì)圖像進(jìn)行邊緣檢測(cè)和輪廓提取，進(jìn)而估計(jì)出矩形的中心相對(duì)于AUV的位姿信息，引導(dǎo)AUV實(shí)現(xiàn)回收。哈爾濱工程大學(xué)為解決AUV自主回收問(wèn)題首先在水下回收平臺(tái)上設(shè)置一處平面回收位置,然后控制AUV接近回收位置上部,最后落下并停靠在回收平臺(tái)上,實(shí)現(xiàn)AUV回收[4-5]。

文中緊密結(jié)合海洋監(jiān)測(cè)和軍事偵察領(lǐng)域中對(duì)AUV水下自主回收的需求，研究了一種比較通用的單目視覺(jué)定位方法,可以快速準(zhǔn)確測(cè)量AUV與回收回收裝置的相對(duì)位置。

1 問(wèn)題描述

AUV自主回收過(guò)程中要使AUV能準(zhǔn)確并安全的進(jìn)入回收裝置，就要求AUV能對(duì)回收裝置進(jìn)行精確定位，文中主要研究利用攝像機(jī)實(shí)現(xiàn)對(duì)回收裝置精確定位。為描述這一問(wèn)題，所示建立回收系統(tǒng)坐標(biāo)系，如圖1所示，其中：AUV坐標(biāo)系為 OaXaYaZa，Oa為 AUV的質(zhì)心，OaYa沿 AUV縱軸向前；攝像機(jī)坐標(biāo)系為OcXcYcZc，Oc為攝像機(jī)的光心，攝像機(jī)沿OaYa安裝在AUV的頭部中心位置；圖像像素坐標(biāo)系為ouv，圖像平面坐標(biāo)系為OtXtYt；回收裝置坐標(biāo)系為OdXdYdZd，Od為回收裝置端面中心。S1、S2、S3、S4為光學(xué)特征點(diǎn)，分別安裝在回收裝置圓形端面的正上、正下、正左和正右，AUV利用頭部的攝像機(jī)采集圖像信息，通過(guò)圖像處理識(shí)別出光學(xué)特征點(diǎn)，利用幾何方法計(jì)算出光學(xué)特征點(diǎn)在攝像機(jī)坐標(biāo)系下的坐標(biāo)，從而利用幾何關(guān)系求出回收裝置端面中心點(diǎn)Od在攝像機(jī)坐標(biāo)系下的坐標(biāo)，并進(jìn)一步將其轉(zhuǎn)化為AUV體坐標(biāo)系下的坐標(biāo)，從而實(shí)現(xiàn)AUV與回收裝置相對(duì)位置的精確測(cè)量。

2 單目幾何定位方法

單目幾何定位是根據(jù)幾何光學(xué)原理對(duì)AUV與回收裝置的相對(duì)位置關(guān)系進(jìn)行計(jì)算。包括攝像機(jī)OXaZa平面與回收裝置端面OXdZd之間的距離D、回收裝置的端面中心Od在攝像機(jī)坐標(biāo)系下的坐標(biāo)為即回收裝置相對(duì)于AUV的垂向、縱向和橫向平移偏差。

圖1 回收系統(tǒng)坐標(biāo)系Fig.1 The coordinate system of the recovery system

2.1 理想情況下單目定位方法

理想情況下AUV與回收裝置端面垂直，即攝像機(jī)成像平面與回收裝置端面平行。

2.1.1 前向定位

如圖1，在回收裝置端面OdYdZd對(duì)稱(chēng)設(shè)置4個(gè)光學(xué)特征點(diǎn)，設(shè)光學(xué)特征點(diǎn)S1和S2的間距為L(zhǎng)，其對(duì)應(yīng)圖像平面坐標(biāo)系下的 像 點(diǎn)分別為 N1(x1，y1)和 N2(x2，y2)，N1、N2間 距 為 r，△x和△y分別為線(xiàn)段N1N2在Xt軸Yt和軸上的投影，f為攝像機(jī)鏡頭的焦距。根據(jù)幾何光學(xué)原理[6]可得到：

其中，△x=x1-x2，△y=y1-y2。

2.1.2 垂直面平移定位

由于兩個(gè)光學(xué)特征點(diǎn)安裝在回收裝置的圓形端面的正上方和正下方，所以回收裝置端面的中心Od在圖像平面坐標(biāo)系下的坐標(biāo)為(xd，yd)，其中在攝像機(jī)坐標(biāo)系下的坐標(biāo)為其中 yd,c=D，則垂直面相對(duì)平移為:

其中zd,c為垂直面內(nèi)縱向相對(duì)位移，xd,c為垂直面內(nèi)橫向相對(duì)位移。為攝像機(jī)與回收裝置之間的相對(duì)位置，由于攝像機(jī)安裝在AUV正前方△y處，所以攝像機(jī)坐標(biāo)系到AUV坐標(biāo)系的轉(zhuǎn)換矩陣為因此AUV與回收裝置的相對(duì)平移

2.2 有俯仰(偏航)時(shí)單目定位方法

當(dāng)AUV發(fā)生橫滾時(shí)，攝像機(jī)平面與回收裝置端面保持平行，此時(shí)前向定位與垂直面平移定位方法與理想情況下相同；當(dāng)AUV發(fā)生偏航或者俯仰時(shí)，攝像機(jī)平面與回收裝置端面也不平行，繼續(xù)使用上述公式進(jìn)行定位會(huì)造成誤差。為此設(shè)計(jì)攝像機(jī)虛擬旋轉(zhuǎn)法來(lái)解決AUV發(fā)生偏航或俯仰時(shí)的精確定位問(wèn)題。需要指出的是攝像機(jī)發(fā)生俯仰時(shí)垂直面內(nèi)的橫向定位不受影響，同理攝像機(jī)發(fā)生偏航時(shí)垂直面內(nèi)的縱向定位計(jì)算不受影像。

2.2.1 旋轉(zhuǎn)定位法

旋轉(zhuǎn)定位法就是將發(fā)生俯仰的攝像機(jī)繞軸虛擬旋轉(zhuǎn)到無(wú)俯仰狀態(tài)，分析旋轉(zhuǎn)前后兩種狀態(tài)進(jìn)行精確定位。

在本文研究中由于俯仰軸不通過(guò)攝像機(jī)光心，攝像機(jī)隨AUV一起發(fā)生俯仰。如圖2示，攝像機(jī)經(jīng)虛擬旋轉(zhuǎn)扶正時(shí)與理想狀態(tài)之間存在確定關(guān)系。是俯仰軸心與攝像機(jī)光心間的距離，和分別是扶正后光心與發(fā)生俯仰前光心的垂向和縱向距離。因此求解出扶正狀態(tài)下的定位信息，就可以通過(guò)確定的轉(zhuǎn)換關(guān)系得到發(fā)生俯仰前狀態(tài)下的定位信息。下面將具體推導(dǎo)從實(shí)際觀察的圖像平面向虛擬成像平面的轉(zhuǎn)換方法，從而推導(dǎo)出虛擬旋轉(zhuǎn)扶正時(shí)的定位表達(dá)式，進(jìn)一步求出有俯仰時(shí)的定位表達(dá)式。

圖2 旋轉(zhuǎn)定位法示意圖Fig.2 The schematic diagram of rotary positioning method

2.2.2 有俯仰(偏航)時(shí)單目定位方法

由于與AUV剛性連接，所以當(dāng)AUV發(fā)生θ角俯仰時(shí)，攝像機(jī)也會(huì)發(fā)生相同角度的俯仰，這樣攝像機(jī)的光心O將偏離原來(lái)位置到達(dá)O′。以θ＞0時(shí)為例，如圖3所示，設(shè)發(fā)生俯仰后攝像機(jī)的圖像平面坐標(biāo)系為(x1，y1)，以O(shè)′為中心將攝像機(jī)扶正，即作-θ角旋轉(zhuǎn)，得到新攝像機(jī)位置下的虛擬圖像平面坐標(biāo)系(x2，y2)。光學(xué)特征點(diǎn)在(x1，y1)坐標(biāo)系中所成像點(diǎn)坐標(biāo)為（x′，y′），在(x2，y2)下的 虛擬像點(diǎn)坐標(biāo)為(x″，y″)。

圖3 旋轉(zhuǎn)定位法坐標(biāo)轉(zhuǎn)換Fig.3 Coordinate conversion of rotary positioning method

下面推導(dǎo)由(x1，y1)坐標(biāo)系下的像點(diǎn)求解出(x2，y2)虛擬坐標(biāo)系下的虛擬像點(diǎn)。四個(gè)光源為 S1、S2、S3和 S4，它們?cè)?x1，y1)坐標(biāo)系下的像點(diǎn)坐標(biāo)分別為為(x1′，y1′)、(x2′，y2′)、(x3′，y3′)、(x4′，y4′)，在(x2，y2)坐標(biāo)系下的像點(diǎn)坐標(biāo)分別為（x1″，y1″)、（x2″，y2″)、（x3″，y3″)、（x4″，y4″)。 設(shè)發(fā)生俯仰后攝像機(jī)光軸與光學(xué)特征點(diǎn) S1和 S2的入射光線(xiàn)所成夾角分別為θ1和θ2。由圖的幾何關(guān)系知:

則有:

1)前向定位

由(9)、(10)可得到△y″=y2″-y1″，虛擬旋轉(zhuǎn)后兩像點(diǎn)垂直面內(nèi)橫向距離不受影響，仍然為:

則虛擬位置下的前向距離為：

由圖 2 可知，前向距離 D=D″-△D(11)，其中△x″=x2″-x1″，△D=△y(1-cosθ)。

2)垂直面平移定位

垂直面平移定位通過(guò)回收裝置坐標(biāo)系的原點(diǎn)計(jì)算。由于AUV俯仰對(duì)垂直面橫向定位沒(méi)有影響，因此：

虛擬位置下垂直面縱向定位可由下式得到

虛擬位置攝像機(jī)位置與無(wú)俯仰攝像機(jī)位置之間的縱向偏差為：

則俯仰狀態(tài)下縱向定位計(jì)算公式為：

采用虛擬旋轉(zhuǎn)法同理可得到發(fā)生偏航角時(shí)的單目定位計(jì)算式如下：(1)前向距離：

其中，

(2)垂直面平移定位

橫向定位:

其中△x=△ysinΨ。

縱向定位：

3 實(shí)驗(yàn)與分析

為測(cè)定單目視覺(jué)定位方法的準(zhǔn)確性，利用現(xiàn)有的單目定位圖像序列及對(duì)應(yīng)的姿態(tài)數(shù)據(jù)進(jìn)行了驗(yàn)證。采集的圖像分辨率為 800×600，焦距 f=8 mm，像元大小為 6.5 μm×6.5 μm。 從距離9米開(kāi)始每隔1米選取一個(gè)測(cè)量點(diǎn)，根據(jù)選定的實(shí)驗(yàn)條件，利用本文的單目定位方法進(jìn)行試驗(yàn)，得到試驗(yàn)結(jié)果如表1所示，δyd,c，δxd,c，δzd,c分別為前向、垂直面內(nèi)橫向和縱向定位誤差。

表1 單目視覺(jué)定位試驗(yàn)誤差表Tab.1 Error table of monocular-vision positioning experiment

由表可見(jiàn)，隨著攝像機(jī)與回收裝置之間距離的減小，前向、垂直面的測(cè)量誤差逐漸減小，其主要原因是距離越遠(yuǎn)，誤差受到圖像量化與光學(xué)標(biāo)志點(diǎn)提取的誤差影響越大，從而導(dǎo)致計(jì)算出的相對(duì)位置信息的誤差變化幅度增大。測(cè)量誤差有時(shí)并未隨著距離的減小而減小，這是因?yàn)樵跀z像機(jī)運(yùn)動(dòng)過(guò)程中，受光線(xiàn)變化等因素的影響使得光學(xué)標(biāo)志點(diǎn)的提取誤差突然增大。

4 結(jié)束語(yǔ)

文中提出了一種基于單目視覺(jué)的近距定位方法來(lái)解決自主水下航行器自主回塢定位問(wèn)題。該方案中的單目定位算法易于實(shí)現(xiàn)，在自主水下航行器近距回塢和回收中能達(dá)到較高的位置測(cè)量精度，具有較好的應(yīng)用價(jià)值，下一步將展開(kāi)并利用水下攝像機(jī)做進(jìn)一步研究和實(shí)驗(yàn)。

[1]高劍，嚴(yán)衛(wèi)生，徐德民，等.自主水下航行器的回塢導(dǎo)引和入塢控制算法[J].西北工業(yè)大學(xué)，2012,48(3)：7-9.GAO Jian,YAN Wei-sheng,XU Demin,et al.Homing guidance and docking control algorithm for autonomous underwater vehicles[J].Computer Engineering and Applications,2012,48(3):7-9.

[2]Scientific report on the theme《Far and near docking methods for AUV》(in russian)[R].,IMTP FEB RAS,1995.

[3]Kushnerik A A,VorotsovA V,Scherbatyuk A Ph.Small AUV docking algorithms near dock unit based on visual data[C]//Biloxi-Marine Technology for Our Future:Global and Local Challenges.Biloxi,2009:1-6.

[4]王智學(xué).AUV回收時(shí)的運(yùn)動(dòng)控制方法研究[D].哈爾濱：哈爾濱工程大學(xué).2006.

[5]潘光，黃明明，宋保維，等.AUV回收技術(shù)現(xiàn)狀及發(fā)展趨勢(shì)[J].西北工業(yè)大學(xué)，2008,16(6):10-14.PAN Guang,HUANG Ming-ming,SONG Bao-wei,et al.Current situation and development trend of AUV recovery technology[J].Torpedo Technology,2008,16(6):10-14.

[6]張治國(guó).基于單目視覺(jué)的定位系統(tǒng)研究[D].武漢：華中科技大學(xué)，2009.