本發(fā)明涉及導(dǎo)航和控制技術(shù)領(lǐng)域，尤其涉及一種載體姿態(tài)獲取方法、裝置及設(shè)備。

背景技術(shù)：

姿態(tài)測量一般指利用傳感器的輸出獲得載體相對于導(dǎo)航坐標(biāo)系的姿態(tài)數(shù)據(jù)，而姿態(tài)測量的方法可從以下幾個方面考慮：

1、以三軸速率陀螺為傳感器，利用其測量得到的姿態(tài)運(yùn)動角速度進(jìn)行積分運(yùn)算獲得載體的姿態(tài)角。該方法動態(tài)性能較好，但缺點(diǎn)是長期來看精度受陀螺零偏的影響嚴(yán)重。特別是微機(jī)械陀螺的零偏變化較大，則解算出來的姿態(tài)將以時間一次方的速度產(chǎn)生漂移。

2、以三軸加速度計為傳感器測量重力加速度在載體坐標(biāo)系上的分量，根據(jù)重力加速度在導(dǎo)航坐標(biāo)下的分量，利用導(dǎo)航坐標(biāo)系與載體坐標(biāo)系之間轉(zhuǎn)換的方向余弦陣列建立方程組，最終獲得載體的姿態(tài)角。該方法靜態(tài)性能較好，但缺點(diǎn)是當(dāng)載體變速運(yùn)動時，加速度計的測量不但包括重力加速度還包括載體本身的加速度，此時再利用該方法計算姿態(tài)角就會產(chǎn)生較大的誤差。

3、以三軸磁力計為傳感器測量地磁感應(yīng)強(qiáng)度在載體坐標(biāo)系上的分量，根據(jù)地磁感應(yīng)強(qiáng)度在導(dǎo)航坐標(biāo)下的分量，利用導(dǎo)航坐標(biāo)系與載體坐標(biāo)系之間轉(zhuǎn)換的方向余弦陣列建立方程組，最終獲得載體的姿態(tài)角。該方法與載體的運(yùn)動關(guān)系不大，但缺點(diǎn)是容易受外界磁干擾的影響，特別是當(dāng)載體包含鐵磁性物質(zhì)時，則通過磁力計就無法確定載體的絕對姿態(tài)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明實(shí)施例提出了一種載體姿態(tài)獲取方法、裝置及設(shè)備，可以解決現(xiàn)有技術(shù)測量環(huán)境改變時載體姿態(tài)測量不準(zhǔn)確的問題。

在一個方面，本發(fā)明實(shí)施例提供一種載體姿態(tài)獲取方法，包括：

實(shí)時獲得載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量；

根據(jù)所述載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量的變化情況，構(gòu)造相應(yīng)的矩陣進(jìn)行載體姿態(tài)解算。

在另一方面，本發(fā)明實(shí)施例提供一種載體姿態(tài)獲取裝置，包括：

載體參量獲取單元，用于實(shí)時獲得所述載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量；

參量變化判斷單元，用于判斷所述載體磁場強(qiáng)度和/或載體重力加速度是否發(fā)生變化；

載體姿態(tài)解算單元，用于根據(jù)所述參量變化判斷單元的判斷結(jié)果構(gòu)造相應(yīng)的矩陣進(jìn)行載體姿態(tài)解算。

在又一方面，本發(fā)明實(shí)施例提供一種載體姿態(tài)獲取設(shè)備，包括：

三個單軸陀螺，分別用于測量載體三軸坐標(biāo)系上的轉(zhuǎn)動角速度；

三個單軸加速度計，分別用于測量載體三軸坐標(biāo)系上的重力加速度；

一個三軸磁傳感器，用于測量載體三軸坐標(biāo)系上的載體轉(zhuǎn)動角速度；

處理器，用于執(zhí)行如上文載體姿態(tài)獲取方法。

本申請有益效果如下：

本申請實(shí)施例提供了一種載體姿態(tài)獲取方法、裝置及設(shè)備，采用三軸速率陀螺、三軸加速度計以及三軸磁力計實(shí)時測量載體轉(zhuǎn)動角速度、重力加速度及地磁強(qiáng)度在載體三軸上的分量，并實(shí)時進(jìn)行動態(tài)姿態(tài)解算，不同的狀態(tài)下采用不同的解算方法，保證解算精度，而且具有設(shè)備重量輕、便于應(yīng)用的優(yōu)勢。

附圖說明

下面將參照附圖描述本發(fā)明的具體實(shí)施例，其中：

圖1示出了本發(fā)明實(shí)施例一載體姿態(tài)獲取方法的流程示意圖；

圖2示出了本發(fā)明實(shí)施例二載體姿態(tài)獲取方法的流程示意圖；

圖3示出了本發(fā)明實(shí)施例二的沿載體坐標(biāo)系分布的傳感器的示意圖；

圖4示出了本發(fā)明實(shí)施例三的載體姿態(tài)獲取裝置的示意圖；

圖5示出了本發(fā)明實(shí)施例四的載體姿態(tài)獲取設(shè)備的示意圖。

具體實(shí)施方式

為了使本發(fā)明的技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖對本發(fā)明的示例性實(shí)施例進(jìn)行進(jìn)一步詳細(xì)的說明，顯然，所描述的實(shí)施例僅是本發(fā)明的一部分實(shí)施例，而不是所有實(shí)施例的窮舉。并且在不沖突的情況下，本說明中的實(shí)施例及實(shí)施例中的特征可以互相結(jié)合。

本發(fā)明實(shí)施例提供的載體姿態(tài)獲取方法、裝置及設(shè)備，采用三軸速率陀螺、三軸加速度計以及三軸磁力計實(shí)時測量載體轉(zhuǎn)動角速度、重力加速度及地磁強(qiáng)度在載體三軸上的分量，并實(shí)時進(jìn)行動態(tài)姿態(tài)解算，保證解算精度，而且具有設(shè)備重量輕、便于應(yīng)用的優(yōu)勢。

如圖1所示，本發(fā)明實(shí)施例一的載體姿態(tài)獲取方法包括下述步驟：

步驟10，實(shí)時獲得載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量；

步驟20，根據(jù)所述載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量的變化情況，構(gòu)造相應(yīng)的矩陣進(jìn)行載體姿態(tài)解算。

如圖2所示，本發(fā)明實(shí)施例二提供一種載體姿態(tài)獲取方法，包括下述步驟：

步驟101，進(jìn)行初始對準(zhǔn)；

在慣性導(dǎo)航系統(tǒng)上電啟動時，載體的姿態(tài)是不確定的，因此在進(jìn)入導(dǎo)航解算前，需要將載體的初始姿態(tài)對準(zhǔn)。

初始對準(zhǔn)采取靜基座自對準(zhǔn)方式，即在系統(tǒng)上電啟動時，需要一段時間保持載體處于相對靜止?fàn)顟B(tài)，此時沒有外界對加速度的干擾，所測得的加速度只有重力加速度。通過重力加速度在各個軸向方向的分量求取載體的初始姿態(tài)角。

步驟102，選取載體坐標(biāo)系，獲得相應(yīng)傳感器的輸出數(shù)據(jù)；

本實(shí)施例中選取“右-前-上”坐標(biāo)系作為載體坐標(biāo)系，該坐標(biāo)系為慣性導(dǎo)航系統(tǒng)常用的一種載體坐標(biāo)系，如圖3所示，利用沿載體坐標(biāo)系分布的三個速率陀螺測量載體轉(zhuǎn)動角速度在載體坐標(biāo)系三軸上的分量，gx、gy、gz分別表示繞x、y、z三個軸線方向的角速率分量。利用沿載體坐標(biāo)系分布的三個重力加速度計測量重力加速度在載體坐標(biāo)系三軸上的分量，ax、ay、az分別表示沿x、y、z三個軸線方向的加速度分量。利用一個磁力計測量周圍磁場強(qiáng)度在載體坐標(biāo)系三軸上的分量，mx、my、mz分別表示在x、y、z三個軸線方向感應(yīng)的磁場強(qiáng)度分量。這些傳感器輸出的數(shù)據(jù)是下文進(jìn)行方程解算時構(gòu)造矩陣的基礎(chǔ)數(shù)據(jù)。

在載體處于相對靜止?fàn)顟B(tài)時，當(dāng)前通過速率陀螺g所測得的角速率值是陀螺零偏的初始值，這是一個初始的不精確觀測量，在下文對姿態(tài)進(jìn)行進(jìn)一步解算過程中，通過相應(yīng)的算法可以對這個估值進(jìn)行精確逼近。

步驟103，根據(jù)姿態(tài)解算算法獲得載體的姿態(tài)數(shù)據(jù)；

根據(jù)現(xiàn)有的姿態(tài)解算算法，對傳感器輸出的狀態(tài)量融合解算，獲得載體的姿態(tài)信息，即姿態(tài)角qe以及陀螺的零偏δb，本實(shí)施例采用的算法如下：

狀態(tài)方程x(t)＝f(t)x(t)+g(t)w(t)；

觀測方程z(t)＝h(t)x(t)+v(t)；

狀態(tài)量

其中，z(t)為測量量矩陣，由三軸加速度值、角速度值和磁場強(qiáng)度值構(gòu)成，h(t)為測量函數(shù)矩陣，由三軸加速度、角速度和磁場強(qiáng)度測量關(guān)系函數(shù)構(gòu)成，v(t)為測量噪聲矩陣，由各傳感器的噪聲信號構(gòu)成，x(t)為狀態(tài)矩陣包含載體的姿態(tài)狀態(tài)量和陀螺零偏，f(t)為狀態(tài)轉(zhuǎn)移函數(shù)，由當(dāng)前狀態(tài)轉(zhuǎn)移到下一狀態(tài)的函數(shù)解算關(guān)系因子構(gòu)成，g(t)為控制矩陣，包含控制的目標(biāo)參量，w(t)為系統(tǒng)噪聲，其中t為時間變量。

通過上述解算算法可以得出姿態(tài)角qe以及陀螺的零偏δb，獲得載體當(dāng)前姿態(tài)。

步驟104，實(shí)時監(jiān)測傳感器的輸出數(shù)據(jù)，判斷加速度和/或磁場環(huán)境參量是否發(fā)生變化，未發(fā)生變化時執(zhí)行步驟106，否則執(zhí)行步驟105；

例如當(dāng)加速度計所獲得的加速度信息發(fā)生變化時，可以根據(jù)實(shí)際應(yīng)用中測得加速度數(shù)據(jù)特性，判定載體處于變速運(yùn)動狀態(tài)，此時需要重新構(gòu)造矩陣進(jìn)行航姿解算，避免因加速度變化而帶來的解算誤差。

例如，當(dāng)磁力計所獲得的磁場信息發(fā)生變化時，根據(jù)實(shí)際應(yīng)用中測得磁場數(shù)據(jù)特性，判定載體受到磁干擾，此時需要重新構(gòu)造對應(yīng)的矩陣進(jìn)行航姿解算，避免因磁干擾而帶來的解算誤差。

步驟105，重新構(gòu)造測量量矩陣z(t)和狀態(tài)矩陣x(t)，然后執(zhí)行步驟103，利用重構(gòu)的矩陣重新計算姿態(tài)角qe以及陀螺的零偏δb，進(jìn)行載體航姿修正；

在本步驟中，當(dāng)檢測到載體的重力加速度發(fā)生變化時，針對測量量矩陣z(t)和狀態(tài)矩陣x(t)進(jìn)行重構(gòu)，重構(gòu)后的矩陣中重力加速度在三軸上的三個分量不再繼續(xù)參與計算，僅僅保留磁場強(qiáng)度和角速度在三軸上分量參與計算。

當(dāng)檢測到磁場強(qiáng)度發(fā)生變化時，針對測量量矩陣z(t)和狀態(tài)矩陣x(t)進(jìn)行重構(gòu)，重構(gòu)后的矩陣中磁場強(qiáng)度在三軸上的三個分量不再參與計算，僅僅保留加速度和角速度在三軸上分量參與計算。

當(dāng)檢測到加速度和磁場強(qiáng)度均發(fā)生變化時，針對測量量矩陣z(t)和狀態(tài)矩陣x(t)進(jìn)行重構(gòu)，重構(gòu)后的矩陣中加速度和磁場強(qiáng)度在三軸上的三個分量不再參與后續(xù)計算，僅僅保留角速度在三軸上的分量參與計算。

步驟106，輸出解算出的載體當(dāng)前的姿態(tài)，流程結(jié)束。

在本發(fā)明實(shí)施例二的步驟103中，將處理下述四種可能狀態(tài)下的數(shù)據(jù)，這四種狀態(tài)分別為：1、重力加速度和磁場強(qiáng)度均未受到影響，2、重力加速度受到影響，3、磁場強(qiáng)度受到影響，4、重力加速度和磁場強(qiáng)度均受到影響。針對第2、3、4種狀態(tài)，步驟105中進(jìn)行了矩陣重構(gòu)，后續(xù)在步驟103中根據(jù)重構(gòu)矩陣?yán)^續(xù)進(jìn)行姿態(tài)解算。

本領(lǐng)域技術(shù)人員可以根據(jù)本發(fā)明實(shí)施例計算獲得的姿態(tài)角qe以及陀螺的零偏δb進(jìn)行載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換。

如圖4所示，本發(fā)明實(shí)施例三提供一種載體姿態(tài)獲取裝置，具體包括下述單元：

載體參量獲取單元401，用于實(shí)時獲得所述載體轉(zhuǎn)動角速度、載體重力加速度和載體磁場強(qiáng)度在載體三軸坐標(biāo)系上的分量；

參量變化判斷單元402，用于判斷所述載體磁場強(qiáng)度和/或載體重力加速度是否發(fā)生變化；

載體姿態(tài)解算單元403，用于根據(jù)所述參量變化判斷單元402的判斷結(jié)果構(gòu)造相應(yīng)的矩陣進(jìn)行載體姿態(tài)解算。

其中，所述載體姿態(tài)解算單元403未接收到所述參量變化判斷單元402的判斷結(jié)果時，利用所述載體參量獲取單元401的所述角速度、所述重力加速度和所述磁場強(qiáng)度通過下述方程進(jìn)行所述載體的姿態(tài)解算，獲得姿態(tài)角qe以及陀螺的零偏δb：

狀態(tài)方程：x(t)＝f(t)x(t)+g(t)w(t)；

觀測方程：z(t)＝h(t)x(t)+v(t)；

狀態(tài)量：

其中，所述z(t)為測量量矩陣，由所述三軸坐標(biāo)系上的三個所述重力加速度值、三個所述角速度值和三個所述磁場強(qiáng)度值構(gòu)成，所述h(t)為測量函數(shù)矩陣，由所述三軸坐標(biāo)系上的三個所述重力加速度值、三個所述角速度值和三個所述磁場強(qiáng)度的測量關(guān)系函數(shù)構(gòu)成，所述v(t)為測量噪聲矩陣，所述x(t)為由所述載體的所述姿態(tài)角和所述陀螺零偏構(gòu)成的狀態(tài)矩陣，所述f(t)為由當(dāng)前狀態(tài)轉(zhuǎn)移到下一狀態(tài)的函數(shù)解算關(guān)系因子構(gòu)成的狀態(tài)轉(zhuǎn)移函數(shù)，所述g(t)為包含控制的目標(biāo)參量的控制矩陣，所述w(t)為系統(tǒng)噪聲，其中所述t為時間變量。

其中當(dāng)所述載體姿態(tài)解算單元403接收到所述參量變化判斷單元402的所述載體磁場強(qiáng)度和/或所述載體重力加速度發(fā)生變化的判斷結(jié)果時，針對所述測量量矩陣z(t)和所述狀態(tài)矩陣x(t)進(jìn)行矩陣重構(gòu)，重構(gòu)后的矩陣中的所述載體磁場強(qiáng)度和/或所述載體重力加速度的三軸分量不再參與計算，利用重構(gòu)后的矩陣帶入所述方程中計算姿態(tài)角qe以及陀螺的零偏δb。

如圖5所示，本發(fā)明實(shí)施例四提供一種載體姿態(tài)獲取設(shè)備，包括：

三個單軸陀螺501，分別用于測量載體三軸坐標(biāo)系上的轉(zhuǎn)動角速度；

三個單軸加速度計502，分別用于測量載體三軸坐標(biāo)系上的重力角速度；

一個三軸磁傳感器503，用于測量載體三軸坐標(biāo)系上的載體轉(zhuǎn)動角速度；

處理器504，用于執(zhí)行上文所述的載體姿態(tài)獲取方法。

其中，單軸陀螺501為單軸微機(jī)械陀螺、單軸加速度計502為單軸微機(jī)械加速度計，三軸磁傳感器503的輸出均為模擬量輸出，信號通過數(shù)模轉(zhuǎn)換a/d505化為數(shù)字信號，數(shù)字信號進(jìn)入數(shù)字信號處理器506進(jìn)行后期信號處理，包括為了去除高頻噪聲，對信號進(jìn)行濾波處理，并進(jìn)行姿態(tài)解算。

其中，數(shù)字信號處理器506在濾波處理過程中，可以選擇四種卡爾曼濾波模型分別對應(yīng)：1、重力加速度和磁場強(qiáng)度均未受到影響，2、重力加速度受到影響，3、磁場強(qiáng)度受到影響，4、重力加速度和磁場強(qiáng)度均受到影響這四種情況，針對每個濾波模型對相關(guān)參數(shù)分別進(jìn)行設(shè)置，從而能夠與不同環(huán)境下的傳感器特性相對應(yīng)。并且還可將濾波的截至頻率設(shè)置的較高，保證傳感器測量數(shù)據(jù)的延時小，利于后期應(yīng)用時利用角速度進(jìn)行穩(wěn)定控制。

本發(fā)明的設(shè)備還可以設(shè)置溫度傳感器等，數(shù)字信號處理器506在數(shù)據(jù)處理過程中通過軟件標(biāo)定和補(bǔ)償，剔除交叉耦合、加速度、溫度以及周圍磁場對傳感器測量輸出的影響，提高傳感器的測量精度。

此外，本實(shí)施例的設(shè)備還包括電源508用于為設(shè)備上電。設(shè)備接口采用雙路rs-232接口507。

為了描述的方便，以上所述裝置的各部分以功能分為各種模塊或單元分別描述。當(dāng)然，在實(shí)施本發(fā)明時可以把各模塊或單元的功能在同一個或多個軟件或硬件中實(shí)現(xiàn)。

本領(lǐng)域內(nèi)的技術(shù)人員應(yīng)明白，本發(fā)明的實(shí)施例可提供為方法、系統(tǒng)、或計算機(jī)程序產(chǎn)品。因此，本發(fā)明可采用完全硬件實(shí)施例、完全軟件實(shí)施例、或結(jié)合軟件和硬件方面的實(shí)施例的形式。而且，本發(fā)明可采用在一個或多個其中包含有計算機(jī)可用程序代碼的計算機(jī)可用存儲介質(zhì)(包括但不限于磁盤存儲器、cd-rom、光學(xué)存儲器等)上實(shí)施的計算機(jī)程序產(chǎn)品的形式。

本發(fā)明是參照根據(jù)本發(fā)明實(shí)施例的方法、設(shè)備(系統(tǒng))、和計算機(jī)程序產(chǎn)品的流程圖和/或方框圖來描述的。應(yīng)理解可由計算機(jī)程序指令實(shí)現(xiàn)流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結(jié)合?？商峁┻@些計算機(jī)程序指令到通用計算機(jī)、專用計算機(jī)、嵌入式處理機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器以產(chǎn)生一個機(jī)器，使得通過計算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器執(zhí)行的指令產(chǎn)生用于實(shí)現(xiàn)在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的裝置。

這些計算機(jī)程序指令也可存儲在能引導(dǎo)計算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備以特定方式工作的計算機(jī)可讀存儲器中，使得存儲在該計算機(jī)可讀存儲器中的指令產(chǎn)生包括指令裝置的制造品，該指令裝置實(shí)現(xiàn)在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能。

這些計算機(jī)程序指令也可裝載到計算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備上，使得在計算機(jī)或其他可編程設(shè)備上執(zhí)行一系列操作步驟以產(chǎn)生計算機(jī)實(shí)現(xiàn)的處理，從而在計算機(jī)或其他可編程設(shè)備上執(zhí)行的指令提供用于實(shí)現(xiàn)在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的步驟。

盡管已描述了本發(fā)明的優(yōu)選實(shí)施例，但本領(lǐng)域內(nèi)的技術(shù)人員一旦得知了基本創(chuàng)造性概念，則可對這些實(shí)施例作出另外的變更和修改。所以，所附權(quán)利要求意欲解釋為包括優(yōu)選實(shí)施例以及落入本發(fā)明范圍的所有變更和修改。