matlab rpy2tr,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。

代码如下：

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

4 function R=EularToR(afa, beta,gama)5

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

10 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 argumentsif numcols(roll) == 3pitch= roll(:,2);%pitch(y)应该是输入参数的第二个值

yaw= roll(:,3);%yaw(z)应该是输入参数的第三个值

roll= roll(:,1);%roll(x)应该是输入参数的第一个值

elseif nargin>= 3%还不太理解这一段的意思pitch= args{1};

yaw= args{2};elseerror(‘RTB:rpy2r:badarg‘, ‘bad arguments‘)

end% optionally convert fromdegrees考虑如果输入的参数是角度，且已经将"deg"作为选项输入函数，需将角度转化为弧度ifopt.deg

d2r= pi/180.0;

roll= roll *d2r;

pitch= pitch *d2r;

yaw= yaw *d2r;

endif ~opt.zyx%XYZ order如果是先绕着X轴，再Y轴，最后Z轴旋转的话，是如下的矩阵相乘顺序if numrows(roll) == 1R= rotx(roll) * roty(pitch) *rotz(yaw);elseR= zeros(3,3,numrows(roll));for i=1:numrows(roll)

R(:,:,i)= rotx(roll(i)) * roty(pitch(i)) *rotz(yaw(i));

end

endelse

% old ZYX order (asper Paul book)如果是先绕着Z轴，再Y轴，再X轴旋转的话，又将是不同的相乘顺序if numrows(roll) == 1R= rotz(roll) * roty(pitch) *rotx(yaw);elseR= 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函数，我们发现了问题所在，这是因为存在两种不同的旋转顺序，导致不一样的矩阵相乘顺序，所以最终得出的旋转矩阵也不一样。

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