circle_轨迹规划_机械臂圆弧_model7gs_机械臂圆弧轨迹规划_matlabcircle

clear; clc; %建立机器人模型 % theta d a alpha offset L1=Link([0 0.4 0.025 pi/2 0 ],'standard'); %定义连杆的D-H参数 L2=Link([pi/2 0 0.56 0 0 ],'standard'); L3=Link([0 0 0.035 pi/2 0 ],'standard'); L4=Link([0 0.515 0 pi/2 0 ],'standard'); L5=Link([pi 0 0 pi/2 0 ],'standard'); L6=Link([0 0.08 0 0 0 ],'standard'); robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','UR'); %连接连杆，机器人取名manman T1=transl(0.5,0.5,0);%根据给定起始点，得到起始点位姿 T2=transl(0,0.5,0);%根据给定终止点，得到终止点位姿 q1=robot.ikine(T1);%根据起始点位姿，得到起始点关节角 q2=robot.ikine(T2);%根据终止点位姿，得到终止点关节角 t=(0:0.1:5);%产生时间向量 %[q ,qd, qdd]=jtraj(q1,q2,t); %五次多项式圆弧轨迹，得到关节角度，角速度，角加速度，t为采样点个数 % q0=q1; q1=q2; tv=t; if length(tv) > 1 tscal = max(tv); t = tv(:)/tscal; else tscal = 1; t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1 end q0 = q0(:); q1 = q1(:); qd0 = zeros(size(q0)); qd1 = qd0; A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal; B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal; C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal; E = qd0*tscal; % as the t vector has been normalized F = q0; tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))]; c = [A B C zeros(size(A)) E F]'; q = tt*c; c = [ zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C zeros(size(A))]'; qdd = tt*c/tscal^2; %[q ,qd, qdd]=jtraj(q1,q2,t); T=robot.fkine(q);%根据插值，得到末端执行器位姿 Tj=transl(T); plot3(Tj(:,1),Tj(:,2),Tj(:,3));%输出末端轨迹 grid on axis([-1 1 -1 1 -1 1]); hold on N=5; i=0; while i<=N robot.plot(q);%动画演示 i=i+1; end; clc;