專利名稱：一種小型水下機器人組合導航系統及導航方法
技術領域：
本發明涉及的是一種小型水下組合導航系統，本發明也涉及一種小型水下組合導 航方法。
背景技術：
目前智能水下機器人向小型化發展是其發展趨勢之一。小型水下機器人由于其體 積小、機動靈活、成本低、搭載方便，在軍民用都有廣泛的應用前景。導航問題仍然是水下機 器人設計所面臨的主要關鍵技術之一。現在水下機器人導航常用方法有航位推算法、慣性 導航及聲學方法等。慣導系統一般體積較大，價格昂貴，最重要的是純慣性導航系統導航 在沒有其它傳感器數據信息進行補償的情況下其定位誤差將隨時間和航程累積和發散，水 聲定位系統(長基線、短基線、超短基線等)定位精度有很大提高，其缺點就是作用距離有 限。航位推算導航是一種低成本的導航方法，是水下機器人重要的導航手段，其方法簡單、 經濟，水下機器人除了配備測速儀、姿態、深度傳感器等外，只需要給定初始位置信息，通過 一定的算法就可構成具有一定精度的實時、可靠的自主式導航系統。微小型水下機器人受 限于體積、成本、能源等限制，這對導航系統提出了更高的要求，這對構造一種小型化組合 導航系統增加了難度。導航系統一般由小型化、低成本傳感器構成，傳感器精度降低，同時， 水下機器人一般工作在特有的海洋水下環境，干擾噪聲信號大，各種水聲傳感器普遍存在 精度低、野點率高的缺點，對傳感器數據進行較好的濾波剔出野點信息尤為重要。航位推算 導航一般需要定期進行位置校正，如果潛深較大通過浮出水面通過GPS進行位置重調，這 將消耗較多能源，這對能源有限的微小型水下機器人尤其不利。因此，研制一套可實現多種 途徑進行位置校正和能夠有效地剔除傳感器野點信息的小型化水下機器人導航系統對微 小型水下機器人的實際工程應用具有重要的意義。申請號為200520020572. 2，名稱為“小型水下自主組合導航裝置”的專利文件中， 提供了一種小型水下自主組合導航裝置。申請號為201010114558. 4，名稱為“一種小型水 下航行器用組合導航系統及方法”的專利文件中，提供了一種由捷聯慣性導航系統、多普勒 導航系統、地形輔助導航系統和磁羅經組成的水下組合導航導航系統和方法。
發明內容
本發明的目的在于提供一種體積小、重量輕、成本低，利于實現水下導航系統小型 化的小型水下機器人組合導航系統。本發明的目的還在于提供一種基于小型水下機器人組 合導航系統的導航方法。本發明的小型水下機器人組合導航系統包括嵌入式導航處理機、耐壓GPS、微型 姿態傳感器、速度計、深度計及繼電器元件，微型姿態傳感器安裝在水下機器人本體耐壓艙 內，微型姿態傳感器的信號傳入嵌入式導航處理機，速度計、深度計安裝在水下機器人本體 下部，水聲通訊聲納安裝在水下機器人本體首部上方，速度計、深度計、水聲通訊聲納信號 通過水密線纜與布置在水下機器人本體耐壓艙內的嵌入式導航處理機相連；嵌入式導航處理機包括CPU核心模塊、AD板、4串口卡、電源板，電源板由MV電池供電并通過PC/104總 線給其他板卡供電；所有板卡數據信息通過PC/104總線傳輸。本發明的小型水下機器人組合導航系統還可以包括1、所述速度計、深度計、水聲通訊聲納電源開關由AD板IO通道執行。2、耐壓GPS、微型姿態傳感器、速度計、水聲通訊聲納接入嵌入式導航處理機8串 口卡進行數據通訊，深度計接入嵌入式導航處理機AD板。3、耐壓GPS、微型姿態傳感器、速度計、水聲通訊聲納通過嵌入式導航處理串口卡 以RS232協議方式接收數據或指令信息。4、所述深度計為小型模擬信號輸出傳感器，通過嵌入式導航處理機AD板采集。本發明的基于小型水下機器人組合導航系統的導航方法為采用由嵌入式導航處理機、耐壓GPS、微型姿態傳感器、速度計、深度計、水聲通訊 聲納及繼電器元件組成的小型水下機器人導航系統；(1)通過微型姿態傳感器、深度計分別獲得水下機器人本體姿態和深度信息，通過 深度計獲得機器人本體速度和高度信息，水下機器人若在水面獲取耐壓GPS信息，若在水 下通過水聲通訊聲納獲取位置和指令信息；所述本體姿態包括艏搖、橫搖、縱搖；(2)采用新息修正的抗野值自適應卡爾曼濾波方法對獲得的傳感器信息進行數據 處理；(3)跟據磁偏角數據庫計算得出磁偏角；(4)接收規劃導航位置校正指令信息，依據機器人本體姿態、深度和速度信息進行 導航位置計算。本發明的基于小型水下機器人組合導航系統的導航方法還可以包括1、所述的基于新息修正的抗野值自適應卡爾曼濾波方法為(1)用標準的卡爾曼濾波方程計算出狀態估計值；(2)計算系統干擾均值q⑷,系統干擾方差矩陣,量測噪聲均值,量測噪聲方差矩陣；系統干擾均值
權利要求
1.一種小型水下機器人組合導航系統，包括嵌入式導航處理機、耐壓GPS、微型姿態傳 感器、速度計、深度計及繼電器元件，其特征是微型姿態傳感器安裝在水下機器人本體耐 壓艙內，微型姿態傳感器的信號傳入嵌入式導航處理機，速度計、深度計安裝在水下機器人 本體下部，水聲通訊聲納安裝在水下機器人本體首部上方，速度計、深度計、水聲通訊聲納 信號通過水密線纜與布置在水下機器人本體耐壓艙內的嵌入式導航處理機相連；嵌入式導 航處理機包括CPU核心模塊、AD板、4串口卡、電源板，電源板由24V電池供電并通過PC/104 總線給其他板卡供電；所有板卡數據信息通過PC/104總線傳輸。
2.根據權利要求1所述的小型水下機器人組合導航系統，其特征是所述速度計、深度 計、水聲通訊聲納電源開關由AD板IO通道執行。
3.根據權利要求2所述的小型水下機器人組合導航系統，其特征是耐壓GPS、微型姿 態傳感器、速度計、水聲通訊聲納接入嵌入式導航處理機8串口卡進行數據通訊，深度計接 入嵌入式導航處理機AD板。
4.根據權利要求3所述的小型水下機器人組合導航系統，其特征是耐壓GPS、微型姿 態傳感器、速度計、水聲通訊聲納通過嵌入式導航處理串口卡以RS232協議方式接收數據 或指令信息。
5.根據權利要求4所述的小型水下機器人組合導航系統，其特征是所述深度計為小 型模擬信號輸出傳感器，通過嵌入式導航處理機AD板采集。
6.基于小型水下機器人組合導航系統的導航方法，其特征是采用由嵌入式導航處理機、耐壓GPS、微型姿態傳感器、速度計、深度計、水聲通訊聲納 及繼電器元件組成的小型水下機器人導航系統；(1)通過微型姿態傳感器、深度計分別獲得水下機器人本體姿態和深度信息，通過深度 計獲得機器人本體速度和高度信息，水下機器人若在水面獲取耐壓GPS信息，若在水下通 過水聲通訊聲納獲取位置和指令信息；所述本體姿態包括艏搖、橫搖、縱搖；(2)采用新息修正的抗野值自適應卡爾曼濾波方法對獲得的傳感器信息進行數據處理；(3)跟據磁偏角數據庫計算得出磁偏角；(4)接收規劃導航位置校正指令信息，依據機器人本體姿態、深度和速度信息進行導航位置計算。
7.根據權利要求6所述的基于小型水下機器人組合導航系統的導航方法，其特征是所 述的基于新息修正的抗野值自適應卡爾曼濾波方法為(1)用標準的卡爾曼濾波方程計算出狀態估計值；(2)計算系統干擾均值,系統干擾方差矩陣,量測噪聲均值,量測噪聲方差 矩陣R⑷；系統干擾均值c{(k) = (\-dk_l)({(k-\) + dk_l X(k/k)-0(k,k-l)X(k-l/k-l)系統干擾方差矩陣 Q(^) = (1-d(k-I) + 式—JKi (眾)/ (k)Kr (k) + F(k / k)- Φ(々, k-\)V(k-Hk- 1)ΦΓ (k/k-1)]量測噪聲均值；r(k) = (1-dk_,)r(k-1) + dk_, Z(k)-Η(Λ)X(k/k-l)( 3 )量測噪聲方差矩陣R(yt) = (1-dk_,) R(yt-1) + dk_, [ε(Λ)εΓ (k) - H⑷/ k - 1)ΗΓ ⑷](4 )其中Χ@)、(m、R⑷分別是狀態X(k)、系統狀態噪聲序列的方差陣Q(k)、量測噪 聲序列方差陣R(k)的估計，ε (k) = Z(k)-r(k-l)-H(k)X(k|k-l)為新息矩陣，dk = l~h/(l"bk+1)，b 是遺忘因子, = ‘ ‘ ； ^ ^1t 1 tJ；(3)利用野值判據進行野值判別|ε(Λ)| > ayjΗ(Λ) P(k, k - 1)Η(Λ)Τ + R(yt)( 5 )其中，α為常數，若( 式判據成立則本次測量值為干擾數據即野值，轉到下一步對新 息進行修正；若(5)式判據不成立，則本次測量值為正常數據，不需要對新息進行修正；(4)對新息進行修正如果k時刻經野值判據(5)判斷為野值，則采用下式對新息進行修正ε (k) = ε (k-1) β !+ ε (k_2) β 2+…+ ε (1) β其中{ β J = dH，d^b, d^b2, ... ‘Ι/-1為前k-1次新息加權系數序列。
8.根據權利要求6或7所述的基于小型水下機器人組合導航系統的導航方法，其特征 是所述導航位置計算的方法為(1)通過速度計接收傳機器人艇體速度、高度數據信息，姿態傳感器獲得機器人艏向、 橫搖、縱搖姿態信息；(2)采用新息修正的抗野值自適應卡爾曼濾波方法對傳感器數據信息進行濾波處理；(3)將艇體速度進行坐標轉換為大地坐標系下速度信息將水下機器人艇體坐標系下的速度通過轉換矩陣(6)式計算求得水下機器人大地坐 標系下的速度，[Vn Ve vu]T = T [Vx Vy vjT(6)αψχεθ ηψ3ηφΗ3￡ψ3 ιθιΧ￡φT= 3ηψ ￡θ( )(εεθ ηφ<xs6(mp其中[Vn Ve VU]T為大地坐標系下水下機器人北向、東向、垂向速度，[Vx Vy νζ]τ分艇體 坐標下的水下機器人縱向、橫向、垂向速度，ψ，θ，識分別為水下機器人的橫搖、縱搖以及艏 向姿態角；(4)根據速度信息前一時刻位置信息進位置推算若不進行校正以前一時刻位置信息作為為位置推算初始點，若水面校正將GPS信息作 為位置推算初始點，若在水下通過水聲通訊校正則將根據母船位置及與水下機器人相對位 置計算求得的機器人位置作為位置推算的初始點，對速度進行積分考慮地球橢球模型獲得 水下機器人位置。
全文摘要
本發明提供的是一種小型水下機器人組合導航系統及導航方法。該組合導航系統包括嵌入式導航處理機、耐壓GPS、微型姿態傳感器、速度計、深度計、水聲通訊設備。當水下機器人載體在水下時，組合導航系統上電后，導航系統自主運行，通過AD板IO通道控制繼電器打開傳感器，進行數據采集，獲得導航初始位置，根據磁偏角數據庫計算得出磁偏角，接收規劃導航位置校正指令信息，進行導航位置推算，從而獲得水下機器人的經緯度位置信息。該系統體積小、重量輕、成本低，利于實現水下導航系統小型化，可應用于要求體積重量小的水下觀察、探測及小型水下機器人。
文檔編號G01C21/16GK102052923SQ20101055904
公開日2011年5月11日 申請日期2010年11月25日 優先權日2010年11月25日
發明者萬磊, 孫玉山, 常文田, 曹建, 王波, 秦再白, 蘇玉民 申請人:哈爾濱工程大學