[發明專利]一種慣導系統全范圍姿態角提取方法

有效

• 申請號: 201811299564.4
• 申請日: 2018-10-30
• 公開（公告）號: CN109115212B
• 公開（公告）日: 2022-04-12
• 發明（設計）人: 孫偉強;趙鳳昊;景森學
• 申請（專利權）人: 中國船舶重工集團公司第七0七研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 天津盛理知識產權代理有限公司 12209
• 代理人: 王倩

本發明涉及一種慣導系統全范圍姿態角提取方法，通過依次分別凍結橫搖角和航向角的方法，可以實現縱搖角度接近±90°式橫搖角和航向角的提取。發明成果設計思路巧妙，具有創新性，較好的解決了慣性導航系統全姿態角提取這一實際工程應用問題，使得慣性導航系統在全姿態范圍的使用有了保證，具有推廣價值。

技術領域

本發明涉及慣性導航系統技術領域，尤其是一種慣導系統全范圍姿態角提取方法，可用于安裝于水下機器人、飛行器等縱搖角度超過90°載體的慣導系統姿態角提取。

背景技術

一般慣性導航系統定義縱搖角域值范圍為：(-90°，90°)，在某些特定應用領域，不僅縱搖角域值范圍要在[-180°，180°)之間，橫搖角域值范圍也要在[-180°，180°)之間，航向角域值范圍在[0°，360°)之間。

由于縱搖角接近±90°時，航向角和橫搖角耦合在一起，無法按照傳統的姿態角提取方法進行提取。

發明內容

本發明的目的在于彌補現有技術的不足之處，根據橫搖角和航向角耦合關系，設計了一種設計科學合理的慣導系統全范圍姿態角提取方法。

本發明的目的是通過以下技術手段實現的：

一種慣導系統全范圍姿態角提取方法，其特征在于：包括以下步驟：

⑴、姿態角解算公式：

定義載體姿態角矩陣為

由公式1可以計算出載體姿態角：

θ＝arctan(-C₃₁/C₃₃)………………………………(2)

上式中θ的定義域為(-0.5π,0.5π)，Φ的定義域為(-0.5π,0.5π)，Ψ的定義域為[0,2π)；

⑵、符號定義：

R_p∈(-π,π]：前一解算周期計算的橫搖角度

R_c∈(-π,π]：當前解算周期計算的橫搖角度

H_p∈[0,2π)：前一解算周期計算的航向角度

H_c∈[0,2π)：當前解算周期計算的航向角度

P_p∈(-π,π]：當前解算周期計算的縱搖角度

k：解算周期計數值，每個解算周期增長1

RsH＝R_c-H_c：橫搖角與航向角之差

RaH＝R_c+H_c：橫搖角與航向角之和

ε：縱搖角接近90°判斷閾值；

⑶、全姿態角提取方法：

首先設置判斷閾值ε，當縱搖角為正接近90°時，計算橫搖角與航向角之差RsH，計算方法見式(4)，根據計算得到的RsH，當前時刻將橫搖角凍結，即當前橫搖角值R_c等于上一解算周期的橫搖角值R_p，橫搖角與航向角之差RsH已經求出，則可計算出當前時刻的航向角H_c，下一解算周期即可更新橫搖角R_c，依次交替計算，即可求出縱搖角為正接近90°時的橫搖角和航向角；