人工勢場演算法，用於路徑規劃

main.m程式

%初始化車的引數
Xo=[0 0];%起點位置
k=15;%計算引力需要的增益係數
K=0;%初始化
m=5;%計算斥力的增益係數，都是自己設定的。
Po=2.5;%障礙影響距離，當障礙和車的距離大於這個距離時，斥力為0，即不受該障礙的影響。也是自己設定。
n=7;%障礙個數
a=0.5;
l=0.2;%步長
J=200;%迴圈迭代次數
%如果不能實現預期目標，可能也與初始的增益係數，Po設定的不合適有關。
%end
%給出障礙和目標資訊
Xsum=[10 10;1 1.2;3 2.5;4 4.5;3 6;6 2;5.5 5.5;8 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,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;
%判斷
if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0) %是應該完全相等的時候算作到達，還是隻是接近就可以？現在按完全相等的時候程式設計。
    K=j;%記錄迭代到多少次，到達目標。
break;
%記錄此時的j值
end%如果不符合if的條件，重新返回迴圈，繼續執行。
end%大迴圈結束

K=j;
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 8];%障礙的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');

 

compute_angle.m 函式

function Y=compute_angle(X,Xsum,n)%Y是引力，斥力與x軸的角度向量,X是起點座標，Xsum是目標和障礙的座標向量,是(n+1)*2矩陣
for i=1:n+1%n是障礙數目
    deltaX(i)=Xsum(i,1)-X(1);
    deltaY(i)=Xsum(i,2)-X(2);
    r(i)=sqrt(deltaX(i)^2+deltaY(i)^2);
    if deltaX(i)>0
        theta=acos(deltaX(i)/r(i));
    else
        theta=pi-acos(deltaX(i)/r(i));
    end
    if i==1%表示是目標
        angle=theta;
    else
        angle=theta;
    end
Y(i)=angle;%儲存每個角度在Y向量裡面，第一個元素是與目標的角度，後面都是與障礙的角度
end
end

 

compute_Attract.m 函式

function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle,b,Po,n)%輸入引數為當前座標，目標座標，增益常數,分量和力的角度
%把路徑上的臨時點作為每個時刻的Xgoal
R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點和目標的距離平方
r=sqrt(R);%路徑點和目標的距離
Yatx=k*r*cos(angle);%angle=Y(1)
Yaty=k*r*sin(angle);
end

compute_repulsion.m 函式

function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)%輸入引數為當前座標，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中
    R0=(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2;
    r0=sqrt(R0);
    if rre(i)>Po%如果每個障礙和路徑的距離大於障礙影響距離，斥力令為0
        Yrerx(i)=0;
        Yrery(i)=0;
        Yatax(i)=0;
        Yatay(i)=0;
    else
%if r0<Po
    if rre(i)<Po/2
        Yrer(i)=m*(1/rre(i)-1/Po)*(1/Rrei(i))*(rat^a);%分解的Fre1向量
        Yata(i)=a*m*((1/rre(i)-1/Po)^2)*(rat^(1-a))/2;%分解的Fre2向量
        Yrerx(i)=(1+0.1)*Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
        Yrery(i)=-(1-0.1)*Yrer(i)*sin(angle_re(i));
        Yatax(i)=Yata(i)*cos(angle_at);%angle_at=Y(1)
        Yatay(i)=Yata(i)*sin(angle_at);
    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%判斷距離是否在障礙影響範圍內
end
    Yrerxx=sum(Yrerx);%疊加斥力的分量
    Yreryy=sum(Yrery);
    Yataxx=sum(Yatax);
    Yatayy=sum(Yatay);
end
 

直接放到matlab執行即可看到效果。