面向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.