專利名稱：微小型動態(tài)載體姿態(tài)測量裝置及其測量方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種微小型載體姿態(tài)測量裝置，特別適用于動態(tài)載體姿態(tài)測量，屬于信號處理、測量、導(dǎo)航領(lǐng)域。
本發(fā)明還涉及根據(jù)此裝置的一種測量方法。
背景技術(shù)：
姿態(tài)信息的測量對于飛機(jī)、車輛、船舶等運(yùn)動載體具有十分重要的意義。傳統(tǒng)方法中，人們用陀螺儀測量運(yùn)動載體的姿態(tài)角。好的陀螺儀具有相當(dāng)高的精度，但同時它也存在價格昂貴、結(jié)構(gòu)復(fù)雜、體積大、質(zhì)量重等缺點，這使它的應(yīng)用受到了很大限制。速率陀螺價格較低、體積小、質(zhì)量輕，但它的精度較低，漂移嚴(yán)重，因此不能直接用來測量姿態(tài)角。
利用加速度傳感器和地磁傳感器測量重力加速度和地磁場的3軸分量也可以解算出載體的姿態(tài)角(如美國專利20020188416)。這類傳感器的體積能夠做到很小，但由于加速度傳感器同時也敏感運(yùn)動加速度，因此容易受到運(yùn)動加速度干擾，不能用在非平衡狀態(tài)下。
另一種方法是在加速度和地磁傳感器的基礎(chǔ)上，加入速率陀螺，構(gòu)造基于四元數(shù)的卡爾曼濾波器，對姿態(tài)角進(jìn)行實時估計(如已授權(quán)的美國專利6647352)。此方法利用3軸速率陀螺輸出更新狀態(tài)量，采用3軸加速度和3軸磁場計的測量值作為觀測值，在最小二乘意義下得到狀態(tài)變量的最優(yōu)估計，進(jìn)而得到姿態(tài)角的估計值。此方法的優(yōu)點在于能夠較好的濾除傳感器信號的隨機(jī)干擾，補(bǔ)償陀螺的漂移，有效地提高了精度。但它要求在長時間內(nèi)重力加速度的測量值是無偏的，也就是說，當(dāng)載體處于長時間處于加速(或者減速、轉(zhuǎn)彎)狀態(tài)時，此方法的精度將嚴(yán)重降低，甚至不可用。為了解決這一問題，人們提出了根據(jù)不同狀態(tài)自動調(diào)整濾波器噪聲的方法。這種方法在長時間處于加、減速或者轉(zhuǎn)彎狀態(tài)時，增加對陀螺信號的依賴程度。其前提是陀螺信號在這一段時間內(nèi)漂移沒有明顯變化，這對陀螺的性能提出了較高的要求，但使用高精度陀螺，會增加系統(tǒng)的價格和重量，而且隨著時間的增長，誤差會不斷累積，因此仍然不能解決這一問題。
此外，利用差分GPS或者GPS陣列的方法測量載體姿態(tài)角也是一種常用方法，這種方法容易受到衛(wèi)星信號丟失的影響，在較多遮擋的情況下效果不好，GPS接收天線的位置和距離也會影響姿態(tài)測量的精度。

發(fā)明內(nèi)容
本發(fā)明介紹了一種微小型動態(tài)載體姿態(tài)測量裝置。該裝置包括3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)、溫度傳感器(104)、單軸速度傳感器(105)、模數(shù)轉(zhuǎn)換電路(106)、微處理器和存儲器(107)、串行通訊接口(108)，以及相應(yīng)軟件測量方法。其中單軸速度傳感器(105)為可選器件，沒有它系統(tǒng)仍能正常工作，但加入單軸速度傳感器(105)有助于提高測量精度。
3軸速率陀螺(101)到單軸速度傳感器(105)5種傳感器的輸出電壓信號分別連接到模數(shù)轉(zhuǎn)換電路(106)的各路輸入端。模數(shù)轉(zhuǎn)換電路(106)的控制信號和數(shù)據(jù)信號分別與微處理器和存儲器(107)的對應(yīng)端口相連接。微處理器和存儲器(107)的輸出端口與串口通訊單元(108)相連接。微處理器和存儲器(107)按照預(yù)設(shè)的采樣率控制模數(shù)轉(zhuǎn)換電路(106)對各路傳感器信號進(jìn)行采樣，并讀取相應(yīng)數(shù)據(jù)，進(jìn)行處理，計算出姿態(tài)角，存儲或輸出到串口通訊單元(108)。
利用上述測量裝置測量載體姿態(tài)信息，步驟如下第一步，將測量裝置與被測載體固連。定義地理坐標(biāo)系和載體坐標(biāo)系如圖2所示。圖2(a)為地理坐標(biāo)系，N、E、D三個正交坐標(biāo)軸分別指向北、東、地3個方向。圖2(b)為載體坐標(biāo)系，x、y、z三個正交軸分別與N、E、D三軸對齊時，各姿態(tài)角定義為零，方向以右手定則為準(zhǔn)。姿態(tài)角的符號表示與范圍可規(guī)定為俯仰角(θ)范圍-90°～90°，滾轉(zhuǎn)角(φ)范圍-180°～180°，航向角(ψ)范圍0°～360°。
按照航向→俯仰→滾轉(zhuǎn)的順序，由慣性坐標(biāo)到載體坐標(biāo)的方向余弦矩陣R可以表示為R=cψ·cθsψ·cθ-sθcψ·sθ·sφ-sψ·cφsψ·sθ·sφ+cψ·cφcθ·sφcψ·sθ·cφ+sψ·sφsψ·sθ·cφ-cψ·sφcθ·cφ]]>其中s和c分別為函數(shù)sin和cos的簡寫。
本發(fā)明用方向余弦矩陣R的第一列和第三列構(gòu)造一個向量v，v＝[r11r21r31r13r23r33]其中rij表示在R中的第i行j列個元素。
第二步，系統(tǒng)初始化。設(shè)定采樣周期，設(shè)定狀態(tài)變量的初始值，狀態(tài)估計誤差的協(xié)方差矩陣的初始值，測量噪聲，過程噪聲。將測量裝置靜態(tài)放置，設(shè)定3軸速率陀螺的零點。
狀態(tài)估計誤差的協(xié)方差矩陣的初始值可任意給定，不影響系統(tǒng)的收斂性。測量噪聲和過程噪聲的大小則要根據(jù)具體傳感器和應(yīng)用環(huán)境決定。狀態(tài)變量的初始值需要通過初始對準(zhǔn)來確定，比如將載體放置于各姿態(tài)角均為零的位置，然后將狀態(tài)變量的初值可以設(shè)置為v0＝[1 0 0 0 0 1]。
第三步，采集傳感器信號。由微處理器控制模數(shù)轉(zhuǎn)換電路按照采樣周期采集各路傳感器數(shù)據(jù)，讀取模數(shù)轉(zhuǎn)換的結(jié)果到微處理器中。
第四步，對傳感器數(shù)據(jù)進(jìn)行補(bǔ)償和標(biāo)定。按照傳感器溫度特性曲線對傳感器采樣結(jié)果進(jìn)行溫度補(bǔ)償。由于安裝引起的誤差也需要進(jìn)行標(biāo)定。
傳感器3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)、單軸速度傳感器(105)的溫度特性曲線要經(jīng)過預(yù)先測定，并保存在存儲器中。根據(jù)采樣時的溫度值，微處理器從存儲器中讀取溫度特性曲線對應(yīng)值，對上述傳感器數(shù)據(jù)進(jìn)行補(bǔ)償。如果不能保證加速度計(103)安裝在載體的轉(zhuǎn)動中心，則根據(jù)當(dāng)前轉(zhuǎn)動角速度與到轉(zhuǎn)動中心的距離計算出由于轉(zhuǎn)動引起的向心加速度，進(jìn)行補(bǔ)償。
ax=ax′-(ωy2+ωz2)Xax---(1)]]>其中Xax為x軸加速度計到轉(zhuǎn)動中心的距離，ax′為加速度計的測量值，ax為補(bǔ)償由于轉(zhuǎn)動和偏心引起的加速度后的加速度值。
第五步，利用陀螺數(shù)據(jù)更新狀態(tài)變量。利用3軸速率陀螺測量值和前一時刻的狀態(tài)變量的估計值可以計算出當(dāng)前時刻狀態(tài)變量的一步預(yù)報估計值。
更新公式如下v^(kh+h|kh)=F·v^(kh)---(2)]]>其中F為陀螺當(dāng)前角速率的函數(shù)， 為kh時刻狀態(tài)變量v的估計值， 為狀態(tài)變量的一步預(yù)報估計。
第六步，根據(jù)觀測值和約束條件修正狀態(tài)變量。將3軸磁場計、單軸加速度計、以及兩個狀態(tài)變量的約束條件作為測量值，對狀態(tài)變量的一步預(yù)報值進(jìn)行修正。
根據(jù)測量值和約束條件，對一步預(yù)報得到的狀態(tài)量 進(jìn)行修正P(kh+h|kh)＝F·P(kh)·FT+ΦevK(kh+h)＝P(kh+h|kh)·CT[C·P(kh+h|kh)·CT+Φey]-1v^(kh+h)=v^(kh+h|kh)+K(kh+h)·[y(kh+h)-C·v^(kh+h|kh)]]]>P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)。
(3)第七步，計算并輸出當(dāng)前姿態(tài)角。根據(jù)狀態(tài)變量計算當(dāng)前姿態(tài)角信息，并根據(jù)需要存儲或者輸出。
通過狀態(tài)變量v解算出姿態(tài)角，具體公式如下俯仰角θ＝-arcsin(r13) (4)滾轉(zhuǎn)角 航向角ψ=arccos(r11/cos(θ)),r31×r23-r33×r21≥02π-arccos(r11/cos(θ)),r31×r32-r33×r21<0---(6)]]>第八步，跳轉(zhuǎn)到第三步或者退出。跳轉(zhuǎn)到第三步繼續(xù)測量，或者停止退出。
第五步和第六步基于以下離散狀態(tài)空間模型v(kh+h)＝F·v(kh)+ev(kh) (7)y(kh+h)＝C·v(kh+h)+ey(kh+h) (8)其中F=I+h×Ω(ω)00Ω(ω),]]>y(kh+h)＝[mx(kh+h) my(kh+h) mz(kh+h) ax(kh+h) 1 1]T
C=cosβ00sinβ000cosβ00sinβ000cosβ00sinβ000g00r11khr21khr31kh000000r13khr23khr33kh]]>ev和ey是功率譜密度為Фev和Фey的零均值白噪聲。
以狀態(tài)方程(7)，測量方程(8)構(gòu)造離散卡爾曼濾波器，其中測量矩陣C由上一時刻狀態(tài)變量構(gòu)成。因為方程(8)中的測量值y僅含有ax，所以不受y、z軸方向運(yùn)動加速度項的影響。如前所述，如果利用 代替ax將會消除x軸方向運(yùn)動加速度的影響，這樣在高動態(tài)環(huán)境下仍能保證高精度測量。
離散狀態(tài)空間方程由下面的連續(xù)方程按照采樣周期h離散化得到。
對方向余弦矩陣R取微分，得到方程如下dRdt=Ω(ω)R---(9)]]>其中Ω(ω)=0ωz-ωy-ωz0ωxωy-ωx0]]>因為v由R的第一列和第三列組成，所以由方程(9)可以得出ddtv=Ω(ω)00Ω(ω)v+ev---(10)]]>其中0為3×3的零矩陣，ev近似為零均值白噪聲，其功率譜密度近似為Фev。
重力加速度矢量在慣性坐標(biāo)系中的坐標(biāo)表示為a0＝
T。在載體坐標(biāo)系中測量值為a＝[axayaz]T。重力加速度矢量由慣性坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系a＝Ra0+ea(11)其中ea為測量噪聲。當(dāng)載體處于靜態(tài)或者低動態(tài)環(huán)境時，ea可以近似為功率譜密度為Фa的零均值白噪聲，但當(dāng)載體處于高動態(tài)環(huán)境時，加速度測量值與重力加速度分量相差很大，特別是存在長周期低頻運(yùn)動加速度時，如長時間盤旋，ea不能認(rèn)為是零均值白噪聲，但此加速度對于x軸加速度分量的影響相對較小。根據(jù)方程(6)，加速度矢量在載體坐標(biāo)系x軸方向的分量為ax＝r13g+eax(12)其中eax可近似為功率譜密度為Фax的零均值白噪聲。如果x軸方向加入速度傳感器，用 代替上式中的ax將會有助于進(jìn)一步提高精度。
地磁矢量在慣性坐標(biāo)系中的坐標(biāo)表示為m0＝[M cosβ 0 M sinβ]T，其中M為地磁矢量的模(后面表述中將M作為單位長度量省略)，β為當(dāng)?shù)氐卮艃A角，忽略地磁偏角或解算出航向角后進(jìn)行修正。在載體坐標(biāo)系中測量值為m＝[mxmymz]T。地磁矢量由慣性坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系m＝Rm0+em(13)其中em為測量噪聲，可近似為獨(dú)立的零均值白噪聲。
注意到向量v的六個元素并不是相互獨(dú)立的，存在如下兩個約束條件r112+r212+r312=1,r132+r232+r332=1---(14)]]>將約束條件(14)作為向量v的“偽測量方程”，與方程(12)、(13)一起構(gòu)成向量v的測量方程。方程(14)中含有非線性項，因此需要做線性化處理。將(14)中二次項中的一項寫到輸出方程中，即將方程(12)、(13)、(14)寫成如下形式mxmymzax11=cosβ00sinβ000cosβ00sinβ000cosβ00sinβ000g00r11r21r31000000r13r23r33×r11r21r31r13r23r33+ey---(15)]]>其中ey＝[emxemyemzeaxec1ec2]T，ec1、ec2為約束條件的計算誤差，可近似為白噪聲。
將方程(10)和方程(15)按采樣周期h離散化就會得到由方程(7)和方程(8)構(gòu)成的離散系統(tǒng)方程。


圖1是該姿態(tài)測量裝置的硬件組成框圖。
圖2是描述慣性坐標(biāo)系與載體坐標(biāo)系，其中(a)慣性坐標(biāo)系(NED)，(b)載體坐標(biāo)系(xyz)。
圖3是測量方法流程圖。
圖4是飛行器坐標(biāo)系。
具體實施例方式
下面以測量飛行器姿態(tài)為例，介紹該姿態(tài)測量裝置和方法的具體實時過程。
圖1為該姿態(tài)測量裝置的硬件組成框圖。3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)、溫度傳感器(104)、單軸速度傳感器(105)、模數(shù)轉(zhuǎn)換電路(106)、微處理器和存儲器(107)、串行通訊接口(108)，其中速度傳感器(105)為可選器件。3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)等器件選用基于MEMS技術(shù)的傳感器，體積小、重量輕。
3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)、溫度傳感器(104)、單軸速度傳感器(105)5種傳感器的輸出電壓信號分別連接到模數(shù)轉(zhuǎn)換電路(106)的各路輸入端。模數(shù)轉(zhuǎn)換電路(106)的控制信號和數(shù)據(jù)信號分別與微處理器和存儲器(107)的對應(yīng)端口相連接。微處理器和存儲器(107)的輸出端口與串口通訊單元(108)相連接。
圖2是描述慣性坐標(biāo)系與載體坐標(biāo)系，其中(a)慣性坐標(biāo)系(NED)，(b)載體坐標(biāo)系(xyz)。
圖3是測量方法流程圖，說明了利用上述裝置測量飛行器姿態(tài)的方法和步驟。
(301)將該測量裝置與飛行器固連，圖4說明了坐標(biāo)軸x、y、z分別與飛行器各對應(yīng)軸向?qū)R。x軸與飛行器機(jī)頭方向?qū)ζ洌瑈軸與飛行器機(jī)翼方向?qū)ζ?，z軸與飛行器機(jī)翼方向垂直。
(302)系統(tǒng)初始化。設(shè)定采樣周期為25Hz；將載體放置于各姿態(tài)角均為零的位置，然后將狀態(tài)變量的初值設(shè)置為；狀態(tài)估計誤差的協(xié)方差矩陣的初始值任意設(shè)定為單位矩陣；設(shè)定測量噪聲，過程噪聲的值。將測量裝置靜態(tài)放置，設(shè)定3軸速率陀螺的測量值為零點。
(303)采集傳感器信號。由微處理器控制模數(shù)轉(zhuǎn)換電路按照采樣周期采集各路傳感器數(shù)據(jù)，讀取模數(shù)轉(zhuǎn)換的結(jié)果到微處理器中。
(304)溫度補(bǔ)償和標(biāo)定，根據(jù)溫度傳感器測量結(jié)果，從存儲器中讀出預(yù)先標(biāo)定好的修正值，對傳感器數(shù)據(jù)進(jìn)行溫度補(bǔ)償。下面所用到的傳感器信號均為經(jīng)過溫度補(bǔ)償后的數(shù)據(jù)。
測量出x軸加速度計到轉(zhuǎn)動中心的距離Xax，根據(jù)當(dāng)前轉(zhuǎn)動角速度與Xax，計算出由于轉(zhuǎn)動引起的向心加速度，進(jìn)行補(bǔ)償。
ax=ax′-(ωy2+ωz2)Xax]]>其中ax′為加速度計的測量值，ax為補(bǔ)償由于轉(zhuǎn)動和偏心引起的加速度后的加速度值。
(305)利用陀螺數(shù)據(jù)更新狀態(tài)變量。利用3軸速率陀螺測量值和前一時刻的狀態(tài)變量的估計值可以計算出當(dāng)前時刻狀態(tài)變量的一步預(yù)報估計值。
更新公式如下v^(kh+h|kh)=F·v^(kh)]]>其中F為陀螺當(dāng)前角速率的函數(shù)， 為kh時刻狀態(tài)變量v的估計值， 為狀態(tài)變量的一步預(yù)報估計。
(306)根據(jù)觀測值和約束條件修正狀態(tài)變量。將3軸磁場計、單軸加速度計、以及兩個狀態(tài)變量的約束條件作為測量值，對狀態(tài)變量的一步預(yù)報值進(jìn)行修正。
根據(jù)測量值和約束條件，對一步預(yù)報得到的狀態(tài)量 進(jìn)行修正P(kh+h|kh)＝F·P(kh)·FT+ΦevK(kh+h)＝P(kh+h|kh)·CT[C·P(kh+h|kh)·CT+Φey]-1v^(kh+h)=v^(kh+h|kh)+K(kh+h)·[y(kh+h)-C·v^(kh+h|kh)]]]>P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)；(307)計算并輸出當(dāng)前姿態(tài)角。根據(jù)狀態(tài)變量計算當(dāng)前姿態(tài)角信息，并根據(jù)需要存儲或者輸出。
通過狀態(tài)變量v解算出姿態(tài)角，具體公式如下俯仰角θ＝-arcsin(r13)
滾轉(zhuǎn)角 航向角ψ=arccos(r11/cos(θ)),r31×r23-r33×r21≥02π-arccos(r11/cos(θ)),r31×r32-r33×r21<0]]>(308)跳轉(zhuǎn)到第三步或者退出。跳轉(zhuǎn)到第三步繼續(xù)測量，或者停止退出。
該裝置體積小、重量輕，與傳統(tǒng)慣性導(dǎo)航裝置相比，對傳感器性能要求不高，可以選用體積很小的MEMS傳感器，且通過特定算法，有效地消除了運(yùn)動加速度的干擾，姿態(tài)測量精度能做到較高，因此特別適用于高動態(tài)環(huán)境中對飛行器、車輛、船舶或其他載體的動態(tài)姿態(tài)進(jìn)行測量。
權(quán)利要求
1.一種微小型動態(tài)載體姿態(tài)測量裝置，其特征在于，該裝置由3軸速率陀螺(101)、3軸磁場計(102)、單軸加速度計(103)、溫度傳感器(104)、模數(shù)轉(zhuǎn)換電路(106)、微處理器和存儲器(107)、串行通訊接口(108)組成；3軸速率陀螺(101)到溫度傳感器(104)4種傳感器的輸出電壓信號分別連接到模數(shù)轉(zhuǎn)換電路(106)的各路輸入端；模數(shù)轉(zhuǎn)換電路(106)的控制信號和數(shù)據(jù)信號分別與微處理器和存儲器(107)的對應(yīng)端口相連接；微處理器和存儲器(107)的輸出端口與串口通訊單元(108)相連接。
2.根據(jù)權(quán)利要求1所述的一種微小型動態(tài)載體姿態(tài)測量裝置，其特征在于，該裝置還有一個單軸速度傳感器(105)；單軸速度傳感器(105)的輸出電壓信號連接到模數(shù)轉(zhuǎn)換電路(106)的輸入端。
3.根據(jù)權(quán)利要求1或2所述的一種微小型動態(tài)載體姿態(tài)測量裝置，其特征在于，3軸器件分別與機(jī)體坐標(biāo)系中各對應(yīng)坐標(biāo)軸對齊。
4.根據(jù)權(quán)利要求1或2所述的一種微小型動態(tài)載體姿態(tài)測量裝置，其特征在于，單軸加速度計安裝方向為運(yùn)動加速度干擾較小的一軸。
5.根據(jù)權(quán)利要求2所述的一種微小型動態(tài)載體姿態(tài)測量裝置，其特征在于，速度傳感器與加速度計同向安裝。
6.根據(jù)權(quán)利要求1所述的一種微小型動態(tài)載體姿態(tài)測量裝置進(jìn)行載體姿態(tài)信息測量的一種測量方法，其特征在于，該測量方法包括如下步驟第一步將測量裝置與被測載體固連；定義地理坐標(biāo)系和載體坐標(biāo)系；按照航向→俯仰→滾轉(zhuǎn)的順序，構(gòu)造由慣性坐標(biāo)到載體坐標(biāo)的方向余弦矩陣R其中，s和c分別為函數(shù)sin和cos的簡寫；用方向余弦矩陣R的第一列和第三列構(gòu)造一個向量v，v＝[r11r21r31r13r23r33]其中rij表示在R中的第i行j列個元素；第二步，系統(tǒng)初始化設(shè)定采樣周期，設(shè)定狀態(tài)變量的初始值，狀態(tài)估計誤差的協(xié)方差矩陣的初始值，測量噪聲，過程噪聲；將測量裝置靜態(tài)放置，設(shè)定3軸速率陀螺的零點；第三步，采集傳感器信號微處理器控制模數(shù)轉(zhuǎn)換電路按照采樣周期采集各路傳感器數(shù)據(jù)，讀取模數(shù)轉(zhuǎn)換的結(jié)果到微處理器中；第四步，對傳感器數(shù)據(jù)進(jìn)行補(bǔ)償和標(biāo)定按照傳感器溫度特性曲線對傳感器采樣結(jié)果進(jìn)行溫度補(bǔ)償；由于安裝引起的誤差也需要進(jìn)行標(biāo)定；第五步，利用陀螺數(shù)據(jù)更新狀態(tài)變量利用3軸速率陀螺測量值和前一時刻的狀態(tài)變量的估計值計算出當(dāng)前時刻狀態(tài)變量的一步預(yù)報估計值；更新公式為v^(kh+h|kh)=F·v^(kh)]]>其中F為陀螺當(dāng)前角速率的函數(shù)， 為kh時刻狀態(tài)變量v的估計值， 為狀態(tài)變量的一步預(yù)報估計；第六步，根據(jù)觀測值和約束條件修正狀態(tài)變量將3軸磁場計、單軸加速度計、以及兩個狀態(tài)變量的約束條件作為測量值，對狀態(tài)變量的一步預(yù)報值進(jìn)行修正；根據(jù)測量值和約束條件，對一步預(yù)報得到的狀態(tài)量 進(jìn)行修正P(kh+h|kh)＝F·P(kh)·FT+Φevk(kh+h)＝P(kh+h|kh)·CT[C·P(kh+h|kh)·CT+Фey]-1v^(kh+h)=v^(kh+h|kh)+K(kh+h)·[y(kh+h)-C·v^(kh+h|kh)]]]>P(kh+h)＝[I-K(kh+h)·C]·P(kh+h|kh)第七步，計算并輸出當(dāng)前姿態(tài)角根據(jù)狀態(tài)變量計算當(dāng)前姿態(tài)角信息，并根據(jù)需要存儲或者輸出；通過狀態(tài)變量v解算出姿態(tài)角，具體公式如下俯仰角θ＝-arcsin(r13)滾轉(zhuǎn)角 航向角ψ=arccos(r11/cos(θ)),r31×r23-r33×r21≥02π-arccos(r11/cos(θ)),r31×r23-r33×r21<0]]>第八步，跳轉(zhuǎn)到第三步或者退出；跳轉(zhuǎn)到第三步繼續(xù)測量，或者停止，退出，完成測量。
全文摘要
一種微小型動態(tài)載體姿態(tài)測量裝置，屬于信號測量、導(dǎo)航技術(shù)領(lǐng)域，其特點在于體積小、重量輕、適用于高動態(tài)環(huán)境，可用在飛行器、車輛、船舶或其他載體的姿態(tài)測量；該裝置包括3軸速率陀螺、3軸磁場計、單軸加速度計、溫度傳感器、模數(shù)轉(zhuǎn)換電路、微處理器和存儲器、串行通訊接口，以及相應(yīng)軟件測量方法；3軸速率陀螺等輸出連接模數(shù)轉(zhuǎn)換電路的輸入端，模數(shù)轉(zhuǎn)換電路的控制信號、數(shù)據(jù)信號分別與微處理器和存儲器對應(yīng)端相連；微處理器、存儲器輸出端與串口通訊單元相連。將該測量裝置與飛行器固連，初始化，采集傳感器信號，補(bǔ)償、標(biāo)定、更新狀態(tài)變量，計算并輸出當(dāng)前姿態(tài)角。該發(fā)明對傳感器性能要求不高，姿態(tài)測量精度較高，特別是適用動態(tài)姿態(tài)測量。
文檔編號G01C21/10GK1740746SQ20051001176
公開日2006年3月1日 申請日期2005年5月23日 優(yōu)先權(quán)日2005年5月23日
發(fā)明者熊沈蜀, 周兆英, 王立代, 魏強(qiáng) 申請人:清華大學(xué)