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

嚴衛生，高 智 ，楊小龍，王衛國

（西北工業大學 航 海學院，陜西 西 安 7 10072）

自主水下航行器在海洋研究、生態監測、軍事偵察等領域有著重要而廣泛的應用。受體積和質量的限制，AUV攜帶的能源十分有限，對于長時間作業的AUV，必須通過支持平臺對AUV進行布放和回收，以補充能源、傳遞信息和維護保障。利用水面船或空中進行布放回收，無法保證回收平臺的隱蔽性，而且易受水面波浪的干擾，在復雜海況下無法對AUV進行回收。因此，在水下設置回收站，實施AUV的自主回收回收成為目前AUV研究的一個重要方向[1]。

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

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

1 問題描述

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

2 單目幾何定位方法

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

圖1 回收系統坐標系Fig.1 The coordinate system of the recovery system

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

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

2.1.1 前向定位

如圖1，在回收裝置端面OdYdZd對稱設置4個光學特征點，設光學特征點S1和S2的間距為L，其對應圖像平面坐標系下的 像 點分別為 N1(x1，y1)和 N2(x2，y2)，N1、N2間 距 為 r，△x和△y分別為線段N1N2在Xt軸Yt和軸上的投影，f為攝像機鏡頭的焦距。根據幾何光學原理[6]可得到：

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

2.1.2 垂直面平移定位

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

其中zd,c為垂直面內縱向相對位移，xd,c為垂直面內橫向相對位移。為攝像機與回收裝置之間的相對位置，由于攝像機安裝在AUV正前方△y處，所以攝像機坐標系到AUV坐標系的轉換矩陣為因此AUV與回收裝置的相對平移

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

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

2.2.1 旋轉定位法

旋轉定位法就是將發生俯仰的攝像機繞軸虛擬旋轉到無俯仰狀態，分析旋轉前后兩種狀態進行精確定位。

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

圖2 旋轉定位法示意圖Fig.2 The schematic diagram of rotary positioning method

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

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

圖3 旋轉定位法坐標轉換Fig.3 Coordinate conversion of rotary positioning method

下面推導由(x1，y1)坐標系下的像點求解出(x2，y2)虛擬坐標系下的虛擬像點。四個光源為 S1、S2、S3和 S4，它們在(x1，y1)坐標系下的像點坐標分別為為(x1′，y1′)、(x2′，y2′)、(x3′，y3′)、(x4′，y4′)，在(x2，y2)坐標系下的像點坐標分別為（x1″，y1″)、（x2″，y2″)、（x3″，y3″)、（x4″，y4″)。 設發生俯仰后攝像機光軸與光學特征點 S1和 S2的入射光線所成夾角分別為θ1和θ2。由圖的幾何關系知:

則有:

1)前向定位

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

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

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

2)垂直面平移定位

垂直面平移定位通過回收裝置坐標系的原點計算。由于AUV俯仰對垂直面橫向定位沒有影響，因此：

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

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

則俯仰狀態下縱向定位計算公式為：

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

其中，

(2)垂直面平移定位

橫向定位:

其中△x=△ysinΨ。

縱向定位：

3 實驗與分析

為測定單目視覺定位方法的準確性，利用現有的單目定位圖像序列及對應的姿態數據進行了驗證。采集的圖像分辨率為 800×600，焦距 f=8 mm，像元大小為 6.5 μm×6.5 μm。 從距離9米開始每隔1米選取一個測量點，根據選定的實驗條件，利用本文的單目定位方法進行試驗，得到試驗結果如表1所示，δyd,c，δxd,c，δzd,c分別為前向、垂直面內橫向和縱向定位誤差。

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

由表可見，隨著攝像機與回收裝置之間距離的減小，前向、垂直面的測量誤差逐漸減小，其主要原因是距離越遠，誤差受到圖像量化與光學標志點提取的誤差影響越大，從而導致計算出的相對位置信息的誤差變化幅度增大。測量誤差有時并未隨著距離的減小而減小，這是因為在攝像機運動過程中，受光線變化等因素的影響使得光學標志點的提取誤差突然增大。

4 結束語

文中提出了一種基于單目視覺的近距定位方法來解決自主水下航行器自主回塢定位問題。該方案中的單目定位算法易于實現，在自主水下航行器近距回塢和回收中能達到較高的位置測量精度，具有較好的應用價值，下一步將展開并利用水下攝像機做進一步研究和實驗。

[1]高劍，嚴衛生，徐德民，等.自主水下航行器的回塢導引和入塢控制算法[J].西北工業大學，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]王智學.AUV回收時的運動控制方法研究[D].哈爾濱：哈爾濱工程大學.2006.

[5]潘光，黃明明，宋保維，等.AUV回收技術現狀及發展趨勢[J].西北工業大學，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]張治國.基于單目視覺的定位系統研究[D].武漢：華中科技大學，2009.