傳統人工勢場法的MATLAB實現

傳統人工勢場法：通過人工建立勢場，將無人機在周圍環境中運動抽象成在引力場中運動，目標對無人機產生吸引力，障礙物對無人機產生排斥力，根據力的疊加原理，可以計算出合力的方向，即無人機飛行的方向，以此來進行路徑規劃。
關于傳統人工勢場法比較詳細的介紹，以及引力勢場和斥力勢場的公式，大家可以參考以下這一篇博客，在此我就不多說了。
傳統人工勢場法比較詳細的介紹
下面的MATLAB代碼可以直接運行出結果，大家可以修改代碼中的一些參數。

%初始化車的參數 Xo=[0 0];%起點位置 k=15;%計算引力需要的增益系數 m=3;%計算斥力的增益系數，都是自己設定的。 Po=1.5;%障礙影響距離，當障礙和車的距離大于這個距離時，斥力為0，即不受該障礙的影響。也是自己設定。 n=5;%障礙個數 l=0.2;%步長 J=1000;%循環迭代次數 %如果不能實現預期目標，可能也與初始的增益系數，Po設置的不合適有關。 %end %給出障礙和目標信息 Xsum=[4 4;1 0.6;2 2.5;1.4 2.1;3.2 2;2.5 1]; %這個向量是(n+1)*2維，其中第一個點[4 4]是目標位置，剩下的都是障礙的位置。 XXX=Xo;%j=1循環初始，將車的起始坐標賦給XXX %***************初始化結束，開始主體循環****************** for j=1:J %循環開始goal(j,1)=XXX(1); %Goal是保存車走過的每個點的坐標。剛開始先將起點放進該向量。goal(j,2)=XXX(2);for i=1:n+1 %計算物體和障礙物、目標點的向量deltaX(i)=Xsum(i,1)-XXX(1);deltaY(i)=Xsum(i,2)-XXX(2);r(i)=sqrt(deltaX(i)^2+deltaY(i)^2);endRgoal=sqrt((XXX(1)-Xsum(1,1))^2+(XXX(2)-Xsum(1,2))^2); %路徑點和目標的距離%目標點對路徑點的引力Fatx=k*Rgoal*(deltaX(1)/Rgoal);Faty=k*Rgoal*(deltaY(1)/Rgoal);%各個障礙物對路徑點的斥力for i=1:nif r(i+1)>PoFrex(i)=0;Frey(i)=0;elseFrex(i)=-m*(1/r(i+1)-1/Po)/r(i+1)/r(i+1)*(deltaX(i+1)/r(i+1));Frey(i)=-m*(1/r(i+1)-1/Po)/r(i+1)/r(i+1)*(deltaY(i+1)/r(i+1));endend%計算合力Fsumx=Fatx+sum(Frex);Fsumy=Faty+sum(Frey);F=sqrt(Fsumx^2+Fsumy^2);%求解下一個路徑點Xnext(1)=(XXX(1)+l*Fsumx/F); %式子中的l是步長Xnext(2)=(XXX(2)+l*Fsumy/F);XXX=Xnext;if (sqrt((XXX(1)-Xsum(1,1))^2+(XXX(2)-Xsum(1,2))^2)<0.1) %當物體接近目標點時k=j; %迭代次數break;end endK=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];x=Xsum(2:n+1,1); y=Xsum(2:n+1,2); plot(x,y,'o',4,4,'v',0,0,'ms',X,Y,'.r');

總結

以上是生活随笔為你收集整理的传统人工势场法的MATLAB实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。