姿态描述方式通常有:旋转矩阵、四元数、欧拉角、旋转向量等,在此给出几种描述相互转换Matlab代码,方便以后使用时查询。
Matlab也有提供一些封装好的函数可直接使用,这里的函数是根据公式进行转换,采用其他语言重写时也比较方便。
欧拉角说明:通常有两类,分别为z-x-z经典的欧拉角和z-y-x泰特-布莱恩表示法,具体参考博客[1],这里采用z-y-x泰特-布莱恩表示法。
旋转向量说明:通常采用[rx,ry,rz,theta]或者[rx*theta,ry*theta,rz*theta],这里采用第二种。
目录
【旋转矩阵—>四元数】
function q = R2qua(R)
%旋转矩阵转四元数
if 1+R(1,1)+R(2,2)+R(3,3) > 0
w = sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
if w > 0
x = (R(3,2)-R(2,3))/(4*w);
y = (R(1,3)-R(3,1))/(4*w);
z = (R(2,1)-R(1,2))/(4*w);
elseif w==0
list = [R(1,1) R(2,2) R(3,3)];
t = sqrt(1-R(1,1)-R(2,2)-R(3,3));
if max(list)==R(1,1)
x = t/4;
w = (R(3,2)-R(2,3))/t;
y = (R(1,3)+R(3,1))/t;
z = (R(2,1)+R(1,2))/t;
elseif max(list)==R(2,2)
y = t/4;
w = (R(1,3)-R(3,1))/t;
x = (R(1,2)+R(2,1))/t;
z = (R(3,2)+R(2,3))/t;
elseif max(list)==R(3,3)
z = t/4;
y = (R(3,2)-R(2,3))/t;
x = (R(1,3)+R(3,1))/t;
w = (R(2,1)-R(1,2))/t;
end
end
end
q = [x y z w];
end
【四元数—>旋转矩阵】
function R = qua2R(q)
% q = [x,y,z,w]
%四元数转旋转矩阵
x = q(1);
y = q(2);
z = q(3);
w = q(4);
R = [1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y;
2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x;
2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y];
end
【旋转矩阵—>欧拉角】
function ang = R2eluer(R)
%旋转矩阵转欧拉角
x = atan2(R(3,2),R(3,3));
y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
z = atan2(R(2,1),R(1,1));
ang = [x y z];
end
【欧拉角—>旋转矩阵】
function R = eluer2R(ang)
%欧拉角转旋转矩阵
x = ang(1);
y = ang(2);
z = ang(3);
Rx = [1 0 0;
0 cos(x) -sin(x);
0 sin(x) cos(x)];
Ry = [cos(y) 0 sin(y);
0 1 0;
-sin(y) 0 cos(y)];
Rz = [cos(z) -sin(z) 0;
sin(z) cos(z) 0;
0 0 1];
R = Rz*Ry*Rx;
end
【欧拉角—>四元数】
function q = eluer2qua(ang)
%欧拉角转四元数
%q = [w,x,y,z]
x = ang(1);
y = ang(2);
z = ang(3);
q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
end
【四元数—>欧拉角】
function ang = qua2eluer(q)
% q = [w,x,y,z]
%四元数转欧拉角
x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
y = asin(2*(q(1)*q(3) - q(2)*q(4)));
z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
ang = [x y z];
end
【旋转向量—>旋转矩阵】
function R = rv2R(rv)
% 输入为列向量
% 旋转向量转旋转矩阵
theta = norm(rv);
rv = rv/theta;
I = eye(3);
R = cos(theta)*I + (1-cos(theta))*rv*rv'+sin(theta)*[0,-rv(3),rv(2);
rv(3),0,-rv(1); -rv(2),rv(1),0];
end
【旋转矩阵—>旋转向量】
function rv = R2rv(R)
% 旋转矩阵转旋转向量
theta = acos((trace(R)-1)/2);
T = (R-R')/(2*sin(theta));
disp(T)
rv = [T(3,2)*theta,T(1,3)*theta,T(2,1)*theta];
end
注:旋转对应的代数运算不同于通常使用的经典代数运算,因此不要对欧拉角、旋转向量和四元数进行加减乘除操作。
最后给出一个好用的在线转换网站:3D Rotation Converter
参考博客: