人工勢場法是由Khatib于1986年提出，其方法是將移動機器人所處的環境用勢場來定義，通過位置信息來控制機器人的避障行駛，基本思想是構造目標位姿引力場和障礙物周圍斥力場共同作用的人工勢場，搜索勢函數的下降方向來尋找無碰撞路徑。人工勢場法避障技術使得機器人的移動能很好的適應機器人周圍環境的變化，實時性高。

1 人工勢場法的原理

人工勢場法原理是：首先構建一個人工虛擬勢場，該勢場由兩部分組成，一部分是目標點對移動機器人產生的引力場，方向由機器人指向目標點，另一部分是障礙物對移動機器人產生的斥力場，方向為由障礙物指向機器人。運行空間的總勢場為斥力場和引力場共同疊加作用，從而通過引力和斥力的合力來控制移動機器人的移動。

2引力函數

引力函數受移動機器人與目標點的距離影響，當目標點與移動機器人的距離越遠時，其所受的勢能越大；當目標點與機器人的距離越近時，其所受的勢能越小。當機器人勢能為零時，則表明機器人到達目標點位置，引力勢函數表示為：

3 斥力函數

斥力函數受移動機器人與障礙物的距離影響，當障礙物與移動機器人的距離越遠時，其所受的勢能越小；當障礙物與機器人的距離越近時，其所受的勢能越大。當機器人勢能為零時，則表明機器人已經脫離障礙物的影響范圍，斥力勢函數表示為：

4 全局勢場函數

根據上述定義的引力場函數和斥力場函數，可以得到整個運行空間的全局勢場函數，機器人的全局勢場大小為機器人所受的斥力勢場和引力勢場之和，故全局勢場總函數為：

5 Matlab代碼實現及仿真結果

我們設置隨機生成20個障礙點，將起點設置為(0,0)，終點設置為(5,8)，步長設置為0.05，其他設置參數詳見后附的matlab代碼，仿真結果如下圖所示。

楊馨怡，重慶大學無線通信技術實驗室碩士研究生，主研方向為無線通信中的預編碼技術。

主函數

%% 初始化

k = 15;%計算引力的增益系數

K = 0;

m = 5;???? %計算斥力的增益系數。

Po =2.5;?? %障礙影響距離，當障礙和車的距離大于這個距離時，斥力為0

a =0.5;

l =0.05;??? %步長

J =200;

start =[0 0];

target= [5 8];

Obscale= ceil(rand(2,20)*10);

x_Obscale= Obscale(1,:)";

y_Obscale= Obscale(2,:)";

Xsum =[target;x_Obscale y_Obscale];

Xj =start;

%% 主體程序

for j =1:J

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

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

%% 調用計算角度模塊

Theta = compute_angle(Xj,Xsum,length(x_Obscale));

%% 調用計算引力模塊

Angle = Theta(1);

angle_at = Theta(1);

[Fatx,Faty] =compute_Attract(Xj,Xsum,k,Angle,0,Po,length(x_Obscale));

for i = 1:length(x_Obscale)

angle_re(i) = Theta(i+1);

end

%% 調用計算斥力模塊

[Frerxx,Freryy,Fataxx,Fatayy] =compute_repulsion(Xj,Xsum,m,angle_at,angle_re,length(x_Obscale),Po,a);

%% 計算合力和方向，

Fsumyj = Faty+Freryy+Fatayy;

Fsumxj = Fatx+Frerxx+Fataxx;

Position_angle(j) = atan(Fsumyj/Fsumxj);

%% 計算下一步位置

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;

end

end

K = j;

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

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

%% 畫出路徑和障礙

plot(x_Obscale,y_Obscale,"bo");

holdon;

plot(start(1),start(2),"ro",target(1),target(2),"ro");

text(start(1)+0.1,start(2),"start");

text(target(1)+0.1,target(2),"target");

holdon;

plot(Goal(:,1),Goal(:,2),"-k");

axis([010 0 10]);

引力函數

function[Yatx,Yaty] = compute_Attract(X,Xsum,k,angle,b,Po,n)

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,Yataxx,Yatayy] = compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)

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

R0 =(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2;

r0 = sqrt(R0);

if rre(i) > Po

Yrerx(i) = 0;

Yrery(i) = 0;

Yatax(i) = 0;

Yatay(i) = 0;

else

if???? rre(i)< Po/2

Yrer(i) =m*(1/rre(i)-1/Po)*(1/Rrei(i))*(rat^a);

Yata(i) = a*m*((1/rre(i)-1/Po)^2)*(rat^(1-a))/2;

Yrerx(i) =(1+0.1)*Yrer(i)*cos(angle_re(i));

Yrery(i) =-(1-0.1)*Yrer(i)*sin(angle_re(i));

Yatax(i) = Yata(i)*cos(angle_at);

Yatay(i) = Yata(i)*sin(angle_at);

else

Yrer(i) =m*(1/rre(i)-1/Po)*1/Rrei(i)*Rat;

Yata(i) =m*((1/rre(i)-1/Po)^2)*rat;

Yrerx(i) =Yrer(i)*cos(angle_re(i));

Yrery(i) =Yrer(i)*sin(angle_re(i));

Yatax(i) = Yata(i)*cos(angle_at);

Yatay(i) = Yata(i)*sin(angle_at);

end

end

end

Yrerxx = sum(Yrerx);

Yreryy = sum(Yrery);

Yataxx = sum(Yatax);

Yatayy = sum(Yatay);

end

角度計算函數

functionY = compute_angle(X,Xsum,n)

for i =1:n+1

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;

end

end

總結

以上是生活随笔為你收集整理的matlab箭头梯度方向场,局部路径规划算法——人工势场法的全部內容，希望文章能夠幫你解決所遇到的問題。

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