Matlab练习——rpy2tr函数与自己实现的ZYX欧拉角的结果不同的问题

问题背景：在根据《机器人导论》这本书的Z-Y-X欧拉角原理用Matlab实现旋转矩阵求解时，发现与直接调用机器人工具箱中的rpy2tr()函数得出的结果并不相同。

首先：先检查自己写的函数是否有错。根据其原理：坐标系B可以用如下方式表示——先将坐标系B和一个已知参考坐标系A重合，先将B绕B的Z轴转afa角，再绕B的Y轴转beta角，最后绕B的X轴转gama角。所以得到的旋转矩阵就是R=Rz*Ry*Rx。

代码如下：

%afa:绕着z轴的旋转角——yaw；beta：绕着y轴的旋转角——pitch；gama：绕着x轴的旋转角——roll（都是角度制）
%机器人工具箱里面提供的函数是rpy2tr()(是弧度制）——且对应的角度是：roll—pitch—yaw

function R=EularToR(afa, beta,gama)

Rz=[cosd(afa), -sind(afa), 0; sind(afa), cosd(afa), 0; 0, 0, 1]; %afa是绕z轴旋转，偏航角——yaw
Ry=[cosd(beta),0, sind(beta);0, 1, 0; -sind(beta), 0, cosd(beta)];%beta是绕y轴旋转，俯仰角——pitch
Rx=[1, 0, 0; 0, cosd(gama), -sind(gama); 0, sind(gama), cosd(gama)];%gama是绕x轴旋转，翻滚角——roll

R=Rz*Ry*Rx;

发现应该没有什么问题。

再次：理解Matlab中rpy2tr函数的实现方式

function T = rpy2tr(roll, varargin)

    R = rpy2r(roll, varargin{:});
    T = r2t(R);

转入rpy2r(roll, varargin{:})函数继续查看

function R = rpy2r(roll, varargin) 
%varargin提供了一种函数可变参数列表机制，
%允许调用者调用该函数时根据需要来改变输入参数的个数

%设置默认参数
    opt.zyx = false;
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
        pitch = roll(:,2);%pitch(y)应该是输入参数的第二个值
        yaw = roll(:,3);%yaw(z)应该是输入参数的第三个值
        roll = roll(:,1);%roll(x)应该是输入参数的第一个值
    elseif nargin >= 3%还不太理解这一段的意思
        pitch = args{1};
        yaw = args{2};
    else
        error('RTB:rpy2r:badarg', 'bad arguments')
    end

    % optionally convert from degrees考虑如果输入的参数是角度，且已经将"deg"作为选项输入函数，需将角度转化为弧度
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    if ~opt.zyx
        % XYZ order如果是先绕着X轴，再Y轴，最后Z轴旋转的话，是如下的矩阵相乘顺序
        if numrows(roll) == 1
            R = rotx(roll) * roty(pitch) * rotz(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotx(roll(i)) * roty(pitch(i)) * rotz(yaw(i));
            end
        end
    else
        % old ZYX order (as per Paul book)如果是先绕着Z轴，再Y轴，再X轴旋转的话，又将是不同的相乘顺序
        if numrows(roll) == 1
            R = rotz(roll) * roty(pitch) * rotx(yaw);
        else
            R = zeros(3,3,numrows(roll));
            for i=1:numrows(roll)
                R(:,:,i) = rotz(roll(i)) * roty(pitch(i)) * rotx(yaw(i));
            end
        end
    end

结论：理解完rpy2tr函数，我们发现了问题所在，这是因为存在两种不同的旋转顺序，导致不一样的矩阵相乘顺序，所以最终得出的旋转矩阵也不一样。

感悟：要多去读源代码！才能彻底领会这些函数的使用方法和实现原理。