clear;
clc
load（‘output1.mat‘）
ty=output1（:1）;
ty=ty‘;
tx=1:21;
tx1=1:0.2:21;
ty1=interp1（txtytx1‘cubic‘）;

load（‘output11.mat‘）
tyy=output11;
tyy_temp=tyy;
for?i?=1:21
???tyy（i:）=tyy_temp（22-i:）;
end


figure??
plot（txty‘o‘tx1ty1‘r‘）;??
title（‘三次多項(xiàng)式插值‘）??

q_c=[];
q_c1=[];
for?i=1:6
????ty=output1（:i）;
????ty=ty‘;
????tx=1:21;
????tx1=1:0.2:21;
????q_c（i:）=interp1（txtytx1‘cubic‘）;
end
for?i=1:6
????tyy1=tyy（:i）;
????tyy1=tyy1‘;
????txx=1:21;
????txx1=1:0.2:21;
????q_c1（i:）=interp1（txxtyy1txx1‘cubic‘）;
end
q_c=q_c‘;
q_c1=q_c1‘;

figure
bil=35;
bil1=bil*0.5;
t1?=?[24?27;
??????2?41;
??????2?11];
x1=t1（12）*bil1;y1=t1（22）*bil1;z1=（t1（31）-1）*bil1;%角點(diǎn)
l=（t1（12）-t1（11）+1）*bil1;w=（t1（22）-t1（21）+1）*bil1;h=（t1（32）-t1（31）+1）*bil1;%長(zhǎng)，寬，高
[a1b1c1]=meshgrid（[0?1]）;
p=alphaShape（l*a1（:）-（l-x1）w*b1（:）-（w-y1）h*c1（:）-（0-z1））;
plot（p‘edgecolor‘‘none‘）
xlabel（‘x‘）;ylabel（‘y‘）;zlabel（‘z‘）;
camlight
hold?on
axis（[-10001000-10001000-10001500]）
view（[0500]）

bil1=bil*0.5;
t1?=?[24?27;
??????2?41;
??????32?41];
x1=t1（12）*bil1;y1=t1（22）*bil1;z1=（t1（31）-1）*bil1;%角點(diǎn)
l=（t1（12）-t1（11）+1）*bil1;w=（t1（22）-t1（21）+1）*bil1;h=（t1（32）-t1（31）+1）*bil1;%長(zhǎng)，寬，高
[a1b1c1]=meshgrid（[0?1]）;
p=alphaShape（l*a1（:）-（l-x1）w*b1（:）-（w-y1）h*c1（:）-（0-z1））;
plot（p‘edgecolor‘‘none‘）
xlabel（‘x‘）;ylabel（‘y‘）;zlabel（‘z‘）;
camlight
hold?on
axis（[-10001000-10001000-10001500]）
view（[0500]）

%%
%建立機(jī)器人模型
%?theta?d?a?alpha?offset
L1=link（[0?0?0?0?0?]‘modified‘）;?%連桿的D-H參數(shù)
L2=link（[0?149.09?0?-pi/2?0?]‘modified‘）;
L3=link（[0?0?431.8?0?0?]‘modified‘）;
L4=link（[0?433.07?20.32?-pi/2?0?]‘modified‘）;
L5=link（[0?0?0?pi/2?0?]‘modified‘）;
L6=link（[0?0?0?-pi/2?0?]‘modified‘）;
robot=Seriallink（[L1?L2?L3?L4?L5?L6]‘name‘‘ABB1410‘‘base‘??...
transl（0?0?0.62）*?trotz（0））;?%連接連桿，機(jī)器人取名puma560
q=[q_c1;q_c];
robot.plot（q）;
t=robot.fkine（q）;%運(yùn)動(dòng)學(xué)正解?

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2020-07-15?22:51??機(jī)械臂避障路徑規(guī)劃仿真\
?????文件????????1068??2020-06-30?20:09??機(jī)械臂避障路徑規(guī)劃仿真\output1.mat
?????文件????????1062??2020-07-02?14:10??機(jī)械臂避障路徑規(guī)劃仿真\output11.mat
?????文件????????2009??2020-07-03?21:34??機(jī)械臂避障路徑規(guī)劃仿真\sancichazhi.m