機器人技術基礎實驗之Robotics Toolbox根據dh引數畫圖
阿新 • • 發佈:2019-01-13
在安裝方面,需要將下載好的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