[发明专利]一种多无人艇协同导航条件下陀螺漂移的滤波方法

说明书

技术领域

本发明属于多无人艇协同导航定位技术领域，尤其涉及一种多无人艇协同导航条件下陀螺漂移的滤波方法。

背景技术

多无人艇的协同导航是利用系统中其他艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身的导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些平台的导航能力。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。水声通信可以实现对各跟随艇的定位和编队航行部署，其依赖性小。水声测距方法测量范围能够达到2km-6km，且测距精度在1m以内。对于载体的运动状态以及时统精度方面均没有苛刻的要求。因此，水声装置广播方式进行通信，采用水声设备辅以无线电时统的方法进行测距。

但是，从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移，使得滤波状态只有三维的问题。

发明内容

本发明实施例的目的在于提供一种多无人艇协同导航条件下陀螺漂移的滤波方法，旨在解决从艇的MEMS陀螺精度较低，难以同时估计初始航向偏差和陀螺漂移的难题，使得滤波状态只有三维的问题。

本发明实施例是这样实现的，一种多无人艇协同导航条件下陀螺漂移的滤波方法，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：

步骤一，装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号；

步骤二，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离；

步骤三，估计初始航向偏差

在采用卡尔曼滤波方法估计MEMS陀螺初始航向偏差的这段时间里，陀螺漂移的影响小到可以忽略不计的地步，所以陀螺漂移先不作为系统状态，设系统状态X_k=[x_k y_k X(3)]^T，x_k、y_k表示从艇在k时刻的位置，先在t₀秒内准确估计出初始航向偏差t₀秒后转而估计陀螺漂移ε，即

步骤四，补偿初始航向偏差

在MEMS陀螺测得的航向中完全消除估计出的初始航向偏差，即

式中表示MEMS陀螺测得的航向，在以后的航向测量值中不再包含初始航向偏差，只有陀螺漂移，在t₀后，令X(3)=0，并重新设置系统噪声方差阵，为估计陀螺漂移做好准备；

步骤五，估计并补偿陀螺漂移

式中v_k为从艇速度，t表示推位时间间隔，ε表示MEMS陀螺漂移，k表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：

X_k+1=f(X_k，u_k，t)+w_k

线性化后得，

X_k+1=F_kX_k+B_ku_k+w_k

式中，X_k=[x_k y_k ε]^T，系统噪声w_k～N(0，Q_k)，

步骤六，建立量测方程，