基于 Matlab 符号运算的欧拉角转换为四元数方法(用欧拉角初始化四元数)

%% 基于 Matlab 符号运算的欧拉角转换为四元数方法(用欧拉角初始化四元数)
close all;
clear;
clc;

%% 符号变量及矩阵
syms croll cpitch cyaw;
syms sroll spitch syaw;

%%  苏联坐标体系：Z(向左/向右)-Y(向上)-X(向前)：火箭坐标系
%%  Z-Y-X(321)：俯仰，偏航，滚转
%%  绕 z 轴 
q_pitch = [cpitch, 0, 0,  spitch];

%%  绕 y 轴 
q_yaw = [cyaw, 0, syaw, 0];           

%%  绕 x 轴
q_roll = [croll, sroll, 0, 0];  

%%  ans1 = q_pitch * q_yaw
ans1(1) = q_pitch(1)*q_yaw(1) - q_pitch(2)*q_yaw(2) - q_pitch(3)*q_yaw(3) - q_pitch(4)*q_yaw(4);
ans1(2) = q_pitch(2)*q_yaw(1) + q_pitch(1)*q_yaw(2) - q_pitch(4)*q_yaw(3) + q_pitch(3)*q_yaw(4);
ans1(3) = q_pitch(3)*q_yaw(1) + q_pitch(4)*q_yaw(2) + q_pitch(1)*q_yaw(3) - q_pitch(2)*q_yaw(4);
ans1(4) = q_pitch(4)*q_yaw(1) - q_pitch(3)*q_yaw(2) + q_pitch(2)*q_yaw(3) + q_pitch(1)*q_yaw(4);

%% final = ans1 * q_roll
final(1) = ans1(1)*q_roll(1) - ans1(2) *q_roll(2) - ans1(3) *q_roll(3) - ans1(4)*q_roll(4);
final(2) = ans1(2) *q_roll(1) + ans1(1)*q_roll(2) - ans1(4)*q_roll(3) + ans1(3) *q_roll(4);
final(3) = ans1(3) *q_roll(1) + ans1(4)*q_roll(2) + ans1(1)*q_roll(3) - ans1(2) *q_roll(4);
final(4) = ans1(4)*q_roll(1) - ans1(3) *q_roll(2) + ans1(2) *q_roll(3) + ans1(1)*q_roll(4);
disp(final');
% 注意：Y轴Z轴位置互换，所以pitch, yaw位置互换
% cpitch*croll*cyaw + spitch*sroll*syaw, 
% cpitch*cyaw*sroll - croll*spitch*syaw,  
% cpitch*croll*syaw + cyaw*spitch*sroll,
% croll*cyaw*spitch - cpitch*sroll*syaw

% %%  苏联坐标体系：Z(向左/向右)-Y(向上)-X(向前)：导弹坐标系
% %%  Y-Z-X(231)：偏航，俯仰，滚转
% %%  绕 y 轴 
% q_yaw = [cyaw, 0, syaw, 0];           
% 
% %%  绕 z 轴 
% q_pitch = [cpitch, 0, 0, spitch];
% 
% %%  绕 x 轴
% q_roll = [croll, sroll, 0, 0];  
% 
% %%  ans1 = q_yaw * q_pitch
% ans1(1) = q_yaw(1)*q_pitch(1) - q_yaw(2)*q_pitch(2) - q_yaw(3)*q_pitch(3) - q_yaw(4)*q_pitch(4);
% ans1(2) = q_yaw(2)*q_pitch(1) + q_yaw(1)*q_pitch(2) - q_yaw(4)*q_pitch(3) + q_yaw(3)*q_pitch(4);
% ans1(3) = q_yaw(3)*q_pitch(1) + q_yaw(4)*q_pitch(2) + q_yaw(1)*q_pitch(3) - q_yaw(2)*q_pitch(4);
% ans1(4) = q_yaw(4)*q_pitch(1) - q_yaw(3)*q_pitch(2) + q_yaw(2)*q_pitch(3) + q_yaw(1)*q_pitch(4);
% 
% %% final = ans1 * q_roll
% final(1) = ans1(1)*q_roll(1) - ans1(2) *q_roll(2) - ans1(3) *q_roll(3) - ans1(4)*q_roll(4);
% final(2) = ans1(2) *q_roll(1) + ans1(1)*q_roll(2) - ans1(4)*q_roll(3) + ans1(3) *q_roll(4);
% final(3) = ans1(3) *q_roll(1) + ans1(4)*q_roll(2) + ans1(1)*q_roll(3) - ans1(2) *q_roll(4);
% final(4) = ans1(4)*q_roll(1) - ans1(3) *q_roll(2) + ans1(2) *q_roll(3) + ans1(1)*q_roll(4);
% disp(final');
% % 注意：Y轴Z轴位置互换，所以pitch, yaw位置互换
% % cpitch*croll*cyaw - spitch*sroll*syaw, 
% % cpitch*cyaw*sroll + croll*spitch*syaw,  
% % cpitch*croll*syaw + cyaw*spitch*sroll,
% % croll*cyaw*spitch - cpitch*sroll*syaw

% %%  中国坐标体系：Z(向上/向下)-Y(向左/向右)-X(向前)：卫星坐标系
% %% Z-X-Y(312)：偏航，滚转，俯仰
% %%  绕 z 轴 
% q_yaw = [cyaw, 0, 0, syaw]; 
% 
% %%  绕 x 轴
% q_roll = [croll, sroll, 0, 0];          
% 
% %%  绕 y 轴 
% q_pitch = [cpitch, 0, spitch, 0];   
% 
% %%  ans1 = q_yaw * q_roll
% ans1(1) = q_yaw(1)*q_roll(1) - q_yaw(2)*q_roll(2) - q_yaw(3)*q_roll(3) - q_yaw(4)*q_roll(4);
% ans1(2) = q_yaw(2)*q_roll(1) + q_yaw(1)*q_roll(2) - q_yaw(4)*q_roll(3) + q_yaw(3)*q_roll(4);
% ans1(3) = q_yaw(3)*q_roll(1) + q_yaw(4)*q_roll(2) + q_yaw(1)*q_roll(3) - q_yaw(2)*q_roll(4);
% ans1(4) = q_yaw(4)*q_roll(1) - q_yaw(3)*q_roll(2) + q_yaw(2)*q_roll(3) + q_yaw(1)*q_roll(4);
% 
% %% final = ans1 * q_pitch
% final(1) = ans1(1)*q_pitch(1) - ans1(2) *q_pitch(2) - ans1(3) *q_pitch(3) - ans1(4)*q_pitch(4);
% final(2) = ans1(2) *q_pitch(1) + ans1(1)*q_pitch(2) - ans1(4)*q_pitch(3) + ans1(3) *q_pitch(4);
% final(3) = ans1(3) *q_pitch(1) + ans1(4)*q_pitch(2) + ans1(1)*q_pitch(3) - ans1(2) *q_pitch(4);
% final(4) = ans1(4)*q_pitch(1) - ans1(3) *q_pitch(2) + ans1(2) *q_pitch(3) + ans1(1)*q_pitch(4);
% disp(final');
% % cpitch*croll*cyaw - spitch*sroll*syaw, 
% % cpitch*cyaw*sroll - croll*spitch*syaw, 
% % croll*cyaw*spitch + cpitch*sroll*syaw, 
% % cpitch*croll*syaw + cyaw*spitch*sroll

% %%  欧美坐标体系：Z(向上/向下)-Y(向左/向右)-X(向前)：飞机坐标系
% %%  Z-Y-X(321)：偏航，俯仰，滚转     
% %%  绕 z 轴 
% q_yaw = [cyaw, 0, 0, syaw]; 
% 
% %%  绕 y 轴 
% q_pitch = [cpitch, 0, spitch, 0];  
% 
% %%  绕 x 轴
% q_roll = [croll, sroll, 0, 0];   
% 
% %%  ans1 = q_yaw * q_pitch
% ans1(1) = q_yaw(1)*q_pitch(1) - q_yaw(2)*q_pitch(2) - q_yaw(3)*q_pitch(3) - q_yaw(4)*q_pitch(4);
% ans1(2) = q_yaw(2)*q_pitch(1) + q_yaw(1)*q_pitch(2) - q_yaw(4)*q_pitch(3) + q_yaw(3)*q_pitch(4);
% ans1(3) = q_yaw(3)*q_pitch(1) + q_yaw(4)*q_pitch(2) + q_yaw(1)*q_pitch(3) - q_yaw(2)*q_pitch(4);
% ans1(4) = q_yaw(4)*q_pitch(1) - q_yaw(3)*q_pitch(2) + q_yaw(2)*q_pitch(3) + q_yaw(1)*q_pitch(4);
% 
% %% final = ans1 * q_roll
% final(1) = ans1(1)*q_roll(1) - ans1(2) *q_roll(2) - ans1(3) *q_roll(3) - ans1(4)*q_roll(4);
% final(2) = ans1(2) *q_roll(1) + ans1(1)*q_roll(2) - ans1(4)*q_roll(3) + ans1(3) *q_roll(4);
% final(3) = ans1(3) *q_roll(1) + ans1(4)*q_roll(2) + ans1(1)*q_roll(3) - ans1(2) *q_roll(4);
% final(4) = ans1(4)*q_roll(1) - ans1(3) *q_roll(2) + ans1(2) *q_roll(3) + ans1(1)*q_roll(4);
% disp(final');
% % cpitch*croll*cyaw + spitch*sroll*syaw, 
% % cpitch*cyaw*sroll - croll*spitch*syaw, 
% % croll*cyaw*spitch + cpitch*sroll*syaw, 
% % cpitch*croll*syaw - cyaw*spitch*sroll

最新版

%% euler2quaternion_deduce.m
%% 基于 Matlab 符号运算的欧拉角转换为四元数方法(用欧拉角初始化四元数)--->欧美坐标体系飞机坐标系
close all;
clear;
clc;

%% ned to body
%% 符号变量及矩阵
syms croll cpitch cyaw;
syms sroll spitch syaw;

%%  欧美坐标体系：Z(向上/向下)-Y(向左/向右)-X(向前)：飞机坐标系
%%  Z-Y-X(321)：偏航，俯仰，滚转     
%%  绕 z 轴 
q_yaw = [cyaw, 0, 0, syaw]; 

%%  绕 y 轴 
q_pitch = [cpitch, 0, spitch, 0];  

%%  绕 x 轴
q_roll = [croll, sroll, 0, 0];   

%%  ans1 = q_yaw * q_pitch
ans1 = quaternion_multiply(q_yaw, q_pitch);

%% q_ned2body = ans1 * q_roll
q_ned2body = quaternion_multiply(ans1, q_roll);
disp('ned to body');
% disp(q_ned2body(1));
% disp(q_ned2body(2));
% disp(q_ned2body(3));
% disp(q_ned2body(4));
% cpitch*croll*cyaw + spitch*sroll*syaw, 
% cpitch*cyaw*sroll - croll*spitch*syaw, 
% croll*cyaw*spitch + cpitch*sroll*syaw, 
% cpitch*croll*syaw - cyaw*spitch*sroll
% 对应函数euler2quaternion_ned2body.m

%% ecef to ned
%% 符号变量及矩阵
syms clongitude clatitude;
syms slongitude slatitude;

%%  欧美坐标体系：Z(指向北极)-Y(右手定则)-X(指向子午线)：地心地固坐标系
%%  Z-Y：经度 longitude，纬度 -90-latitude    
%%  绕 z 轴 
q_longitude = [clongitude, 0, 0, slongitude]; 

%%  绕 y 轴 
q_latitude = [clatitude, 0, -slatitude, 0];  

%% q_ecef2ned = q_longitude * q_latitude
q_ecef2ned = quaternion_multiply(q_longitude, q_latitude);
disp('ecef to ned');
% disp(q_ecef2ned(1));
% disp(q_ecef2ned(2));
% disp(q_ecef2ned(3));
% disp(q_ecef2ned(4));
% clatitude*clongitude
% slatitude*slongitude
% -clongitude*slatitude
% clatitude*slongitude
% 对应函数euler2quaternion_ecef2ned.m

%% ecef to body
%% q_ecef2body_inv = q_ned2body_inv * q_ecef2ned_inv

%% 四元数求逆
q_ned2body_inv = quaternion_inv(q_ned2body);
q_ecef2ned_inv = quaternion_inv(q_ecef2ned);
q_ecef2body_inv = quaternion_multiply(q_ned2body_inv, q_ecef2ned_inv);
q_ecef2body = quaternion_inv(q_ecef2body_inv);
disp('ecef to body');
% disp(q_ecef2body(1));
% disp(q_ecef2body(2));
% disp(q_ecef2body(3));
% disp(q_ecef2body(4));
% clatitude*clongitude*(cpitch*croll*cyaw + spitch*sroll*syaw) + clongitude*slatitude*(croll*cyaw*spitch + cpitch*sroll*syaw)
% - clatitude*slongitude*(cpitch*croll*syaw - cyaw*spitch*sroll) - slatitude*slongitude*(cpitch*cyaw*sroll - croll*spitch*syaw)

% clatitude*clongitude*(cpitch*cyaw*sroll - croll*spitch*syaw) - clatitude*slongitude*(croll*cyaw*spitch + cpitch*sroll*syaw) 
% - clongitude*slatitude*(cpitch*croll*syaw - cyaw*spitch*sroll) + slatitude*slongitude*(cpitch*croll*cyaw + spitch*sroll*syaw)

% clatitude*clongitude*(croll*cyaw*spitch + cpitch*sroll*syaw) - clongitude*slatitude*(cpitch*croll*cyaw + spitch*sroll*syaw) 
% + clatitude*slongitude*(cpitch*cyaw*sroll - croll*spitch*syaw) - slatitude*slongitude*(cpitch*croll*syaw - cyaw*spitch*sroll)

% clatitude*clongitude*(cpitch*croll*syaw - cyaw*spitch*sroll) + clatitude*slongitude*(cpitch*croll*cyaw + spitch*sroll*syaw) 
% + clongitude*slatitude*(cpitch*cyaw*sroll - croll*spitch*syaw) + slatitude*slongitude*(croll*cyaw*spitch + cpitch*sroll*syaw);
% 对应函数euler2quaternion_ecef2body.m

%% 测试

%% ned to body
roll_ned = 0;
pitch_ned = 45/57.3;
yaw_ned = 90/57.3;

%% ecef to ned
longitude = 100;
latitude = 0;

%% ecef to body
q_ecef2body_ = euler2quaternion_ecef2body(roll_ned, pitch_ned, yaw_ned, longitude, latitude);  % 机体系相对于地心地固系姿态四元数
Matrix = quaternion2matrix_body(q_ecef2body_);  % 机体系相对于地心地固系姿态变换矩阵
[roll_ecef, pitch_ecef, yaw_ecef] = matrix2euler(Matrix);
disp(roll_ecef * 57.3);
disp(pitch_ecef * 57.3);
disp(yaw_ecef * 57.3);

%% euler2quaternion_ned2body.m
function q = euler2quaternion_ned2body(roll, pitch, yaw)
% 欧拉角求四元数，取决于欧拉角参考系是哪个，包括 ned,ecef;本函数参考系是 ned
% roll 滚转角
% pitch 俯仰角
% yaw 偏航角
q = zeros(4,1);

croll = cos(roll);
sroll = sin(roll);
cpitch = cos(pitch);
spitch = sin(pitch);
cyaw = cos(yaw);
syaw = sin(yaw);


q(1) = cpitch*croll*cyaw + spitch*sroll*syaw; 
q(2) = cpitch*cyaw*sroll - croll*spitch*syaw;
q(3) = croll*cyaw*spitch + cpitch*sroll*syaw; 
q(4) = cpitch*croll*syaw - cyaw*spitch*sroll;

end

%% euler2quaternion_ecef2ned.m
function q = euler2quaternion_ecef2ned(longitude, latitude)
% 地心地固系转到北东地系四元数
% longitude 经度
% longitude 纬度
q = zeros(4,1);

clongitude = cos(longitude);
slongitude = sin(longitude);
clatitude = cos(latitude);
slatitude = sin(latitude);

q(1) = clatitude*clongitude;
q(2) = slatitude*slongitude;
q(3) = -clongitude*slatitude;
q(4) = clatitude*slongitude;

end

%% euler2quaternion_ecef2body
function q = euler2quaternion_ecef2body(roll, pitch, yaw, longitude, latitude)
% 欧拉角求四元数，取决于欧拉角参考系是哪个，包括 ned,ecef;本函数参考系是 ecef
% roll 滚转角
% pitch 俯仰角
% yaw 偏航角
q = zeros(4,1);

croll = cos(roll);
sroll = sin(roll);
cpitch = cos(pitch);
spitch = sin(pitch);
cyaw = cos(yaw);
syaw = sin(yaw);

clongitude = cos(longitude);
slongitude = sin(longitude);
clatitude = cos(latitude);
slatitude = sin(latitude);

q(1) = clatitude*clongitude*(cpitch*croll*cyaw + spitch*sroll*syaw) + clongitude*slatitude*(croll*cyaw*spitch + cpitch*sroll*syaw)...
        - clatitude*slongitude*(cpitch*croll*syaw - cyaw*spitch*sroll) - slatitude*slongitude*(cpitch*cyaw*sroll - croll*spitch*syaw);

q(2) = clatitude*clongitude*(cpitch*cyaw*sroll - croll*spitch*syaw) - clatitude*slongitude*(croll*cyaw*spitch + cpitch*sroll*syaw)...
        - clongitude*slatitude*(cpitch*croll*syaw - cyaw*spitch*sroll) + slatitude*slongitude*(cpitch*croll*cyaw + spitch*sroll*syaw);

q(3) = clatitude*clongitude*(croll*cyaw*spitch + cpitch*sroll*syaw) - clongitude*slatitude*(cpitch*croll*cyaw + spitch*sroll*syaw)...
        + clatitude*slongitude*(cpitch*cyaw*sroll - croll*spitch*syaw) - slatitude*slongitude*(cpitch*croll*syaw - cyaw*spitch*sroll);

q(4) =  clatitude*clongitude*(cpitch*croll*syaw - cyaw*spitch*sroll) + clatitude*slongitude*(cpitch*croll*cyaw + spitch*sroll*syaw)...
        + clongitude*slatitude*(cpitch*cyaw*sroll - croll*spitch*syaw) + slatitude*slongitude*(croll*cyaw*spitch + cpitch*sroll*syaw);

end

%% quaternion2matrix_body.m
function Matrix = quaternion2matrix_body(q)
% 四元数求参考系到机体系变换矩阵，参考系包括ned, ecef
% q 4x1
% Matrix 3x3
q0 = q(1);
q1 = q(2);
q2 = q(3);
q3 = q(4);
Matrix = zeros(3,3);
Matrix(1,1) = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
Matrix(1,2) = 2.0 * (q1 * q2 + q0 * q3);
Matrix(1,3) = 2.0 * (q1 * q3 - q0 * q2);
Matrix(2,1) = 2.0 * (q1 * q2 - q0 * q3);
Matrix(2,2) = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3;
Matrix(2,3) = 2.0 * (q2 * q3 + q0 * q1);
Matrix(3,1) = 2.0 * (q1 * q3 + q0 * q2);
Matrix(3,2) = 2.0 * (q2 * q3 - q0 * q1);
Matrix(3,3) = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
end

%% matrix2euler.m
function [roll, pitch, yaw] = matrix2euler(Matrix)
% 变换矩阵求欧拉角，参考系包括ned, ecef 到机体系欧拉角，取决于Matrix矩阵的参考系是哪个 + 姿态角转轴及转换次序
% roll 滚转角
% pitch 俯仰角
% yaw 偏航角
% Matrix 3x3
roll = atan2(Matrix(2,3), Matrix(3,3));
pitch = -asin(Matrix(1,3));
yaw = atan2(Matrix(1,2), Matrix(1,1));
end

%% quaternion_multiply.m
function r = quaternion_multiply(p,q)
% 四元数相乘
% r = p * q

% p 4x1
% q 4x1
% r 4x1

r(1) = p(1)*q(1) - p(2) *q(2) - p(3) *q(3) - p(4)*q(4);
r(2) = p(2) *q(1) + p(1)*q(2) - p(4)*q(3) + p(3) *q(4);
r(3) = p(3) *q(1) + p(4)*q(2) + p(1)*q(3) - p(2) *q(4);
r(4) = p(4)*q(1) - p(3) *q(2) + p(2) *q(3) + p(1)*q(4);

end

%% quaternion_inv.m
function q_inv = quaternion_inv(q)
% 四元数求逆
% q 4x1
% q_inv 4x1

q_inv(1) = q(1);
q_inv(2) = -q(2);
q_inv(3) = -q(3);
q_inv(4) = -q(4);

end