蔡迎波，李德彪

（中國(guó)人民解放軍92941部隊(duì)95分隊(duì)，遼寧 葫蘆島 125001）

基于單目視覺(jué)的AUV水下定位方法

蔡迎波，李德彪

（中國(guó)人民解放軍92941部隊(duì)95分隊(duì)，遼寧 葫蘆島 125001）

為解決傳統(tǒng)水下定位傳感器在定位方面的不足，提出一種基于目標(biāo)光源的單目視覺(jué)四自由度定位方法，推導(dǎo)了四自由度定位算法原理，提出了深度定位、水平定位和艏向定位方法。設(shè)計(jì)了作為目標(biāo)的共線排列定位光源系統(tǒng)。靜態(tài)和動(dòng)態(tài)試驗(yàn)證明所提出的單目視覺(jué)定位方法原理正確，目標(biāo)光源系統(tǒng)合理可行，可以在深度為4～0.5 m范圍內(nèi)進(jìn)行穩(wěn)定的四自由度定位。經(jīng)過(guò)校正，與SBL和測(cè)深儀的相對(duì)精度達(dá)到10 cm以內(nèi)。實(shí)驗(yàn)說(shuō)明上述方法對(duì)AUV執(zhí)行近距離使命任務(wù)具有實(shí)際意義。

單目視覺(jué)；水下視覺(jué)；視覺(jué)定位；AUV

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

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

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

1 基于目標(biāo)光源的單目視覺(jué)4DOF定位原理

這里的4DOF特指AUV的橫向、縱向、艏向和垂向（單目攝像機(jī)位于AUV腹部正中、下視，目標(biāo)位于AUV下方）。一般來(lái)說(shuō)，單目視覺(jué)是無(wú)法進(jìn)行距離測(cè)量的。但當(dāng)存在關(guān)于環(huán)境的先驗(yàn)信息時(shí)，單目測(cè)量就成為可能。在水下，與雙目乃至多目相比，單目視覺(jué)不僅構(gòu)造簡(jiǎn)潔，安裝簡(jiǎn)便，而且更容易獲得較高精度。此外，水下成像存在嚴(yán)重的衰減，當(dāng)距離增加時(shí)，圖像銳度和亮度會(huì)急劇下降?；谏鲜隹紤]提出了基于目標(biāo)光源系統(tǒng)的單目視覺(jué)四自由度定位方法。單目攝像機(jī)觀察兩個(gè)已知模式的目標(biāo)光源，從而確定自身相對(duì)于目標(biāo)的相對(duì)位姿，進(jìn)而達(dá)到AUV相對(duì)目標(biāo)定位的目的。

1.1 深度定位

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

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

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

圖1 坐標(biāo)系及定位原理圖Fig.1 Coordination systems and positioning diagram

1.2 水平定位

水平定位是指在水平面內(nèi)目標(biāo)參考點(diǎn)相對(duì)于攝像機(jī)光心的相對(duì)平移量。假設(shè)光源I0為目標(biāo)參考點(diǎn)，對(duì)應(yīng)像點(diǎn)在圖像坐標(biāo)系的坐標(biāo)為(u0,v0)。設(shè)I0相對(duì)于Camera坐標(biāo)系的坐標(biāo)為(,,)，易知=H，則水平平移量可表示為

1.3 艏向定位

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

在一些特殊情況：

2 目標(biāo)光源系統(tǒng)設(shè)計(jì)

光源系統(tǒng)的設(shè)計(jì)非常關(guān)鍵，因?yàn)楣庠磦€(gè)數(shù)、光源間距以及光源亮度、形狀都會(huì)對(duì)定位效果產(chǎn)生很大影響。以高度定位為例，假設(shè)可以準(zhǔn)確地提取圖像特征，則最小可分辨的高度變化，即高度分辨率，就是恰能引起像點(diǎn)發(fā)生一個(gè)像素大小變化的那些高度變化。如圖1所示，Sx是像素尺寸，則高度測(cè)量的理論分辨率可表達(dá)為

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

但光源間距越大，單目定位的盲區(qū)也越大。假設(shè)攝像機(jī)水平和垂直視角均為α，則有效定位區(qū)域?yàn)槟軌蛲瑫r(shí)看到兩盞光源的區(qū)域，該區(qū)域?yàn)樵趦晒庠粗悬c(diǎn)上方的一個(gè)近似倒圓錐體。在該近似圓錐體下方是定位的盲區(qū)，此盲區(qū)的高度可表示為(D/2)/tan(α/2)，即D愈大盲區(qū)就愈高。因此從有效區(qū)域看，D愈小愈好。

為平衡近距離盲區(qū)與遠(yuǎn)距離精度，設(shè)計(jì)如圖2所示的光源系統(tǒng)：所有光源共線排列，中心的光源標(biāo)志目標(biāo)參考點(diǎn)，以形狀區(qū)別于其余光源；中心光源兩邊對(duì)稱排列若干對(duì)形狀相同光源；所有相鄰光源間距均相等；所有光源均采用散光LED，以保證光強(qiáng)各向均勻。在遠(yuǎn)距離時(shí)，攝像機(jī)可看到的光源數(shù)目多，在近處時(shí)看到的光源數(shù)目少，算法自動(dòng)選擇可看到的最遠(yuǎn)光源對(duì)，以保證盡量高的定位精度，同時(shí)定位盲區(qū)由相鄰光源間距決定。

圖2 目標(biāo)光源系統(tǒng)示意圖Fig.2 Light source system of target

3 系統(tǒng)實(shí)現(xiàn)及實(shí)驗(yàn)精度分析

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

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

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

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

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

式中，halt為測(cè)深儀測(cè)量值。據(jù)此對(duì)視覺(jué)測(cè)量的深度進(jìn)行校正：

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

4 動(dòng)態(tài)定位試驗(yàn)與性能分析

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

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

5 結(jié) 論

靜態(tài)和動(dòng)態(tài)試驗(yàn)證明所提出的單目視覺(jué)定位方法原理正確，目標(biāo)光源系統(tǒng)合理可行，可以在深度為4～ 0.5 m范圍內(nèi)進(jìn)行穩(wěn)定的四自由度定位。經(jīng)過(guò)校正，與SBL和高度計(jì)的相對(duì)精度達(dá)到10 cm以內(nèi)。該方法對(duì)AUV執(zhí)行近距離使命任務(wù)具有實(shí)際意義。目前艏向角精度還未經(jīng)驗(yàn)證，實(shí)驗(yàn)方法還需要改進(jìn)。載體AUV發(fā)生橫搖或縱搖時(shí)對(duì)方法產(chǎn)生的影響是下一步研究的目標(biāo)。

（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] 李佩娟, 徐曉蘇, 張濤, 等. 信息融合技術(shù)在水下組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 張紅良, 郭宇鵬, 李壯, 等. 一種視覺(jué)輔助的慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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—），男，高級(jí)工程師，主要研究方向?yàn)閼T性導(dǎo)航。E-mail：lidbcn@sina.com