%clear all, clc function [B] = Init_trajectory_planning(a_a,a_d) % výpočtové konstanty %a_a = 10; % zrychlení [mm/s2] %a_d = 20; % zpomalení [mm/s2] dt = 0.001; % výpočtový časový krok k = -100; % konstanta, aby program poznal, že je konec matice a nehlásil chybu v0 = 0; % rychlost na počátku pohybu s_prubeh = 0; % počáteční poloha time_celk = 0; % výchozí čas n = 1; % parametr určující pož. řádek matice souřadnic i = 1; % přičítací index pro výsledné vektory null = 1; % konstanta pro změnu řádku matice p = 0; % konstanta pro indikaci prvního nebo opakovaného průchodu souřadnicí % matice souřadnic A w=2; % X[mm] Y[mm] Z[mm] v[mm/s] A = [0 0 20 w 30 50 20 w 30 50 0 w 15 65 0 w 0 50 0 w 0 15 0 w 15 0 0 w 30 15 0 w 30 15 20 w 8 77 20 w 8 77 0 w 15 70 0 w 22 77 0 w 22 77 20 w 37.5 65 20 w 37.5 65 0 w 52.5 0 0 w 67.5 65 0 w 67.5 65 20 w 75 65 20 w 75 65 0 w 75 15 0 w 90 0 0 w 105 15 0 w 105 65 0 w 105 65 20 w 112.5 65 20 w 112.5 65 0 w 142.5 65 0 w 127.5 65 0 w 127.5 0 0 w 127.5 0 20 w 0 0 20 w 0 0 0 w 0 0 0 0 0 0 0 0 % nevyhodnocovaný řádek pro trajektorii k k k k]; %----------------------------------------------------------------------- % smyčka pro propočítání trajektorie while n < (length(A)-3)%~ % -------< první výběr z matice A >-------------------- x0 = A(n,1); % souradnice y0 = A(n,2); z0 = A(n,3); x1 = A(n+1,1); y1 = A(n+1,2); z1 = A(n+1,3); v_p = A(n,4); % rychlosti %vk = A(n+1,4); % ------< podmínka pro vybírání dalších řádků matice A >-------- if A(n+3,1) == k && A(n+3,2) == k && A(n+3,3) == k && A(n+3,4) == k x0 = A(n+1,1); y0 = A(n+1,2); z0 = A(n+1,3); x1 = A(n+1,1); y1 = A(n+1,2); z1 = A(n+1,3); v_p = A(n,4); % rychlosti %vk = A(n+1,4); else % když null = 0, tak se vybere další řádek matice if null == 0 x0 = A(n+1,1); y0 = A(n+1,2); z0 = A(n+1,3); x1 = A(n+2,1); y1 = A(n+2,2); z1 = A(n+2,3); x2 = A(n+3,1); y2 = A(n+3,2); z2 = A(n+3,3); v_p = A(n+1,4); % rychlosti %vk = A(n+2,4); n = n+1; % přičítání parametru n else x0 = A(n,1); y0 = A(n,2); z0 = A(n,3); x1 = A(n+1,1); y1 = A(n+1,2); z1 = A(n+1,3); x2 = A(n+2,1); y2 = A(n+2,2); z2 = A(n+2,3); v_p = A(n,4); %vk = A(n+1,4); end end %----------------< interpolace jednoho profilu >------------------------- %________________________________________________________________________ % výpočtové parametry s = sqrt(power(x0-x1,2)+power(y0-y1,2)+power(z0-z1,2)); % úhel mezi vektory u = [x1-x0 ; y1-y0; z1-z0]; v = [x2-x1 ; y2-y1; z2-z1]; u_length = sqrt(power(x0-x1,2)+power(y0-y1,2)+power(z0-z1,2)); v_length = sqrt(power(x2-x1,2)+power(y2-y1,2)+power(z2-z1,2)); fi=acos(((u(1)*v(1))+(u(2)*v(2))+(u(3)*v(3)))/(u_length*v_length)); % koncová rychlost jako funkce změny úhlu dráhy if fi < (pi/2) %v_konc = v_p*(1-(fi*(2/pi))); v_konc = v_p*(1-(fi*(2/pi))^2); else v_konc = 0; end %disp(['p = ',num2str(p),' [1]']) if p == 1 % pokud je p = 1, tak cyklus probíhá znova v_konc = A(n,5); % načítání koncové souřadnice z matice A - pro opakované cykly if n == 1 v0 = 0; else v0 = A(n-1,5); end else A(n,5) = v_konc; % ukládání koncové rychlosti do matice A end %p = 0; % disp(x0) % disp(x1) % disp([' ']) % disp(['fi = ',num2str(fi*(180/pi)),' mm']) % disp(['u = ',num2str(u_length),' mm']) % disp(['v = ',num2str(v_length),' mm']) % disp(['n = ',num2str(n),' [1]']) % disp([' ']) % disp(['v0 = ',num2str(v0),' mm/s']) % disp(['v_konc = ',num2str(v_konc),' mm/s']) % disp(['v_p = ',num2str(v_p),' mm/s']) % disp([' ']) %pause if A(n+3,1) == k v_konc = 0; end %--------------------------------------------------------------------- % výpočet časů - dostatečná vzdálenost t_a1 = abs((v_p-v0)/a_a); t_da = abs((v_p-v0)/a_d); t_d1 = (v_p-v_konc)/a_d; t_ad = t_a1 + t_d1; % dosažená rychlost při krátké dráze v_traj = sqrt((s-power(v0,2)*((1/(2*a_a))-(1/a_a))+(power(v_konc,2)/(2*a_d)))/((1/(2*a_a))+(1/a_d)-(1/(2*a_d)))); % výpočet časů - kratší vzdálenost t1 = (v_traj-v0)/a_a; t11 = abs((v_traj-v0)/a_d); t2 = (v_traj-v_konc)/a_d; t12 = t1 + t2; % podminka pro čas, pokud je poč. rychlost větší než požadovaná if v0 > v_p t_a1 = t_da; t1 = t11; end % výběr správného času zrychlení a zpomalení if t12 < t_ad t_a = t1; t_d = t2; v_prom = v_traj; else t_a = t_a1; t_d = t_d1; v_prom = v_p; end % dosahovaná rychlost při příliš krátké dráze v_dos_a = v0 + a_a*t_a; v_dos_d = v0 - a_d*t_d; if v_dos_a < v_konc %t_a = (-v0 + sqrt(power(v0,2)+(2*a_a*s)))/(a_a); t_a = (2*s)/(v0+v_dos_a); elseif v_dos_d > v_konc %t_d = (v0 - sqrt(power(v0,2)-(2*a_d*s)))/(a_d); t_d = (2*s)/(v0+v_dos_d); end % vypočet dráhy při zrychlení, zpomalení i konstantní rychlosti if v0 < v_p s_a = 0.5*a_a*power(t_a,2)+v0*t_a; else s_a = -0.5*a_a*power(t_a,2)+v0*t_a; end s_d = v_prom*t_d - 0.5*a_d*power(t_d,2); s_v = s - s_a - s_d; %A(n,5) = v_konc; % disp(['sa = ',num2str(s_a),' mm']) % disp(['sd = ',num2str(s_d),' mm']) % disp(['s = ',num2str(s),' mm']) % disp(['ta = ',num2str(t_a),' s']) % % disp(['td = ',num2str(t_d),' s']) % % disp(['A = [',num2str(A(n,1)),' ',num2str(A(n,2)),' ',num2str(A(n,3)),' ',num2str(A(n,4)),' ',num2str(A(n,5)),']']) %pause % podmínka pro příliš krátkou dráhu if s <= s_a && v0 < v_konc disp('ZRYCHLOVANI'); disp('Pozadovane rychlosti nelze na dane draze dosahnout'); disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti'); disp(' '); dv_max_a = a_a * t_a; v_konc = v0 + dv_max_a; A(n,5) = v_konc; p = 1; n = 1; null = 1; i = 1; v0 = 0; v_prubeh = 0; s_prubeh = 0; t_dop=0; time_celk = 0; continue %error('Pozadovane rychlosti nelze na dane draze dosahnout') elseif s <= s_d && v0 > v_konc && s ~= 0 disp('BRZDENI'); disp('Pozadovane rychlosti nelze na dane draze dosahnout'); disp('Zvolte delsi drahu nebo mensi rozdil koncove a pocatecni rychlosti'); disp(' '); dv_max_d = a_d * t_d; v0 = v_konc + dv_max_d; A(n-1,5) = v0; p = 1; n = 1; null = 1; i = 1; v0 = 0; v_prubeh = 0; s_prubeh = 0; t_dop=0; time_celk = 0; continue %error('Pozadovane rychlosti nelze na dane draze dosahnout') end % čas pohybu konstantni rychlostí t_v = s_v / v_p; % podmínka pro počátek nebo konec drahy, kdy se rychlost nemění if t_d < 0 t_d = 0; elseif t_a < 0 t_a = 0; end t_celk = t_a + t_v + t_d; % počáteční hodnoty pro každou smyčku v_prubeh(i) = v0; %s_prubeh(i) = 0; t_dop(i)=0; %průběžné hodnoty % disp(['s = ',num2str(s),' mm']) % disp(['n = ',num2str(n),' [1]']) % % disp(['v0 = ',num2str(v0),' mm/s']) % disp(['v_konc = ',num2str(v_konc),' mm/s']) % disp(['v_p = ',num2str(v_p),' mm/s']) % disp([' ']) % disp(['t = ',num2str(time_celk(i)),' s']) % disp(['t_dop = ',num2str(t_dop(i)),' s']) % disp(['t_celk = ',num2str(t_celk),' s']) % disp([' ']) % smyčka výpočtu jednoho profilu while t_dop(i) < t_celk % přičítání času i = i+1; time_celk(i) = time_celk(i-1) + dt; t_dop(i) = t_dop(i-1)+dt; t_obr(i) = t_celk - t_dop(i); % výběr zrychlení if t_dop(i) > t_a if t_obr(i) < 0 a(i) = 0; else if t_obr(i) < t_d a(i) = -a_d; else a(i) = 0; end end else if v0 < v_p a(i) = a_a; else a(i) = -a_d; end end % numericka integrace v_prubeh(i) = v_prubeh(i-1)+a(i)*dt; s_prubeh(i) = s_prubeh(i-1) + 0.5*(v_prubeh(i)+v_prubeh(i-1))*dt; %m(i)=n; end % zapamatovani rychlosti na konci profilu t_cc = length(v_prubeh); v0 = v_prubeh(t_cc); null = 0; end B = A; %------------------< VYKRESLENI >------------------------------------ % t_c = length(time_celk); % % % průměrné hodnoty rychlosti a zrychlení % v_avg = s_prubeh(t_c)/time_celk(t_c); % a_avg = v_prubeh(t_c)/time_celk(t_c); % % disp(['Poloha v čase t, s = ',num2str(s_prubeh(t_c)),' mm']) % disp(['Průměrná rychlost, v_stř = ',num2str(v_avg),' mm/s']) % disp(['Půměrné zrychlení, a_stř = ',num2str(a_avg),' mm/s2']) % % disp(['Čas t = ',num2str(time_celk(t_c)),' s']) % -----------------< grafy >----------------------------------- % % tloustka čas v grafech % w = 2; % % % vykresleni % figure(1) % subplot(3,1,1);plot(time_celk,a, 'k','linewidth',w) % hold on % plot([0 time_celk(t_c)],[a_avg a_avg],'r --') % grid on % title('Průběh zrychlení') % xlabel('t [s]') % ylabel('a [mm/s2]') % % xlim([0 t_c*dt]); % ylim([-1.5*abs(min(a)) 1.5*max(a)]); % hold off % % % prubeh rychlosti % subplot(3,1,2);plot(time_celk,v_prubeh,'k','linewidth',w) % hold on % plot([0 time_celk(t_c)],[v_avg v_avg],'r --') % grid on % title('Průběh rychlosti') % xlabel('t [s]') % ylabel('v [mm/s]') % % xlim([0 t_c*dt]); % ylim([-1.5*v0 1.5*max(v_prubeh)]); % hold off % % % prubeh polohy % subplot(3,1,3);plot(time_celk,s_prubeh,'k','linewidth',w) % hold on % grid on % title('Průběh souřadnice polohy') % xlabel('t [s]') % ylabel('s [mm]') % %xlim([0 t_c*dt]); % ylim([-1.5*x0 1.5*max(s_prubeh)]);