[发明专利]一种惯导系统全范围姿态角提取方法

摘要

本发明涉及一种惯导系统全范围姿态角提取方法，通过依次分别冻结横摇角和航向角的方法，可以实现纵摇角度接近±90°式横摇角和航向角的提取。发明成果设计思路巧妙，具有创新性，较好的解决了惯性导航系统全姿态角提取这一实际工程应用问题，使得惯性导航系统在全姿态范围的使用有了保证，具有推广价值。

主权项

1.一种惯导系统全范围姿态角提取方法，其特征在于：包括以下步骤：⑴、姿态角解算公式：定义载体姿态角矩阵为由公式1可以计算出载体姿态角：θ＝arctan(‑C31/C33)………………………………(2)上式中θ的定义域为(‑0.5π,0.5π)，Φ的定义域为(‑0.5π,0.5π)，Ψ的定义域为[0,2π)；⑵、符号定义：Rp∈(‑π,π]：前一解算周期计算的横摇角度Rc∈(‑π,π]：当前解算周期计算的横摇角度Hp∈[0,2π)：前一解算周期计算的航向角度Hc∈[0,2π)：当前解算周期计算的航向角度Pp∈(‑π,π]：当前解算周期计算的纵摇角度k：解算周期计数值，每个解算周期增长1RsH＝Rc‑Hc：横摇角与航向角之差RaH＝Rc+Hc：横摇角与航向角之和ε：纵摇角接近90°判断阈值；⑶、全姿态角提取方法：首先设置判断阈值ε，当纵摇角为正接近90°时，计算横摇角与航向角之差RsH，计算方法见式(4)，根据计算得到的RsH，当前时刻将横摇角冻结，即当前横摇角值Rc等于上一解算周期的横摇角值Rp，横摇角与航向角之差RsH已经求出，则可计算出当前时刻的航向角Hc，下一解算周期即可更新横摇角Rc，依次交替计算，即可求出纵摇角为正接近90°时的横摇角和航向角；当纵摇角为负接近90°时，计算横摇角与航向角之和RaH，计算方法见式(4)，根据计算得到的RaH，当前时刻将航向角冻结，即当前航向角值Hc等于上一解算周期的航向角值Hp，横摇角与航向角之和RaH已经求出，则可计算出当前时刻的横摇角Rc，下一解算周期即可更新航向角Hc，依次交替计算，即可求出纵摇角为负接近90°时的横摇角和航向角；纵摇角不接近±90°时，可按照正常的横摇角和航向角提取方法公式(3)和公式(4)计算横摇角Rc和航向角Hc。