[发明专利]一种大姿态四元数全角域转换欧拉角方法

权利要求书

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°时的多值问题，使得姿态具有连贯性，在极小的时间间隔内不会发生大范围跳变。