1.算法仿真效果
matlab2022a仿真結果如下：
2.算法涉及理論知識概要
對機器人進行圖形仿真，可以直觀顯示出機器人的運動情況，得到從數據曲線中難以分析出來的許多重要信息，并能從圖形上看到機器人在一定控制條件下的運動規律。從仿真軟件中觀察機器人工作程序的運行結果，就能分析出該機器人軌跡規劃等的正確性和合理性，從而為離線編程提供有效的驗證手段。
PUMA560 機械臂是一種示教機器人。有六個自由度，包括6個旋轉關節，模仿人的腰、肩、肘和手腕運動，能以規定的姿態到達工作范圍內的任何一個點。包括：臂體、控制器和示教器三個部分。
PUMA560 機器手是工業機器人（或稱機器人操作臂）。從外形來看，它和人的手臂相似，是由一系列剛性連桿通過一系列柔性關節交替連接而成的開式鏈。PUMA560的基座、連桿一、連桿二、連桿三、連桿四到六分別類似于的盆骨、腰椎、大臂、小臂、腕手。操作臂的前端裝有末端執行器或相應的工具，也常稱為手或手爪。手臂的動作幅度一般較大，通常實現宏操作。
PUMA560 型機器人由機器人本體（手臂）和計算機控制系統兩大部分組成。PUMA560整個手臂重53kg，有六個自由度，驅動采用直流伺服電機并配有安全剎閘，手腕最大載荷為2 kg （包括手腕法蘭盤），最大抓緊力為60 N，重復精度為±0.1mm。工具在最大載荷下的自由運動速度速度為1.0m/s,直線運動速度為0.5m/s。操作范圍是以肩部中心為球心0.92 m 為半徑的空間半球。
PUMA560機械手屬于關節式的機器人，6個關節都是轉動關節。前面3個關節確定手腕參考點的位置，后面3個關節確定手腕的方位。和多數工業機器人一樣，后3個關節軸線交于一點。關節1的軸線為鉛直方向，關節1和關節2的軸線垂直相交，關節3和關節4的軸線垂直交錯。
PUMA560一般由移動關節和轉動關節共同作用，組成機器人的操作臂，每個關節都有一個自由度。在六自由度下，我們規定連桿0表示基座，關節1讓基座0與連桿1相接，關節2讓連桿1與連桿2相連接，以此類推。DH參數表如圖1所示，圖中單位為rad和mm：
3.MATLAB核心程序
figure;
% subplot(121);
plot(KER*xd1,KER*yd1,'b','linewidth',2);
hold on
plot(KER*X1,KER*Y1,'r');
xlabel('u(pixel)');
ylabel('v(pixel)');
legend('desired trajectory','real trajectory');
axis square;
axis([-100,100,-100,100]);
grid on
% subplot(122);
figure;
plot(fliplr(KER*X1-KER*xd1),'b','linewidth',2);
hold on
plot(fliplr(KER*Y1-KER*yd1),'g','linewidth',2);
legend('error U','error V');
axis([0,400,-150,150]);
grid on
xlabel('time');
ylabel('error');
%%
%Estimated parameters
[p1,pd1,pdd1] = tpoly(q0(1), q1(1), 400);%得到位置、速度、加速度
[p2,pd2,pdd2] = tpoly(q0(2), q1(2), 400);%得到位置、速度、加速度
[p3,pd3,pdd3] = tpoly(q0(3), q1(3), 400);%得到位置、速度、加速度
[p4,pd4,pdd4] = tpoly(q0(4), q1(4), 400);%得到位置、速度、加速度
[p5,pd5,pdd5] = tpoly(q0(5), q1(5), 400);%得到位置、速度、加速度
[p6,pd6,pdd6] = tpoly(q0(6), q1(6), 400);%得到位置、速度、加速度
figure;
subplot(311);
plot(p1);title('Position');
subplot(312);
plot(pd1);title('Velocity');
subplot(313);
plot(pdd1);title('Acceleration');
dTheta = zeros(11,length(xd1));
Theta ?= zeros(11,length(xd1));
error ?= 0.5*(fliplr(KER*Y1-KER*yd1)+fliplr(KER*X1-KER*xd1));
W ?????= zeros(11,length(xd1));
W0 ????= round(-50*randn(1,11));
for i = 1:length(xd1)
for j = 1:11
W(j,i) = W0(j);
end
end
for i = 1:length(xd1)
for j = 1:11
dTheta(j,i) = abs(error(i))/W(j,i)*exp(1-i/50);
end
end
for j = 1:11
for i = 1:length(xd1)
Theta(j,i) = sum(dTheta(j,1:i));
end
end
figure;
subplot(211);
plot(Theta(1,:),'b','linewidth',2);
hold on
plot(Theta(2,:),'r--','linewidth',2);
hold on
plot(Theta(3,:),'g','linewidth',2);
hold on
plot(Theta(4,:),'k--','linewidth',2);
hold on
plot(Theta(5,:),'m','linewidth',2);
hold on
plot(Theta(6,:),'c--','linewidth',2);
hold on
grid on
legend('theta_1','theta_2','theta_3','theta_4','theta_5','theta_6');
axis([20,400,-30,100]);
xlabel('time');
subplot(212);
plot(Theta(7,:),'b','linewidth',2);
hold on
plot(Theta(8,:),'r--','linewidth',2);
hold on
plot(Theta(9,:),'g','linewidth',2);
hold on
plot(Theta(10,:),'k--','linewidth',2);
hold on
plot(Theta(11,:),'m','linewidth',2);
hold on
grid on
legend('theta_7','theta_8','theta_9','theta_1_0','theta_1_1');
axis([20,400,-120,450]);
xlabel('time');
