五自由度机械臂运动学分析（三转动+两移动自由度）详解

1正运动学分析

采用标准的D-h法进行机械腿模型分析：

D-h表如下

（2）通过（1）求解出机器人各位姿变换矩阵后，求解机器人手臂变换矩阵。通过matlab 计算，写出机器人末端位置。

正运动学分析

根据D-H表规定得到如下变换矩阵为：

由此可得机器人相邻两关节位姿分别为：

所以，坐标系{4}相对于基坐标系的变换矩阵为：

相对于基坐标系的旋转矩阵

位置矢量

根据DH参数求解变换矩阵的函数trans：

%输入JD，即6个关节变量的值，求解正运动方程

function [ T ] = trans( theta, d, a, alpha )

T =[

     cos(theta),   -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);

     sin(theta),   cos(theta)*cos(alpha),  -cos(theta)*sin(alpha), a*sin(theta);

     0,           sin(alpha),              cos(alpha),            d;

     0,                      0,                      0,           1            ];

end

3机器人模型建立

所设计的机器人由多个连杆机构组成，其关节类型包括旋转关节和移动关节两种。利用Matlab中机器人仿真工具箱Robotics Tool中Link和SerialLink两个函数可建立机器人模型[] 。其中，函数表达式如下：

式中，theta为关节变量；d为偏置距离；alpha为扭转角；a为连杆长度；sigma表示关节类型（0为旋转关节，1为移动关节）；前四个参数分别对应表1中的，，，。

具体程序编制如下：

% DH parameters  theta     d        a    alpha  qlim    

L(1) = Link('d', 126, 'a',0, 'alpha', 0,'qlim',[0,pi/2]);L(1).offset=-pi/2;%定义连杆

L(2) = Link('theta', 0, 'a', 0, 'alpha', -pi/2,'qlim',[0,720]);L(2).offset=300;

L(3) = Link('theta', 0, 'a', 0, 'alpha',  0,'qlim',[127.5,707.85]);L(3).offset=300;

L(4) = Link('d', 242, 'a', 0, 'alpha',-pi/2,'qlim',[-pi,pi]);L(4).offset=pi/2;

L(5) = Link('d', 0, 'a', 126.97, 'alpha',0,'qlim',[-pi,pi]);L(5).offset=-pi/2;

robot = SerialLink(L,'name' , '机器人'); %定义机器人模型函数

robot.display() %显示D-H表

h=1000;%工作空间参数

robot.plotopt = {'workspace',[-h,h,-h,h,-h,h],'tilesize',h}; %设置工作空间

robot.teach()%显示机器人模型

运行上述程序，即可得到码垛机器人模型如图 3-3

4机器臂正运动验证

init_ang=[0,300,300,0 0]; %任意起点

disp('工具箱求解得到旋转矩阵如下：')

robot.fkine(init_ang)

%%

%自定义矩阵

disp('通过变换矩阵求解旋转矩阵如下：')

fkine(init_ang)

如图所示：任意选取某点工具箱仿真姿态结果与变换矩阵计算结果一致。验证了正运动学方程的正确性。

需要完整代码看付费内容或者加扣扣2386317960