%斥力計算
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