matlab中旋转矩阵函数

matlab里的旋转矩阵、四元数、欧拉角

% 旋转矩阵转四元数
q = dcm2quat(R)
% 欧拉角转四元数
q = angle2quat(r1,r2,r3,S)
% 旋转矩阵转欧拉角
[r1,r2,r3] = dcm2angle(R,S)
% 四元数转欧拉角
[r1,r2,r3] = quat2angle([q0 q1 q2 q3],S)
% S是['ZYX','ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’]
% 四元数转旋转矩阵
R = quat2dcm([q0 q1 q2 q3])
% 欧拉角转旋转矩阵
R = angle2dcm(r1,r2,r3,S)

四元数

% 四元数单元化
q1=quatnormalize(q1);
% 四元数转为旋转矩阵，q1是单元化四元数
R1=quat2dcm(q1);%q1的第一位是实部
% 模（Modulus）
quatmod(p)
% 范数（Norm）
quatnorm(p)
% 单位化（Normalize）
quatnormalize(p)
% 求逆（Inverse）
quatinv(p)
% 四元数除法
quatdivide(q,p)
% 四元数乘法
quatmultiply(p,q)
% 共轭四元数
quatconj(p)
% 旋转函数
quatrotate(p)
% 四元数和欧拉角互换的函数
quat2angle(p)
angle2quat(p)

根据两向量计算向量之间的旋转矩阵和四元数

function [R, q, theta] = vec2quat_R(v1, v2)
	% 将向量转换为单位向量
	u1 = v1/norm(v1);
	u2 = v2/norm(v2);
	if norm(u1+u2) == 0
		q = [0 0 0 0];
	else
		u = cross(u1,u2);
		u = u/norm(u);
		theta = acos(dot(u1,u2));
		q = [cos(theta/2) sin(theta/2)*u];
	end
	% 四元数转为方向余弦矩阵
	dcm=[2*q(1).^2-1+2*q(2)^2  2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)-q(1)*q(3));
    2*(q(2)*q(3)-q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)+q(1)*q(2));
    2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)-q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
	% 四元数转为旋转矩阵
	rot = permute(dcm, [2 1 3]);
end

欧拉角转旋转矩阵

function R = Eular2R(x,y,z,mode)
	Rotx = [1      0      0;
	    0 cos(x) -sin(x);
	    0 sin(x) cos(x)];
	Roty = [cos(y)  0 sin(y);
	    0       1      0;
	    -sin(y) 0 cos(y)];
	Rotz = [cos(z) -sin(z) 0;
	    sin(z) cos(z)  0;
	    0      0       1];
    switch mode
	    case 1 %ZYX
	    	R = Rotz*Roty*Rotx;
	    case 1 %XYZ
	    	R = Rotx*Roty*Rotz;
	    case 1 %ZXY
	    	R = Rotz*Rotx*Roty;
	    case 1 %YZX
	    	R = Roty*Rotz*Rotx;
	    otherwise
	    	R = Rotz*Roty*Rotx;
	 end	    

旋转矩阵转欧拉角

function eular = R2eular(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));
	eular = [x y z];

旋转矩阵转四元数

function q = R2quat(R)
	t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
	q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
end

参考链接

Matlab ——旋转矩阵，四元数，欧拉角之间的转换