三自由度并联机器人工作空间求解

%---------------3-DOF PKM WORKSPACE----------------------------- % % This program serves to figure out the workspace of a kind % of PKM with 3 RPRS leg based of a simple discretization % method % %--------------------------------------------------------------- %% CCC clear clc close all %% parameter intialization %input angles theta = 0; psi = 0; phi =0; r0= 492; %(O-Pi0) distance alpha1= 0*pi/180; %orientation of the spherical joint in horizontal plane (around Z-Axis) alpha2= 60*pi/180; %orientation of the spherical joint in vertical plane theta1=pi/4; %rail tilting angle %legs length %end; l2=[60,60,60].'; l3=[644,644,644].'; Lmin =[100 100 100]; %actuators stroke Lmax =ones(1,3)*sqrt(120^2+300^2); l1max=350; L = zeros(1,3); %initialize leg lengths vector beta =180*pi/180; %range of the platform spherical joints %centers of base joints (in the base reference frame) p10= [r0/2; -r0*sqrt(3)/2; 0]; %triangel distribution p20= [r0/2; r0*sqrt(3)/2; 0]; p30= [-r0; 0; 0]; P0= [p10 p20 p30]; %centers of mobile platform joints (in the mobile frame) b= 168; % P15-P25 distance (platform triangle edge length) b1= [b*sqrt(3)/6; -b/2; 0]; b2= [b*sqrt(3)/6; b/2; 0]; b3= [-b*sqrt(3)/3; 0; 0]; B= [b1 b2 b3]; %orientations of spherical joints on the mobile platform jBp = [cos(alpha2)*cos(alpha1), cos(alpha2)*sin(alpha1), -cos(alpha2)*cos(alpha1); ... -cos(alpha2)*sin(alpha1), cos(alpha2)*cos(alpha1), cos(alpha2)*sin(alpha1); ... -sin(alpha2) , -sin(alpha2) , -sin(alpha2)]; R = Rot_mat2(phi, theta, psi); %orientation of the mobile platform %% WSP discretization %COMPUTE THE CONSTANT ORIENTATION WORKSPACE nb_values_eta = 91; nb_values_gamma = 61; nb_val = [nb_values_eta; nb_values_gamma]; WSP = zeros(nb_values_eta,nb_values_gamma); %Initialize the workspace boundary matrix. Cv = [0; 0; 620]; %origin of spherical coordinate system eta = 0; CHECK = 0; rho_incr = 20; epsilon = 4; gamma_incr = 180/(nb_values_gamma-1); eta_incr = 360/(nb_values_eta-1); wh = waitbar(0,'Computation in process...'); %% WSP search while (eta <= 360), gamma = 0; ce = cos(eta*pi/180); se = sin(eta*pi/180); while (gamma <= 180), CHECK = 0; rho = 0; cg = cos(gamma*pi/180); sg = sin(gamma*pi/180); orient_search_vect = [sg*ce; sg*se; cg];%spherical coordinates while ~CHECK, C = Cv+rho*orient_search_vect; %position of the mobile platform ConfigCheck; %check the current configuration rho = rho+rho_incr; end delta_rho = rho/2; while (delta_rho > epsilon/2), %Workspace Boundary Algorithm C = Cv+rho*orient_search_vect; %position of the search vector ConfigCheck; %check the current configuration if ~CHECK, rho = rho+delta_rho; else rho = rho-delta_rho; end delta_rho = delta_rho/2; end C = Cv+rho*orient_search_vect; %position of the mobile platform ConfigCheck; %check the current configuration if CHECK, rho = rho-epsilon; end WSP(round(eta/eta_incr+1),round(gamma/gamma_incr+1)) = rho; %worspace boundary gamma = gamma+gamma_incr; end eta = eta+eta_incr; waitbar(eta/360); end close(wh); OWplot; %Vizualization of the constant-orientation workspace OWvol;