之前用牛顿下山法进行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 LinkToDeg = 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【控制科学与工程（随手笔记（4）--工业机器人仿真）】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];
