// // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab // Copyright (C) ????-2008 - INRIA // // This file is distributed under the same license as the Scilab package. // function [xdot]=car(t,x) // // xdot=zeros(1,4) ; // car length for the control computation LCpct = bigL * 1.; // calcul de u1 et u2 // if (t<0) u1=0 ; u2=0 ; elseif (t < bigT) // motion 1 tau = t/bigT ; phi=tau*tau*(3-2*tau) ; dphi = 6*tau*(1-tau) ; a = (1-phi)*a1 + phi*a0 ; da = ((a0-a1)/bigT) * dphi ; f = p(1).*(a-a0)^3 + p(2).*(a-a0)^4 + p(3).*(a-a0)^5 ; df = 3*p(1).*(a-a0)^2 + 4*p(2).*(a-a0)^3 + 5*p(3).*(a-a0)^4 ; ddf = 6*p(1).*(a-a0) + 12*p(2).*(a-a0)^2 + 20*p(3).*(a-a0)^3 ; dddf= 6*p(1).*1 + 24*p(2).*(a-a0) + 60*p(3).*(a-a0)^2 ; k = ddf / ((1+df*df)^(3/2)) ; dk = ( dddf - 3*df*ddf*ddf/(1+df*df)) / ((1+df*df)^(3/2)) ; u1 = ((1+df*df)^(1/2)) * da ; u2 = (bigL*dk / (1+(bigL*k)^2) ) * da ; elseif (t < (2*bigT) ) // motion 2 tau = t/bigT -1 ; phi=tau*tau*(3-2*tau) ; dphi = 6*tau*(1-tau) ; a = (1-phi)*a0 + phi*a1 ; da = ((a1-a0)/bigT) * dphi ; f = p(1).*(a-a0)^3 + p(2).*(a-a0)^4 + p(3).*(a-a0)^5 ; df = 3*p(1).*(a-a0)^2 + 4*p(2).*(a-a0)^3 + 5*p(3).*(a-a0)^4 ; ddf = 6*p(1).*(a-a0) + 12*p(2).*(a-a0)^2 + 20*p(3).*(a-a0)^3 ; dddf= 6*p(1).*1 + 24*p(2).*(a-a0) + 60*p(3).*(a-a0)^2 ; k = ddf / ((1+df*df)^(3/2)) ; dk = ( dddf - 3*df*ddf*ddf/(1+df*df)) / ((1+df*df)^(3/2)) ; u1 = ((1+df*df)^(1/2)) * da ; u2 = (LC*dk / (1+(LC*k)^2) ) * da ; else u1=0; u2=0; end xdot(1)= u1 * cos(x(1,3)) ; xdot(2)= u1 * sin(x(1,3)) ; xdot(3)= u1 * tan(x(1,4)) / bigL ; xdot(4)= u2 ; function [xdot]=car2T(t,x) // xdot=zeros(1,6) ; // calcul de u1 et u2 // if (t<0) u1=0 ; u2=0 ; elseif (t < bigT) // motion tau = t/bigT ; phi=tau*tau*(3-2*tau) ; dphi = 6*tau*(1-tau) ; a = (1-phi)*a1 + phi*a0 ; da = ((a0-a1)/bigT) * dphi ; [ff df d2f d3f d4f d5f] = cr2Tfjt(a) ; [k2 k1 k0 dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f) ; u1 = ( (1+(d2*k2)^2)*(1+(d1*k1)^2)*(1+df^2) )^(1/2) * da ; u2 = da * bigL * dk0 / (1+(bigL*k0)^2) ; else u1=0; u2=0; end xdot(1)= u1 * cos(x(1,3)) ; xdot(2)= u1 * sin(x(1,3)) ; xdot(3)= u1 * tan(x(1,6)) / bigL ; xdot(4)= u1 * sin(x(1,3)-x(1,4)) / d1 ; xdot(5)= u1 * sin(x(1,4)-x(1,5)) * cos(x(1,3)-x(1,4)) / d2 ; xdot(6)= u2 ; function [ff,df,d2f,d3f,d4f,d5f]=cr2Tfjt(a) // // M= [ (a-a0)^5 (a-a0)^6 (a-a0)^7 (a-a0)^8 (a-a0)^9 5*(a-a0)^4 6*(a-a0)^5 7*(a-a0)^6 8*(a-a0)^7 9*(a-a0)^8 20*(a-a0)^3 30*(a-a0)^4 42*(a-a0)^5 56*(a-a0)^6 72*(a-a0)^7 60*(a-a0)^2 120*(a-a0)^3 210*(a-a0)^4 336*(a-a0)^5 504*(a-a0)^6 120*(a-a0)^1 360*(a-a0)^2 840*(a-a0)^3 1680*(a-a0)^4 3024*(a-a0)^5 120 720*(a-a0)^1 2520*(a-a0)^2 6720*(a-a0)^3 15120*(a-a0)^4 ]*p ; ff = M(1) + b0 ; df = M(2) ; d2f = M(3) ; d3f = M(4) ; d4f = M(5) ; d5f = M(6) ; function [k2,k1,k0,dk0]=cr2Tfk(df,d2f,d3f,d4f,d5f) // // // computation of curvatures from derivatives of b=f(a) // g = (1+df^2)^(-1/2); dg = - df*d2f*g^3 ; d2g = - g*g*(d2f^2*g+df*d3f*g+3*df*d2f*dg) ; d3g = .... - 2*g*dg*(d2f^2*g+df*d3f*g+3*df*d2f*dg) .... - g^2*(3*d2f*d3f*g+df*d4f*g .... + 4*d2f^2*dg+4*df*d3f*dg+3*df*d2f*d2g) ; // k2 = d2f * g^3 ; dk2 = d3f*g^3 + 3*d2f*g^2*dg ; d2k2 = g^2*(d4f*g+6*d3f*dg+3*d2f*d2g) + 6*g*dg^2*d2f ; d3k2 = 2*g*dg*(d4f*g+6*d3f*dg+3*d2f*d2g) .... + g^2*(d5f*g+7*d4f*dg+9*d3f*d2g+3*d2f*d3g) .... +6*dg^3*d2f+12*g*dg*d2g*d2f+6*g*dg^2*d3f ; // g2 = (1+(d2*k2)^2)^(-1/2) ; dg2 = -d2^2*k2*dk2*g2^3 ; d2g2 = -d2^2*g2^2*(dk2^2*g2+k2*d2k2*g2+3*k2*dk2*dg2) ; // h2 = g2^3*g ; dh2 = g2^2*(3*dg2*g+g2*dg); d2h2 = 2*g2*dg2*(3*dg2*g+g2*dg) .... + g2^2*(3*d2g2*g+4*dg2*dg+g2*d2g) ; // k1 = g2*k2 + d2*h2*dk2 ; dk1 = dg2*k2 + g2*dk2 + d2 * (dh2*dk2+h2*d2k2) ; d2k1 = d2g2*k2 + 2*dg2*dk2 + g2*d2k2 .... + d2 * (d2h2*dk2+2*dh2*d2k2+h2*d3k2) ; // g1 = (1+(d1*k1)^2)^(-1/2) ; dg1 = - d1^2*k1*dk1*g1^3 ; // k0 = g1*k1 + d1*g1^3*g2*g*dk1 ; dk0 = dg1*k1 + g1*dk1 .... + d1*g1^2*(3*dg1*g2*g*dk1+g1*dg2*g*dk1 .... + g1*g2*dg*dk1+g1*g2*g*d2k1) ; function coef=cr2Tkf(b,theta2,theta12,theta01,phi) // // M = [ 1*(a1-a0)^5 1*(a1-a0)^6 1*(a1-a0)^7 1*(a1-a0)^8 1*(a1-a0)^9 5*(a1-a0)^4 6*(a1-a0)^5 7*(a1-a0)^6 8*(a1-a0)^7 9*(a1-a0)^8 20*(a1-a0)^3 30*(a1-a0)^4 42*(a1-a0)^5 56*(a1-a0)^6 72*(a1-a0)^7 60*(a1-a0)^2 120*(a1-a0)^3 210*(a1-a0)^4 336*(a1-a0)^5 504*(a1-a0)^6 120*(a1-a0)^1 360*(a1-a0)^2 840*(a1-a0)^3 1680*(a1-a0)^4 3024*(a1-a0)^5 ] ; // // df = tan(theta2) ; // // curvatures k2=tan(theta12)/d2;k1=tan(theta01)/d1;k0=tan(phi)/bigL; // ddf = k2*((1+df*df)^(3/2)) ; // // derivative of k2 dk2ds2 = ( (1+(d2*k2)^2)/d2 )*( (1+(d2*k2)^2)^(1/2)*k1 - k2 ) ; dk2 = (1+df*df)^(1/2) * dk2ds2 ; // dddf = dk2 * (1+df*df)^(3/2) + 3*k2*df*ddf*(1+df*df)^(1/2) ; // // second derivative of k2 dk1ds1 = ( (1+(d1*k1)^2)/d1 )*( (1+(d1*k1)^2)^(1/2)*k0 - k1 ) ; dk1ds2 = dk1ds1 * (1+(d2*k2)^2)^(1/2) ; // ddk2ds2 = .... 3*d2*k2*dk2ds2*(1+(d2*k2)^2)^(1/2)*k1 .... + (1+(d2*k2)^2)^(3/2)*dk1ds2/d2 ... - (1/d2+3*d2*k2*k2)*dk2ds2 ; // ddk2 = .... df*ddf*((1+df*df)^(-1/2))*dk2ds2 .... + (1+df*df)*ddk2ds2 ; // ddddf = .... ddk2 * (1+df*df)^(3/2) .... + 6*dk2*df*ddf*(1+df*df)^(1/2) .... + 3*k2*ddf*ddf*(1+df*df)^(1/2) .... + 3*k2*df*dddf*(1+df*df)^(1/2) .... + 3*k2*(df*ddf)^2*(1+df*df)^(-1/2) ; // // // // coef =inv(M)* [b;df;ddf;dddf;ddddf] ; function []=ptcr(state) // x=state(1);y=state(2);theta=state(3);phi=state(4); // // plot the car // // rear wheels wheel1 = [ -1/8 1/8 ; 1/6 1/6 ] ; wheel2 = [ -1/8 1/8 ; -1/6 -1/6 ] ; // front wheels wheel3 = [(1-cos(phi)/8) (1+cos(phi)/8); (1/6-sin(phi)/8) (1/6+sin(phi)/8) ] ; wheel4 = [(1-cos(phi)/8) (1+cos(phi)/8); (-1/6-sin(phi)/8) (-1/6+sin(phi)/8) ] ; // // car + wheels xy_car = [ [ -1/3 7/6 4/3 4/3 7/6 -1/3 -1/3; -1/3 -1/3 -1/6 1/6 1/3 1/3 -1/3; ] wheel1 wheel2 wheel3 wheel4 ] ; xy_car = .... [x x x x x x x x x x x x x x x; y y y y y y y y y y y y y y y ] + .... bigL*[cos(theta) -sin(theta);sin(theta) cos(theta)]*xy_car ; // xpoly(xy_car(1,1:7),xy_car(2,1:7),"lines") ; // The 4 wheels xpolys(matrix(xy_car(1,8:15),2,4),matrix(xy_car(2,8:15),2,4),[1 1 1 1]) function ptcr2T(state) // plot the car with two trailers // x0=state(1);y0=state(2);theta0=state(3);theta1=state(4); theta2=state(5);phi=state(6); theta = theta0 ; x=x0 ; y=y0 ; // rear wheels wheel1 = [ -1/8 1/8 ; 1/6 1/6 ] ; wheel2 = [ -1/8 1/8 ; -1/6 -1/6 ] ; // front wheels wheel3 = [ (1-cos(phi)/8) (1+cos(phi)/8) (1/6-sin(phi)/8) (1/6+sin(phi)/8) ] ; wheel4 = [ (1-cos(phi)/8) (1+cos(phi)/8) (-1/6-sin(phi)/8) (-1/6+sin(phi)/8) ] ; // // car + wheels xy_car = [ [ -1/3 7/6 4/3 4/3 7/6 -1/3 -1/3 -1/3 -1/3 -1/6 1/6 1/3 1/3 -1/3 ] wheel1 wheel2 wheel3 wheel4 ] ; xy_car = diag([x,y])*ones(2,15) + ... bigL*[cos(theta0) -sin(theta0);sin(theta0) cos(theta0)]*xy_car ; // // // Trailer 1 x = x - d1*cos(theta1) ; y = y - d1*sin(theta1) ; // // shift + rotation xy_T1 =diag([x,y])*ones(2,11) + ... [cos(theta1) -sin(theta1);sin(theta1) cos(theta1)]*xy_T1 ; // // Trailer 2 x = x - d2*cos(theta2) ; y = y - d2*sin(theta2) ; // // shift + rotation xy_T2 = diag([x,y])*ones(2,11)+ ... [cos(theta2) -sin(theta2);sin(theta2) cos(theta2)]*xy_T2 ; // // plots // // Car xpoly(xy_car(1,1:7),xy_car(2,1:7),"lines") ; // The 4 wheels xpolys(matrix(xy_car(1,8:15),2,4),matrix(xy_car(2,8:15),2,4),[1 1 1 1]) // trailer 1 xpoly(xy_T1(1,1:5),xy_T1(2,1:5),"lines") ; // hitch and wheels xpolys(matrix(xy_T1(1,6:11),2,3),matrix(xy_T1(2,6:11),2,3),[1 1 1 1]); // // trailer 2 xpoly(xy_T2(1,1:5),xy_T2(2,1:5),"lines") ; // hitch and wheels xpolys(matrix(xy_T2(1,6:11),2,3),matrix(xy_T2(2,6:11),2,3),[1 1 1 1]);