一种多无人艇协同导航条件下陀螺漂移的滤波方法

摘要

本发明公开了一种多无人艇协同导航条件下陀螺漂移的滤波方法，该滤波方法为：装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，使用EKF算法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。本发明采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿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陀螺漂移，n表示算法执行次数，注意到项表明算法在状态一步预测中用陀螺漂移的估计值补偿了航向测量值的误差，表示成一般形式得：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)，表示w_k服从均值为0，方差为Q_k的高斯分布步骤六、建立量测方程，$Z_k=r=h\left(X_k\right)=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2}+V_k=HX+V_k$式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，V_k～N(0，R_k)为测量噪声；步骤七、用扩展卡尔曼滤波修正从艇推位误差，${\hat{X}}_{k/k-1}=F_k{\hat{X}}_{k-1}$$P_{k/k-1}=F_k{P}_{k-1}{F_k}^T+B_{k-1}{Q}_{k-1}{B}_{k-1}^T$$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$P_k＝(I‑K_kH_k)P_k/k‑1式中，为滤波输出的状态估计，P_k/k‑1为状态和及其方差一步预测，K_k为滤波增益。