控制科学与工程：随手笔记（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<eps
        break
    end
    E_tmp = E;
    lamada=1;
    while E-E_tmp<=0 %新产生的误差小于或等于原来误差
        fprintf('%ld ',E_tmp);
        [U,S,V]=svd(J1);
        J=V*(inv(S))*(U');
%         dT=Target.A-Link(7).A;
%         DER=dT/Link(7).A;
%         D=[DER(1,4) DER(2,4) DER(3,4) DER(3,2) DER(1,3) DER(2,1)]';
        dth=lamada*J*err;

        th1_tmp=th1+dth(1)*ToDeg;
        th2_tmp=th2+dth(2)*ToDeg;
        dz3_tmp=dz3+dth(3);
        th4_tmp=th4+dth(4)*ToDeg;
        th5_tmp=th5+dth(5)*ToDeg;
        th6_tmp=th6+dth(6)*ToDeg;
        
        %判断是否在工作范围内（此处被我注释掉了，因为我运行的点是随机选择的，不能保证在其工作角度范围内）
%         th1_tmp=Restrain_Value(th1_tmp,-180,180);
%         th2_tmp=Restrain_Value(th2_tmp,-150,150);
%         dz3_tmp=Restrain_Value(dz3_tmp,-100,100);
%         th4_tmp=Restrain_Value(th4_tmp,-150,150);
%         th5_tmp=Restrain_Value(th5_tmp,-150,150);
%         th6_tmp=Restrain_Value(th6_tmp,-150,150);
        
        %DH_FK_6DOF(th1_tmp,th2_tmp,dz3_tmp,th4_tmp,th5_tmp,th6_tmp,0,0,1);
        J1=Build_6DOF_Jacobian(th1_tmp,th2_tmp,dz3_tmp,th4_tmp,th5_tmp,th6_tmp);
        err = Err_6DOF(Target, Link(7));
        E_tmp = err'*err;
        lamada=lamada/2.0;
    end
    
    th1=th1_tmp;
    th2=th2_tmp;
    dz3=dz3_tmp;
    th4=th4_tmp;
    th5=th5_tmp;
    th6=th6_tmp;
    
    
    if E_tmp<eps
        break
    end
    
    num=num+1;
end
end

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];