clear
clc
Xo=[0 0];%起点位置
k=15;%计算引力需要的增益系数
m=10;%计算斥力系数
Po=2.5;%障碍影响距离
Pstop=0.1;
n=7;
l=0.2;
J=600;
Xsum=[10 10;4.5 5;5 4.5;5.5 4;4 5.5;5 3;3 5;10.2 10.2];
Xj=Xo;
ex=0;
madx=1;
mad(madx,1)=Xsum(1,1);%将目标点存入
mad(madx,2)=Xsum(1,2);
for j=1:J
Goal(j,1)=Xj(1);
Goal(j,2)=Xj(2);
Theta=compute_angle(Xj,Xsum,n);
Angle=Theta(1);
angle_at=Theta(1);
%调用引力模块
[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,m,n,Po);
for i=1:n
angle_re(i)=Theta(i+1);
end
%调用斥力模块
[Yrerxx,Yreryy]=compute_repulsion(Xj,Xsum,m,angle_re,n,Po);
%判断是否在局部极小值点
angleatt(j)=Angle;
Fatt=sqrt(Fatx^2+Faty^2);
Frep=sqrt(Yrerxx^2+Yreryy^2);
if Yrerxx>=0
anglerep(j)=atan(Yreryy/Yrerxx);
else
anglerep(j)=pi+atan(Yreryy/Yrerxx);
end
if abs((Fatt-Frep)/Fatt)<0.5&&cos(angleatt(j)-anglerep(j))<-0.9%判断
%计算新目标点
change=angleatt(j)-pi/2;%角度
Xsum(1,1)=Xj(1)+Po*cos(change);
Xsum(1,2)=Xj(2)+Po*sin(change)
madx=madx+1;
mad(madx,1)=Xsum(1,1);%将目标点存入
mad(madx,2)=Xsum(1,2);
end
Fsumyj=Faty+Yreryy;
Fsumxj=Fatx+Yrerxx;
Position_angle(j)=atan(Fsumyj/Fsumxj);
if Fsumxj<0
Xnext(1)=Xj(1)-l*cos(Position_angle(j));
Xnext(2)=Xj(2)-l*sin(Position_angle(j));
else
Xnext(1)=Xj(1)+l*cos(Position_angle(j));
Xnext(2)=Xj(2)+l*sin(Position_angle(j));
end
Xj=Xnext;
if (ex==1)
break;
ex=0;
end
if (abs(Xj(1)-Xsum(1,1))<=0.1&&abs(Xj(2)-Xsum(1,2))<=0.1)
if madx==1
ex=1;
else
madx=madx-1;
Xsum(1,1)=mad(madx,1);
Xsum(1,2)=mad(madx,2);
end
end
end
K=j
Xxx=mad(:,1);
Yyy=mad(:,2);
X=Goal(:,1);
Y=Goal(:,2);
x=[4.5 5 5.5 4 5 3 10.2];
y=[5 4.5 4 5.5 3 5 10.2];
plot(x,y,'o',Xxx,Yyy,'v',0,0,'ms',X,Y,'.r');
%plot(x,y,'o',Xsum(1,1),Xsum(1,2),'v',0,0,'ms');
function Y=compute_angle(X,Xsum,n)
for i=1:n+1
deltaXi=Xsum(i,1)-X(1);
deltaYi=Xsum(i,2)-X(2);
ri=sqrt(deltaXi^2+deltaYi^2);
if deltaXi>0
theta=asin(deltaYi/ri);
else
theta=pi-asin(deltaYi/ri);
end
if i==1
angle=theta;
else
angle=theta+pi;
end
Y(i)=angle;
end
end
%计算引力程序
% function[Yatx,Yaty]=compute_Attract(X,Xsum,k,angle)
% R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;
% r=sqrt(R);
% Yatx=k*r*cos(angle);
% Yaty=k*r*sin(angle);
% end
%改进斥力场函数
function[Yatx,Yaty]=compute_Attract(X,Xsum,k,angle,m,n,Po)
R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;
r=sqrt(R);
for i=1:n
Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;
rri(i)=sqrt(Rrei(i));
if rri(i)>Po
Yrer(i)=0;
else
%Yrer(i)=m*((1/rri(i)-1/Po)^2)*(rrir/2)^2;
Yrer(i)=(m*(1/rri(i)-1/Po)^2)*r;
%Yrer(i)=(1/rri(i)-1/Po)^2
end
end
xx=sum(Yrer);
Yatx=(k*r+xx)*cos(angle);
Yaty=(k*r+xx)*sin(angle);
%Yatx=(k*r)*cos(angle);
% Yaty=(k*r)*sin(angle);
end
%计算斥力程序
function [Yrerxx,Yreryy]=compute_repulsion(Xj,Xsum,m,angle_re,n,Po)
Rrer=(Xj(1)-Xsum(1,1))^2+(Xj(2)-Xsum(1,2))^2;
rrir=sqrt(Rrer);
for i=1:n
Rrei(i)=(Xj(1)-Xsum(i+1,1))^2+(Xj(2)-Xsum(i+1,2))^2;
rri(i)=sqrt(Rrei(i));
if rri(i)>Po
Yrerx(i)=0;
Yrery(i)=0;
else
%Yrer(i)=m*((1/rri(i)-1/Po)^2)*(rrir/2)^2;
% Yrer(i)=m*(1/rri(i)-1/Po)^2*1/(rri(i)^2);
% Yrer(i)=m*(1/rri(i)-1/Po)^2*1/(rri(i)^2);
Yrer(i)=m*(1/rri(i)-1/Po)*(1/(rri(i)^2))*(rrir^2);
Yrerx(i)=Yrer(i)*cos(angle_re(i));
Yrery(i)=Yrer(i)*sin(angle_re(i));
end
end
Yrerxx=sum(Yrerx);
Yreryy=sum(Yrery);
end
改进斥力场函数和使用虚拟中间目标点法解决传统人工势场法局部极小值问题.rar
3星 · 超过75%的资源 需积分: 50 197 浏览量
2019-07-02
10:49:40
上传
评论 20
收藏 1KB RAR 举报
tao-top
- 粉丝: 10
- 资源: 15