1. 程式人生 > >機器人技術基礎實驗之Robotics Toolbox根據dh引數畫圖

機器人技術基礎實驗之Robotics Toolbox根據dh引數畫圖

在安裝方面,需要將下載好的rar檔案解壓(名稱為rvctools),放置於matlab資料夾的toolbox內並且利用matlab工具欄的setpath命令,將資料夾設定為搜尋目錄,在使用時,輸入 startup_rvc啟動該工具箱
該工具箱下載地址為:http://www.petercorke.com/Robotics_Toolbox.html
示例程式,需自行代入DH引數進行計算。

clear;
L1=Link('d',81,'a',0,'alpha',pi/2);
L2=Link('d',0,'a',192.5,'alpha',-pi/2);
L3=Link('d',0,'a',400,'alpha',0);
L4=Link('d',0,'a',168.5,'alpha',0);
L5=Link('d',0,'a',400,'alpha',0);
L6=Link('d',0,'a',136.3,'alpha',0);
L7=Link('d',0,'a',133.75,'alpha',0);
b=isrevolute(L1);
robot=SerialLink([L1,L2,L3,L4,L5,L6,L7]);
robot.name='lyb';
robot.comment='lyb';
robot.display();
theta0=[0,0,-pi/2,-pi/2,pi/2,pi/2,-pi/2];
robot.plot(theta0)

用工具箱正解函式fkine求解:

clear;
clc;
%建立機器人模型
%       theta    d           a        alpha     offset
SL1=Link([0       0          0.180    -pi/2        0     ],'standard'); 
SL2=Link([0       0          0.600     0           0     ],'standard');
SL3=Link([0       0          0.130    -pi/2        0     ],'standard');
SL4=Link([0       0.630       0        pi/2        0     ],'standard');
SL6=Link([0       0.1075      0        0           0     ],'standard');
SL5=Link([0       0           0       -pi/2        0     ],'standard');
starobot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','standard');
stat06=starobot.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函式
stamyt06=mystafkine(0,0,pi/2,0,0,pi/2)  %手寫正解函式

正解函式


function [T06]=mystafkine(theta1,theta2,theta3,theta4,theta5,theta6)
SDH=[theta1   0       0.180   -pi/2;
     theta2   0       0.600    0;
     theta3   0       0.130   -pi/2;
     theta4   0.630   0        pi/2;
     theta5   0       0       -pi/2;
     theta6   0.1075  0        0];

 T01=[cos(SDH(1,1))  -sin(SDH(1,1))*cos(SDH(1,4))   sin(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*cos(SDH(1,1));
      sin(SDH(1,1))   cos(SDH(1,1))*cos(SDH(1,4))  -cos(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*sin(SDH(1,1));
      0               sin(SDH(1,4))                 cos(SDH(1,4))                  SDH(1,2);
      0               0                             0                              1];
 T12=[cos(SDH(2,1))  -sin(SDH(2,1))*cos(SDH(2,4))   sin(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*cos(SDH(2,1));
      sin(SDH(2,1))   cos(SDH(2,1))*cos(SDH(2,4))  -cos(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*sin(SDH(2,1));
      0               sin(SDH(2,4))                 cos(SDH(2,4))                  SDH(2,2);
      0               0                             0                              1];
 T23=[cos(SDH(3,1))  -sin(SDH(3,1))*cos(SDH(3,4))   sin(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*cos(SDH(3,1));
      sin(SDH(3,1))   cos(SDH(3,1))*cos(SDH(3,4))  -cos(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*sin(SDH(3,1));
      0               sin(SDH(3,4))                 cos(SDH(3,4))                  SDH(3,2);
      0               0                             0                              1];
 T34=[cos(SDH(4,1))  -sin(SDH(4,1))*cos(SDH(4,4))   sin(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*cos(SDH(4,1));
      sin(SDH(4,1))   cos(SDH(4,1))*cos(SDH(4,4))  -cos(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*sin(SDH(4,1));
      0               sin(SDH(4,4))                 cos(SDH(4,4))                  SDH(4,2);
      0               0                             0                              1];
 T45=[cos(SDH(5,1))  -sin(SDH(5,1))*cos(SDH(5,4))   sin(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*cos(SDH(5,1));
      sin(SDH(5,1))   cos(SDH(5,1))*cos(SDH(5,4))  -cos(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*sin(SDH(5,1));
      0               sin(SDH(5,4))                 cos(SDH(5,4))                  SDH(5,2);
      0               0                             0                              1];
 T56=[cos(SDH(6,1))  -sin(SDH(6,1))*cos(SDH(6,4))   sin(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*cos(SDH(6,1));
      sin(SDH(6,1))   cos(SDH(6,1))*cos(SDH(6,4))  -cos(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*sin(SDH(6,1));
      0               sin(SDH(6,4))                 cos(SDH(6,4))                  SDH(6,2);
      0               0                             0                              1];

 T06=T01*T12*T23*T34*T45*T56;
end