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