[發明專利]一種旋轉體姿態解算方法

有效

• 申請號: 201611228008.9
• 申請日: 2016-12-27
• 公開（公告）號: CN106595669B
• 公開（公告）日: 2023-04-11
• 發明（設計）人: 吳盤龍;鄧宇浩;李星秀;朱建良;王超塵
• 申請（專利權）人: 南京理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 南京蘇創專利代理事務所(普通合伙) 32273
• 代理人: 張學彪

本發明公開了一種旋轉體姿態解算方法，包括以下步驟：步驟1、利用三軸陀螺角速率信息采用旋轉矢量法優化算法解算姿態信息；步驟2、利用地磁信息計算，通過積分比值法求解橫滾角信息；步驟3、將地磁信號解算的滾轉角和陀螺信號解算的俯仰角和偏航角作為下一時刻解算四元數；步驟4、重復步驟1至步驟3即實現姿態更新。本發明利用陀螺當前角速率、角增量，以及上一時刻角增量信息，計算出姿態信息，并利用兩軸地磁信號解算出的滾轉角修正以獲得更高的精度。

技術領域

本發明屬于姿態測量領域，涉及一種旋轉體姿態解算方法。

背景技術

高動態環境下捷聯慣導系統的姿態解算是提高系統精度的關鍵技術。姿態解算是指利用載體傳感器的輸出計算分析得到的姿態角，包括航向角、俯仰角、滾轉角。對作高動態運動處在高動態環境的導彈炮彈等載體來說，姿態測量精度是決定其捷聯慣導系統能否正常工作的關鍵性因素。旋轉彈體繞自身縱軸高速旋轉，高精度陀螺儀的量程有限，難以應用在高速高旋的彈載環境。地磁場探測具有響應速度快、體積小、抗高過載能力強、無積累誤差等優點，適合用于高速高旋的常規彈藥彈體姿態測量。

磁傳感器測量彈體姿態的方法有多種方式，非正交兩軸傳感器、三軸正交傳感器、四軸傳感器等。非正交磁傳感器組合測量法解算載體姿態角主要有極值比值法等。

然而傳統的姿態解算算法存在一些不足之處。在實際采樣中，極值比值法在每個旋轉周期僅取一個數據，這就容易產生數據偶然性誤差。在噪聲較大的高動態環境下，隨著誤差的不斷積累，傳統的姿態解算將不再適用。

因此，針對上述問題提出一種新的旋轉體姿態解算方法。

發明內容

本發明的目的就在于為了解決上述問題而提供一種穩定性和抗干擾性較高的旋轉體姿態解算方法。

本發明通過以下技術方案來實現上述目的，一種旋轉體姿態解算方法，利用單軸磁傳感器S1和單軸磁傳感器S2對旋轉體進行測量，在旋轉體上設置彈體坐標系Ox₁y₁z₁，原點O為旋轉體的質點，Ox₁軸與旋轉體縱軸重合，Oy₁軸和Oz₁軸相互垂直并與Ox₁軸垂直，單軸磁傳感器S1和單軸磁傳感器S2均設置在Ox₁z₁平面內，單軸磁傳感器S1沿Oz₁軸安裝，單軸磁傳感器S2與Ox₁軸成λ角安裝，單軸磁傳感器S1的測量值為H_S1，單軸磁傳感器S2的測量值為H_S2，包括以下步驟：

步驟1、計算積分數學模型f(θ_m)的值；

步驟2、根據得到f(θ_m)的函數求解俯仰角θ_m；計算測量值H_S1或H_S2為零時的橫滾角γ；

步驟3、以步驟2的俯仰角θ_m和橫滾角γ作為初值，以俯仰角、俯仰角速度和橫滾角為狀態集，以測量值H_S1或H_S2為觀測集，用容積卡爾曼算法進行濾波。

更進一步的，步驟1中積分數學模型f(θ_m)由下式表示：

式中，ψ表示航向角，θ_m表示包含磁傾角的俯仰角，γ表示橫滾角。

更進一步的，步驟2中俯仰角θ_m通過下式計算得到：

式中，和不同時為零。

更進一步的，步驟2中磁傳感器S1的測量值H_S1＝0時，橫滾角γ通過下式計算得到：