UUV水下回收中的视觉和短基线定位融合

黎南，湛鑫，陈涛，严浙平

(1.海军驻大连地区军代表室，辽宁 大连 116021;2.中国船舶重工集团公司 第703研究所，黑龙江 哈尔滨 150036;3.哈尔滨工程大学自动化学院，黑龙江 哈尔滨 150001)

有效地布放回收是UUV(unmanned underwater vehicle)能否广泛应用的一个关键因素.相对于布放来说，UUV的回收难度很大.因此，世界上UUV领域的研究人员也发展出了各式各样的回收方式，而且均以实现UUV的自动回收为最终目标［1］.本文研究了一种利用水下坞舱自动回收UUV的方式.在这种方式中，UUV需要以一定的定位导引方法识别UUV与坞舱的相对方向和相对位置，从而控制自己精确地进入坞舱之中.对于坞舱水下回收UUV中的定位导引系统，采用了适用远距离定位的短基线定位系统和适用于近端精确定位的视觉导引定位系统.由于2套定位系统在不同作用范围下的精度和可靠性不同，为了UUV能简单、高效、安全地实现与坞舱对接回收，本文重点研究了短基线和视觉在UUV水下回收中的定位融合方法，并通过水池试验验证了所提方法的有效性.

1 UUV水下回收定位系统及其原理

UUV的回收过程是一种空间立体运动，主要需要控制4个自由度:纵向、横向、垂向和艏向.当UUV开始进入水下对接回收任务后，其回收方式类似于直升机降落的坐落坞舱式回收，如图1所示.UUV从对接起始位置水平直航到回收坞舱的正上方，粗略调整姿态和艏向，然后垂直降落到距坞舱较近高度的正上方，接下来精确调整UUV的姿态和艏向，缓慢降落至回收坞舱上.UUV距离坞舱较远时视觉定位精度差，SBL定位精度高;距离较近时，SBL声定位将会出现盲区，而视觉定位的精度高［2］;距离中等时，2种定位信号均有效.

图1 UUV水下回收方式Fig.1 UUV underwater docking mode

1.1 短基线定位系统及其原理

图2是短基线定位系统的简图.声呐基阵由安装在 UUV 底部的4个换能器 T0、T1、T2、T3构成，信标F置于坞舱上.

图2 UUV水下回收中的短基线定位系统Fig.2 SBL position system for UUV underwater docking

声呐基阵的4个换能器安装在一个平面上，基阵的几何中心为UUV的几何中心，定义为短基线坐标系的原点，同时也是随船坐标系的原点.x轴指向为UUV的艏向、y轴为UUV右舷并在基阵平面上、z轴垂直xoy平面指向水底，如图3所示.

图3 短基线定位系统坐标系及原理Fig.3 SBL position system coordinate and principle

图3中换能器T0、T3间距为 a，T0、T1间距为 b.则在随船坐标系下各换能器的坐标为T0(a/2，b/2，0)，T1(a/2，b/2，0)，T2(－ a/2，b/2，0)，T3(－ a/2，－b/2，0).

定义信标的坐标F(x，y，z)，并设信标到各个换能器的直线距离为 R0、R1、R2、R3，可以通过声呐特性求得信标到基阵的距离.设换能器T0发射声信号到信标F并接收到应答信号所需时间为t0，若声速为 v，则有 R0=vt0/2，同理可得 R1、R2、R3.

由几何关系，能得到短基线定位方程为:

消去z得:

由式(5)很容易求解出x、y.

根据式(1)～(4)可解得4个深度值z，把求解到的4个z值进行平均，得到z的平均值:

1.2 视觉定位系统及其原理

图4是视觉定位系统的简图.

图4 UUV水下回收中的视觉定位系统Fig.4 Vision position system for UUV underwater docking

在UUV的腹部位置安装3个摄像头，这样配置可根据实际对接需要，配合坞舱上的导引，灯选择单目视觉定位导引或双目视觉定位导引方案.本文以单目视觉定位导引的原理为例进行介绍.在利用视觉定位导引UUV水下回收的过程中，主要需要获取的是UUV相对于坞舱的艏向和位置信息.

1.2 .1 UUV相对坞舱艏向的确定

坞舱艏向依靠导引灯的安装位置来确定.如图5所示，在坞舱底部纵中线上等距离安装了大小和形状不同的8个导引灯，其中，正中心大灯的箭头形状能标示坞舱的艏向.当采用单目识别时，单摄像头至少要能获取到2个或以上的导引灯图像，将几个导引光源连线后就能判断坞舱艏向角.

图5 导引光源布置示意Fig.5 Guided lamp collocation sketch map

图6 艏向和垂向视觉定位原理Fig.6 Heading and vertical position principle

1.2 .2 UUV相对坞舱位置的确定

1)垂向定位.

2)平移定位.

依据此几何关系能求解出摄像头光心和坞舱的相对位置.假设摄像头坐标系到UUV随船坐标系的变换矩阵为aHc=［△x 0 △z］T，其中△x、△z为摄像头相对UUV中心的横向和垂向距离，最终可以求解出UUV与坞舱的相对位置为

2 定位数据的自适应野值剔除方法

视觉和短基线定位传感器均会产生野值.而利用带野值的数据进行数据融合将会导致融合结果的严重错误.常用的剔除野值的方法有莱以特准则(3σ 准则)和格罗布斯(Grubbs)准则［3-4］.这2 种方法的基础是统计决策，都需要预先设定门限，这就使它们失去了实时性.本文采取一种在线的自适应野值剔除算法，其野值剔除步骤如下:

1)首先把采集的数据每M个采样时刻分为一组，去掉最大的和最小的，第1个数据(中间时刻的数据)就取剩余的M－2个数的平均值;

2)同理得到了第2个数据(第2组数据中间时刻的数据)，以及第N组数据中间时刻的数据.依次这样的步骤算n次，得到一组采样值，先进行最小二乘法拟合数据N个采样时刻的数值;

3)接着对上述处理的数据做残差变化率计算，得到每一时刻的数据变化率;

4)去掉数据变化率中的最大值和最小值，将剩余的平均值作为平均数据变化率的初值;

5)实时计算k大于N×M以后的数据变化率:

6)当数据满足以下2个条件时，就认为该数据是野值，要将其剔除:

对于野值，根据与变化率阈值的关系进行相应的替换.若data(k)大于thra(k)的变化率，将此数据用data(k－1)代替;若变化率介于 thra(k)和thrb(k)之间，将此数据用数据 2data(k－1)－data(k－2)代替.

图7给出了在进行UUV水下回收水池试验时，利用上述野值剔除方法对一段时间的SBL定位X轴向的数据进行在线野值剔除的结果.从图7可以看到绝大部分的野值均被在线剔除掉了.

图7 数据的野值剔除Fig.7 Abnormal value eliminating of data

3 定位数据的软阈值小波去噪方法

短基线和视觉定位数据不可避免地会包含噪声，这些噪声包括信号干扰、水下声信息传输时反射声信号噪声、摄像机在水下背景光变化等.

本文在进行定位数据融合前，采用一种软阈值小波方法进行数据去噪.小波去噪的原理是:数据经过小波正交变换后，实际信号的小波系数要大于噪声信号的小波系数［5］.根据这一原理设定某个阈值，将低于阈值的小波系数进行衰减处理，一般直接设置成零，而高于阈值的小波系数则进行保留或者适当地压缩处理［6］.根据对小波阈值系数的处理方式不同，可以分为硬阈值滤波方法和软阈值滤波方法，图8给出了2种阈值的选取方法.

硬阈值的设置方式为

软阈值的设置方式为

图8 小波去噪的硬阈值和软阈值方法Fig.8 Hard and soft limen method of wavelet de-noising

从图8中可以看出硬阈值方法存在阈值λ处的不连续性，这将导致滤波后信号出现部分振荡.而软阈值方法在阈值λ处是连续的，间接反映到滤波后的信号光滑性［7］.本文采用软阈值小波方法进行数据的去噪处理.

图9为对进行野值剔除后的短基线定位数据应用软阈值小波去噪后的结果，可以看出去噪效果比较理想.

图9 数据的去噪Fig.9 De-noising of data

4 短基线与视觉定位自适应融合方法

4.1 系统模型描述

首先，建立视觉的测量模型如下:

同样建立SBL的测量模型为

式中:Zs(k)=［xsyszs］T，xs、ys和 zs为 SBL 传感器测得的UUV与对接中心点的2个轴向距离和高度距离，Hs为SBL定位的观测转移矩阵.

Vv(k)和Vs(k)分别为零均值的高斯测量噪声，协方差为Rv(k)和Rs(k).

考虑一离散非线性动态过程模型1，如式(6)所示:

式中:X(k)∈Rn，为第k时刻系统状态向量:

ω(k)∈Rn，为一组零均值高斯白噪声序列，假设其协方差矩阵为 Q(k)=E［ω(k)ω(k)T］.F(k+1，k)∈Rn×n为状态转移矩阵，T 为采样周期.Z(k)∈Rl，为传感器测量向量，v(k)∈Rl为零均值高斯观测噪声，且其协方差为

将式(6)进行线性化得到如下方程:

考虑到式(6)所描述的模型，可以通过扩展卡尔曼滤波的方法得到估计值(k|k)和预测值(k+1|k).

估计的不确定性由如下矩阵得到

预测方程定义如下:

式中:用Zv(k)和Zs(k)代替了Zj(k).

最后，根据标准Bar-Shalom公式，将2个相互联系的估计做最优结合，可得到状态的全局估计.

4.2 自适应卡尔曼融合方法

卡尔曼滤波方法(Kalman filter，KF)是应用最广的数据融合方法，也是数据融合中进行位置估计的有效方法［8］.而传统的KF算法要求先得到系统过程噪声和测量噪声的先验统计信息，而在实际中，这是非常困难的.噪声信息的估计误差会严重降低KF算法的准确性，甚至会导致滤波器的发散［9］.

本文提出了一种基于模糊逻辑的在线自适应KF算法，通过获取的实时测量数据，自适应调整噪声的协方差矩阵.据此，推导出了基于更新的自适应估计算法来调节噪声协方差矩阵R(k)，并且采用了协方差匹配的方法.这种方法的基本思想是使系统残差的协方差实际值与理论值相一致.更新序列或残差的理论协方差如式(7):

理论协方差可以通过KF算法实时得到.考虑到更新序列R(k)有效性，它的实际协方差C(k)通过大小为N的移动估计窗口内的平均值估计得到公式如下:

式中:i0=k－N+1是估计窗口的第1个样本.根据经验选择窗口尺寸的大小，以进行统计滤波.在算法过程中，对2个协方差进行实时比较，如果发现R(k)的实际协方差与其理论值有差异，那么调整R来修正差异.为检测理论值S(k)与实际值Cr(k)之间的差异，定义了一个新的变量，称为匹配度(Dom).

采用模糊推理的方法来调整R.根据式(7)可以看出，R的增长会导致S的增长，反之亦然.因此，可以根据Dom的值通过R来改变S的值，最终减少S与之间的偏差.

综上所述，可以定义自适应算法的以下3条基本原则:

1)如果Dom≅0，保持R不变;

2)如果Dom＞0，R减少;

3)如果Dom＜0，R增加.

因此，R通过如下方式调整:

式中:△R(k)是每个时间采样点R要增加或减少的值，R(k+1)是模糊推理系统的输出值，Dom是输入值.

4.3 试验结果

图10～12给出了水池试验数据融合结果.

图10 视觉和SBL的X轴向定位融合Fig.10 Vision and SBL position fusion in X axis

图11 视觉和SBL的Y轴向定位融合Fig.11 Vision and SBL position fusion in Y axis

图12 视觉和SBL的Z轴向定位融合Fig.12 Vision and SBL position fusion in Z axis

试验数据选取了UUV在坞舱上方缓慢下降过程中的数据.该过程中，X和Y轴向的SBL、视觉定位数据基本上保持在零位置上做类似周期的摆动，在Z方向上随着UUV逐渐接近坞舱，SBL的定位精度越来越差.但融合后的定位数据是始终有效的，且精度较高，从实际回收对接效果看，能为UUV的回收提供可靠的定位数据源.

5 结束语

本文基于UUV所配置的视觉和短基线定位系统，对其水下回收中的定位进行融合，以提高定位的精度.首先采用自适应野值剔除和软阈值小波去噪方法对定位数据进行预处理，然后利用自适应模糊融合方法进行定位融合.水池试验表明，定位传感器的绝大部分野值被剔除且去噪效果明显，融合后的数据好于单个任意定位传感器的数据，能为UUV的水下回收提供更好的定位数据源.

［1］燕奎臣，吴利红.AUV水下对接关键技术研究［J］.机器人，2007，29(3):267-273.

YAN Kuichen，WU Lihong.A survey on the key technologies for underwater AUV docking［J］.Robot，2007，29(3):267-273.

［2］吴永亭，周兴华，杨龙.水下声学定位系统及其应用［J］.海洋测绘.2003，23(4):18-21.

WU Yongting，ZHOU Xinghua，YANG Long.Underwater acoustic positioning system and its application［J］.Oceanic Topography，2003，23(4):18-21.

［3］朱荣生，施小成.一种用于GPS数据处理中剔除野值的算法［J］.中国惯性技术学报，2006，8(2):27-30.

ZHU Rongsheng，SHI Xiaocheng.Method of rejecting outliers in the data process of GPS［J］.Journal of Chinese Inertial Technology，2006，8(2):27-30.

［4］张帆，卢峥.自适应抗野值Kalman滤波［J］.电机与控制学报，2007，11(2):188-195.

ZHANG Fan，LU Zheng.Robust Kalman filter for outliers suppression［J］.Electric Machines and Control，2007，11(2):188-195.

［5］孙延奎.小波分析及应用［M］.北京:机械工业出版社，2005:6-8.

［6］DONOHO D L.De-oising by Soft-threhoding［J］.IEEE Trans on Info Theory，1995，5(41):613-627.

［7］DONG C Y，YUAN Q.A combined wavelet analysis-fuzzy adaptive algorithm for rada/infrared data fusion［J］.Expert Systems with Aplications，2010，37(3):2563-2570.

［8］李洪志.信息融合技术［M］.北京:国防工业出版社，1996:1-5.

［9］邓自立.信息融合滤波理论及其应用［M］.哈尔滨:哈尔滨工业大学出版社，2007:10-13.

［10］宋振华，战兴群.基于多传感器融合的水下机器人导航系统［J］.微计算机信息，2008，24(2):240-241.SONG Zhenhua，ZHAN Xingqun.Autonomous navigation of underwater vehicle based on multi-sensor fusion technology［J］.Microcomputer Information，2008，24(2):240-241.