[数据结构与算法]【智能泊车】基于MATLAB的智能泊车算法的仿真

%约束空间生成
if((sqrt((x0-xd+r)^2+(y0-yd)^2)-3*r)>0)
   fprintf('距离过远，最小半径泊车方法无法泊车\n'); 
   return;
end
if((t1-t2)+(pi/8)<0 | (t1-t2)-(pi/8)>0)
    fprintf('终点车身偏角过大，最小半径泊车方法无法泊车\n');
    return;
end
%第一阶段轨迹
 
x1=x0+r;
y1=y0;
fai1=fai;
  
for i=pi:ds:(pi+t1)
     x=x1+r*cos(i);
     y=y1+r*sin(i);
     
     theta=i-(pi/2);%theta0为起始点车身偏角
     
     jiao1=atan((width/2)/(long-houxuan));
     jiao2=jiao1;
     jiao3=atan((width/2)/houxuan);
     jiao4=jiao3;
     jiao1=theta-jiao1;
     jiao2=theta+jiao2;
     jiao3=theta+pi-jiao3;
     jiao4=theta+pi+jiao4;
     
     r1=sqrt((width/2)^2+(long-houxuan)^2);%以下描述车身的四个端点
     r2=sqrt((width/2)^2+houxuan^2);
     youqianx=x+r1*cos(jiao1);
     youqiany=y+r1*sin(jiao1);
     
     zuoqianx=x+r1*cos(jiao2);
     zuoqiany=y+r1*sin(jiao2);
     
     zuohoux=x+r2*cos(jiao3);
     zuohouy=y+r2*sin(jiao3);
     
     youhoux=x+r2*cos(jiao4);
     youhouy=y+r2*sin(jiao4);
     
     h1=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-r');
     h2=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-r');
     h3=plot([zuohoux,youhoux],[zuohouy,youhouy],'-r');
     h4=plot([youhoux,youqianx],[youhouy,youqiany],'-r');
      
     
     jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子
     jiao6=jiao5;
     jiao5=theta-jiao5;
     jiao6=theta+jiao6;
     jiao7=theta+(pi/2);
     jiao8=theta-(pi/2);
     
     jiao9=theta+fai1;
     jiaoa=theta;
     
     r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);
     r4=width/2;
     
     yqianlunzx=x+r3*cos(jiao5);
     yqianlunzy=y+r3*sin(jiao5);
     yqianlunqx=yqianlunzx+lunjin*cos(jiao9);
     yqianlunqy=yqianlunzy+lunjin*sin(jiao9);
     yqianlunhx=yqianlunzx-lunjin*cos(jiao9);
     yqianlunhy=yqianlunzy-lunjin*sin(jiao9);
     h5=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');
     
     zqianlunzx=x+r3*cos(jiao6);
     zqianlunzy=y+r3*sin(jiao6);
     zqianlunqx=zqianlunzx+lunjin*cos(jiao9);
     zqianlunqy=zqianlunzy+lunjin*sin(jiao9);
     zqianlunhx=zqianlunzx-lunjin*cos(jiao9);
     zqianlunhy=zqianlunzy-lunjin*sin(jiao9);
     h6=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');
     
     zhoulunzx=x+r4*cos(jiao7);
     zhoulunzy=y+r4*sin(jiao7);
     zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);
     zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);
     zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);
     zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);
     h7=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');
     
     yhoulunzx=x+r4*cos(jiao8);
     yhoulunzy=y+r4*sin(jiao8);
     yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);
     yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);
     yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);
     yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);
     h8=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');
     %在此添加判决函数，判断是否第一阶段会碰到障碍物
     if(yhoulunzx-x6>=0 & yhoulunzy-y6>=0)
         fprintf('第一阶段碰障，最小半径泊车方法无法泊车\n');
         delete(h1);
         delete(h2);
         delete(h3);
         delete(h4);
         delete(h5);
         delete(h6);
         delete(h7);
         delete(h8);
         return;
     end

     pause(0.01);
    
     delete(h1);
     delete(h2);
     delete(h3);
     delete(h4);
     delete(h5);
     delete(h6);
     delete(h7);
     delete(h8);
               
end