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

Abstract

本发明公开了一种多无人艇协同导航条件下陀螺漂移的滤波方法，该滤波方法为：装有高精度惯导设备的两艘主艇交替向从艇发送加有时间戳的水声测距信号，从艇利用多普勒测速仪测得的速度和MEMS陀螺测得的航向进行航迹推算，并通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，使用EKF算法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。本发明采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿MEMS陀螺初始航向偏差和漂移，提高了定位精度；为了提高可观测性，考虑两艘主艇交替向从艇发送测距信息，采用同一个状态先估计初始航向偏差，再彻底消除航向偏差并估计陀螺漂移，收到了很好滤波效果。

Description

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

技术领域

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

背景技术

多无人艇的协同导航是利用系统中其他艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，装备低精度导航设备的艇可以提高自身的导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时，协同导航可以在一定程度上恢复这些平台的导航能力。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。水声通信可以实现对各跟随艇的定位和编队航行部署，其依赖性小。水声测距方法测量范围能够达到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)，

步骤六，建立量测方程，

$Z_k=r=h\left(X_k\right)=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2}+V_k=\mathrm{HX}+V_k$

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置， $H=\frac{\∂h}{\∂X}=\left[\begin{array}{ccc}(x_b-x_a)/r& (y_b-y_a)/r& 0\end{array}\right],$ 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=E\left[(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right],$ 为滤波输出的状态估计，为状态和及其方差一步预测，K_k为滤波增益。

进一步，在步骤一中，两辆主艇上分别装有高精度惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒。

进一步，在步骤二中，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移。

本发明提供的多无人艇协同导航条件下陀螺漂移的滤波方法，通过采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿MEMS陀螺初始航向偏差和漂移，提高了定位精度；为了提高可观测性，考虑了两艘主艇交替向从艇发送测距信息，将航向偏差和陀螺漂移列为系统方程的两个状态难以用EKF同时估计和补偿，采用同一个状态先估计初始航向偏差，再彻底消除航向偏差并估计陀螺漂移，收到了很好的滤波效果。

附图说明

图1是本发明实施例提供的多无人艇协同导航条件下陀螺漂移的滤波方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图及具体实施例对本发明的应用原理作进一步描述。

如图1所示，本发明实施例的多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：

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

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

S103：使用卡尔曼滤波方法对从艇的位置进行更新和修正，估计并补偿陀螺漂移。

本发明的具体步骤为：

方案为：两条主艇广播水声信号

步骤一，系统安装与配置

两辆主艇上分别装有高精度惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒，从艇上的多普勒测速仪给出速度大小，MEMS陀螺给出航向，从艇上的水声通信模块接收主艇发来的测距信息，利用艇上自带的电脑进行航迹推算和滤波，计算所在经纬度位置并估计陀螺漂移；

步骤二，实验具体实施方法

将所有相关设备安装在从艇上，读取MEMS陀螺输出航向作为静态数据并保存，一小时后，主艇设备安装完毕，开始实验，三艇在水面上行驶时保持三角队形，提高从艇测距时的可观测性，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒，从艇上的水声通信模块接收主艇发来的测距信息，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移；

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

在采用EKF估计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)，

步骤六，建立量测方程，

$Z_k=r=h\left(X_k\right)=\sqrt{{(x_a-x_b)}^2+{(y_a-y_b)}^2}+V_k=\mathrm{HX}+V_k$

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置， $H=\frac{\∂h}{\∂X}=\left[\begin{array}{ccc}(x_b-x_a)/r& (y_b-y_a)/r& 0\end{array}\right],$ 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=E\left[(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right],$ 为滤波输出的状态估计，为状态和及其方差一步预测，K_k为滤波增益。

本发明提供的协同导航实验方法具有以下优点：

一、由于主艇无线电广播位置信息，从艇被动接受主艇发射的无线电信号，所以从艇的数量可以任意增加，不会影响实验步骤的设计，所以本发明的扩展性很好，用两主艇和一从艇可以模拟多从艇的情形。

二、从艇安装低精度惯导设备(如MEMS)进行推位，并通过测量和主艇的距离利用主艇的高精度惯导设备提高定位精度，大大降低了设备的成本。

三、使用同一个状态先估计初始航向偏差，再估计陀螺漂移，解决了难以同时估计初始航向偏差和陀螺漂移的难题，使得滤波状态只有三维(另外两维是横纵坐标)，计算简单，收到了很好的滤波效果。

为了进一步说明上述滤波算法的有效性，在水面进行了三艇实验，采用两主艇和一从艇通信的方法，MEMS航向偏差为10度，陀螺漂移为每小时10度，采集数据并绘制轨迹

通过实验分析比较可得：由于修正了初始航向偏差和陀螺漂移，EKF滤波的精度比航推好的多，定位误差在100米以内，达到了要求，滤波得到的从艇轨迹与真实轨迹几乎完全吻合，而用低精度MEMS和多普勒测速仪推位得到的轨迹和真实轨迹有很大偏差。在准确估计了初始航向偏差后，通过将相应状态清零并重新设置方差阵，在陀螺漂移和初始航向偏差数量级差别很大的情况下也准确的估计出了陀螺漂移，收到了较好的滤波效果。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，该多无人艇协同导航条件下陀螺漂移的滤波方法包括以下步骤：

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

步骤二，从艇利用多普勒测速仪测得的速度和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为滤波增益。

2.如权利要求1所述的多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，在步骤一中，两辆主艇上分别装有高精度惯导设备和水声通信模块，两主艇将水声通信信号和发送时间戳交替发送给从艇，间隔10秒。

3.如权利要求1所述的多无人艇协同导航条件下陀螺漂移的滤波方法，其特征在于，在步骤二中，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离，利用测量的速度和航向进行航迹推算，并利用主从艇的距离作为观测量修正位置信息并估计陀螺漂移。