2024年机器人大作业代码

%% matlab 代码运行较快，但simulink运行极慢，simulink运行结果存放在out.mat中。 % 故建议直接单击运行，代码中已经包含显示simulink结果图像的代码。 %% 设置机器人的MDH参数，使用RBT工具箱建立机器人模型，得到变换矩阵，一系列雅可比矩阵和动力学方程 % load('model1.mat'); clear;clc; % 三个关节角度符号变量 syms q1 q2 q3 dq1 dq2 dq3 % 质量 m1 = 1.5;m2 = 1;m3 = 1; % 三根连杆的长度 l1=0.138;l2=0.135;l3=0.147; % 三个关节的角度偏置 q10 = 0;q20 = -pi/2;q30=pi/2; % 三个关节的角度范围 q1_min=-pi/2;q1_max=pi/2;q2_min=0;q2_max=85/180*pi;q3_min=-10/180*pi;q3_max=pi/2; % 变换矩阵 Robot = make_robot(m1,m2,m3,l1,l2,l3,q1_min,q1_max,q2_min,q2_max,q3_min,q3_max,q10,q20,q30); T01=Robot.A(1,[q1 q2 q3]).T; T12=simplify(Robot.A(2,[q1 q2 q3]).T); T23=simplify(Robot.A(3,[q1 q2 q3]).T); T3H =[1 0 0 l3;0 1 0 0;0 0 1 0;0 0 0 1]; T0H_f = simplify(T01*T12*T23*T3H); % 求逆 T0H_i = simplify(inv(T0H_f)); % 求雅可比矩阵，雅可比矩阵的导数，雅可比矩阵的逆 [J,J_d,J_i] = all_jacob(T0H_f,q1,q2,q3,dq1,dq2,dq3); % MVG->关节空间下的矩阵，M0,Vo,Go->笛卡尔坐标系下的矩阵 [M,V,G,Mo,Vo,Go] = newton_euler(J,J_d,J_i,T01,T12,T23,T3H); % 检验参数值 double(subs(T0H_f,{'q1','q2','q3'},[0 0 0])) % double(subs(V,{'q1','q2','q3','dq1','dq2','dq3'},[1 1 1 1 1 1])) % double(subs(G,{'q1','q2','q3','g'},[1 1 1 9.8])) % double(subs(M,{'q1','q2','q3'},[1 1 1])) % double(subs(J,{'q1','q2','q3'},[1 1 1])) % double(subs(V+G,{'q1','q2','q3','dq1','dq2','dq3','g'},[0 0 0 0 0 0 9.8])) %% 生成函数句柄 Jf = matlabFunction(J);J_df = matlabFunction(J_d);J_if = matlabFunction(J_i); Mf = matlabFunction(Mo);Vf = matlabFunction(Vo);Gf = matlabFunction(Go); T0H_ff = matlabFunction(T0H_f);T0H_if = matlabFunction(T0H_i); %% 画坐标系 Robot.plot([0 0 0]); %初始状态 view(135,35); hold on trplot(double(subs(T01,{'q1','q2','q3'},[0 0 0])),'frame','joint1','arrow','length',0.3,'rgb','width',1,'text_opts', {'FontSize', 10,'FontWeight','normal'}); hold on trplot(double(subs(T01*T12,{'q1','q2','q3'},[0 0 0])),'frame','joint2','arrow','length',0.3,'rgb','width',1,'text_opts', {'FontSize', 10,'FontWeight','normal'}); hold on trplot(double(subs(T01*T12*T23,{'q1','q2','q3'},[0 0 0])),'frame','joint3','arrow','length',0.3,'rgb','width',1,'text_opts', {'FontSize', 10,'FontWeight','normal'}); f = getframe(gcf); imwrite(f.cdata,'axis.png'); %% 通过约束法生成工作空间的点集 N=3000; q1_list=q1_min+(q1_max-q1_min)*rand(1,N); %关节1限制 q2_list = zeros(1,N); q3_list = zeros(1,N); % 由于关节2与关节3之间存在约束，通过for循环寻找合适的随机值 for i=1:N q2_temp = q2_min+(q2_max-q2_min)*rand(1); q3_temp = q3_min+(q3_max-q3_min)*rand(1); while (q2_temp-q3_temp)<(-70/180*pi) || (q2_temp-q3_temp)>(60/180*pi) %检验是否满足约束条件 q2_temp = q2_min+(q2_max-q2_min)*rand(1); q3_temp = q3_min+(q3_max-q3_min)*rand(1); end q2_list(i)=q2_temp; q3_list(i)=q3_temp; end % 保存为sim.mat % save('sim.mat','Jf','J_df','J_if','Mf','Vf','Gf','T0H_ff','T0H_if'); %% 工作空间绘制 % 通过工具箱建立机械臂模型用于画图 figure(2) for i=1:N cartesian_mat = Robot.fkine([q1_list(i),q2_list(i),q3_list(i)]).T; cartesian_x = cartesian_mat(1,4); caresian_y = cartesian_mat(2,4); cartesian_z = cartesian_mat(3,4); plot3(cartesian_x,caresian_y,cartesian_z,'b.','MarkerSize',0.5);%画出落点 hold on % fprintf("i:%d [%f,%f,%f]\n",i,space_x,space_y,space_z); end xlabel('X轴'); ylabel('Y轴'); zlabel('Z轴'); grid on; hold off %% 计算末端执行器最大速度 max_joint_rate = [2*pi*500/60;2*pi*500/60;2*pi*500/60]; dq1_list = max_joint_rate(1)*rand(1,N); %关节1速度 dq2_list = max_joint_rate(2)*rand(1,N); %关节2速度 dq3_list = max_joint_rate(3)*rand(1,N); %关节3速度 max_end_point_rate = [-inf;-inf;-inf]; for i=1:N temp_J = double(subs(J,{'q1','q2','q3'},[q1_list(i),q2_list(i),q3_list(i)])); temp_x_list = temp_J*[q1_list(i);q2_list(i);q3_list(i)]; for j=1:3 temp_max = max(abs(temp_x_list(j,:))); if temp_max>max_end_point_rate(j) max_end_point_rate(j) = temp_max; end end end fprintf("max_end_point_rate: [%f,%f,%f]\n",max_end_point_rate(1),max_end_point_rate(2),max_end_point_rate(3)); %% 计算最大关节加速度——关节速度为0 dq_list = [0;0;0]; ddq_list = [-inf;-inf;-inf]; for i=1:N temp_V = Vf(dq_list(1),dq_list(2),dq_list(3),q2_list(i),q3_list(i)); temp_G = Gf(9.8,q2_list(i),q3_list(i)); temp_M = Mf(q2_list(i),q3_list(i)); temp_ddq_list = temp_M\(0.52-temp_G-temp_V); for j=1:3 temp_max = max(abs(temp_ddq_list(j,:))); if temp_max>ddq_list(j) ddq_list(j) = temp_max; end end end max_joint_acc_theta_0 = ddq_list; fprintf("max_joint_acc(theta=0): [%f,%f,%f]\n",ddq_list(1),ddq_list(2),ddq_list(3)); %% 计算最大关节加速度——关节速度为最大 dq_list = max_joint_rate; ddq_list = [-inf;-inf;-inf]; for i=1:N temp_V = Vf(dq_list(1),dq_list(2),dq_list(3),q2_list(i),q3_list(i)); temp_G = Gf(9.8,q2_list(i),q3_list(i)); temp_M = Mf(q2_list(i),q3_list(i)); temp_ddq_list = temp_M\(0.52-temp_G-temp_V); for j=1:3 temp_max = max(abs(temp_ddq_list(j,:))); if temp_max>ddq_list(j) ddq_list(j) = temp_max; end end end max_joint_acc_theta_max = ddq_list; fprintf("max_joint_acc(theta=max): [%f,%f,%f]\n",ddq_list(1),ddq_list(2),ddq_list(3)); %% 计算末端执行器最大加速度——关节速度为0 dq_list = [0;0;0]; ddx_list = [-inf;-inf;-inf]; for i=1:N tmep_V = Vf(dq_list(1),dq_list(2),dq_list(3),q2_list(i),q3_list(i)); temp_G = Gf(9.8,q2_list(i),q3_list(i)); temp_M = Mf(q2_list(i),q3_list(i)); temp_J = Jf(q1_list(i),q2_list(i),q3_list(i)); temp_J_d = J_df(dq_list(1),dq_list(2),dq_list(3),q1_list(i),q2_list(i),q3_list(i)); temp_ddx_list = temp_J*(temp_M\(0.52-temp_G-temp_V))+temp_J_d*dq_list; for j=1:3 temp_max = max(abs(temp_ddx_list(j,:))); if temp_max>ddx_list(j) ddx_list(j) = temp_max; end end end max_end_acc_theta_0 = ddx_list; fprintf("max_end_acc(theta=0): [%f,%f,%f]\n",ddx_list(1),ddx_list(2),ddx_list(3)); %% 计算末端执行器最大加速度——关节速度为最大 dq_list = max_joint_rate; ddx_list = [-inf;-inf;-inf]; for i=1:N tmep_V = Vf(dq_list(1),dq_list(2),dq_list(3),q2_list(i),q3_list(i)); temp_G = Gf(9.8,q2_list(i),q3_list(i)); temp_M = Mf(q2_list(i),q3_list(i)); temp_J = Jf(q1_list(i),q2_list(i),q3_list(i)); temp_J_d = J_df(dq_list(1),dq_list(2),dq_list(3),q1_list(i),q2_list(i),q3_list(i)); temp_ddx_list = temp_J*(temp_M\(0.52-temp_G-temp_V))+temp_J_d*dq_list; for j=1:3 temp_max = max(abs(temp_ddx_list(j,:))); if temp_max>ddx_list(j) ddx_list(j) = temp_max; end end end max_end_acc_theta_max = ddx_list; fprintf("max_end_acc(theta=max): [%f,%f,%f]\n",ddx_list(1),ddx_list(2),ddx_list(3)); %% 路径生成 start_pt = double(subs(T0H_f,{'q1','q2','q3'},[0 0 0])); % 10ms一周期 500ms一段 一共四段 即仿真时间一共2s step = 51; % 设置坐标 x_list = [start_pt(1,4),start_pt(1,4)+0.030,start_pt(1,4)+0.120,start_pt(1,4)+0.085,start_pt(1,4)]; y_list = [start_pt(2,4),start_pt(2,4)+0.050,start_pt(2,4),start_pt(2,4)-0.085,start_pt(2,4)]; z_list = [start_pt(3,4),start_pt(3,4)-0.060,start_pt(3,4)-0.120,start_pt(3,4)-0.060,start_pt(3,4)]; x_target = [];dx_target = [];ddx_target = []; for i=1:length(x_list)-1 [xs,xsd,xsdd] = lspb(x_list(i), x_list(i+1), step); [ys,ysd,ysdd] = lspb(y_list(i), y_list(i+1), step); [zs,zsd,zsdd] = lspb(z_list(i), z_list(i+1), step); x_target = [x_target;xs,ys,zs]; dx_target = [dx_target;xsd,ysd,zsd]; ddx_target = [ddx_target;xsdd,ysdd,zsdd]; end % 生成simulink需要的带