无人水下航行器 (unmanned underwater vehicle,UUV)在执行完水下作业任务之后,或在自身数据及能量传输等需求下,需要利用母船进行回收。UUV自身的定位以及相对母船船坞的定位是保证有效回收的关键因素。UUV在水下作业时只能依靠捷联惯性导航(strapdown inertial navigation system,SINS)和航位推算(dead reckoning,DR)等传统方法。然而,随着航行距离和时间的增加,导航定位误差会逐渐发散[1]。近年来利用水下应答器定位(underwater transponder positioning,UTP)方法辅助UUV水下导航取得了很大的进展[2-3]。国内外学者进行的大量仿真以及试验研究表明,UUV通过与已知坐标的水下应答器相对定位实现自身的导航定位,可有效地提高UUV水下导航定位精度[2-6]。利用应答器对UUV进行定位可有效提高定位精度,有利于保证安全可靠回收。A.J.Plueddemann等[7]于2010年3月利用UUV在巴罗海岸进行了为期8天的冰下环境观测,用于估计冬季楚克其海域流向北冰洋的海流密度、盐度等环境信息。采用低频应答器辅助航行器导航和通信,利用超短基线声学导航实现航行器的回收。实验取得了成功。J. Vaganay等[8]将一个应答器布放在船坞附近,通过对UUV自身位移以及UUV与应答器之间相对距离的测量,实现对母船回收UUV的定位,最终完成UUV的回收任务。水声定位虽然可以取得较高的定位精度,并且不存在定位误差的累积发散问题,但是,当UUV与应答器相对距离较近时,水声定位信号会受到严重干扰。对于回收对接过程定位精度要求较高的情况下,水下视觉定位(vision positioning,VP)是理想的选择。文献[9]研究了无人水下航行器水下回收视觉导引定位技术,对回收策略,定位原理以及图像处理方法进行了研究。虽然水下视觉具有极高的定位精度,然而由于水下光源的衰减比较快,可视距离受限,水下视觉定位只能用于近距离的定位。文献[10]研究了短基线和水下视觉定位融合算法,获得了很好的定位效果。然而,对于多种传感器定位数据的融合仅采用时域的集中式融合方法,未能进行时频分析和更加充分的融合,未考虑数据采集的异步性和采样频率的不同。
本文主要研究了UUV回收过程中利用水下应答器定位与视觉定位辅助捷联惯导系统,以实现对UUV的较精确定位。为进一步提高多传感器数据融合效果,采用异步加权多尺度贯序Kalman滤波器进行融合滤波,并仿真验证了UUV回收定位效果。
1 UTP定位原理图 1所示为UTP定位示意图。
在母船船坞附近安装应答器,UUV在执行作业任务中航行到应答器作用区域,UUV所搭载的换能器向母船船坞附近的应答器发射讯问脉冲信号,应答器收到UUV向其发送的讯问信号之后,向UUV发射相应的应答信号。根据渡越时间,结合UUV自身的运动信息,可以解算出应答器与UUV的相对坐标。
2 视觉定位原理UUV回收视觉定位需要在母船船坞确定一组定位点,以利于图像特征的提取。通常在母船船坞的特定位置布放一组光源信号。对于坐底回收的方式,水下相机安装于UUV重心的正下方,摄像机坐标系与运动坐标系各轴向平行,利于解算。在船坞底部十字形排列一组光源。由于水下光源组的位置固定已知,这样利用单个摄像机即可实现定位。视觉定位示意如图 2所示。
3 SINS/DVL/UTP/VP组合导航系统误差方程将捷联惯导(SINS)、多普勒(DVL)、水下信标(UTP)、视觉定位(VP)进行优化组合构成组合导航系统有利于导航精度的提高。因而建立组合导航系统误差方程。
3.1 系统状态误差方程式中:
其中:
x为状态向量:
式中:φn为姿态误差角,ΔVn为速度误差,ΔP为位置误差,εbb为运动坐标系下陀螺漂移标定误差,εrb为陀螺一阶马尔可夫随机噪声,Tg为相关时间,▽ab为加速度计偏置误差,建模为相关时间为Ta的一阶马尔可夫过程,ΔvD为DVL速度偏移误差,建模为相关时间为TD的一阶马尔可夫过程,ΔKD为DVL刻度系数误差。
w为系统噪声矩阵:
系统噪声方差矩阵为
3.2 SINS/DVL/UTP/VP组合导航定位系统观测方程1) 将SINS系统测得速度与DVL测得速度之差作为观测量有
式中:
观测噪声:
观测噪声方差矩阵:
2) 将SINS系统测得位置与UTP测得位置之差作为观测量,并将经纬度量纲转化成米有
式中:
3) 将SINS系统测得位置与VP测得位置之差作为观测量,并将经纬度量纲转化成米有
式中:C3=[03×6 Λ 03×15,其中Λ=daig{RM+h(RN+h)cos L 1};n3=[NN NE Nξ]T表示UTP系统测量误差,其方差阵为
4 异步加权多尺度贯序Kalman滤波对于单个动态模型与多个不同采样率的测量模型组成的系统,有动态模型:
式中:x∈Rn×1,A∈Rn×n,w∈Rn×1,T0表示系统建模周期,k表示离散时间量,w(kT0)是白噪声序列。
假设系统有N0个独立量测的传感器,则有量测模型:
式中:i=1,2,…,N0;zi∈Rdi×1;Ci∈Rdi×n;ni∈Rdi×1;Ti表示传感器i的采样周期,且有Ti=τiT0(i=1,2,…,N0;τi为正整数);zi(kTi)表示第i个传感器在kTi时刻的量测向量,量测噪声ni(kTi)是一个白噪声序列:
式中i,j=1,2,…,N0;Ri(kTi)>0。
假设w,ni,x0相互统计独立,则
式中j=1,2,…;k=1,2,…;i=1,2,…,N0
分割状态向量为M=2N-1的数据块有
式中m=1,2,…。
状态块X(m+1)的第s个元素为
式中:
过程噪声块向量有
式中:
从而有
(1) |
式中:
其中噪声协方差矩阵为
式中:m=1,2,…; m′=1,2,…;
尺度i+1上的平滑信号Xi+1,V(m)向尺度i分解,得到尺度i上的平滑信号Xi,V(m)和细节信号Xi,D(m)有
(2) |
多尺度重构变换为
记为
则可将式(2)的多尺度变换表示为
多尺度重构变换为
式中
其中:
LX是将X(m)变换成适合小波变换形式的线性算子,例如对于X=(x11,x12),(x21,x22)T有
将WX作用于式(1)两边有
式中:
且有
假设T1=T0,n1=1。对于任意时刻k,有
当传感器i在k时刻有测量值,则
(3) |
记作
则式(3)整理为
整理为多尺度模型:
假设m≠1,则第m块的状态可由第(m-1)块的估计值进行预测,有
对于每一数据块,依次用Z(m,1),Z(m,2),…,Z(m,M)对时刻点s=1,2,…,M的数据进行更新。
当s=1时,利用测量值z1(m,1)更新
式中:
利用模值函数依次判断各传感器是否有测量值:
式中i=2,3,…,N。
若有测量值则更新系统:
式中:
进行加权融合得
式中:
其中[·]Λ表示取对角矩阵。
如果传感器i无测量值,则
当所有传感器的测量值都用于更新之后,有
在有测量值时:
在无测量值时:
当s≠1时,依次利用z1(m,s),z2(m,s),…,zN0(m,s)进行更新,首先利用第1个传感器的测量值z1(m,s)更新,有
其中:
然后依次判断其他传感器,如有测量值,则更新系统,有
式中:
进行加权融合:
式中:
其中[·]Λ表示取对角矩阵。
如果传感器i此时无观测值,则有:
当数据块的所有观测值均用于状态估计之后,则有:
对结果进行逆小波变换最终得到X(m)估计值。
5 SINS/DVL/UTP/VP组合导航系统仿真验证假设UUV初始时刻静止于北纬2.05 °、东经18.78 °、深度0 m;初始艏向角为北偏东45 °,横摇角和纵摇角均为0 °;无初始位置、姿态、速度误差。εbb=0.02 °/h;Tg=0.5 h;Ta=0.5 h;σg2=(0.04°/h)2;σr2=(0.01 °/h)2;σa2=(0.000 1 g)2;ΔKD=0.005;TD=1/12 h;σD2=2×(0.1 m/s)2/1 800;σΔ2=(0.01 m/s)2;σn2=(5 m)2;SINS采样时间0.1 s;DVL采样时间0.5 s;UTP采样时间1 s;VP采样时间1 s;采用Haar小波进行多尺度变换。UUV从静止开始加速到2 m/s然后下潜,折线运动,仿真时长20 000 s。UUV航行轨迹如图 3所示。
在UUV执行任务的初始阶段,SINS和DVL起作用。在第18 000 s时刻,UUV进入UTP作用区域;在第19 990 s时刻,UUV进入VP作用区域;在第19 995 s时刻,UUV舍弃UTP测量数据。导航误差如图 4所示,图 5所示为局部放大图。
均方根误差(root mean square error,RMSE)为
式中:(x,y,z)为观测坐标、
由图 4、5以及表 1可见单纯依靠SINS和DVL导航的情况下,由于导航误差的累积使得导航误差随着航行距离和时间的增长而加大。在第1 800 s时UUV进入UTP作用区域,可有效地校准导航误差,有利于UUV准确确定自身位置,并安全有效地接近母船船坞。在UUV进入VP作用范围后,进一步获得更高的定位精度,有利于UUV安全的坐底船坞。
6 结束语本文针对UUV回收过程中的导航定位问题,研究了如何采用水下应答器定位和水下视觉定位辅助捷联惯导设备实现UUV的精确定位,提出的异步加权多尺度贯序Kalman滤波器算法,解决了各传感器采样不同步,频率不一致的问题,有效地实现了各传感器的数据融合。通过仿真验证,本研究有利于UUV准确确定自身位置,并安全有效地接近母船船坞、实现安全的坐底船坞等回收过程,具有一定的实用意义。但对UUV需要较多的导航定位设备的配置,增加了UUV的成本。
[1] | 边信黔, 周佳加, 严浙平, 等. 基于EKF的无人潜航器航位推算算法[J]. 华中科技大学学报:自然科学版,2011, 39 (3) : 100 –104. |
[2] | DJAPIC V, NAD D, FERRI G, et al. Novel method for underwater navigation aiding using a companion underwater robot as a guiding platforms[C]//Proceedings of 2013 MTS/IEEE OCEANS-Bergen. Bergen, Norway, 2013:1-10. |
[3] | HEGENAS Y, KENNKETH G, KENT H O, et al. Underwater transponder positioning and navigation of autonomous underwater vehicles[C]//Proceedings of 2009 Oceans. Biloxi, USA, 2009:1-7. |
[4] | MORGADO M, OLIVEIRA P, SILVESTRE C. Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles:an experimental validation[J]. Journal of field robotics,2013, 30 (1) : 142 –170. |
[5] | GENG Yanrui, SOUSA J. Hybrid derivative-free EKF for USBL/INS tightly-coupled integration in AUV[C]//Proceedings of International Conference on Autonomous and Intelligent Systems. Povoa de Varzim, Portugal, 2010:1-6. |
[6] | RIDAO P, RIBAS D, HERNÀNDEZ E, et al. USBL/DVL navigation through delayed position fixes[C]//Proceedings of IEEE International Conference on Robotics and Automation. Shanghai, China, 2011:2344-2349. |
[7] | PLUEDDEMANN A J, KUKULYA A L, STOKEY R, et al. Autonomous underwater vehicle operations beneath coastal sea ice[J]. IEEE/ASME transactions on mechatronics,2012, 17 (1) : 54 –64. |
[8] | VAGANAY J, BACCOU P, JOUVENCEL B. Homing by acoustic ranging to a single beacon[C]//Proceedings of MTS/IEEE Conference and Exhibition OCEANS. Providence, USA, 2000:1457-1462. |
[9] | 王晓娟. 基于视觉的AUV水下回收导引定位技术研究[D]. 哈尔滨:哈尔滨工程大学, 2010:6-8. |
[10] | 黎南, 湛鑫, 陈涛, 等. UUV水下回收中的视觉和短基线定位融合[J]. 智能系统学报,2013, 8 (2) : 156 –161. |