Xo=[0 0];%起點位置 
k=2;%計算引力需要的增益系數 
m=5;%計算斥力的增益系數，都是自己設定的。 
Po=4.5;%障礙影響距離，當障礙和車的距離大于這個距離時，斥力為0，即不受該障礙的影響。也是自己設定。 
n=7;%障礙個數 
l=0.5;%步長 
J=200;%循環迭代次數 
%如果不能實現預期目標，可能也與初始的增益系數，Po設置的不合適有關。 
%end 
%給出障礙和目標信息 
Xsum=[10 10;1 1.2;3 2.5;4 4.5;3 6;6 2;5.5 5.5;12 8.5];%這個向量是(n+1)*2維，其中[10 10]是目標位置，剩下的都是障礙的位置。 
Xj=Xo;%j=1循環初始，將車的起始坐標賦給Xj 
%***************初始化結束，開始主體循環****************** 
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)%計算出目標對車的引力在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)%計算出斥力在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 
%判斷 
if ((Xj(1)==Xsum(1,1))&((Xj(2)==Xsum(1,2))%是應該完全相等的時候算作到達，還是只是接近就可以？現在按完全相等的時候編程。 
K=j%記錄迭代到多少次，到達目標。 
break; 
%記錄此時的j值 
end%如果不符合if的條件，重新返回循環，繼續執行。 
end%大循環結束 
Goal(K,1)=Xsum(1,1);%把路徑向量的最后一個點賦值為目標 
Goal(K,2)=Xsum(1,2); 
%*******畫出障礙，起點，目標，路徑點**** 
%畫出路徑 
X=Goal(:,1) 
Y=Goal(:,2) 
%路徑向量Goal是二維數組,X,Y分別是數組的x,y元素的集合，是兩個一維數組。 
x=[1 3 4 3 6 5.5 12];%障礙的x坐標 
y=[1.2 2.5 4.5 6 2 5.5 8.5]; 
plot(x,y,'o',10,10,'v',0,0,'ms',X,Y,'*r')%把路徑點，障礙，起點，目標分別用不同的標記畫出。 
% 其中調用的函數如下： 
function Y=compute_angle(X,Xsum,n)%Y是引力，斥力與x軸的角度向量,X是起點坐標，Xsum是目標和障礙的坐標向量,是(n+1)*2矩陣 
for i=1:n+1%n是障礙數目 
deltaXi=Xsum(i,1)-X(1) 
deltaYi=Xsum(i,2)-X(2) 
ri=sqrt(deltaXi^2+deltaYi^2) 
if deltaXi>0 
theta=asin(deltaXi/ri) 
else 
theta=pi-asin(deltaXi/ri) 
end 
if i==1%表示是目標 
angle=theta 
else 
angle=pi+theta 
end 
Y(i)=angle%保存每個角度在Y向量里面，第一個元素是與目標的角度，后面都是與障礙的角度 
end 
%引力計算 
function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle)%輸入參數為當前坐標，目標坐標，增益常數,分量和力的角度 
%把路徑上的臨時點作為每個時刻的Xgoal 
R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點和目標的距離平方 
r=sqrt(R);%路徑點和目標的距離 
%deltax=Xgoal(1)-X(1); 
%deltay=Xgoal(2)-X(2); 
Yatx=k*r*cos(angle);%angle=Y(1) 
Yaty=k*r*sin(angle); 
end 
%斥力計算 
function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po)%輸入參數為當前坐標，Xsum是目標和障礙的坐標向量，增益常數,障礙，目標方向的角度 
Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點和目標的距離平方 
rat=sqrt(Rat);%路徑點和目標的距離 
for i=1:n 
Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;%路徑點和障礙的距離平方 
rre(i)=sqrt(Rrei(i));%路徑點和障礙的距離保存在數組rrei中 
if rre(i)>Po%如果每個障礙和路徑的距離大于障礙影響距離，斥力令為0 
Yrerx(i)=0 
Yrery(i)=0 
Yatax(i)=0 
Yatay(i)=0 
else 
Yrer(i)=m*(1/rre(i)-1/Po)*1/Rrei(i)*Rat%分解的Fre1向量 
Yata(i)=m*((1/rre(i)-1/Po)^2)*rat%分解的Fre2向量 
Yrerx(i)=Yrer(i)*cos(angle_re(i))%angle_re(i)=Y(i+1) 
Yrery(i)=Yrer(i)*sin(angle_re(i)) 
Yatax(i)=Yata(i)*cos(angle_at)%angle_at=Y(1) 
Yatay(i)=Yata(i)*sin(angle_at) 
end%判斷距離是否在障礙影響范圍內 
end 
Yrerxx=sum(Yrerx)%疊加斥力的分量 
Yreryy=sum(Yrery) 
Yataxx=sum(Yatax) 
Yatayy=sum(Yatay) 
end