一种基于四元数的跨象限姿态角的计算方法
【技术领域】
[0001] 本发明涉及基于四元数的姿态角的计算方法技术领域。更具体地，涉及一种基于 四元数的跨象限姿态角的计算方法。
【背景技术】
[0002] 现有技术中，由四元数求解欧拉角通常会涉及到反三角函数运算。因此，反三角函 数运算存在的值域要求和奇异性问题就不可避免。通常对欧拉角进行解算时，要对欧拉角 范围进行限定，并且在接近象限的范围内进行解算时精度会有所下降。
[0003] 因此，需要提供一种基于四元数的跨象限姿态角的计算方法。
【发明内容】
[0004] 本发明的目的在于提供一种基于四元数的跨象限姿态角的计算方法。
[0005] 为达到上述目的，本发明采用下述技术方案：
[0006] -种基于四元数的跨象限姿态角的计算方法，包括如下步骤：
[0007] 由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；
[0008] 由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中间 值；
[0009] 根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数;根据 本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；
[0010] 由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由本周 期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；
[0011] 由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚转角 终值计算得到本周期的滚转角的解耦矩阵；
[0012] 由本周期的四元数确定本周期的四元数转换矩阵；
[0013] 由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态转换 矩阵计算得到本周期的俯仰角的转换矩阵；
[0014] 由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；
[0015] 由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；
[0016] 根据本周期的俯仰角中间值确定俯仰角的计数；
[0017] 由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。
[0018] 优选地，所述俯仰角初值的计算公式为：
[0019] 0〇 = sin_1[2(qi0q20+q00q30)];
[0020] 其中，θ〇为俯仰角初值;qQ()、qi()、q2()、q3()为初始四元数；
[0021 ]所述偏航角初值的计算公式为：
[0022]
[0023]其中，Φο为偏航角初值；
[0024]所述滚转角初值的计算公式为：
[0025]
[0026] 其中，γ 〇为滚转角初值；
[0027] 所述俯仰角初值、所述偏航角初值和所述滚转角初值的数值都小于90°。
[0028] 优选地，所述本周期的偏航角中间倌的计算公式为：
[0029]
[0030]其中，为本周期的偏航角中间值;q〇n、qin、q2n、q3n为本周期的四元数;η为本周期 的周期数，且η为大于或等于2的正整数；
[0031] 所述本周期的滚转角中间值的计算公式为：
[0032]
[0033]其中，丫_为本周期的滚转角中间值;9〇1^111、92 11、9311为本周期的四元数。
[0034]优选地，当上一周期的偏航角中间值大于V3且所述本周期的偏航角中间值小于_ Ji/3时，偏航角的计数增加1;当上一周期的偏航角中间值小于-V3且所述本周期的偏航角 中间值大于V3时，偏航角的计数减小1;当上一周期的滚转角中间值大于V3且所述本周期 的滚转角中间值小于-V3时，滚转角的计数增加1;当上一周期的滚转角中间值小于-V3且 所述本周期的滚转角中间值大于V3时，滚转角的计数减小1。
[0035]优选地，所述本周期的偏航角终值的计算公式为：
[0036] - φη??+aJT;
[0037] 其中，为本周期的偏航角终值;a为偏航角的计数；
[0038] 所述本周期的滚转角终值的计算公式为：
[0039] γ nt= γ nm+blT;
[0040] 其中，γ nt为本周期的滚转角终值;b为滚转角的计数。
[0041] 优选地，所述本周期的偏航角的解耦矩阵的计算公式为：
[0042]
[0043] 其中，为本周期的偏航角的解耦矩阵;i])nt为本周期的偏航角终值；
[0044] 所述本周期的滚转角的解耦矩阵的计算公式为：
[0045]
[0046] 其中，k为本周期的滚转角的解耦矩阵；Ynt为本周期的滚转角终值。
[0047] 优选地，所述本周期的四元数转换矩阵的表达式为：
[0048]
[0049] 其中，^为本周期的四元数转换矩阵;q()n、qln、q2n、q 3n为本周期的四元数。
[0050] 优选地，所述本周期的俯仰角的转换矩阵的计算公式为：
[0051]
[0052] 其中，为本周期的俯仰角的转换矩阵；
[0053] 所沭太固期的俯仰角的转换矩阵的表达式为：
[0054]
[0055] 其中，〇20工21丄22工23丄24工25丄2 6工27和〇28为本周期的俯仰角的转换矩阵的矩阵 J L· 〇
[0056] 优选地，所述本周期的俯仰角对应的四元数的计算公式为：
[0057]
[0058] 其中，q〇n-e、qln-e、q2n-e、q3n-e为本周期的俯仰角对应的四元数。
[0059] 优选地，所述本周期的俯仰角中间值的计算公式为：
[0060] -sin [2(qin-0q2n-0+q0n-0q3n-0)]；
[0061 ]其中，Θ?为本周期的俯仰角中间值；
[0062] 当所述本周期的俯仰角中间值大于V4时，偏航角的计数增加1;当所述本周期的 俯仰角中间值小于-V4时，偏航角的计数减小1;
[0063] 所沭本周期的俯仰角终值的计算公式为：
[0064]
[0065] 其中，9nt为本周期的俯仰角终值;d为俯仰角的计数。
[0066]本发明的有益效果如下：
[0067]本发明的所述计算方法根据四元数解算的特点，对三个姿态角进行解耦处理，同 时对基准坐标系进行定量旋转，使姿态角解算得到的值始终在小于45°的范围内，从而能够 避免现有技术的传统的解算方法导致的奇异性和精度下降，以保证解算的精度。
【附图说明】
[0068] 下面结合附图对本发明的【具体实施方式】作进一步详细的说明。
[0069] 图1为本发明实施例提供的基于四元数的跨象限姿态角的计算方法的流程图。
【具体实施方式】
[0070] 为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说 明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体 描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。
[0071] 如图1所示，本实施例提供的基于四元数的跨象限姿态角的计算方法包括如下步 骤：
[0072] S1:由初始四元数分别计算获得俯仰角初值、偏航角初值和滚转角初值；
[0073] S2:由本周期的四元数分别计算获得本周期的偏航角中间值和本周期的滚转角中 间值；
[0074] S3:根据本周期的偏航角中间值和上一周期的偏航角中间值确定偏航角的计数； 根据本周期的滚转角中间值和上一周期的滚转角中间值确定滚转角的计数；
[0075] S4:由本周期的偏航角中间值和偏航角的计数计算得到本周期的偏航角终值；由 本周期的滚转角中间值和滚转角的计数计算得到本周期的滚转角终值；
[0076] S5:由本周期的偏航角终值计算得到本周期的偏航角的解耦矩阵；由本周期的滚 转角终值计算得到本周期的滚转角的解耦矩阵；
[0077] S6:由本周期的四元数确定本周期的四元数转换矩阵；
[0078] S7:由本周期的偏航角的解耦矩阵、本周期的滚转角的解耦矩阵和本周期的姿态 转换矩阵计算得到本周期的俯仰角的转换矩阵；
[0079] S8:由本周期的俯仰角的转换矩阵计算得到本周期的俯仰角对应的四元数；
[0080] S9:由本周期的俯仰角对应的四元数计算得到本周期的俯仰角中间值；
[0081 ] S10:根据本周期的俯仰角中间值确定俯仰角的计数；
[0082] S11:由本周期的俯仰角中间值和俯仰角的计数计算得到本周期的俯仰角终值。
[0083]在本实施例的一种优选实施方式中，在上述步骤S11之后还包括如下步骤：
[0084] S12:将本周期的偏航角终值、本周期的滚转角终值和本周期的俯仰角终值输出以 供用户使用，并作为下一周期计算的初值。
[0085] 上述步骤S1中，俯仰角初值的计算公式为：
[0086] 0〇 = sin_1[2(qioq20+qooq30)]；公式（1)
[0087]其中，θ〇为俯仰角初值;9〇()、91()、92()、93()为初始四元数。
[0088] 上述步骤S1中，偏航角初值的计算公式为：
[0089]
公式(2)
[0090] 其中，为偏航角初值。
[0091 ]上述步骤S1中，滚转角初值的计算公式为：
[0092]
公式（3)
[0093]其中，γ 0为滚转角初值。
[0094]上述步骤S1中，俯