基于单目视觉的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