innfos-gluon 机械臂正逆运动学Matlab机器人工具箱及矩阵验算

 

 改进DH表：

i	theta	d	a	alpha
1	0	0	0	0
2	-pi/2	79.2	0	-pi/2
3	0	0	173	0
4	pi/2	0	173	0
5	0	79.2	0	pi/2
6	0	41.3	0	-pi/2

正运动学验算：

%定义各连杆
L(1)=Link([0,0,0,0,0],'modified');
L(2)=Link([0,79.2,0,-pi/2,0],'modified');
L(3)=Link([0,0,173,0,0],'modified');
L(4)=Link([0,0,173,0,0],'modified');
L(5)=Link([0,79.2,0,pi/2,0],'modified');
L(6)=Link([0,41.3,0,-pi/2,0],'modified');

%连杆偏置
L(2).offset=-pi/2;
L(4).offset=pi/2;

%限制关节
L(1).qlim=[-140,140]/180*pi;
L(2).qlim=[-90,90]/180*pi;
L(3).qlim=[-140,140]/180*pi;
L(4).qlim=[-140,140]/180*pi;
L(5).qlim=[-140,140]/180*pi;
L(6).qlim=[-360,360]/180*pi;

innfos_robot=SerialLink(L,'name','innfos_robot')
% innfos_robot.base=transl(0,0,101.5);
innfos_robot.teach
hold on
% innfos_robot.n%显示关节数

%设定各关节角度值进行正运动学计算
q1=[pi/4,pi/4,pi/4,pi/4,pi/4,pi/4];
innfos_robot.fkine(q1)

 结果为：

接下来用matlab矩阵验算：

th1=pi/4;th2=pi/4;th3=pi/4;th4=pi/4; th5=pi/4; th6=pi/4;%设定各关节角度值进行正运动学验算
d1=79.2; a2=173; a3=173; d4=79.2;d5=41.3;
T01=[cos(th1),-sin(th1),0,0;
    sin(th1),cos(th1),0,0;
    0,0,1,0;
    0,0,0,1];
T12=[sin(th2),cos(th2),0,0;
    0,0,1,d1;
    cos(th2),-sin(th2),0,0;
    0,0,0,1];
T23=[cos(th3),-sin(th3),0,a2;
    sin(th3),cos(th3),0,0;
    0,0,1,0;
    0,0,0,1];
T34=[-sin(th4),-cos(th4),0,a3;
    cos(th4),-sin(th4),0,0;
    0,0,1,0;
    0,0,0,1];
T45=[cos(th5),-sin(th5),0,0;
    0,0,-1,-d4;
    sin(th5),cos(th5),0,0;
    0,0,0,1];
T56=[cos(th6),-sin(th6),0,0;
    0,0,1,d5;
    -sin(th6),-cos(th6),0,0;
    0,0,0,1];
T06=T01*T12*T23*T34*T45*T56

 结果为：

 结果一致。

接下来进行逆运动学解算：

（待学习）