航跡規劃——人工勢場算法

人工勢場算法簡介

---

  人工勢場法路徑規劃是由Khatib提出的一種虛擬力法（Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.）。它的基本思想是將機器人在周圍環境中的運動，設計成一種抽象的人造引力場中的運動，目標點對移動機器人產生“引力”，障礙物對移動機器人產生“斥力”，最後通過求合力來控制移動機器人的運動。應用勢場法規劃出來的路徑一般是比較平滑並且安全，但是這種方法存在局部最優點問題。
  該算法在網上已經有較多的詳細解釋，在這裏不做具體解釋，主要講解下Matlab下代碼的編寫。想了解算法原理的可以看最後的參考資料。

---

人工勢場算法缺陷

1. 當物體離目標點比較遠時，引力將變的特別大，相對較小的斥力在甚至可以忽略的情況下，物體路徑上可能會碰到障礙物。
2. 當物體離目標點比較遠時，引力將變的特別大，相對較小的斥力在甚至可以忽略的情況下，物體路徑上可能會碰到障礙物。
3. 在某個點，引力和斥力剛好大小相等，方向想反，則物體容易陷入局部最優解或震盪。

---

缺陷解決方法

缺陷一：對於可能會碰到障礙物的問題，可以通過修正引力函數來解決，避免由於離目標點太遠導致引力過大。

（b）目標點附近有障礙物導致目標不可達的問題，引入一種新的斥力函數。

缺陷三：局部最優問題是一個人工勢場法的一個大問題，這裏可以通過加一個隨機擾動，讓物體跳出局部最優值。類似於梯度下降法局部最優值的解決方案。

---

重點代碼解析

---

計算目標點和障礙物對智能體所在位置的角度

---

輸入	意義
X	起點座標
Xsum	目標和障礙的座標向量,是(n+1)*2矩陣

輸出	意義
Y	是引力，斥力與x軸的角度向量

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
end

---

目標點對智能體所在位置引力計算函數

---

輸入	意義
X	當前座標
Xsum	障礙的座標
k	增益常數
angle	角度分量
b	null
Po	障礙影響距離，大於Po時影響力爲0
n	障礙物數量

輸出	意義
Yatx	引力在智能體當前位置的 X 軸分量
Yaty	引力在智能體當前位置的 Y 軸分量

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

---

障礙物對智能體所在位置斥力計算函數

---

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<Po
        if rre(i)<Po/2
       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%判斷距離是否在障礙影響範圍內
end
   Yrerxx=sum(Yrerx);%疊加斥力的分量
   Yreryy=sum(Yrery);
   Yataxx=sum(Yatax);
   Yatayy=sum(Yatay);
end

完整Matlab代碼下載鏈接

---

人工勢場算法完整“.m”文件下載

參考資料

---

1.路徑規劃-人工勢場法
2.機器人路徑規劃_人工勢場法