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函數，我們發現了問題所在，這是因為存在兩種不同的旋轉順序，導致不一樣的矩陣相乘順序，所以最終得出的旋轉矩陣也不一樣。

 

感悟：要多去讀源代碼！才能徹底領會這些函數的使用方法和實現原理。