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

之前用牛顿下山法进行matlab仿真时一直出现误差收敛到一定范围之后就无法继续下降的问题，现修改代码如下：
原代码可查看：https://blog.csdn.net/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=1;
while 1
    figure(1);
    J1=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6);
    %view(-90,0);
    %view(0,90);
    err = Err_6DOF(Target,Link(7));
    E = err'*err;
    if E

Err_6DOF.m

function err = Err_6DOF(Target, Current)
    a=eye(3);
%     Target.p= Target.A(1:3,4);
%     Target.x= Target.A(:,1);
%     Target.y= Target.A(:,2);
%     Target.z= Target.A(:,3);
    Target.R=[Target.x(1:3),Target.y(1:3),Target.z(1:3)];

    Perr = Target.p - Current.p;
    Rerr = Current.R' * Target.R;
    b=[Rerr(3,2)-Rerr(2,3),Rerr(1,3)-Rerr(3,1),Rerr(2,1)-Rerr(1,2)]';
    theta = acos((Rerr(1,1)+Rerr(2,2)+Rerr(3,3)-1)/2.0);
if Rerr~=a
    Werr=Current.R *theta/(2*sin(theta))*b;
else
    Werr=[0 0 0]';  
end
err = [Perr;Werr];