傳統(tǒng)人工勢(shì)場(chǎng)法的MATLAB實(shí)現(xiàn)

傳統(tǒng)人工勢(shì)場(chǎng)法：通過(guò)人工建立勢(shì)場(chǎng)，將無(wú)人機(jī)在周圍環(huán)境中運(yùn)動(dòng)抽象成在引力場(chǎng)中運(yùn)動(dòng)，目標(biāo)對(duì)無(wú)人機(jī)產(chǎn)生吸引力，障礙物對(duì)無(wú)人機(jī)產(chǎn)生排斥力，根據(jù)力的疊加原理，可以計(jì)算出合力的方向，即無(wú)人機(jī)飛行的方向，以此來(lái)進(jìn)行路徑規(guī)劃。
關(guān)于傳統(tǒng)人工勢(shì)場(chǎng)法比較詳細(xì)的介紹，以及引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)的公式，大家可以參考以下這一篇博客，在此我就不多說(shuō)了。
傳統(tǒng)人工勢(shì)場(chǎng)法比較詳細(xì)的介紹
下面的MATLAB代碼可以直接運(yùn)行出結(jié)果，大家可以修改代碼中的一些參數(shù)。

%初始化車的參數(shù) Xo=[0 0];%起點(diǎn)位置 k=15;%計(jì)算引力需要的增益系數(shù) m=3;%計(jì)算斥力的增益系數(shù)，都是自己設(shè)定的。 Po=1.5;%障礙影響距離，當(dāng)障礙和車的距離大于這個(gè)距離時(shí)，斥力為0，即不受該障礙的影響。也是自己設(shè)定。 n=5;%障礙個(gè)數(shù) l=0.2;%步長(zhǎng) J=1000;%循環(huán)迭代次數(shù) %如果不能實(shí)現(xiàn)預(yù)期目標(biāo)，可能也與初始的增益系數(shù)，Po設(shè)置的不合適有關(guān)。 %end %給出障礙和目標(biāo)信息 Xsum=[4 4;1 0.6;2 2.5;1.4 2.1;3.2 2;2.5 1]; %這個(gè)向量是(n+1)*2維，其中第一個(gè)點(diǎn)[4 4]是目標(biāo)位置，剩下的都是障礙的位置。 XXX=Xo;%j=1循環(huán)初始，將車的起始坐標(biāo)賦給XXX %***************初始化結(jié)束，開(kāi)始主體循環(huán)****************** for j=1:J %循環(huán)開(kāi)始goal(j,1)=XXX(1); %Goal是保存車走過(guò)的每個(gè)點(diǎn)的坐標(biāo)。剛開(kāi)始先將起點(diǎn)放進(jìn)該向量。goal(j,2)=XXX(2);for i=1:n+1 %計(jì)算物體和障礙物、目標(biāo)點(diǎn)的向量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); %路徑點(diǎn)和目標(biāo)的距離%目標(biāo)點(diǎn)對(duì)路徑點(diǎn)的引力Fatx=k*Rgoal*(deltaX(1)/Rgoal);Faty=k*Rgoal*(deltaY(1)/Rgoal);%各個(gè)障礙物對(duì)路徑點(diǎn)的斥力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%計(jì)算合力Fsumx=Fatx+sum(Frex);Fsumy=Faty+sum(Frey);F=sqrt(Fsumx^2+Fsumy^2);%求解下一個(gè)路徑點(diǎn)Xnext(1)=(XXX(1)+l*Fsumx/F); %式子中的l是步長(zhǎng)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) %當(dāng)物體接近目標(biāo)點(diǎn)時(shí)k=j; %迭代次數(shù)break;end endK=j; goal(K,1)=Xsum(1,1);%把路徑向量的最后一個(gè)點(diǎn)賦值為目標(biāo) goal(K,2)=Xsum(1,2);%***********************************畫(huà)出障礙，起點(diǎn)，目標(biāo)，路徑點(diǎn)************************* %畫(huà)出路徑 X=goal(:,1); Y=goal(:,2); %路徑向量Goal是二維數(shù)組,X,Y分別是數(shù)組的x,y元素的集合，是兩個(gè)一維數(shù)組。 %x=[1 3 4 3 6 5.5 8];%障礙的x坐標(biāo) %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');

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。