路徑規劃算法3.1 人工勢場法APF

• 前言
• 電場與電勢場
• 人工勢場
• 人工勢場的構建
• 梯度下降與局部最小問題
• 后記

前言

人工勢場法APF(Artificial Potential Field)，是非場經典的尋路方法，常用于移動機器人的局部路徑規劃，其主要思想是通過目標的引力與障礙物的斥力共同引導機器人的移動。

電場與電勢場

回顧電學中的正負電荷，同性電荷相斥，異性電荷相吸。如下圖所示，黑色電荷位置固定。左邊紅色的正電荷一方面受到黑色正電荷的斥力，遠離正電荷；另一方面受到負電荷的引力，靠近正電荷。

進一步，固定的黑色電荷在空間中形成了電場，場中的每個位置都存在電勢。紅色電荷受電場方向的力影響而運動，也就是向電勢場的負梯度方向運動，勢能下降。

人工勢場

通過模仿電勢場，我們制造了一個人工場，根據場景中的障礙物和目標點計算場景中每個位置的勢能，然后讓機器人沿著負梯度方向（勢能下降的方向）前進，就能夠到達目標點，如下圖所示。

其中，峰值凸包表示障礙物及其附近，紅色點線表示機器人移動的路徑，路徑的終點勢能最低。

人工勢場的構建

人工勢場包括引力場(Attractive)和斥力場(Repulsive)兩部分組成：
$$U(q)=U_{attr}(q)+U_{rep}(q)$$
引力場和斥力場可以自行設定，不過實際應用中通常需要遵守一定的設計準則。常用的引力場函數如下：
$$\begin{gathered} U_{attr}(q)=\{\begin{array}{l}\frac{1}{2}\lambda d(q,q_{goal}{)}^2,d(q,q_{goal})\le d_{thres}\\ \lambda d(q,q_{goal})d_{thres}?\frac{1}{2}\lambda d_{thres}^2,d(q,q_{goal})>d_{thres}\end{array} \\ d(q,q_{goal})=||q?q_{goal}|{|}_2 \end{gathered}$$
其梯度可表示為：
$$?U_{attr}=\{\begin{array}{l}\lambda (q?q_{goal}),d(q,q_{goal})\le d_{thres}\\ \frac{\lambda d_{thres}(q?q_{goal})}{d(q,q_{goal})},d(q,q_{goal})>d_{thres}\end{array}$$
在這個引力場設計中，考慮了機器人位置與目標位置的距離閾值，當前距離小于閾值時，距離越遠梯度越大，機器人受到的引力越大；當前距離大于閾值時，距離越大梯度越小，機器人受到的引力越小，這就能避免由于距離過大導致引力太大導致碰撞障礙物的問題。

斥力場函數可設計為：
$$\begin{gathered} U_{req}(q)=\{\begin{array}{l}\frac{1}{2}\mu (\frac{1}{D(q,q_{obst})}?\frac{1}{D_{thres}}{)}^2,D(q,q_{obst})\le D_{thres}\\ 0,D(q,q_{obst})>D_{thres}\end{array} \\ D(q,q_{obst})=||q?q_{obst}|{|}_2 \end{gathered}$$
梯度為：
$$?U_{req}=\{\begin{array}{l}?\mu (\frac{1}{D(q,q_{obst})}?\frac{1}{D_{thres}})(\frac{1}{D_{(}q,q_{obst})}{)}^2\frac{(q?q_{obst})}{D(q,q_{obst})},D(q,q_{obst})\le D_{thres}\\ 0,D(q,q_{obst})>D_{thres}\end{array}$$
當前位置距離障礙物位置越近，梯度越大，機器人受到的斥力越大；當前位置與障礙物位置大于閾值時，機器人不再受到該障礙物的斥力。

將引力場與斥力場疊加獲得人工勢場：
$$\begin{gathered} U(q)=U_{attr}(q)+\sum _{i=1}^n{U}_{req}^i(q) \\ F(q)=??U(q) \end{gathered}$$

梯度下降與局部最小問題

根據獲得的人工勢場，從起點開始，計算當前位置的梯度，向梯度的負方向前進，然后再次計算當前位置的梯度，再往梯度的負方向前進，直至到達終點。這就是通過梯度下降進行尋路的過程。

然而，我們知道，優化理論中的梯度下降法容易陷入局部最優解。相同地，路徑規劃也有容易陷入局部最優的缺點。

一個很簡單的例子是，機器人的當前位置與障礙物，目標點共線，如下圖所示。機器人將在直線上左右搖擺，無法到達終點。此時需要給機器人一個擾動，令其逃出局部最小。

后記

人工勢場還有許多變種方法，后續有機會我還會對這些變種進行學習記錄。

總結

以上是生活随笔為你收集整理的路径规划算法3.1 人工势场法APF的全部內容，希望文章能夠幫你解決所遇到的問題。

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