课程设计基于matlab机械臂末端轨迹规划的源码.zip

% % 当给定一个轨迹时，计算出机械臂末端执行器在关节角速度和角加速度约束下沿着该轨迹的最快速度 % % clear all; % close all; % my_robot % % t = 10; % r = 0.3; % % % % T1 = transl(2,1,2) * rpy2tr(pi,0,0); % % T2 = transl(3,-1,1) * rpy2tr(0,0,0); % T1 = transl(4, 1, 1) * rpy2tr(0,pi/2,pi/2,'xyz'); % T2 = transl(2, -2, 0) * rpy2tr(0,pi/2,pi/2,'xyz'); % T3 = transl(4, -1, 1) * rpy2tr(0,pi/2,pi/2,'xyz'); % T1s = transl(3,-2,2) * rpy2tr(0,0,pi/3,'xyz'); % T2s = transl(3,0,1) * rpy2tr(0,0,pi,'xyz'); % T3s = transl(2,2,0.5) * rpy2tr(pi/6,pi/4,pi,'xyz'); % % T1 = transl(3,0,0.5) * rpy2tr(0,0,pi,'xyz'); % T2 = transl(3,-2,0) * rpy2tr(0,0,pi,'xyz'); % T3 = transl(1,-2,1) * rpy2tr(0,0,pi,'xyz'); % T4 = transl(1,2,0) * rpy2tr(0,0,pi,'xyz'); % T5 = transl(3,2,1) * rpy2tr(0,0,pi,'xyz'); % T6 = transl(3,0,0.5) * rpy2tr(0,0,pi,'xyz'); % % T = cat(3,T1,T2,T3,T4,T5,T6); % T = cat(3,T1s,T2s,T3s); % flag_dq = 1; % flag_ddq = 1; % % cnt = 0; % ddqlim = ddqlimo*0.998; % dqlim = dqlimo*0.998; % step = t/1000; % % [trans,voli,a,V,A] = Cartesian_circle_traj_3p_poly(T4,T5,T6,t,r,step); % [trans,voli,a,V,A] = Cartesian_traj_poly_five(T1s,T3s,t,0.3,step); % % [trans,voli,a,V,A] = Cartesian_traj_multi_points_arc_transition_modify(jixiebi,T,0.4,t,[r r],step); % % [trans,voli,a,V,A] = Cartesian_traj_multi_points_bezier_curve_transiation(T,1.2,t,[r r],6,step); % % [trans,voli,a,V,A] = Cartesian_traj_poly_three(T1,T2,t,r,step); % % [trans,voli,a,V,A] = Cartesian_traj_sin(T1,T2,t,r,step); % % [trans,voli,a,V,A] = Cartesian_traj_ladder(T1,T2,t,r,step); % % [trans,~,~,V,A] = Cartesian_traj_sigmoid(T1,T2,T,0.2,7); % angle = Inverse_kinematic(jixiebi,trans); % % 速度级运动学求角速度 % for i = 1:length(V) % d_angle(i,:) = (jacob_matrix(jixiebi,angle(i,:))^-1*V(i,:)')'; % end % % % 加速度级运动学求角加速度 % JM_dot = jacob_derivative(jixiebi,angle,d_angle); % for i = 1:length(V) % JM = jacob_matrix(jixiebi,angle(i,:)); % dd_angle(i,:) = (JM^-1*(A(i,:)-(JM_dot(:,:,i)*d_angle(i,:)')')')'; % % end % % max_dangle = max(abs(d_angle)); % max_ddangle = max(abs(dd_angle)); % dist_dangle = max_dangle - dqlim; % dist_ddangle = max_ddangle - ddqlim; % x = max(dist_ddangle./ddqlim); % x1 = x; % dx = 0; % % kp = 0.5; % while flag_dq || flag_ddq % step = t/1000; % % [trans,voli,a,V,A] = Cartesian_circle_traj_3p_poly(T4,T5,T6,t,r,step); % [trans,voli,a,V,A] = Cartesian_traj_poly_five(T1s,T3s,t,0.1,step); % % [trans,voli,a,V,A] = Cartesian_traj_multi_points_arc_transition_modify(jixiebi,T,0.4,t,[r r],step); % % [trans,voli,a,V,A] = Cartesian_traj_multi_points_bezier_curve_transiation(T,1.2,t,[r r],6,step); % % [trans,voli,a,V,A] = Cartesian_traj_poly_three(T1,T2,t,r,step); % % [trans,voli,a,V,A] = Cartesian_traj_sin(T1,T2,t,r,step); % % [trans,voli,a,V,A] = Cartesian_traj_ladder(T1,T2,t,r,step); % % [trans,~,~,V,A] = Cartesian_traj_sigmoid(T1,T2,T,0.2,7); % angle = Inverse_kinematic(jixiebi,trans); % % 速度级运动学求角速度 % for i = 1:length(V) % d_angle(i,:) = (jacob_matrix(jixiebi,angle(i,:))^-1*V(i,:)')'; % end % % % 加速度级运动学求角加速度 % JM_dot = jacob_derivative(jixiebi,angle,d_angle); % for i = 1:length(V) % JM = jacob_matrix(jixiebi,angle(i,:)); % dd_angle(i,:) = (JM^-1*(A(i,:)-(JM_dot(:,:,i)*d_angle(i,:)')')')'; % % end % % max_dangle = max(abs(d_angle)); % max_ddangle = max(abs(dd_angle)); % dist_dangle = max_dangle - dqlim; % dist_ddangle = max_ddangle - ddqlim; % ts = [t t t t t t]; % fisMat = readfis('two_order_kp_fuzzy_control.fis'); % x = max(dist_ddangle./ddqlim); % x2 = x; % dx = x2-x1; % sp = evalfis([max(dist_ddangle./ddqlim) dx],fisMat); % kp = 0.5+sp*0.5; % if kp > 0.9 % kp = 0.9; % elseif kp <0.3 % kp = 0.3; % end % ts = ts + kp*ts.*dist_ddangle./ddqlim; % t = max(ts); % x1 = x2; % cnt = cnt + 1; % trace(cnt) = t; % disp(num2str(t)) % fdist_dangle = ~isempty(find(dist_dangle<0, 1)); % fdist_ddangle = ~isempty(find(dist_ddangle<0, 1)); % % x = find(abs(dist) == min(abs(dist))); % frdist = ~isempty(find(abs((max_ddangle-ddqlim)./ddqlim)<2e-3, 1)); % if fdist_dangle && fdist_ddangle && frdist % flag_dq = 0; % flag_ddq = 0; % end % % end % % 当给定一个轨迹时，计算出机械臂末端执行器在关节角速度和角加速度约束下沿着该轨迹的最快速度 clear all; close all; my_robot t = 10; r = 0.3; step = t/1000; % % T1 = transl(2,1,2) * rpy2tr(pi,0,0); % T2 = transl(3,-1,1) * rpy2tr(0,0,0); T1 = transl(4, 1, 1) * rpy2tr(0,pi/2,pi/2,'xyz'); T2 = transl(2, -2, 0) * rpy2tr(0,pi/2,pi/2,'xyz'); T3 = transl(4, -1, 1) * rpy2tr(0,pi/2,pi/2,'xyz'); T1s = transl(3,-2,2) * rpy2tr(0,0,pi/3,'xyz'); T2s = transl(3,0,1) * rpy2tr(0,0,pi,'xyz'); T3s = transl(2,2,0.5) * rpy2tr(pi/6,pi/4,pi,'xyz'); T4s = transl(2,1,0) * rpy2tr(0,0,pi,'xyz'); T5s = transl(2,0,1) * rpy2tr(0,0,pi,'xyz'); T1 = transl(3,0,0.5) * rpy2tr(0,0,pi,'xyz'); T2 = transl(3,-2,0) * rpy2tr(0,0,pi,'xyz'); T3 = transl(1,-2,1) * rpy2tr(0,0,pi,'xyz'); T4 = transl(1,2,0) * rpy2tr(0,0,pi,'xyz'); T5 = transl(3,2,1) * rpy2tr(0,0,pi,'xyz'); T6 = transl(3,0,0.5) * rpy2tr(0,0,pi,'xyz'); % T = cat(3,T1,T2,T3,T4,T5,T6); T = cat(3,T1s,T2s,T3s,T4s,T5s); flag_dq = 1; flag_ddq = 1; cnt = 0; dqlim = dqlimo*0.998; ddqlim = ddqlimo*0.998; [trans,voli,a,V,A] = Cartesian_traj_poly_five(T1s,T3s,t,0.3,step); angle = Inverse_kinematic(jixiebi,trans); % 速度级运动学求角速度 for i = 1:length(V) d_angle(i,:) = (jacob_matrix(jixiebi,angle(i,:))^-1*V(i,:)')'; end % 加速度级运动学求角加速度 JM_dot = jacob_derivative(jixiebi,angle,d_angle); for i = 1:length(V) JM = jacob_matrix(jixiebi,angle(i,:)); dd_angle(i,:) = (JM^-1*(A(i,:)-(JM_dot(:,:,i)*d_angle(i,:)')')')'; end max_dangle = max(abs(d_angle)); max_ddangle = max(abs(dd_angle)); dist_dangle = max_dangle - dqlim; dist_ddangle = max_ddangle - ddqlim; x1 = max(dist_ddangle./ddqlim); dx = 0; ts1 = [t t t t t t t t t t t t]; fisMat = readfis('two_order_kp_fuzzy_control.fis'); while flag_dq || flag_ddq step = t/1000; % [trans,voli,a,V,A] = Cartesian_traj_free_curve_bezier(T,t,r,6,step); % [trans,voli,a,V,A] = Cartesian_circle_traj_3p_poly(T4,T5,T6,t,r,step); [trans,voli,a,V,A] = Cartesian_traj_poly_five(T1s,T3s,t,0.3,step); % [trans,voli,a,V,A] = Cartesian_traj_multi_points_arc_transition_modify(jixiebi,T,0.4,t,[r r],step); % [trans,voli,a,V,A] = Cartesian_traj_multi_points_bezier_curve_transiation(T,1.2,t,[r r],6,step); % [trans,voli,a,V,A] = Cartesian_traj_poly_three(T1,T2,t,r,step); % [trans,voli,a,V,A] = Cartesian_traj_sin(T1,T2,t,r,step); % [trans,voli,a,V,A] = Cartesian_traj_ladder(T1,T2,t,r,step); % [trans,~,~,V,A] = Cartesian_traj_sigmoid(T1,T2,T,0.2,7); angle = Inverse_kinematic(jixiebi,trans); % 速度级运动学求角速度 for i = 1:length(V) d_angle(i,:) = (jacob_matrix(jixiebi,angle(i,:))^-1*V(i,:)')'; end % 加速度级运动学求角加速度 JM_dot = jacob_derivative(jixiebi,angle,d_angle); for i = 1:length(V) JM = jacob_matrix(jixiebi,angle(i,:)); dd_angle(i,:) = (JM^-1*(A(i,:)-(JM_dot(:,:,i)*d_angle(i,:)')')')'; end max_dangle = max(abs(d_angle)); max_ddangle = max(abs(dd_angle)); dist_dangle = max_dangle - dqlim; dist_ddangle = max_ddangle - ddqlim; cnt = cnt + 1; trace(cnt) = t; disp(num2str(t)) fdist_dangle = isempty(find(max_dangle - dqlimo>0, 6)); fdist_ddangle = isempty(find(max_ddangle - ddqlimo>0, 6)); % x = find(abs(dist) == min(abs(dist))); frdist1 = ~isempty(find(abs((max_dangle-ddqlim)./dqlim)<2e-3, 1)); frdist2 = ~isempty(find(abs((max_ddangl