專利名稱：微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置及方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種姿態(tài)檢測(cè)裝置，具體地說是一種微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝 置。本發(fā)明還涉及一種基于此裝置的姿態(tài)檢測(cè)方法。
背景技術(shù)：
運(yùn)動(dòng)載體的姿態(tài)測(cè)量是進(jìn)行控制和導(dǎo)航的基礎(chǔ)。水下運(yùn)動(dòng)體所處的特殊環(huán)境使其 與外界信息相對(duì)封閉，因此需要姿態(tài)測(cè)量系統(tǒng)具有較高的自主性。傳統(tǒng)的姿態(tài)測(cè)量方法主 要有慣性測(cè)量、衛(wèi)星無線電測(cè)量以及地磁測(cè)量等。由于水下運(yùn)動(dòng)體所處的環(huán)境限制，對(duì)衛(wèi)星 和無線電信號(hào)具有極大的屏蔽作用，在水下環(huán)境很難使用這種測(cè)量方法；利用地磁進(jìn)行姿 態(tài)檢測(cè)主要對(duì)載體坐標(biāo)系下的地磁場(chǎng)進(jìn)行測(cè)量，經(jīng)過換算可以得出運(yùn)動(dòng)載體的姿態(tài)信息， 但地磁模型僅僅是對(duì)地球磁場(chǎng)的近似描述，而且運(yùn)動(dòng)體的金屬外殼對(duì)磁場(chǎng)具有一定的屏蔽 作用，載體內(nèi)部的電氣機(jī)構(gòu)也會(huì)對(duì)磁場(chǎng)造成一定的干擾，所以利用地磁場(chǎng)測(cè)量運(yùn)動(dòng)體的姿 態(tài)具有很大的誤差，常用于和其他方法做組合測(cè)量。慣性測(cè)量裝置主要有陀螺儀和加速度計(jì)兩種，按照結(jié)構(gòu)的不同又可以分為平臺(tái)式 和捷聯(lián)式兩種形式，平臺(tái)式慣性測(cè)量系統(tǒng)主要依靠轉(zhuǎn)子陀螺的定軸性和進(jìn)動(dòng)性跟蹤運(yùn)動(dòng)體 的導(dǎo)航坐標(biāo)系，但是平臺(tái)式慣性測(cè)量系統(tǒng)結(jié)構(gòu)復(fù)雜，制造成本高，體積大，質(zhì)量重，嚴(yán)重限制 了它的使用范圍。捷聯(lián)式慣性測(cè)量裝置使用為速率陀螺，速率陀螺成本低、體積小、質(zhì)量輕， 但是精度較低，漂移嚴(yán)重，因此也不能單獨(dú)用來進(jìn)行姿態(tài)角的測(cè)量。另一種慣性測(cè)量方法是利用加速度計(jì)測(cè)量重力在運(yùn)動(dòng)體坐標(biāo)系下各軸的分量進(jìn) 行解算，但由于加速度計(jì)同時(shí)也敏感載體運(yùn)動(dòng)的加速度，因此容易受到運(yùn)動(dòng)加速度干擾，不 能在非平衡狀態(tài)下使用。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種適用于水下環(huán)境以及對(duì)自主性姿態(tài)測(cè)量要求較高的 環(huán)境的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置。本發(fā)明的目的還在于提供一種基于本發(fā)明的微 型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置的檢測(cè)方法。本發(fā)明的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置由IMU慣性測(cè)量單元模塊1、 運(yùn)動(dòng)控制執(zhí)行部件2、運(yùn)動(dòng)學(xué)模型估計(jì)3、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4、水下導(dǎo)航計(jì)算機(jī)5和串口輸 出6組成；IMU慣性測(cè)量單元模塊1、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4、水下導(dǎo)航計(jì)算機(jī)5和串口輸出6 依此相連，運(yùn)動(dòng)控制執(zhí)行部件2連接運(yùn)動(dòng)學(xué)模型估計(jì)3，運(yùn)動(dòng)學(xué)模型估計(jì)3連接動(dòng)態(tài)測(cè)量數(shù) 據(jù)補(bǔ)償4。所述IMU慣性測(cè)量單元模塊1包括三軸加速度計(jì)、三軸陀螺儀、三軸溫度計(jì)以及數(shù) 字信號(hào)輸出電路。所述運(yùn)動(dòng)控制執(zhí)行部件2為運(yùn)動(dòng)體上的所有的控制執(zhí)行機(jī)構(gòu)。所述運(yùn)動(dòng)學(xué)模型估計(jì)3對(duì)運(yùn)動(dòng)體控制執(zhí)行部件進(jìn)行精確的動(dòng)力學(xué)描述。所述動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)5進(jìn)行姿態(tài)的解算，并通過串口輸出。基于微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置的檢測(cè)方法為所用微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置由IMU慣性測(cè)量單元模塊1、運(yùn)動(dòng)控制執(zhí) 行部件2、運(yùn)動(dòng)學(xué)模型估計(jì)3、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4、水下導(dǎo)航計(jì)算機(jī)5和串口輸出6組成； IMU慣性測(cè)量單元模塊1、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4、水下導(dǎo)航計(jì)算機(jī)5和串口輸出6依此相連， 運(yùn)動(dòng)控制執(zhí)行部件2連接運(yùn)動(dòng)學(xué)模型估計(jì)3，運(yùn)動(dòng)學(xué)模型估計(jì)3連接動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4 ；測(cè)量方法包括如下步驟第一步將IMU慣性測(cè)量單元模塊1按照運(yùn)動(dòng)體坐標(biāo)系方向安裝固定，按照航向、 俯仰、橫滾的順序構(gòu)造導(dǎo)航坐標(biāo)系到運(yùn)動(dòng)體坐標(biāo)系的方向余弦矩陣；第二步在運(yùn)動(dòng)體穩(wěn)定環(huán)境下設(shè)定運(yùn)動(dòng)體的初始狀態(tài)，航向角由上位機(jī)給定，俯仰 角和橫滾角通過三軸加速度計(jì)的輸出進(jìn)行解算；第三步在運(yùn)動(dòng)體運(yùn)動(dòng)過程中，根據(jù)運(yùn)動(dòng)體的運(yùn)動(dòng)控制執(zhí)行部件2的輸出信息，經(jīng) 過動(dòng)力學(xué)模型估計(jì)3，估計(jì)運(yùn)動(dòng)體在載體坐標(biāo)系下的三軸加速度；第四步IMU慣性測(cè)量單元1的測(cè)量數(shù)據(jù)和第三步的輸出數(shù)據(jù)進(jìn)行動(dòng)態(tài)測(cè)量數(shù)據(jù) 補(bǔ)償4，去除運(yùn)動(dòng)控制中產(chǎn)生的加速度，以及慣性測(cè)量單元1測(cè)量數(shù)據(jù)的溫度漂移；第五步在導(dǎo)航計(jì)算機(jī)5中對(duì)第四步輸出的數(shù)據(jù)進(jìn)行濾波，分別利用補(bǔ)償、濾波 后的加速度計(jì)數(shù)據(jù)、角速度數(shù)據(jù)進(jìn)行姿態(tài)解算，并進(jìn)行組合分析，給出運(yùn)動(dòng)體姿態(tài)的最優(yōu)估 計(jì)，通過串口輸出。進(jìn)行下一個(gè)采樣周期的姿態(tài)測(cè)量。本發(fā)明實(shí)施時(shí)IMU慣性測(cè)量單元模塊1選用ADIS16355，水下導(dǎo)航計(jì)算機(jī)需用 Cyclone系列FPGA芯片EP1C12Q240I 7，運(yùn)動(dòng)控制執(zhí)行部件2即為水下運(yùn)動(dòng)體的推力發(fā)動(dòng) 機(jī)，反饋給動(dòng)力學(xué)模型估計(jì)3的是運(yùn)動(dòng)體對(duì)發(fā)動(dòng)機(jī)的控制參數(shù)，動(dòng)力學(xué)模型估計(jì)3是水下運(yùn) 動(dòng)載體的力學(xué)模型，由于動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4是算法部分，編程后下載到導(dǎo)航計(jì)算機(jī)中進(jìn) 行。水下運(yùn)動(dòng)體在發(fā)射前保持穩(wěn)定狀態(tài)，只需上位機(jī)給出水下運(yùn)動(dòng)體初始航向角，發(fā)射出去 后微型水下運(yùn)動(dòng)體姿態(tài)檢測(cè)裝置即可按說明書中算法實(shí)時(shí)給出水下運(yùn)動(dòng)體姿態(tài)信息。該裝置體積小、重量輕，于其他慣性導(dǎo)航裝置相比，多了一種動(dòng)態(tài)姿態(tài)測(cè)量方法， 增強(qiáng)慣性姿態(tài)測(cè)量的精度，具有高度自主性，因此特別適用于水下環(huán)境以及對(duì)自主性姿態(tài) 測(cè)量要求較高的環(huán)境。本發(fā)明的水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置，具有體積小、重量輕、精度高、隱蔽性強(qiáng)、 使用環(huán)境廣泛等特點(diǎn)。工作時(shí)通過對(duì)運(yùn)動(dòng)控制執(zhí)行部件的輸出進(jìn)行動(dòng)力學(xué)模型估計(jì)，對(duì)慣 性測(cè)量單元的動(dòng)態(tài)測(cè)量數(shù)據(jù)進(jìn)行補(bǔ)償，將補(bǔ)償后的測(cè)量數(shù)據(jù)送入導(dǎo)航計(jì)算機(jī)進(jìn)行姿態(tài)解 算，得出運(yùn)動(dòng)體的實(shí)時(shí)姿態(tài)信息，通過串口輸出。本發(fā)明特別適用于水下環(huán)境以及對(duì)自主性 姿態(tài)測(cè)量要求較高的環(huán)境。


圖1是微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置框圖。圖2是運(yùn)動(dòng)載體受力分析和坐標(biāo)系。圖3是導(dǎo)航坐標(biāo)系和載體坐標(biāo)系的轉(zhuǎn)換。
具體實(shí)施例方式下面結(jié)合附圖舉例對(duì)本發(fā)明做更詳細(xì)的描述結(jié)合圖1，本發(fā)明的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置包括IMU慣性測(cè)量單元1， 運(yùn)動(dòng)控制執(zhí)行部件2，動(dòng)力學(xué)模型估計(jì)3，動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4，導(dǎo)航計(jì)算機(jī)5，串口輸出6，以 及相應(yīng)軟件測(cè)量方法。IMU慣性測(cè)量單元模塊1包括三軸加速度計(jì)、三軸陀螺儀、三軸溫度計(jì)以及配套數(shù) 字信號(hào)輸出電路，器件坐標(biāo)要與運(yùn)動(dòng)體坐標(biāo)對(duì)齊，并采用模塊化器件便于安裝調(diào)試。運(yùn)動(dòng)控制執(zhí)行部件2包含運(yùn)動(dòng)體所有的控制執(zhí)行機(jī)構(gòu)。運(yùn)動(dòng)學(xué)模型估計(jì)3能夠?qū)\(yùn)動(dòng)體控制執(zhí)行部件進(jìn)行較精確的動(dòng)力學(xué)描述。動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)5進(jìn)行姿態(tài)的解算，并通過 串口輸出。本裝置測(cè)量步驟如下第一步IMU慣性測(cè)量單元1按照運(yùn)動(dòng)載體坐標(biāo)系方向安裝，測(cè)量載體各軸加速度 A和角速度ω分量以及溫度信息Τ。第二步載體運(yùn)動(dòng)過程中，載體的運(yùn)動(dòng)控制執(zhí)行部件2反饋控制的執(zhí)行參數(shù)ε (比 如發(fā)動(dòng)機(jī)推動(dòng)時(shí)間)。第三步動(dòng)力學(xué)模型估計(jì)3根據(jù)運(yùn)動(dòng)控制執(zhí)行部件2的反饋參數(shù)，對(duì)載體運(yùn)動(dòng)變化 ΔΑ = f(e Hhf進(jìn)行估計(jì)，f( ε )為發(fā)動(dòng)機(jī)推力，hf為流體阻力，運(yùn)動(dòng)體受力分析和坐標(biāo)見 圖2。第四步IMU慣性測(cè)量單元1的測(cè)量數(shù)據(jù)(Α，ω)和第3步的輸出數(shù)據(jù)(ΔΑ，0)進(jìn) 行動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償4，2 =」-ΔΛ = ω ,去除運(yùn)動(dòng)控制中產(chǎn)生的加速度。第五步在導(dǎo)航計(jì)算機(jī)5中對(duì)第四步輸出的數(shù)據(jù)進(jìn)行濾波、溫度補(bǔ)償，分別利用補(bǔ) 償、濾波后的加速度計(jì)數(shù)據(jù)、角速度數(shù)據(jù)進(jìn)行姿態(tài)解算。加速度計(jì)測(cè)姿態(tài)解算按照導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的轉(zhuǎn)換矩陣計(jì)算，坐標(biāo)轉(zhuǎn)換過 程見圖3。
cosi//· sin 0sini/Acos0-sin0
cosi/a sin 0 sin 7 - sin ψ cos γ sin i/a sin 0 sin 7 + cosi/a cos 7 cos 0 sin 7 cosi/a sin θ cos γ + ηψ sin 7 sin i/a sin 0 cos 7 - cosi/a sin 7 cos^cos/式中ψ為航向角，θ為俯仰角，Υ為橫滾角，由于載體運(yùn)動(dòng)加速度通過步驟四的 補(bǔ)償，基本消除其對(duì)加速度姿態(tài)的影響，以Il A I -11作為姿態(tài)角測(cè)量方差。另外重力加速度在 航向角上沒有分量，加速度計(jì)姿態(tài)測(cè)量不包括航向角。角速度姿態(tài)測(cè)量利用速率陀螺測(cè)量的加速度采用四元數(shù)法解算。令ρ = % + q'i + qj + q3k ,則載體坐標(biāo)到導(dǎo)航坐標(biāo)的轉(zhuǎn)換可以表示為
仏2 + ql - ql - ￠1 + 姚) 2(仏？3 -q0q2)
2(^1^2 n) ql + ql - ql - 4 2( + q0q,)
Iiq^3+q0q2) 2(q2q3 - q^) ql + ql ~ q1, ~ ql_
且有隊(duì)2 + q\ + q22 +^32 =1； Cbp =
=Tt T = 根據(jù)捷聯(lián)矩陣T和C6p的關(guān)系，以穩(wěn)態(tài)時(shí)的加速度計(jì)姿態(tài)測(cè)量給出的俯仰角和橫滾角，以及上位機(jī)給出的航向角作為初始狀態(tài)，可以求出四元數(shù)的初始狀態(tài)Q(O)。四元數(shù)的微 分方程為0 = \Q(0，利用速率陀螺的輸出ω，用4階龍格庫塔法動(dòng)態(tài)求解Q⑴，通過捷聯(lián)矩 陣T實(shí)時(shí)解算運(yùn)動(dòng)載體姿態(tài)。姿態(tài)角的取值范圍為ψe
，θ e [-90°，90° ], y e [-180°， 180° ]，通過適當(dāng)?shù)膿Q算可以將反三角函數(shù)求出的角度值范圍進(jìn)行規(guī)整。最后利用聯(lián)邦卡爾曼濾波器將所測(cè)的兩組姿態(tài)角進(jìn)行組合，得出最優(yōu)估計(jì)姿態(tài)角 度信息，通過串口輸出。
權(quán)利要求
1.一種微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是由IMU慣性測(cè)量單元模 塊(1)、運(yùn)動(dòng)控制執(zhí)行部件( 、運(yùn)動(dòng)學(xué)模型估計(jì)( 、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)、水下導(dǎo)航計(jì)算 機(jī)(5)和串口輸出(6)組成；IMU慣性測(cè)量單元模塊(1)、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償G)、水下導(dǎo)航 計(jì)算機(jī)(5)和串口輸出(6)依此相連，運(yùn)動(dòng)控制執(zhí)行部件(2)連接運(yùn)動(dòng)學(xué)模型估計(jì)(3)，運(yùn) 動(dòng)學(xué)模型估計(jì)( 連接動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)。
2.根據(jù)權(quán)利要求1所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是所述 IMU慣性測(cè)量單元模塊(1)包括三軸加速度計(jì)、三軸陀螺儀、三軸溫度計(jì)以及數(shù)字信號(hào)輸出 電路。
3.根據(jù)權(quán)利要求1或2所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是 所述運(yùn)動(dòng)控制執(zhí)行部件( 為運(yùn)動(dòng)體上的所有的控制執(zhí)行機(jī)構(gòu)。
4.根據(jù)權(quán)利要求1或2所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是 所述運(yùn)動(dòng)學(xué)模型估計(jì)( 對(duì)運(yùn)動(dòng)體控制執(zhí)行部件進(jìn)行精確的動(dòng)力學(xué)描述。
5.根據(jù)權(quán)利要求3所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是所述 運(yùn)動(dòng)學(xué)模型估計(jì)( 對(duì)運(yùn)動(dòng)體控制執(zhí)行部件進(jìn)行精確的動(dòng)力學(xué)描述。
6.根據(jù)權(quán)利要求1或2所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是 所述動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)( 進(jìn)行姿態(tài)的解算，并通過 串口輸出。
7.根據(jù)權(quán)利要求3所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是所述 動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)( 進(jìn)行姿態(tài)的解算，并通過串口 輸出。
8.根據(jù)權(quán)利要求4所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是所述 動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)( 進(jìn)行姿態(tài)的解算，并通過串口 輸出。
9.根據(jù)權(quán)利要求5所述的微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置，其特征是所述 動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)輸出的數(shù)據(jù)直接輸入導(dǎo)航計(jì)算機(jī)( 進(jìn)行姿態(tài)的解算，并通過串口 輸出。
10.一種基于微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置的檢測(cè)方法，其特征是所用微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置由IMU慣性測(cè)量單元模塊(1)、運(yùn)動(dòng)控制執(zhí)行 部件O)、運(yùn)動(dòng)學(xué)模型估計(jì)(3)、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償G)、水下導(dǎo)航計(jì)算機(jī)( 和串口輸出 (6)組成；IMU慣性測(cè)量單元模塊(1)、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償G)、水下導(dǎo)航計(jì)算機(jī)(5)和串口 輸出(6)依此相連，運(yùn)動(dòng)控制執(zhí)行部件( 連接運(yùn)動(dòng)學(xué)模型估計(jì)(3)，運(yùn)動(dòng)學(xué)模型估計(jì)(3) 連接動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償；測(cè)量方法包括如下步驟第一步將IMU慣性測(cè)量單元模塊(1)按照運(yùn)動(dòng)體坐標(biāo)系方向安裝固定，按照航向、俯 仰、橫滾的順序構(gòu)造導(dǎo)航坐標(biāo)系到運(yùn)動(dòng)體坐標(biāo)系的方向余弦矩陣；第二步在運(yùn)動(dòng)體穩(wěn)定環(huán)境下設(shè)定運(yùn)動(dòng)體的初始狀態(tài)，航向角由上位機(jī)給定，俯仰角和 橫滾角通過三軸加速度計(jì)的輸出進(jìn)行解算；第三步在運(yùn)動(dòng)體運(yùn)動(dòng)過程中，根據(jù)運(yùn)動(dòng)體的運(yùn)動(dòng)控制執(zhí)行部件O)的輸出信息，經(jīng)過 動(dòng)力學(xué)模型估計(jì)(3)，估計(jì)運(yùn)動(dòng)體在載體坐標(biāo)系下的三軸加速度；第四步IMU慣性測(cè)量單元(1)的測(cè)量數(shù)據(jù)和第三步的輸出數(shù)據(jù)進(jìn)行動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ) 償G)，去除運(yùn)動(dòng)控制中產(chǎn)生的加速度，以及慣性測(cè)量單元(1)測(cè)量數(shù)據(jù)的溫度漂移；第五步在導(dǎo)航計(jì)算機(jī)(5)中對(duì)第四步輸出的數(shù)據(jù)進(jìn)行濾波，分別利用補(bǔ)償、濾波后的 加速度計(jì)數(shù)據(jù)、角速度數(shù)據(jù)進(jìn)行姿態(tài)解算，并進(jìn)行組合分析，給出運(yùn)動(dòng)體姿態(tài)的最優(yōu)估計(jì)， 通過串口輸出。
全文摘要
本發(fā)明提供的是一種微型水下運(yùn)動(dòng)體自主姿態(tài)檢測(cè)裝置該裝置。由IMU慣性測(cè)量單元模塊(1)、運(yùn)動(dòng)控制執(zhí)行部件(2)、運(yùn)動(dòng)學(xué)模型估計(jì)(3)、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)、水下導(dǎo)航計(jì)算機(jī)(5)和串口輸出(6)組成；IMU慣性測(cè)量單元模塊(1)、動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)、水下導(dǎo)航計(jì)算機(jī)(5)和串口輸出(6)依此相連，運(yùn)動(dòng)控制執(zhí)行部件(2)連接運(yùn)動(dòng)學(xué)模型估計(jì)(3)，運(yùn)動(dòng)學(xué)模型估計(jì)(3)連接動(dòng)態(tài)測(cè)量數(shù)據(jù)補(bǔ)償(4)。本發(fā)明特別適用于水下環(huán)境以及對(duì)自主性姿態(tài)測(cè)量要求較高的環(huán)境。具有體積小、重量輕、精度高、隱蔽性強(qiáng)、使用環(huán)境廣泛等特點(diǎn)。
文檔編號(hào)G01C21/16GK102087110SQ20101061222
公開日2011年6月8日 申請(qǐng)日期2010年12月29日 優(yōu)先權(quán)日2010年12月29日
發(fā)明者蘭賀, 孫堯, 安偉光, 梁鋒, 趙新華 申請(qǐng)人:哈爾濱工程大學(xué)