基于單目視覺的AUV水下定位方法

蔡迎波，李德彪

（中國人民解放軍92941部隊95分隊，遼寧 葫蘆島 125001）

基于單目視覺的AUV水下定位方法

蔡迎波，李德彪

（中國人民解放軍92941部隊95分隊，遼寧 葫蘆島 125001）

為解決傳統水下定位傳感器在定位方面的不足，提出一種基于目標光源的單目視覺四自由度定位方法，推導了四自由度定位算法原理，提出了深度定位、水平定位和艏向定位方法。設計了作為目標的共線排列定位光源系統。靜態和動態試驗證明所提出的單目視覺定位方法原理正確，目標光源系統合理可行，可以在深度為4～0.5 m范圍內進行穩定的四自由度定位。經過校正，與SBL和測深儀的相對精度達到10 cm以內。實驗說明上述方法對AUV執行近距離使命任務具有實際意義。

單目視覺；水下視覺；視覺定位；AUV

定位是自動水下機器人（Autonomous Underwater Vehicle, AUV）應該具備的基本功能之一。傳統水下定位傳感器多使用聲吶[1-2]，包括長基線（LBL）、短基線（SBL）、超短基線（USBL）及測深儀等。聲吶在大范圍避障中具有良好性能，但在小范圍時精度較差，數據更新率低，易受水面、水底或其他聲反射源干擾，在淺水、中等深度水域中可能有多徑傳播帶來的模糊，存在最小測量距離的限制[2,3]等。視覺傳感器具有分辨率高和在近距離幾乎不存在盲區的優點，在小范圍定位應用中有很大發展前途。

法國、英國和挪威聯合研制的ALIVE AUV利用視覺確定與一個帶有已知標志的目標的相對位姿。系統建有目標的3維CAD模型，把提取的圖像特征與模型比較得到初始位姿估計值，再通過迭代法得到最終的位姿估計值[4]。澳大利亞CSIRO ICT使用單目攝像機來獲取一種特殊目標的位姿和深度。該特殊目標是改進的自相似標記（Self Similar Landmark, SSL），它易于在水下被定位并且具有旋轉不變性[5]。韓國KRISO-KORDI的IsiMI AUV在目標圓周上布放5個圓形光源，通過一系列的圖像處理獲得對接目標的深度、姿態以及方向信息[6]。

由于光線在水下迅速衰減，針對普通景物特征的定位方法作用距離短，而且當環境變化，特別是水下透光性變差時，方法性能會迅速惡化甚至失效。本文提出一種基于目標光源的單目視覺四自由度定位方法，采用光源系統作為目標可以抵抗環境的干擾，并在一定程度上擴大了方法的作用距離，理論上僅需要兩個光源就可以進行四自由度定位。此外，由于在水下不方便進行實際測量，視覺定位的精度很難描述與衡量。眾所周知，SBL和測深儀已經是使用較成熟的海洋定位傳感器，因此本文使用SBL和測深儀的測量值作為參考對視覺定位精度進行描述。

1 基于目標光源的單目視覺4DOF定位原理

這里的4DOF特指AUV的橫向、縱向、艏向和垂向（單目攝像機位于AUV腹部正中、下視，目標位于AUV下方）。一般來說，單目視覺是無法進行距離測量的。但當存在關于環境的先驗信息時，單目測量就成為可能。在水下，與雙目乃至多目相比，單目視覺不僅構造簡潔，安裝簡便，而且更容易獲得較高精度。此外，水下成像存在嚴重的衰減，當距離增加時，圖像銳度和亮度會急劇下降。基于上述考慮提出了基于目標光源系統的單目視覺四自由度定位方法。單目攝像機觀察兩個已知模式的目標光源，從而確定自身相對于目標的相對位姿，進而達到AUV相對目標定位的目的。

1.1 深度定位

單目攝像機固定于AUV腹部正中，垂直下視。單目定位需要已知目標信息。理想情況下僅采用兩盞間距已知、可互相區別的目標光源即可進行4自由度定位。采用針孔攝像機模型，下面將由幾何光學推導定位原理公式。

如圖1所示，首先建立三個坐標系：Camera坐標系(Xc,Yc,Zc)，原點O在鏡頭光心；圖像坐標系（U, V），建立在圖像平面內，原點(U0,V0)為Camera坐標系原點O在其上的投影；世界坐標系(Xw,Yw,Zw)，由目標光源系統決定。三個坐標系可以通過平移互相轉換，原點位于同一垂直線上[7]。

垂向深度為攝像機光心與目標光源所在水平面之間的距離，圖1中H是光心O距目標平面1的垂向深度。圖1中I0和I1為兩盞目標光源，D為已知的兩燈間距， f為鏡頭焦距，Δu為兩燈像點在圖像平面上的距離。根據幾何光學原理有式(1)成立，從而可以求得垂向深度H。

圖1 坐標系及定位原理圖Fig.1 Coordination systems and positioning diagram

1.2 水平定位

水平定位是指在水平面內目標參考點相對于攝像機光心的相對平移量。假設光源I0為目標參考點，對應像點在圖像坐標系的坐標為(u0,v0)。設I0相對于Camera坐標系的坐標為(,,)，易知=H，則水平平移量可表示為

1.3 艏向定位

艏向定位是指攝像機坐標系的x軸正向，ˉ即ˉˉ→AUV坐標系的縱軸正向，與兩目標光源所成直線之間的夾角。設兩光源I0和I1的像點在圖像坐標系的坐標為(u0,v0)，(u1,v1)，則定義相對艏向角ψ為

在一些特殊情況：

2 目標光源系統設計

光源系統的設計非常關鍵，因為光源個數、光源間距以及光源亮度、形狀都會對定位效果產生很大影響。以高度定位為例，假設可以準確地提取圖像特征，則最小可分辨的高度變化，即高度分辨率，就是恰能引起像點發生一個像素大小變化的那些高度變化。如圖1所示，Sx是像素尺寸，則高度測量的理論分辨率可表達為

可知光源間距D越大，理論上高度分辨率越高，且此種影響隨高度增加而加劇。設Sx=8.05μm ，f=3.8mm，且保持深度H為4 m不變，則當D=0.5 m時，測深精度為0.13 m，當D=2 m時，測深精度為0.03 m。從精度角度看，D愈大愈好。

但光源間距越大，單目定位的盲區也越大。假設攝像機水平和垂直視角均為α，則有效定位區域為能夠同時看到兩盞光源的區域，該區域為在兩光源中點上方的一個近似倒圓錐體。在該近似圓錐體下方是定位的盲區，此盲區的高度可表示為(D/2)/tan(α/2)，即D愈大盲區就愈高。因此從有效區域看，D愈小愈好。

為平衡近距離盲區與遠距離精度，設計如圖2所示的光源系統：所有光源共線排列，中心的光源標志目標參考點，以形狀區別于其余光源；中心光源兩邊對稱排列若干對形狀相同光源；所有相鄰光源間距均相等；所有光源均采用散光LED，以保證光強各向均勻。在遠距離時，攝像機可看到的光源數目多，在近處時看到的光源數目少，算法自動選擇可看到的最遠光源對，以保證盡量高的定位精度，同時定位盲區由相鄰光源間距決定。

圖2 目標光源系統示意圖Fig.2 Light source system of target

3 系統實現及實驗精度分析

普通攝像機要在水下使用必須加裝防水窗，但防水窗會帶來平面屈光效應[8]，增加了機器視覺處理的復雜性。英國Tritech公司的水下攝像機采用了與水折射率近似相同的防水窗材料，大大減小了屈光效應，適用于水下機器視覺研究。選用了Tritech的水下低照度黑白攝像機Tornado，其焦距為3.8 mm，水平和垂直視角分別為82°和68.6°，分辨率為768×494。

實驗在50m×30m×10m的水池中進行，目標光源系統置于水池底部。由于水下實測難度大，以用法較成熟的SBL和測深儀的測量值為基準來描述所提單目定位的精度。實驗中單目攝像機與SBL及高度計均安裝于同一統一框架中。在視覺與SBL同時生效的高度范圍即4～2.5 m進行定位實驗（SBL作用距離不能太近）。圖4的兩條曲線分別為x方向和y方向視覺與SBL測量值之間的偏差，可見在兩方向上，兩測量值偏差都近似為一個定值±0.4 m。這一方面說明視覺定位具有穩定性，另一方面說明視覺和SBL之間存在穩定誤差，可以認為此誤差是由系統誤差造成。因此對視覺定位結果進行校正：

圖3 定位算法框圖Fig.3 Block diagram of the positioning algorithm

圖4 視覺與SBL的水平定位偏差曲線Fig.4 Horizontal positioning differences between SBL and vision

圖5為深度在0.8～4 m范圍內測深儀與視覺測量值的偏差情況，偏差變化范圍在0.1 m以內，且隨深度增加呈近似線性增加。對偏差e作線性擬合：

式中，halt為測深儀測量值。據此對視覺測量的深度進行校正：

圖5 視覺與測深儀的深度定位偏差曲線Fig.5 Height positioning difference between altimeter and vision

4 動態定位試驗與性能分析

仍將目標光源系統置于水池底部，用繩索懸吊單目攝像機的固定框架，從光源上方約3 m處開始分段緩慢下放，直至約0.5 m時止。這是一個動態連續過程，圖6給出這一過程的定位曲線圖。圖6(a)的高度曲線呈臺階式降落，反映了分段緩慢下放的過程，除個別點有毛刺外曲線連續光滑，說明方法正確反映了高度變化；圖6(b)(c)(d)曲線基本上都呈現連續的、較光滑的波動，說明整體上方法是穩定的，并且能反映出攝像機在人工下放過程中的輕微緩慢晃動。在0～200點的開始時段以及800點以后尾部時段，圖6(a)(b)(c)(d)曲線均出現較頻繁的毛刺，這可能由于開始和結束時攝像機的較明顯晃動引起，可以通過濾波將這些毛刺去除。這些曲線說明方法在有效范圍內具有可用的實時性和穩定性。

圖6 動態定位曲線圖Fig.6 Dynamic positioning curves: (a) heave; (b) surge; (c) sway; (d) yaw

5 結 論

靜態和動態試驗證明所提出的單目視覺定位方法原理正確，目標光源系統合理可行，可以在深度為4～ 0.5 m范圍內進行穩定的四自由度定位。經過校正，與SBL和高度計的相對精度達到10 cm以內。該方法對AUV執行近距離使命任務具有實際意義。目前艏向角精度還未經驗證，實驗方法還需要改進。載體AUV發生橫搖或縱搖時對方法產生的影響是下一步研究的目標。

（References）：

[1] Horgan J, Toal D. Review of machine vision applications in unmanned underwater vehicles[C]//Proceedings of 9th International Conference on Control, Automation, Robotics and Vision. Singapore, 2006: 1-6.

[2] Podder T, Sibenac M, Bellingham J, et al. AUV docking system for sustainable science missions[C]//Proc. of IEEE International Conference on Robotics & Automation. 2004: 4478-4485.

[3] Park J Y, Jun B H, Lee P M, et al. Experiments on vision guided docking of an autonomous underwater vehicle using one camera[J]. Ocean Engineering, 2009, 36(1): 48-61.

[4] 李佩娟, 徐曉蘇, 張濤, 等. 信息融合技術在水下組合導航系統中的應用[J]. 中國慣性技術學報, 2009, 17(3): 344-349. Li Pei-juan, Xu Xiao-su, Zhang Tao, et al. Application of information fusion to integrated navigation system of underwater vehicle[J]. Journal of Chinese Inertial Technology, 2009, 17(3): 344-349.

[5] Feng Guo-hu, Wu Wen-qi, Cao Ju-liang, et al. Algorithm for monocular visual Odometry/SINS integrated navigation[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 302-306.

[6] 張紅良, 郭宇鵬, 李壯, 等. 一種視覺輔助的慣性導航系統動基座初始對準方法[J]. 中國慣性技術學報, 2014, 22(4): 469-473. Zhang Hong-liang, Guo Peng-yu, Li Zhuang, et al. Vision aided alignment method for inertial navigation system on moving base[J]. Journal of Chinese Inertial Technology, 2014, 22(4): 469-473.

AUV underwater positioning method based on monocular-vision

CAI Ying-bo, LI De-biao
(Element 95, Unit 92941 of PLA, Huludao 125001, China)

To overcome the performance deficiency of conventional underwater positioning sensor in positioning, a 4DOF positioning approach using monocular vision based on target light-source was put forward. The principle of 4DOF positioning calculation was derived, and the methods of depth location, horizontal location and heading location were presented. The location light-source system with collinear permutation was designed to be used as target light. The static and dynamic tests verify that the principle of the proposed method is correct, and the target light-source positioning system based on monocularvision is reasonable and feasible, which can implement stable 4DOF-positioning within depth 4-0.5 m. The relative accuracy related to SBL and altimeter is within 10 cm after correction. Test results show that the proposed method have practical value for the AUV undertaking short-distance mission.

monocular vision; underwater vision; visual positioning; autonomous underwater vehicle

TP24

B

1005-6734(2015)04-0489-04

10.13695/j.cnki.12-1222/o3.2015.04.013

2015-04-07；

2015-07-28

蔡迎波（1967—），男，高級工程師，主要研究方向為慣性導航。E-mail：lidbcn@sina.com