[發明專利]一種提高基于MEMS陀螺的無人艇協同導航定位精度的方法

在審

• 申請號: 201410216171.8
• 申請日: 2014-05-21
• 公開（公告）號: CN103968842A
• 公開（公告）日: 2014-08-06
• 發明（設計）人: 徐博;金辰;劉楊;董海波;邱立民;賀浩;高偉;白金磊;單為
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種提高基于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）正向濾波

正向濾波回到斷點。