【机器人学】0-1.姿态的表示方法及相互转换【附MATLAB代码】

目录

理论公式

matlab代码

理论公式

matlab代码

function PT  = PoseTrans  
PT.AxisAngle2Quat=@AxisAngle2Quat;
PT.AxisAngle2RotMat=@AxisAngle2RotMat;
PT.Euler2RotMat=@Euler2RotMat;
PT.Quat2RotMat=@Quat2RotMat;
PT.Quat2AxisAngle=@Quat2AxisAngle;
PT.RotMat2AxisAngle=@RotMat2AxisAngle;
PT.RotMat2Euler=@RotMat2Euler;
PT.RotMat2Quat=@RotMat2Quat;
end
%  轴角转四元数
function [result,quat]=AxisAngle2Quat(rk)
    theta=(rk(1)^2+rk(2)^2+rk(3)^2)^0.5;
    if(abs(theta)<1e-8)
        quat=[1;0;0;0];
        result=0;
    else
        rkTemp=rk/theta;
        thetaHalf=theta/2;
        sinTH=sin(thetaHalf);
        cosTH=cos(thetaHalf);
        quat(1)=cosTH;
        quat(2)=rkTemp(1)*sinTH;
        quat(3)=rkTemp(2)*sinTH;
        quat(4)=rkTemp(3)*sinTH;
        result=1;
    end
    quat=quat';
end
%  轴角转旋转矩阵
function [result,rotMat]=AxisAngle2RotMat(rk)
    theta=(rk(1)^2+rk(2)^2+rk(3)^2)^0.5;
    if(abs(theta)<1e-8)
        rotMat=eye(3);
        result=0;
    else
        rkTemp=rk/theta;
        cosT=cos(theta);
        sinT=sin(theta);
        rotMat=[rkTemp(1)^2*(1-cosT)+cosT rkTemp(1)*rkTemp(2)*(1-cosT)-rkTemp(3)*sinT rkTemp(1)*rkTemp(3)*(1-cosT)+rkTemp(2)*sinT
           rkTemp(1)*rkTemp(2)*(1-cosT)+rkTemp(3)*sinT rkTemp(2)^2*(1-cosT)+cosT rkTemp(2)*rkTemp(3)*(1-cosT)-rkTemp(1)*sinT
           rkTemp(1)*rkTemp(3)*(1-cosT)-rkTemp(2)*sinT rkTemp(2)*rkTemp(3)*(1-cosT)+rkTemp(1)*sinT rkTemp(3)^2*(1-cosT)+cosT];
        result=0;
    end
end
%  欧拉角角转旋转矩阵
function rotMat=Euler2RotMat(rpy)
    rx=rpy(1);
    ry=rpy(2);
    rz=rpy(3);
    
    cRx=cos(rx);
    sRx=sin(rx);
    cRy=cos(ry);
    sRy=sin(ry);
    cRz=cos(rz);
    sRz=sin(rz);
    rotMat=[cRz*cRy cRz*sRy*sRx-sRz*cRx cRz*sRy*cRx+sRz*sRx
       sRz*cRy sRz*sRy*sRx+cRz*cRx sRz*sRy*cRx-cRz*sRx
       -sRy       cRy*sRx                      cRy*cRx];
end
% 四元数转轴角
function [result,rk]=Quat2AxisAngle(quat)
    theta=acos(quat(1))*2;
    if(abs(theta)<1e-8)
        rk=[0;0;0];
        result=1;
    else
        thetaHalf=theta/2;
        sinTH=sin(thetaHalf);
        rkTemp=quat(2:4)/sinTH;
        rk=rkTemp*theta;
        result=0;
    end
end
% 旋转矩阵转轴角
function [result,rk]=RotMat2AxisAngle(rotMat)
    theta=acos((rotMat(1,1)+rotMat(2,2)+rotMat(3,3)-1)/2);
    sinT=norm([rotMat(3,2)-rotMat(2,3);rotMat(1,3)-rotMat(3,1);rotMat(2,1)-rotMat(1,2)]/2);
    if(abs(sinT)<1e-18)
        rk=[0;0;0];
        result=0;
    else
        rkTemp=1/2/sinT*[rotMat(3,2)-rotMat(2,3);rotMat(1,3)-rotMat(3,1);rotMat(2,1)-rotMat(1,2)];
        rk=theta*rkTemp;
        result=0;
    end
end
% 旋转矩阵转欧拉角
function rpy=RotMat2Euler(rotMat)
	rpy(2) = atan2(-rotMat(3,1), sqrt(rotMat(1,1)^2+rotMat(2,1)^2));
	rpy(3) = atan2(rotMat(2,1)/cos(rpy(2)),rotMat(1,1)/cos(rpy(2)));
    rpy(1) = atan2(rotMat(3,2)/cos(rpy(2)),rotMat(3,3)/cos(rpy(2)));
end
% 旋转矩阵转四元数
function [result,quat]=RotMat2Quat(rotMat)
    quat(1)=sqrt(rotMat(1,1)+rotMat(2,2)+rotMat(3,3)+1)/2;
    if(quat(1)<1e-8)
        quat=[0;0;0;0];
        result=1;
    else
        quat(2)=(rotMat(3,2)-rotMat(2,3))/(4*quat(1));
        quat(3)=(rotMat(1,3)-rotMat(3,1))/(4*quat(1));
        quat(4)=(rotMat(2,1)-rotMat(1,2))/(4*quat(1));
        result=0;
    end
end
%  四元数转旋转矩阵
function rotMat=Quat2RotMat(Quat)
    q0 = Quat(1);
    q1 = Quat(2);
    q2 = Quat(3);
    q3 = Quat(4);
    rotMat = [1-2*q2*q2-2*q3*q3,2*q1*q2-2*q0*q3,2*q1*q3+2*q0*q2;
              2*q1*q2+2*q0*q3,1-2*q1*q1-2*q3*q3,2*q2*q3-2*q0*q1;
              2*q1*q3-2*q0*q2,2*q2*q3+2*q0*q1,1-2*q1*q1-2*q2*q2];
end