[發明專利]基于克雷洛夫角的慣性導航高精度姿態角解算方法和系統

有效

• 申請號: 202110721264.6
• 申請日: 2021-06-28
• 公開（公告）號: CN113447025B
• 公開（公告）日: 2022-07-29
• 發明（設計）人: 魏宗康;高榮榮;朱嘉婧
• 申請（專利權）人: 北京航天控制儀器研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 臧春喜

1.一種基于克雷洛夫角的慣性導航高精度姿態角解算方法，其特征在于，包括：

確定克雷洛夫角及克雷洛夫角對應的角速度方程；其中，克雷洛夫角用于描述導航坐標系與本體坐標系之間的關系，導航坐標系經過三次轉動后到達本體坐標系，三次轉動對應的轉動角分別記作偏航角俯仰角ψ和滾轉角γ；捷聯式慣性系統對應的本體坐標系記作O-X′Y′Z′，為動系，OX′軸表示載體的縱軸，OZ′軸表示載體的橫縱，OY′軸與OX′軸和OZ′軸組成右手坐標系；描述載體運動姿態角對應的導航坐標系記作O-XYZ，為定系，初始時刻OX軸指東、OY軸指北、OZ軸指天；本體坐標系O-X′Y′Z′與導航坐標系O-XYZ的原點重合，均以捷聯式慣性系統的本體的幾何中心為原點；導航坐標系到本體坐標系的轉動過程如下：導航坐標系O-XYZ繞OZ軸轉動到達坐標系O-LNZ；坐標系O-LNZ繞ON軸轉動ψ，到達坐標系O-X′NM；坐標系O-X′NM繞OX′軸轉動γ，到達本體坐標系O-X′Y′Z′；

計算得到偏航角俯仰角ψ和滾轉角γ在t_k時刻的值，分別記作：ψ_k和γ_k；

確定轉動過程中，t_k時刻時本體坐標系相對于導航坐標系的角速度

根據克雷洛夫角對應的角速度方程，以及確定的ψ_k、γ_k和進行迭代解算，得到迭代解算結果；

根據迭代解算結果，更新姿態坐標變換矩陣，并支撐速度更新和位置更新，以提高慣性導航的精度；

其中：

確定轉動過程中，t_k時刻時本體坐標系相對于導航坐標系的角速度包括：

在轉動過程中，獲取t_k時刻陀螺儀輸出的角速度：其中，陀螺儀安裝在捷聯式慣性系統本體上；和為t_k時刻本體坐標系的三軸角速度分量；

則，t_k時刻時本體坐標系相對于導航坐標系的角速度為：

克雷洛夫角對應的角速度方程為：

其中，表示本體坐標系相對于導航坐標系的角速度；

根據克雷洛夫角對應的角速度方程，以及確定的ψ_k、γ_k和進行迭代解算，得到迭代解算結果，包括：

設m＝0，計算迭代的初值：

其中，ΔT表示采樣時間，t_k+1＝t_k+ΔT；

進行如下迭代計算：

其中，上標(m)表示第m次迭代，下標k+1表示第t_k+1時刻；

若滿足則取m＝m+1，繼續進行迭代計算；

若不滿足則迭代終止，得到偏航角俯仰角ψ和滾轉角γ在t_k+1時刻的值，分別記作：ψ_k+1和γ_k+1：

根據迭代解算結果，更新姿態坐標變換矩陣，并支撐速度更新和位置更新，包括：

根據ψ_k+1和γ_k+1進行姿態坐標變換矩陣更新，得到更新后的本體坐標系相對導航坐標系的坐標變換矩陣

將更新后的坐標變換矩陣以及視加速度和重力加速度作為速度微分方程的輸入，經積分解算后，得到更新后的速度V；

將更新后的速度V作為位置微分方程的輸入，經積分解算后，得到更新后的位置r。

2.根據權利要求1所述的基于克雷洛夫角的慣性導航高精度姿態角解算方法，其特征在于，偏航角俯仰角ψ和滾轉角γ的取值范圍均為：-180°～+180°。

3.根據權利要求1所述的基于克雷洛夫角的慣性導航高精度姿態角解算方法，其特征在于，安裝在捷聯式慣性系統本體上的陀螺儀為3個單自由度陀螺儀或2個雙自由度陀螺儀。

4.一種基于克雷洛夫角的慣性導航高精度姿態角解算系統，其特征在于，包括：

第一確定模塊，用于確定克雷洛夫角及克雷洛夫角對應的角速度方程；其中，克雷洛夫角用于描述導航坐標系與本體坐標系之間的關系，導航坐標系經過三次轉動后到達本體坐標系，三次轉動對應的轉動角分別記作偏航角俯仰角ψ和滾轉角γ；捷聯式慣性系統對應的本體坐標系記作O-X′Y′Z′，為動系，OX′軸表示載體的縱軸，OZ′軸表示載體的橫縱，OY′軸與OX′軸和OZ′軸組成右手坐標系；描述載體運動姿態角對應的導航坐標系記作O-XYZ，為定系，初始時刻OX軸指東、OY軸指北、OZ軸指天；本體坐標系O-X′Y′Z′與導航坐標系O-XYZ的原點重合，均以捷聯式慣性系統的本體的幾何中心為原點；導航坐標系到本體坐標系的轉動過程如下：導航坐標系O-XYZ繞OZ軸轉動到達坐標系O-LNZ；坐標系O-LNZ繞ON軸轉動ψ，到達坐標系O-X′NM；坐標系O-X′NM繞OX′軸轉動γ，到達本體坐標系O-X′Y′Z′；

計算模塊，用于計算得到偏航角俯仰角ψ和滾轉角γ在t_k時刻的值，分別記作：ψ_k和γ_k；

第二確定模塊，用于確定轉動過程中，t_k時刻時本體坐標系相對于導航坐標系的角速度

解算模塊，用于根據克雷洛夫角對應的角速度方程，以及確定的ψ_k、γ_k和進行迭代解算，得到迭代解算結果；

更新模塊，用于根據迭代解算結果，更新姿態坐標變換矩陣，并支撐速度更新和位置更新，以提高慣性導航的精度；

其中：

第二確定模塊，具體用于：

在轉動過程中，獲取t_k時刻陀螺儀輸出的角速度：其中，陀螺儀安裝在捷聯式慣性系統本體上；和為t_k時刻本體坐標系的三軸角速度分量；

則，t_k時刻時本體坐標系相對于導航坐標系的角速度為：

克雷洛夫角對應的角速度方程為：

其中，表示本體坐標系相對于導航坐標系的角速度；

解算模塊，具體用于：

設m＝0，計算迭代的初值：

其中，ΔT表示采樣時間，t_k+1＝t_k+ΔT；

進行如下迭代計算：

其中，上標(m)表示第m次迭代，下標k+1表示第t_k+1時刻；

若滿足則取m＝m+1，繼續進行迭代計算；

若不滿足則迭代終止，得到偏航角俯仰角ψ和滾轉角γ在t_k+1時刻的值，分別記作：ψ_k+1和γ_k+1：

更新模塊，具體用于：

根據ψ_k+1和γ_k+1進行姿態坐標變換矩陣更新，得到更新后的本體坐標系相對導航坐標系的坐標變換矩陣

將更新后的坐標變換矩陣以及視加速度和重力加速度作為速度微分方程的輸入，經積分解算后，得到更新后的速度V；

將更新后的速度V作為位置微分方程的輸入，經積分解算后，得到更新后的位置r。