欧拉角和四元数之间转换公式推导

 前言

       首先明确一点不同的旋转顺序，欧拉角和四元数之间转换的公式是不一样的，以下推导使用的旋转顺序为：，，。

1.欧拉角转四元数

        绕X轴旋转角度为，绕Y轴旋转角度为，绕Z轴旋转角度为，把这三个角轴转换为四元数得到以下公式：

                                                         

                                                        

                                                        

        下面用到四元数的乘法，四元数的相关知识可以参考附录的论文1或者2，都有很详细的推导和讲述，四元数相乘得到：

                                                                                     

        简记，，，最终的结果为如下公式：

                                              

                                              

                                              

                                             

        在推导过程中完全手推容易出错，可以借助matlab，可以大大提高推导公式的效率，降低出错可能性。如下为matlab代码，供大家参考：

% 测试欧拉角到四元数之间的转换
% addpath('utils'); %函数路径

syms pitch0  % x
syms roll0   % y
syms yaw0    % z 
q_x=[cos(pitch0/2),1*sin(pitch0/2),0,0]; % x轴：[1,0,0] 绕x轴旋转pitch0
q_y=[cos(roll0/2),0,1*sin(roll0/2),0];   % y轴：[0,1,0] 绕y轴旋转roll0
q_z=[cos(yaw0/2),0,0,1*sin(yaw0/2)];     % z轴：[0,0,1] 绕z轴旋转yaw0

q= quaternProd(quaternProd(q_z, q_x),q_y)

% q的最终结果
% cos(pitch0/2)*cos(roll0/2)*cos(yaw0/2) - sin(pitch0/2)*sin(roll0/2)*sin(yaw0/2),
% cos(roll0/2)*cos(yaw0/2)*sin(pitch0/2) - cos(pitch0/2)*sin(roll0/2)*sin(yaw0/2), 
% cos(pitch0/2)*cos(yaw0/2)*sin(roll0/2) + cos(roll0/2)*sin(pitch0/2)*sin(yaw0/2), 
% cos(pitch0/2)*cos(roll0/2)*sin(yaw0/2) + cos(yaw0/2)*sin(pitch0/2)*sin(roll0/2)]



% 下面函数是四元数乘法
% 四元数相乘
function ab = quaternProd(a, b)

    ab(1) = a(1).*b(1)-a(2).*b(2)-a(3).*b(3)-a(4).*b(4);
    ab(2) = a(1).*b(2)+a(2).*b(1)+a(3).*b(4)-a(4).*b(3);
    ab(3) = a(1).*b(3)-a(2).*b(4)+a(3).*b(1)+a(4).*b(2);
    ab(4) = a(1).*b(4)+a(2).*b(3)-a(3).*b(2)+a(4).*b(1);
    
end

2.四元数转欧拉角

         表示导航系到载体系的转换矩阵，设偏航、俯仰，滚转的角度分别为 ， ,  ，则旋转矩阵表示如下：

                    

 

        另外，用四元数形式表示的旋转矩阵表示形式如下：

                               

        根据上面两个公式的对应关系可得：

                                                                  

        所以，欧拉角可解为：

                                                         

 

参考论文

1. Indirect Kalman Filter for 3D Attitude Estimation

2. Quaternion kinematics for the error-state Kalman filter

3. Euler angles, quaternions, and transformation matrices for space shuttle analysis