【實例簡介】

人工勢場法路徑規(guī)劃是由Khatib提出的一種虛擬力法(Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.)。它的基本思想是將機器人在周圍環(huán)境中的運動，設(shè)計成一種抽象的人造引力場中的運動，目標點對移動機器人產(chǎn)生“引力”，障礙物對移動機器人產(chǎn)生“斥力”，最后通過求合力來控制移動機器人的運動。應(yīng)用勢場法規(guī)劃出來的路徑一般是比較平滑并且安全，但是這種方法存在局部最優(yōu)點問題。

【實例截圖】

【核心代碼】

%初始化車的參數(shù)

Xo=[0 0];%起點位置

k=15;%計算引力需要的增益系數(shù)

K=0;%初始化

m=5;%計算斥力的增益系數(shù)，都是自己設(shè)定的。

Po=2.5;%障礙影響距離，當障礙和車的距離大于這個距離時，斥力為0，即不受該障礙的影響。也是自己設(shè)定。

n=7;%障礙個數(shù)

a=0.5;

l=0.2;%步長

J=200;%循環(huán)迭代次數(shù)

%如果不能實現(xiàn)預(yù)期目標，可能也與初始的增益系數(shù)，Po設(shè)置的不合適有關(guān)。

%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循環(huán)初始，將車的起始坐標賦給Xj

%***************初始化結(jié)束，開始主體循環(huán)******************

for j=1:J %循環(huán)開始

Goal(j,1)=Xj(1); %Goal是保存車走過的每個點的坐標。剛開始先將起點放進該向量。

Goal(j,2)=Xj(2);

%調(diào)用計算角度模塊

Theta=compute_angle(Xj,Xsum,n);%Theta是計算出來的車和障礙，和目標之間的與X軸之間的夾角，統(tǒng)一規(guī)定角度為逆時針方向，用這個模塊可以計算出來。

%調(diào)用計算引力模塊

Angle=Theta(1);%Theta(1)是車和目標之間的角度，目標對車是引力。

angle_at=Theta(1);%為了后續(xù)計算斥力在引力方向的分量賦值給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

%調(diào)用計算斥力模塊

[Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%計算出斥力在x,y方向的分量數(shù)組。

%計算合力和方向，這有問題，應(yīng)該是數(shù)，每個j循環(huán)的時候合力的大小應(yīng)該是一個唯一的數(shù)，不是數(shù)組。應(yīng)該把斥力的所有分量相加，引力所有分量相加。

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) %是應(yīng)該完全相等的時候算作到達，還是只是接近就可以？現(xiàn)在按完全相等的時候編程。

K=j;%記錄迭代到多少次，到達目標。

break;

%記錄此時的j值

end%如果不符合if的條件，重新返回循環(huán)，繼續(xù)執(zhí)行。

end%大循環(huán)結(jié)束

K=j;

Goal(K,1)=Xsum(1,1);%把路徑向量的最后一個點賦值為目標

Goal(K,2)=Xsum(1,2);

%***********************************畫出障礙，起點，目標，路徑點*************************

%畫出路徑

X=Goal(:,1);

Y=Goal(:,2);

%路徑向量Goal是二維數(shù)組,X,Y分別是數(shù)組的x,y元素的集合，是兩個一維數(shù)組。

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');

function Y=compute_angle(X,Xsum,n)%Y是引力，斥力與x軸的角度向量,X是起點坐標，Xsum是目標和障礙的坐標向量,是(n 1)*2矩陣

for i=1:n 1%n是障礙數(shù)目

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

function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)%輸入?yún)?shù)為當前坐標，Xsum是目標和障礙的坐標向量，增益常數(shù),障礙，目標方向的角度

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));%路徑點和障礙的距離保存在數(shù)組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

if rre(i)

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%判斷距離是否在障礙影響范圍內(nèi)

end

Yrerxx=sum(Yrerx);%疊加斥力的分量

Yreryy=sum(Yrery);

Yataxx=sum(Yatax);

Yatayy=sum(Yatay);

end

總結(jié)

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

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