1.基本原理

人工勢場法的基本原理為，根據地圖內障礙物、目標點等的分布，構造一個人工勢場，無人機由勢能較高的位置向勢能較低的位置移動。就好比是一個電場，電場內不同位置的電勢能不同，對帶電物體產生的力也不同，但這個力對物體運動的影響一定是由高勢能到低勢能的。只不過電場內的勢能是物理上定義的，而人工勢場的勢能是根據我們的需求自己定義的。只需要定義合適的勢函數，使得障礙物附近的勢能大、目標點附近的勢能小，就可以引導無人機飛往目標點而原理障礙物。這種方法不依賴于全局的障礙物信息，可以實現局部范圍內的障礙物檢測+避障。

這里使用常見的勢函數定義，在python實現避障仿真，并參考文獻[^1]中方法，針對人工勢場法可能出現的局部極小值等問題進行改進。

2.勢函數和勢場

在單機飛行的避障中，對無人機產生影響的因素有障礙物和目標點，如果是多機編隊還需考慮機間的影響。這里只考慮障礙物產生的斥力場和目標點產生的引力場。

常見的斥力勢函數為
$$U_{rep}(P)=?\begin{array}{r}\frac{1}{2}\eta (\frac{1}{d(P,P_{ob})}?\frac{1}{Q}{)}^2,ifd(P,P_{ob})\le Q\\ 0,ifd(P,P_{ob})>Q\end{array}$$
其中$P$為無人機當前坐標，$P_{ob}$為被計算勢函數的障礙物坐標；$Q$為障礙物作用范圍，該范圍外的障礙物不會對無人機飛行產生影響；$\eta$為比例系數。

常見的引力勢函數為
$$U_{att}(P)=\frac{1}{2}\xi d^2(P,P_{goal})$$
其中$P$為無人機當前坐標，$P_{goal}$為目標點坐標，$\xi$為比例系數。

在以上勢函數的定義下，物體距離目標點越近，目標點產生的引力勢越小；物體距離障礙物越近，障礙物產生的引力勢越大。物體向勢較小的位置移動，故能接近目標點而遠離障礙物。

得到勢場內勢的分布后，可以通過兩種方法引導無人機飛行。一種為計算無人機周邊位置的勢，以勢能最小的位置作為下一位置，由于只使用位置控制而不考慮速度和加速度，故在復雜場景下使用這種方法，得到的仿真軌跡不平滑。

另一種方法為對勢能計算負梯度得到下降方向，以該方向作為加速度的方向，通過加速度-速度-位置的方法實現控制。具體計算方法為

$$\begin{array}{rl}{F}_{att}& =??U_{att}(P)=?\xi (P?P_{goal})\\ F_{rep}^i& =??U_{rep}(P)=?\begin{array}{r}\eta (\frac{1}{d(P,P_{ob})}?\frac{1}{Q})?\frac{1}{d(P,P_{ob}{)}^3}?(P?P_{ob}),ifd(P,P_{ob})\le Q\\ 0,ifd(P,P_{ob})>Q\end{array}\\ F_{rep}& =\sum _i{F}_{eqp}^i\end{array}\text{\hspace{1em}?}$$

3.方法改進

如果僅使用以上的方法，筆者遇到的主要問題有：

局部極小值，這是梯度下降算法中的一個常見問題。某一位置處勢能低于其周邊所有點，但是它并不是目標點，就好比是一個小球自上而下滾落，途中落入一個小坑，如下圖所示（圖片來源[^2]）。由于物體移動的趨勢為向勢能更低處，所以此時它無論如何是無法離開這個極小值點的。

或者是如下圖所示的情況，障礙物產生的斥力和目標點產生的引力方向相反，
物體沒有縱向的力，無法越過障礙物。

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-b5jM8Ghv-1658911680145)(https://cdn.jsdelivr.net/gh/kun-k/blogweb/imageimage-20220715112653149.png)]

目標點與障礙物過于接近時，物體總是無法到達。由于障礙物存在斥力，目標點存在引力，如果此時斥力大于引力，則物體只能在目標點附近徘徊。如果設置參數、調小引力，則可能導致物體在移動過程中與障礙物過近或撞上障礙物。

目標點產生的勢與距離正相關，物體距離目標點很遠時，引力遠大于斥力，導致斥力相對小、避障效果差。

接下來針對以上3個問題設計解決方案。

針對局部極小值問題，首先需對是否陷入局部極小進行檢測，方法為在時間$t$內計算物體的位移，如果該位移小于某一閾值，則認為陷入局部極小值。論文[^1]中提出的方法為，計算時間$t$內物體相對目標點的位移
$$V_{ra}=\frac{\Delta X}{t}=\frac{||P_0?P_G||?||P_1?P_G||}{t}$$
當$V_{ra}$小于一閾值時認為陷入局部極小值。

筆者嘗試了兩種解決方案。第一種為隨機游走，在陷入局部極小值后物體隨意改變移動方向一次，一段時間后再次進行判斷。在很多情況下，這種方法可以時物體最終走出局部極小值點，但是由于移動方向是隨機的，物體往往需要徘徊一段時間后才能離開。

第二種方法為論文[^1]中的方法，首先有計劃地改變斥力的方向。
$$F_{rep?n}^i=\left[\begin{array}{cc}cos\theta & ?sin\theta \\ sin\theta & cos\theta \end{array}\right]F_{rep}^i$$
即將每個斥力都逆時針旋轉一個角度$\theta$。同時對引力進行調整
$$\begin{gathered} K_v=\frac{3l}{2l+V_{ra}} \\ {K}_d=3?e^{?\frac{(||P?P_G||?0.5{)}^2}{2}}+1 \\ {K}_e\ge 1 \\ {F}_{att?n}=K_v{K}_d{K}_e{F}_{att} \end{gathered}$$
其中，$l$為移動步長，即無斥力影響下每個時間步的位移。而$V_{ra}$總是小于步長的，所以$K_v$總是大于1，且隨著$V_{ra}$的減小而增大。在局部極小中起到的效果為，當物體陷入局部極小，$V_{ra}$會減小，$K_v$增大，進而使引力的影響增大。同時斥力的方向改變，起到的效果示意如下，圖片取自參考論文中。

此外，針對$\theta$的選取，這里再做一些改進。論文中選擇了$\theta =60°$對上圖的情況進行測試，得到的效果很好。但是在很多情況下，固定角度$\theta$不能離開局部極小值，陷入局部游走，或得到的解不理想，如下圖所示的情況：

左側是在$\theta =60°$下求解的情況，右側為$\theta =?60°$，顯然右側的情況更好一些。考慮到$\theta$的作用為增加側向的力，使物體沿側向離開障礙物的趨勢更大，可以根據引力和斥力的方向設計$\theta$的正負。如下圖所示，沿逆時針方向，引力與斥力的角度小于180°時，斥力逆時針旋轉，否則順時針旋轉。

目標點與障礙物接近的問題，1.中的參數$K_d$可以解決該問題。

$K_d$與物體距離目標點的距離相關，物體距離目標點的距離接近$0.5$時，$K_d$的值會比較大，而物體遠離目標點時，$K_d$指數減小至1。繪制$K_d$關于距離的變換圖像為

故當物體距離目標點很近時，引力的作用顯著增大，障礙物的作用相對減小。可以根據實際需求適當調整$K_d$中的參數。

目標點產生的勢與距離成正相關，導致距離很遠時斥力的作用相對小，避障效果差。一種常見的解決方法為，為斥力勢乘以一個系數$||P_{ob},P_{goal}|{|}_2^k$，$k$可取2，此時相當于為斥力乘以障礙物到目標點的距離，使斥力大小也與距離相關。

4.實現過程和效果

經過以上改進后，$F_{rep}$和$F_{att}$的計算公式為：
$$\begin{gathered} K_v=\frac{3l}{2l+V_{ra}} \\ {K}_d=3?e^{?\frac{(||P?P_G||?0.5{)}^2}{2}}+1 \\ {K}_e\ge 1 \\ {F}_{att}=?K_v{K}_d{K}_e?U_{att}(P)=?\xi (P?P_{goal}) \end{gathered}$$

$F_{rep}$的計算公式為
$$\begin{gathered} {F}_{rep}^i=??U_{rep}(P)=?\begin{array}{r}\eta (\frac{1}{d(P,P_{ob})}?\frac{1}{Q})?\frac{d(P_{goal},P_{ob}{)}^2}{d(P,P_{ob}{)}^3}?(P?P_{ob}),ifd(P,P_{ob})\le Q\\ 0,ifd(P,P_{ob})>Q\end{array} \\ {F}_{rep}=\sum _i{F}_{eqp}^i \end{gathered}$$

局部極小值下的$F_{rep}$為

$$F_{rep?n}=\left[\begin{array}{cc}cos\theta & ?sin\theta \\ sin\theta & cos\theta \end{array}\right]F_{rep}$$
展示幾組結果：

將避障與之前已經實現的軌跡規劃結合，得到的結果為

5.代碼

''' python_avoid_APF.py 人工勢場法避障的python實現 '''import math import matplotlib.pyplot as plt import numpy as np import cv2''' P 初始位置 V 初始速度 P_aim 目標點 mymap 儲存有障礙物信息的地圖 Kg,Kr 避障控制器參數（引力，斥力） Q_search 搜索障礙物距離 epsilon 誤差上限 Vl 速率上限 Ul 控制器輸出上限 dt 迭代時間 ''' def avoid_APF(P_start, V_start, P_aim, mymap, Kg=0.5, kr=20,Q_search=20, epsilon=2, Vl=2, Ul=2, dt=0.2):def distance(A, B):return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2)def myatan(a, b):if a == 0 and b == 0:return Noneif a == 0:return math.pi / 2 if b > 0 else math.pi * 3 / 2if b == 0:return 0 if a > 0 else -math.piif b > 0:return math.atan(b / a) if a > 0 else (math.atan(b / a) + math.pi)return math.atan(b / a + 2 * math.pi) if a > 0 else math.atan(b / a) + math.pidef isClockwise(a, b):da = b - aif 0 < da < math.pi or -math.pi * 2 < da < -math.pi:return Falsereturn True# 讀取初始狀態P_start = np.array(P_start) # 初始位置pos_record = [P_start] # 記錄位置V_start = np.array(V_start).T # 初始速度# 地圖尺寸size_x = mymap.shape[0]size_y = mymap.shape[1]# 設置繪圖參數plt.axis('equal')plt.xlim(0, size_x)plt.ylim(0, size_y)# 繪制地圖（障礙物和航路點）for i in range(mymap.shape[0]):for j in range(mymap.shape[1]):if mymap[i][j] == 0:plt.plot(i, j, 'o', c='black')plt.plot([P_start[0], P_aim[0]], [P_start[1], P_aim[1]], 'o')# 計算周邊障礙物搜素位置direction_search = np.array([-2, -1, 0, 1, 2]) * math.pi / 4# 開始飛行pos_num = 0 # 已經記錄的位置的總數P_curr = P_start # 當前位置V_curr = V_startob_flag = False # 用于判斷局部極小值while distance(P_curr, P_aim) > epsilon:Frep = np.array([0, 0]) # 斥力angle_curr = myatan(V_curr[0], V_curr[1])for dir in direction_search:angle_search = angle_curr + dirfor dis_search in range(Q_search):P_search = [int(P_curr[0] + dis_search * math.sin(angle_search)),int(P_curr[1] + dis_search * math.cos(angle_search))]if not (0 <= P_search[0] < size_x and # 超出地圖范圍，地圖內障礙，均視作障礙物0 <= P_search[1] < size_y andmymap[int(P_search[0])][int(P_search[1])] == 1):d_search = distance(P_curr, P_search) # 被搜索點與當前點的距離Frep = Frep + \kr * (1 / d_search - 1 / Q_search) / (d_search ** 3) * \(P_curr - P_search) * (distance(P_search, P_aim) ** 2)breakFatt = -Kg * (P_curr - P_aim) # 計算引力if pos_num >= 1:# 計算上兩個時刻物體相對終點的位移，以判斷是否陷入局部極小值p0 = pos_record[pos_num - 1]p1 = pos_record[pos_num - 2]Vra = (distance(p0, P_aim) - distance(p1, P_aim)) / dtif abs(Vra) < 0.6 * Vl: # 陷入局部極小值if ob_flag == False:# 之前不是局部極小狀態時，根據當前位置計算斥力偏向角thetaangle_g = myatan(Fatt[0], Fatt[1])angle_r = myatan(Frep[0], Frep[1])if angle_r == None or angle_g == None:print('111')if isClockwise(angle_g, angle_r):theta = 15 * math.pi / 180else:theta = -15 * math.pi / 180ob_flag = True# 之前為局部極小，則繼續使用上一時刻的斥力偏向角thetaFrep = [math.cos(theta) * Frep[0] - math.sin(theta) * Frep[1],math.sin(theta) * Frep[0] + math.cos(theta) * Frep[1]]else: # 離開局部極小值ob_flag = Falsel = VlKv = 3 * l / (2 * l + abs(Vra))Kd = 15 * math.exp(-(distance(P_curr, P_aim) - 3) ** 2 / 2) + 1Ke = 3Fatt = Kv * Kd * Ke * Fatt # 改進引力U = Fatt + Frep # 計算合力if np.linalg.norm(U, ord=np.inf) > Ul: # 控制器輸出限幅U = Ul * U / np.linalg.norm(U, ord=np.inf)V_curr = V_curr + U * dt # 計算速度if np.linalg.norm(V_curr) > Vl: # 速度限幅V_curr = Vl * V_curr / np.linalg.norm(V_curr)P_curr = P_curr + V_curr * dt # 計算位置print(P_curr, V_curr, distance(P_curr, P_aim))pos_record.append(P_curr)pos_num += 1pos_record = np.array(pos_record).Tplt.plot(pos_record[0], pos_record[1], '--')plt.show()if __name__ == "__main__":# 讀取地圖圖像并二值化img = cv2.imread('map.png')gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)dst = cv2.dilate(dst, None, iterations=1)dst = cv2.erode(dst, None, iterations=4) / 255avoid_APF([0, 0], [1, 1], [80, 80], dst)

代碼中參數比較多，可能會有些亂，但是我都增加了注釋，思路也還可以，和上面說明的算法原理都是一致的。地圖我使用Windows畫圖工具簡單繪制，黑色表示障礙物。

目前還存在一些避障失敗的情況，也沒有設置安全距離，比較刁鉆的情況下可能會直接跨越障礙物，之后還會繼續調試改進。更加復雜的情況可以則使用路徑規劃，設置更多航路點再進行飛行。

也還沒有實現三維空間內的避障，但是根據算法原理來看，應該是可以直接擴展到三維空間內的，這部分等拿到AirSim中無人機障礙物檢測的接口后再繼續修改調試。

參考來源：

[1] Li H . Robotic Path Planning Strategy Based on Improved Artificial Potential Field[C]// 2020 International Conference on Artificial Intelligence and Computer Engineering (ICAICE). 2020.

[2]https://mp.weixin.qq.com/s/JwpQAXDw9Rt1vlDDIZJMXA

總結

以上是生活随笔為你收集整理的11(0)-AirSim+四旋翼仿真-人工势场法避障的全部內容，希望文章能夠幫你解決所遇到的問題。

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