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

摘要

本发明涉及一种用于微机电系统陀螺误差快速补偿的提高基于MEMS陀螺的无人艇协同导航定位精度的方法。本发明包括：建立正向滤波模型；存储正向滤波数据；逆向滤波系统模型建立；建立逆向滤波量测模型；利用建立的系统模型和量测模型；正向滤波。本发明将MEMS陀螺测量航向的误差也扩充成为了状态向量，使得滤波过程中能够更有效地补偿MEMS陀螺的误差。逆向滤波在协同导航模型中操作简单，易于实现，只需要改变速度v的符号并反向利用正向滤波时存储的数据，包括从艇航推得到的位置和主从艇的距离。由于逆向滤波可以反复利用一小段已经存储的数据，不需要采集更多的数据，从而显著提高了滤波估计的速度，而计算机计算速度较快。

主权项

一种提高基于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）建立逆向滤波量测模型式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，V_k‑N(0,R_k)为测量噪声；（5）利用建立的系统模型和量测模型，反向利用正向滤波时存储的从艇速度、航向进行时间更新，利用主从艇的距离进行量测更新，用扩展卡尔曼滤波估计从艇的位置和MEMS陀螺的误差，时间更新：量测更新：P_k＝(I‑K_kH_k)P_k/k‑1式中，为滤波输出的状态估计，P_k/k‑1为状态和及其方差一步预测，K_k为滤波增益；（6）正向滤波正向滤波回到断点。