-% complete trajectory algorithm
-
-% aproximace rohu splinem 5.radu, spline zacina v pulce kratsiho ze dvou
-% navazujicich useku
-
-clear all;
-
-% zadani libovolneho poctu bodu
-edges = [0 5 8 2 1 4;
- 0 8 0 1 1 2];
-edges = rand([2,10])*10;
-
-% vykresleni primek
-figure(2);
-clf
-plot(edges(1,:), edges(2,:), 'r');
-axis equal
-grid
-title('lin2spline');
-xlabel('x');
-ylabel('y');
-hold on
-
-Xlast = edges(:, 1);
-
-% aproximace rohu
-for i=2:length(edges)-1
- vect0 = edges(:, i-1) - edges(:, i);
- vect1 = edges(:, i+1) - edges(:, i);
-
- % polovina useku
- vect0 = vect0 / 2;
- vect1 = vect1 / 2;
-
- absVect0 = sqrt(vect0(1)^2 + vect0(2)^2);
- absVect1 = sqrt(vect1(1)^2 + vect1(2)^2);
-
- if (absVect0 > absVect1)
- vect0 = vect0 / (absVect0/absVect1);
- distance = absVect1;
- else
- vect1 = vect1 / (absVect1/absVect0);
- distance = absVect0;
- end
- % pocatecni a koncovy bod
- X0 = edges(:, i) + vect0;
- X1 = edges(:, i) + vect1;
-
- % nakresli rovny usek
- t=0:0.01:1;
- usecka=Xlast*ones(1,length(t))+(X0-Xlast)*t;
- plot(usecka(1,:), usecka(2,:), 'b.');
-
- Xlast = X1;
-
- % uhly segmentu
- angle0 = acos((vect0(1)) /(distance)) /pi * 180;
- if (vect0(2) < 0)
- angle0 = 360 - angle0;
- end
-
- angle1 = acos((vect1(1)) /(distance)) /pi * 180;
- if (vect1(2) < 0)
- angle1 = 360 - angle1;
- end
-
- % optimalizace ... i s absolutni vzdalenosti
- m = fminsearch(@(m)plotspline(angle0,angle1,distance,m), 1);
-
- % vypocet polynomu (to uz dela plotspline, ale ja nevim, jak to z toho dostat po dobehu fminsearch)
- p = [cos(angle0/180*pi) sin(angle0/180*pi);
- 0 0;
- cos(angle1/180*pi) sin(angle1/180*pi);];
-
-
- x0 = X0(1);
- y0 = X0(2);
- x1 = X1(1);
- y1 = X1(2);
- xx0 = m*(p(2,1) - p(1,1));
- yy0 = m*(p(2,2) - p(1,2));
- xx1 = m*(p(3,1) - p(2,1));
- yy1 = m*(p(3,2) - p(2,2));
-
- px = [
- -3*xx1-3*xx0-6*x0+6*x1
- 7*xx1+8*xx0+15*x0-15*x1
- -4*xx1-6*xx0-10*x0+10*x1
- 0
- xx0
- x0];
-
-
- py = [
- -3*yy1-3*yy0-6*y0+6*y1
- 7*yy1+8*yy0+15*y0-15*y1
- -4*yy1-6*yy0-10*y0+10*y1
- 0
- yy0
- y0];
-
- % vykresleni aproximacniho splinu
- figure(2);
- plot(polyval(px,t), polyval(py,t), '.b');
- pause
+% complete trajectory algorithm\r
+\r
+% aproximace rohu splinem 5.radu, spline zacina v pulce kratsiho ze dvou\r
+% navazujicich useku\r
+\r
+clear all; close all; clc;\r
+\r
+% zadani libovolneho poctu bodu \r
+edges = [0 5 1 1 6 0 6;\r
+ 0 8 7 1 2 5 6];\r
+\r
+% vykresleni primek\r
+figure(3);\r
+plot(edges(1,:), edges(2,:), 'r');\r
+%grid\r
+title('lin2spline');\r
+xlabel('x');\r
+ylabel('y');\r
+hold on\r
+\r
+% aproximace rohu\r
+for i=2:length(edges)-1\r
+ vect0 = edges(:, i-1) - edges(:, i);\r
+ vect1 = edges(:, i+1) - edges(:, i);\r
+ \r
+ % polovina useku\r
+ vect0 = vect0 / 2;\r
+ vect1 = vect1 / 2;\r
+ \r
+ absVect0 = sqrt(vect0(1)^2 + vect0(2)^2);\r
+ absVect1 = sqrt(vect1(1)^2 + vect1(2)^2);\r
+\r
+ if (absVect0 > absVect1)\r
+ vect0 = vect0 / (absVect0/absVect1);\r
+ distance = absVect1;\r
+ else\r
+ vect1 = vect1 / (absVect1/absVect0);\r
+ distance = absVect0;\r
+ end\r
+ % pocatecni a koncovy bod\r
+ X0 = edges(:, i) + vect0;\r
+ X1 = edges(:, i) + vect1;\r
+ \r
+ % uhly segmentu\r
+ angle0 = acos((vect0(1)) /(distance)) /pi * 180;\r
+ if (vect0(2) < 0)\r
+ angle0 = 360 - angle0;\r
+ end\r
+ \r
+ angle1 = acos((vect1(1)) /(distance)) /pi * 180;\r
+ if (vect1(2) < 0)\r
+ angle1 = 360 - angle1;\r
+ end\r
+ \r
+ % optimalizace ... i s absolutni vzdalenosti\r
+ %m = fminsearch(@(m)plotspline(angle0,angle1,distance,m), 1);\r
+ % NYNI VYCHYTAVKA / APROXIMACE POMOCI ELIPSY\r
+ A = 6860; B = 4.4;\r
+ alpha = abs(angle0 - angle1);\r
+ m = sqrt(B-((alpha)-180).^2./A);\r
+ m = m * distance;\r
+\r
+ \r
+ % vypocet polynomu (to uz dela plotspline, ale ja nevim, jak to z toho dostat po dobehu fminsearch)\r
+ p = [cos(angle0/180*pi) sin(angle0/180*pi);\r
+ 0 0;\r
+ cos(angle1/180*pi) sin(angle1/180*pi);];\r
+\r
+\r
+ x0 = X0(1);\r
+ y0 = X0(2);\r
+ x1 = X1(1);\r
+ y1 = X1(2);\r
+ xx0 = m*(p(2,1) - p(1,1));\r
+ yy0 = m*(p(2,2) - p(1,2));\r
+ xx1 = m*(p(3,1) - p(2,1));\r
+ yy1 = m*(p(3,2) - p(2,2));\r
+\r
+ px = [ \r
+ -3*xx1-3*xx0-6*x0+6*x1\r
+ 7*xx1+8*xx0+15*x0-15*x1\r
+ -4*xx1-6*xx0-10*x0+10*x1\r
+ 0\r
+ xx0\r
+ x0];\r
+\r
+\r
+ py = [\r
+ -3*yy1-3*yy0-6*y0+6*y1\r
+ 7*yy1+8*yy0+15*y0-15*y1\r
+ -4*yy1-6*yy0-10*y0+10*y1\r
+ 0\r
+ yy0\r
+ y0];\r
+ \r
+ % vykresleni aproximacniho splinu\r
+ figure(3);\r
+ for t=0:0.01:1,\r
+ plot(polyval(px,t), polyval(py,t), '.b');\r
+ end\r
+ \r
+ figure(4)\r
+\r
+ title('Curvature');\r
+ xlabel('t');\r
+ grid on\r
+ hold on\r
+\r
+ for t=0:0.01:1,\r
+ kappa = ((5*(-3*xx1-3*xx0-6*x0+6*x1)*t^4+4*(7*xx1+8*xx0+15*x0-15*x1)*t^3+3*(-4*xx1-6*xx0-10*x0+10*x1)*t^2+xx0)*(20*(-3*yy1-3*yy0-6*y0+6*y1)*t^3+12*(7*yy1+8*yy0+15*y0-15*y1)*t^2+6*(-4*yy1-6*yy0-10*y0+10*y1)*t)-(5*(-3*yy1-3*yy0-6*y0+6*y1)*t^4+4*(7*yy1+8*yy0+15*y0-15*y1)*t^3+3*(-4*yy1-6*yy0-10*y0+10*y1)*t^2+yy0)*(20*(-3*xx1-3*xx0-6*x0+6*x1)*t^3+12*(7*xx1+8*xx0+15*x0-15*x1)*t^2+6*(-4*xx1-6*xx0-10*x0+10*x1)*t))/((5*(-3*xx1-3*xx0-6*x0+6*x1)*t^4+4*(7*xx1+8*xx0+15*x0-15*x1)*t^3+3*(-4*xx1-6*xx0-10*x0+10*x1)*t^2+xx0)^2+(5*(-3*yy1-3*yy0-6*y0+6*y1)*t^4+4*(7*yy1+8*yy0+15*y0-15*y1)*t^3+3*(-4*yy1-6*yy0-10*y0+10*y1)*t^2+yy0)^2)^(3/2);\r
+ plot(t, kappa, '+m');\r
+ end\r
+\r
end
\ No newline at end of file
-function vdiff = plotspline(a1, a2, distance, m)
-if (m<0),
- vdiff = inf;
- return
-end
-% a1=60;
-% a2=0;
-% m=1.30;
-p = [
- cos(a1/180.*pi) sin(a1/180.*pi);
- 0 0;
- cos(a2/180.*pi) sin(a2/180.*pi);
- ]*distance;
-
-
-x0 = p(1,1);
-y0 = p(1,2);
-x1 = p(3,1);
-y1 = p(3,2);
-xx0 = m.*(p(2,1) - p(1,1)) / distance;
-yy0 = m.*(p(2,2) - p(1,2)) / distance;
-xx1 = m.*(p(3,1) - p(2,1)) / distance;
-yy1 = m.*(p(3,2) - p(2,2)) / distance;
-
-px = [
- -3.*xx1-3.*xx0-6.*x0+6.*x1
- 7.*xx1+8.*xx0+15.*x0-15.*x1
- -4.*xx1-6.*xx0-10.*x0+10.*x1
- 0
- xx0
- x0];
-
-
-
-py = [
- -3.*yy1-3.*yy0-6.*y0+6.*y1
- 7.*yy1+8.*yy0+15.*y0-15.*y1
- -4.*yy1-6.*yy0-10.*y0+10.*y1
- 0
- yy0
- y0];
-
-figure(1);
-subplot(2,2,1);
-plot(p(:,1), p(:,2), 'r');
-hold on
-title(sprintf('Angle = %d^o', abs(a1-a2)));
-t=0:0.01:1;
-plot(polyval(px,t), polyval(py,t), 'b');
-axis equal
-hold off
-grid on
-
-subplot(2,2,2);
-kappa = ((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).*(20.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^3+12.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^2+6.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t)-(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).*(20.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^3+12.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^2+6.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t))./((5.*(-3.*xx1-3.*xx0-6.*x0+6.*x1).*t.^4+4.*(7.*xx1+8.*xx0+15.*x0-15.*x1).*t.^3+3.*(-4.*xx1-6.*xx0-10.*x0+10.*x1).*t.^2+xx0).^2+(5.*(-3.*yy1-3.*yy0-6.*y0+6.*y1).*t.^4+4.*(7.*yy1+8.*yy0+15.*y0-15.*y1).*t.^3+3.*(-4.*yy1-6.*yy0-10.*y0+10.*y1).*t.^2+yy0).^2).^(3/2);
-plot(t, kappa);
-title('Curvature');
-xlabel('t');
-grid on
-
-subplot(2,2,4);
-cla
-vmin=inf;
-vmax=-inf;
-v=sqrt(polyval(polyder(px),t).^2+...
- polyval(polyder(py),t).^2);
-vmin=min(v);
-vmax=max(v);
-plot(t, v);
-title('Speed');
-xlabel('t');
-grid on
-
-subplot(2,2,3);
-plot(v,kappa, '+');
-xlabel('Speed');
-ylabel('Curvature');
-grid on
-
-vdiff = vmax-vmin;
\ No newline at end of file
+function prujezd = plotspline(a1, a2, distance, m)\r
+if (m<0),\r
+ prujezd = inf;\r
+ return\r
+end\r
+% a1=60;\r
+% a2=0;\r
+% m=1.30;\r
+\r
+p = [\r
+ cos(a1/180*pi) sin(a1/180*pi);\r
+ 0 0;\r
+ cos(a2/180*pi) sin(a2/180*pi);\r
+ ];\r
+\r
+\r
+x0 = p(1,1) * distance;\r
+y0 = p(1,2) * distance;\r
+x1 = p(3,1) * distance;\r
+y1 = p(3,2) * distance;\r
+xx0 = m*(p(2,1) - p(1,1));\r
+yy0 = m*(p(2,2) - p(1,2));\r
+xx1 = m*(p(3,1) - p(2,1));\r
+yy1 = m*(p(3,2) - p(2,2));\r
+\r
+px = [ \r
+ -3*xx1-3*xx0-6*x0+6*x1\r
+ 7*xx1+8*xx0+15*x0-15*x1\r
+ -4*xx1-6*xx0-10*x0+10*x1\r
+ 0\r
+ xx0\r
+ x0];\r
+ \r
+ \r
+ \r
+py = [\r
+ -3*yy1-3*yy0-6*y0+6*y1\r
+ 7*yy1+8*yy0+15*y0-15*y1\r
+ -4*yy1-6*yy0-10*y0+10*y1\r
+ 0\r
+ yy0\r
+ y0];\r
+ \r
+\r
+ \r
+figure(1);\r
+subplot(4,1,1);\r
+cla\r
+axis equal\r
+hold on\r
+plot(p(:,1), p(:,2));\r
+title(sprintf('Angle = %d^o', abs(a1-a2)));\r
+for t=0:0.01:1,\r
+ plot(polyval(px,t), polyval(py,t), '.');\r
+end\r
+\r
+\r
+% vypocet drahy\r
+subplot(4,1,2);\r
+cla\r
+hold on\r
+grid on\r
+title('Distance');\r
+xlabel('t');\r
+\r
+s = 0:1:100;\r
+s(1) = 0;\r
+for t=0:1:100,\r
+ if (t~=0)\r
+ deltaS = sqrt((polyval(px, t/100)-polyval(px, t/100-0.01))^2 + (polyval(py, t/100)-polyval(py, t/100-0.01))^2);\r
+ s(t + 1) = s(t) + deltaS;\r
+ end\r
+ plot(t/100, s(t+1), '+');\r
+end\r
+\r
+\r
+subplot(4,1,3);\r
+cla\r
+title('Curvature');\r
+xlabel('t');\r
+grid on\r
+hold on\r
+\r
+kappa = 0:1:100;\r
+for i=0:1:100,\r
+ t=i/100;\r
+ kappa(i+1) = ((5*(-3*xx1-3*xx0-6*x0+6*x1)*t^4+4*(7*xx1+8*xx0+15*x0-15*x1)*t^3+3*(-4*xx1-6*xx0-10*x0+10*x1)*t^2+xx0)*(20*(-3*yy1-3*yy0-6*y0+6*y1)*t^3+12*(7*yy1+8*yy0+15*y0-15*y1)*t^2+6*(-4*yy1-6*yy0-10*y0+10*y1)*t)-(5*(-3*yy1-3*yy0-6*y0+6*y1)*t^4+4*(7*yy1+8*yy0+15*y0-15*y1)*t^3+3*(-4*yy1-6*yy0-10*y0+10*y1)*t^2+yy0)*(20*(-3*xx1-3*xx0-6*x0+6*x1)*t^3+12*(7*xx1+8*xx0+15*x0-15*x1)*t^2+6*(-4*xx1-6*xx0-10*x0+10*x1)*t))/((5*(-3*xx1-3*xx0-6*x0+6*x1)*t^4+4*(7*xx1+8*xx0+15*x0-15*x1)*t^3+3*(-4*xx1-6*xx0-10*x0+10*x1)*t^2+xx0)^2+(5*(-3*yy1-3*yy0-6*y0+6*y1)*t^4+4*(7*yy1+8*yy0+15*y0-15*y1)*t^3+3*(-4*yy1-6*yy0-10*y0+10*y1)*t^2+yy0)^2)^(3/2);\r
+ plot(t, kappa(i+1), '+');\r
+end\r
+\r
+subplot(4,1,4);\r
+cla\r
+plot(s, kappa, '+');\r
+title('Curvature on Distance');\r
+xlabel('s');\r
+grid on\r
+hold off\r
+\r
+%vytvoreni deltaKappa\r
+deltaKappa = 0:1:100;\r
+for i=2:1:101,\r
+ deltaKappa(i) = kappa(i) - kappa(i-1);\r
+end\r
+\r
+prujezd = 0;\r
+for i=2:1:101,\r
+ deltaKK = abs(deltaKappa(i) - deltaKappa(i-1)) / (s(i) - s(i-1));\r
+ prujezd = prujezd + deltaKK;\r
+end\r
+\r
+ \r
+ \r
+ \r
projects / eurobot / public.git / commitdiff