fataexception matlab,人工势场法路径规划（附MAtlab程序）
概念解釋
人工勢(shì)場(chǎng)法路徑規(guī)劃是由Khatib提出的一種虛擬力法(Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.)。它的基本思想是將機(jī)器人在周圍環(huán)境中的運(yùn)動(dòng)，設(shè)計(jì)成一種抽象的人造引力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生“引力”，障礙物對(duì)移動(dòng)機(jī)器人產(chǎn)生“斥力”，最后通過求合力來(lái)控制移動(dòng)機(jī)器人的運(yùn)動(dòng)。應(yīng)用勢(shì)場(chǎng)法規(guī)劃出來(lái)的路徑一般是比較平滑并且安全，但是這種方法存在局部最優(yōu)點(diǎn)問題。
如圖所示，機(jī)器人在一個(gè)二維環(huán)境下運(yùn)動(dòng)，圖中指出了機(jī)器人，障礙和目標(biāo)之間的相對(duì)位置。
這個(gè)圖比較清晰的說明了人工勢(shì)場(chǎng)法的作用，物體的初始點(diǎn)在一個(gè)較高的“山頭”上，要到達(dá)的目標(biāo)點(diǎn)在“山腳”下，這就形成了一種勢(shì)場(chǎng)，物體在這種勢(shì)的引導(dǎo)下，避開障礙物，到達(dá)目標(biāo)點(diǎn)。
人工勢(shì)場(chǎng)包括引力場(chǎng)合斥力場(chǎng)，其中目標(biāo)點(diǎn)對(duì)物體產(chǎn)生引力，引導(dǎo)物體朝向其運(yùn)動(dòng)(這一點(diǎn)有點(diǎn)類似于A*算法中的啟發(fā)函數(shù)h)。障礙物對(duì)物體產(chǎn)生斥力，避免物體與之發(fā)生碰撞。物體在路徑上每一點(diǎn)所受的合力等于這一點(diǎn)所有斥力和引力的和。這里的關(guān)鍵是如何構(gòu)建引力場(chǎng)和斥力場(chǎng)。下面我們分別討論一下：
Fig .引力場(chǎng)模型
Fig 斥力場(chǎng)模型
資料鏈接
路徑規(guī)劃算法初探http://blog.csdn.net/u011978022/article/details/49912515
關(guān)于人工勢(shì)場(chǎng)方法的研http://kovan.ceng.metu.edu.tr/~kadir/academia/courses/grad/cs548/hmws/hw2/report/apf.pdf
人工勢(shì)場(chǎng)方法整理http://letsmakerobots.com/artificial-potential-field-approach-and-its-problems
人工勢(shì)場(chǎng)方法的改進(jìn)版本http://www.doc88.com/p-738493052458.html
人工勢(shì)場(chǎng)方法論壇版 http://www.ilovematlab.cn/thread-188840-1-1.html
人工勢(shì)場(chǎng)法matlab 程序末點(diǎn)震蕩版：http://download.csdn.net/detail/programming2015/8589191#comment
人工勢(shì)場(chǎng)法簡(jiǎn)介PPThttp://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
人工勢(shì)場(chǎng)法matlab程序改進(jìn)成功版本：http://www.ilovematlab.cn/thread-93531-1-1.html
MATLAB代碼實(shí)現(xiàn)
傳統(tǒng)人工勢(shì)場(chǎng)法程序
主程序：
clear clc
Xo=[0 0];%起點(diǎn)位置
k=15;%計(jì)算引力需要的增益系數(shù)
m=4;%計(jì)算斥力的增益系數(shù)，都是自己設(shè)定的。
Po=2.5;%障礙影響距離，當(dāng)障礙和車的距離大于這個(gè)距離時(shí)，斥力為0，即不受該障礙的影響。也是自己設(shè)定。 n=7;%障礙個(gè)數(shù) l=0.2;%步長(zhǎng)
J=600;%循環(huán)迭代次數(shù)
%如果不能實(shí)現(xiàn)預(yù)期目標(biāo)，可能也與初始的增益系數(shù)，Po設(shè)置的不合適有關(guān)。
%end
%給出障礙和目標(biāo)信息
Xsum=[10 10;1 1.5;3 2.2;4 4.5;3 6;6 2;5.5 6;8 8.2];%這個(gè)向量是(n+1)*2維，其中[10 10]是目標(biāo)位置，剩下的都是障礙的位置。
Xj=Xo;%j=1循環(huán)初始，將車的起始坐標(biāo)賦給Xj
%***************初始化結(jié)束，開始主體循環(huán)****************** for j=1:J%循環(huán)開始
Goal(j,1)=Xj(1);%Goal是保存車走過的每個(gè)點(diǎn)的坐標(biāo)。剛開始先將起點(diǎn)放進(jìn)該向量。 Goal(j,2)=Xj(2);
%調(diào)用計(jì)算角度模塊
Theta=compute_angle(Xj,Xsum,n);%Theta是計(jì)算出來(lái)的車和障礙，和目標(biāo)之間的與X軸之間的夾角，統(tǒng)一規(guī)定角度為逆時(shí)針方向，用這個(gè)模塊可以計(jì)算出來(lái)。 %調(diào)用計(jì)算引力模塊
Angle=Theta(1);%Theta(1)是車和目標(biāo)之間的角度，目標(biāo)對(duì)車是引力。 angle_at=Theta(1);%為了后續(xù)計(jì)算斥力在引力方向的分量賦值給angle_at
[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle);%計(jì)算出目標(biāo)對(duì)車的引力在x,y方向的兩個(gè)分量值。 for i=1:n
angle_re(i)=Theta(i+1);%計(jì)算斥力用的角度，是個(gè)向量，因?yàn)橛衝個(gè)障礙，就有n個(gè)角度。 end
%調(diào)用計(jì)算斥力模塊
[Yrerxx,Yreryy]=compute_repulsion(Xj,Xsum,m,angle_re,n,Po);%計(jì)算出斥力在x,y方向的分量數(shù)組。
%計(jì)算合力和方向，這有問題，應(yīng)該是數(shù)，每個(gè)j循環(huán)的時(shí)候合力的大小應(yīng)該是一個(gè)唯一的數(shù)，不是數(shù)組。應(yīng)該把斥力的所有分量相加，引力所有分量相加。 Fsumyj=Faty+Yreryy;%y方向的合力 Fsumxj=Fatx+Yrerxx;%x方向的合力
Position_angle(j)=atan(Fsumyj/Fsumxj);%合力與x軸方向的夾角向量
%計(jì)算車的下一步位置 if Fsumyj < 0 && Fsumxj <0
Xnext(1)=Xj(1)-l*cos(Position_angle(j)); Xnext(2)=Xj(2)-l*sin(Position_angle(j)); else
Xnext(1)=Xj(1)+l*cos(Position_angle(j)); Xnext(2)=Xj(2)+l*sin(Position_angle(j)); end
%保存車的每一個(gè)位置在向量中 Xj=Xnext; %判斷
if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%是應(yīng)該完全相等的時(shí)候算作到達(dá)，還是只是接近就可以？現(xiàn)在按完全相等的時(shí)候編程。 %K=j%記錄迭代到多少次，到達(dá)目標(biāo)。 break; end end K=j;
Goal(K,1)=Xsum(1,1);%把路徑向量的最后一個(gè)點(diǎn)賦值為目標(biāo) Goal(K,2)=Xsum(1,2);
%***********************************畫出障礙，起點(diǎn)，目標(biāo)，路徑點(diǎn)************************* %畫出路徑 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.5 2.2 4.5 6 2 6 8.2 ];
plot(x,y,'o',Xsum(1,1),Xsum(1,2),'v',0,0,'ms',X,Y,'.r');
計(jì)算角度分程序：
function
Y=compute_angle(X,Xsum,n)%Y是引力，斥力與x軸的角度向量,X是起點(diǎn)坐標(biāo)，
Xsum是目標(biāo)和障礙的坐標(biāo)向量,是(n+1)*2矩陣
for i=1:n+1%n是障礙數(shù)目
deltaXi=Xsum(i,1)-X(1)
deltaYi=Xsum(i,2)-X(2)
ri=sqrt(deltaXi^2+deltaYi^2)
if deltaXi>0
theta=asin(deltaXi/ri)
else
theta=pi-asin(deltaXi/ri)
end
if i==1%表示是目標(biāo)
angle=theta else
angle=pi+theta end
Y(i)=angle%保存每個(gè)角度在Y向量里面，第一個(gè)元素是與目標(biāo)的角度，后面都是與障礙的角度
end
計(jì)算引力分程序：
function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle)%輸入?yún)?shù)為當(dāng)前坐標(biāo)，目標(biāo)坐標(biāo)，增益常數(shù),分量和力的角度
%把路徑上的臨時(shí)點(diǎn)作為每個(gè)時(shí)刻的
Xgoal
R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點(diǎn)和目標(biāo)的距離平方
r=sqrt(R);%路徑點(diǎn)和目標(biāo)的距離
Yatx=k*r*cos(angle);
Yaty=k*r*sin(angle);
end
計(jì)算斥力分程序：
%斥力計(jì)算
function [Yrerxx,Yreryy]=compute_repulsion(X,Xsum,m,angle_re,n,Po)%輸入?yún)?shù)為當(dāng)前坐標(biāo)，Xsum是目標(biāo)和障礙的坐標(biāo)向量，增益常數(shù),障礙，目標(biāo)方向的角度
for i=1:n
Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;%路徑點(diǎn)和障礙的距離平方
rre(i)=sqrt(Rrei(i));%路徑點(diǎn)和障礙的距離保存在數(shù)組rrei中
if rre(i)>Po%如果每個(gè)障礙和路徑的距離大于障礙影響距離，斥力令為0
Yrerx(i)=0
Yrery(i)=0
else
Yrer(i)=m*(1/rre(i)-1/Po)^2*1/(rre(i)^2)%分解的Fre1向量
Yrerx(i)=Yrer(i)*cos(angle_re(i))%angle_re(i)=Y(i+1)
Yrery(i)=Yrer(i)*sin(angle_re(i)) end%判斷距離是否在障礙影響范圍內(nèi)
end
Yrerxx=sum(Yrerx)%疊加斥力的分量
Yreryy=sum(Yrery)
改進(jìn)勢(shì)場(chǎng)法程序：
主程序：
clear all;
%障礙和目標(biāo)，起始位置都已知的路徑規(guī)劃,意圖實(shí)現(xiàn)從起點(diǎn)可以規(guī)劃出一條避開障礙到達(dá)目標(biāo)的路徑。
%初始化車的參數(shù) Xo=[0 0];
%起點(diǎn)位置
k=15;%計(jì)算引力需要的增益系數(shù)
K=0;%初始化
m=5;%計(jì)算斥力的增益系數(shù)，都是自己設(shè)定的。
Po=2.5;%障礙影響距離，當(dāng)障礙和車的距離大于這個(gè)距離時(shí)，斥力為0，即不受該障礙的影響。也是自己設(shè)定。
n=7;%障礙個(gè)數(shù)
a=0.5;
l=0.2;%步長(zhǎng)
J=200;%循環(huán)迭代次數(shù)
%如果不能實(shí)現(xiàn)預(yù)期目標(biāo)，可能也與初始的增益系數(shù)，Po設(shè)置的不合適有關(guān)。
%end
%給出障礙和目標(biāo)信息
Xsum=[10 10;1 1.5;3 2.2;4 4.5;3 6;6 2;5.5 6;8 8.2];%這個(gè)向量是(n+1)*2維，其中[10 10]是目標(biāo)位置，剩下的都是障礙的位置。
Xj=Xo;%j=1循環(huán)初始，將車的起始坐標(biāo)賦給Xj
%***************初始化結(jié)束，開始主體循環(huán)******************
for j=1:J%循環(huán)開始
Goal(j,1)=Xj(1);%Goal是保存車走過的每個(gè)點(diǎn)的坐標(biāo)。剛開始先將起點(diǎn)放進(jìn)該向量。
Goal(j,2)=Xj(2); %調(diào)用計(jì)算角度模塊
Theta=compute_angle(Xj,Xsum,n);%Theta是計(jì)算出來(lái)的車和障礙，和目標(biāo)之間的與X軸之間的夾角，統(tǒng)一規(guī)定角度為逆時(shí)針方向，用這個(gè)模塊可以計(jì)算出來(lái)。
%調(diào)用計(jì)算引力模塊
Angle=Theta(1);%Theta(1)是車和目標(biāo)之間的角度，目標(biāo)對(duì)車是引力。
angle_at=Theta(1);%為了后續(xù)計(jì)算斥力在引力方向的分量賦值給angle_at
[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n);%計(jì)算出目標(biāo)對(duì)車的引力在x,y方向的兩個(gè)分量值。
for i=1:n
angle_re(i)=Theta(i+1);%計(jì)算斥力用的角度，是個(gè)向量，因?yàn)橛衝個(gè)障礙，就有n個(gè)角度。
end
%調(diào)用計(jì)算斥力模塊
[Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%計(jì)算出斥力在x,y方向的分量數(shù)組。
%計(jì)算合力和方向，這有問題，應(yīng)該是數(shù)，每個(gè)j循環(huán)的時(shí)候合力的大小應(yīng)該是一個(gè)唯一的數(shù)，不是數(shù)組。應(yīng)該把斥力的所有分量相加，引力所有分量相加。Fsumyj=Faty+Freryy+Fatayy;%y方向的合力
Fsumxj=Fatx+Frerxx+Fataxx;%x方向的合力
Position_angle(j)=atan(Fsumyj/Fsumxj);%合力與x軸方向的夾角向量 %計(jì)算車的下一步位置
Xnext(1)=Xj(1)+l*cos(Position_angle(j));
Xnext(2)=Xj(2)+l*sin(Position_angle(j)); %保存車的每一個(gè)位置在向量中
Xj=Xnext; %判斷
if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%是應(yīng)該完全相等的時(shí)候算作到達(dá)，還是只是接近就可以？現(xiàn)在按完全相等的時(shí)候編程。
K=j;%記錄迭代到多少次，到達(dá)目標(biāo)。
break;
%記錄此時(shí)的j值
end%如果不符合if的條件，重新返回循環(huán)，繼續(xù)執(zhí)行。
end%大循環(huán)結(jié)束 K=j;
Goal(K,1)=Xsum(1,1);%把路徑向量的最后一個(gè)點(diǎn)賦值為目標(biāo)
Goal(K,2)=Xsum(1,2);
%***********************************畫出障礙，起點(diǎn)，目標(biāo)，路徑點(diǎn)************************* %畫出路徑
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.5 2.2 4.5 6 2 6 8.2 ];
plot(x,y,'o',10,10,'v',0,0,'ms',X,Y,'.r');
計(jì)算角度分程序：
function Y=compute_angle(X,Xsum,n)%Y是引力，斥力與x軸的角度向量,X是起點(diǎn)坐標(biāo)，Xsum是目標(biāo)和障礙的坐標(biāo)向量,是(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%表示是目標(biāo)
angle=theta;
else
angle=theta;
end
Y(i)=angle;%保存每個(gè)角度在Y向量里面，第一個(gè)元素是與目標(biāo)的角度，后面都是與障礙的角度
end
計(jì)算引力分程序：
function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle,b,Po,n)%輸入?yún)?shù)為當(dāng)前坐標(biāo)，目標(biāo)坐標(biāo)，增益常數(shù),分量和力的角度
%把路徑上的臨時(shí)點(diǎn)作為每個(gè)時(shí)刻的Xgoal
R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點(diǎn)和目標(biāo)的距離平方
r=sqrt(R);%路徑點(diǎn)和目標(biāo)的距離
Yatx=k*r*cos(angle);%angle=Y(1)
Yaty=k*r*sin(angle);
計(jì)算斥力分程序：
%斥力計(jì)算 function
[Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)%輸入?yún)?shù)為當(dāng)前坐標(biāo)，Xsum是目標(biāo)和障礙的坐標(biāo)向量，增益常數(shù),障礙，目標(biāo)方向的角 度
Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路徑點(diǎn)和目標(biāo)的距離平方
rat=sqrt(Rat);%路徑點(diǎn)和目標(biāo)的距離
for i=1:n
Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;%路徑點(diǎn)和障礙的距離平方
rre(i)=sqrt(Rrei(i));%路徑點(diǎn)和障礙的距離保存在數(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%如果每個(gè)障礙和路徑的距離大于障礙影響距離，斥力令為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^a);%分解的Fre2向量
Yrerx(i)=Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
Yrery(i)=-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)=a*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);
總結(jié)
以上是生活随笔為你收集整理的fataexception matlab,人工势场法路径规划（附MAtlab程序）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 用Python实现黑客帝国代码雨效果
- 下一篇： vue 源码分析(尚硅谷视频学习笔记)