仰角初值θ〇、偏航角初值和滚转角初值γ 〇的数值都小于90°。
[0095] 上述步骤S2中，本周期的偏航角中间值的计算公式为：
[0096]
公式（4)
[0097] 其中，为本周期的偏航角中间值;q〇n、qin、q2n、q3n为本周期的四元数;η为本周期 的周期数，且η为大于或等于2的正整数。
[0098] 上述步骤S2中，本周期的滚转角中间值的计算公式为：
[0099]
公式（5)
[0100] 具中，γ·为本周期的浓转角中丨日」值;9〇1^11^2 1^311为本周期的四元数。
[0101] 上述步骤S3中，当上一周期的偏航角中间值大于V3且本周期的偏航角中间值小 于-V3时，偏航角的计数增加1;当上一周期的偏航角中间值小于-V3且本周期的偏航角中 间值大于V3时，偏航角的计数减小1;当上一周期的滚转角中间值大于V3且本周期的滚转 角中间值小于-V3时，滚转角的计数增加1;当上一周期的滚转角中间值小于-V3且本周期 的滚转角中间值大于V3时，滚转角的计数减小1。
[0102] 上述步骤S4中，本周期的偏航角终值的计算公式为：
[0103] i])nt = i]w+a3T;公式（6)
[0104] 其中，为本周期的偏航角终值;a为偏航角的计数。
[0105]上述步骤S4中，本周期的滚转角终值的计算公式为：
[0106] γ nt= γ nm+hn；公式（7)
[0107] 其中，Ynt为本周期的滚转角终值;b为滚转角的计数。
[0108] 上述步骤S5中，本周期的偏航角的解耦矩阵的计算公式为：
[0109]
公式（8)
[0110] 其中，为本周期的偏航角的解耦矩阵;为本周期的偏航角终值。
[0111] 上述步骤S5中，本周期的滚转角的解耦矩阵的计算公式为：
[0112]
公式(9)
[0113] 其中，为本周期的滚转角的解耦矩阵；丫 nt为本周期的滚转角终值。
[0114] 上述步骤S6中，本周期的四元数转换矩阵的表达式为：
[0115]
[0116] 公式(10)
[0117] 其中，4为本周期的四元数转换矩阵;9〇1^111、取1^311为本周期的四元数。
[0118] 上述步骤S7中，本周期的俯仰角的转换矩阵的计算公式为：
[0119] 公式（11)
[0120] 其中，&为本周期的俯仰角的转换矩阵。
[0121] 本周期的俯仰角的转换矩阵的表达式为：
[0122]
公式（12)
[01 23] 其中，C2Q、C21、C22、C23、C24、C25、C26、C27和C28为本周期的俯仰角的转换矩阵的矩阵 J L· 〇
[0124] 上述步骤S8中，本周期的俯仰角对应的四元数的计算公式为：
[0125]
;公式（13)
[0126] 其中，qQn-e、qln-e、q2n-e、q3n-e为本周期的俯仰角对应的四元数。
[0127] 上述步骤S9中，本周期的俯仰角中间值的计算公式为：
[0128] 0^= sin-1 [2(qin-0q2n-0+q0n-0q3n-0)]；公式（14)
[0129] 其中，Θ?为本周期的俯仰角中间值。
[0130] 上述步骤S10中，当本周期的俯仰角中间值大于π/4时，偏航角的计数增加1;当本 周期的俯仰角中间值小于-V4时，偏航角的计数减小1。
[0131] 上述步骤S11中，本周期的俯仰角终值的计算公式为：
[0132]
公式（15)
[0133] 其中，0nt为本周期的俯仰角终值;d为俯仰角的计数。
[0134] 显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对 本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可 以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发 明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。
【主权项】
1. 一种基于四元数的跨象限姿态角的计算方法，其特征在于，包括如下步骤： 由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值； 由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间值； 根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数;根据本周 期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数； 由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周期的 滚转角中间值和滚转角的计数计算得到本周期的滚转角终值； 由本周期的偏航角终值计算得到本周期的偏航角的解禪矩阵；由本周期的滚转角终值 计算得到本周期的滚转角的解禪矩阵； 由本周期的四元数确定本周期的四元数转换矩阵； 由本周期的偏航角的解禪矩阵、本周期的滚转角的解禪矩阵和本周期的姿态转换矩阵 计算得到本周期的俯仰角的转换矩阵； 由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数； 由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值； 根据本周期的俯仰角中间值确定俯仰角的计数； 由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。2. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述俯 仰角初值的计算公式为：其中，蛛为俯仰角初值;q日日、qi日、q2〇、Q3彷初始四元数； 所述偏航角初值的计算公式为：其中，(60为偏航角初值； 所述滚转角初值的计算公式为：其中，YO为滚转角初值； 所述俯仰角初值、所述偏航角初值和所述滚转角初值的数值都小于90°。3. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的偏航角中间值的计算公式为：其中，IlW为本周期的偏航角中间值;9〇。、91。、92。、93。为本周期的四元数;〇为本周期的周 期数，且n为大于或等于2的正整数； 所述本周期的滚转角中间值的计算公式为：其中，丫 nm为本周期的滚转角中间值;9〇。、91。、92。、93。为本周期的四元数。4. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，当上一 周期的偏航角中间值大于V3且所述本周期的偏航角中间值小于-31/3时，偏航角的计数增 加1;当上一周期的偏航角中间值小于-n/3且所述本周期的偏航角中间值大于V3时，偏航 角的计数减小1;当上一周期的滚转角中间值大于V3且所述本周期的滚转角中间值小于-n/3时，滚转角的计数增加1;当上一周期的滚转角中间值小于-V3且所述本周期的滚转角 中间值大于V3时，滚转角的计数减小1。5. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的偏航角终值的计算公式为： I^nt - 4皿+曰; 其中，4nt为本周期的偏航角终值;a为偏航角的计数； 所述本周期的滚转角终值的计算公式为： 丫 nt= 丫皿+bJT; 其中，丫 nt为本周期的滚转角终值;b为滚转角的计数。6. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的偏航角的解禪矩阵的计算公式为：其中，为本周期的偏航角的解禪矩阵;为本周期的偏航角终值； 所述本周期的滚转角的解禪矩阵的计算公式为：其中，Cy。,为本周期的滚转角的解禪矩阵；Tnt为本周期的滚转角终值。7. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的四元数转换矩阵的表达式为：其中，Cnq为本周期的四元数转换矩阵;90。冲。、92。、93。为本周期的四元数。8. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的俯仰角的转换矩阵的计算公式为： c《"=c;,;c"机; 其中，馬，,为本周期的俯仰角的转换矩阵； 所述本周期的俯仰角的转换矩阵的表达式为：其中，〔2日心1心2心3心4心日心6心7和〔28为本周期的俯仰角的转换矩阵的矩阵元。9. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述本 周期的俯仰角对应的四元数的计算公式为：其中，斯.一、A,, 9、衝9-3、恥,-W为本周期的俯仰角对应的四元数。10. 根据权利要求1所述的基于四元数的跨象限姿态角的计算方法，其特征在于，所述 本周期的俯仰角中间值的计算公式为：其中，为本周期的俯仰角中间值； 当所述本周期的俯仰角中间值大于V4时，偏航角的计数增加1;当所述本周期的俯仰 角中间值小于-V4时，偏航角的计数减小1; 所述本周期的俯仰角终值的计算公式为：其中，哉,为本周期的俯仰角终值;d为俯仰角的计数。
【专利摘要】本发明公开一种基于四元数的跨象限姿态角的计算方法，包括如下步骤：分别计算获得俯仰角初值、偏航角初值和滚转角初值；分别计算获得本周期的偏航角中间值和本周期的滚转角中间值；确定偏航角的计数；确定滚转角的计数；计算得到本周期的偏航角终值；计算得到本周期的滚转角终值；计算得到本周期的偏航角的解耦矩阵；计算得到本周期的滚转角的解耦矩阵；确定本周期的四元数转换矩阵；计算得到本周期的俯仰角的转换矩阵；计算得到本周期的俯仰角对应的四元数；计算得到本周期的俯仰角中间值；确定俯仰角的计数；计算得到本周期的俯仰角终值。所述计算方法能够避免现有技术的传统的解算方法导致的奇异性和精度下降，以保证解算的精度。
【IPC分类】G01C21/20
【公开号】CN105651285
【申请号】
【发明人】杨兴光, 杨凡
【申请人】北京电子工程总体研究所
【公开日】2016年6月8日
【申请日】2016年1月7日