问题背景:在根据《机器人导论》这本书的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)