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

有效

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

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

①、姿態角解算公式：

定義載體姿態角矩陣為

由公式(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°的判斷閾值；

③、全姿態角提取方法：

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

當縱搖角為負接近90°時，根據公式(4)，計算橫搖角與航向角之和RaH，根據計算得到的RaH，當前時刻將航向角凍結，即當前航向角值H_c等于上一解算周期的航向角值H_p，橫搖角與航向角之和RaH已經求出，則可計算出當前時刻的橫搖角R_c，下一解算周期即可更新航向角H_c，依次交替計算，即可求出縱搖角為負接近90°時的橫搖角和航向角；

縱搖角不接近±90°時，可按照正常的橫搖角和航向角提取方法公式(3)和公式(4)計算橫搖角R_c和航向角H_c。