标签：Xj 10 0.09 angle 势场 0.5 sqrt 二维 无人机
通过这一算法可以实现二维平面内无人机的路径规划。在该平面内存在已知的障碍，势场法通过给终点添加引力，对障碍增加斥力，使得无人机能够顺利的避障并到达终点。通过对传统势场法的改进，避免了无人机容易陷入极值的问题，使搜索更易实现。
clear all;
x=[1 3 4 7 6 5.5 8 9.5];%???x??
y=[1.5 2.2 4.5 6 2 6 7.8 7];
plot(x,y,'O',10,10,'v',0,0,'ms');
hold on
%??????????????????,??????????????????????????
%???????
Xo=[0 0];%????
k=20;%???????????
K=0;%???
m=5;%??????????????????
Po=2;%??????????????????????????0??????????????????
n=8;%????
a=0.5;
l=0.05;%??
J=500;%??????
%???????????????????????Po?????????
%end
%?????????
Xsum=[10 10;1 1.5;3 2.2;4 4.5;7 6;6 2;5.5 6;8 7.8;9.5 7];%?????(n+1)*2????[10 10]?????????????????
Xj=Xo;%j=1??????????????Xj
[x,y]=meshgrid(-1:0.5:12,-1:0.5:12);
z=0.5*k./(sqrt((x-10).^2+(y-10).^2+0.09))-0.5*m*(1./(sqrt((x-1).^2+(y-1.5).^2+0.09))-1/3.5).^2-0.5*m*(1./(sqrt((x-3).^2+(y-2.2).^2+0.09))-1/3.5).^2 ...
-0.5*m*(1./(sqrt((x-4).^2+(y-4.5).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-7).^2+(y-6).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-6).^2+(y-2).^2+0.09))-1/3.5).^2 ...
-0.5*m*(1./(sqrt((x-5.5).^2+(y-6).^2+0.09))-1/4).^2-0.5*m*(1./(sqrt((x-8).^2+(y-7.8).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-9.5).^2+(y-7).^2+0.09))-1/3).^2;
% contour(x,y,z,[-50:20:1000]);
[C,h]=contour(x,y,z,[-80:10:300]);
set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2)
colormap cool
[px,py]=gradient(z);%????x,y??????????
quiver(x,y,px,py,'k') %???????????
p=sqrt(px.^2+py.^2);
%%??
t=1;
M(t)=getframe;
t=t+1;
%***************????????????******************
for j=1:J%????
Goal(j,1)=Xj(1);%Goal???????????????????????????
Goal(j,2)=Xj(2);
%????????
Theta=compute_angle(Xj,Xsum,n);%Theta??????????????????X????????????????????????????????
%????????
Angle=Theta(1);%Theta?1????????????????????
angle_at=Theta(1);%???????????????????angle_at
[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n);%???????????x,y?????????
for i=1:n
angle_re(i)=Theta(i+1);%?????????????????n??????n????
end
%????????
[Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%??????x,y????????
%????????????????????j???????????????????????????????????????????????
Fsumyj=Faty+Freryy+Fatayy;%y?????
Fsumxj=Fatx+Frerxx+Fataxx;%x?????
Position_angle(j)=atan(Fsumyj/Fsumxj);%???x????????
%?????????
Xnext(1)=Xj(1)+l*cos(Position_angle(j));
Xnext(2)=Xj(2)+l*sin(Position_angle(j));
%?????????????
Xj=Xnext;
X=Goal(:,1);
Y=Goal(:,2);
plot(X,Y,'.r');
M(t)=getframe;t=t+1;
%??
if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%??????????????????????????????????????
K=j;%??????????????
break;
%?????j?
end%?????if????????????????
end%?????
D130
标签：Xj,10,0.09,angle,势场,0.5,sqrt,二维,无人机 来源： https://blog.csdn.net/ccsss22/article/details/122263041
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。