六自由度机械臂的正运动学解with RTB

       本文采用机械人工具箱（RTB），通过MDH法建立坐标参数表并计算正运动学。基于工具箱生成的三维图与自己建立的坐标系初始状态比对，进一步验证正确性。

---

                                                                               图1 坐标系及参数表

---

matlab代码：

function MyCoordinateARM6RPart
%计算操作臂部分坐标系并验证
%工具：RobotToolBox V9.10
%时间：2019/1/30
%更新：2019/

%%
%parameters of coordinate
clear all; 
clc;
tic
%****************************************************
d1=0.50;d5=0.45;d6=0.20;
a2=0.4; a3=0.35; a6=0.55;
alpha1=pi/2;alpha4=pi/2; alpha5=pi/2;
%****************************************************
%%
%MDH coordinate
%         θ    d   a    α       offset  
L(1)=Link([0   d1   0    0          0  ],'modified');
L(2)=Link([0   0    0    alpha1     0  ],'modified');
L(3)=Link([0   0   a2    0          0  ],'modified');
L(4)=Link([0   0   a3    0          0  ],'modified');
L(5)=Link([0   d5   0    alpha4     0  ],'modified');
L(6)=Link([0   d6   0    alpha5     0  ],'modified');
L(7)=Link([0   0   a6     0         0  ],'modified');

%%
%posture
qr=[0 0 0 pi/2 0 pi/2 0] ;  % ready
qu=[0 pi/3 -pi/6 5*pi/9 -3*pi/4 pi/2 0]; % standup 
%%
%display table of MDH
robot=SerialLink(L,'name','robot6R','manufacturer','Unimation','comment','AK&B');
robot.display();  %display MDH table

%%
%in workspace display climb
% robot.fkine:forward compute of kinematics 
% robot.plot :plot state
%qr:robot posture of ready
%qu:robot posture of standup
robot.fkine(qr) %zero
robot.plot(qr); %ready

robot.fkine(qu); 
robot.plot(qu); %standup
%%
%inverse kinematics 
%ikine6s:only adapt have a sphere wrist joint robot and only standard DH 
%ikine :suit for general joint

%%
%jtraj: Compute a joint space trajectory between two configurations;
%       A quintic (5th order) polynomial is used with default zero boundary
%       conditions for velocity and acceleration.
% qd:velocity
% qdd:acceleration
% N=100;
% [q,qd,qdd]=jtraj(qr,qu,N);

t=0:0.01:1;
[q,qd,qdd]=jtraj(qr,qu,t);
plot(t,qd,t,qdd); 

T=robot.fkine(q);  %transform Matrix of the endcoordinate respacts to basementCoordinate
plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:)));
hold on;
robot.plot(q);

%compute Jacobian matrix
% J=robot.jacob0(qr);
%compute torque of joints
% tao=robot.jacob0(qr)'*[0 0 20 0 0 0]'
%%
teach(robot)

%%
tic;
end

---

代码生成结果：

1.代码生成参数表

---

 

 

2.坐标验证段图像

---

3.轨迹规划示教段图像

---

4.两处位置的正运动解

qr:起始时刻工具端坐标位置

qr=

    1.0000         0    0.0000    1.7500
   -0.0000   -0.0000    1.0000    0.2000
         0   -1.0000   -0.0000    0.5000
         0         0         0    1.0000

qu:终时刻工具端关节坐标位置

qu =

    0.7660   -0.4545    0.4545    1.3601
    0.0000   -0.7071   -0.7071   -0.1414
    0.6428    0.5417   -0.5417    1.5559
         0         0         0    1.0000

---

总结

        MDH建立坐标系的时候尤其注意Z轴与X轴关系，同时在建系的过程中时刻想着二者是如何绕着旋转的会简化操作和理解。同时，MDH是建立在坐标系的前端，所以最后一个坐标系其实是控制的最后一个关节处而没有控制到连杆的末端，这样通过增加的关节，也即平移坐标系实现实际工具端的位置。