【Python】PythonRoboticsのpotential_field_planningの理解

# はじめに
[PythonRopbotics](https://atsushisakai.github.io/PythonRobotics/)を知っているでしょうか。
[arXiv](https://arxiv.org/abs/1808.10703)に投稿されてもいるPythonでロボットのアルゴリズムを書いているOSSです。
この方の[ブログ](https://myenigma.hatenablog.com/about)もあります。ロボットの研究を始めてからよく読ませて参考にさせてもらっています。

# ポテンシャル法
自分も[こちら](https://qiita.com/koichi_baseball/items/0a6eef85be8d700f6125)の記事で書きましたが、ロボットの経路計画法でポテンシャル法というものがあります。
ロボットの障害物回避などに使われ、障害物と目的地にポテンシャル関数を定義してそれの勾配にそった経路で目的地までの経路を出という手法です。

この記事ではPythonRobotics内のポテンシャル法のプログラムを理解することを目的に自分なりに解説していきます。

ポテンシャル法の参考論文
https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf

# プログラム
## main関数とパラメータ

numpyとmatplotlibをimportしています。
パラメータとして、
・目的地のポテンシャルの重み
・障害物のポテンシャルの重み
・ポテンシャル法を計算するエリアの幅
を指定しています。

main関数内では、スタート、ゴール、障害物の位置、ポテンシャル法のグリッド、ロボットのサイズを指定しています。
これはコメントでわかりますね。

```Python
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はそれぞれ以下のようになります。

```math
minx,y = 障害物の座標で最も小さい座標 - エリアの幅 / 2  \\

maxx,y = 障害物の座標で最も大きい座標 - エリアの幅 / 2
```

それらを利用してx,yそれぞれの計算範囲に使われる`xw,yw`を出しています。
次に上記の値とグリッドからポテンシャルを格納する配列(```pmap```)を用意します。

その後にゴールからのポテンシャル(```ug```)と障害物によるポテンシャル(```uo```)を計算し、それの合計(```uf```)をそれぞれの座標の```pmap```に格納する、となっています。

```Python
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にして、以下の式に代入します。この式の返り値が障害物によるポテンシャルです。

```math
0.5 × ETA(ポテンシャルの重み) × (\frac{1}{dq} - \frac{1}{rr})^2
```

また、```dq```がロボットのサイズより大きかった場合は障害物によるポテンシャルは0になります。

```Python
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座標上を上下左右に動くようになっています。
以下の部分です。

```Python
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
```

この`inx,iny`座標のポテンシャルを`pmap`から取得し、上下左右どこに進めば最もポテンシャルが小さいかを求め、その方向に進む、という進み方をしています。
進んだ後の座標とゴールの距離を計算し、その距離がグリッドの値より小さくなるまで繰り返しています。

動く部分の解説は以上となります。

```Python
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/