1、人工勢場法基本原理

人工勢場法的基本原理就是將無人車假設成一個點，該點在一個虛擬力場中運動，虛擬力場是由目標點對無人車的引力場和障礙物對無人車的斥力場組成。引力場由目標點產生，斥力場是由所有的障礙物產生的和力場組成。因此，人工勢場法的勢場函式定義為引力場與斥力場的和，如下面的式子所示。無人車的移動方向為勢場函式下降的方向。這樣，無人車就可以找到一條無碰撞的路徑。
$U(q)=U_{att}(q)+U_{rep}(q)$U(q)=Uatt​(q)+Urep​(q)
在上式中，q是無人車的座標點，$U_{att}(q)$Uatt​(q)是引力場，$U_{}$

rep(q)U_{rep}(q)Urep​(q)是斥力場，U(q)是勢場和。
勢場函式的負梯度定義為人工力，人工力由引力和斥力組成。引力為引力場的負梯度，斥力為斥力場的負梯度。因此，無人車在勢場中所受力為：
$F(q)=-\nabla U_{att}(q)-\nabla U_{rep}(q)=F_{att}(q)+F_{rep}(q)$F(q)=−∇Uatt​(q)−∇Urep​(q)=Fatt​(q)+Frep​(q)
上式中，F(q)為無人車所受合力，$F_{att}$Fatt​為引力，引導無人車走向目標點，$F_r$

epF_{rep}Frep​為斥力，使無人車遠離障礙物點。
引力場的計算公式如下：
$U_{att}=\frac {1}{2}K_{att}(q-q_g)^2$Uatt​=21​Katt​(q−qg​)2
上式中， $K_{att}$Katt​為引力增益常數， $q_g$qg​為目標點的位置。根據上式可以計算出引力的大小為：
$F_{att}=-\nabla U_{att}(q)=-K_{att}(q-q_g)$Fatt​=−∇Uatt​(q)=−Katt​(q−qg​)
$F_{att}$Fatt​的方向指向目標點，與無人車當前位置到目標點位置的距離成線性關係。$F_{}$

attF_{att}Fatt​可以分解為x軸方向的力與y軸方向的力：
$F_{attx}=-K_{att}(x-x_g)$Fattx​=−Katt​(x−xg​)
$F_{atty}=-K_{att}(y-y_g)$Fatty​=−Katt​(y−yg​)
斥力場的計算公式如下：
$U_{rep}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ \frac {1}{2}K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})^2, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Urep​={0,21​Krep​(ρobs​(q)1​−ρ0​1​)2,​ifρobs​(q)≥ρ0​ifρobs​(q)<ρ0​​
其中$K_{rep}$Krep​為斥力增益常數，$\rho_{obs}(q)=||q-q_{obs}||$ρobs​(q)=∣∣q−qobs​∣∣，$q_{obs}$qobs​為障礙物座標。$\rho_0$ρ0​為最大影響距離，若障礙物與無人車距離大於$\rho_0$ρ0​，則將斥力場視為0。設定$\rho_0$ρ0​可以消除離無人車較遠的障礙物對無人車軌跡的影響，$\rho_0$ρ0​的值取決於無人車速度與加速度大小。與引力計算相似，斥力的計算公式為：
$F_{rep}=-\nabla U_{rep}(q)=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{q-q_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frep​=−∇Urep​(q)={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣q−qobs​​,​ifρobs​(q)≥ρ0​ifρobs​(q)<ρ0​​
$F_{rep}$Frep​的方向指向障礙物點的反方向，$F_{rep}$Frep​可以分解為x軸方向的力與y軸方向的力，如下所示：
$F_{repx}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{x-x_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frepx​={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣x−xobs​​,​ifρobs​(q)≥ρ0​ifρobs​(q)<ρ0​​
$F_{repy}=\begin {cases} 0, &amp; \text {if $ \rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{y-y_{obs}}{||q-q_{obs}||}, &amp; \text{if $ \rho_{obs}(q)&lt;\rho_{0}$} \end{cases}$Frepy​={0,Krep​(ρobs​(q)1​−ρ0​1​)(ρobs2​(q)1​)∣∣q−qobs​∣∣y−yobs​​,​ifρobs​(q)≥ρ0​ifρobs​(q)<ρ0​​
以上的斥力計算是針對單個障礙物而言，但是環境中往往有多個障礙物，所以斥力場與斥力應該是多個障礙物對無人車的斥力場和與斥力和，則：
$U(q)=U_{att}(q)+\sum_{i=1}^n U_{rep}(q)$U(q)=Uatt​(q)+i=1∑n​Urep​(q)
$F(q)=F_{att}(q)+\sum_{i=1}^n F_{rep}(q)$F(q)=Fatt​(q)+i=1∑n​Frep​(q)
其中n為障礙物個數。無人車受力情況如下圖所示，其中F為無人車所受合力。

2、人工勢場法缺陷

(1)容易陷入區域性極小值。當無人車所受引力與斥力大小相等、方向相反時，無人車所受合力為0，此時無人車陷入了局部極小值點。在區域性極小值點周圍的各個位置的合力都指向區域性極小值，導致無人車在區域性極小值點周圍振盪，無法自行走出該區域。
(2)統人工勢場法引力部分與無人車和目標點位置距離成正比，當無人車距離目標點較遠時，引力部分過大，可能導致無人車與障礙物相撞。
(3)目標點附近有障礙物時，無人車可能無法到達目標點。

3、人工勢場法改進方法

3.1、人工勢場法引力場改進

傳統人工勢場法引力部分與無人車和目標點位置距離成正比，當無人車距離目標點較遠時，引力部分過大，可能導致無人車與障礙物相撞。針對這一問題，對引力場公式進行改進，具體如下。
引力場的大小由無人車與目標點的距離決定，當值非常大時，引力將會變得非常大，則斥力相對於引力而言就會變得非常小，這就會導致無人車與障礙物相撞。因此，有必要對引力場加以改進，公式如下：
$U_{rep}=\begin {cases} \frac{1}{2}K_{att}(q-q_g)^2, &amp; \text {if $(q-q_g)\leq d$} \\ dK_{att}(q-q_g), &amp; \text{if $ (q-q_g)&gt; d$} \end{cases}$Urep​={21​Katt​(q−qg​)2,dKatt​(q−qg​),​if(q−qg​)≤dif(q−qg​)>d​
$U_{rep}=\begin {cases} -K_{att}(q-q_g), &amp; \text {if $(q-q_g)\leq d$} \\ -dK_{att}\frac {q-q_g}{||q-q_g||}, &amp; \text{if $ (q-q_g)&gt; d$} \end{cases}$Urep​={−Katt​(q−qg​),−dKatt​∣∣q−qg​∣∣q−qg​​,​if(q−qg​)≤dif(q−qg​)>d​
其中，d為閾值，當無人車與目標點距離小於等於d時，引力與距離成正比；當無人車與目標點距離大於d時，將引力視為常數；

3.2、模擬退火法

3.2.1、模擬退火法原理

當人工勢場法陷入區域性極小值時，在當前點$x$x處選擇一個隨機點$x_1$x1​，然後根據上面計算勢場函式的公式，分別計算出點$x$x與點$x_1$x1​處的勢場$U(x)$U(x)與$U(x_1)$U(x1​)，如果滿足$U(x_1)\leq U(x)$U(x1​)≤U(x)，則接受點$x_1$x1​作為下一個點；如果滿足$U(x_1)&gt;U(x)$U(x1​)>U(x)，則計算概率$P=e^{-\frac{\Delta}{T}},\Delta=U(x_1)-U(x)$P=e−TΔ​,Δ=U(x1​)−U(x)，並以概率$P$P接受該點作為下一個點。同時$T$T以一定的方式減小,一般$T(t)=\alpha T(t-1),0.85&lt;\alpha&lt;1$T(t)=αT(t−1),0.85<α<1。重複以上步驟直到逃離區域性極小值。
模擬退火法的演算法步驟如下：
1、設定$x=S$x=S($S$S區域性極小值或者起點)。
2、選擇退火策略，設定$T=T_0$T=T0​。
3、While($T\geq T_f$T≥Tf​並且沒有逃離區域性極小值)
3.1、產生一個隨機點 $x_1=x+\Delta x$x1​=x+Δx($\Delta x$Δx為點$x$x附近一個隨機點，距離點$x$x一個步長)。
3.2、計算$U(x_1)$U(x1​)，即點$x_1$x1​處的勢能。
3.3、設定$\Delta=U(x_1)-U(x)$Δ=U(x1​)−U(x)。
3.4、如果$\Delta \leq 0$Δ≤0，設定$x=x_1$x=x1​。
3.5、如果$\Delta&gt;0$Δ>0，以概率P設定$x=x_1$x=x1​，其中$P=e^{-\frac{\Delta}{T}},\Delta=U(x_1)-U(x)$P=e−TΔ​,Δ=U(x1​)−U(x)（具體為： 設定一個隨機概率a，當P>a時，設定$x=x_1$x=x1​，反之，這個隨機點為失敗的隨機點，再重新設定隨機點）
3.6、如果$U(x_1)\leq U(S)$U(x1​)≤U(S)，成功逃離區域性極小值。
4、如果沒有逃離區域性極小值，返回失敗；否則，返回成功。

在實際的程式設計過程中，不是以$U(x_1)\leq U(S)$U(x1​)≤U(S)為標誌判斷是否程式逃離區域性極小值，而是計算當前點與進入區域性極小值時的點之間的距離，若該距離大於設定值，則認為逃離區域性極小值。通常設定值為步長的5-10倍，本程式設定值為步長的5倍。當無人車陷入區域性極小值時啟用模擬退火法。而判斷無人車是否陷入區域性極小值是根據無人車是否陷入振盪或者所受合力是否為0，判斷是否陷入振盪則是根據第j次與第j-2次小車的位置判斷，若這兩個點距離小於步長的0.1倍，則認為小車陷入振盪。
基於模擬退火法的人工勢場法演算法流程圖如下：

3.2.2、模擬結果

在模擬實驗中，將障礙物看成半徑為0.5的圓，其他引數的設定如下表所示：

下圖為沒有改進前的人工勢場法：

下圖為改進後的人工勢場法，其中藍色部分為傳統人工勢場法規劃出的路徑，綠色部分為基於模擬退火法的人工勢場法：

4、程式碼解釋與連結

1、執行main.m即可，程式碼裡面有詳細的備註，大家應該都可以看得懂，且在main.m檔案開頭備註部分有我的QQ號，歡迎大家一起交流。
2、在main.m最下面部分有對路徑的優化演算法，路徑優化有兩個演算法，分別check_lineA.m和check_lineB.m。為由於樓主最近比較忙，所以沒有時間更新對優化路徑演算法的解釋，有興趣的小夥伴可以自己先看看。
3、沒有改進前的人工勢場法：https://download.csdn.net/download/chengchang1234/11139704
基於模擬退火法的人工勢場法：https://download.csdn.net/download/chengchang1234/11239864