專利名稱：一種載體姿態(tài)測量方法及其系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及姿態(tài)測量領(lǐng)域，更具體的說，本發(fā)明涉及一種用于飛行器等載體的姿態(tài)測量。
背景技術(shù)：
姿態(tài)測量是指利用各種傳感器進(jìn)行測量分析得到飛行器等載體相對(duì)于參考基準(zhǔn)的姿態(tài)數(shù)據(jù)，這里所說的姿態(tài)數(shù)據(jù)通常指載體的姿態(tài)角，包括航向角ψ、俯仰角θ和滾轉(zhuǎn)角γ。現(xiàn)有的姿態(tài)測量方法主要包括以下兩種(1)傳感器由三軸速率陀螺組成，速率陀螺測量得到的是載體運(yùn)動(dòng)的角速度，必須對(duì)其進(jìn)行積分運(yùn)算才能得到姿態(tài)角。但是微機(jī)電速率陀螺存在嚴(yán)重零點(diǎn)漂移問題，因此經(jīng)過積分計(jì)算得到的姿態(tài)角將不再準(zhǔn)確，這種測量系統(tǒng)受陀螺漂移影響嚴(yán)重。
(2)本申請(qǐng)人在申請(qǐng)?zhí)枮?1110135.0的中國專利“基于微機(jī)電技術(shù)的微型導(dǎo)航系統(tǒng)”中提供了一種由三軸磁強(qiáng)計(jì)和三軸加速度計(jì)組成的傳感器測量載體姿態(tài)的方法，該專利在本申請(qǐng)中引入作為參考。在該方法中，用三軸加速度計(jì)測量重力加速度g在載體坐標(biāo)系上三個(gè)正交軸的分量，用三軸磁強(qiáng)計(jì)測量地磁感應(yīng)強(qiáng)度h在載體坐標(biāo)系上三個(gè)正交軸的分量，根據(jù)重力加速度g和地磁感應(yīng)強(qiáng)度h在地理坐標(biāo)系中的表示，利用表述地理坐標(biāo)系和載體坐標(biāo)系之間相互轉(zhuǎn)換關(guān)系的方向余弦矩陣建立方程組，最后求得載體的姿態(tài)角。當(dāng)載體的以一定姿態(tài)平穩(wěn)運(yùn)動(dòng)時(shí)，利用該方法測量得到的姿態(tài)角精度較高。但是當(dāng)載體以加速度運(yùn)動(dòng)時(shí)或者在運(yùn)動(dòng)過程中受到擾動(dòng)時(shí)，例如飛行器在飛行過程中受氣流影響而具有某種加速度時(shí)，則三軸加速度計(jì)測量的加速度中既包括重力加速度還包括有載體的運(yùn)動(dòng)加速度，此時(shí)利用該方法計(jì)算得到的姿態(tài)角就具有相當(dāng)大的誤差。
因此就需要有一種方法能夠在做加速運(yùn)動(dòng)時(shí)，消除載體的運(yùn)動(dòng)加速度對(duì)姿態(tài)測量的影響。

發(fā)明內(nèi)容
本發(fā)明的在于克服現(xiàn)有技術(shù)的缺點(diǎn)和不足，提供一種載體姿態(tài)測量方法及其系統(tǒng)，以消除載體的運(yùn)動(dòng)加速度對(duì)姿態(tài)測量的影響。
為了實(shí)現(xiàn)上述目的，本發(fā)明提供一種載體姿態(tài)測量方法，包括測量重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量；還包括測量載體轉(zhuǎn)動(dòng)角速度在載體坐標(biāo)系三軸上的分量[ωxωyωz]；利用測得的載體轉(zhuǎn)動(dòng)角速度分量對(duì)測得的重力加速度和地磁感應(yīng)強(qiáng)度分量進(jìn)行卡爾曼濾波，得到重力加速度和地磁感應(yīng)強(qiáng)度分量真值的估計(jì)值；最后用所述估計(jì)值進(jìn)行載體姿態(tài)解算；其中，所述卡爾曼濾波的狀態(tài)方程為S·=ΩS+W;]]>所述卡爾曼濾波的量測方程為Z＝HS+V；S和Z分別為所述重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量[XgYgZgXmYmZm]的真值和量測值， 為所述真值對(duì)時(shí)間的導(dǎo)數(shù)，H為量測陣，V為量測噪聲，W為系統(tǒng)激勵(lì)噪聲，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>本發(fā)明還提供一種載體姿態(tài)測量系統(tǒng)，包括傳感器和數(shù)據(jù)處理裝置，所述數(shù)據(jù)處理裝置根據(jù)所述傳感器測得的信號(hào)進(jìn)行載體姿態(tài)解算；所述傳感器包括沿載體坐標(biāo)系三軸布置的三軸加速度計(jì)，用于測量重力加速度在載體坐標(biāo)系三軸上的分量；沿載體坐標(biāo)系三軸布置的三軸磁強(qiáng)計(jì)，用于測量地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量；沿載體坐標(biāo)系三軸布置的三軸速率陀螺，用于測量載體加速度在載體坐標(biāo)系三軸上的分量。
所述加速度計(jì)為微機(jī)電加速度計(jì)，所述磁強(qiáng)計(jì)為微機(jī)電磁強(qiáng)計(jì)，所述速率陀螺為微機(jī)電速率陀螺。
所述載體姿態(tài)測量系統(tǒng)還包括第一濾波電路和第一放大電路，所述三軸加速度計(jì)的輸出信號(hào)經(jīng)第一濾波電路和第一放大電路濾波和放大后，再經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
所述載體姿態(tài)測量系統(tǒng)還包括第二濾波電路和第二放大電路，所述三軸磁強(qiáng)計(jì)的輸出信號(hào)經(jīng)第二濾波電路和第二放大電路濾波和放大后，再經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
所述載體姿態(tài)測量系統(tǒng)還包括第三濾波電路，所述三軸速率陀螺的輸出信號(hào)經(jīng)第三濾波電路濾波后經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
所述數(shù)據(jù)處理裝置包括卡爾曼濾波模塊和姿態(tài)解算模塊；其中卡爾曼濾波模塊，利用測得的載體轉(zhuǎn)動(dòng)角速度的分量[ωxωyωz]對(duì)測得的重力加速度和地磁感應(yīng)強(qiáng)度的分量[XgYgZgXmYmZm]進(jìn)行卡爾曼濾波，輸出重力加速度和地磁感應(yīng)強(qiáng)度分量真值的估計(jì)值；其中，所述卡爾曼濾波的狀態(tài)方程為S·=ΩS+W;]]>所述卡爾曼濾波的量測方程為Z＝HS+V；S和Z分別為所述重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量[XgYgZgXmYmZm]的真值和量測值， 為所述真值對(duì)時(shí)間的導(dǎo)數(shù)，H為量測陣，V為量測噪聲，W為系統(tǒng)激勵(lì)噪聲，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0;]]>姿態(tài)解算模塊，用于根據(jù)卡爾曼濾波模塊輸出的重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量真值的估計(jì)值進(jìn)行載體姿態(tài)的解算。
本發(fā)明對(duì)現(xiàn)有的姿態(tài)測量系統(tǒng)進(jìn)行了融合，得到了一個(gè)新型的包括九個(gè)傳感器(三軸加速度計(jì)、三軸磁強(qiáng)計(jì)和三軸速率陀螺)的姿態(tài)測量系統(tǒng)。它采用包括卡爾曼濾波內(nèi)在的算法進(jìn)行數(shù)據(jù)處理，用穩(wěn)定的陀螺信號(hào)來減小運(yùn)動(dòng)加速度對(duì)系統(tǒng)的影響，而且不存在陀螺的積分漂移問題，從而得到的載體姿態(tài)動(dòng)態(tài)精度和穩(wěn)定性優(yōu)于現(xiàn)有的姿態(tài)測量系統(tǒng)。本發(fā)明可以實(shí)現(xiàn)載體的全姿態(tài)測量，即俯仰角測量范圍為-90°～+90°，滾轉(zhuǎn)角測量范圍為-180°～+180°，航向角測量范圍為0°～360°。進(jìn)一步的，本發(fā)明中的傳感器采用一體化MEMS芯片集成封裝技術(shù)，使儀器更加小型化。


圖1是地理坐標(biāo)系與載體坐標(biāo)系示意圖，其中(a)為地理坐標(biāo)系，(b)為載體坐標(biāo)系；圖2是本發(fā)明的載體姿態(tài)測量系統(tǒng)的結(jié)構(gòu)示意圖；圖3是圖2的載體姿態(tài)測量系統(tǒng)中的傳感器的分布示意圖；圖4是圖2的載體姿態(tài)測量系統(tǒng)中的數(shù)據(jù)處理裝置進(jìn)行信號(hào)處理的流程圖。
具體實(shí)施例方式
下面結(jié)合附圖和具體實(shí)施方式
對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述。
在一個(gè)實(shí)例性的實(shí)施方式中，坐標(biāo)系的建立如圖1所示。圖1(a)是以載體所在位置為原點(diǎn)建立三個(gè)正交軸組成的地理坐標(biāo)系北-東-地(即N-E-D)，其中N軸是由載體指向正北方，E軸是由載體指向正東方，D軸是由載體指向地心。圖1(b)是固連于載體100的三軸正交載體坐標(biāo)系X-Y-Z，其中X軸處于載體對(duì)稱平面內(nèi)，由載體質(zhì)心指向載體運(yùn)動(dòng)前向，Y軸垂直于載體對(duì)稱平面并指向右方；Z軸在載體對(duì)稱平面內(nèi)且垂直于X軸指向下方，圖1(b)中是以飛機(jī)作為載體100的一個(gè)實(shí)例。
本發(fā)明的姿態(tài)測量系統(tǒng)如圖2所示，它布置在如圖1中的載體100上。該姿態(tài)測量系統(tǒng)中的傳感器包括三軸正交的加速度計(jì)10、三軸正交的磁強(qiáng)計(jì)20、三軸正交的速率陀螺30。傳感器在載體100上的分布如圖3所示，三軸正交的加速度計(jì)10包括第一加速度計(jì)11、第二加速度計(jì)12和第三加速度計(jì)13，這三個(gè)加速度計(jì)11、12和13分別沿載體坐標(biāo)系的三個(gè)正交軸X-Y-Z平行配置，用于分別測量重力加速度在載體坐標(biāo)系三個(gè)正交軸X-Y-Z上的分量。三軸正交的磁強(qiáng)計(jì)20包括第一磁強(qiáng)計(jì)21、第二磁強(qiáng)計(jì)22和第三磁強(qiáng)計(jì)23，這三個(gè)磁強(qiáng)計(jì)21、22和23分別與載體坐標(biāo)系的三個(gè)正交軸X-Y-Z平行配置，用于分別測量地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三個(gè)正交軸X-Y-Z上的分量。三軸正交的速率陀螺30包括第一速率陀螺31、第二速率陀螺32和第三速率陀螺33，這三個(gè)速率陀螺31、32和33分別與載體坐標(biāo)系的三個(gè)正交軸X-Y-Z平行配置，用于分別測量載體轉(zhuǎn)動(dòng)角速度在三個(gè)正交軸X-Y-Z上的分量。
為了讓姿態(tài)測量系統(tǒng)200適用于微型飛行器，在本發(fā)明中，傳感器采用了體積小、質(zhì)量輕、功耗小的硅微結(jié)構(gòu)傳感器，這種傳感器是在單晶硅、石英晶體、鈮酸鋰等電光材料芯片上應(yīng)用光刻、腐蝕、沉積、離子注入、鍵合等微機(jī)械加工技術(shù)生產(chǎn)而成。具體地說，在本發(fā)明中，加速度計(jì)10為三軸正交的微機(jī)電加速度計(jì)，磁強(qiáng)計(jì)20為三軸正交的微機(jī)電磁強(qiáng)計(jì)，速率陀螺30為三軸正交的微機(jī)電速率陀螺，均為市售的產(chǎn)品。
返回圖2，加速度計(jì)10、磁強(qiáng)計(jì)20和速率陀螺30的輸出為模擬的電壓信號(hào)，分別經(jīng)過第一濾波電路14、第二濾波電路24和第三濾波電路34對(duì)信號(hào)進(jìn)行濾波，用于濾去傳感器輸出信號(hào)中的高頻噪聲。從濾波電路14和24輸出的信號(hào)經(jīng)過分別經(jīng)過第一放大電路15和第二放大電路25放大后進(jìn)入A/D轉(zhuǎn)換器40轉(zhuǎn)化為數(shù)字信號(hào)。由于速率陀螺30的電壓輸出范圍一般已滿足A/D轉(zhuǎn)換器40的輸入要求，因此濾波電路34輸出的信號(hào)直接進(jìn)入A/D轉(zhuǎn)換器40轉(zhuǎn)化為數(shù)字信號(hào)。
A/D轉(zhuǎn)換器40輸出的數(shù)字信號(hào)傳輸至一個(gè)數(shù)據(jù)處理裝置50進(jìn)行載體姿態(tài)的計(jì)算。其中，數(shù)據(jù)處理裝置50可以是一個(gè)具有內(nèi)嵌程序的微形處理器，由該內(nèi)嵌的程序進(jìn)行載體姿態(tài)的計(jì)算，這適用于載體100為微型飛行器的情況；數(shù)據(jù)處理裝置50也可以是一個(gè)PC機(jī)，由PC機(jī)內(nèi)存儲(chǔ)的計(jì)算機(jī)程序進(jìn)行載體姿態(tài)的計(jì)算，這適用于對(duì)姿態(tài)測量系統(tǒng)200的體積和重量要求不嚴(yán)的載體，例如大型的飛行器。特別是，當(dāng)數(shù)據(jù)處理裝置50為PC機(jī)時(shí)，A/D轉(zhuǎn)換器40可以通過一個(gè)數(shù)據(jù)傳輸模塊(圖中未示出)將數(shù)字信號(hào)傳輸至PC機(jī)，該數(shù)據(jù)傳輸模塊可以為USB傳輸模塊、串口傳輸模塊等。
如前所述，姿態(tài)測量系統(tǒng)進(jìn)行載體姿態(tài)計(jì)算的程序裝載于數(shù)據(jù)處理裝置50中，該程序包括數(shù)據(jù)獲取模塊、信號(hào)融合模塊、卡爾曼濾波模塊和姿態(tài)解算模塊，并按圖4所示的流程圖進(jìn)行。
如圖4所示，在步驟401中，數(shù)據(jù)獲取模塊控制數(shù)據(jù)處理裝置50接收來自于A/D轉(zhuǎn)換器40的數(shù)字信號(hào)，該信號(hào)為加速度計(jì)10、磁強(qiáng)計(jì)20和速率陀螺30測得的電壓信號(hào)經(jīng)模數(shù)轉(zhuǎn)換后得到的數(shù)字電壓信號(hào)。
在步驟402中，與所引用的申請(qǐng)?zhí)枮?1110135.0專利的方法相同，采用一個(gè)信號(hào)融合模塊對(duì)信號(hào)進(jìn)行處理。該信號(hào)融合模塊包括信號(hào)濾波單元和信號(hào)轉(zhuǎn)換單元。其中信號(hào)濾波單元對(duì)數(shù)據(jù)獲取模塊得到的數(shù)字信號(hào)進(jìn)行濾波，用于進(jìn)一步濾除傳感器信號(hào)中的高頻噪聲，減小噪聲對(duì)姿態(tài)測量的影響。數(shù)字信號(hào)濾波采用有限沖擊相應(yīng)(FIR)數(shù)字濾波，即數(shù)字濾波的輸出取決于有限個(gè)過去的輸入和現(xiàn)在的輸入x(n)、x(n-1)、…、x(n-N+1)。在本發(fā)明中采用了二次濾波，其濾波的輸入和輸出關(guān)系是y(n)＝h0x(n)+h1x(n-1)式中h0和h1為濾波系數(shù)。
信號(hào)轉(zhuǎn)換單元將經(jīng)信號(hào)濾波單元濾波后數(shù)字電壓信號(hào)轉(zhuǎn)化為對(duì)應(yīng)的物理量，即將與加速度計(jì)10對(duì)應(yīng)的數(shù)字電壓信號(hào)轉(zhuǎn)化為表示加速度的數(shù)字信號(hào)，將與磁強(qiáng)計(jì)20對(duì)應(yīng)的數(shù)字電壓信號(hào)轉(zhuǎn)化為表示磁感應(yīng)強(qiáng)度的數(shù)字信號(hào)，將與速率陀螺30對(duì)應(yīng)的數(shù)字電壓信號(hào)轉(zhuǎn)化為表示角速度的數(shù)字信號(hào)。其處理算式為u(n)＝(v(n)-v0)/kv。式中v(n)為輸入的電壓信號(hào)，u(n)為對(duì)應(yīng)的物理量，v0為零點(diǎn)電壓，kv為變化幅值，各傳感器的零點(diǎn)和幅值由預(yù)先標(biāo)定確定。
在步驟403中，卡爾曼濾波模塊用速率陀螺30測得的角速度對(duì)加速度計(jì)10和磁強(qiáng)計(jì)20測得的重力加速度和地磁感應(yīng)強(qiáng)度的分量進(jìn)行卡爾曼濾波。這一步驟是本發(fā)明的核心部分，為了清楚的說明該步驟，設(shè)定三軸磁強(qiáng)計(jì)在載體坐標(biāo)系X、Y和Z軸上的測得的地磁感應(yīng)強(qiáng)度的分量分別為Xm、Ym和Zm；三軸加速度計(jì)在載體坐標(biāo)系X、Y和Z軸上的測得的重力加速度分量分別為Xg、Yg和Zg；三軸速率陀螺在載體坐標(biāo)系X、Y和Z軸上的測得的載體轉(zhuǎn)動(dòng)角速度分量分別為ωx、ωy和ωz。
在本發(fā)明中，卡爾曼濾波算法的狀態(tài)方程和量測方程是針對(duì)本發(fā)明的特點(diǎn)而設(shè)計(jì)的，能夠計(jì)算得到符合系統(tǒng)性能要求的姿態(tài)角。狀態(tài)方程和量測方程分別如下S·=ΩS+W]]>(狀態(tài)方程)，Z＝HS+V(量測方程)。
其中S和Z分別為地磁感應(yīng)強(qiáng)度和重力加速度在載體坐標(biāo)系上的分量[XgYgZgXmYmZm]的真值和量測值， 為所述真值對(duì)時(shí)間的導(dǎo)數(shù)，H為量測陣(在本實(shí)施例中取作單位陣)，V為量測噪聲，W為系統(tǒng)激勵(lì)噪聲(為了計(jì)算方便，在本實(shí)施例中設(shè)定W＝0)，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>在本發(fā)明中，卡爾曼濾波算法的狀態(tài)方程描述了地磁感應(yīng)強(qiáng)度和重力加速度在載體坐標(biāo)系上的分量的真值S隨時(shí)間變化的規(guī)律，可見真值S要受由載體轉(zhuǎn)動(dòng)加速度[ωxωyωz]組成的矩陣Ω的影響?？柭鼮V波算法的量測方程描述了地磁感應(yīng)強(qiáng)度和重力加速度在載體坐標(biāo)系上的分量的量測值Z與真值S的關(guān)系。在本發(fā)明中，卡爾曼濾波過程的輸入值是當(dāng)前時(shí)刻實(shí)測的量測值Z，并結(jié)合前一時(shí)刻對(duì)真值S的估計(jì)，最終輸出當(dāng)前時(shí)刻真值S的估計(jì) 。
根據(jù)本發(fā)明給出的卡爾曼濾波算法的狀態(tài)方程和量測方程，本領(lǐng)域的技術(shù)人員即可對(duì)信號(hào)進(jìn)行卡爾曼濾波。下面以一個(gè)卡爾曼濾波的具體實(shí)現(xiàn)為例詳細(xì)描述卡爾曼濾波狀態(tài)方程需要進(jìn)行離散化處理，得到如下離散狀態(tài)方程
Sk＝φk，k-1Sk-1其中φk,k-1=I+TΩ+ΩT22+···,]]>用下標(biāo)k表示k時(shí)刻。
根據(jù)通用卡爾曼濾波過程進(jìn)行卡爾曼濾波，Sk的估計(jì) 按下述方程求解狀態(tài)一步預(yù)測S^k,k-1=φk,k-1S^k-1]]>根據(jù)當(dāng)前量測值Zk和預(yù)測狀態(tài) 來估計(jì)當(dāng)前時(shí)刻估計(jì)值 S^k=S^k,k-1+Kk(Zk-HkS^k,k-1)]]>濾波增益Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1,]]>一步預(yù)測均方誤差Pk,k-1=φk+1,kPk-1φk+1,kT,]]>估計(jì)均方誤差Pk=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT,]]>Rk為量測噪聲V的方差陣，具有關(guān)系Cov[Vk,Vj]=E[VkVjT]=Rkδkj.]]>通過上述的卡爾曼濾波過程，即可得到[XgYgZgXmYmZm]在k時(shí)刻的真值Sk的估計(jì)值 。本領(lǐng)域的一般技術(shù)人員可以理解，在給定卡爾曼濾波的狀態(tài)方程和量測方程后即可根據(jù)卡爾曼濾波的思想進(jìn)行信號(hào)濾波，但在具體實(shí)施時(shí)可對(duì)具體過程作各種變化，本發(fā)明并不局限于前述具體的實(shí)現(xiàn)。
在步驟404中，姿態(tài)計(jì)算模塊根據(jù)卡爾曼濾波后得到的估計(jì)值 進(jìn)行載體姿態(tài)解算，在這里姿態(tài)的解算與所引用的申請(qǐng)?zhí)枮?1110135.0專利的方法相同，區(qū)別之處在于在本發(fā)明中進(jìn)行姿態(tài)解算為經(jīng)過卡爾曼濾波后得到的物理量。在不引起混淆的情況下，在下面的描述中，仍然用[XgYgZgXmYmZm]表示經(jīng)卡爾曼濾波后得到的k時(shí)刻各個(gè)物理量的估計(jì)值。具體描述如下載體在空間中的姿態(tài)可用載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的運(yùn)動(dòng)來表示，運(yùn)動(dòng)角度稱為載體的姿態(tài)角。導(dǎo)航學(xué)中常用航向角ψ、俯仰角θ和滾轉(zhuǎn)角γ作為載體的姿態(tài)角，起始時(shí)兩坐標(biāo)系重合(N與X軸、E與Y軸、D與Z軸相對(duì)應(yīng))，隨后載體繞D軸(Z)偏行ψ角，再繞水平Y(jié)’軸俯仰θ角，最后繞X”軸滾轉(zhuǎn)γ角。載體坐標(biāo)系與地理坐標(biāo)系中的矢量可通過以下方向余弦矩陣進(jìn)行相互轉(zhuǎn)換
Cnb=cosψcosγ+sinψsinθsinγsinψcosθcosψsinγ-sinψsinθcosγcosψsinθsinγ-sinψcosγcosψcosθ-sinψsinγ-cosψsinθγcos-cosθsinγsinθcosθcosγ=[Tij]3×3]]>其上標(biāo)b表示載體坐標(biāo)系，下標(biāo)n表示地理坐標(biāo)系，為矩陣Tij中的第i行第j列元素。
地磁場h和重力加速度g在地理坐標(biāo)系和姿態(tài)坐標(biāo)系中的表示通過方向余弦矩陣Cnb進(jìn)行轉(zhuǎn)換，從而得到以下姿態(tài)角解算算式，其中h為地磁感應(yīng)強(qiáng)度，β為地磁傾角。
XmYmZm=hCnbcosβ0sinβ⇒h×(T11cosβ+T13sinβ)=Xmh×(T21cosβ+T23sinβ)=Ymh×(T31cosβ+T33sinβ)=Zm]]>XgYgZg=Cnb00g⇒xg=T13gyg=T23gzg=T33g]]>雖然在本發(fā)明中采用了卡爾曼濾波來消除載體運(yùn)動(dòng)加速度對(duì)姿態(tài)測量的影響，但是通過本發(fā)明提供的帶有九個(gè)傳感器(三軸加速度計(jì)、三軸磁強(qiáng)計(jì)和三軸速率陀螺)的姿態(tài)測量系統(tǒng)，本領(lǐng)域的技術(shù)人員也可以設(shè)計(jì)出其它的姿態(tài)測量算法來進(jìn)行高精度的姿態(tài)測量，因此本發(fā)明的姿態(tài)測量系統(tǒng)并不局限于使用本發(fā)明提供的姿態(tài)測量方法，其它的測量方法也可以適用于本發(fā)明的姿態(tài)測量系統(tǒng)。
權(quán)利要求
1.一種載體姿態(tài)測量方法，包括測量重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量；其特征在于，還包括測量載體轉(zhuǎn)動(dòng)角速度在載體坐標(biāo)系三軸上的分量[ωxωyωz]；利用測得的載體轉(zhuǎn)動(dòng)角速度分量對(duì)測得的重力加速度和地磁感應(yīng)強(qiáng)度分量進(jìn)行卡爾曼濾波，得到重力加速度和地磁感應(yīng)強(qiáng)度分量真值的估計(jì)值；最后用所述估計(jì)值進(jìn)行載體姿態(tài)解算；其中，所述卡爾曼濾波的狀態(tài)方程為S·=ΩS+W;]]>所述卡爾曼濾波的量測方程為Z＝HS+V；S和Z分別為所述重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量[XgYgZgXmYmZm]的真值和量測值， 為所述真值對(duì)時(shí)間的導(dǎo)數(shù)，H為量測陣，V為量測噪聲，W為系統(tǒng)激勵(lì)噪聲，Ω=-ωibbk00-ωibbk,]]>ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>
2.一種載體姿態(tài)測量系統(tǒng)，包括傳感器和數(shù)據(jù)處理裝置，所述數(shù)據(jù)處理裝置根據(jù)所述傳感器測得的信號(hào)進(jìn)行載體姿態(tài)解算；其特征在于，所述傳感器包括沿載體坐標(biāo)系三軸布置的三軸加速度計(jì)，用于測量重力加速度在載體坐標(biāo)系三軸上的分量；沿載體坐標(biāo)系三軸布置的三軸磁強(qiáng)計(jì)，用于測量地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量；沿載體坐標(biāo)系三軸布置的三軸速率陀螺，用于測量載體加速度在載體坐標(biāo)系三軸上的分量。
3.根據(jù)權(quán)利要求2所述的載體姿態(tài)測量系統(tǒng)，其特征在于，還包括第一濾波電路和第一放大電路，所述三軸加速度計(jì)的輸出信號(hào)經(jīng)第一濾波電路和第一放大電路濾波和放大后，再經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
4.根據(jù)權(quán)利要求2所述的載體姿態(tài)測量系統(tǒng)，其特征在于，還包括第二濾波電路和第二放大電路，所述三軸磁強(qiáng)計(jì)的輸出信號(hào)經(jīng)第二濾波電路和第二放大電路濾波和放大后，再經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
5.根據(jù)權(quán)利要求2所述的載體姿態(tài)測量系統(tǒng)，其特征在于，還包括第三濾波電路，所述三軸速率陀螺的輸出信號(hào)經(jīng)第三濾波電路濾波后經(jīng)A/D轉(zhuǎn)換器進(jìn)入所述數(shù)據(jù)處理裝置。
6.根據(jù)權(quán)利要求2所述的載體姿態(tài)測量系統(tǒng)，其特征在于，所述數(shù)據(jù)處理裝置包括卡爾曼濾波模塊和姿態(tài)解算模塊；其中卡爾曼濾波模塊，利用測得的載體轉(zhuǎn)動(dòng)角速度的分量[ωxωyωz]對(duì)測得的重力加速度和地磁感應(yīng)強(qiáng)度的分量[XgYgZgXmYmZm]進(jìn)行卡爾曼濾波，輸出重力加速度和地磁感應(yīng)強(qiáng)度分量真值的估計(jì)值；其中，所述卡爾曼濾波的狀態(tài)方程為S·=ΩS+W;]]>所述卡爾曼濾波的量測方程為Z＝HS+V；S和Z分別為所述重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量[XgYgZgXmYmZm]的真值和量測值， 為所述真值對(duì)時(shí)間的導(dǎo)數(shù)，H為量測陣，V為量測噪聲，W為系統(tǒng)激勵(lì)噪聲，Ω=-ωibbk00-ωibbk,]]>ωibbk=0-ωzωyωz0-ωx-ωyωx0;]]>姿態(tài)解算模塊，用于根據(jù)卡爾曼濾波模塊輸出的重力加速度和地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系三軸上的分量真值的估計(jì)值進(jìn)行載體姿態(tài)的解算。
7.根據(jù)權(quán)利要求2所述的載體姿態(tài)測量系統(tǒng)，其特征在于，所述加速度計(jì)為微機(jī)電加速度計(jì)，所述磁強(qiáng)計(jì)為微機(jī)電磁強(qiáng)計(jì)，所述速率陀螺為微機(jī)電速率陀螺。
全文摘要
本發(fā)明公開了一種載體姿態(tài)測量方法和系統(tǒng)。該系統(tǒng)包括傳感器和數(shù)據(jù)處理裝置，傳感器包括三軸加速度計(jì)、三軸磁強(qiáng)計(jì)和三軸速率陀螺，分別測量重力加速度、地磁感應(yīng)強(qiáng)度和載體運(yùn)動(dòng)角速度在載體坐標(biāo)系三軸上的分量。在進(jìn)行信號(hào)處理時(shí)，利用測得的載體轉(zhuǎn)動(dòng)角速度分量對(duì)測得的重力加速度和地磁感應(yīng)強(qiáng)度分量進(jìn)行卡爾曼濾波，得到重力加速度和地磁感應(yīng)強(qiáng)度分量真值的估計(jì)值，最后用該估計(jì)值進(jìn)行載體姿態(tài)解算。本發(fā)明用穩(wěn)定的陀螺信號(hào)來減小運(yùn)動(dòng)加速度對(duì)系統(tǒng)的影響，而且不存在陀螺的積分漂移問題，從而得到的載體姿態(tài)動(dòng)態(tài)精度和穩(wěn)定性優(yōu)于現(xiàn)有的姿態(tài)測量系統(tǒng)。本發(fā)明中的傳感器采用一體化MEMS芯片集成封裝技術(shù)，使儀器更加小型化。
文檔編號(hào)G01C21/10GK1664506SQ200410004660
公開日2005年9月7日 申請(qǐng)日期2004年3月5日 優(yōu)先權(quán)日2004年3月5日
發(fā)明者周兆英, 朱榮, 王鼎渠, 王曉浩, 宋宇寧, 王勁東 申請(qǐng)人:清華大學(xué)