自主水下机器人回收导航多传感器融合研究

2021-11-19包灵卉曾庆军朱志宇戴晓强

计算机仿真 2021年10期

包灵卉，曾庆军，朱志宇，戴晓强

(江苏科技大学电子信息学院，江苏镇江212003)

1 引言

自主水下机器人(Autonomous Underwater Vehicle，AUV)作为探索和开发海洋的新型手段，近年来成为当今研究的重要领域。随着应用市场的日益复杂，AUV需要更长的工作时间、更强的自主性及更强的通讯能力[1]。由于AUV自身携带能源有限且水下通信速率低，所以需要及时导航定位、布放回收，以用于能源补充、数据上传或者接受新的任务[2]。

由于目前常用的水下导航设备有捷联惯性导航系统(Strapdown Inertial Navigation Systems，SINS)、多普勒里程仪(Doppler Velocity Log，DVL)及超短基线定位系统(Ultra-Short Baseline，USBL)等。以上几种传感器可以通过先进的滤波融合技术进行融合，达到AUV对接回收中导航的精度要求[3-4]。USBL定位系统配备的基阵一般尺寸小且安装简易，可以根据实际情况设计其布阵方式，来增强系统的定位精度。挪威的Kongsberg Simrad公司研发了一款名为HiPAP超短基线定位系统，定位精度可以达到0.02m，其中HiPAP102型超短基线工作水深达到了10000m。国内对USBL的技术研究较晚，主要研究机构有哈尔滨工程大学、浙江大学、中科院沈阳自动化研究所等[5-6]。最具代表性的是哈尔滨工程大学研究的USBL定位系统，2014年在“蛟龙号”上亮相，为中国的海洋事业做出巨大贡献；浙江大学流体动力与机电系统国家重点实验室针对在AUV入坞过程中喇叭状的回收坞对海豚-ⅡAUV的影响作用，进行了一种光学引导回收对接的研究；沈阳自动化研究所设计研究了回收中的对接坞，其主要对于AUV进入对接坞时的各种水动力问题展开了研究；但目前国内在AUV回收组合导航多传感器融合这方面的研究开展得较少。

文献[4]提出一种基于人工蜂群算法(ABC)改进的径向基函数(RBF)神经网络增强自适应无迹卡尔曼滤波算法(AUKF)，但是其公式复杂，在实际情况下很难实现。文献[5]采用联邦滤波技术设计了SINSUSBLDVL组合导航系统方案，提出了一种改进的自适应UKF算法，改善了滤波精度和发散问题，但是在建模中采用松组合的方式，USBL系统需经过一系列的相位差和方位角解算方可进入滤波器中进行滤波，在滤波之前就易产生不小的误差。本文为了提高AUV在回收过程中的导航定位精度，在两方面做了改进和创新。在建模方面，采用改进的基于斜距的SINSUSBLDVL多传感器融合系统方案，用基于斜距的建模方式避免了USBL系统中的基阵相位差计算的误差；在滤波算法方面，提出了自适应无迹卡尔曼滤波算法(Adaptive Unscented kalman filtering，AUKF)，以无迹卡尔曼滤波为原型构建优化问题，消除滤波的奇异值和发散问题。仿真证得基于斜距的SINSUSBLDVL多传感器融合系统和AUKF算法能够有效地降低AUV导航系统的定位误差，可以达到AUV安全回收的要求。

2 AUV回收系统原理

根据此AUV运动系统的特点对AUV对接过程分为四个阶段，如图1所示，在整个对接回收过程中，主要在前两个阶段即直线归位阶段和直线跟踪阶段采用多传感器融合的方式进行导航，在USBL作用范围，为了使AUV在耗能较少的情况下航行至中轴线上，需要时刻调整AUV与回收坞的相对位置，这就需要利用基于USBL的多传感器融合提供的相对位置/姿态信息，当AUV即将进入直线对接阶段时(距离对接口3到5米)，需要改变控制策略，即通过视觉导航提供的相对位置/姿态信息来调整 AUV 与回收坞的位置，保证AUV沿中轴线航行，且艏向角指向对接口[5]。

图1 AUV回收对接流程框图

如图2所示，USBL发射基阵6安装在回收坞2上，声单元之间的相互位置已精确测定且构成声基阵坐标系4，应答器5安装在AUV自主航行器3上；该系统通过测定水听器的相位差来计算AUV到回收坞的水平和垂直角度，在通过测定声波的传播时间来确定AUV到回收坞的相对距离1，最终得到AUV相对于换能器的位置信息。从而确定水下载体的相对位置。

图2 超短基线水下定位原理图

图3为USBL发射基阵排列图，发射基阵是由四个水听器组成的，基阵中心安装一个换能器用于发射信号。

图3 USBL发射基阵排列图

本文以江苏科技大学自主研发的“探海I型”AUV为研究对象，如图4所示。光纤惯导SINS选用中科探海(苏州)海洋科技有限责任公司定制的GIF6536A型光纤惯导，计程仪DVL选用LinkQuest公司的NavQuest 600 Micro DVL，深度计采用MICROSENSOR公司的MPM3808型I2C数字压力传感器。USBL选用的是Blueprint公司生产的SeaTrac X150 USBL超短基线定位系统。

图4 “探海I型”AUV

3 多传感器融合系统结构建模

3.1 三种传感器融合系统状态方程设计

在SINSUSBLDVL多传感器融合系统中，虽然SINS具有完全的自主导航能力，但由于陀螺存在随机漂移，容易出现发散的问题。多普勒计程仪可以从载体发射水声信号的回波中解算出载体的对地速度。利用DVL精确的速度信息，可以有效地抑制SINS导航误差的发散。在此系统中DVL的速度信息可以与SINS解算的速度信息做差作为量测值，再通过滤波器进行最优估计，从而修正导航定位信息。USBL系统一般通过直接解算时延差和斜距得到的AUV的绝对位置信息来辅助SINS系统，但USBL系统的基阵极易偏移，产生的误差将影响AUV的定位精度和稳定性[6]。本文设计了一种新的计算方法，以到达距离差和斜距之差，作为滤波器的量测值。USBL系统的发射端由多个水听器组成，通过回波到达每个水听器之间的时延差计算而得的距离差，再测得回波到达中心水听器的时间得到斜距；这是USBL定位系统得到的到达距离差和斜距。至于SINS定位系统，通过AUV的位置信息的坐标转换可得到AUV上各水听器的位置，从而可得到各水听器与回收坞上的到达距离差和斜距，分别将两系统计算的到达距离差和斜距做差，作为滤波器的量测值进行滤波融合，再由精确的到达距离差和斜距通过物理计算得到相对位置信息。综上所述设计出基于到达距离差和斜距的SINSUSBLDVL多传感器融合子系统原理框图，如图5所示。

图5 SINSUSBLDVL多传感器融合系统原理图

选择SINS的速度误差、姿态误差、位置误差、加速度计零偏和陀螺漂移作为状态变量，则SINS系统状态变量为

XSINS=[δVEδVNδVUφEφNφUδLδλ

(1)

(2)

WSINS为过程噪声矩阵。GSINS为过程噪声驱动矩阵。

FSINS可由SINS误差方程确定。

根据改进的基于斜距的USBL的误差模型，选取到达距离差和斜距误差作为状态量，USBL系统状态变量为

(3)

式中，δRUSBLX为超短基线系统的斜距误差，δRUSBL13、δRUSBL24为接收器到两个水听器之间的距离差的量测误差。

建立USBL系统状态方程为

(4)

其中，状态转移矩阵为

(5)

式中，τδR、τδΔR13、τδΔR24分别为一阶马尔科夫过程的相关时间，即接收器到中心换能器的时间、接收器到1、3水听器的时间差和接收器到2、4水听器的时间差。

根据DVL的误差模型，选取东北天向速度误差作为状态量，则DVL状态向量为

(6)

其中，δVEDVL，δVNDVL，δVUDVL是DVL的东向，北向，天向测速误差，均假设为白噪声。

建立DVL系统状态方程为

(7)

其中，状态转移矩阵为

(8)

综上所述可得SINSUSBLDVL系统状态方程为

(9)

3.2 三种传感器融合系统量测方程设计

在SINSUSBLDVL多传感器融合系统中，将DVL的速度信息及USBL的位置信息分别与SINS的相应信息相减得到量测量。量测量ZSINS/USBL/DVL可进一步表示为

(10)

但为了避免系统由坐标转换和基阵偏移带来的误差，本文设计了新的组合模型，即将SINS和USBL的到达距离差和斜距之差作为其中量测量，由改进的基于斜距的多传感器融合的原理，量测量ZSINS/USBL/DVL可进一步表示为

(11)

由此可以得出SINSUSBLDVL系统的量测方程为

(12)

其中，量测矩阵为

(13)

(14)

其中，δVED、δVND、δVUD为DVL的东北天向测速误差，δR、δR13、δR24为USBL的斜距和到达距离差的误差。

3.3 多传感器融合中AUKF算法

一般来说，Kalman滤波是状态估计中相对简单实用的方法，但必须在随机动态系统的结构参数和噪声统计特性参数都已知的情况下才能发挥作用。自适应滤波算法可以在进行状态估计的同时，通过量测输出在线实时地估计系统的噪声参数。虽然自适应滤波算法解决了随机系统的模型误差的影响，但是由于水下环境复杂，且USBL系统易收到水下其余杂质声波的影响，易产生奇异值，针对此问题，提出了一种AUKF算法，步骤如下

1)初始化

(15)

2)噪声估计

3)时间更新

(16)

一步预测误差协方差矩阵

(17)

和量测一步预测

(18)

4)量测更新

计算K时刻系统状态估计值k和误差协方差矩阵Pk，其中

(19)

4 SINSUSBLDVL多传感器融合系统仿真验证

为了验证所建立的基于斜距的SINSUSBLDVL多传感器融合系统的误差模型以及所提出的AUKF算法的性能，在MATLAB环境下进行仿真，仿真系统采用AUV真实导航传感器误差数据生成动态数据的方式。首先建立了SINSUSBLDVL多传感器融合系统(模型1)和基于斜距的SINSUSBLDVL多传感器融合系统(模型2)的仿真平台，其次在基于斜距的模型中设计了自适应卡尔曼滤波算法(算法1)和AUKF算法(算法2)，并进行比较。仿真参数如表1所示。

表1 导航误差参数

设置一条AUV运动的轨迹曲线，如图6所示，包括平直前行、转弯、上升、下滑等过程。初始位置(120°，31°)，系统仿真时间为600s，仿真结果如图7和图8所示。

图6 AUV轨迹仿真

图7 有无基于斜距的多传感器融合模型滤波误差输出结果对比

图8 基于斜距的多传感器融合模型滤波算法误差对比

图7为有无基于斜距的SINSUSBLDVL多传感器融合系统输出的误差对比图，即红线代表的是基于斜距的SINSUSBLDVL多传感器融合系统的速度和位置误差，可以看出采用基于斜距的方法建模，避免了位置解算中基阵偏移所带来的误差，速度误差降低了60%左右，经纬度误差更是从3m降低到0.5m之内。

图8为基于斜距的SINSUSBLDVL多传感器融合系统在自适应卡尔曼滤波算法和AUKF算法上速度与位置的误差曲线，可以看出，AUKF算法比自适应卡尔曼滤波的效果更好，减少了滤波的奇异值与发散，滤波精度及稳定性都得到了提高。其原因主要在于，AUKF算法在进行状态估计的同时还可以通过量测输出在线实时地估计系统的噪声参数，更好地降低了模型误差，抑制了滤波的发散。