人工勢場法是一種原理比較簡單的移動機器人路徑規劃算法，它將目標點位置視做勢能最低點，將地圖中的障礙物視為勢能高點，計算整個已知地圖的勢場圖，然后理想情況下，機器人就像一個滾落的小球，自動避開各個障礙物滾向目標點。

• 參考：
  源代碼potential_field_planning.py
  課件CMU RI 16-735機器人路徑規劃第4講：人工勢場法

具體地，目標點的勢能公式為：

其中寫道，為防止距離目標點較遠時的速度過快，可以采用分段函數的形式描述，后文會進行展示。
而障礙物的勢能表示為：

即在障礙物周圍某個范圍內具有高勢能，范圍外視障礙物的影響為0。
最終將目標點和障礙物的勢能相加，獲得整張勢能地圖：

以下是參考鏈接中的源代碼，比較容易懂：

"""Potential Field based path plannerauthor: Atsushi Sakai (@Atsushi_twi)Ref: https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf"""from collections import deque import numpy as np import matplotlib.pyplot as plt# Parameters KP = 5.0 # attractive potential gain ETA = 100.0 # repulsive potential gain AREA_WIDTH = 30.0 # potential area width [m] # the number of previous positions used to check oscillations OSCILLATIONS_DETECTION_LENGTH = 3show_animation = Truedef calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):"""計算勢場圖gx,gy: 目標坐標ox,oy: 障礙物坐標列表reso: 勢場圖分辨率rr: 機器人半徑sx,sy: 起點坐標"""# 確定勢場圖坐標范圍：minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0# 根據范圍和分辨率確定格數：xw = int(round((maxx - minx) / reso))yw = int(round((maxy - miny) / reso))# calc each potentialpmap = [[0.0 for i in range(yw)] for i in range(xw)]for ix in range(xw):x = ix * reso + minx # 根據索引和分辨率確定x坐標for iy in range(yw):y = iy * reso + miny # 根據索引和分辨率確定x坐標ug = calc_attractive_potential(x, y, gx, gy) # 計算引力uo = calc_repulsive_potential(x, y, ox, oy, rr) # 計算斥力uf = ug + uopmap[ix][iy] = ufreturn pmap, minx, minydef calc_attractive_potential(x, y, gx, gy):"""計算引力勢能：1/2*KP*d"""return 0.5 * KP * np.hypot(x - gx, y - gy)def calc_repulsive_potential(x, y, ox, oy, rr):"""計算斥力勢能：如果與最近障礙物的距離dq在機器人膨脹半徑rr之內：1/2*ETA*(1/dq-1/rr)**2否則：0.0"""# search nearest obstacleminid = -1dmin = float("inf")for i, _ in enumerate(ox):d = np.hypot(x - ox[i], y - oy[i])if dmin >= d:dmin = dminid = i# calc repulsive potentialdq = np.hypot(x - ox[minid], y - oy[minid])if dq <= rr:if dq <= 0.1:dq = 0.1return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2else:return 0.0def get_motion_model():# dx, dy# 九宮格中 8個可能的運動方向motion = [[1, 0],[0, 1],[-1, 0],[0, -1],[-1, -1],[-1, 1],[1, -1],[1, 1]]return motiondef oscillations_detection(previous_ids, ix, iy):"""振蕩檢測：避免“反復橫跳”"""previous_ids.append((ix, iy))if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):previous_ids.popleft()# check if contains any duplicates by copying into a setprevious_ids_set = set()for index in previous_ids:if index in previous_ids_set:return Trueelse:previous_ids_set.add(index)return Falsedef potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):# calc potential fieldpmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)# search pathd = np.hypot(sx - gx, sy - gy)ix = round((sx - minx) / reso)iy = round((sy - miny) / reso)gix = round((gx - minx) / reso)giy = round((gy - miny) / reso)if show_animation:draw_heatmap(pmap)# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event',lambda event: [exit(0) if event.key == 'escape' else None])plt.plot(ix, iy, "*k")plt.plot(gix, giy, "*m")rx, ry = [sx], [sy]motion = get_motion_model()previous_ids = deque()while d >= reso:minp = float("inf")minix, miniy = -1, -1# 尋找8個運動方向中勢場最小的方向for i, _ in enumerate(motion):inx = int(ix + motion[i][0])iny = int(iy + motion[i][1])if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:p = float("inf") # outside areaprint("outside potential!")else:p = pmap[inx][iny]if minp > p:minp = pminix = inxminiy = inyix = minixiy = miniyxp = ix * reso + minxyp = iy * reso + minyd = np.hypot(gx - xp, gy - yp)rx.append(xp)ry.append(yp)# 振蕩檢測，以避免陷入局部最小值：if (oscillations_detection(previous_ids, ix, iy)):print("Oscillation detected at ({},{})!".format(ix, iy))breakif show_animation:plt.plot(ix, iy, ".r")plt.pause(0.01)print("Goal!!")return rx, rydef draw_heatmap(data):data = np.array(data).Tplt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)def main():print("potential_field_planning start")sx = 0.0 # start x position [m]sy = 10.0 # start y positon [m]gx = 30.0 # goal x position [m]gy = 30.0 # goal y position [m]grid_size = 0.5 # potential grid size [m]robot_radius = 5.0 # robot radius [m]# 以下障礙物坐標是我進行修改后的，用來展示人工勢場法的困于局部最優的情況：ox = [15.0, 5.0, 20.0, 25.0, 12.0, 15.0, 19.0, 28.0, 27.0, 23.0, 30.0, 32.0] # obstacle x position list [m]oy = [25.0, 15.0, 26.0, 25.0, 12.0, 20.0, 29.0, 28.0, 26.0, 25.0, 28.0, 27.0] # obstacle y position list [m]if show_animation:plt.grid(True)plt.axis("equal")# path generation_, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)if show_animation:plt.show()if __name__ == '__main__':print(__file__ + " start!!")main()print(__file__ + " Done!!")

人工勢場法的一項主要缺點就是可能會落入局部最優解，下圖是源代碼運行后的結果：

下面是在我添加了一些障礙物后，人工勢場法困于局部最優解的情況：雖然還沒有到達目標點，但勢場決定了路徑無法再前進。

需要注意的是，源代碼在計算目標點勢場的時候，使用的是某x，y位置距離目標點的距離的一次項，并未如課件中所示使用二次項，也是為了使勢場變化沒有那么快。下面是按照課件中所說，使用距離的二次項運行的結果，我們可以看到，為運行正常，KP需要調得很低：

KP = 0.1 def calc_attractive_potential(x, y, gx, gy):"""計算引力勢能：1/2*KP*d^2"""return 0.5 * KP * np.hypot(x - gx, y - gy)**2

正常運行：

困在局部最優點：

可以從勢場圖中看到，引力變化較上一個例子快得多。

最后，我們將程序修改成上面課件截圖中所示的分段函數：

KP = 0.25 dgoal = 10 def calc_attractive_potential(x, y, gx, gy):"""計算引力：如課件截圖"""dg = np.hypot(x - gx, y - gy)if dg<=dgoal:U = 0.5 * KP * np.hypot(x - gx, y - gy)**2else:U = dgoal*KP*np.hypot(x - gx, y - gy) - 0.5*KP*dgoalreturn U

正常運行：

困于局部最優：

可以看到引力勢場分段的效果。

總結

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

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