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

2015-02-26孙海涛晏正新

兵器装备工程学报 2015年5期

孙海涛，晏正新，王玥，张皓

(1.中国船舶重工集团公司第七一〇研究所，湖北宜昌 443003;

2.江南工业集团有限公司，长沙 410025)

AUV(Autonomous Underwater Vehicle)水下自主对接技术为其完成水下作业能源供给、任务使命下载和数据回传等任务提供了重要技术保障。

在AUV与水下平台自主对接中，成功对接的关键在于近端对接时AUV姿态的数据实时准确获取和对AUV的精准控制，传感器数据的精准程度直接决定了完成自主对接的效果，一般采用精度较高的视觉传感器和短基线(Short Baseline，SBL)定位系统进行数据融合［1］。AUV与传感器间的距离决定了这两种传感器的能力，当距离较远时，SBL发挥主要作用，其获取的姿态数据足以使AUV朝大体的方向接近对接平台;当距离适中时，两种传感器同时工作，数据融合系统开始发挥作用［2］。

由于视觉传感器和SBL定位系统在不同作用范围下的精度和可靠性不同，为了使AUV能在两套定位系统同时工作时实现精准的自主对接，本文重点研究了两套定位系统同时工作时的定位数据融合方法，并通过水池试验验证了方法的有效性。

1 融合数据的预处理

本文在传感器数据融合之前，对传感器采集过来的数据采用了一种基于软阈值小波方法进行数据滤波［3－4］。

在AUV与平台对接过程中，AUV携带了多种传感器，用到的视觉传感器和SBL每次开机时间并不是同步的，并且视觉传感器的采样频率为0.5 Hz，SBL传感器的频率为2 Hz，由于传感器测量数据的时间并不严格均匀，数据并不是都为均匀采样，存在着时间基准点和采样基准点的不统一。另外各个传感器安装的位置不同，所获取的空间位置必须进行相应的空间坐标变换，统一到同一参考坐标系下来。在进行数据融合之前，传感器的数据必须要进行时间和空间的统一配准。

通常为简化算法，选择一个传感器做为融合中心时基，其他的传感器都随此融合中心进行配准，作为中心时基的传感器一般要求测量频率较高。在视觉传感器和SBL数据融合系统中，以SBL传感器的时间基作为融合中心基，将视觉传感器的数据匹配到SBL时间基上，视觉传感器的采样大多为0.5 Hz，但存在一定比例的数据采样频率并不严格稳定，其采样依然看作是不均匀采样，对采样时刻不均匀数据一般采用曲线拟合法，在具体的时间配准过程应用如下步骤:

1)各个传感器的开机时间不一样，数据融合的起点设定在最晚开机的传感器之后;

2)对于视觉采样的数据，选取N个采样点先进行数据拟合，得到基于这N个点的光滑曲线函数y=f(x);

3)根据SBL的采样频率，选取下一拍即x(k+1)时间点得到拟合的视觉采样数据，如果视觉传感器在这的拍点上未进行采样，则将拟合的数据作为次拍的采样数据，反之，则用视觉传感器实测数据作为采样数据，送入融合中心。

传感器的空间配准主要是对传感器在各个坐标系下进行空间坐标变化，将各个传感器的坐标配准到同一坐标系统下，一般都配准到传感器精度较高的传感器系统坐标下［5］。对于视觉传感器和SBL数据融合系统，视觉传感器在其有效的距离内传感器精度要远高于SBL，选择将SBL的空间配准到视觉传感器坐标系统下。

试验采用高度计在初始位置对AUV和对接平台静止数据进行初始校准。

2 系统模型描述

目标测量向量位于直角标系下，并且视觉的测量模型如式(1)

其中，Zv(k)=［xvyvzv］T，xv、yv和 zv为视觉传感器测得AUV与对接中心点两个轴向距离和高度距离。

SBL的测量模型如式(3)

其中，Zs(k)=［xsyszs］T，xs、ys和 zs为 SBL 传感器测的AUV与对接中心点两个轴向距离和高度距离。

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

离散非线性动态过程模型如式(5)

其中，X(k)∈Rn，X(k)为第k时刻系统的状态向量，ω(k)∈Rn，ω(k)为一组零均值高斯白噪声序列，假设其协方差矩阵为 Q(k)，Q(k)=E［ω(k)ω(k)T］。F(k+1，k)∈Rn×n，F(k+1，k)，为已知的状态转移矩阵。

式(6)中，T为采样周期。Z(k)∈Rl，Z(k)为传感器测量向量，v(k)∈Rl，v(k)为零均值高斯观测噪声，且其协方差为R(k)=E［v(k)v(k)T］。

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

首先得到如下估计方程

最后，根据标准Bar-Shalom公式，将两个相互联系的估计做最优结合，即可得到状态的全局估计如式(15)所示

3 模糊自适应EKF融合算法

传统的卡尔曼滤波算法要求先得到系统过程噪声和测量噪声的先验统计信息，即矩阵Q(k)和R(k)。在实际应用中，这些数据信息的初始值均为估计得到，是根据系统的先前经验而得到的，并且不能做到与系统状态时刻进行的实时更新，这种先验的估计的准确程度的决定了算法的的精确度［6］。也就是说EKF估计算法的实际应用效果与这些先验的统计信息有着直接的关系［7］。事实证明，噪声信息的估计误差会严重降低EKF算法的准确性，甚至会导致滤波器的发散［8，9］。

因此，本文提出了一种基于模糊逻辑的在线自适应卡尔曼算法。通过获取的实时测量数据，自适应调整噪声的协方差矩阵，这种方法的基本思想是使系统残差的协方差实际值与理论值相一致［10］。残差 R(k)如式(16)所示的理论协方差

实际协方差C(k)通过大小为N的移动估计窗口内的平均值估计得到，如式(17)所示

其中:i0=k－N+1是估计窗口的第1个样本。根据经验选择窗口尺寸的大小，以进行统计滤波。在算法运行过程中，对实际协方差和理论协方差进行实时比较，如果R(k)的实际协方差和理论协方差有差异，那么调整R(k)修正差异。

定义残差的实际协方差和理论协方差的比值

Tr()表示对矩阵求迹。从式(18)可知，如果量测噪声增大，则(k)增大，继而Ra(k)增大，此时需要增大R(k)使Ra(k)回到1附近;反之，如果量测噪声减少，就需要减小Ra(k)。Ra(k)的变化由t(k)来控制，即t(k)=FIS(Ra(k))。

定义模糊子集equal1表示在1附近，more1表示基本大于1，less1表示基本小于1，调整系数t(k)的FIS规则如下:

1)if Ra(k)is equal1，then t(k)is equal1

2)if Ra(k)is lessl1，then t(k)is less1

3)if Ra(k)is more1，then t(k)is more1

4 实验结果

图1～图3为AUV对视觉数据和SBL数据进行自适应融合后的水池试验结果，选取系统开机后350 s到550 s短基线定位系统和视觉定位系统获取AUV三个轴向上的位置数据，经过模糊自适应算法进行数据融合后，其结果表明融合后的位置数据好于单个任意传感器系统的数据，能为AUV水下对接控制系统提供更为可靠的定位数据。

图1 X轴向数据融合

图2 Y轴向数据融合

图3 Z轴向数据融合

5 结束语

本文研究了基于视觉与短基线定位系统的AUV数据融合定位方法，为提高水下定位精度，提出了一种基于模糊自适应思想的卡尔曼滤波数据融合方法。水池试验结果表明:融合后的定位数据优于单传感器定位系统，为AUV水下自主对接提供了更可靠的定位数据。

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

［2］晏正新.UUV回收对接的数据融合及预报方法研究［D］.哈尔滨，哈尔滨工程大学，2012:38 －41.

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

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

［5］邓自立.信息融合滤波理论及其应用［M］.哈尔滨:哈尔滨工业大学出版社，2007:78－156;272－404.

［6］程峥.多传感器目标跟踪中的数据融合理论与算法［D］.西安:西北工业大学，2007:9 －15.

［7］王晓娟.基于视觉的AUV水下回收导引定位技术研究［D］.哈尔滨:哈尔滨工程大学，2011:68 －69.

［8］宋振华，战兴群，张炎华.基于多传感器融合的水下机器人导航系统［J］.机器人技术，2008，24(1－2):240－241.

［9］ GAN Q，HARRIS C.J.Comparison of two measurement fusion methods for kalman-filter based multisensor data fusion［J］.IEEE Trans.on Aerospace and Electronic Systems，2001，37(1):273 －279.

［10］严涛，王跃刚，杨波，等.模糊自适应卡尔曼滤波算法在航位推算系统中的应用［J］.计算机测量与控制，2012，20(3):774－796.

(责任编辑杨继森)