資源簡(jiǎn)介
基于改進(jìn)DH參數(shù)的機(jī)器人正解和逆解程序,逆解采用解析解形式,輸出8組關(guān)節(jié)角度解

代碼片段和文件信息
clc
clear?all
close?all
deg=pi/180;
warning?off;
%%?讀取數(shù)據(jù)_轉(zhuǎn)換旋轉(zhuǎn)矩陣
data?=?xlsread(‘handian.xlsx‘);
%第一個(gè)點(diǎn)構(gòu)建位姿矩陣
E1=deg2rad(data(15:7));
p1=data(12:4)‘/1000;
R1=eul2rotm(E1?‘ZYX‘);
R11=R1;
R11(:4)=p1;
R11(4:)=[0?0?0?1];%旋轉(zhuǎn)矩陣加位置,構(gòu)建點(diǎn)1位姿矩陣
%第二個(gè)點(diǎn)構(gòu)建位姿矩陣
E2=deg2rad(data(25:7));
p2=data(22:4)‘/1000;
R2=eul2rotm(E2‘ZYX‘);
R22=R2;
R22(:4)=p2;
R22(4:)=[0?0?0?1];%旋轉(zhuǎn)矩陣加位置,構(gòu)建點(diǎn)1位姿矩陣
%%?建立機(jī)器人_計(jì)算正逆運(yùn)動(dòng)學(xué)
L1=?link(‘d‘?0?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?0‘modified‘‘qlim‘?[-180?180].*deg);
L2?=?link(‘d‘?0?‘a(chǎn)‘?0.325?‘a(chǎn)lpha‘?-pi/2‘modified‘‘qlim‘?[-150?-14].*deg);
L3?=?link(‘d‘?0?‘a(chǎn)‘?1.150?‘a(chǎn)lpha‘?0‘modified‘‘qlim‘?[-147?90].*deg);
L4?=?link(‘d‘?1.225?‘a(chǎn)‘?0.300?‘a(chǎn)lpha‘?-pi/2?‘modified‘‘qlim‘?[-210?210].*deg);
L5?=?link(‘d‘?0?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?pi/2‘‘modified‘?‘qlim‘?[-130?130].*deg);
L6?=?link(‘d‘?0.225?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?-pi/2‘modified‘‘qlim‘?[-210?210].*deg);
robot=Seriallink([L1L2L3L4L5L6]‘name‘‘安川ms165‘);??%Seriallink類函數(shù)
robot.display();%link類函數(shù),顯示建立機(jī)器人DH參數(shù)
robot.plot([0?0?0?0?0?0]);%初始位置
q1=mymodikine(R11);
q2=mymodikine(R22);
q11=q1(2:);
q22=q2(2:);
%q1=robot.ikine(R11);%計(jì)算機(jī)器人逆運(yùn)動(dòng)學(xué),關(guān)節(jié)坐標(biāo)向量
%q2=robot.ikine(R22);
t=?linspace(0211);%步長(zhǎng)11,等分10段
g=jtraj(q11q22t);%五次多項(xiàng)式規(guī)劃軌跡
T=robot.fkine(g);%正運(yùn)動(dòng)學(xué)計(jì)算軌跡每個(gè)點(diǎn)的位姿
robot.plot(g)%顯示機(jī)器人軌跡
teach(robot);
%%?計(jì)算軌跡長(zhǎng)度
long=0;
i=1;
for?i?=?1:11
????x=T(14i);
????y=T(24i);
????z=T(34i);
????long=long+sqrt(x^2?+?y^2?+?z^2);
????i=i+1;
end
%%?旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角
for?j=?1:11
????Eul=rotm2eul(T(1:31:3j)?‘ZYX‘);
????Eo=Eul.*(180/pi);
????a=T(1:34j)‘;
????save(j:)=[a?Eo?];
????j=j+1;
end
?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件???????1775??2019-09-25?16:20??jtraj\guiji.m
?????文件??????11202??2019-09-24?18:15??jtraj\handian.xlsx
?????文件???????2769??2019-09-25?16:18??jtraj\myfkine.m
?????文件???????6534??2019-09-25?16:30??jtraj\mymodikine.m
????..A..H.???????165??2019-09-23?10:14??jtraj\~$handian.xlsx
?????目錄??????????0??2019-09-25?18:10??jtraj
-----------?---------??----------?-----??----
????????????????22445????????????????????6
評(píng)論
共有 條評(píng)論