【路徑規劃】基於matlab改進的人工勢場演算法機器人避障路徑規劃【含Matlab原始碼 1151期】

語言: CN / TW / HK
時間 2022-07-06 05:15:40
主題:

一、簡介

人工勢場法是區域性路徑規劃的一種比較常用的方法。這種方法假設機器人在一種虛擬力場下運動。 在這裡插入圖片描述 如圖所示，機器人在一個二維環境下運動，圖中指出了機器人，障礙和目標之間的相對位置。 在這裡插入圖片描述 這個圖比較清晰的說明了人工勢場法的作用，物體的初始點在一個較高的“山頭”上，要到達的目標點在“山腳”下，這就形成了一種勢場，物體在這種勢的引導下，避開障礙物，到達目標點。

人工勢場包括引力場合斥力場，其中目標點對物體產生引力，引導物體朝向其運動（這一點有點類似於A*演算法中的啟發函式h）。障礙物對物體產生斥力，避免物體與之發生碰撞。物體在路徑上每一點所受的合力等於這一點所有斥力和引力的和。這裡的關鍵是如何構建引力場和斥力場。下面我們分別討論一下：

引力場：

常用的引力函式： 在這裡插入圖片描述 這裡的ε是尺度因子.ρ(q,q_goal)表示物體當前狀態與目標的距離。引力場有了，那麼引力就是引力場對距離的導數（類比物理裡面W=FX）：、 在這裡插入圖片描述 關於梯度的演算法可以參考相關資料，簡單提一下，二元函式梯度是醬紫的[δx,δy],這個符號是偏導數，不太對，見諒。 在這裡插入圖片描述 Fig .引力場模型

斥力場： 在這裡插入圖片描述 公式（3）是傳統的斥力場公式，現在還沒有搞清楚是怎麼推匯出來的。公式中η是斥力尺度因子，ρ（q,q_obs）代表物體和障礙物之間的距離。ρ_0代表每個障礙物的影響半徑。換言之，離開一定的距離，障礙物就對物體沒有斥力影響。

斥力就是斥力場的梯度 在這裡插入圖片描述 在這裡插入圖片描述 Fig 斥力場模型

總的場就是斥力場合引力場的疊加，也就是U=U_att+U_rep,總的力也是對對應的分力的疊加，如下圖所示： 在這裡插入圖片描述 二、存在的問題 （a） 當物體離目標點比較遠時，引力將變的特別大，相對較小的斥力在甚至可以忽略的情況下，物體路徑上可能會碰到障礙物

（b）當目標點附近有障礙物時，斥力將非常大，引力相對較小，物體很難到達目標點

（c）在某個點，引力和斥力剛好大小相等，方向想反，則物體容易陷入區域性最優解或震盪

三、各種改進版本的人工勢場法 （a）對於可能會碰到障礙物的問題，可以通過修正引力函式來解決，避免由於離目標點太遠導致引力過大 在這裡插入圖片描述 和（1）式相比，（5）式增加了範圍限定。d*_goal 給定了一個閾值限定了目標和物體之間的距離。對應的梯度也就是引力相應變成： 在這裡插入圖片描述 （b）目標點附近有障礙物導致目標不可達的問題，引入一種新的斥力函式 在這裡插入圖片描述 這裡在原有斥力場的基礎上，加上了目標和物體距離的影響,（n是正數，我看到有篇文獻上n=2）。直觀上來說，物體靠近目標時，雖然斥力場要增大，但是距離在減少，所以在一定程度上可以起到對斥力場的拖拽作用

相應斥力變成： 在這裡插入圖片描述 所以可以看到這裡引力分為兩個部分，程式設計時要格外注意

（c）區域性最優問題是一個人工勢場法的一個大問題，這裡可以通過加一個隨機擾動，讓物體跳出區域性最優值。類似於梯度下降法區域性最優值的解決方案。

二、原始碼

```c clear all; %障礙和目標，起始位置都已知的路徑規劃,意圖實現從起點可以規劃出一條避開障礙到達目標的路徑。 %初始化車的引數 Xo=[0 0];% 起點位置 k=1000;% 計算引力需要的增益係數 %K=0;% 初始化 m=10;% 計算斥力的增益係數，都是自己設定的。 Po=1;%障礙影響距離，當障礙和車的距離大於這個距離時，斥力為0，即不受該障礙的影響。也是自己設定。 n=9;%障礙個數 a=0.5; l=0.1;% 步長 J=300;%迴圈迭代次數 r = 0.5; %如果不能實現預期目標，可能也與初始的增益係數， Po 設定的不合適有關。 %end %給出障礙和目標資訊 Xsum=[10 10;1 1.5;3 2.2;4 4.5;3 6;6 2.5;5.5 6; 6 4.5;9 9;8.5 5];% 這個向量是(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,r);%計算出斥力在x,y 方向的分量陣列。 %計算合力和方向，這有問題，應該是數，每個j 迴圈的時候合力的大小應該是一個唯一的數，不是陣列。應該把斥力的所有分量相加，引力所有分量相加。 Fsumyj=Faty+Freryy+Fatayy;%y 方向的合力 Fsumxj=Fatx+Frerxx+Fataxx;%x 方向的合力 Position_angle(j)=atan(Fsumyj/Fsumxj);% 合力與x 軸方向的夾角向量 %計算車的下一步位置

%儲存車的每一個位置在向量中
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); %***** 畫出障礙， 起點， 目標， 路徑點******* %畫出路徑

%路徑向量Goal 是二維陣列,X,Y 分別是陣列的x,y 元素的集合，是兩個一維陣列。 x=[1 3 4 3 6 5.5 6 9 8.5];% 障礙的x 座標 y=[1.5 2.2 4.5 6 2.5 6 4.5 9 5]; plot(10,10,'v',0,0,'ms',X,Y ,'-k','linewidth',3);

Pathkm = Kl %斥力計算 function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a,r)% 輸入引數為當前座標， 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))-r;% 路徑點和障礙的距離儲存在陣列rrei 中 R0=(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2; r0=sqrt(R0)-r; if rre(i)>Po% 如果每個障礙和路徑的距離大於障礙影響距離，斥力令為0 Yrerx(i)=0; Yrery(i)=0; Yatax(i)=0; Yatay(i)=0; else %if r0Xsum(i+1,2) Yrer(i)=m*(1/rre(i)-1/Po)*((1/rre(i))^2)*(rat);% 分解的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/rre(i))^2)*(rat);% 分解的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); end
else if X(2)>Xsum(i+1,2) Yrer(i)=m(1/rre(i)-1/Po)((1/rre(i))^2)(rat);% 分解的Fre1 向量 Yata(i)=am((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)=1Yrer(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/rre(i))^2)Rat;% 分解的Fre1 向量 Yata(i)=am((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%判斷距離是否在障礙影響範圍內

end Yrerxx=sum(Yrerx);% 疊加斥力的分量 Yreryy=sum(Yrery); Yataxx=sum(Yatax); Yatayy=sum(Yatay); ```

三、執行結果

在這裡插入圖片描述

四、備註

版本：2014a

「其他文章」
「」