專利名稱：一種用于水下滑翔器的組合導(dǎo)航裝置及方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種用于水下滑翔器的組合導(dǎo)航裝置設(shè)計(jì)及方法，該裝置具有體積小、低功耗、長航時(shí)等特點(diǎn)，可實(shí)現(xiàn)水下滑翔器的準(zhǔn)確定位和自主導(dǎo)航。
背景技術(shù)：
電子羅盤可分為平面電子羅盤和三維電子羅盤，比起平面羅盤，三維電子羅盤因其內(nèi)部加入了傾角傳感器，克服了平面電子羅盤在使用中的嚴(yán)格限制，即使羅盤發(fā)生了傾斜，航向數(shù)據(jù)依然準(zhǔn)確無誤。有的羅盤還內(nèi)置了溫度補(bǔ)償，最大限度得減少傾斜角和指向角的溫度漂移。電子羅盤技術(shù)在國外應(yīng)用比較多，但是在水下應(yīng)用還存在一些問題。GPS技術(shù)在導(dǎo)航、定位、測速、定向方面有著廣泛的應(yīng)用，但由于其信號(hào)常被地形、 地物遮擋，導(dǎo)致精度大大降低，甚至不能使用。尤其是在高樓林立城區(qū)和植被茂密的林區(qū)， GPS信號(hào)的有效性僅為60 %。該技術(shù)在國內(nèi)外應(yīng)用都比較廣泛，也比較成熟，但應(yīng)用于海上潛水器的導(dǎo)航還沒有成熟的應(yīng)用方案。以慣性導(dǎo)航為主與GPS、電子羅盤集成的組合導(dǎo)航系統(tǒng)在國內(nèi)外陸用、航空、航天以及水面載體有著廣泛的應(yīng)用，但水下滑翔器在水下工作時(shí)間很長，并且GPS信息在水下不能使用，單一慣性導(dǎo)航不能滿足定位要求，且由于水下滑翔器對(duì)功耗、體積和精度要求較高，從而在水下滑翔器中組合導(dǎo)航系統(tǒng)均未能得到較好應(yīng)用。如何使GPS、電子羅盤、捷聯(lián)式慣性組合導(dǎo)航系統(tǒng)能利用三者的優(yōu)勢，使系統(tǒng)整體性能達(dá)到最優(yōu)，并能滿足水下滑翔器長航時(shí)、低功耗、小體積的要求，這種系統(tǒng)在國內(nèi)、外均無報(bào)道。

發(fā)明內(nèi)容
本發(fā)明的技術(shù)解決問題是克服現(xiàn)有技術(shù)的不足，設(shè)計(jì)了一種新的水下滑翔器的組合導(dǎo)航裝置及方法，其算法克服了傳統(tǒng)組合導(dǎo)航系統(tǒng)由于誤差隨時(shí)間累積而不能準(zhǔn)確長時(shí)間定位的欠缺，并采取智能導(dǎo)航工作模式切換、多源系統(tǒng)自適應(yīng)匹配、濾波和智能導(dǎo)航定位優(yōu)化算法，提供了可靠性更高也更精確的定位服務(wù)。再加上硬件設(shè)計(jì)和軟件優(yōu)化，使得設(shè)備體積更小、功耗更低、航時(shí)更長，完成水下潛水器的自主導(dǎo)航和定位。本發(fā)明的技術(shù)解決方案為一種用于水下滑翔器的組合導(dǎo)航裝置，包括電子羅盤、 微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)、全球定位系統(tǒng)(GPS)接收模塊、數(shù)字信號(hào)處理(DSP) 處理模塊；電子羅盤測量方位角得到航向信息；微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)測量姿態(tài)和航向變化，并借助電子羅盤航向信息進(jìn)行校正；全球定位系統(tǒng)(GPS)接收模塊則完成對(duì)水下滑翔器的水面定位、微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)的標(biāo)定及校正；數(shù)字信號(hào)處理(DSP)處理模塊完成對(duì)導(dǎo)航系統(tǒng)中各個(gè)模塊數(shù)據(jù)的轉(zhuǎn)化、處理，并將航向位置等數(shù)據(jù)輸出到水下滑翔器中心計(jì)算機(jī)，實(shí)現(xiàn)自主導(dǎo)航與定位。所述電子羅盤采用型號(hào)為ADIS16405，其內(nèi)包括三軸陀螺，三軸加速度計(jì)，三軸磁力計(jì)。
4
所述數(shù)字信號(hào)處理(DSP)處理模塊采用型號(hào)為TMS320C5505。所述方法如下采用電子羅盤測量方位角得到航向信息；采用微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)測量姿態(tài)和航向變化，并借助電子羅盤航向信息進(jìn)行校正；采用全球定位系統(tǒng)(GPQ接收模塊則完成對(duì)水下滑翔器的水面定位、微機(jī)電系統(tǒng)慣性測量單元 (MEMS-IMU)的標(biāo)定及校正；采用數(shù)字信號(hào)處理(DSP)處理模塊完成對(duì)導(dǎo)航系統(tǒng)中各個(gè)模塊數(shù)據(jù)的轉(zhuǎn)化、處理，并將航向位置等數(shù)據(jù)輸出到水下滑翔器中心計(jì)算機(jī)，實(shí)現(xiàn)自主導(dǎo)航與定位。所述電子羅盤是由三維磁阻傳感器、雙軸傾角傳感器和微控制單元(MCU)組成； 三維磁阻傳感器用來測量地球磁場；傾角傳感器是在羅盤隨著潛水體的傾斜而處于非水平狀態(tài)時(shí)，對(duì)羅盤進(jìn)行傾角補(bǔ)償，保證了數(shù)據(jù)依然準(zhǔn)確無誤；微控制單元(MCU)處理磁力儀和傾角傳感器的信號(hào)以及數(shù)據(jù)輸出和軟鐵、硬鐵補(bǔ)償。一種用于水下滑翔器的組合導(dǎo)航裝置的方法，算法包括以下(1)初始時(shí)刻，GPS對(duì)導(dǎo)航測量單元包括三維電子羅盤、三軸加速度計(jì)和三軸陀螺進(jìn)行初始校正，得到初始信息；電子羅盤提供載體的初始航向信息，通過傳遞對(duì)準(zhǔn)給捷聯(lián)慣性導(dǎo)航系統(tǒng)以獲得初始的失準(zhǔn)角信息，而微機(jī)電系統(tǒng)慣性測量單元測得的角速率和加速度信息由捷聯(lián)慣性導(dǎo)航系統(tǒng)導(dǎo)航姿態(tài)解算模塊來處理，導(dǎo)航姿態(tài)解算系統(tǒng)由角速率和加速度信息解算出載體的速度、位置及姿態(tài)信息；陀螺和加速度計(jì)分別用來測量載體的角運(yùn)動(dòng)和線運(yùn)動(dòng)信息，導(dǎo)航解算模塊根據(jù)這些測量信息建立姿態(tài)陣解算的數(shù)學(xué)平臺(tái)，從而解算出運(yùn)動(dòng)載體的航向、姿態(tài)、速度及位置；(2)在長時(shí)間運(yùn)行時(shí)，采用電子羅盤的航向角信息來對(duì)MEME-SINS輸出的信息進(jìn)行校正，相應(yīng)的差值作為無跡卡爾曼濾波器的觀測量，經(jīng)無跡卡爾曼濾波估計(jì)出的慣導(dǎo)參數(shù)誤差反饋到捷聯(lián)慣性導(dǎo)航系統(tǒng)導(dǎo)航解算模塊內(nèi)，在力學(xué)編排方程中校正慣性傳感器的輸出，推算的速度和經(jīng)緯度、建立的姿態(tài)矩陣數(shù)字平臺(tái)，將校正后的參數(shù)代入下一次運(yùn)算；經(jīng)過反饋校正后，捷聯(lián)慣性導(dǎo)航系統(tǒng)輸出的導(dǎo)航參數(shù)就是組合導(dǎo)航系統(tǒng)的輸出；(3)針對(duì)水下滑翔器的具體情況，利用航位推算原理，分析航位推算的誤差源，在其基礎(chǔ)上推導(dǎo)航位推算誤差方程，并應(yīng)用航位推算誤差方程對(duì)初始誤差角、刻度因子和陀螺隨機(jī)常值漂移進(jìn)行補(bǔ)償；對(duì)于電子羅盤，通過對(duì)軟鐵及硬鐵干擾的分析和建模，提出應(yīng)用分段補(bǔ)償?shù)姆椒▽?duì)其進(jìn)行誤差補(bǔ)償；(4)水下滑翔器工作時(shí)間是以水下為主，隔段時(shí)間會(huì)上浮接收GPS信息，為了保證系統(tǒng)低功耗，在水上水下采取不同的算法水下采用AUKF算法；水上采用聯(lián)邦濾波算法；(5)采用水下滑翔器每次工作前首先進(jìn)行靜止零位誤差校正；滑翔體在水中滑翔后浮出水面，首先進(jìn)行靜止零位誤差校正，再利用航位推算位置誤差來計(jì)算初始航向誤差角的方法，對(duì)動(dòng)態(tài)粗對(duì)準(zhǔn)的初始航向角進(jìn)行補(bǔ)償，完成動(dòng)態(tài)精對(duì)準(zhǔn)過程；(6)在陀螺、加速度計(jì)和電子羅盤等多傳感器數(shù)據(jù)中，采用新息法來剔除野值。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于(1)應(yīng)用于水下滑翔器基于電子羅盤、MEMS捷聯(lián)慣性系統(tǒng)與GPS的系統(tǒng)集成技術(shù)， 實(shí)現(xiàn)較簡單，體積較小、成本較低、易于優(yōu)化；(2)本發(fā)明可智能切換導(dǎo)航工作模式和信息融合算法，采用組合系統(tǒng)的低功耗設(shè)計(jì)技術(shù)，使水下滑翔器更具有自主性、更合理地利用導(dǎo)航信息資源，適應(yīng)環(huán)境能力強(qiáng)；(3)基于聯(lián)邦無跡卡爾曼濾波器與自適應(yīng)無跡卡爾曼濾波器的導(dǎo)航和定位算法， 使得系統(tǒng)具有長航時(shí)性、高可靠性和高精度。


圖1為一種用于水下滑翔器的組合導(dǎo)航系統(tǒng)原理示意圖；圖2為捷聯(lián)式慣性導(dǎo)航系統(tǒng)原理圖；圖3為航位推算原理圖；圖4為理想原始測量數(shù)據(jù)獲取具體實(shí)施方案下面結(jié)合附圖對(duì)發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說明圖1是水下滑翔器的組合導(dǎo)航系統(tǒng)原理示意圖，選用的新型IMU單元將三軸陀螺， 三軸加速度計(jì)，三軸高性能磁力計(jì)集成與一體，該單元體積小，集成度高，功耗低，帶有自校正和標(biāo)定功能。在水下滑翔器放入水中之前，要首先用靜止零位誤差校正的方法對(duì)陀螺產(chǎn)生零位誤差進(jìn)行校正。電子羅盤除了自帶的誤差補(bǔ)償外，還應(yīng)利用GPS信息和分段補(bǔ)償?shù)姆椒▽?duì)其進(jìn)行誤差補(bǔ)償。這樣就使導(dǎo)航測量單元(包括三維電子羅盤、三軸加速度計(jì)和三軸陀螺)得到精確的初始信息。圖4是理想原始測量數(shù)據(jù)信息獲取路徑，在經(jīng)過對(duì)慣性器件標(biāo)定并獲取理想原始信息的基礎(chǔ)上，設(shè)計(jì)基于捷聯(lián)狀態(tài)下的適合水下滑翔器運(yùn)動(dòng)環(huán)境的姿態(tài)測量算法。當(dāng)滑翔器進(jìn)入水下前行時(shí)，因水下的環(huán)境相對(duì)穩(wěn)定，前行速度均勻，利用海流數(shù)據(jù)庫得出海流速度，推算出恒定的外在干擾以用于后續(xù)的航位推算。水下只有電子羅盤和 MEMS-IMU單元進(jìn)行工作，電子羅盤提供載體的初始航向信息，通過傳遞對(duì)準(zhǔn)給捷聯(lián)慣性導(dǎo)航系統(tǒng)以獲得初始的失準(zhǔn)角信息，而MEMS測得的角速率和加速度信息由SINS導(dǎo)航姿態(tài)解算模塊來處理，導(dǎo)航姿態(tài)解算系統(tǒng)由角速率和加速度信息解算出載體的速度、位置和姿態(tài)信息。陀螺和加速度計(jì)分別用來測量載體的角運(yùn)動(dòng)和線運(yùn)動(dòng)信息，導(dǎo)航解算模塊根據(jù)這些測量信息建立姿態(tài)陣解算的數(shù)學(xué)平臺(tái)，從而解算出運(yùn)動(dòng)載體的航向、姿態(tài)、速度及位置。由于慣性導(dǎo)航儀因陀螺的漂移存在隨時(shí)間積累的誤差，故在長時(shí)間運(yùn)行時(shí)，采用電子羅盤的航向角信息來對(duì)MEMS-SINS輸出的信息進(jìn)行校正，相應(yīng)的差值作為卡爾曼濾波器的觀測量，經(jīng)卡爾曼濾波估計(jì)出的慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)誤差反饋到SINS導(dǎo)航解算模塊內(nèi)，在力學(xué)編排方程中校正慣性傳感器的輸出、推算的速度和經(jīng)緯度、建立的姿態(tài)矩陣數(shù)字平臺(tái)，將校正后的參數(shù)代入下一次運(yùn)算。因此，經(jīng)過反饋校正后，慣導(dǎo)系統(tǒng)輸出的導(dǎo)航參數(shù)就是組合導(dǎo)航系統(tǒng)的輸出。 在水下只有MEMS-IMU和電子羅盤的作用，采用自適應(yīng)UKF方式來進(jìn)行濾波，但當(dāng)滑翔器經(jīng)過M小時(shí)浮到水面，接收到的GPS信號(hào)對(duì)MEMS-IMU進(jìn)行校正，由于導(dǎo)航傳感器的增加，就需采用基于UKF的聯(lián)邦濾波算法處理多傳感器的信息融合來提高導(dǎo)航精度。這時(shí)導(dǎo)航系統(tǒng)處在動(dòng)態(tài)環(huán)境下，要在短時(shí)間和復(fù)雜環(huán)境下對(duì)捷聯(lián)慣組進(jìn)行動(dòng)態(tài)高精度初始對(duì)準(zhǔn)，利用GPS信息對(duì)MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)予以重新對(duì)準(zhǔn)和校正的同時(shí)，還要利用航位推算位置誤差來計(jì)算初始航向誤差角的方法，對(duì)動(dòng)態(tài)粗對(duì)準(zhǔn)的初始航向角進(jìn)行補(bǔ)償，通過信頁
息融合方法，完成動(dòng)態(tài)精對(duì)準(zhǔn)過程。在水下到水上，再到水下的過程中，由于傳感器的數(shù)量變化導(dǎo)致不同信息量的融合，為了降低功耗，提高系統(tǒng)的處理效率，采用切換式組合方式進(jìn)行航位推算，即GPS模式和DR模式。對(duì)于陀螺、加速度計(jì)和電子羅盤等多傳感器信號(hào)數(shù)據(jù)中出現(xiàn)的野值，采用新息法來剔除這些野值，這種算法不再依賴于先驗(yàn)的傳感器特性，而是通過每次濾波迭代的時(shí)候進(jìn)行判斷，為了濾波的連續(xù)性，必須在野值被剔除后進(jìn)行補(bǔ)值，利用對(duì)當(dāng)前新息值加權(quán)的方法進(jìn)行補(bǔ)值，減小新息對(duì)濾波精度的影響。本發(fā)明具體如下一種用于水下滑翔器的組合導(dǎo)航裝置及方法，其特點(diǎn)在于由 GPS接收模塊、MEMS-IMU單元及電子羅盤組合成的導(dǎo)航系統(tǒng)，根據(jù)不同環(huán)境自主重構(gòu)組合方法，并融入最優(yōu)算法，充分利用有限的導(dǎo)航數(shù)據(jù)實(shí)現(xiàn)高精度、高可靠性的水下滑翔體定位與導(dǎo)航，具體步驟如下(1)算法方面具體為(a)航位推算在MEMS捷聯(lián)姿態(tài)算法對(duì)準(zhǔn)的基礎(chǔ)上，結(jié)合電子羅盤信息、GPS接收機(jī)輸出信息、海流數(shù)據(jù)等進(jìn)行推算，它們當(dāng)中任何一個(gè)精度出現(xiàn)誤差都會(huì)影響到航位推算的精度，這其中以MEMS陀螺和電子羅盤的影響最大，除航位推算系統(tǒng)外，海流、潮汐和風(fēng)是影響航位推算的三個(gè)重要的因素。(b)海流數(shù)據(jù)庫的應(yīng)用海流主要包括洋流、潮汐或潮流、風(fēng)、涌、浪等。在近海航行或遠(yuǎn)海航行時(shí)，掌握大洋海流的運(yùn)動(dòng)規(guī)律，準(zhǔn)確估計(jì)其流向和流速是提高定位精度、減少事故、節(jié)省能源的重要手段。為了準(zhǔn)確地計(jì)算或者估算海流的大小，還需要通過對(duì)一些固定目標(biāo)進(jìn)行觀測，對(duì)數(shù)據(jù)加以修正。(c)切換式組合的應(yīng)用采用切換式組合方式的航位推算方案，切換式組合方案有兩種工作狀態(tài)GPS模式和航位推算(DR)模式，系統(tǒng)工作在何種模式取決于GPS信號(hào)的有效性和DOP精度因子。當(dāng)滑翔器浮到水面時(shí)，系統(tǒng)工作在GPS模式，同時(shí)利用GPS的輸出數(shù)據(jù)，刷新DR系統(tǒng)的推算位置，對(duì)航位進(jìn)行校正；一旦滑翔器潛入水中，GPS定位數(shù)據(jù)失效或DOP增大到預(yù)定的門限，則切換到DR模式。(d)如圖3，假設(shè)載體在tg時(shí)刻的位置為(Xh，y^，^)，行駛了 At(W1)時(shí)間，在、時(shí)刻到達(dá)位置(Xi，Ji, Zi)，在、時(shí)刻方位角為Ai,傾斜角為Ii,坐標(biāo)位置可以表示為
Xi = X^1 +ALgpos 4 經(jīng) in Zij yt = yt_x + AL 經(jīng) in A1 經(jīng) in I1 .W1+ALgposTiVi = Vh+VV在圖3中的非水平段，位置除了有χ和y方向上的變化，還有ζ方向的變化，在水平段位置只有X和y方向上的變化。由于滑翔器是緩慢勻速前行，再結(jié)合海底環(huán)境，可以認(rèn)為是相對(duì)穩(wěn)定的環(huán)境下前行，所以可以將速度ν看成恒值，根據(jù)水下海流速度確定水下滑翔器的滑翔速度。前面(1)的(b)中提到了建立海流的數(shù)據(jù)庫，這樣就可假定在水下前行受到的阻力也是恒定值，采用一些白噪聲作為隨機(jī)干擾Vv，推算出Vi。由以上可以得出位置和速度信息。(e)由導(dǎo)航傳感器本身的元件誤差和外界環(huán)境及算法引起的系統(tǒng)誤差是船位推算
7的誤差源。在元件誤差中，對(duì)陀螺影響較大的誤差主要是陀螺的常值誤差和陀螺漂移誤差， 在水下采用將電子羅盤和MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)輸出的兩個(gè)航向信息進(jìn)行融合，用電子羅盤的信息估計(jì)陀螺誤差，不斷修正MEMS捷聯(lián)慣性導(dǎo)航系統(tǒng)輸出的航向。(f)電子羅盤的航向輸出極易受外界干擾磁場的影響而使精度降低，本發(fā)明選用的新型組合導(dǎo)航單元內(nèi)含了三維電子羅盤，其微處理器中內(nèi)置了誤差補(bǔ)償算法，可以對(duì)電子羅盤受到的軟、硬磁干擾進(jìn)行補(bǔ)償，但效果有限。為了提高電子羅盤的定向精度，在使用前進(jìn)行誤差補(bǔ)償。因電子羅盤誤差方程在整量程擬合曲線時(shí)，尾端出現(xiàn)較大誤差，所以將整個(gè)區(qū)域劃分為兩部分，前部分應(yīng)用八位置最小二乘法進(jìn)行誤差補(bǔ)償；后部分不考慮高次諧波項(xiàng)再構(gòu)造另一誤差模型方程，在該部分取若干采樣點(diǎn)，將采樣點(diǎn)代入誤差方程，利用最小二乘法解算出誤差補(bǔ)償系數(shù)，再將誤差補(bǔ)償系數(shù)代入模型方程可得到磁航向角。(g)在實(shí)際水下導(dǎo)航過程中，由于數(shù)學(xué)模型本身的不確定性，再加上環(huán)境噪聲的影響，預(yù)測出來的結(jié)果帶有誤差。為了減小誤差，使得結(jié)果盡可能地接近真實(shí)值，需要進(jìn)行補(bǔ)償。在傳統(tǒng)的卡爾曼濾波器的基礎(chǔ)上提出將自適應(yīng)濾波和無跡卡爾曼濾波器(UKF)相結(jié)合的自適應(yīng)UKF(AUKF)算法。采用方差調(diào)整的原則，將存在偏差的協(xié)方差矩陣進(jìn)行自適應(yīng)調(diào)整，以合理調(diào)整在濾波解中的作用。合理的自適應(yīng)因子能平衡動(dòng)力學(xué)模型信息與觀測信息的權(quán)比，而且能夠控制動(dòng)力學(xué)模型誤差對(duì)導(dǎo)航參數(shù)解的影響。由于預(yù)測殘差能反映動(dòng)力學(xué)模型的誤差量級(jí)。若觀測信息可靠，動(dòng)力學(xué)模型也可靠，則由動(dòng)力學(xué)模型預(yù)測的狀態(tài)參數(shù)， 并由狀態(tài)參數(shù)與觀測信息求得的預(yù)測殘差應(yīng)該很??；反之，若觀測信息可靠，且預(yù)測殘差很大，則表明動(dòng)力學(xué)模型誤差較大。如此可以依據(jù)預(yù)測殘差構(gòu)造判別統(tǒng)計(jì)量。自適應(yīng)因子能夠很好地抑制初始值偏差和動(dòng)力學(xué)模型異常擾動(dòng)誤差對(duì)導(dǎo)航解的影響，所以將自適應(yīng)因子引入到UKF算法能夠很好地提高濾波器的性能。(h)在水下只有MEMS-IMU和電子羅盤的作用，采用上述方式來濾波，但當(dāng)滑翔器浮到水面，接收GPS信號(hào)進(jìn)行信息校正時(shí)，由于導(dǎo)航傳感器的增加，這時(shí)需采用新的濾波算法處理多傳感器的信息融合來提高導(dǎo)航精度。組合導(dǎo)航系統(tǒng)測量信息有冗余，當(dāng)某個(gè)敏感器性能下降，測量噪聲增大時(shí)，聯(lián)邦濾波算法通過調(diào)整動(dòng)態(tài)分配因子，降低該子濾波器的置信度，從而保證主濾波器的高精度估計(jì)結(jié)果，確保了導(dǎo)航系統(tǒng)的精度。同時(shí)將UKF方法應(yīng)用到聯(lián)邦濾波中，由于UKF避免了線性化引入的截?cái)嗾`差，所以基于UKF的聯(lián)邦濾波算法在觀測信息較少以及系統(tǒng)初始估計(jì)偏差較大時(shí)，仍能保持比傳統(tǒng)的EKF方法更高的精度，具有更好的魯棒性。(i)因每次的系統(tǒng)啟動(dòng)而使陀螺產(chǎn)生零位誤差，采用水下滑翔器每次工作前首先進(jìn)行靜止零位誤差校正的方法。算法如下
權(quán)利要求
1.一種用于水下滑翔器的組合導(dǎo)航裝置，其特征在于包括電子羅盤、微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)、全球定位系統(tǒng)(GPS)接收模塊、數(shù)字信號(hào)處理(DSP)處理模塊；電子羅盤測量方位角得到航向信息；微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)測量姿態(tài)和航向變化，并借助電子羅盤航向信息進(jìn)行校正；全球定位系統(tǒng)(GPS)接收模塊則完成對(duì)水下滑翔器的水面定位、微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)的標(biāo)定及校正；數(shù)字信號(hào)處理(DSP) 處理模塊完成對(duì)導(dǎo)航系統(tǒng)中各個(gè)模塊數(shù)據(jù)的轉(zhuǎn)化、處理，并將航向位置等數(shù)據(jù)輸出到水下滑翔器中心計(jì)算機(jī)，實(shí)現(xiàn)自主導(dǎo)航與定位。
2.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的組合導(dǎo)航裝置，其特征在于所述電子羅盤采用型號(hào)為ADIS16405，其內(nèi)包括三軸陀螺，三軸加速度計(jì)，三軸磁力計(jì)。
3.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的組合導(dǎo)航裝置，其特征在于所述數(shù)字信號(hào)處理(DSP)處理模塊采用型號(hào)為TMS320C5505。
4.一種如權(quán)利要求1所述的一種用于水下滑翔器的組合導(dǎo)航裝置的方法，其特征在于所述方法如下采用電子羅盤測量方位角得到航向信息；采用微機(jī)電系統(tǒng)慣性測量單元 (MEMS-IMU)測量姿態(tài)和航向變化，并借助電子羅盤航向信息進(jìn)行校正；采用全球定位系統(tǒng) (GPS)接收模塊則完成對(duì)水下滑翔器的水面定位、微機(jī)電系統(tǒng)慣性測量單元(MEMS-IMU)的標(biāo)定及校正；采用數(shù)字信號(hào)處理(DSP)處理模塊完成對(duì)導(dǎo)航系統(tǒng)中各個(gè)模塊數(shù)據(jù)的轉(zhuǎn)化、 處理，并將航向位置等數(shù)據(jù)輸出到水下滑翔器中心計(jì)算機(jī)，實(shí)現(xiàn)自主導(dǎo)航與定位。
5.根據(jù)權(quán)利要求4所述的一種用于水下滑翔器的組合導(dǎo)航裝置的方法，其特征在于所述電子羅盤是由三維磁阻傳感器、雙軸傾角傳感器和微控制單元(MCU)組成；三維磁阻傳感器用來測量地球磁場；傾角傳感器是在羅盤隨著潛水體的傾斜而處于非水平狀態(tài)時(shí)，對(duì)羅盤進(jìn)行傾角補(bǔ)償，保證了數(shù)據(jù)依然準(zhǔn)確無誤；微控制單元(MCU)處理磁力儀和傾角傳感器的信號(hào)以及數(shù)據(jù)輸出和軟鐵、硬鐵補(bǔ)償。
6.根據(jù)權(quán)利要求4所述的一種用于水下滑翔器的組合導(dǎo)航裝置的方法，其特征在于， 算法包括以下(1)初始時(shí)刻，GPS對(duì)導(dǎo)航測量單元包括三維電子羅盤、三軸加速度計(jì)和三軸陀螺進(jìn)行初始校正，得到初始信息；電子羅盤提供載體的初始航向信息，通過傳遞對(duì)準(zhǔn)給捷聯(lián)慣性導(dǎo)航系統(tǒng)以獲得初始的失準(zhǔn)角信息，而微機(jī)電系統(tǒng)慣性測量單元測得的角速率和加速度信息由捷聯(lián)慣性導(dǎo)航系統(tǒng)導(dǎo)航姿態(tài)解算模塊來處理，導(dǎo)航姿態(tài)解算系統(tǒng)由角速率和加速度信息解算出載體的速度、位置及姿態(tài)信息；陀螺和加速度計(jì)分別用來測量載體的角運(yùn)動(dòng)和線運(yùn)動(dòng)信息，導(dǎo)航解算模塊根據(jù)這些測量信息建立姿態(tài)陣解算的數(shù)學(xué)平臺(tái)，從而解算出運(yùn)動(dòng)載體的航向、姿態(tài)、速度及位置；(2)在長時(shí)間運(yùn)行時(shí)，采用電子羅盤的航向角信息來對(duì)MEME-SINS輸出的信息進(jìn)行校正，相應(yīng)的差值作為無跡卡爾曼濾波器的觀測量，經(jīng)無跡卡爾曼濾波估計(jì)出的慣導(dǎo)參數(shù)誤差反饋到捷聯(lián)慣性導(dǎo)航系統(tǒng)導(dǎo)航解算模塊內(nèi)，在力學(xué)編排方程中校正慣性傳感器的輸出， 推算的速度和經(jīng)緯度、建立的姿態(tài)矩陣數(shù)字平臺(tái)，將校正后的參數(shù)代入下一次運(yùn)算；經(jīng)過反饋校正后，捷聯(lián)慣性導(dǎo)航系統(tǒng)輸出的導(dǎo)航參數(shù)就是組合導(dǎo)航系統(tǒng)的輸出；(3)針對(duì)水下滑翔器的具體情況，利用航位推算原理，分析航位推算的誤差源，在其基礎(chǔ)上推導(dǎo)航位推算誤差方程，并應(yīng)用航位推算誤差方程對(duì)初始誤差角、刻度因子和陀螺隨機(jī)常值漂移進(jìn)行補(bǔ)償；對(duì)于電子羅盤，通過對(duì)軟鐵及硬鐵干擾的分析和建模，提出應(yīng)用分段補(bǔ)償?shù)姆椒▽?duì)其進(jìn)行誤差補(bǔ)償；(4)水下滑翔器工作時(shí)間是以水下為主，隔段時(shí)間會(huì)上浮接收GPS信息，為了保證系統(tǒng)低功耗，在水上水下采取不同的算法水下采用AUKF算法；水上采用聯(lián)邦濾波算法；(5)采用水下滑翔器每次工作前首先進(jìn)行靜止零位誤差校正；滑翔體在水中滑翔后浮出水面，首先進(jìn)行靜止零位誤差校正，再利用航位推算位置誤差來計(jì)算初始航向誤差角的方法，對(duì)動(dòng)態(tài)粗對(duì)準(zhǔn)的初始航向角進(jìn)行補(bǔ)償，完成動(dòng)態(tài)精對(duì)準(zhǔn)過程；(6)在陀螺、加速度計(jì)和電子羅盤等多傳感器數(shù)據(jù)中，采用新息法來剔除野值。
全文摘要
本發(fā)明公布了一種用于水下滑翔器的組合導(dǎo)航裝置及方法，裝置包括電子羅盤、微機(jī)電系統(tǒng)慣性測量單元、全球衛(wèi)星系統(tǒng)接收模塊、數(shù)字信號(hào)處理處理模塊。本發(fā)明采用MEMS陀螺、加速度計(jì)組成的姿態(tài)測量單元和電子羅盤集成為慣性組合導(dǎo)航與定位系統(tǒng)來與控制系統(tǒng)配合，使用DSP技術(shù)作為導(dǎo)航解算部件，經(jīng)過在全溫范圍內(nèi)對(duì)各傳感器進(jìn)行降噪、溫度補(bǔ)償、非線性校正、交叉耦合補(bǔ)償及航位推算等多種算法，完成水下滑翔體的自主準(zhǔn)確定位。本發(fā)明的優(yōu)點(diǎn)在于體積小、集成度高、功耗低、長航時(shí)、成本低等，能迅速準(zhǔn)確地得到水下滑翔器當(dāng)前位姿信息，使它保持本體平衡，同時(shí)為其提供航跡和位置參數(shù)。
文檔編號(hào)G01C17/32GK102519450SQ20111041211
公開日2012年6月27日 申請日期2011年12月12日 優(yōu)先權(quán)日2011年12月12日
發(fā)明者劉虎, 周智愷, 徐元, 陳熙源, 黃浩乾 申請人:東南大學(xué)