自主式水下航行器水下回收融合引导技术方案及算法

赵蕊，许建

中国舰船研究设计中心， 湖北 武汉 430064

0 引 言

目前，海洋资源开发日益受到重视，作为勘探和开发的先进装备[1]——无缆自主式水下航行器（autonomous underwater vehicle, AUV ）能够完成海洋环境监测、海底地貌地形测量、水下资源勘探和取样等多种任务[2-4]。随着AUV技术的不断发展及其研究的不断深入，AUV的自主与机动能力有了很大提高，已广泛应用于海底探测和水下作业[5-7]。AUV在水下作业的时间也受其所携带的能源限制，即完成某项任务后需回收进行能源补充、数据交换、新任务下载等[8-10]。因此，在执行水下观测等这种耗时任务时，对于提高工作效率、节省人力和成本而言，AUV水下自动对接系统有着重要意义[11-12]。然而，水下环境复杂而特殊，AUV水下对接定位仍面临极大的挑战，从而影响AUV的定位精度，故自主回收对接技术成为了国内外业界一个研究的热点和难点[13-15]。

在水下平台的支持与布放回收技术领域，我国相关研究起步较晚[16]，与国外的差距明显，而这也是水下平台搭载作业中最核心的技术。鉴此，为克服单一传感器的可靠性和辨识准确性低[17-19]的问题，本文将面向移动平台（或移动目标）的AUV回收对接作业，提出一种基于惯性导航（INS）、声学超短基线定位（USBL）、光学等信号的多信息融合的水下引导系统方案，并结合回收对接过程中的5个阶段建立运动方程，设计基于扩展联邦卡尔曼滤波器（federated extended Kalman filter）及融合INS和USBL及光学信息的导航系统，得到一种基于上述3种信息的多源数据融合导航算法。最后，设计几条典型对接轨迹，通过仿真验证AUV回收对接引导过程中的各阶段所用方法的可行性。

1 多源数据融合引导技术方案及实现框架

1.1 技术方案

本文提出面向移动目标的AUV水下回收对接引导过程（也称定位导航）采用INS和USBL及光学引导相结合的方式[20-24]。其中，在中远程阶段，采用声呐辅助定位[25]，即通过安装在航行器底部的声呐定位设备（例如USBL）与布置在移动目标背部的声应答器之间的声传输来完成；光学引导过程则由前、后和末期这3个阶段组成，分别对应于近程、垂向末端及平移锁紧阶段，此引导过程是通过AUV底部安装的水下摄像机对移动目标背部布置的引导用LED灯阵和灯带进行感知来获取相应的导航信息。图1所示为AUV水下回收对接引导的全过程。下文将详述各阶段操作的过程。

图1 AUV回收作业对接引导的全过程Fig.1 Whole process of guiding AUV docking in recovery operation

阶段1：AUV完成作业后，通过自主导航返回到与移动目标处于中远距离的A处（约1～2 km），经与移动目标协商并确定回收后，再航行至移动目标上方附近的B处（位于移动目标20 m外）。在此阶段，AUV使用自身携带的声呐定位系统定期测定移动目标的相对位置以辅助AUV导航。

阶段2：AUV到达移动目标附近的B处后，移动目标背部布置的LED灯阵开启，灯阵发出的光束方向朝向AUV（在灯阵光束方向可调的情况下），使AUV进入光学引导范围内。届时，AUV使用自身携带的摄像机捕获移动目标上的灯阵，引导AUV向移动平台正上方移动，边缓慢下降边靠近移动目标。此时，灯阵光束方向也同步缓慢移向移动目标上方，AUV开始追踪灯阵并运动至移动目标正上方的C处（约距离移动目标10～20 m）。在此阶段，AUV离移动目标较远，摄像机拍摄到的灯阵光晕相互重叠，会形成单光源，故AUV采用融合追踪光源的方式来导航靠近移动目标，并到达移动目标的正上方。

阶段3：AUV到达移动目标的正上方后，AUV控制自身速度、姿态，并与移动目标保持基本一致。AUV基本保持与移动目标以相同方向和速度航行，逐渐靠近移动目标并到达D处（距离移动目标约10 m）。此时，移动目标正上方的AUV离目标仍较远，AUV上的摄像机拍摄到的灯阵光晕仍相互重叠，会形成单光源，在此阶段，AUV继续采用融合追踪光源的导航方式。

阶段4：随着AUV靠近移动目标，AUV上的摄像机逐渐能够完全分辨出每个LED光源的轮廓，此时，AUV使用相应的光学引导算法，得到AUV与移动目标间精准的相对位置及相对姿态，从而进行声、光融合导航。经过稳定估计出高精度的AUV相对位置及相对姿态，直至航行到移动目标正上方的E处（距离移动目标约1 m），AUV调整好姿态使其与移动目标保持一致。在此阶段，AUV采用融合了INS和USBL及光学引导的导航方式。

阶段5：AUV航行至移动目标正上方的E处后，此时，移动目标的LED灯阵关闭，LED灯带开启。AUV调整好姿态继续缓慢向前相对平移，并根据自身携带的摄像机拍摄出视角内LED灯带的图像，解算出AUV相对于移动目标的横向及纵向角度偏差。在此阶段，AUV采用融合了INS和高度计及光学引导的导航方式。

最后，只需将移动目标对接口中心轴航向距离调整至限位机构的误差范围内的F处，沿摇杆下滑，进一步调整艏向偏差，与AUV完成对接锁紧即可。从上述AUV水下回收对接的全过程来看，INS和USBL及光学引导系统的多源数据融合导航在不同阶段各子系统参与融合的数据有所不同。

1.2 实现框架

图2所示为实现水下移动目标与AUV之间回收对接的引导系统的总体框架，图中标示了各部分组件及其相互关系。

图2 AUV与移动目标实现对接的系统总体框架Fig.2 General framework of implementation for AUV docking system with mobile platform

由图2可见，水下引导系统主要由处理器、水下摄相机、USBL和INS以及布置在移动目标背部的LED灯阵与USBL声应答器等设备组成。其中，布置在移动目标背部的USBL声应答器用于响应安装在AUV中的USBL发送的询问信号，AUV中的USBL需定位时会发出声脉冲信号，移动目标上的USBL声应答器收到并发送应答信号，经水下传输后回到AUV，再经USBL中的水听器阵列接收应答信号，解算出相对位置。

如图2（a）所示，移动目标与AUV水声通信时需获取双方的航态信息。一方面，移动目标将自身航态信息（例如位置、姿态、速度等）发送给AUV的光学引导统；另一方面，移动目标需控制LED灯阵的光束进行偏转，控制器则需获取AUV的航态信息用于控制光束发出的方向，以实时追踪位置。此外，设置的LED灯阵还需有合理的间距和排列方式。

如图2（b）所示，AUV水下引导子系统的处理器是整个系统的核心，其功能主要是实时处理摄像机采集的视频流，以及运行光学引导相关算法，获取定位信息，再结合USBL和INS系统的定位信息进行融合引导。由于引导系统需要的是相对位置，因此处理器还需要接收移动目标的航态信息，经融合导航处理后，输出整个系统的最终定位信息，再经通信接口（RS232）与AUV进行信息传输，将AUV的航态信息发送至移动目标的控制器。届时，所得定位信息将会作为AUV控制系统的输入，引导下一时刻的运动。

2 多源数据融合导航算法研究

2.1 联邦卡尔曼滤波器原理

如上所述，本文提出的AUV水下回收对接引导系统融合了INS和USBL及光学定位这3种导航系统，它们的传感器观测得到的信息数据量测值均不相同，精度各一。因此，为获取高精度的导航定位信息，还需要将各传感器观测得到的量测值进行组合滤波[26-27]。对于组合导航系统而言，相比采用一个卡尔曼滤波器来集中处理所有导航传感器的信息，联邦卡尔曼滤波器（federated Kalman filter,FKF）可克服前者的状态维数高、计算量大、容错性低等缺点[28]。因此，本文采用了FKF作为状态估计的方法。FKF是一种典型的信息融合技术[29]，其由多个子滤波器和一个主滤波器组成，采取两个阶段分散滤波，最后，通过数据融合方法提高滤波精度。

INS和USBL及光学定位这3种传感器各有优缺点。例如，INS输出的姿态、速度及位置等三维信息具有更新速率高、短期精度和稳定性好的优点，但量测信息输入到子滤波器后误差会随时间而积累；USBL可输出精度较高的相对位置信息，但更新速率慢；光学定位系统可输出高精度的相对位置和相对姿态信息，更新速率快，但作用距离短。

本文提出的采用基于FKF的多源数据融合导航方案中，若USBL量测值与光学量测值在同一时刻到来时，首先处理的是USBL量测值，然后是2个子滤波器分别进行扩展卡尔曼滤波，再将滤波结果传输给主滤波器进行数据融合，最后得到最优估计值。图3所示为多速率联邦扩展卡尔曼滤波器总体框架。图中，z1，z2分别为INS和USBL量测信息，,,分别为光学前、后期及末期量测值。

图3 多速率联邦扩展卡尔曼滤波器总体框架Fig.3 General framework of federate extended Kalman filter with multi-rates

联邦扩展卡尔曼滤波器的数据融合原理是：AUV多源数据融合引导系统中的全部数据在几个子滤波器间进行初始分配，将分配的数据与局部滤波器融合，完成局部更新，重新组合成新的系统总信息。在此系统中，一般将INS作为公共参考系统，参与2个子滤波器的数据分配。根据信息守恒原理，分配系数满足：

式中：γi为 第i个 子滤波器的信息分配系数；γm为主滤波器系数。

2.2 联邦滤波器算法

设AUV的运动状态方程为

式中：xk,xk−1分别为AUV在k和k-1时刻的状态向量；Fk−1为AUV在k−1时 刻 的 状 态 转 移 矩 阵；ωk−1为AUV过程噪声矩阵，也为零均值的高斯分布，其噪声方差矩阵为Qk−1。

在卡尔曼滤波过程中，所有子滤波器与主滤波器共用一个状态方程。不同的观测系统对AUV的状态信息进行独立测量，则第i个（i=1,2,···,m）子滤波器的量测方程为

式中：zi,k为第i个 观测系统在k时刻对AUV观测得到的观测向量；Hi,k为zi,k对应的观测矩阵；xi,k为第i个滤波器采集到的AUV的状态向量；vi,k为观测的噪声矩阵。

假设AUV各子系统本身的过程噪声与观测噪声不相关，且子系统间的噪声也互不相关，则满足卡尔曼滤波的基本要求。

联邦滤波算法可分为如下几个步骤：

步骤1：信息分配。根据方差上界技术，对子滤波器与主滤波器设置的系统初始值包括误差协方差Pi,0、 过程噪声Qi,0、估计信息值以及全局最优滤波初始值Pg,0，Qg,0和，并分别表示如下：

步骤2：状态更新。将主滤波器和子滤波器分别独立进行状态更新。

步骤3：量测更新。利用子系统量测值对其相应的子滤波器进行量测更新。其中，子系统量测值表示为Ki,k，观测噪声方差表示为Ri,k，I为初始噪声矩阵。

因主滤波器无量测信息，故仅对主滤波器的状态向量信息值Pm,k和进行更新：

步骤4：信息融合。将所有子滤波器与主滤波器的估计信息进行融合，计算全局状态估计信息。

步骤5：信息重置。将 全局误差协方差Pg,k、公共过程噪声Qg,k以 及全局估计状态向量xˆg,k按照信息分配原则重置各子滤波器和主滤波器，如下式所示。

由上述步骤可见，各子滤波器和主滤波器可以同时更新状态，并进行信息融合。子滤波器的状态估计值虽不是最优的，但可以证明全局滤波估计值是最优的。图4所示为整个联邦扩展卡尔曼滤波处理流程。

图4 多速率联邦扩展卡尔曼滤波数据处理流程Fig.4 Data processing flow of federate extended Kalman filter with multi-rates

2.3 AUV运动状态方程

为方便建立AUV的运动状态方程，对坐标系进行了定义，如图5所示。建立的两种坐标系中中，导航坐标系（n系）原点O一般选在海面上的某一点，xn轴 指向正北，yn轴指向正东，zn轴垂直于地面指向地心；载体坐标系（b系）建立在AUV上，坐标系原点一般选在AUV的重心上，xb轴为指向艏向的AUV纵轴，yb轴为指向右舷的AUV横轴，zb轴垂直于AUV向下。AUV的姿态用欧拉角表示，绕xb轴 旋转的为横滚角φ ， 绕yb轴旋转的是俯仰角θ ，绕zb轴 旋转的航向角ψ。

图5 导航坐标系和载体坐标系定义Fig.5 Definitions of navigation and body coordinate system

如第1节所述，AUV对接过程中的多源数据融合引导系统在不同阶段所获量测信息不同，且引导的全过程被分为5个阶段和3种导航方式。当AUV距移动目标较远时，仅能获取INS和USBL信息，此时采用声学辅助导航方式。假定回收开始时，AUV经与移动目标协商获取移动平台目标j的初始位置（Pj,0=[xj,0yj,0zj,0]）、姿态（Θj=[φjθjψj]） 与速度（vj=[x˙jy˙jz˙j]），然后，移动目标保持定向匀速航行。此时，以东、北、天这3种导航坐标系作为参考系，选取AUV与移动目标 间 的 相 对 位 置（ ΔxkΔykΔzk）、AUV的 姿 态（ φkθkψk）、速度（x˙ky˙kz˙k）作为状态向量（共9维），也即

式中： ωk−1为 回收初始状态矩阵；Rnb(Θk−1)为k-1时刻AUV的载体坐标系向导航坐标系转换的旋转矩阵，并定义为

1）当AUV处于回收引导阶段1时，INS系统可以输出AUV的三维位置、姿态和速度，结合移动目标的初始的三维位置、姿态和速度，即可以获得相对位置的量测值。观测向量z1,k与状态向量xk的形 式 一 致，观 测矩阵 为H1,k=I9×9。对于USBL系统，其仅能获取相对位置的信息，且观测矩阵为H2,k=[I3×303×6]。此时，利用USBL测量信息来纠正INS系统发散的位置误差，而在获取USBL信息时，可计算USBL与INS两个传感器获取的位置差值，用于补偿INS系统的量测值。

2）当AUV处于回收引导阶段2和阶段3时，光学引导系统开始进入前期工作。此时，AUV距离移动目标较远，光学引导系统仅能够获得AUV相对于移动目标的横向角偏差α和纵向角偏差β ，取z′2,k=[αkβk]，并分别建立如下光学前量测方程z′2,k和 量测矩阵H2′,k。其中，h(xk)为AUV相对移动目标在k时刻时的位置函数，v2,k为相对于移动目标在k时刻时的速度函数。

4）当AUV处于回收引导阶段4时，光学引导系统开始进入后期工作。此时，光学引导系统能获得AUV相对于移动目标的相对位置xk和相对姿态信息（ Δφk，Δθk，Δψk），此时，可以扩充状态向量，在加入相对姿态信息后，AUV的状态向量（共12维）更新为x′k=[xkΔφkΔθkΔψk]，并分别建立如下光学后量测方程z′2′,k和 量测矩阵H2′′,k。

5）当AUV处于回收引导阶段5时，采取追踪灯带的方式对接。此时，光学引导系统可以量测AUV相对于移动目标的横向角偏差α与纵向角偏差β，测量方式与光学引导的前期工作相似，并分别建立如下观测向量z′2′,′k和 量 测 矩 阵H2′′,′k。

式中：n为 灯的编号，n=1,2,3,4；d为灯间距量测矩阵。

3 仿真分析

3.1 多源数据融合引导轨迹设定

为对多种航行轨迹下的数据融合导航性能进行对比，设计了3种典型的对接航行轨迹，如图6所示。本文仿真涵盖了整个AUV回收对接引导的全过程，其中，假设回收对接的初始阶段AUV在水下约20 m处航行，而移动目标则在水下约40 m处航行，AUV分别从3个方向朝向移动目标航行，起始航向角偏差分别为0°，180°和90°。

图6 AUV回收对接的3种航行轨迹Fig.6 Three trajectories of AUV docking in recovery operation

3.2 融合引导仿真

根据2.1节关于联邦卡尔曼滤波的原理，仿真前需设定参数，包括滤波器需要的初始值、噪声参数以及需要对比的真实值和量测值。例如，量测系统数据更新频率（INS解算模拟模块和光学引导模拟模块为10 Hz，USBL测量模拟模块为0.2 Hz）、仿真时间（906 s）、采样间隔（0.1 s）。其他参数具体设定如下：

1）初始的状态向量和误差协方差。

以上式中：x0，分别为前3个阶段使用的状态向量和第4，5阶段（光学引导后期和末期）使用的扩展后的状态向量；P0，P为对应的误差协方差；Q0，Q为相应的过程噪声方差初始值；R1，R2，R，R′2′，R分别为INS和USBL及光学引导的前期、后期和末期的观测噪声方差初始值。

此外，USBL和光学系统引导的量测值精度与AUV和移动目标的相对距离有关，其标准差设置为相对距离的1%。而USBL的数据更新率FUSBL一般与声波往返时间tRTT以及处理器处理时间tp有关，即

假定声学信号处理时间tp为500 ms，声速取1 500 m/s，近距离20 m的数据更新率约1.9 Hz，而远距离1.5 km的数据更新率约0.4 Hz，加上数据传输等因素造成的延迟，实际的数据更新率还会小于上述值。为便于仿真，本文数据更新率统一取为0.2 Hz，融合导航模块运行1次，获得如图7所示的相对位置轨迹。图中：绿线表示真实值的轨迹；黑线表示INS解算结果；蓝线表示INS加入USBL的估计值后，采用联邦卡尔曼滤波方法有效修正的INS积累误差；红线表示加入光学信号引导并融合后的相对位置。

通过观察图7中的轨迹可以发现，仅使用INS解算的轨迹逐渐发散，这是因为其测量器件存在常值漂移和随机误差。蓝线所示的INS加入了USBL估计值并采用联邦卡尔曼滤波方法有效修正的INS积累误差精度不够，且USBL数据更新率低。而红线所示加入了光学信号引导并融合后的相对位置得到的定位精度更高、稳定性更好，结果满足对接要求。

图7 AUV回收对接在不同轨迹下滤波估计的相对位置与真实值对比Fig.7 Comparison of filter estimated relative position and true value for different trajectories of AUV docking in recovery operation

4 结 语

本文以移动平台搭载的AUV回收对接引导过程为研究对象，通过分析其水下回收的重难点，融合获得来自INS和USBL及光学系统的多源数据，提出了一种面向移动平台的AUV水下回收对接引导系统的方案。研究中，将AUV水下回收对接过程分为5个阶段，根据不同阶段使用不同的导航方式，制定了基于扩展联邦滤波器的水下回收对接多源数据融合引导系统，得到了融合相应的导航算法。该系统结合了不同传感器系统的优点，有效克服了集中式滤波存在的状态维度高、计算量大的缺点，提高了AUV水下回收对接引导过程中的定位精度。仿真结果表明，所提方法可以满足工程要求。