[發明專利]一種多無人艇協同導航條件下陀螺漂移的濾波方法

有效

• 申請號: 201310725949.3
• 申請日: 2013-12-26
• 公開（公告）號: CN103697892B
• 公開（公告）日: 2017-01-11
• 發明（設計）人: 徐博;金辰;劉楊;董海波;單為;邱立民;白金磊;易楚偉;張廣拓
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

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陀螺漂移，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)，

步驟六，建立量測方程：

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

式中，觀測量Z_k表示主從艇的距離r，x_a、y_a表示主艇的位置，x_b、y_b表示從艇位置，H=∂h∂X=(xb-xa)/r(yb-ya)/r0,]]>V_k～N(0，R_k)為測量噪聲；

步驟七，用擴展卡爾曼濾波修正從艇推位誤差：

X^k/k-1=FkX^k-1]]>

Pk/k-1=FkPk-1FkT+Bk-1Qk-1Bk-1T]]>

Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1]]>

X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)]]>

P_k=(I-K_kH_k)P_k/k-1

式中，Pk=E[(Xk-X^k)(Xk-X^k)T],]]>為濾波輸出的狀態估計，為狀態和方差一步預測，K_k為濾波增益。