一種載體姿態獲取方法、裝置及設備與流程

2023-06-10 02:21:01 2

本發明涉及導航和控制技術領域，尤其涉及一種載體姿態獲取方法、裝置及設備。

背景技術：

姿態測量一般指利用傳感器的輸出獲得載體相對於導航坐標系的姿態數據，而姿態測量的方法可從以下幾個方面考慮：

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

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

3、以三軸磁力計為傳感器測量地磁感應強度在載體坐標系上的分量，根據地磁感應強度在導航坐標下的分量，利用導航坐標系與載體坐標系之間轉換的方向餘弦陣列建立方程組，最終獲得載體的姿態角。該方法與載體的運動關係不大，但缺點是容易受外界磁幹擾的影響，特別是當載體包含鐵磁性物質時，則通過磁力計就無法確定載體的絕對姿態。

技術實現要素：

本發明實施例提出了一種載體姿態獲取方法、裝置及設備，可以解決現有技術測量環境改變時載體姿態測量不準確的問題。

在一個方面，本發明實施例提供一種載體姿態獲取方法，包括：

實時獲得載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量；

根據所述載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量的變化情況，構造相應的矩陣進行載體姿態解算。

在另一方面，本發明實施例提供一種載體姿態獲取裝置，包括：

載體參量獲取單元，用於實時獲得所述載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量；

參量變化判斷單元，用於判斷所述載體磁場強度和/或載體重力加速度是否發生變化；

載體姿態解算單元，用於根據所述參量變化判斷單元的判斷結果構造相應的矩陣進行載體姿態解算。

在又一方面，本發明實施例提供一種載體姿態獲取設備，包括：

三個單軸陀螺，分別用於測量載體三軸坐標系上的轉動角速度；

三個單軸加速度計，分別用於測量載體三軸坐標系上的重力加速度；

一個三軸磁傳感器，用於測量載體三軸坐標系上的載體轉動角速度；

處理器，用於執行如上文載體姿態獲取方法。

本申請有益效果如下：

本申請實施例提供了一種載體姿態獲取方法、裝置及設備，採用三軸速率陀螺、三軸加速度計以及三軸磁力計實時測量載體轉動角速度、重力加速度及地磁強度在載體三軸上的分量，並實時進行動態姿態解算，不同的狀態下採用不同的解算方法，保證解算精度，而且具有設備重量輕、便於應用的優勢。

附圖說明

下面將參照附圖描述本發明的具體實施例，其中：

圖1示出了本發明實施例一載體姿態獲取方法的流程示意圖；

圖2示出了本發明實施例二載體姿態獲取方法的流程示意圖；

圖3示出了本發明實施例二的沿載體坐標系分布的傳感器的示意圖；

圖4示出了本發明實施例三的載體姿態獲取裝置的示意圖；

圖5示出了本發明實施例四的載體姿態獲取設備的示意圖。

具體實施方式

為了使本發明的技術方案及優點更加清楚明白，以下結合附圖對本發明的示例性實施例進行進一步詳細的說明，顯然，所描述的實施例僅是本發明的一部分實施例，而不是所有實施例的窮舉。並且在不衝突的情況下，本說明中的實施例及實施例中的特徵可以互相結合。

本發明實施例提供的載體姿態獲取方法、裝置及設備，採用三軸速率陀螺、三軸加速度計以及三軸磁力計實時測量載體轉動角速度、重力加速度及地磁強度在載體三軸上的分量，並實時進行動態姿態解算，保證解算精度，而且具有設備重量輕、便於應用的優勢。

如圖1所示，本發明實施例一的載體姿態獲取方法包括下述步驟：

步驟10，實時獲得載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量；

步驟20，根據所述載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量的變化情況，構造相應的矩陣進行載體姿態解算。

如圖2所示，本發明實施例二提供一種載體姿態獲取方法，包括下述步驟：

步驟101，進行初始對準；

在慣性導航系統上電啟動時，載體的姿態是不確定的，因此在進入導航解算前，需要將載體的初始姿態對準。

初始對準採取靜基座自對準方式，即在系統上電啟動時，需要一段時間保持載體處於相對靜止狀態，此時沒有外界對加速度的幹擾，所測得的加速度只有重力加速度。通過重力加速度在各個軸向方向的分量求取載體的初始姿態角。

步驟102，選取載體坐標系，獲得相應傳感器的輸出數據；

本實施例中選取「右-前-上」坐標系作為載體坐標系，該坐標係為慣性導航系統常用的一種載體坐標系，如圖3所示，利用沿載體坐標系分布的三個速率陀螺測量載體轉動角速度在載體坐標系三軸上的分量，gx、gy、gz分別表示繞x、y、z三個軸線方向的角速率分量。利用沿載體坐標系分布的三個重力加速度計測量重力加速度在載體坐標系三軸上的分量，ax、ay、az分別表示沿x、y、z三個軸線方向的加速度分量。利用一個磁力計測量周圍磁場強度在載體坐標系三軸上的分量，mx、my、mz分別表示在x、y、z三個軸線方向感應的磁場強度分量。這些傳感器輸出的數據是下文進行方程解算時構造矩陣的基礎數據。

在載體處於相對靜止狀態時，當前通過速率陀螺g所測得的角速率值是陀螺零偏的初始值，這是一個初始的不精確觀測量，在下文對姿態進行進一步解算過程中，通過相應的算法可以對這個估值進行精確逼近。

步驟103，根據姿態解算算法獲得載體的姿態數據；

根據現有的姿態解算算法，對傳感器輸出的狀態量融合解算，獲得載體的姿態信息，即姿態角qe以及陀螺的零偏δb，本實施例採用的算法如下：

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

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

狀態量

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

通過上述解算算法可以得出姿態角qe以及陀螺的零偏δb，獲得載體當前姿態。

步驟104，實時監測傳感器的輸出數據，判斷加速度和/或磁場環境參量是否發生變化，未發生變化時執行步驟106，否則執行步驟105；

例如當加速度計所獲得的加速度信息發生變化時，可以根據實際應用中測得加速度數據特性，判定載體處於變速運動狀態，此時需要重新構造矩陣進行航姿解算，避免因加速度變化而帶來的解算誤差。

例如，當磁力計所獲得的磁場信息發生變化時，根據實際應用中測得磁場數據特性，判定載體受到磁幹擾，此時需要重新構造對應的矩陣進行航姿解算，避免因磁幹擾而帶來的解算誤差。

步驟105，重新構造測量量矩陣z(t)和狀態矩陣x(t)，然後執行步驟103，利用重構的矩陣重新計算姿態角qe以及陀螺的零偏δb，進行載體航姿修正；

在本步驟中，當檢測到載體的重力加速度發生變化時，針對測量量矩陣z(t)和狀態矩陣x(t)進行重構，重構後的矩陣中重力加速度在三軸上的三個分量不再繼續參與計算，僅僅保留磁場強度和角速度在三軸上分量參與計算。

當檢測到磁場強度發生變化時，針對測量量矩陣z(t)和狀態矩陣x(t)進行重構，重構後的矩陣中磁場強度在三軸上的三個分量不再參與計算，僅僅保留加速度和角速度在三軸上分量參與計算。

當檢測到加速度和磁場強度均發生變化時，針對測量量矩陣z(t)和狀態矩陣x(t)進行重構，重構後的矩陣中加速度和磁場強度在三軸上的三個分量不再參與後續計算，僅僅保留角速度在三軸上的分量參與計算。

步驟106，輸出解算出的載體當前的姿態，流程結束。

在本發明實施例二的步驟103中，將處理下述四種可能狀態下的數據，這四種狀態分別為：1、重力加速度和磁場強度均未受到影響，2、重力加速度受到影響，3、磁場強度受到影響，4、重力加速度和磁場強度均受到影響。針對第2、3、4種狀態，步驟105中進行了矩陣重構，後續在步驟103中根據重構矩陣繼續進行姿態解算。

本領域技術人員可以根據本發明實施例計算獲得的姿態角qe以及陀螺的零偏δb進行載體坐標繫到導航坐標系的轉換。

如圖4所示，本發明實施例三提供一種載體姿態獲取裝置，具體包括下述單元：

載體參量獲取單元401，用於實時獲得所述載體轉動角速度、載體重力加速度和載體磁場強度在載體三軸坐標系上的分量；

參量變化判斷單元402，用於判斷所述載體磁場強度和/或載體重力加速度是否發生變化；

載體姿態解算單元403，用於根據所述參量變化判斷單元402的判斷結果構造相應的矩陣進行載體姿態解算。

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

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

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

狀態量：

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

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

如圖5所示，本發明實施例四提供一種載體姿態獲取設備，包括：

三個單軸陀螺501，分別用於測量載體三軸坐標系上的轉動角速度；

三個單軸加速度計502，分別用於測量載體三軸坐標系上的重力角速度；

一個三軸磁傳感器503，用於測量載體三軸坐標系上的載體轉動角速度；

處理器504，用於執行上文所述的載體姿態獲取方法。

其中，單軸陀螺501為單軸微機械陀螺、單軸加速度計502為單軸微機械加速度計，三軸磁傳感器503的輸出均為模擬量輸出，信號通過數模轉換a/d505化為數位訊號，數位訊號進入數位訊號處理器506進行後期信號處理，包括為了去除高頻噪聲，對信號進行濾波處理，並進行姿態解算。

其中，數位訊號處理器506在濾波處理過程中，可以選擇四種卡爾曼濾波模型分別對應：1、重力加速度和磁場強度均未受到影響，2、重力加速度受到影響，3、磁場強度受到影響，4、重力加速度和磁場強度均受到影響這四種情況，針對每個濾波模型對相關參數分別進行設置，從而能夠與不同環境下的傳感器特性相對應。並且還可將濾波的截至頻率設置的較高，保證傳感器測量數據的延時小，利於後期應用時利用角速度進行穩定控制。

本發明的設備還可以設置溫度傳感器等，數位訊號處理器506在數據處理過程中通過軟體標定和補償，剔除交叉耦合、加速度、溫度以及周圍磁場對傳感器測量輸出的影響，提高傳感器的測量精度。

此外，本實施例的設備還包括電源508用於為設備上電。設備接口採用雙路rs-232接口507。

為了描述的方便，以上所述裝置的各部分以功能分為各種模塊或單元分別描述。當然，在實施本發明時可以把各模塊或單元的功能在同一個或多個軟體或硬體中實現。

本領域內的技術人員應明白，本發明的實施例可提供為方法、系統、或電腦程式產品。因此，本發明可採用完全硬體實施例、完全軟體實施例、或結合軟體和硬體方面的實施例的形式。而且，本發明可採用在一個或多個其中包含有計算機可用程序代碼的計算機可用存儲介質(包括但不限於磁碟存儲器、cd-rom、光學存儲器等)上實施的電腦程式產品的形式。

本發明是參照根據本發明實施例的方法、設備(系統)、和電腦程式產品的流程圖和/或方框圖來描述的。應理解可由電腦程式指令實現流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結合。可提供這些電腦程式指令到通用計算機、專用計算機、嵌入式處理機或其他可編程數據處理設備的處理器以產生一個機器，使得通過計算機或其他可編程數據處理設備的處理器執行的指令產生用於實現在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的裝置。

這些電腦程式指令也可存儲在能引導計算機或其他可編程數據處理設備以特定方式工作的計算機可讀存儲器中，使得存儲在該計算機可讀存儲器中的指令產生包括指令裝置的製造品，該指令裝置實現在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能。

這些電腦程式指令也可裝載到計算機或其他可編程數據處理設備上，使得在計算機或其他可編程設備上執行一系列操作步驟以產生計算機實現的處理，從而在計算機或其他可編程設備上執行的指令提供用於實現在流程圖一個流程或多個流程和/或方框圖一個方框或多個方框中指定的功能的步驟。

儘管已描述了本發明的優選實施例，但本領域內的技術人員一旦得知了基本創造性概念，則可對這些實施例作出另外的變更和修改。所以，所附權利要求意欲解釋為包括優選實施例以及落入本發明範圍的所有變更和修改。