动态避障仿真.rar

clc; close all; clear; f=5; n=360/f; %% startpoint=[3 5];%起点 endpoint=[250 250];%终点 uuv_r=3;%机器人半径 step=3;%机器人速度变化 %% obs=[33 88];%obs起点 vo_1=0.5;%障碍物x轴的速度变化率 vo_2=-4.5*vo_1;%障碍物的运动方程y=-4.5x+225，vo_2为障碍物y轴的速度变化 obs_r=5;%障碍物半径 obs2=[200 130];%obs2起点 obs2_r=5; vo2_1=-1.5; vo2_2=0.5; obs3=[100 7];%obs3起点 obs3_r=5; vo3_1=0; vo3_2=2; obs4=[150 210];%obs4起点 obs4_r=5; vo4_1=2; vo4_2=0; obs_step=[vo_1 vo_2];%obs速度变化 obs_step2=[vo2_1 vo2_2]; %obs2速度变化 obs_step3=[vo3_1 vo3_2]; %obs3速度变化 obs_step4=[vo4_1 vo4_2]; %obs3速度变化 %% %% Safe_d=8;Safe_Dd=45;%安全和威胁距离设定 Flag_break=0; Flag_break2=0; Flag_break3=0; Flag_break4=0; flag1=0; %% A=[]; B=[]; C=[]; D=[]; Z=[]; T=0; robot=startpoint; kt=(caculatebeta(robot,endpoint)); if(kt==0) kt=n; end while norm(robot-endpoint)>step if(norm(robot-endpoint))>step function_circle(robot(1),robot(2),uuv_r);%画圆 fillcircle(robot(1),robot(2),uuv_r);%填充颜色 %kt=caculatebeta(robot,endpoint); robot=robot+[step*cos(kt),step*sin(kt)]; function_circle(obs(1),obs(2),obs_r); obs(1)=obs(1)+obs_step(1); obs(2)=obs(2)+obs_step(2); function_circle(obs3(1),obs3(2),obs3_r); obs3(1)=obs3(1)+obs_step3(1); obs3(2)=obs3(2)+obs_step3(2); if T>65 function_circle(obs4(1),obs4(2),obs4_r); obs4(1)=obs4(1)+obs_step4(1); obs4(2)=obs4(2)+obs_step4(2); end if T>30 function_circle(obs2(1),obs2(2),obs2_r); obs2(1)=obs2(1)+obs_step2(1); obs2(2)=obs2(2)+obs_step2(2); end DI=sqrt((robot(2)-obs(2))^2+(robot(1)-obs(1))^2); DI2=sqrt((robot(2)-obs2(2))^2+(robot(1)-obs2(1))^2); DI3=sqrt((robot(2)-obs3(2))^2+(robot(1)-obs3(1))^2); DI4=sqrt((robot(2)-obs4(2))^2+(robot(1)-obs4(1))^2); %-----------------------------------------------------------------------------------第一个障碍物 if(DI<Safe_Dd&&Flag_break==0)%在机器人观察得到障碍物时，预测机器人是否会与障碍物相撞 a=2*robot(2)-2*obs(2); b=step*sin(kt)-obs_step(2); c=step*cos(kt)-obs_step(1); d=2*robot(1)-2*obs(1); t=round(-(a*b+c*d)/(2*b^2+2*c^2)); if t>0 dist=sqrt(power((robot(2)+step*sin(kt)*t-obs(2)-vo_2*t),2)+power((robot(1)+step*cos(kt)*t-obs(1)-vo_1*t),2));%将时间T带入求机器人与障碍物的最小距离 if(dist<Safe_d) v_car=[step*cos(kt) step*sin(kt)]; v=v_car-obs_step; %机器人和障碍物的相对速度 vv=norm(v); %相对速度的模 aa=dot(v_car,v)/(step*vv); %相对速度和机器人速度的夹角的余弦 bb=sqrt(1-aa^2); %相对速度和机器人速度的夹角的正弦 vt=vv*bb; %速度变化量 p=DI/Safe_Dd; point1=robot; point1=point1+(3/8)*(obs-robot); tn1=(point1(2)-obs(2))/(point1(1)-obs(1)); tn2=5/sqrt((point1(2)-obs(2))^2+(point1(1)-obs(1))^2-25); if((v_car(2)/v_car(1))<(v(2)/v(1))) k1=(tn1-tn2)/(1+tn1*tn2); %机器人于障碍物的切线的斜率 else k1=(tn1+tn2)/(1-tn1*tn2); end alfa1=abs(atan(k1)-atan(v_car(2)/v_car(1))); %切线和机器人速度方向的夹角 alfa2=abs(acos(aa)); %相对速度和机器人速度方向的夹角 rrr=abs(alfa1-alfa2); if((v_car(2)/v_car(1))<(v(2)/v(1))) kt1=caculatebeta(robot,endpoint)-rrr*aa*vv/step; %机器人速度方向的变化情况 else kt1=caculatebeta(robot,endpoint)+rrr*aa*vv/step; %机器人速度方向的变化情况 end step1 = step+rrr*vt; %break; Flag_break=1; flag1=0; else kt=caculatebeta(robot,endpoint); step1 =3; end end end if(DI<Safe_Dd&&Flag_break==1) p=DI/Safe_Dd; if((v_car(2)/v_car(1))<(v(2)/v(1))) kt=kt1; %机器人速度方向的变化情况 step =step1; else kt=kt1; %机器人速度方向的变化情况 step =step1; end if(A(T)>A(T-1)&&flag1==0) flag1=1; end end if(flag1==1) kt=caculatebeta(robot,endpoint); step =3; Flag_break=0; end %----------------------------------------------------------------------------------------第二个障碍物 if(DI3<Safe_Dd&&Flag_break3==0)%在机器人观察得到障碍物时，预测机器人是否会与障碍物相撞 a3=2*robot(2)-2*obs3(2); b3=step*sin(kt)-obs_step3(2); c3=step*cos(kt)-obs_step3(1); d3=2*robot(1)-2*obs3(1); t3=round(-(a3*b3+c3*d3)/(2*b3^2+2*c3^2)); if t3>0 dist3=sqrt(power((robot(2)+step*sin(kt)*t3-obs3(2)-vo3_2*t3),2)+power((robot(1)+step*cos(kt)*t3-obs3(1)-vo3_1*t3),2));%将时间T带入求机器人与障碍物的最小距离 if(dist3<Safe_d) v3_car=[step*cos(kt) step*sin(kt)]; v3=v3_car-obs_step3; %机器人和障碍物的相对速度 vv3=norm(v3); %相对速度的模 aa3=dot(v3_car,v3)/(step*vv3); %相对速度和机器人速度的夹角的余弦 bb3=sqrt(1-aa3^2); %相对速度和机器人速度的夹角的正弦 vt3=vv3*bb3; %速度变化量 %p=DI/Safe_Dd; point3=robot; point3=point3+(3/8)*(obs3-robot); tn31=(point3(2)-obs3(2))/(point3(1)-obs3(1)); tn32=5/sqrt((point3(2)-obs3(2))^2+(point3(1)-obs3(1))^2-25); if((v3_car(2)/v3_car(1))<(v3(2)/v3(1))) k31=(tn31-tn32)/(1+tn31*tn32); %机器人于障碍物的切线的斜率 else k31=(tn31+tn32)/(1-tn31*tn32); end alfa31=abs(atan(k31)-atan(v3_car(2)/v3_car(1))); %切线和机器人速度方向的夹角 alfa32=abs(acos(aa3)); %相对速度和机器人速度方向的夹角 rrr3=abs(alfa31-alfa32); if((v3_car(2)/v3_car(1))<(v3(2)/v3(1))) kt31=caculatebeta(robot,endpoint)-rrr3*aa3*vv3/step; %机器人速度方向的变化情况 else kt31=caculatebeta(robot,endpoint)+rrr3*aa3*vv3/step; %机器人速度方向的变化情况 end step3 = step+rrr3*vt3; %break; Flag_break3=1; flag1=0; else kt=caculatebeta(robot,endpoint); step3 =3; end end end if(DI3<Safe_Dd&&Flag_break3==1) %p=DI/Safe_Dd; if((v3_car(2)/v3_car(1))<(v3(2)/v3(1))) kt=kt31; %机器人速度方向的变化情况 step = step3; else kt=kt31; %机器人速度方向的变化情况 step = step3