面向AUV自主回收的單目視覺定位算法

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

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

自主水下航行器在海洋研究、生態(tài)監(jiān)測、軍事偵察等領(lǐng)域有著重要而廣泛的應(yīng)用。受體積和質(zhì)量的限制，AUV攜帶的能源十分有限，對(duì)于長時(shí)間作業(yè)的AUV，必須通過支持平臺(tái)對(duì)AUV進(jìn)行布放和回收，以補(bǔ)充能源、傳遞信息和維護(hù)保障。利用水面船或空中進(jìn)行布放回收，無法保證回收平臺(tái)的隱蔽性，而且易受水面波浪的干擾，在復(fù)雜海況下無法對(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]。韓國的A.A.Kushnerik等人[3]利用視覺輔助方法進(jìn)行了AUV回收的研究。他們根據(jù)AUV的回收結(jié)構(gòu)設(shè)計(jì)了一個(gè)矩形標(biāo)志，用AUV自身安裝的攝像機(jī)對(duì)其成像，然后對(duì)圖像進(jìn)行邊緣檢測和輪廓提取，進(jìn)而估計(jì)出矩形的中心相對(duì)于AUV的位姿信息，引導(dǎo)AUV實(shí)現(xiàn)回收。哈爾濱工程大學(xué)為解決AUV自主回收問題首先在水下回收平臺(tái)上設(shè)置一處平面回收位置,然后控制AUV接近回收位置上部,最后落下并?？吭诨厥掌脚_(tái)上,實(shí)現(xiàn)AUV回收[4-5]。

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

1 問題描述

AUV自主回收過程中要使AUV能準(zhǔn)確并安全的進(jìn)入回收裝置，就要求AUV能對(duì)回收裝置進(jìn)行精確定位，文中主要研究利用攝像機(jī)實(shí)現(xiàn)對(duì)回收裝置精確定位。為描述這一問題，所示建立回收系統(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ī)采集圖像信息，通過圖像處理識(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ì)位置的精確測量。

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ì)稱設(shè)置4個(gè)光學(xué)特征點(diǎn)，設(shè)光學(xué)特征點(diǎn)S1和S2的間距為L，其對(duì)應(yīng)圖像平面坐標(biāo)系下的 像 點(diǎn)分別為 N1(x1，y1)和 N2(x2，y2)，N1、N2間 距 為 r，△x和△y分別為線段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)法來解決AUV發(fā)生偏航或俯仰時(shí)的精確定位問題。需要指出的是攝像機(jī)發(fā)生俯仰時(shí)垂直面內(nèi)的橫向定位不受影響，同理攝像機(jī)發(fā)生偏航時(shí)垂直面內(nèi)的縱向定位計(jì)算不受影像。

2.2.1 旋轉(zhuǎn)定位法

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

在本文研究中由于俯仰軸不通過攝像機(jī)光心，攝像機(jī)隨AUV一起發(fā)生俯仰。如圖2示，攝像機(jī)經(jīng)虛擬旋轉(zhuǎn)扶正時(shí)與理想狀態(tài)之間存在確定關(guān)系。是俯仰軸心與攝像機(jī)光心間的距離，和分別是扶正后光心與發(fā)生俯仰前光心的垂向和縱向距離。因此求解出扶正狀態(tài)下的定位信息，就可以通過確定的轉(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將偏離原來位置到達(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的入射光線所成夾角分別為θ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)垂直面平移定位

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

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

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

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

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

其中，

(2)垂直面平移定位

橫向定位:

其中△x=△ysinΨ。

縱向定位：

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

為測定單目視覺定位方法的準(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米開始每隔1米選取一個(gè)測量點(diǎn)，根據(jù)選定的實(shí)驗(yàn)條件，利用本文的單目定位方法進(jìn)行試驗(yàn)，得到試驗(yàn)結(jié)果如表1所示，δyd,c，δxd,c，δzd,c分別為前向、垂直面內(nèi)橫向和縱向定位誤差。

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

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

4 結(jié)束語

文中提出了一種基于單目視覺的近距定位方法來解決自主水下航行器自主回塢定位問題。該方案中的單目定位算法易于實(shí)現(xiàn)，在自主水下航行器近距回塢和回收中能達(dá)到較高的位置測量精度，具有較好的應(yīng)用價(jià)值，下一步將展開并利用水下攝像機(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ā)展趨勢[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]張治國.基于單目視覺的定位系統(tǒng)研究[D].武漢：華中科技大學(xué)，2009.