[發明專利]一種基于捷聯慣導的雙歐拉全姿態解算方法

有效

• 申請號: 202011287699.6
• 申請日: 2020-11-17
• 公開（公告）號: CN112484720B
• 公開（公告）日: 2023-04-04
• 發明（設計）人: 孫磊;雷曉敏;張召友
• 申請（專利權）人: 天津津航計算技術研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 中國兵器工業集團公司專利中心 11011
• 代理人: 劉二格

1.一種基于捷聯慣導的雙歐拉全姿態解算方法，其特征在于，包括如下步驟：

步驟一姿態矩陣的更新；

步驟二利用姿態矩陣求解正反歐拉角；

步驟三對反歐拉角進行定義域調整；

步驟四正、反歐拉角精華區切換；

所述步驟一中，姿態矩陣更新時，需預設的參數包括：t_k時刻的載體坐標系為p(k)，導航坐標系為n(k)，t_k+1時刻的載體坐標系為p(k+1)，導航坐標系為n(k+1)；記p(k)至p(k+1)的轉動四元數為q(h)，n(k)至n(k+1)的轉動四元數為p(h)，n(k)至p(k)的轉動四元數為Q(t_k)，n(k+1)至p(k+1)的轉動四元數為Q(t_k+1)，其中h＝t_k+1-t_k為更新周期；

所述步驟一中，姿態矩陣更新的過程為：對于空間中任一矢量r，用姿態矩陣描述，有

其中，r^n(k+1)為t_k+1時刻導航坐標系的矢量，r^p(k+1)為t_k+1時刻載體坐標系的矢量，為t_k+1時刻載體坐標系到導航坐標系的姿態矩陣，為t_k時刻到t_k+1時刻導航坐標系的姿態矩陣，為t_k時刻載體坐標系到導航坐標系的姿態矩陣，為t_k+1時刻到t_k時刻載體坐標系的姿態矩陣；

根據四元數與姿態矩陣的等效關系，得到：

因為

于是得到

其中，表示向量叉乘，p*(h)表示p(h)的共軛矩陣，以此類推，t_k+1時刻的通過四元數的更新求得，有姿態矩陣以下簡稱

所述步驟二中，利用姿態矩陣求解正反歐拉角時，首先確定正歐拉角對應的姿態矩陣的形式、反歐拉角對應的姿態矩陣的形式；其次進行無效姿態處理；最后進行常規姿態求解；

所述步驟二中，正歐拉角對應的姿態矩陣的形式

其中，P、R、H分別表示正歐拉俯仰角、滾轉角和航向角；

所述步驟二中，反歐拉角對應的姿態矩陣的形式

其中，Pr、Rr、Hr分別表示反歐拉俯仰角、滾轉角和航向角；

所述步驟二中，無效姿態處理的過程為：

求解正歐拉角時，當俯仰角P為±90°時，航向角H與滾轉角R均為無意義的值，對H和R賦零值；同理，求解反歐拉角時，當滾轉角Rr為±90°時，俯仰角Pr與航向角Hr均為無意義的值，對Pr和Hr賦零值；

所述步驟二中，常規姿態求解的過程為：

常規姿態下，P、Rr處于定義域(-90°,+90°)，根據矩陣內元素進行反三角函數計算；

求解正歐拉角，P通過的反正弦函數求出唯一解；R通過與相除，計算反正切函數求出唯一解；H通過與相除，計算反正切函數求出唯一解；

求解反歐拉角，Rr通過的反正弦函數求出唯一解；Pr通過與相除，計算反正切函數求出唯一解；Hr通過與相除，計算反正切函數求出唯一解；

所述步驟三中，對反歐拉角進行定義域調整的目的是把反歐拉俯仰角Pr的范圍從[-180°，+180°]調整到[-90°,+90°]，其過程為：對上一步計算出來的Pr取正方向的補角，對Hr和Rr取反方向的補角；若Pr大于+90°，則Pr＝+180°–Pr；若Pr小于-90°，則Pr＝-180°–Pr；當Pr不處于[-90°,+90°]范圍時，若Hr0，則Hr＝+180v+Hr；若Hr0，則Hr＝-180v+Hr；同理，若Rr0，則Rr＝+180°+Rr；若Rr0，則Rr＝-180v+Rr；

所述步驟四中，正、反歐拉角精華區切換的過程為：

如果|P|＝45°，則輸出正歐拉角(R,P,H)表示姿態；否則，輸出反歐拉角(Rr,Pr,Hr)表示姿態。