はじめに
PythonRopboticsを知っているでしょうか。
arXivに投稿されてもいるPythonでロボットのアルゴリズムを書いているOSSです。
この方のブログもあります。ロボットの研究を始めてからよく読ませて参考にさせてもらっています。
ポテンシャル法
自分もこちらの記事で書きましたが、ロボットの経路計画法でポテンシャル法というものがあります。
ロボットの障害物回避などに使われ、障害物と目的地にポテンシャル関数を定義してそれの勾配にそった経路で目的地までの経路を出という手法です。
この記事ではPythonRobotics内のポテンシャル法のプログラムを理解することを目的に自分なりに解説していきます。
ポテンシャル法の参考論文
https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf
プログラム
main関数とパラメータ
numpyとmatplotlibをimportしています。
パラメータとして、
・目的地のポテンシャルの重み
・障害物のポテンシャルの重み
・ポテンシャル法を計算するエリアの幅
を指定しています。
main関数内では、スタート、ゴール、障害物の位置、ポテンシャル法のグリッド、ロボットのサイズを指定しています。
これはコメントでわかりますね。
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]
show_animation = True
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, 10.0] # obstacle x position list [m]
oy = [25.0, 15.0, 26.0, 25.0, 10.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()
calc_potential_field関数(ポテンシャル場の計算)
この関数は全体のポテンシャル場を計算するものです。
この関数の引数は
・ゴールの座標(x,y)
・障害物の座標(x,y)
・グリッド
・ロボットのサイズ
となっています。
ox,oyには障害物の座標がリストで入っています。したがってminx,yとmaxx,yはそれぞれ以下のようになります。
minx,y = 障害物の座標で最も小さい座標 - エリアの幅 / 2 \\
maxx,y = 障害物の座標で最も大きい座標 - エリアの幅 / 2
それらを利用してx,yそれぞれの計算範囲に使われるxw,ywを出しています。
次に上記の値とグリッドからポテンシャルを格納する配列(pmap)を用意します。
その後にゴールからのポテンシャル(ug)と障害物によるポテンシャル(uo)を計算し、それの合計(uf)をそれぞれの座標のpmapに格納する、となっています。
def calc_potential_field(gx, gy, ox, oy, reso, rr):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
maxy = max(oy) + AREA_WIDTH / 2.0
xw = int(round((maxx - minx) / reso))
yw = int(round((maxy - miny) / reso))
# calc each potential
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
x = ix * reso + minx
for iy in range(yw):
y = iy * reso + miny
ug = calc_attractive_potential(x, y, gx, gy)
uo = calc_repulsive_potential(x, y, ox, oy, rr)
uf = ug + uo
pmap[ix][iy] = uf
return pmap, minx, miny
calc_attractive_potentialとcalc_repulsive_potential関数(引力/斥力ポテンシャルの計算)
calc_attractive_potential関数(引力ポテンシャルの計算)
この関数はゴールからの引力ポテンシャルを計算する関数です。
これは単純にnumpyのhypotを使用して2点間の距離を出して、重みをかけているのみとなっています。
calc_repulsive_potential関数(斥力ポテンシャルの計算)
この関数は障害物による斥力ポテンシャルを計算する関数です。
oxとoyすべての要素それぞれに対してnumpyのhypotを使用して2点間の距離を計算し、最も小さい要素(ここではインデックス(minid)で紐づかせている)の2点間の距離(dq)を計算しています。
そして、dqがロボットのサイズ以下であった場合
0.1以下であった場合は0.1にして、以下の式に代入します。この式の返り値が障害物によるポテンシャルです。
0.5 × ETA(ポテンシャルの重み) × (\frac{1}{dq} - \frac{1}{rr})^2
また、dqがロボットのサイズより大きかった場合は障害物によるポテンシャルは0になります。
def calc_attractive_potential(x, y, gx, gy):
return 0.5 * KP * np.hypot(x - gx, y - gy)
def calc_repulsive_potential(x, y, ox, oy, rr):
# search nearest obstacle
minid = -1
dmin = float("inf")
for i, _ in enumerate(ox):
d = np.hypot(x - ox[i], y - oy[i])
if dmin >= d:
dmin = d
minid = i
# calc repulsive potential
dq = np.hypot(x - ox[minid], y - oy[minid])
if dq <= rr:
if dq <= 0.1:
dq = 0.1
return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
else:
return 0.0
potential_field_planning関数
これはポテンシャル法の経路計画部分を計算している関数になります。
上記のpotential_field_planning関数からpmap,minx,minyを受け取ります。
コメントの#search path直下部分でスタートとゴールの距離dと計算される座標ix,iy,gix,giyを計算しています。
(ix,iy,gix,giyの式については完全に理解しきれていないです。。。後日修正予定)
その後、animationの設定をして、while d >= resoの部分からゴールに到達するまでのループがあります。
get_motiobn_model関数は動き方の配列を返す関数であり、x,y座標上を上下左右に動くようになっています。
以下の部分です。
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
このinx,iny座標のポテンシャルをpmapから取得し、上下左右どこに進めば最もポテンシャルが小さいかを求め、その方向に進む、という進み方をしています。
進んだ後の座標とゴールの距離を計算し、その距離がグリッドの値より小さくなるまで繰り返しています。
動く部分の解説は以上となります。
def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
# calc potential field
pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr)
# search path
d = 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()
while d >= reso:
minp = float("inf")
minix, miniy = -1, -1
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]):
p = float("inf") # outside area
else:
p = pmap[inx][iny]
if minp > p:
minp = p
minix = inx
miniy = iny
ix = minix
iy = miniy
xp = ix * reso + minx
yp = iy * reso + miny
d = np.hypot(gx - xp, gy - yp)
rx.append(xp)
ry.append(yp)
if show_animation:
plt.plot(ix, iy, ".r")
plt.pause(0.01)
print("Goal!!")
return rx, ry
さいごに
まだまだ理解しきれていない部分もありますが、こんな感じで理解しようとしてみました。
もし間違っているところがあったらコメントいただけると嬉しいです。
参考
PythonRoboticsのページ
https://github.com/AtsushiSakai/PythonRobotics
potential_field_planningの全文はここにあります。
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
↑の方がやっているブログ
https://myenigma.hatenablog.com/