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

有效

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

技術領域

本發明屬于多無人艇協同導航定位技術領域，尤其涉及一種多無人艇協同導航條件下陀螺漂移的濾波方法。

背景技術

多無人艇的協同導航是利用系統中其他艇的高精度導航信息，通過一定的信息交換，實現艇間導航資源的共享，裝備低精度導航設備的艇可以提高自身的導航精度。當某些艇由于傳感器或環境因素喪失獨立導航能力時，協同導航可以在一定程度上恢復這些平臺的導航能力。因此研究無人水面艇的協同導航具有重要的理論價值和實際意義。水聲通信可以實現對各跟隨艇的定位和編隊航行部署，其依賴性小。水聲測距方法測量范圍能夠達到2km-6km，且測距精度在1m以內。對于載體的運動狀態以及時統精度方面均沒有苛刻的要求。因此，水聲裝置廣播方式進行通信，采用水聲設備輔以無線電時統的方法進行測距。

但是，從艇的MEMS陀螺精度較低，難以同時估計初始航向偏差和陀螺漂移，使得濾波狀態只有三維的問題。

發明內容

本發明實施例的目的在于提供一種多無人艇協同導航條件下陀螺漂移的濾波方法，旨在解決從艇的MEMS陀螺精度較低，難以同時估計初始航向偏差和陀螺漂移的難題，使得濾波狀態只有三維的問題。

本發明實施例是這樣實現的，一種多無人艇協同導航條件下陀螺漂移的濾波方法，該多無人艇協同導航條件下陀螺漂移的濾波方法包括以下步驟：

步驟一，裝有高精度慣導設備的兩艘主艇交替向從艇發送加有時間戳的水聲測距信號；

步驟二，從艇利用多普勒測速儀測得的速度和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)，

步驟六，建立量測方程，