quasi-static approach MATLAB code

UGV code:

function [ par,normaltra,normaldel] = FPol( fpol,xmin,xmax,XK )

L=1;

l=0.35;

Rc=0.1;

r=Rc;

H=0.336;

mr=50;

mc=300;

mb=20;

a=(L+l+l+Rc+Rc)/XK;

a1=xmin;

a0=a1-(XK+1)*a;

for i=1:1:(XK+1)

f(i)=a0;

a0=a0+a;

end

x1=f';

x2=[xmin:a:xmax]';

x2=[x2;xmax];

x=[x1;x2];

y=subs(fpol,x);

if length(x)>length(y)

val=y;

y(1:length(x))=val;

y=y';

end

Ad=[(0):(pi/19):pi];

At=Ad;

n=length(x);

x1m = ones(n,1);

fx1m = ones(n,1);

ang=ones(n,1);

n=length(x);

Xprincipio=x(1:2);

Yprincipio=y(1:2);

Rprincipio=polyfit(Xprincipio,Yprincipio,1);

pdteprincipio=Rprincipio(1);

Thetaprincipio=atan(pdteprincipio);

ang(1)=Thetaprincipio;

senoprincipio=abs(sin(Thetaprincipio));

cosenoprincipio=abs(cos(Thetaprincipio));

if Rprincipio(1)>=0

x1m(1) = x(1) - Rc*senoprincipio;

fx1m(1) = y(1) + Rc*cosenoprincipio;

end

if Rprincipio(1)<0

x1m(1) = x(1) + Rc*senoprincipio;

fx1m(1) = y(1) + Rc*cosenoprincipio;

end

n=length(x);

XFinal=x(n-1:n);

YFinal=y(n-1:n);

RFinal=polyfit(XFinal,YFinal,1);

pdteFinal=RFinal(1);

ThetaFinal=atan(pdteFinal);

ang(n)=ThetaFinal;

senoFinal=abs(sin(ThetaFinal));

cosenoFinal=abs(cos(ThetaFinal));

if RFinal(1)>=0

x1m(n) = x(n) - Rc*senoFinal;

fx1m(n) = y(n) + Rc*cosenoFinal;

end

if RFinal(1)<0

x1m(n) = x(n) + Rc*senoFinal;

fx1m(n) = y(n) + Rc*cosenoFinal;

end

for i=2:n-1;

X=x(i-1:i+1);

Y=y(i-1:i+1);

R=polyfit(X,Y,1);

pdte=R(1);

Theta=atan(pdte);

ang(i)=Theta;

seno=abs(sin(Theta));

coseno=abs(cos(Theta));

if R(1)>=0

x1m(i) = x(i) - Rc*seno;

fx1m(i) = y(i) + Rc*coseno;

end

if R(1)<0

x1m(i) = x(i) + Rc*seno;

fx1m(i) = y(i) + Rc*coseno;

end

D=ones(10);

for i=1:length(Ad)

for j=1:length(At)

D(i,j)=sqrt(((L-l*cos(At(j))-l*cos(Ad(i)))^2)+((l^2)*(sin(At(j))-sin(Ad(i)))^2));

end

c=length(Ad);

for i=(XK+2):length(x)

for a=1:c

for b=1:c

Valor=zeros(n,1);

v=i;

while v>1

v=v-1;

Valor(v)=sqrt((((x1m(v)-x1m(i))^2)+(((fx1m(v)-fx1m(i))^2))));

if Valor(v)>D(a,b) %Esto ocurre cuando nos hemos "pasado"

%el punto

if abs(Valor(v+1)-D(a,b))>abs(Valor(v)-D(a,b))

Result(a,b,i)=v;

v=0;

else

Result(a,b,i)=v+1;

v=0;

end

for i=(XK+2):length(x)

for a=1:c

for b=1:c

PT=Result(a,b,i);

xcd=x1m(i);

ycd=fx1m(i);

xd=x(i);

yd=y(i);

xct=x1m(PT);

yct=fx1m(PT);

xt=x(PT);

yt=y(PT);

mu=atan((ycd-yct)/(xcd-xct));

M=D(a,b);

d=sqrt( (L^2)+(l^2)-(2*L*l*cos(At(b))));

alpha1=asin((L/d)*sin(At(b)));

b1=pi-alpha1-At(b);

b2=Ad(a)-b1;

h2=l*sin(b2);

alpha2=asin(h2/M);

mu2= alpha1+alpha2;

landa=(2*pi)-mu2-At(b)-Ad(a)-mu;

teta=-pi+At(b)+mu+mu2;

xbd= xcd-(l/2)*cos(landa);

ybd=ycd+(l/2)*sin(landa);

xbt= xct+(l/2)*cos(mu+mu2);

ybt=yct+(l/2)*sin(mu+mu2);

Cmx=xct+l*cos(mu2+mu)+(L/2)*cos(teta)-(H/2)*sin(teta);

Cmy=yct+l*sin(mu2+mu)+(L/2)*sin(teta)+(H/2)*cos(teta);

angt=ang(PT);

angd=ang(i);

xe=xcd;

ye=ycd;

G2x=((mc*Cmx)+(xbd*mb)+(xbt*mb)+(mr*xct))/(mc+mr+2*mb);

G2y=((mc*Cmy)+(ybt*mb)+(ybd*mb)+(mr*yct))/(mc+mr+2*mb);

G2Ex=xe-G2x;

G2Ey=ye-G2y;

G2Tx=xt-G2x;

G2Ty=yt-G2y;

G3x=((mc*Cmx)+(xbd*mb)+(xbt*mb)+(mr*xct)+(mr*xcd))/(mc+mr+mr+2*mb);

G3y=((mc*Cmy)+(ybt*mb)+(ybd*mb)+(mr*yct)+(mr*ycd))/(mc+mr+mr+2*mb);

G3Dx=xd-G3x;

G3Dy=yd-G3y;

G3Tx=xt-G3x;

G3Ty=yt-G3y;

A1=[cos(angt), -sin(angt), cos(angd), -sin(angd), 0];

A2=[sin(angt), cos(angt), sin(angd), cos(angd), 0];

A3=[r, 0 , 0, 0, 1];

A4=[-G2Ty*cos(angt)+G2Tx*sin(angt), G2Ty*sin(angt)+G2Tx*cos(angt), -G2Ey*cos(angd)+G2Ex*sin(angd), G2Ey*sin(angd)+G2Ex*cos(angd), -1];

A5=[-G3Ty*cos(angt)+G3Tx*sin(angt), G3Ty*sin(angt)+G3Tx*cos(angt), -G3Dy*cos(angd)+G3Dx*sin(angd), G3Dy*sin(angd)+G3Dx*cos(angd), 0];

A=[A1;A2;A3;A4;A5];

g=9.81;

C=[ 0 ; (2*mr+mc+2*mb)*g ; 0 ; G2Ex*(mr)*g ; 0];

W=A\C;

PAR(a,b,i)=W(5);

NORMALTRA(a,b,i)=W(2);

NORMALDEL(a,b,i)=W(4);

end

par=PAR;

normaltra=NORMALTRA;

normaldel=NORMALDEL;

end

function [peso fidelan fitrase ]=grafica( par,normaltra,normaldel,xmin,xmax,XK )

ValorInicioAd=0;

ValorFinalAd=180;

RangoAd=ValorFinalAd-ValorInicioAd;

DivisionesAd=19;

ValorInicioAt=0;

ValorFinalAt=180;

RangoAt=ValorFinalAt-ValorInicioAt;

DivisionesAt=19;

Ad=[ValorInicioAd:(RangoAd/DivisionesAd):ValorFinalAd];

At=[ValorInicioAt:(RangoAt/DivisionesAt):ValorFinalAt];

Ad=Ad*(pi/180);

At=At*(pi/180);

L=1;

l=0.35;

Rc=0.1;

r=Rc;

H=0.336;

iterancia=(L+2*l+2*Rc)/XK;

(xmax-xmin)/(iterancia*10);

inicio= XK+2;

q1=0.3;

q2=0.3;

q3=0.2;

q4=0.2;

for i=1:11

for a=1:20

for b=1:20

par(a,b,i)=par(a,b,inicio);

normaldel(a,b,i)=normaldel(a,b,inicio);

normaltra(a,b,i)=normaltra(a,b,inicio);

end

inicio=inicio+(xmax-xmin)/(iterancia*10)

inicio=fix(inicio)

end

par0=par(10,10,1);

fid0=Ad(10);

fit0=At(10);

ntra=normaltra(10,10,1);

ndel=normaldel(10,10,1);

fidelan(1)=Ad(10);

fitrase(1)=At(10);

for i=1:10

for a=1:20

for b=1:20

fid=Ad(a);

fit=At(b);

RTV(a,b)= (abs(par(a,b,i+1)-par0));

FAV(a,b)=(abs(fid-fid0));

RAV(a,b)=(abs(fit-fit0));

end

div=max(RTV);

div=max(div);

RTV=RTV/div

div=max(FAV);

div=max(div);

FAV=FAV/div

div=max(RAV);

div=max(div);

RAV=RAV/div

pesomatrix=q1*RTV+q2*RAV+q3*FAV

peso(:,:,i)=pesomatrix;

[vector fila]=min(pesomatrix)

[valor colum]=min(vector)

fil=fila(colum)

fidelan(i+1)=Ad(fil)

fitrase(i+1)=At(colum)

par0=par(fil,colum,i+1);

ntra=normaltra(fil,colum,i+1);

ndel=normaldel(fil,colum,i+1);

fid0=Ad(fil);

fit0=At(colum);

figure(i)

surf(peso(:,:,i))

title(i)

hold on

end