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

介绍了python库的安装及其链接

https://blog.csdn.net/weixin_41826637/article/details/80966836

不懂的matlab函数

函数	用法
inv	矩阵求逆---------inv(A)*b=A\b
pause	pause(n)----暂停n秒 pause;-----暂停等待按键触发

使用matlab基本函数进行机器人建模仿真

Build_6DOF_Robot_DH.m

// A highlighted block
%% 建立机器人DH参数表

UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';

Link=struct('name','Body','theta',0,'di',0,'ai',0,'alpha',0,'az',UZ);
Link(1)=struct('name','Base','theta',0,'di',0,'ai',0,'alpha',0,'az',UZ);
Link(2)=struct('name','J1','theta',pi/2,'di',100,'ai',0,'alpha',-pi/2,'az',UZ);
Link(3)=struct('name','J2','theta',-pi/2,'di',0,'ai',0,'alpha',-pi/2,'az',UZ);
Link(4)=struct('name','J3','theta',0,'di',80,'ai',0,'alpha',0,'az',UZ);
Link(5)=struct('name','J4','theta',0,'di',80,'ai',0,'alpha',pi/2,'az',UZ);
Link(6)=struct('name','J5','theta',0,'di',0,'ai',0,'alpha',-pi/2,'az',UZ);
Link(7)=struct('name','J6','theta',pi/2,'di',80,'ai',0,'alpha',pi/2,'az',UZ);
Link(8)=struct('name','J7','theta',0,'di',50,'ai',0,'alpha',0,'az',UZ);
Link(9)=struct('name','J8','theta',0,'di',-100,'ai',0,'alpha',0,'az',UZ);

Build_DH_Matrix.m

function Build_DH_Matrix(JointNum)
%% 输入关节数量，生成齐次变换矩阵A、旋转矩阵R
global Link



for i=1:JointNum+3
    %获取关节参数
    Cth=cos(Link(i).theta);
    Sth=sin(Link(i).theta);
    Ca=cos(Link(i).alpha);
    Sa=sin(Link(i).alpha);
    ai=Link(i).ai;
    di=Link(i).di;
 
    
    %添加关节变换信息
    Link(i).x=[Cth       Sth        0     0]';
    Link(i).y=[-Sth*Ca   Cth*Ca     Sa    0]';
    Link(i).z=[Sth*Sa    -Cth*Sa    Ca    0]';
    Link(i).p=[ai*Cth    ai*Sth     di    1]';
    
    Link(i).R=[Link(i).x(1:3),Link(i).y(1:3),Link(i).z(1:3)];
    Link(i).A=[Link(i).x,Link(i).y, Link(i).z, Link(i).p];
end

Connect3D.m

function Connect3D(p1,p2,option,pt)

h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)

DH_FK_6DOF.m

function pic=DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,th7,th8,fcla)

global Link
ToDeg = 180/pi;
ToRad = pi/180;

Build_6DOF_Robot_DH;    %构建DH表

% 获取关节变量
Link(2).theta=th1*ToRad;
Link(3).theta=th2*ToRad;
Link(4).di=80+dz3;
Link(5).theta=th4*ToRad;
Link(6).theta=th5*ToRad;
Link(7).theta=th6*ToRad;
Link(8).theta=th7*ToRad;
Link(9).theta=th8*ToRad;


Build_DH_Matrix(6);     %计算齐次变换矩阵



radius    = 10;         %初始化
len       = 30;
joint_col = 0;
plot3(0,0,0,'ro'); 


% 绘制连杆及其关节
for i=2:9
    Link(i).A =Link(i-1).A*Link(i).A;
    Link(i).x= Link(i).A(1:3,1);
    Link(i).y= Link(i).A(1:3,2);
    Link(i).z= Link(i).A(1:3,3);
    Link(i).p= Link(i).A(1:3,4);
    Link(i).R=[Link(i).x,Link(i).y,Link(i).z];
    Connect3D(Link(i-1).p,Link(i).p,'b',3); 
    plot3(Link(i).p(1),Link(i).p(2),Link(i).p(3),'rx');
    hold on;
    if i<=7
         DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
    end
end

% 设置坐标系信息
grid on;
%view(129,28);
axis([-400,400,-400,400,-400,400]);
xlabel('x');
ylabel('y');
zlabel('z');

%生成动画帧
drawnow;
pic=getframe;

% 判断是否清空坐标系
if(fcla)
    cla;
end

DH_IK_6DOF.m

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

    global Link
    
    ToDeg = 180/pi;
    ToRad = pi/180;
    
    Xc=Target.p(1)-80*Target.y(1);
    Yc=Target.p(2)-80*Target.y(2);
    Zc=Target.p(3)-80*Target.y(3);
    th1=atan2(Yc,Xc);
    th2=-atan2(Zc-100,sqrt((Xc)^2+(Yc)^2))-pi/2;
    dz3=sqrt((Xc)^2+(Yc)^2+(Zc-100)^2)-160;
    
	th4=atan2((sin(th1)*Target.y(1)-cos(th1)*Target.y(2)),cos(th1)*cos(th2)*Target.y(1)+sin(th1)*cos(th2)*Target.y(2)-sin(th2)*Target.y(3))+pi;
    th5=atan2((sqrt(1-(-cos(th1)*sin(th2)*Target.y(1)-sin(th1)*sin(th2)*Target.y(2)-cos(th2)*Target.y(3))^2)),(-cos(th1)*sin(th2)*Target.y(1)-sin(th1)*sin(th2)*Target.y(2)-cos(th2)*Target.y(3)));   
    th6=-atan2((-cos(th1)*sin(th2)*Target.x(1)-sin(th1)*sin(th2)*Target.x(2)-cos(th2)*Target.x(3)),(-cos(th1)*sin(th2)*Target.z(1)-sin(th1)*sin(th2)*Target.z(2)-cos(th2)*Target.z(3)))+pi/2;

    th1=th1*ToDeg;
    th2=th2*ToDeg;
    th4=th4*ToDeg;
    th5=th5*ToDeg;
    th6=th6*ToDeg;
    

Draw_6DOF_Workplace.m

close all;
clc;
clear;

global Link
ToDeg = 180/pi;
ToRad = pi/180;

num=1;
th_interval = 30;
d_interval = 10;

for th1=-180:th_interval:180
    for th2=-150:th_interval:150
        for dz3=0:d_interval:100
            for th4=-150:th_interval:150
                for th5=-150:th_interval:150
                    for th6=-150:th_interval:150
                        theta1=th1*ToRad;
                        theta2=th2*ToRad;
                        di3=dz3;
                        theta4=th4*ToRad;
                        theta5=th5*ToRad;
                        theta6=th6*ToRad;
                        A1=[cos(theta1) 0 -sin(theta1) 0;
                            sin(theta1) 0 cos(theta1)  0;
                            0 -1 0 100;
                            0 0 0 1];
                        A2=[cos(theta2) 0 -sin(theta2) 0;
                            sin(theta2) 0 cos(theta2) 0;
                            0 -1 0 0;
                            0 0 0 1];
                        A3=[1 0 0 0;
                            0 1 0 0;
                            0 0 1 dz3;
                            0 0 0 1];
                        A4=[cos(theta4) 0 sin(theta4) 0;
                            sin(theta4) 0 -cos(theta4) 0;
                            0 1 0 80;
                            0 0 0 1];
                        A5=[cos(theta5) 0 -sin(theta5) 0;
                            sin(theta5) 0 cos(theta5) 0;
                            0 -1 0 0;
                            0 0 0 1];
                        A6=[cos(theta6) 0 sin(theta6) 0;
                            sin(theta6) 0 -cos(theta6) 0;
                            0 1 0 80;
                            0 0 0 1];
                                    
                        A = A1 * A2 * A3 * A4 * A5 * A6;                    
                        point1(num) = A(1,4);
                        point2(num) = A(2,4);
                        point3(num) = A(3,4);
                        num = num + 1;  
                    end
                end
            end
        end
    end
end
plot3(point1,point2,point3,'r*');
axis([-500,500,-500,500,-500,500]);
xlabel('x');
ylabel('y');
zlabel('z');
 %view(-90,0);
 %view(0,90);
grid on;

DrawCylinder.m

function h = DrawCylinder(pos, az, radius,len, col)
% draw closed cylinder
%
%******** rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps 
	rot = eye(3);
else
    ax = ax/ax_n;
    ay = cross(az,ax);
    ay = ay/norm(ay);
    rot = [ax ay az];
end

%********** make cylinder
% col = [0 0.5 0];  % cylinder color

a = 20;    % number of side faces
theta = (0:a)/a * 2*pi;

x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));

for n=1:size(x,1)
   xyz = [x(n,:);y(n,:);z(n,:)];
   xyz2 = rot * xyz;
   x2(n,:) = xyz2(1,:);
   y2(n,:) = xyz2(2,:);
   z2(n,:) = xyz2(3,:);
end

%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);

for n=1:2
	patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
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(1:3);
    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 theta==0
%         Werr=[0,0,0]';
%     else
%         Werr = theta/(2.0*sin(theta)) * [Rerr(3,2)-Rerr(2,3);Rerr(1,3)-Rerr(3,1);Rerr(2,1)-Rerr(1,2)];
%         Werr = Current.R * Werr;
%     end
if Rerr~=a
    Werr=theta/(2*sin(theta))*b;
else
    Werr=[0 0 0]';  
end
err = [Perr;Werr];

MOV_6DOF_Robot.m

%% 初始化
close all;
clear;
clc;

ToDeg = 180/pi;
ToRad = pi/180;
step=20;
th1=90;th2=-90;dz3=0;th4=0;th5=0;th6=90;

%view(-90,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0); 
pause; 
% 关节1
for i=0:step:180
   DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,1); 
end
for i=180:-step:0
    if i==0
     DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,0);  
    else
     DH_FK_6DOF(th1+i,th2,dz3,th4,th5,th6,0,0,1); 
    end
end
%% 关节2
for i=0:step:180
   DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,1); 
end
for i=180:-step:0
    if i==0
     DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,0);  
    else
     DH_FK_6DOF(th1,th2+i,dz3,th4,th5,th6,0,0,1); 
    end
end
%% 关节3
for i=0:10:100
   DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,1); 
end
for i=100:-10:0
    if i==0
     DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,0);  
    else
     DH_FK_6DOF(th1,th2,dz3+i,th4,th5,th6,0,0,1); 
    end
end
%% 关节4
for i=0:step:180
   DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,1); 
end
for i=180:-step:0
    if i==0
     DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,0);  
    else
     DH_FK_6DOF(th1,th2,dz3,th4+i,th5,th6,0,0,1); 
    end
end
%% 关节5
for i=0:step:180
   DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,1); 
end
for i=180:-step:0
    if i==0
     DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,0);  
    else
     DH_FK_6DOF(th1,th2,dz3,th4,th5+i,th6,0,0,1); 
    end
end
% 关节6
for i=0:step:180
   DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,1); 
end
for i=180:-step:0
    if i==0
     DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,0);  
    else
     DH_FK_6DOF(th1,th2,dz3,th4,th5,th6+i,0,0,1); 
    end
end

Restrain_Value.m

function x = Restrain_Value(x,min,max)
    if x<min
        x=min;
    elseif x>max
        x=max;
    else
        x=x;
    end

Build_6DOF_Jacobian.m

function J=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6)
%% 输入关节变量，输出jacobian
global Link

ToDeg = 180/pi;
ToRad = pi/180;
JointNum=6;
J=zeros(6,6);

Link(2).theta=th1*ToRad;
Link(3).theta=th2*ToRad;
Link(4).di=80+dz3;
Link(5).theta=th4*ToRad;
Link(6).theta=th5*ToRad;
Link(7).theta=th6*ToRad;

Build_DH_Matrix(JointNum);
Link(1).p=Link(1).p(1:3);
for i=2:9
    Link(i).A =Link(i-1).A*Link(i).A;
    Link(i).x= Link(i).A(1:3,1);
    Link(i).y= Link(i).A(1:3,2);
    Link(i).z= Link(i).A(1:3,3);
    Link(i).p= Link(i).A(1:3,4);
    Link(i).R=[Link(i).x,Link(i).y,Link(i).z];
end


for i=2:7
    if i==4
        J(:,i-1)=[Link(i-1).z(1:3);[0 0 0]'];
    else
        J(:,i-1)=[cross(Link(i-1).z(1:3),(Link(7).p(1:3)-Link(i-1).p(1:3)));Link(i-1).z(1:3)];
    end
end

% for i=1:6
%     for j=1:6
%          fprintf('%2.2f  ,',J(i,j));
%     end
%     fprintf('\n');
% end

Draw_6DOF.m

close all;
clc;
clear;

global Link

ToDeg = 180/pi;
ToRad = pi/180;
th1=130;
th2=-60;
dz3=10;
th4=-120;
th5=72;
th6=110; 
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);



 %view(-90,0);
% pause;
% view(0,90);




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)';


num=1;
%% 解析解运动
pause;
for i=1:20
    Target.p(3)=Target.p(3)+i;
[th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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);


%% 微分运动
pause;
j=20;%z轴方向上的移动总步数
step=-1;%步长
for i=1:j
    J1=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6);
     [U,S,V]=svd(J1);
     J=V*(inv(S))*(U');
    D=[0 0 step*i 0 0 0]';
    dth=J*D;
    th1=th1+dth(1)*ToDeg;
    th2=th2+dth(2)*ToDeg;
    dz3=dz3+dth(3);
    th4=th4+dth(4)*ToDeg;
    th5=th5+dth(5)*ToDeg;
    th6=th6+dth(6)*ToDeg;
    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);

%% 写郑----直接描
pause;
num=1;
step2=1;
cen_point=[-50 0 -50]';
x=[];
y=[];
z=[];
for i=5:step2*0.7:15
    Target.p=[i 150 -8*i+260]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=27:-step2:17
    Target.p=[i 150 8*i+8]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=0:2*step2:31
    Target.p=[i 150 140]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=-2:2*step2:36
    Target.p=[i 150 80]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=16:-step2*0.5:4
    Target.p=[i 150 (i-6)*40/3]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=16:step2:32
    Target.p=[i 150 -40/6*(i-16)+80]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=140:-step2*7:-50
    Target.p=[50 150 i]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=50:step2*2:80
    Target.p=[i 150 140]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=80:-step2:60
    Target.p=[i 150 3*(i-40)+20]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=60:step2:90
    Target.p=[i 150 -2*(i-40)+120]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=90:-step2*2:65
    Target.p=[i 150 0.5*i-25]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
for i=65:-step2:57 
    Target.p=[i 150 -2*i+140]'+cen_point;
    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(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
view(0,0);
%% 写郑---图片形式
pause;
view(150,15);
num=1;
load('FP.mat');
[FPh,FPl]=size(FP);
x=[];
y=[];
z=[];
for i=1:FPh; 
    Target.p(1)=FP(i,2);
    Target.p(2)=150;
    Target.p(3)=FP(i,1);

    [th1,th2,dz3,th4,th5,th6]=DH_IK_6DOF(Target);
    
    x(num)=Link(7).p(1);
    y(num)=Link(7).p(2);
    z(num)=Link(7).p(3);
    num=num+1;
    plot3(x,y,z,'r.');hold on;
    if i==FPh
        DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
    else
        DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,1);
    end
end
view(180,0);


%% 牛顿拉夫逊迭代法
% pause;

% th1=100;
% th2=-60;
% dz3=10;
% th4=-100;
% th5=72;
% th6=10; 
% DH_FK_6DOF(th1,th2,dz3,th4,th5,th6,0,0,0);
% [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,0);

Word.m

clear,clc
A=imread('郑.jpg');
B=rgb2gray(A);
g_max=double(max(max(B)));
g_min=double(min(min(B)));
T=round((g_max-g_min)/2);
D=(double(B)>=T);
[Dm,Dn]=size(D);

num=1;
for m=1:Dm
    if mod(m,2)==0
        n=Dn;
         for ii=1:Dn
            if D(m,n)==0
                FP(num,1)=(Dm-m)-200;%z轴坐标
                FP(num,2)=(Dn-n)-200;%x轴坐标
                num=num+1;
            end
            n=n-1;
         end
    else
        n=1;
        for ii=1:Dn
            if D(m,n)==0
                FP(num,1)=(Dm-m)-200;
                FP(num,2)=(Dn-n)-200;
                num=num+1;
            end
            n=n+1;
        end
    end
end

save('FP.mat','FP');

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;
    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=J*D;

        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,0,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;
    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