控制科学与工程(随手笔记(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基本函数进行机器人建模仿真 控制科学与工程(随手笔记(2)--工业机器人仿真、matlab)
文章图片

控制科学与工程(随手笔记(2)--工业机器人仿真、matlab)
文章图片

控制科学与工程(随手笔记(2)--工业机器人仿真、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 Linkfor 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=[CthSth00]'; Link(i).y=[-Sth*CaCth*CaSa0]'; Link(i).z=[Sth*Sa-Cth*SaCa0]'; Link(i).p=[ai*Cthai*Sthdi1]'; 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 LinkToDeg = 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 colora = 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 xmax x=max; else x=x; end

Build_6DOF_Jacobian.m
function J=Build_6DOF_Jacobian(th1,th2,dz3,th4,th5,th6) %% 输入关节变量,输出jacobian global LinkToDeg = 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]; endfor 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 LinkToDeg = 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; endDH_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 endsave('FP.mat','FP');

【控制科学与工程(随手笔记(2)--工业机器人仿真、matlab)】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

    推荐阅读