豆粑粑 matlab 演示侧位停车，别人的code

meatball1982

今天，在论坛上看到一个用matlab 演示侧位停车的code。觉得挺好的，
转过来。留用。
http://www.ilovematlab.cn/thread-506120-1-2.html

1. clear;clc;clf
   
2. A=[0,15,15,0,0;0,0,26,26,0];%车子的相对坐标
   
3. L1=[0,24,24,0;0,0,64,64];%车库的相对坐标
   
4. R=42;%转弯半径
   
5. 
   
6. %O1为原点
   
7. L1(1,:)=L1(1,:)-12;%车库
   
8. L1(2,:)=L1(2,:)-92;
   
9. plot(L1(1,:),L1(2,:),'linewidth',2,'color','k')
   
10. hold on
    
11. plot(L1(1,[1,4]),L1(2,[1,4]),'--','linewidth',2,'color','k')
    
12. grid on
    
13. box on
    
14. xlim([-90,50])
    
15. ylim([-120,55])
    
16. 
    
17. plot(A(1,:)-7.5,A(2,:)-13)
    
18. Th=linspace(0,2*pi,101);
    
19. A(1,:)=A(1,:)-R-7.5;
    
20. A(2,:)=A(2,:)-0-13;
    
21. 
    
22. MaxTheta=atan(26/15);%60度
    
23. Theta1=linspace(pi,MaxTheta+pi,11);%往右打
    
24. Theta2=linspace(MaxTheta,0,21);%往左打
    
25. plot([0,-2*R*cos(MaxTheta)],[0,-2*R*sin(MaxTheta)],'k.-')
    
26. 
    
27. C1X=R*cos(Th);
    
28. C1Y=R*sin(Th);
    
29. plot(C1X,C1Y)
    
30. C2X=C1X-2*R*cos(MaxTheta);
    
31. C2Y=C1Y-2*R*sin(MaxTheta);
    
32. plot(C2X,C2Y,'linewidth',1,'color','b')
    
33. 
    
34. for N1N=1:numel(Theta1)-1
    
35.     Theta=Theta1(N1N+1)-Theta1(N1N);
    
36.     Dir=[cos(Theta),-sin(Theta);sin(Theta),cos(Theta)];
    
37.     A=Dir*A;
    
38.     plot(A(1,:),A(2,:),'m')   
    
39.     axis equal
    
40.     xlim([-90,50])
    
41.     ylim([-120,55])
    
42.     pause(.1)
    
43. end
    
44. 
    
45. %平移坐标系到O2
    
46. A(1,:)=A(1,:)+2*R*cos(MaxTheta);
    
47. A(2,:)=A(2,:)+2*R*sin(MaxTheta);
    
48. 
    
49. for N2N=1:numel(Theta2)-1
    
50.     Theta=Theta2(N2N+1)-Theta2(N2N);
    
51.     Dir=[cos(Theta),-sin(Theta);sin(Theta),cos(Theta)];
    
52.     A=Dir*A;
    
53.     AShow(1,:)=A(1,:)-2*R*cos(MaxTheta);%画图时平移回去
    
54.     AShow(2,:)=A(2,:)-2*R*sin(MaxTheta);
    
55.     plot(AShow(1,:),AShow(2,:),'r')   
    
56.     axis equal
    
57.     xlim([-90,50])
    
58.     ylim([-120,55])
    
59.     pause(.1)
    
60. end
    
61. 
    
62. clear;clc;clf
    
63. W=15;%宽度
    
64. H=26;%长度
    
65. Car=[0,W,W,0,0;0,0,H,H,0];%车子的相对坐标
    
66. L1=[0,24,24,0;0,0,64,64];%车库的相对坐标
    
67. R=42;%转弯半径
    
68. %O1为原点
    
69. L1(1,:)=L1(1,:)-12;%车库
    
70. L1(2,:)=L1(2,:)-92;
    
71. plot(L1(1,:),L1(2,:),'linewidth',2,'color','k')
    
72. hold on
    
73. plot(L1(1,[1,4]),L1(2,[1,4]),'--','linewidth',2,'color','k')
    
74. grid on
    
75. box on
    
76. xlim([-90,50])
    
77. ylim([-120,55])
    
78. Car(1,:)=Car(1,:)-W/2;
    
79. Car(2,:)=Car(2,:)-H/2;
    
80. plot(Car(1,:),Car(2,:),'m','linewidth',1)
    
81. Th=linspace(0,2*pi,101);
    
82. Car(1,:)=Car(1,:)-R;
    
83. Car(2,:)=Car(2,:)-0;
    
84. plot(Car(1,:),Car(2,:),'k','linewidth',2)
    
85. Vel=5;%5;%线速度
    
86. MaxTheta=atan(H/W);%60度
    
87. OmegaR=-Vel/R;%右转角速度
    
88. OmegaL=Vel/R;%左转角速度
    
89. Time0=0;%初始时刻
    
90. TimeR2L=(pi-(pi+MaxTheta))/OmegaR;%换向时刻。
    
91. TimeEnd=(MaxTheta-0)/OmegaL+TimeR2L;%结束
    
92. TimeR=linspace(Time0,TimeR2L,19);%往右打
    
93. TimeL=linspace(TimeR2L,TimeEnd,19);%往右打
    
94. plot([0,-2*R*cos(MaxTheta)],[0,-2*R*sin(MaxTheta)],'k.-')
    
95. C1X=R*cos(Th);
    
96. C1Y=R*sin(Th);
    
97. plot(C1X,C1Y)
    
98. C2X=C1X-2*R*cos(MaxTheta);
    
99. C2Y=C1Y-2*R*sin(MaxTheta);
    
100. plot(C2X,C2Y,'linewidth',1,'color','b')
     
101. for N1N=1:numel(TimeR)-1
     
102.     DeltaTR=TimeR(N1N+1)-TimeR(N1N);
     
103.     ThetaR=DeltaTR*OmegaR;
     
104.     Dir=[cos(ThetaR),sin(ThetaR);-sin(ThetaR),cos(ThetaR)];
     
105.     Car=Dir*Car;
     
106.     plot(Car(1,:),Car(2,:),'m')
     
107.     axis equal
     
108.     xlim([-90,50])
     
109.     ylim([-120,55])
     
110.     title(['耗时：',num2str(round(TimeR(N1N+1)*10)/10),'sec'])
     
111.     pause(DeltaTR/10)%实际速度
     
112. end
     
113. %平移坐标系到O2
     
114. Car(1,:)=Car(1,:)+2*R*cos(MaxTheta);
     
115. Car(2,:)=Car(2,:)+2*R*sin(MaxTheta);
     
116. for N2N=1:numel(TimeL)-1
     
117.     DeltaL=TimeL(N1N+1)-TimeL(N1N);
     
118.     ThetaL=DeltaL*OmegaL;
     
119.     Dir=[cos(ThetaL),sin(ThetaL);-sin(ThetaL),cos(ThetaL)];
     
120.     Car=Dir*Car;
     
121.     AShow(1,:)=Car(1,:)-2*R*cos(MaxTheta);%画图时平移回去
     
122.     AShow(2,:)=Car(2,:)-2*R*sin(MaxTheta);
     
123.     plot(AShow(1,:),AShow(2,:),'r')
     
124.     axis equal
     
125.     xlim([-90,50])
     
126.     ylim([-120,55])
     
127.     title(['耗时：',num2str(round(TimeL(N2N+1)*10)/10),'sec'])
     
128.     pause(DeltaL/10)
     
129. end
     
130. plot(AShow(1,:),AShow(2,:),'b','linewidth',2)
     
131. 
     
132. h=gcf;
     
133. fi_na=['./fig_car'];
     
134. fun_work_li_035_myfig_out(h,fi_na,3);
     

复制代码