一種多無人艇協同導航條件下陀螺漂移的濾波方法
2023-08-03 09:16:06 1
一種多無人艇協同導航條件下陀螺漂移的濾波方法
【專利摘要】本發明公開了一種多無人艇協同導航條件下陀螺漂移的濾波方法，該濾波方法為：裝有高精度慣導設備的兩艘主艇交替向從艇發送加有時間戳的水聲測距信號，從艇利用都卜勒測速儀測得的速度和MEMS陀螺測得的航向進行航跡推算，並通過水聲信號發送和接收的時間差乘以聲速計算主從艇的距離，使用EKF算法對從艇的位置進行更新和修正，估計並補償陀螺漂移。本發明採用擴展卡爾曼濾波方法對自身的航跡推位進行修正，估計並補償MEMS陀螺初始航向偏差和漂移，提高了定位精度；為了提高可觀測性，考慮兩艘主艇交替向從艇發送測距信息，採用同一個狀態先估計初始航向偏差，再徹底消除航向偏差並估計陀螺漂移，收到了很好濾波效果。
【專利說明】一種多無人艇協同導航條件下陀螺漂移的濾波方法
【技術領域】
[0001]本發明屬於多無人艇協同導航定位【技術領域】，尤其涉及一種多無人艇協同導航條件下陀螺漂移的濾波方法。
【背景技術】
[0002]多無人艇的協同導航是利用系統中其他艇的高精度導航信息，通過一定的信息交換，實現艇間導航資源的共享，裝備低精度導航設備的艇可以提高自身的導航精度。當某些艇由於傳感器或環境因素喪失獨立導航能力時，協同導航可以在一定程度上恢復這些平臺的導航能力。因此研究無人水面艇的協同導航具有重要的理論價值和實際意義。水聲通信可以實現對各跟隨艇的定位和編隊航行部署，其依賴性小。水聲測距方法測量範圍能夠達到2km-6km，且測距精度在Im以內。對於載體的運動狀態以及時統精度方面均沒有苛刻的要求。因此，水聲裝置廣播方式進行通信，採用水聲設備輔以無線電時統的方法進行測距。
[0003]但是，從艇的MEMS陀螺精度較低，難以同時估計初始航向偏差和陀螺漂移，使得濾波狀態只有三維的問題。
【發明內容】
[0004]本發明實施例的目的在於提供一種多無人艇協同導航條件下陀螺漂移的濾波方法，旨在解決從艇的MEMS陀螺精度較低，難以同時估計初始航向偏差和陀螺漂移的難題，使得濾波狀態只有三維的問題。
[0005]本發明實施例是這樣實現的，一種多無人艇協同導航條件下陀螺漂移的濾波方法，該多無人艇協同導航條件下陀螺漂移的濾波方法包括以下步驟:`[0006]步驟一，裝有高精度慣導設備的兩艘主艇交替向從艇發送加有時間戳的水聲測距信號;
[0007]步驟二，從艇利用都卜勒測速儀測得的速度和MEMS陀螺測得的航向進行航跡推算，並通過水聲信號發送和接收的時間差乘以聲速計算主從艇的距離；
[0008]步驟二，估計初始航向偏差
[0009]在採用卡爾曼濾波方法估計MEMS陀螺初始航向偏差的這段時間裡，陀螺漂移的影響小到可以忽略不計的地步，所以陀螺漂移先不作為系統狀態，設系統狀態Xk=[Xk ykX(3)]T, xk、yk表示從艇在k時刻的位置，先在h秒內準確估計出初始航向偏差Δφ，t0秒後轉而估計陀螺漂移ε，即
[0010]
Δφ (? <?0)
^(3) = { Ψ
[0011]步驟四，補償初始航向偏差
[0012]在MEMS陀螺測得的航向中完全消除估計出的初始航向偏差，即
[0013]
【權利要求】
1.一種多無人艇協同導航條件下陀螺漂移的濾波方法，其特徵在於，該多無人艇協同導航條件下陀螺漂移的濾波方法包括以下步驟: 步驟一，裝有慣導設備的兩艘主艇交替向從艇發送加有時間戳的水聲測距信號； 步驟二，從艇利用都卜勒測速儀測得的速度和MEMS陀螺測得的航向進行航跡推算，並通過水聲信號發送和接收的時間差乘以聲速計算主從艇的距離； 步驟二，估計初始航向偏差: 在採用卡爾曼濾波方法估計MEMS陀螺初始航向偏差的這段時間裡，陀螺漂移的影響小到可以忽略不計的地步，所以陀螺漂移先不作為系統狀態，設系統狀態Xk=[xk yk X (3)]T, xk、yk表示從艇在k時刻的位置，先在h秒內準確估計出初始航向偏差Δφ，t0秒後轉而估計陀螺漂移ε，即
2.如權利要求1所述的多無人艇協同導航條件下陀螺漂移的濾波方法，其特徵在於，在步驟一中，兩輛主艇上分別裝有慣導設備和水聲通信模塊，兩主艇將水聲通信信號和發送時間戳交替發送給從艇，間隔10秒。
3.如權利要求1所述的多無人艇協同導航條件下陀螺漂移的濾波方法，其特徵在於，在步驟二中，通過水聲信號發送和接收的時間差乘以聲速計算主從艇的距離，利用測量的速度和航向進行航跡推算，並利用主從艇的距離作為觀測量修正位置信息並估計陀螺漂移。
【文檔編號】G01C21/20GK103697892SQ201310725949
【公開日】2014年4月2日 申請日期:2013年12月26日 優先權日:2013年12月26日
【發明者】徐博, 金辰, 劉楊, 董海波, 單為, 邱立民, 白金磊, 易楚偉, 張廣拓 申請人:哈爾濱工程大學