智能水下机器人自主回收的双目视觉定位

徐硕， 姜言清,3， 李晔,3， 刘善昌， 丁硕硕， 马东， 齐昊东， 张文君

(1.哈尔滨工程大学 水下机器人技术重点实验室，黑龙江 哈尔滨 150001； 2.哈尔滨工程大学 船舶工程学院，黑龙江 哈尔滨 150001； 3.哈尔滨工程大学 三亚南海创新发展基地，海南 三亚 572000)

智能水下机器人(autonomous underwater vehicle，AUV)在海洋勘探和观测中发挥着越来越重要的作用[1-3]。然而，几乎所有的任务AUV都会有水面船只或岸上工作人员参加，这增加了任务的成本和工作周期，且不符合其自主化发展方向。水下对接技术使水下机器人能够自主地被引导到水下基站、给电池充电、下载数据、上传新的任务计划，并安全地停放，直到执行下一个任务[4-6]。AUV对接的导航过程可分为归航和对接2个阶段[6-7]，前一阶段是指水下机器人距离对接船坞10 m以上的情况，后一阶段是指水下机器人与对接站之间的距离小于10 m的情况。超短基线(ultra-short base line，USBL)定位系统通常用于归航的阶段，利用它可以得到AUV水下的真实位置[8]。在对接阶段，因具有更高的定位精度，基于相机的光学引导[9]和电磁引导[10]是更好的选择。电磁引导需要提前布放无线电传感器阵列组成定位网络，AUV将被动地接收无线传感器传来的定位信息，判断自身位置完成对接。由于水对电磁波的衰减作用，往往需要较大功率维持信号的稳定；另一方面，预先布置无线电网络往往增加对接的工作量，导致灵活性下降。光学引导技术因其成本较低、精度和灵活度高等原因，成为AUV对接更合适的引导方法。

伊文康等[11]针对AUV水下光视觉引导对接难点，提出一套完整的自主对接引导方案，并通过水池定点试验和对接实验，以验证该方案的可行性。然而AUV在识别基站8个光源标识时，必须保证AUV与基站保持垂直方向一致，才能保证准确地匹配到光源标识。Lin等[12]在研究AUV对接过程中的实时定位问题上取得了很好的效果。但在检测基站部分，仅通过识别单一光源标识约束，得到基站相对于AUV的大致方向，并不能得出基站的准确位置，这会给AUV的控制带来很大的挑战。朱志宇等[13]提出一种基于立体视觉的定位方法，该方法利用单一光源标识获得基站相对于AUV的三维位置，这也会带来所得结果误差较大的问题。

本文目标基站采用4个光源标识，通过计算所有光源标识位置取均值，得到精确的基站位置；利用自适应图像二值化算法和伪光源剔除算法，保证光源阵列准确的像素坐标；通过定义光源特征和误匹配检测算法，准确地配准立体图像的光源标识。

1 基站双目视觉定位方法

本方法的总流程图如图1所示。双目相机采集水下基站的图像对，利用光源识别算法得到图像对中光源中心的像素坐标。光源匹配算法将双目左图和右图中的光源一一对应。立体测距算法通过三角测量原理得到每个光源在相机坐标系下的三维坐标，将其均值作为基站的坐标。

图1 视觉定位方法的总流程Fig.1 Overall flow chart of visual positioning method

1.1 光源识别

波长为500～600 nm的黄绿光在水中的散射率最小[14]，因此选择波长540 nm的发光二极管(light-emitting diode，LED)。为保证光源识别的成功率，采用4个LED组成正方形灯阵，安装在喇叭口位置，如图2所示。

图2 LED光源的安装示意Fig.2 LED light sources location

1.1.1 自适应二值化

光源标识在图像上的成像有如下特点：1)图像中，光源的像素灰度值明显比周围像素大；2)光源中心的像素为明亮的绿色；3)光源的成像为较小的圆形独立区域。图3为某一次的光源标识阵列立体图像，每张图片分辨率为1 280×720。

图3 光源标识阵列立体图像Fig.3 Light sources in image pair

基于上述特点，采用自适应二值化进行图像处理。具体步骤为：1)灰度化左右相机图像，作为输入；2)设置初始阈值T=250，对灰度值小于阈值的像素值取0，否则取1，得到二值图像；3)对二值图像进行轮廓检测，判断轮廓个数N是否大于4，并且至少4个轮廓边界像素个数大于30；4)若N<4则失败，将当前阈值减小一定步长T′=T-5，重新二值化。图4为阈值T=230时的二值化左相机图像。

图4 自适应二值化后的左图像Fig.4 Left image after adaptive binarization

1.1.2 伪光源剔除

由于光源强光高亮的特点，得到的二值化图像中可以很容易地检测到亮斑。然而，二值图像中可能存在伪光源标识，如图4所示，图像右上角的亮斑被错误地检测到。分析具体原因为：1)周围环境光照较强，导致某些反光较强的物体被检测到；2)周围环境中存在其他光源，干扰了检测结果；3)周围存在物体对光源标识发出的强绿光反射，从而被检测到。

对于上述前2个原因，可以通过降低环境光照强度和移除其他光源来避免。然而对于物体反射光源光照，则无法杜绝，特别是当AUV在接近水面处进行对接时，水面倒影处的伪光源阵列，会严重干扰对接精度，甚至导致对接失败。因此，需要对伪光源进行检测并剔除。

对二值图像的轮廓检测技术已经非常成熟，本文选择最外层边界跟踪法[15]提取光源标识轮廓，可以得到轮廓边缘点{ni}和中心点C。在检测过程中，根据光标识阵列成像轮廓适中的情况，对每个检测到的轮廓边缘像素点个数Ni，必须满足30

数字图像有多种色彩空间，如：RGB色彩空间和HSV色彩空间。RGB色域利用R(红色，[0,255])，G(绿色，[0,255])和B(蓝色，[0,255])3种颜色值的组合还原真实色。然而在光源标识显示的亮绿色范围，RGB值并不是线性变化的，因此很难用阈值范围判断该颜色是否满足条件。HSV色域更符合颜色的线性变化：1)H值表示色调，范围[0°,360°]，其中绿色对应于120°左右；2)S值表示饱和度，即该颜色更接近于光谱色的程度，范围[0,1]，值越大，则颜色越饱和；3)V值表示颜色明亮的程度，即发光体的光亮度，范围[0,1]，值越大，则颜色越亮。

根据HSV色彩空间的特点，可以更容易地检测出光源标识。经过大量实验，设置60

虽然上述方法能检测出许多伪光源标识，并不能做到完全剔除。因此，对包含很少伪光源的集合S={si}，采用K均值聚类，具体步骤为：

1)设置聚类中心。经过上述剔除算法后，S={si}一般不超过8个，因此可以设置类别为：

Num=Size(S)-3

(1)

式中Size(·)表示个数；

2)初始化。随机选取Num个光源，各自为一类，类中心点Cki为光源中心点Ci；

3)计算非该类光源中心点Cj到类中心点Cki的距离，将光源划分到距离最近的类别中；

4)计算每一类别的新的中心点Cki；

5)返回步骤3)，直到类别不再变化。

经过多次随机初始化，选取包含4个光源的类别作为最终结果。图5表示某一次光源剔除最终的聚类结果，其中红色表示正确的光源标识，蓝色表示伪光源。

图5 K均值聚类结果Fig.5 K-means clustering results

1.2 光源特征提取和匹配

得到双目相机左图和右图中各自的光源标识集合后，需要对其进行配准，从而进行立体测距。在视觉同步定位与建图技术领域，通常采用提取图像点特征的方式进行一定规则的匹配[16]。本文采用灰度质心法和拟合椭圆主方向作为光源的特征，并进行光源匹配。

1.2.1 光源灰度特征提取

光源标识在图像中成像是块区域，由像素集合组成，因此可以在块区域内进行特征提取，唯一表征此光源。

如图6所示，由于亮度不均匀，图像块内的像素灰度值各不相同。灰度质心是指以图像块灰度值作为权重的中心。首先，在检测到的光源图像块Opatch中，定义图像块的矩为：

(2)

式中：(x,y)表示图像块任意像素的位置；I(x,y)表示该像素的灰度值。进一步可以得到该像素块的质心：

(3)

图6 图像块灰度图示意Fig.6 Image block grayscale

连接图像块的几何中心O和质心C，得到一个方向向量C，则光源的灰度特征方向为：

θc=arctan(m01/m10)

(4)

1.2.2 光源方向特征提取

检测到光源标识轮廓后，利用最小二乘的椭圆拟合算法[17]，得到光源轮廓近似椭圆的参数。已知平面椭圆的一般方程：

ax2+bxy+cy2+dx+ey+f=0

(5)

其中，A=[a,b,c,d,e,f]T，X=[x2,xy,y2,x,y,1]T，则式(5)变成矩阵形式ATX=0。已知一个椭圆方程有6个自由度，需要至少6个已知点唯一确定一个椭圆，因此拟合椭圆的最优化问题为：

(6)

式中：D是数据样本矩阵，维数为n×6；n表示样本数。由于优化的是椭圆方程，并且参数可以随意缩放，可以简单地添加约束Δ2=4ac-b2=1，并引入拉格朗日因子λ，可以得到以下2个方程：

(7)

式中：C是添加约束后的6×6常数矩阵，除元素项C(1,3)=C(3,1)=2和C(2,2)=-1外，其余均为0。令S=DTD，上述方程可以写成：

(8)

求解方程(8)的非负特征值对应的特征向量，即可得到拟合椭圆的参数。通过将一般方程标准化，即可得到椭圆的主方向θq，图7为解算的光源拟合椭圆及其方向。

图7 拟合椭圆及其主方向Fig.7 Fit ellipse and its main direction

综上，将光源标识质心方向θc和拟合椭圆的主方向θq，共同作为光源的特征。

1.2.3 光源匹配

光源匹配指的是将左右图像中得到的光源标识进行一一对应，以便后续进行解算中心点的3D位置。通过对得到的光源特征，即质心方向θc和拟合椭圆的主方向θq进行比较，满足小于一定阈值时，即认为是同一光源。然而由于传感器自身的原因，使得左右图像中光源的成像形状略有不同，有可能导致匹配不上，虽然实验中情况很少，但不能忽视，本文通过极线搜索进行判别。

如图8所示，π1和π2分别为左右相机的成像平面，光源中心P在π1上的成像点为p1，根据针孔相机模型，当光源中心深度P未知时，其在π2上的成像点应该在极线l2上。因此，选取中心深度P在左相机坐标系下的归一化点P′(x′,y′,1)和较远处任意给定深度值的点P″(x″,y″,z″)，通过双目相机的转移矩阵T=[R|t]和右相机内参Kr，即可得到右相机像素平面下的成像点，从而唯一确定极线l2。当光源匹配后，判断π2上的光源成像点是否在极线l2上，即可排除错误的匹配。

图8 极线搜索示意Fig.8 Schematic diagram of epipolar search

1.3 立体测距

一旦确定立体相机的外参矩阵T=[R|t]，即可通过三角测量计算每个匹配好的光源中心点对在相机坐标系下的三维位置。如图9所示。

图9 三角测量示意Fig.9 Schematic diagram of the triangulation measurements

考虑左右图像π1和π2，以π1建立右手坐标系，则π2的变换矩阵为T=[R|t]。相机光心为O1和O2。在π1中检测到光源中心点p1，对应π2中的光源中心点p2。理论上直线O1p1与O2p2在场景中会相交于一点P，该点即2个光源中心点对应的光源标识在三维空间中的位置。然而由于噪声和检测精度的影响，2条直线往往无法相交。因此，可以通过最小二乘法求解。根据相机位姿变换关系可得：

z1X′1=z2RX′2+t

(9)

(10)

由于噪声和误差的存在，不能直接求解出深度，因此需要构建一个最小二乘求解：

(11)

将求得的z2值代入式(10)中，即可解出光源中心点P在左相机坐标系下的三维位置。对4个光源中心点求平均值，最终得到水下基站的坐标。

2 陆上实验分析与验证

为验证算法的鲁棒性和测量精度，进行陆上的定点测试。测试设备包括一个分辨率为1 280×720的双目相机，树莓派4B(8 GB内存)以及正方形LED光源阵列，如图10所示。本实验选取规格20 m长度的走廊，如图11所示。实验中，选取8个位置对光源标识阵列进行测量，包括识别的准确性和相对距离的精度对比，位置4～20 m任意选取。

图10 实验硬件示意Fig.10 Tested equipment in the experiment

图11 实验选取的走廊Fig.11 The corridor chosen for the experiment

2.1 鲁棒性测试

光源识别是光视觉引导的基础，因此需要对光源识别的鲁棒性进行实验分析。所提出的方法采用自适应二值化、K均值聚类和HSV彩色域校准算法提高光源识别的准确率。利用单一的判别算法对比，验证其鲁棒性。

部分鲁棒性测试4组结果如图12所示，图像分别为自适应二值化算法、K均值聚类算法、HSV彩色域校准和本文光源识别算法的识别结果。可以发现，自适应二值化算法均无法识别出光源；K均值聚类算法虽然识别出光源个数为4，但仅有1组测试准确识别；HSV彩色域校准可以较好地排除非光源颜色光斑的干扰，但不能剔除同种颜色的反射光斑；本文所提出的光源识别算法可以准确识别基站光源，因此对于伪光源干扰有很好的鲁棒性。

图12 鲁棒性测试部分结果Fig.12 Part of robustness test results

2.2 定位精度测试

将算法得到的基站距离与真实的基站距离作对比，进一步验证所提方法的定位精度。利用理论精度为1 cm 的激光测距仪测量相机到基站中心的真实距离。8组测试距离对比结果如表1所示。

表1可以看出，基站与相机的相对距离的误差均在10%以内，平均精度达到5.3%，比较满足AUV近距离对接时的定位精度。可以发现，第5～8组测试的距离误差都偏高，其原因为双目相机测距会随着距离的增大而误差增大，光源成像越小，其不确定性也就越大，从而导致误差增大。此外，第2、3、7组测试的误差也偏大，原因为图片矫正算法为近似真空模型，对图像中心区域的去畸变效果最好，因此当光源成像在图像边缘时，基站位置误差也会偏大。

表1 定位精度对比结果Table 1 Positioning accuracy comparison results

3 结论

1)所提方法中的伪光源剔除算法，能有效地避免误检测的问题，提高光源识别的鲁棒性。

2)所提方法利用了2种光源特征进行匹配，并通过极线搜索来保证匹配结果的准确，利用三角测量得到基站的相对距离，在陆地上20 m 内能达到5.3%的平均精度。该方法可以得到精确的水下基站三维位置。