%%
%%逆運動學(xué)驗證
clear;
clc;
%建立機器人模型
%?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‘‘puma560‘‘base‘??...
transl（0?0?0.62）*?trotz（0））;?%連接連桿，機器人取名puma560
%?robot.plot（[0pi/2000pi/2]）;%輸出機器人模型，后面的六個角為輸出時的theta姿態(tài)
robot.plot（[000000]）
figure（1）
%?robot.teach（）?
robot.display（）;?%顯示D-H表
hold?on
T1?=?transl（5000-400）;	%起點
T2?=?transl（-300-800-100）;	%終點
%ctraj?利用勻加速勻減速規(guī)劃軌跡%
T?=?ctraj（T1T250）;
Tj?=?transl（T）;
%輸出末端軌跡%
plot3（Tj（:1）Tj（:2）Tj（:3））;
grid?on;

T=Tj;
G?=?[];??%用來存儲新的障礙節(jié)點，用于柵格地圖的更新
%?N?=?ndims（T）;
N?=?size（T1）;
Q?=?[];?%可行路徑上的最終各點的關(guān)節(jié)角
c?=?[0?0?0?0?0?0?0?0];?%不可行點判別數(shù)組??%?A為路徑可行判別數(shù)，可行為1，不可行為0
t0=[0?0?0?0?0?0];
e0?=?[];
temp0=0;
sum?=?[];
for?i?=1:N????%分別對各點進行碰撞檢測
????t?=?T（i:）;
????q?=?transl（t）;
????TH?=?niyundx_change（q）;
????for?n=1:8
????????for?m=1:6
????????????if?TH（nm）>pi
????????????????TH（nm）=TH（nm）-2*pi;
????????????elseif?TH（nm）<-pi
??????