豆粑粑 matlab 演示侧位停车，别人的code - Mathematica中文论坛-非官方豆粑粑

clear;clc;clf
A=[0,15,15,0,0;0,0,26,26,0];%车子的相对坐标
L1=[0,24,24,0;0,0,64,64];%车库的相对坐标
R=42;%转弯半径
%O1为原点
L1(1,:)=L1(1,:)-12;%车库
L1(2,:)=L1(2,:)-92;
plot(L1(1,:),L1(2,:),'linewidth',2,'color','k')
hold on
plot(L1(1,[1,4]),L1(2,[1,4]),'--','linewidth',2,'color','k')
grid on
box on
xlim([-90,50])
ylim([-120,55])
plot(A(1,:)-7.5,A(2,:)-13)
Th=linspace(0,2*pi,101);
A(1,:)=A(1,:)-R-7.5;
A(2,:)=A(2,:)-0-13;
MaxTheta=atan(26/15);%60度
Theta1=linspace(pi,MaxTheta+pi,11);%往右打
Theta2=linspace(MaxTheta,0,21);%往左打
plot([0,-2*R*cos(MaxTheta)],[0,-2*R*sin(MaxTheta)],'k.-')
C1X=R*cos(Th);
C1Y=R*sin(Th);
plot(C1X,C1Y)
C2X=C1X-2*R*cos(MaxTheta);
C2Y=C1Y-2*R*sin(MaxTheta);
plot(C2X,C2Y,'linewidth',1,'color','b')
for N1N=1:numel(Theta1)-1
Theta=Theta1(N1N+1)-Theta1(N1N);
Dir=[cos(Theta),-sin(Theta);sin(Theta),cos(Theta)];
A=Dir*A;
plot(A(1,:),A(2,:),'m')
axis equal
xlim([-90,50])
ylim([-120,55])
pause(.1)
end
%平移坐标系到O2
A(1,:)=A(1,:)+2*R*cos(MaxTheta);
A(2,:)=A(2,:)+2*R*sin(MaxTheta);
for N2N=1:numel(Theta2)-1
Theta=Theta2(N2N+1)-Theta2(N2N);
Dir=[cos(Theta),-sin(Theta);sin(Theta),cos(Theta)];
A=Dir*A;
AShow(1,:)=A(1,:)-2*R*cos(MaxTheta);%画图时平移回去
AShow(2,:)=A(2,:)-2*R*sin(MaxTheta);
plot(AShow(1,:),AShow(2,:),'r')
axis equal
xlim([-90,50])
ylim([-120,55])
pause(.1)
end
clear;clc;clf
W=15;%宽度
H=26;%长度
Car=[0,W,W,0,0;0,0,H,H,0];%车子的相对坐标
L1=[0,24,24,0;0,0,64,64];%车库的相对坐标
R=42;%转弯半径
%O1为原点
L1(1,:)=L1(1,:)-12;%车库
L1(2,:)=L1(2,:)-92;
plot(L1(1,:),L1(2,:),'linewidth',2,'color','k')
hold on
plot(L1(1,[1,4]),L1(2,[1,4]),'--','linewidth',2,'color','k')
grid on
box on
xlim([-90,50])
ylim([-120,55])
Car(1,:)=Car(1,:)-W/2;
Car(2,:)=Car(2,:)-H/2;
plot(Car(1,:),Car(2,:),'m','linewidth',1)
Th=linspace(0,2*pi,101);
Car(1,:)=Car(1,:)-R;
Car(2,:)=Car(2,:)-0;
plot(Car(1,:),Car(2,:),'k','linewidth',2)
Vel=5;%5;%线速度
MaxTheta=atan(H/W);%60度
OmegaR=-Vel/R;%右转角速度
OmegaL=Vel/R;%左转角速度
Time0=0;%初始时刻
TimeR2L=(pi-(pi+MaxTheta))/OmegaR;%换向时刻。
TimeEnd=(MaxTheta-0)/OmegaL+TimeR2L;%结束
TimeR=linspace(Time0,TimeR2L,19);%往右打
TimeL=linspace(TimeR2L,TimeEnd,19);%往右打
plot([0,-2*R*cos(MaxTheta)],[0,-2*R*sin(MaxTheta)],'k.-')
C1X=R*cos(Th);
C1Y=R*sin(Th);
plot(C1X,C1Y)
C2X=C1X-2*R*cos(MaxTheta);
C2Y=C1Y-2*R*sin(MaxTheta);
plot(C2X,C2Y,'linewidth',1,'color','b')
for N1N=1:numel(TimeR)-1
DeltaTR=TimeR(N1N+1)-TimeR(N1N);
ThetaR=DeltaTR*OmegaR;
Dir=[cos(ThetaR),sin(ThetaR);-sin(ThetaR),cos(ThetaR)];
Car=Dir*Car;
plot(Car(1,:),Car(2,:),'m')
axis equal
xlim([-90,50])
ylim([-120,55])
title(['耗时：',num2str(round(TimeR(N1N+1)*10)/10),'sec'])
pause(DeltaTR/10)%实际速度
end
%平移坐标系到O2
Car(1,:)=Car(1,:)+2*R*cos(MaxTheta);
Car(2,:)=Car(2,:)+2*R*sin(MaxTheta);
for N2N=1:numel(TimeL)-1
DeltaL=TimeL(N1N+1)-TimeL(N1N);
ThetaL=DeltaL*OmegaL;
Dir=[cos(ThetaL),sin(ThetaL);-sin(ThetaL),cos(ThetaL)];
Car=Dir*Car;
AShow(1,:)=Car(1,:)-2*R*cos(MaxTheta);%画图时平移回去
AShow(2,:)=Car(2,:)-2*R*sin(MaxTheta);
plot(AShow(1,:),AShow(2,:),'r')
axis equal
xlim([-90,50])
ylim([-120,55])
title(['耗时：',num2str(round(TimeL(N2N+1)*10)/10),'sec'])
pause(DeltaL/10)
end
plot(AShow(1,:),AShow(2,:),'b','linewidth',2)
h=gcf;
fi_na=['./fig_car'];
fun_work_li_035_myfig_out(h,fi_na,3);

复制代码