CN111966954B - 一种大姿态四元数全角域转换欧拉角方法 - Google Patents

Abstract

本发明公开的一种大姿态四元数全角域转换欧拉角方法，属于姿态捕获及控制领域。本发明根据俯仰角的不同区间将姿态划分为三类，分别给出各区间内的四元数转换欧拉角的方法以及取舍的方法，实现全角域下的转换。本发明通过交叉迭代法解决俯仰角在死锁区间时偏航角和滚动角奇异的问题；通过正弦符号法解决俯仰角跨越±90°时转换结果选取错误的问题；解决全角域下四元数转换欧拉角的多值取舍问题；通过旋转矩阵内部信息对姿态进行解析预估，提高转化数值的精确度。本发明能够在浮点数运算能力不够强大的控制器上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角。本发明应用领域包括无人机大姿态避障、机器人位姿获取、人体关节姿态捕捉等领域。

Description

一种大姿态四元数全角域转换欧拉角方法

技术领域

本发明涉及一种大姿态四元数全角域转换欧拉角方法，尤其涉及一种用于大范围姿态采集设备的四元数全角域转换欧拉角方法，属于姿态捕获及控制领域。

背景技术

载体的姿态解算广泛应用于航空航天、工业机器人、人体姿态识别等热门领域。姿态解算是利用滤波后的陀螺仪、加速度计等数据，通过计算融合等方法实时解算出载体的姿态数据。常用的姿态描述参数有方向余弦矩阵、欧拉角、四元数、旋转矢量、罗德里格参数等等。诸多姿态描述参数中四元数和欧拉角是当前工程实践中最常用的两种方法，两者相比，欧拉角表示姿态更为直观清晰，几何意义更为明显；而四元数具有运算简单，无奇异，全范围姿态表示等优势。为了直观的理解姿态信息，通常选取欧拉角作为姿态表示方法，即使用俯仰角，偏航角，滚动角来表示出载体的空间姿态。在对飞行器的姿态进行解算时，其俯仰角通常仅在小幅度的范围内进行变换，但是当遇到一些大姿态的特殊场景，如大姿态避障、人体关节姿态捕捉等场景时，就不能只考虑俯仰角在小范围区间的情况了，而需要考虑全角度下的姿态变换情况。此时选用欧拉角进行表示，则会出现其固有的“死锁”缺陷，而四元数并不存在这一问题，所以如何将四元数通过一定的转换方法得到精确的欧拉角姿态信息成为目前需要解决的技术问题。为了解决四元数与欧拉角之间的全角度问题，在检索专利库和文献库中，有一项专利“一种面向大姿态机动的四元数全角域转换欧拉角的获取方法”和一篇文献“一种新的全角度四元数与欧拉角的转换算法”提出了解决方案，不过这两种技术存在下述缺点。

在对飞行器的姿态进行解算时，其俯仰角通常仅在小幅度的范围内进行变换，但是当遇到一些大姿态的特殊场景，如大姿态避障、人体关节姿态捕捉等场景时，就不能只考虑俯仰角在小范围区间的情况了，而需要考虑全角度下的姿态变换情况。此时选用欧拉角进行表示，则会出现其固有的“死锁”缺陷，而四元数并不存在这一问题，面对不同的应用场景，精确的四元数和形象的欧拉角之间的转换成为需要解决的技术问题。目前欧拉角可通过超复数映像转为四元数，并且这个过程是一一对应的，使得通过欧拉角得到四元数是比较容易的。但是一个四元数在大姿态范围内通常会对应一个或是两个欧拉角，这也正是四元数转换为欧拉角的难点所在。为了解决这个问题，张帆、曹喜滨、邹经湘所著的“一种新的全角度四元数与欧拉角的转换算法”中提出了一种基于分象限的四元数欧拉角转化方法，但是这篇文献中对于欧拉角的“死锁”问题没有考虑，导致在部分角度区间会转换错误，这也就使得文中所提出的算法在工程实践中难以完成转换任务。除此之外，黄玉、武立华在专利“一种面向大姿态机动的四元数全角域转换欧拉角的获取方法”中针对飞行器间接的提出了通过四元数预估欧拉角旋转方向的方法，然而该方法仅是采用统一的算法去优化分象限的缺陷，同样并非解决死锁区间内出现的奇异值和四元数转换欧拉角出现的多值问题。

发明内容

本发明的目的是提供一种大姿态四元数全角域转换欧拉角方法，能够解决下述技术问题：(1)通过交叉迭代法解决俯仰角在死锁区间时偏航角和滚动角奇异的问题；(2)通过正弦符号法解决俯仰角跨越±90°时转换结果选取错误的问题；(3)解决全角域下四元数转换欧拉角的多值取舍问题；(4)通过旋转矩阵内部信息对姿态进行解析预估，显著提高转化数值的精确度。本发明能够在浮点数运算能力不够强大的控制器上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角。本发明的应用领域包括无人机大姿态避障、机器人位姿获取、人体关节姿态捕捉等领域。

本发明的目的是通过下述技术方案实现的。

本发明的一种大姿态四元数全角域转换欧拉角方法，根据俯仰角的不同区间将姿态划分为三类，并分别给出各区间内的四元数转换欧拉角的方法以及取舍的方法，以实现全角域下的转换。第一部分为四元数转换俯仰角时对应单值的区间(-90°+ε,+90°-ε)，在此部分对滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的滚动角和偏航角，解决此部分区间内的四元数转换欧拉角的问题。第二部分为四元数转换俯仰角时对应多值的区间(-180°,-90°-ε)和(90°+ε,+180°)，在此部分对俯仰角、滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的俯仰角、滚动角和偏航角。第三部分为俯仰角在“死锁角”前后邻域内的死区区间(-90°-ε,-90°+ε)和(+90°-ε,+90°+ε)，在此部分使用交叉迭代法解决俯仰角在死锁区间时偏航角和滚动角奇异的问题。在转换完成后，对第一部分和第二部分计算出的多值姿态采用“绝对值平方和法”选取正确的姿态角，对第三部分计算出的多值姿态采用“正弦符号法”选取正确的姿态角。本发明通过旋转矩阵内部所包含的信息优化转换结果，使得全角域下的姿态转换结果连贯准确，为姿态解析控制提供可靠的数据基础。

本发明公开的一种大姿态四元数全角域转换欧拉角的方法，包括如下步骤：

步骤一：根据俯仰角θ的不同区间将姿态划分为三部分：第一部分为四元数转换俯仰角时对应单值的区间(-90°+ε,+90°-ε)；第二部分为四元数转换俯仰角时对应多值的区间 (-180°,-90°-ε)和(90°+ε,+180°)，第三部分为俯仰角在“死锁角”前后邻域内的死区区间(-90°-ε,-90°+ε)和(+90°-ε,+90°+ε)，其中ε为正小值，ε根据运算精度而定。

步骤二：在第一部分区间对滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的滚动角和偏航角，解决此部分区间内的四元数转换欧拉角的问题。

在第一部分区间中，由于此区间内arcsin为单值函数，故根据Z-Y-X坐标系下的旋转矩阵 C₀直接求得当前时刻的俯仰角θ₀：

θ₀＝-arsin(C₁₃)

其中C₁₃为矩阵C₀第一行三列位置对应的元素。

θ₀,ψ₀分别为滚动角、俯仰角和偏航角，q₀,q₁,q₂,q₃分别为四元数的一个实数单位和三个虚数单位。

旋转矩阵和四元数矩阵的关系提供了四元数与欧拉角转换的数学基础，其余步骤皆是通过旋转矩阵的信息将四元数转换为欧拉角，即根据矩阵中不同位置的元素，使用矩阵各位置中所对应的四元数来解算优化出正确的欧拉角。

由旋转矩阵C₀，得到偏航角

和滚动角ψ₀的计算公式：

ψ0＝arctan(C₁₂/C₁₁)

但是由于arctan函数的值域在

之间，故此时根据四元数解算出的欧拉角会出现“漏解”情况，所以需要对

角和ψ₀角进行全角度的补偿运算。补偿计算时需考虑两点：

第一点为：根据C₃₃、C₁₁的正负性和cos(θ₀)的正负性来确定所求角是否在

区间内。

第二点为：根据C₂₃和C₁₂的正负确定进行+π补偿还是-π补偿。

对滚动角

和偏航角ψ₀进行全角度的补偿运算的计算公式为：

θ₀＝-arcsin(C₁₃)

其中sign(·)为符号函数。

步骤三：在第二部分区间对俯仰角、滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的俯仰角、滚动角和偏航角。

在第二部分区间中，由于在此区间中arcsin非单值函数，故无法直接使用第一部分求取俯仰角θ₀的公式计算俯仰角θ₀，需要根据C₁₃的符号进行修正计算俯仰角θ₀。同时，滚动角

角和偏航角ψ₀角仍需采取和第一部分类似的全角度补偿运算，此时使用-π修正，具体修正公式如下：

步骤四：在第三部分区间中使用交叉迭代法解决俯仰角在死锁区间时偏航角和滚动角奇异的问题。

在第三部分区间中，此时由于俯仰角的余弦值cos(θ₀)接近于0。因此在根据旋转矩阵C₀进行计算时，(C₂₃/C₃₃)和(C₁₂/C₁₁)两项会出现分子分母同时趋向于0的情况，即数学中的“未定式”，会导致计算结果产生很大的跳变，在工程实践中所述现象为欧拉角表示姿态时所固有的“死锁环”现象。为了解决“死锁环”问题，通过交叉迭代法解决俯仰角在死锁区间内偏航角和滚动角计算值奇异的问题；交叉迭代法实现方法如下：

步骤4-a：由于不能使用(C₂₃/C₃₃)和(C₁₂/C₁₁)两组数直接进行计算，需要通过寻找旋转矩阵C₀中的其他项进行代替运算。θ₀在死区区间时，得到：

此时使用(1+sinθ₀)和(sinθ₀-1)避免使用cos(θ₀)计算偏航角和滚动角时导致的计算值奇异的问题。由于死锁角有两处分别为+90°和-90°，在该两处使用不同的计算公式来确定ψ₀和

的关系式。

步骤4-b：若在(+90°-ε,+90°+ε)区间时，得到

步骤4-c：若在(-90°-ε,-90°+ε)区间时：得到

步骤4-d：步骤4-b和4-c仅得到ψ₀和

的关系式，无法单独结算出滚动角

和偏航角ψ₀，故设定“死区”范围±90°-ε～±90°+ε，通过获取进入“死区”范围前一时刻t₀的ψ₀角和

角以更新当前时刻t₁的欧拉角，即利用t₀时刻的ψ₀角计算t₁时刻的

角，即利用t₀时刻的

角计算t₁时刻的ψ₁角，不断进行交叉迭代，直至俯仰角θ角离开“死区”。交叉迭代法是基于姿态不会大范围跳变的原理，使用旋转矩阵的内部信息对姿态进行预估的一种方法，使用交叉迭代法能够显著提高四元数转换欧拉角在死锁区间内的转换精度。

步骤五：当俯仰角θ在死区区间外，此时采用“绝对值平方和法”对转换后的偏航角和滚动角的多值结果进行适当的取舍优选，当俯仰角θ在死区区间内，采用“正弦符号法”对俯仰角θ进行取舍解决俯仰角θ在跨越±90°时的多值问题，使得姿态具有连贯性，在极小的时间间隔内不会发生大范围跳变。

通过步骤二至四实现全角度下的四元数与欧拉角转换，但在步骤二和步骤三中，此时俯仰角θ在死区区间外，使用步骤二和步骤三中的四元数转换欧拉角公式计算会出现一组四元数对应两个欧拉角的情况。此时采用“绝对值平方和法”对转换结果进行适当的取舍优选，即将计算出的欧拉角的三个角度的绝对值分别与前一时刻欧拉角的三个角度的绝对值进行做差，并进行平方求和。采用“绝对值平方和”方法能够保证下一时刻计算出的欧拉角与前一时刻具有连贯性。在步骤四中，由于使用“交叉迭代法”，滚动角和偏航角不存在使用反函数而导致的多值问题，但此时俯仰角θ在跨越±90°时仍有着多值问题，采用“正弦符号法”对俯仰角θ进行取舍，即借助C₂₃(C₁₂)进行辅助判断。判断方法如下：

综合使用“绝对值平方和”法和“正弦符号法”两种取舍算法能够使得姿态具有连贯性，在极小的时间间隔内不会发生大范围跳变。

还包括步骤六：将步骤一至步骤五所述的大姿态四元数全角域转换欧拉角方法应用于无人机大姿态避障、机器人位姿获取或人体关节姿态捕捉领域，能够在浮点数运算能力不够强大的微控制处理器上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角，进而得到准确无跳变的欧拉角，以更好的根据姿态对载体进行控制。

作为优选，将步骤一至步骤五所述的大姿态四元数全角域转换欧拉角方法应用于人体关节姿态捕捉领域。在人体关节姿态捕捉领域中，人的部分关节在生理约束下常常会出现俯仰角在“死锁角”附近停留的情况或是出现大姿态的情况，通过步骤一至步骤五的转换方法能够得到稳定连贯的姿态数据，为姿态复现及运动分析提供可靠的数据基础，进而解决人体关节姿态捕捉相关技术问题。

有益效果：

1、本发明公开的一种大姿态四元数全角域转换欧拉角方法，根据俯仰角的不同区间将姿态划分为三类：第一部分为“死锁角”前后邻域内的死区区间，第二部分为四元数和欧拉角单值对应的区间，第三部分为四元数和欧拉角多值对应的区间；即详细分析全角域下的四元数转换欧拉角流程，分别给出各区间内的四元数转换欧拉角的方法以及取舍的方法，以实现全角域下的转换，并在全角域划分为的三个区间内提出不同的多值取舍方法，进而保证各区间内转换值可靠准确。

2、本发明公开的一种大姿态四元数全角域转换欧拉角方法，采用“交叉迭代法”利用旋转矩阵的内部信息计算出欧拉角死锁区间内滚动角和偏航角的替代值，进而保证姿态的连贯性，更好的分析解算姿态。

3、本发明公开的一种大姿态四元数全角域转换欧拉角方法，采用“正弦符号法”解决俯仰角跨越死锁角时出现的多值取舍错误的问题，进而保证死锁区间内俯仰角的平滑不跳变，获得更稳定的姿态。

4、本发明公开的一种大姿态四元数全角域转换欧拉角方法，应用于无人机大姿态避障、机器人位姿获取、人体关节姿态捕捉等领域，能够在浮点数运算能力不够强大的MCU上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角，进而通过四元数转换得到准确无跳变的欧拉角，以更好的根据姿态对载体进行控制。

5、本发明公开的一种大姿态四元数全角域转换欧拉角方法，在人体关节姿态捕捉领域中，人的部分关节在生理约束下常常会出现俯仰角在“死锁角”附近停留的情况或是出现大姿态的情况，通过步骤一至步骤五的转换方法能够得到稳定连贯的姿态数据，为姿态复现及运动分析提供可靠的数据基础，进而解决人体关节姿态捕捉相关技术问题。

附图说明

图1本发明公开的一种大姿态全范围的四元数转换欧拉角方法流程图。

图2是交叉迭代计算死区欧拉角示意图。

图3是整体流程下的角度优选取舍方法示意图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

为了验证方法的可行性，选择在STM32作为微处理器的人体姿态采集模块上对四元数转换欧拉角，假设俯仰角从0°连续变换至360°，滚动角和偏航角基本保持30°不变。

本实施例公开的一种大姿态全范围的四元数转换欧拉角方法，具体实现步骤如下：

步骤一：由于选择的微处理器的浮点数运算力限制，当俯仰角在死锁角0.1°邻域内便会出现较为明显的滚动角偏航角奇异现象，故选择0.1作为ε的取值。根据俯仰角θ的不同区间将姿态划分三部分：第一部分为四元数转换俯仰角时对应单值的区间(-89.9°,+89.9°)，二部分为四元数转换俯仰角时对应多值的区间(-180°,-90.1°)和(90.1°,+180°)，第三部分为俯仰角在“死锁角”前后邻域内的死区区间(-90.1°,-89.9°)和(+89.9°,+90.1°)。

步骤二：将人体姿态采集设备所获取的四元数带入并计算旋转矩阵的值：

根据上一时刻的姿态角确定当前时刻俯仰角在第一部分区间，将四元数带入旋转矩阵求得各元素后计算求得欧拉角

θ₁,ψ₁：

θ₁₁＝-arcsin(C₁₃)

步骤三：根据上一时刻的姿态角确定当前时刻俯仰角在第二部分区间，将四元数带入旋转矩阵求得各元素后计算求得欧拉角

θ₂,ψ₂：

θ₂＝-π*sign(C₁₃)-arcsin(C₁₃)

步骤四：当俯仰角θ进入死锁区间(-90.1°,-89.9°)或(+89.9°,+90.1°)，此时采用交叉迭代法计算滚动角

和偏航角ψ，而俯仰角θ则通过正弦符号法来进行计算和取舍。具体步骤如下：

输入进入死锁区间前一时刻的欧拉角和当前时刻的四元数，将四元数带入旋转矩阵求得矩阵对应元素，然后按照附图2所示方法带入公式求得滚动角

和偏航角ψ₁。

步骤4-a：当俯仰角θ在(+89.9°,+90.1°)区间时：

步骤4-b：当俯仰角θ在(-90.1°,-89.9°)区间时：

在表1中以

ψ₀＝30°，θ₀从89.91逐渐增加到89.99为例做计算示意，可见，通过该方法滚动角

和偏航角ψ并没有漂移，相比于直接使用旋转结算出的不确定结果本方法具有较大的实用性与准确性。

表1

步骤五：在步骤一至步骤四中通过相应的方法计算得到单值函数无法直接求解出的俯仰角、滚动角和偏航角，并在死锁区间求得准确的欧拉角代替奇异角，在计算后需要对计算的多值结果进行合理取舍。

当俯仰角θ在死区区间外，此时采用“绝对值平方和法”对转换后的偏航角和滚动角的多值结果进行适当的取舍优选：即将步骤二和步骤三两部分区间内计算求得欧拉角

θ₁,ψ₁和

θ₂,ψ₂与上一时刻的欧拉角

θ₀,ψ₀进行比较：

比较ans1和ans2的大小，选取小的为当前时刻的欧拉角。

当俯仰角θ在死区区间内，采用“正弦符号法”对俯仰角θ进行取舍解决俯仰角θ在跨越±90°时的多值问题，因为此时如果使用非死锁区间的绝对值平方和法，在89.99变化到 90.01时，会取舍到错误的俯仰角θ值，故提出正弦符号法对俯仰角θ值进行取舍。具体步骤为根据当前时刻

的符号，来对俯仰角θ的值进行计算：

在表2中以俯仰角θ跨越+90°为例对此方法进行计算示意，可见转化后的角度符合期望。

表2

还包括步骤六：将步骤一至步骤五所述的大姿态四元数全角域转换欧拉角方法应用于无人机大姿态避障、机器人位姿获取或人体关节姿态捕捉领域，能够在浮点数运算能力不够强大的微控制处理器上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角，进而得到准确无跳变的欧拉角，以更好的根据姿态对载体进行控制。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种大姿态四元数全角域转换欧拉角的方法，其特征在于：包括如下步骤，

步骤一：根据俯仰角θ的不同区间将姿态划分为三部分：第一部分为四元数转换俯仰角时对应单值的区间(-90°+ε，+90°-ε)；第二部分为四元数转换俯仰角时对应多值的区间(-180°，-90°-ε)和(90°+ε，+180°)，第三部分为俯仰角在“死锁角”前后邻域内的死区区间(-90°-ε，-90°+ε)和(+90°-ε，+90°+ε)，其中ε为正小值，ε根据运算精度而定；

步骤二：在第一部分区间对滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的滚动角和偏航角，解决此部分区间内的四元数转换欧拉角的问题；

步骤二实现方法为，

在第一部分区间中，由于此区间内arcsin为单值函数，故根据Z-Y-X坐标系下的旋转矩阵C₀直接求得当前时刻的俯仰角θ₀：

θ₀＝-arsin(C₁₃)

其中C₁₃为矩阵C₀第一行三列位置对应的元素；

θ₀，ψ₀分别为滚动角、俯仰角和偏航角，q₀，q₁，q₂，q₃分别为四元数的一个实数单位和三个虚数单位；

旋转矩阵和四元数矩阵的关系提供了四元数与欧拉角转换的数学基础，其余步骤皆是通过旋转矩阵的信息将四元数转换为欧拉角，即根据矩阵中不同位置的元素，使用矩阵各位置中所对应的四元数来解算优化出正确的欧拉角；

由旋转矩阵C₀，得到偏航角

和滚动角ψ₀的计算公式：

ψ₀＝arctan(C₁₂/C₁₁)

但是由于arctan函数的值域在

之间，故此时根据四元数解算出的欧拉角会出现“漏解”情况，所以需要对

角和ψ₀角进行全角度的补偿运算；补偿计算时需考虑两点：

第一点为：根据C₃₃、C₁₁的正负性和cos(θ₀)的正负性来确定所求角是否在

区间内；

第二点为：根据C₂₃和C₁₂的正负确定进行+π补偿还是-π补偿；

对

角和ψ₀角进行全角度的补偿运算的计算公式为：

θ₀＝-arcsin(C₁₃)

步骤三：在第二部分区间对俯仰角、滚动角和偏航角的多值情况进行补偿运算，得到单值函数无法直接求解出的俯仰角、滚动角和偏航角；

步骤三实现方法为，

在第二部分区间中，由于在此区间中arcsin非单值函数，故无法直接使用第一部分求取俯仰角θ₀的公式计算俯仰角θ₀，需要根据C₁₃的符号进行修正计算俯仰角θ₀；同时，滚动角

角和偏航角ψ₀角仍需采取和第一部分类似的全角度补偿运算，此时使用-π修正，具体修正公式如下：

θ₀＝-π*sign(C₁₃)-arcsin(C₁₃)

步骤四：在第三部分区间中使用交叉迭代法解决俯仰角在死锁区间时偏航角和滚动角奇异的问题；

步骤四实现方法为，

在第三部分区间中，此时由于俯仰角的余弦值cos(θ₀)接近于0；因此在根据旋转矩阵C₀进行计算时，(C₂₃/C₃₃)和(C₁₂/C₁₁)两项会出现分子分母同时趋向于0的情况，即数学中的“未定式”，会导致计算结果产生很大的跳变，所述现象为欧拉角表示姿态时所固有的“死锁环”现象；为了解决“死锁环”问题，通过交叉迭代法解决俯仰角在死锁区间内偏航角和滚动角计算值奇异的问题；交叉迭代法实现方法如下：

步骤4-a：由于不能使用(C₂₃/C₃₃)和(C₁₂/C₁₁)两组数直接进行计算，需要通过寻找旋转矩阵C₀中的其他项进行代替运算；θ₀在死区区间时，得到：

此时使用(1+sinθ₀)和(sinθ₀-1)避免使用cos(θ₀)计算偏航角和滚动角时导致的计算值奇异的问题；由于死锁角有两处分别为+90°和-90°，在该两处使用不同的计算公式来确定ψ₀和

的关系式；

步骤4-b：若在(+90°-ε，+90°+ε)区间时，得到

步骤4-c：若在(-90°-ε，-90°+ε)区间时：得到

步骤4-d：步骤4-b和4-c仅得到ψ₀和

的关系式，无法单独结算出滚动角

和偏航角ψ₀，故设定“死区”范围±90°-ε～±90°+ε，通过获取进入“死区”范围前一时刻t₀的ψ₀角和

角以更新当前时刻t₁的欧拉角，即利用t₀时刻的ψ₀角计算t₁时刻的

角，即利用t₀时刻的

角计算t₁时刻的ψ₁角，不断进行交叉迭代，直至俯仰角θ角离开“死区”；交叉迭代法是基于姿态不会大范围跳变的原理，使用旋转矩阵的内部信息对姿态进行预估的一种方法，使用交叉迭代法能够显著提高四元数转换欧拉角在死锁区间内的转换精度；

步骤五：当俯仰角θ在死区区间外，此时采用“绝对值平方和法”对转换后的偏航角和滚动角的多值结果进行适当的取舍优选，当俯仰角θ在死区区间内，采用“正弦符号法”对俯仰角θ进行取舍解决俯仰角θ在跨越±90°时的多值问题，使得姿态具有连贯性，在极小的时间间隔内不会发生大范围跳变。

2.如权利要求1所述的一种大姿态四元数全角域转换欧拉角的方法，其特征在于：还包括步骤六，将步骤一至步骤五所述的大姿态四元数全角域转换欧拉角方法应用于无人机大姿态避障、机器人位姿获取或人体关节姿态捕捉领域，能够在浮点数运算能力不够强大的微控制处理器上处理大姿态的四元数数据，实现大姿态四元数全角域转换欧拉角，进而得到准确无跳变的欧拉角，以更好的根据姿态对载体进行控制。

3.如权利要求2所述的一种大姿态四元数全角域转换欧拉角的方法，其特征在于：将步骤一至步骤五所述的大姿态四元数全角域转换欧拉角方法应用于人体关节姿态捕捉领域；在人体关节姿态捕捉领域中，人的部分关节在生理约束下常常会出现俯仰角在“死锁角”附近停留的情况或是出现大姿态的情况，通过步骤一至步骤五的转换方法能够得到稳定连贯的姿态数据，为姿态复现及运动分析提供可靠的数据基础，进而解决人体关节姿态捕捉相关技术问题。

4.如权利要求1所述的一种大姿态四元数全角域转换欧拉角的方法，其特征在于：步骤五实现方法为，

通过步骤二至四实现全角度下的四元数与欧拉角转换，但在步骤二和步骤三中，此时俯仰角θ在死区区间外，使用步骤二和步骤三中的四元数转换欧拉角公式计算会出现一组四元数对应两个欧拉角的情况；此时采用“绝对值平方和法”对转换结果进行适当的取舍优选，即将计算出的欧拉角的三个角度的绝对值分别与前一时刻欧拉角的三个角度的绝对值进行做差，并进行平方求和；采用“绝对值平方和”方法能够保证下一时刻计算出的欧拉角与前一时刻具有连贯性；在步骤四中，由于使用“交叉迭代法”，滚动角和偏航角不存在使用反函数而导致的多值问题，但此时俯仰角θ在跨越±90°时仍有着多值问题，采用“正弦符号法”对俯仰角θ进行取舍，即借助C₂₃(C₁₂)进行辅助判断；判断方法如下：

综合使用“绝对值平方和”法和“正弦符号法”两种取舍算法能够使得姿态具有连贯性，在极小的时间间隔内不会发生大范围跳变。

Images