文章目录
- matlab里的旋转矩阵、四元数、欧拉角
- 四元数
- 根据两向量计算向量之间的旋转矩阵和四元数
- 欧拉角转旋转矩阵
- 旋转矩阵转欧拉角
- 旋转矩阵转四元数
- 参考链接
matlab里的旋转矩阵、四元数、欧拉角
| 旋转矩阵 | dcm | R |
|---|---|---|
| 四元数 | quat | q=[q0,q1,q2,q3] |
| 欧拉角 | angle | [row,pitch,yaw] |
% 旋转矩阵转四元数
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) == 0q = [0 0 0 0];elseu = 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 modecase 1 %ZYXR = Rotz*Roty*Rotx;case 1 %XYZR = Rotx*Roty*Rotz;case 1 %ZXYR = Rotz*Rotx*Roty;case 1 %YZXR = Roty*Rotz*Rotx;otherwiseR = 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 ——旋转矩阵，四元数，欧拉角之间的转换
