智能系统：UUV水下回收中的视觉和短基线定位融合

第8卷第2期 智能系统学报 Vol.8 No.2 2013年4月 CAAI Transactions on Intelligent Systems Apr.2013 D0I:10.3969/i.issn.16734785.201301020 网络出版t地址：htp://www.cnki.net/kcma/detail/23.1538.TP.20130409.1436.001.html UUV水下回收中的视觉和短基线定位融合 黎南'，湛鑫2，陈涛3，严浙平3 (1.海军驻大连地区军代表室，辽宁大连116021；2.中国船舶重工集团公司第703研究所，黑龙江哈尔滨150036； 3.哈尔滨工程大学自动化学院，黑龙江哈尔滨150001) 摘要：为了使UUV在水下坞舱回收过程中利用视觉和短基线(short baseline-SBL)进行导引定位，提出了一种视觉 和短基线的自适应融合定位方法，以提高导引定位的精度.介绍了短基线定位和视觉定位2种定位系统及其工作原 理，以及定位数据的野值剔除和去噪方法.野值剔除采用了一种基于数据变化率的自适应在线野值剔除方法，数据 去噪采用了软阔值小波滤波方法.针对传统卡尔曼滤波进行数据融合时先验知识不足的缺点，提出了一种基于模糊 逻辑的在线自适应卡尔曼滤波融合方法.通过获取的实时测量数据，实时调整噪声的协方差矩阵来融合2种定位数 据。水下回收水池试验结果表明，定位传感器的绝大部分野值被剔除且去噪效果明显，视觉和短基线融合后的定位 精度有很大提高，证明了所提方法的有效性， 关键词：UUV;水下回收；视觉定位；短基线定位：数据融合 中图分类号：TP18:U661.313文献标志码：A文章编号：16734785(2013)02015606 Data fusion method of vision and SBL position for UUV underwater docking LI Nan',ZHAN Xin2,CHEN Tao3,YAN Zheping (1.Military Representative office of PCA Navy in Dalian,Dalian 116021,China;2.703 Research Institute of China Shipbuilding In- dustry Corp.,Haerbin 150004,China;3.College of Automation,Harbin Engineering University,Haerbin 150001,China) Abstract:The vision position and SBL position are applied to underwater docking of UUV,and thus,an adaptive data fusion method for vision and SBL was proposed for position precision improvements.Firstly,SBL,vision posi- tion system and their principles were introduced.Next,abnormal value eliminating and denoising methods were de- scribed and an adaptive online method based on change rate of data was proposed to eliminate the abnormal value. A soft threshold wavelet filtering method was also proposed for denoising.Taking into consideration the lack of prior knowledge for fusion by using Kalman filter,an adaptive online Kalman filter fusion method based on fuzzy logic was proposed.Covariance matrix of noise was adjusted online for fusion of the two types of position data.Finally, the results of underwater docking in pool tests show that most abnormal values and noise were eliminated remark- ably.The results also indicate the position precision was improved by data fusion of vision and SBL position,which prove the proposed method was effective. Keywords:unmanned underwater vehicle;underwater docking;vision position;SBL position;data fusion 有效地布放回收是UUV(unmanned underwater且均以实现UUV的自动回收为最终目标l.本文研 vehicle)能否广泛应用的一个关键因素.相对于布放究了一种利用水下坞舱自动回收UUV的方式.在这 来说，UUV的回收难度很大.因此，世界上UUV领种方式中，UUV需要以一定的定位导引方法识别 域的研究人员也发展出了各式各样的回收方式，而 UUV与坞舱的相对方向和相对位置，从而控制自己 精确地进人坞舱之中.对于坞舱水下回收UV中的 收稿日期：201301-16.网络出版日期：201304-09 基金项目：国家自然科学基金资助项目(51179038)：中央高校基本科 定位导引系统，采用了适用远距离定位的短基线定 研业务费专项资金资助项目(HEUCF041323). 通信作者：陈涛.E-mail:chentao_7777@163.com. 位系统和适用于近端精确定位的视觉导引定位系

第2期 黎南，等：UUV水下回收中的视觉和短基线定位融合 ·157· 统.由于2套定位系统在不同作用范围下的精度和 标系的原点，同时也是随船坐标系的原点，x轴指向 可靠性不同，为了UUV能简单、高效、安全地实现与 为UUV的艏向y轴为UUV右舷并在基阵平面上、z 坞舱对接回收，本文重点研究了短基线和视觉在 轴垂直xoy平面指向水底，如图3所示. UUV水下回收中的定位融合方法，并通过水池试验 随船 验证了所提方法的有效性. 坐标系 1UUV水下回收定位系统及其原理 7 UUV的回收过程是一种空间立体运动，主要需 基阵元件 要控制4个自由度：纵向、横向、垂向和艏向，当 R UUV开始进入水下对接回收任务后，其回收方式类 R 似于直升机降落的坐落坞舱式回收，如图1所示. UUV从对接起始位置水平直航到回收坞舱的正上 F(x,y,z】 方，粗略调整姿态和艏向，然后垂直降落到距坞舱较 近高度的正上方，接下来精确调整UUV的姿态和艏 图3短基线定位系统坐标系及原理 向，缓慢降落至回收坞舱上.UV距离坞舱较远时 Fig.3 SBL position system coordinate and principle 图3中换能器T。、T3间距为a,T。、T间距为b. 视觉定位精度差，SBL定位精度高；距离较近时， 则在随船坐标系下各换能器的坐标为T。(a/2,b/2, SBL声定位将会出现盲区，而视觉定位的精度高2]； 0),T1(a/2,b/2,0),T2(-a/2,b/2,0),T3(-a/2, 距离中等时，2种定位信号均有效 -b/2,0). 定义信标的坐标F(x,y,z),并设信标到各个换 能器的直线距离为R,、R1、R2、R3,可以通过声呐特 性求得信标到基阵的距离.设换能器T。发射声信号 到信标F并接收到应答信号所需时间为。，若声速 为v,则有R=t2,同理可得R1、R2、R 由几何关系，能得到短基线定位方程为： 图1UUV水下回收方式 R6=(x-a/2)2+(y+b/2)2+z2, (1) Fig.1 UUV underwater docking mode R=(x-a/2)2+(y-b/2)2+2, (2) 1.1短基线定位系统及其原理 R=(x+a/2)2+(y-b/2)2+2, (3) 图2是短基线定位系统的简图.声呐基阵由安 R=(x+a/2)2+(y+b/2)2+z2. (4) 装在UUV底部的4个换能器T、T1、T2、T3构成，信 消去z得： 标F置于坞舱上. R3-Ri=2ax,R2-Ri=2ax, (5) R-R=2by,R3-R吃=2by UUV 由式(5)很容易求解出xy. 根据式(1)~(4)可解得4个深度值z,把求解 SBI 基 到的4个z值进行平均，得到z的平均值： z=(1+2+3+24)/4。 1.2视觉定位系统及其原理 图4是视觉定位系统的简图. 标 坞舱 7 摄像头3摄像头1·摄像头2 7431260- 图2UUV水下回收中的短基线定位系统 Fig.2 SBL position system for UUV underwater docking 声呐基阵的4个换能器安装在一个平面上，基 图4UUV水下回收中的视觉定位系统 阵的几何中心为UUV的几何中心，定义为短基线坐 Fig.4 Vision position system for UUV uderwater docking

·158 智能系统学报 第8卷 在UUV的腹部位置安装3个摄像头，这样配置 1.2.2UUV相对坞舱位置的确定 可根据实际对接需要，配合坞舱上的导引，灯选择单 1)垂向定位. 目视觉定位导引或双目视觉定位导引方案.本文以 如图6所示，I1(1,y1)和I2(2,y2)两导引灯的 单目视觉定位导引的原理为例进行介绍.在利用视 距离为D,对摄像机进行标定后镜头的焦距为已知， 觉定位导引UUV水下回收的过程中，主要需要获取 令镜头焦距为￡根据几何关系有如下等式成立： 的是UUV相对于坞舱的艏向和位置信息， r/D=f/H,其中r为两导引灯在图像上的距离r= 1.2.1UUV相对坞舱艏向的确定 √(1-x2)+(y1-y2),进而可得到UUV搭载的 坞舱艏向依靠导引灯的安装位置来确定，如图 摄像头光心距离坞舱的高度距离H=Dfr. 5所示，在坞舱底部纵中线上等距离安装了大小和 2)平移定位. 形状不同的8个导引灯，其中，正中心大灯的箭头形 设导引光源的坐标为(x6,y%,6),且在成像平 状能标示坞舱的艏向。当采用单目识别时，单镊像头 面上的坐标为(x,y).由垂向视觉定位知道6=H, 至少要能获取到2个或以上的导引灯图像，将几个 则在成像平面上相对平移的几何关系有 导引光源连线后就能判断坞舱艏向角， 鸟豌赠何 S16404 太LED 小LED 350小LED大LE2 依据此几何关系能求解出摄像头光心和坞舱的 相对位置.假设摄像头坐标系到UUV随船坐标系的 2800 变换矩阵为“H。=[△x0△z]T,其中△x、△z为摄 像头相对UUV中心的横向和垂向距离，最终可以求 图5导引光源布置示意 解出UUV与坞舱的相对位置为 Fig.5 Guided lamp collocation sketch map [6y哈6]T=[x6y%6]T+H 为提高艏向定位精度，一般选取在图像上相距 2定位数据的自适应野值剔除方法 最远的2个导引光源.因为光源在坞舱上是按纵向 安装的，很容易得知两光源连线与图像行扫描线的 视觉和短基线定位传感器均会产生野值.而利 夹角即为UUV对坞舱的艏向角.如图6所示， 用带野值的数据进行数据融合将会导致融合结果的 I(x1,y2)和I2(x2,y2)为图像上两光源，且假设I乃 严重错误.常用的剔除野值的方法有莱以特准则 方向为坞舱艏向，则夹角山为UUV相对坞舱的艏向 (3o准则)和格罗布斯(Grubbs)准则34.这2种方 角.则业的计算公式为 法的基础是统计决策，都需要预先设定门限，这就使 arctan(2), 它们失去了实时性.本文采取一种在线的自适应野 x2-1 值别除算法，其野值剔除步骤如下： 1)首先把采集的数据每M个采样时刻分为一 -T-arctan y2-y ,x2 y1; 组，去掉最大的和最小的，第1个数据（中间时刻的 a-arctan(<<y 数据)就取剩余的M-2个数的平均值； 2)同理得到了第2个数据（第2组数据中间时 刻的数据)，以及第N组数据中间时刻的数据.依次 这样的步骤算n次，得到一组采样值，先进行最小二 视觉图像 (x,y) 乘法拟合数据N个采样时刻的数值； 3)接着对上述处理的数据做残差变化率计算， 得到每一时刻的数据变化率； 4)去掉数据变化率中的最大值和最小值，将剩 余的平均值作为平均数据变化率的初值； 5)实时计算k大于N×M以后的数据变化率： rateave(k)=rateave(k-1)+ (rate(k)-rate(k-N-1))/(N+1). 图6艏向和垂向视觉定位原理 6)当数据满足以下2个条件时，就认为该数据 Fig.6 Heading and vertical position principle 是野值，要将其剔除：

第2期 黎南，等：UUV水下回收中的视觉和短基线定位融合 159. 1 thrb(k)=max(I rateave(-1)1, I rateave(-2)1,...,I rateave(-10)1), ②thra(k)=3×thrb(k). 对于野值，根据与变化率阈值的关系进行相应 的替换.若data(k)大于thra(k)的变化率，将此数据 用data(k-l)代替；若变化率介于thra(k)和 (a)硬阈值 (b)软國值 thrb(k)之间，将此数据用数据2data(-1)- data(k-2)代替， 图8小波去噪的硬阑值和软阈值方法 图7给出了在进行UUV水下回收水池试验时， Fig.8 Hard and soft limen method of wavelet de-noising 利用上述野值剔除方法对一段时间的SBL定位X 从图8中可以看出硬阈值方法存在阈值入处 轴向的数据进行在线野值剔除的结果.从图7可以 的不连续性，这将导致滤波后信号出现部分振荡.而 看到绝大部分的野值均被在线剔别除掉了， 软阈值方法在阈值入处是连续的，间接反映到滤波 后的信号光滑性.本文采用软阈值小波方法进行 数据的去噪处理。 图9为对进行野值剔除后的短基线定位数据应 用软阈值小波去噪后的结果，可以看出去噪效果比 较理想。 ·惊始数据 ×野值别除后数据 579 10 11 t/s 图7数据的野值剔除 x剔野值数据 Fig.7 Abnormal value eliminating of data ·小波滤波数据 3 定位数据的软阈值小波去噪方法 7 tis 90 短基线和视觉定位数据不可避免地会包含噪 图9数据的去噪 声，这些噪声包括信号干扰、水下声信息传输时反射 Fig.9 De-noising of data 声信号噪声、摄像机在水下背景光变化等。 4短基线与视觉定位自适应融合方法 本文在进行定位数据融合前，采用一种软阈值 小波方法进行数据去噪.小波去噪的原理是：数据经 4.1系统模型描述 过小波正交变换后，实际信号的小波系数要大于噪 首先，建立视觉的测量模型如下： 声信号的小波系数5].根据这一原理设定某个阈 Z(k)=H(k,X(k))+V(k), 值，将低于阈值的小波系数进行衰减处理，一般直接 式中：Z(k)=[x。y。,],y,和，为视觉传感器 设置成零，而高于阈值的小波系数则进行保留或者 测得的UUV与对接中心点的2个轴向距离和高度 适当地压缩处理]，根据对小波阈值系数的处理方 距离，H。为视觉定位的观测转移矩阵， 式不同，可以分为硬阈值滤波方法和软阈值滤波方 同样建立SBL的测量模型为 法，图8给出了2种阈值的选取方法 Z(k)=H,(k,X(k))+V,(k). 硬阚值的设置方式为 式中：Z(k)=[x,y,]T,xy,和a,为SBL传感器 测得的UUV与对接中心点的2个轴向距离和高度 ∫k,9k1≥； 94= 距离，H,为SBL定位的观测转移矩阵 l0,|0k|<入 V(k)和V,(k)分别为零均值的高斯测量噪声， 软阈值的设置方式为 协方差为R(k)和R,(). r0,k-入，0i.k≥入； 考虑一离散非线性动态过程模型1，如式(6)所示： 0,k={0, |0kI<入； X(k+1)=F(k+1,k)K(k)+(),(6) 01.k+入，03.k≤-入. Z(k)=H(k,X(k)+(k). 式中：X(k)∈R",为第k时刻系统状态向量：

·160· 智能系统学报 第8卷 X(k)=[x(k)(k)y()()名(k)(k)]T, KF算法，通过获取的实时测量数据，自适应调整噪 X(h)=[x,()(k)y,()(),()()]. 声的协方差矩阵.据此，推导出了基于更新的自适应 o()∈R”,为一组零均值高斯白噪声序列，假 估计算法来调节噪声协方差矩阵R(k),并且采用 设其协方差矩阵为Q(k)=E[ω(k)w(k)]. 了协方差匹配的方法.这种方法的基本思想是使系 F(k+1,k)∈R×m为状态转移矩阵，T为采样周期. 统残差的协方差实际值与理论值相一致.更新序列 Z(k)∈R',为传感器测量向量，()∈R'为零均值 或残差的理论协方差如式(7)： 高斯观测噪声，且其协方差为 S(k)=H(k)P(k1k-1)H(k)+R(k).(7) R(k)=E[v(k)v(k)]. 理论协方差可以通过KF算法实时得到.考虑 将式(6)进行线性化得到如下方程： 到更新序列R(k)有效性，它的实际协方差C(k)通 z(k)≈H(k,(k|k-1))+ 过大小为N的移动估计窗口内的平均值估计得到 调只1X刻-1k-1+到 公式如下： 2(k1k-1)+C(k)[X(k)-(k1k-1)]+ c):贵宫 (k)=C(k)X(k)+2(k)+v(k). 式中：o=k-N+1是估计窗口的第1个样本，根据 式中： 经验选择窗口尺寸的大小，以进行统计滤波.在算法 c(=证之l1w, 过程中，对2个协方差进行实时比较，如果发现 R(k)的实际协方差与其理论值有差异，那么调整R 2(k1k-1)=H(k,(k1k-1)), 来修正差异.为检测理论值S(k)与实际值C,(k)之 Z(k)=2(k1k-1)-C(k)(k1k-1). 间的差异，定义了一个新的变量，称为匹配度 考虑到式(6)所描述的模型，可以通过扩展卡 (Dom). 尔曼滤波的方法得到估计值￠（丨k)和预测值 Dom(k)=S(k)-C(). (k+11k). 采用模糊推理的方法来调整R.根据式(7)可 估计的不确定性由如下矩阵得到 以看出，R的增长会导致S的增长，反之亦然.因此， P-(k1)=P-(kIk-1)+C(k)R-()C(k). 可以根据Dom的值通过R来改变S的值，最终减 预测方程定义如下： 少S与C,之间的偏差， (k+11k)=F(k+1,k)(k|k), 综上所述，可以定义自适应算法的以下3条基 P(k+11)= 本原则： F(k +1,k)P(kl k)FT(+1,k)+(k). 1)如果Dom兰0，保持R不变； 为了更新状态向量，(k1-1)通过结合传感 2)如果Dom>0,R减少； 器获的测量数据Z,(k)和Z,(k)得到.新状态向量 3)如果Dom<0,R增加. 的局部估计为 因此，R通过如下方式调整： (kIk)=(k|k-1)+K(k)[Z(k)- R(k+1)=R(k)+△R(k). H(k,(k1k-1))]j=s,. 式中：△R(k)是每个时间采样点R要增加或减少的值， 式中：用Z(k)和Z.(k)代替了Z(k). R(k+1)是模糊推理系统的输出值，Dom是输入值。 K(k)= 4.3试验结果 P(k|k-1)C()(C(k)P(k-1)C()+R(), 图10~12给出了水池试验数据融合结果. P(k|k)=(I-K(k)C,(k))P(k|k-1). 0.6r *融合数据 x日 最后，根据标准Bar-Shalom公式，将2个相互联 0.5 ·视觉 系的估计做最优结合，可得到状态的全局估计， 4.2自适应卡尔曼融合方法 卡尔曼滤波方法(Kalman filter,KF)是应用最 广的数据融合方法，也是数据融合中进行位置估计 要n2 的有效方法⑧.而传统的KF算法要求先得到系统 0.1 过程噪声和测量噪声的先验统计信息，而在实际中， 3.03.54.04.5 505高600 这是非常困难的.噪声信息的估计误差会严重降低 is KF算法的准确性，甚至会导致滤波器的发散9 图10视觉和SBL的X轴向定位融合 本文提出了一种基于模糊逻辑的在线自适应 Fig.10 Vision and SBL position fusion in X axis

第2期 黎南，等：UUV水下回收中的视觉和短基线定位融合 ·161· [J].海洋测绘.2003,23(4)：18-21. 0.3 *融合数据 WU Yongting,ZHOU Xinghua,YANG Long.Underwater acoustic positioning system and its application[J].Oceanic Top0 graphy,2003,23(4):18-21. [3]朱荣生，施小成.一种用于GS数据处理中剔除野值的 算法[J].中国惯性技术学报，2006,8(2)：27-30. ZHU Rongsheng,SHI Xiaocheng.Method of rejecting outli- ers in the data process of GPS[J].Journal of Chinese Iner- tial Technology,2006,8(2):27-30. 0.2 3.5 4.04.5 5.05.5 6010 [4]张帆，卢峥.自适应抗野值Kalman滤波[J].电机与控 制学报，2007,11(2)：188-195. t/s ZHANG Fan,LU Zheng.Robust Kalman filter for outliers 图11视觉和SBL的Y轴向定位融合 suppression[J].Electric Machines and Control,2007,11 Fig.11 Vision and SBL position fusion in Y axis (2):188-195. 0.3 [5]孙延奎.小波分析及应用[M].北京：机械工业出版杜， 融合数据 2005:68. 0.2 [6]DONOHO D L.De-oising by Soft-threhoding[J].IEEE 0.1 Trans on Info Theory,1995,5(41):613-627. [7]DONG C Y,YUAN Q.A combined wavelet analysis-fuzzy E adaptive algorithm for rada/infrared data fusion[J].Expert Systems with Aplications,2010,37(3):2563-2570. -0.1 [8]李洪志.信息融合技术[M].北京：国防工业出版社， 8×10 1996:1-5. -0.303.5 4.04.55.05.56.0 [9]邓自立.信息融合滤波理论及其应用[M].哈尔滨：哈 tis 尔滨工业大学出版社，2007：10-13. 图12视觉和SBL的Z轴向定位融合 [10]宋振华，战兴群.基于多传感器融合的水下机器人导 Fig.12 Vision and SBL position fusion in Z axis 航系统[J].微计算机信息，2008,24(2)：240-241. 试验数据选取了UUV在坞舱上方缓慢下降过 SONG Zhenhua,ZHAN Xingqun.Autonomous navigation of 程中的数据.该过程中，X和Y轴向的SBL、视觉定 underwater vehicle based on multi-sensor fusion technology [J].Microcomputer Information,2008,24(2):240-241. 位数据基本上保持在零位置上做类似周期的摆动， 作者简介： 在Z方向上随着UUV逐渐接近坞舱，SBL的定位精 黎南，男，1969年生，高级工程师， 度越来越差.但融合后的定位数据是始终有效的，且 主要研究方向为船舶自动控制，发表学 精度较高，从实际回收对接效果看，能为UUV的回 术论文近10篇. 收提供可靠的定位数据源 5结束语 本文基于UUV所配置的视觉和短基线定位系 统，对其水下回收中的定位进行融合，以提高定位的 湛鑫，女，1974年生，副译审，主要 精度.首先采用自适应野值剔除和软阈值小波去噪 研究方向为科技翻译和情报信息处理， 方法对定位数据进行预处理，然后利用自适应模糊 发表学术论文3篇， 融合方法进行定位融合.水池试验表明，定位传感器 的绝大部分野值被剔除且去噪效果明显，融合后的 数据好于单个任意定位传感器的数据，能为UUV的 水下回收提供更好的定位数据源。 参考文献： 陈涛，男，1983年生，讲师，主要研 究方向为水下无人航行器技术，发表学 [1]燕奎臣，吴利红.AUV水下对接关键技术研究[J].机器 术论文10余篇。 人，2007,29(3)：267-273. YAN Kuichen,WU Lihong.A survey on the key technolo- gies for underwater AUV docking J].Robot,2007,29 (3):267-273. [2]吴永亭，周兴华，杨龙.水下声学定位系统及其应用