[发明专利]一种估计初始航向偏差滤波的方法

说明书

技术领域

本发明属于多无人艇协同导航定位技术领域，尤其涉及一种估计初始航向偏差滤波的方法。

背景技术

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

从艇的惯导设备精度较低，通过测量和装备有高精度惯导设备的主艇的距离，采用扩展卡尔曼滤波方法对自身的航迹推位进行修正，估计并补偿初始航向偏差，提高定位精度。由于MEMS陀螺无法进行初始对准，初始航向偏差是随机的。为了提高可观测性，考虑两艘主艇交替向从艇发送测距信息。

到目前为止，还没有将航向偏差作为状态并将主从艇距离作为观测量进行卡尔曼滤波的先例。

发明内容

本发明实施例的目的在于提供一种估计初始航向偏差滤波的方法，旨在解决目前还没有将航向偏差作为状态并将主从艇距离作为观测量进行卡尔曼滤波的问题。

本发明实施例是这样实现的，一种估计初始航向偏差滤波的方法，该估计初始航向偏差滤波的方法包括以下步骤：

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

步骤二、建立系统方程

式中，x_k、y_k表示从艇在k时刻的位置，v_k为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示初始航向偏差，在方程中估计值修正了航向测量值的偏差，表示成：

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

线性化后得，

X_k+1＝F_kX_k+B_ku_k+w_k

式中，系统噪声w_k～N(0，Q_k)，

步骤三、建立量测方程：

Zk=r=h(Xk)=(xa-xb)2+(ya-yb)2+Vk=HX+Vk]]>