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');