之前用牛顿下山法进行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];