机器人学——姿态表示方式相互转换(Matlab代码)

片酷

已于 2023-10-20 22:06:36 修改

阅读量837

点赞数

文章标签： matlab 算法开发语言机器人

于 2023-10-20 22:02:41 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_55188463/article/details/133954221

版权

姿态描述方式通常有：旋转矩阵、四元数、欧拉角、旋转向量等，在此给出几种描述相互转换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

参考博客：

[1]三维旋转之欧拉角 - 知乎