[发明专利]一种提高基于MEMS陀螺的无人艇协同导航定位精度的方法

说明书

技术领域

本发明涉及一种用于微机电系统(micro-electro-mechanical system,MEMS)陀螺误差快速补偿的提高基于MEMS陀螺的无人艇协同导航定位精度的方法。

背景技术

无人水面艇是一种在水面行驶，可以由人进行遥控操作或者自主操作的水面船艇。随着对海洋开发认识的加深，无人水面艇以其机动性强、成本低等优势成为研究热点。多无人艇的协同导航是利用系统中主艇的高精度导航信息，通过一定的信息交换，实现艇间导航资源的共享，使装备低精度导航设备的从艇可以提高自身的导航精度。因此研究无人水面艇的协同导航具有重要的理论价值和实际意义。

GPS虽然能提供精确的位置信息，并能定时校正从艇惯导误差，但容易受到地理条件和人为干扰。与之相比，运用发送时间同步的水声数据可以算出主从艇的相对距离，这一信息可以协助从艇进行导航定位。从艇上运用由扩展卡尔曼滤波（EKF）融合测得的距离和位置信息估计来减小估计误差。

近年来,MEMS陀螺作为惯性领域一个十分重要的分支,获得了长足的发展。由于其成本低、尺寸小、重量轻、可靠性高等优点,在低成本惯性系统中得到越来越多的应用，在协同导航中可以作为从艇惯性导航设备进行装备。受MEMS陀螺目前精度较低的限制,在使用前,必须要建立合理的漂移误差模型,从而对陀螺误差进行估算和补偿。为了解决陀螺误差的可观测性问题，可以先估计陀螺初始航向偏差，再估计陀螺漂移。采用逆向滤波方法来估计陀螺误差能大大缩短估计时间，提高定位精度。

发明内容

本发明的目的在于提供一种反向利用存储的数据进行逆向滤波，来缩短MEME系统误差的估计时间，进而提高协同导航定位精度的提高基于MEMS陀螺的无人艇协同导航定位精度的方法。

本发明的目的是这样实现的：

（1）建立正向滤波模型：

从艇上的水声通信模块接收主艇发来的信息，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离作为观测量，利用多普勒测速仪测量的速度和MEMS陀螺测出的航向，通过扩展卡尔曼滤波估计从艇的位置和MEMS陀螺的误差

状态一步预测方程为：

式中，x_k、y_k表示从艇在k时刻的位置，v_k为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示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)，量测方程为

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，V_k～N(0,R_k)为量测噪声；

（2）存储正向滤波数据

在滤波的同时存储所测量的从艇速度、航向和主从艇的距离；

（3）逆向滤波系统模型建立

逆向滤波相比正向滤波速度等值反向，即令建立状态一步预测模型：

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)；

（4）建立逆向滤波量测模型