匀加速运动控制

clc,clear all;

%******************************************
%main
%
%循环计算走到目标点；
%
%*****************************************

%init param;
FR=60;
%case1
% Xi=[0,0];
Xi=[0,0];
Xf=[300,0];
 %Xf=[100,0];
CP=[200,200];
%plot param
i=0;
%位移和速度
SX=Xi(1);
SV=Xi(2);
d=SX-Xf(1);
%每个周期的循环计算；
%while(d>0.05 | d<-0.05 )
while((SV>3 | SV<-3 ) | SX<Xf(1))
Xi(1)=SX;
Xi(2)=SV;
[Phx,Phv] =zero_pathspeed(Xi,Xf,FR,CP);

%update current pos
SV=Phv;
SX=SX+Phx;
d=SX-Xf(1);
i=i+1;
pv(i)=SV;
px(i)=SX;
end

close all
figure(1);
plot(pv,'r');
hold on
plot(px,'b');

 

 

 

 

 

function [pathx,pathv] = zero_pathspeed(xi,xf,fr,ccp)
%***************************************:
%计算下一周期速度和角速度的
%input:
%param1-init state:Sx',Sy',Vx',Vy',Vw'当前速度和位移、角速度；
%param2-final state
%param3-Frame rate
%param4-capacity:Vx.max,ax.max,vw.max,aw.max;
%
%output:
%param1-当前时刻位移
%param2-当前时刻速度
%
%***************************************

vmax=ccp(1);amax=ccp(2);
t1=0;a1=0;s1=0;v1=0;
s0=xi(1);v0=xi(2);
sf=xf(1);vf=xf(2);
%起点到终点的距离
wf=sf-s0;

%case1,2.1,2.2,2.3,3

if (v0<0)
t1=-v0/amax;
a1=amax;
v1=v0+a1/fr;
s1=v0/fr+amax/2/fr/fr;
elseif (v0>=0 & v0<vmax & wf>(v0*v0)/(2*amax))
%s2=v0*()/amax+()*()/2/amax;
va=sqrt(wf*amax+v0*v0/2);
t11=(vmax-v0)/amax;
t22=(va-v0)/amax;
%case a 能达到vmax
if(t11<t22)
a1=amax;
t1=t11;
v1=v0+a1/fr;
if(v1>vmax)
    v1=vmax;
end
s1=v0/fr+amax/2/fr/fr;
%case b 能达到va
else
a1=amax;
t1=t22;
v1=v0+a1/fr;
if(v1>va) v1=va;
end
s1=v0/fr+amax/2/fr/fr;
end

elseif (v0==vmax & wf>(v0*v0)/(2*amax))
a1=0;
t1=wf/vmax-vmax/2/amax;
v1=vmax;
s1=v0/fr;
elseif (v0>0 & v0<=vmax & (wf-v0*v0/2/amax)<0.5)
a1=-amax;
t1=v0/amax;
v1=v0+a1/fr;
s1=v0/fr+a1/2/fr/fr;
elseif(v0>vmax)
a1=-amax;
t1=(v0-vmax)/amax;
v1=v0+a1/fr;
fprintf('elseif /n');
s1=v0/fr+a1/2/fr/fr;
else
fprintf('else /n');
v0=v0
v1=v1
wf=wf
end

pathx=s1;
pathv=v1;