[發(fā)明專利]一種估計初始航向偏差濾波的方法

在審

• 申請?zhí)枺?/td>: 201310713137.7
• 申請日: 2013-12-23
• 公開（公告）號: CN103712625A
• 公開（公告）日: 2014-04-09
• 發(fā)明（設(shè)計）人: 徐博;金辰;劉楊;單為;邱立民;董海波;白金磊;陶冠時;易楚偉
• 申請（專利權(quán)）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

技術(shù)領(lǐng)域

本發(fā)明屬于多無人艇協(xié)同導航定位技術(shù)領(lǐng)域，尤其涉及一種估計初始航向偏差濾波的方法。

背景技術(shù)

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

從艇的慣導設(shè)備精度較低，通過測量和裝備有高精度慣導設(shè)備的主艇的距離，采用擴展卡爾曼濾波方法對自身的航跡推位進行修正，估計并補償初始航向偏差，提高定位精度。由于MEMS陀螺無法進行初始對準，初始航向偏差是隨機的。為了提高可觀測性，考慮兩艘主艇交替向從艇發(fā)送測距信息。

到目前為止，還沒有將航向偏差作為狀態(tài)并將主從艇距離作為觀測量進行卡爾曼濾波的先例。

發(fā)明內(nèi)容

本發(fā)明實施例的目的在于提供一種估計初始航向偏差濾波的方法，旨在解決目前還沒有將航向偏差作為狀態(tài)并將主從艇距離作為觀測量進行卡爾曼濾波的問題。

本發(fā)明實施例是這樣實現(xiàn)的，一種估計初始航向偏差濾波的方法，該估計初始航向偏差濾波的方法包括以下步驟：

步驟一、將所有相關(guān)設(shè)備安裝在從艇上，讀取MEMS陀螺輸出航向作為靜態(tài)數(shù)據(jù)并保存，一小時后，主艇設(shè)備安裝完畢，開始實驗，三艇在水面上行駛時保持三角隊形，兩主艇將水聲通信信號和發(fā)送時間戳交替發(fā)送給從艇，間隔10秒，從艇上的水聲通信模塊接收主艇發(fā)來的測距信息，通過水聲信號發(fā)送和接收的時間差乘以聲速計算主從艇的距離，利用測量的速度和航向進行航跡推算，并利用主從艇的距離作為觀測量修正位置信息并估計初始航向偏差；

步驟二、建立系統(tǒng)方程

式中，x_k、y_k表示從艇在k時刻的位置，v_k為從艇速度，t表示推位時間間隔，表示MEMS陀螺測得的航向，表示初始航向偏差，在方程中估計值修正了航向測量值的偏差，表示成：

X_k+1＝f(X_k，u_k，t)+w_k

線性化后得，

X_k+1＝F_kX_k+B_ku_k+w_k

式中，系統(tǒng)噪聲w_k～N(0，Q_k)，

步驟三、建立量測方程：

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