控制科学与工程：随手笔记（4）--工业机器人仿真

之前用牛顿下山法进行matlab仿真时一直出现误差收敛到一定范围之后就无法继续下降的问题，现修改代码如下：
原代码可查看：https://blog.youkuaiyun.com/qq_37708045/article/details/88637326

%% 牛顿拉夫逊迭代法
pause;
Target.A=Link(7).A;
Target.x=Target.A(:,1);
Target.y=Target.A(:,2);
Target.z=Target.A(:,3);
Target.p=Target.A(1:3,4);
for i=1:20
    Target.p(3)=Target.p(3)+i;
    [th1,th2,dz3,th4,th5,th6]=Newton_Raphson(th1,th2,dz3,th4,th5,th6,Target);    
    DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
    x(num)=Link(7).p(1);
    y(num)=Link(7).p(2);
    z(num)=Link(7).p(3);
    num=num+1;
    h=plot3(x,y,z,'r.');hold on;
end
 DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);

Newton_Raphson.m

function [th1,th2,dz3,th4,th5,th6] = Newton_Raphson(th1,th2,dz3,th4,th5,th6,Target)

global Link

ToDeg = 180/pi;
ToRad = pi/180;

eps=1e-6;
num