ベイズ最適化でロボットの警備姿勢経路計画【Python】

この記事の内容

・「ロボットが死角を覗くには，どの場所からどの方向に向いたら効率がいいか？」という問題を解く

・ベイズ最適化による解法のコードを記載(PythonのBayesian Optimization使用)

・この記事では，ロボットの姿勢は出すが経路（姿勢の順番）は出さないので注意

この記事のプログラムを実行すれば，以下のようなロボットの姿勢が得られます．

黒い丸が覗きたい点（死角）で，ロボット（四角）の視野に入ると赤色になります．

ロボットの視野は黄色い扇形で，ロボットの数は一つです．図では，1回目，2回目，3回目の姿勢を同時に表示しています．

この例の場合は，3回の姿勢で80%の死角を覗いた状態です．

問題設定
解法
実行結果
最後に

問題設定

まず，以下のように環境に覗きたい点（×）を置きます．覗きたい点は，死角となっている点とも言えます．

我々が望むことは，

・死角に置かれた点（×）を視野にできるだけに入れたい

・できるだけ最初の位置から離れてほしくない

です．

「できるだけ最初の位置から離れてほしくない」のは，ロボットには効率よく移動して，最後には同じ位置に戻ってきてほしいからです．

ロボットは指定した死角（×）を視野に入れるまで何回か違う体勢をとり，その各姿勢（位置と方向）を求めます．

解法

この問題を，ベイズ最適化を用いて解きます．

ベイズ最適化とは，簡単に言うと目的関数（最小化もしくは最大化したいもの）を最小化もしくは最大化するような解（今回の場合，ロボットの位置と方向）を効率よく求める手法です．

一度目のベイズ最適化

目的関数

一度のベイズ最適化を行った（一つの姿勢を求めた）ときに

・死角に置かれた点（×）を視野にできるだけに入れたい

かつ

・できるだけ最初の位置から離れてほしくない

ので，最大化したい目的関数は以下のように表せます．

${\rm maximize} \quad N(x)-\beta L(x,x_{base})$

$N(x)$ はロボットの姿勢（位置 $x$ ，方向 $\theta$ ）を取ったときに視野に入った死角（×）の数で， $L(x,x_{base})$ はロボットの位置 $x$ と最初の位置 $x_{base}$ の距離です．

$\beta$ は重みで，良い感じに設定します．ロボットの位置が色々な場所に行っても特に問題ないのであれば， $\beta$ は０でOKです．

例えば，ベイズ最適化によって姿勢を一つだけ求めれば，以下のようになります．

水色～紫色の点はベイズ最適化で探している過程で，その結果，上図のような姿勢が最も黒い点（死角）を視野に入れることができるという結果が出ました．おそらく，最適解に近いと思います．

また，緑の点（大きいほう）は近づけたい位置 $x_{base}$ です．

以上のベイズ最適化を，死角（×）が無くなるまで繰り返します．

二回目以降のベイズ最適化

上のベイズ最適化を２回繰り返すと，例えば以下のような状態になります．

しかし，同じ範囲を何度もベイズ最適化で探索するのは非効率です．なぜなら，残りの死角点がたくさんある場所の周辺に解はありそうなので，その辺に絞って探した方がいいからです．

したがって，2回目以降は以下のように残りの死角点の重心 $x_{G}\pm\sigma$ の範囲に絞って探索を行います． $\sigma$ は標準偏差です．単位を合わせるために標準偏差を選びましたが，分散でも何でもいいと思います．とにかく，残りの死角点の重心付近で探します．

今回は，Pythonのベイズ最適化ライブラリ『Bayesian Optimization』を使います．

以前，『Bayesian Optimization』によるベイズ最適化の記事を書きました．

今回の記事も↑の記事を基本としていますから，以降で説明する内容がよく分からない場合は↑の記事をご覧ください．

プログラム（コード）

上で説明したことを実行するコードは以下の通りです．

コードに関する詳しい解説は省きますが，ページの一番下にあるコメント欄に質問を頂ければ，できるだけお答えしたいと思います．

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#ドローン=ロボットです

import math
import numpy as np
from bayes_opt import BayesianOptimization
from matplotlib import pyplot as plt
import random
from matplotlib import cm
import matplotlib.patches as patches
import matplotlib.patches as pat
import copy

try_ = 1

x1_max = 80
x1_min = 10
x2_max = 80
x2_min = 10
x3_max = 360 #ドローンの向き(度)
x3_min = 0

View_Range = 25 #ドローンの視野の長さ
View_Angle = 90 #ドローンの視野の角度(度)

#各種ノード-------
#近づけたいドローンノード．
before_drone_nodes = [[0, 5, 5]]
#死角ノード
blind_nodes = [[i, 50+random.uniform(-20,20), 50+random.uniform(-20,20)] for i in range(20)]
#障害物ノード
obs_nodes = [[i, 50+random.uniform(-20,20), 20+random.uniform(-10,10)] for i in range(30)]
#----------------
n = 0 #近くしたいドローンノードの番号（これまでの重心や地上ロボットの位置など）

def Obstacle(obs_nodes): #障害物を生成
    x1 = [x[1] for x in obs_nodes]
    x2 = [x[2] for x in obs_nodes]
    xo1_min = min(x1)
    xo1_max = max(x1)
    xo2_min = min(x2)
    xo2_max = max(x2)
    obs = np.array([[1, xo1_min, xo2_min], #[番号,x座標,y座標]
                    [2, xo1_max, xo2_min],
                    [3, xo1_max, xo2_max],
                    [4, xo1_min, xo2_max]])
    return [obs]

def Intusion_link(nodei, nodej): #侵入判定
    #Falseなら侵入してない
    for k in range(len(obs)):
        if nodei[0] == nodej[1]:
            xcu, xcd = 100000000, 1000000000
        else:
            xcu = ((nodei[0]-nodej[0])*obs[k][3][2] - (nodej[1]*nodei[0] - nodei[1]*nodej[0])) \
            /(nodei[1] - nodej[1])
            xcd = ((nodei[0]-nodej[0])*obs[k][0][2] - (nodej[1]*nodei[0] - nodei[1]*nodej[0])) \
            /(nodei[1] - nodej[1])

        if nodei[0] == nodej[0]:
            ycr, ycl = 100000000, 1000000000
        else:
            ycr = ((nodei[0]-nodej[0])*obs[k][1][1] + (nodej[0]*nodei[0] - nodei[0]*nodej[0])) \
            /(nodei[0] - nodej[0])
            ycl = ((nodei[0]-nodej[0])*obs[k][0][1] + (nodej[0]*nodei[0] - nodei[0]*nodej[0])) \
            /(nodei[0] - nodej[0])

        if min(nodei[0], nodej[0]) - 0.001 <= xcu <= max(nodei[0], nodej[0]) + 0.001 \
        and min(nodei[0], nodej[0]) - 0.001 <= obs[k][2][2] <= max(nodei[0], nodej[0]) + 0.001 \
        and obs[k][0][1] <= xcu <= obs[k][1][1]:
            return True

        if min(nodei[0], nodej[0]) - 0.001 <= xcd <= max(nodei[0], nodej[0]) + 0.001 \
        and min(nodei[0], nodej[0]) - 0.001 <= obs[k][0][2] <= max(nodei[0], nodej[0]) + 0.001 \
        and obs[k][2][1] <= xcd <= obs[k][3][1]:
            return True

        if min(nodei[0], nodej[0]) - 0.001 <= ycr <= max(nodei[0], nodej[0]) + 0.001 \
        and min(nodei[0], nodej[0]) - 0.001 <= obs[k][2][1] <= max(nodei[0], nodej[0]) + 0.001 \
        and obs[k][1][2] <= ycr <= obs[k][2][2]:
            return True

        if min(nodei[0], nodej[0]) - 0.001 <= ycl <= max(nodei[0], nodej[0]) + 0.001 \
        and min(nodei[0], nodej[0]) - 0.001 <= obs[k][0][1] <= max(nodei[0], nodej[0]) + 0.001 \
        and obs[k][0][2] <= ycl <= obs[k][3][2]:
            return True
    return False

def Intusion_node(node): #ノードが障害物内部ならTrue.nodeは番号なし
    for k in range(len(obs)):
        if obs[k][1][1] > node[0] > obs[k][0][1] \
        and obs[k][2][2] > node[1] > obs[k][0][2]:
            return True
    return False

def Coordinate_n(n, nodes):
    xn1 = nodes[n][1]
    xn2 = nodes[n][2]
    return xn1, xn2

def Hit_sector(center_x, center_y, target_x, target_y, start_angle, end_angle, radius):
    dx = target_x - center_x
    dy = target_y - center_y
    sx = math.cos(math.radians(start_angle))
    sy = math.sin(math.radians(start_angle))
    ex = math.cos(math.radians(end_angle))
    ey = math.sin(math.radians(end_angle))

    # 円の内外判定
    if dx ** 2 + dy ** 2 > radius ** 2:
        return False

    # 扇型の角度が180を超えているか
    if sx * ey - ex * sy > 0:
        # 開始角に対して左にあるか
        if sx * dy - dx * sy < 0:
            return False
        # 終了角に対して右にあるか
        if ex * dy - dx * ey > 0:
            return False
        # 扇型の内部にあることがわかった
        return True
    else:
        # 開始角に対して左にあるか
        if sx * dy - dx * sy >= 0:
            return True
        # 終了角に対して右にあるか
        if ex * dy - dx * ey <= 0:
            return True
        # 扇型の外部にあることがわかった
        return False

def Find_blind_nodes(x1,x2,x3,i):
    #ドローンが座標(x1,x2)のとき,
    #ある死角ノードiが見えればTrueを返す
    xi1, xi2 = Coordinate_n(i, blind_nodes)
    start_angle = x3 - View_Angle/2
    end_angle = x3 + View_Angle/2
    if Hit_sector(x1, x2, xi1, xi2, start_angle, end_angle, View_Range) == True \
    and Intusion_link([x1, x2], [xi1, xi2]) == False:
        return True
    return False

def Number_of_blind_nodes(x1, x2, x3):
    N = 0
    for i in range(len(blind_nodes)):
        if Find_blind_nodes(x1,x2,x3,i) == True:
            N += 1
    return N

def Number_of_Find_nodes(x1, x2, x3):
    N = 0
    Find_nodes = []
    for i in range(len(blind_nodes)):
        if Find_blind_nodes(x1,x2,x3,i) == True:
            N += 1
            Find_nodes.append(blind_nodes[i])
    return N, Find_nodes #最適ノードのためだけ

def Distance(x1, x2, n, nodes):
    xn1, xn2 = Coordinate_n(n, nodes)
    return np.sqrt((x1-xn1)**2+(x2-xn2)**2)

def obs_and_blind_delete(obs, blind_nodes): #obs内部にある死角ノードを取り除く
    del_list = []
    for i in range(len(blind_nodes)):
        for k in range(len(obs)):
            if obs[k][1][1] > blind_nodes[i][1] > obs[k][0][1] \
            and obs[k][2][2] > blind_nodes[i][2] > obs[k][0][2]:
                del_list.append(i)
    for d in del_list:
        for j in range(len(blind_nodes)):
            if blind_nodes[j][0] == d:
                del blind_nodes[j]
                break
    return blind_nodes

def Center_of_nodes(nodes): #番号付きノードの重心
    sum_x, sum_y = 0, 0
    N = len(nodes)
    for i in range(N):
        sum_x += nodes[i][1]
        sum_y += nodes[i][2]
    center_of_nodes = [100, sum_x/N, sum_y/N]
    return center_of_nodes

def Sigma(nodes): #標準偏差（2次元）
    xg = Center_of_nodes(nodes)
    sum_1, sum_2 = 0, 0
    for i in range(len(nodes)):
        sum_1 += (nodes[i][1] - xg[1])**2
        sum_2 += (nodes[i][2] - xg[2])**2
    sum_1 /= len(nodes)
    sum_2 /= len(nodes)
    sig1, sig2 = np.sqrt(sum_1), np.sqrt(sum_2)
    sig = [101, sig1, sig2]
    return xg, sig

def f(x1, x2, x3):
    global try_
    print("Try : " , try_ )
    #ドローンが障害物内にないか
    if Intusion_node([x1, x2]) == True:
        I = 1
    else:
        I = 0
    #最適ノードから見えるノードの数
    N = Number_of_blind_nodes(x1, x2, x3)
    print('N = ', N)
    #近づけたいドローンノードとドローン最適場所の距離
    if len(sol) == 0:
        R = Distance(x1, x2, n, before_drone_nodes)
    else:
        center_of_before_nodes = Center_of_nodes(sol)
        R = Distance(x1, x2, n, [center_of_before_nodes])
    beta = 0.1 - len(sol)*0.005
    gamma = 10**8
    score = N - beta*R - gamma*I
    print('y :', score)

    try_ += 1

    return score

def plot_field(bo, sol, blind_nodes_original, blind_nodes):
    # Figureを作成
    fig = plt.figure(figsize=(5, 5))
    ax = fig.add_subplot(111)

    #死角ノードプロット
    for i in range(len(blind_nodes_original)):
        xb1, xb2 = Coordinate_n(i, blind_nodes_original)
        plt.scatter(xb1, xb2, c='red', s=30)
    plt.scatter(xb1, xb2, c='red', s=1, label='FIND')
    for i in range(len(blind_nodes)):
        xb1, xb2 = Coordinate_n(i, blind_nodes)
        plt.scatter(xb1, xb2, c='black', s=30)
    plt.scatter(xb1, xb2, c='black', s=1, label='BLIND')
    #近づけたい点
    xn1, xn2 = Coordinate_n(n, before_drone_nodes)
    plt.scatter(xn1, xn2, c='green', s=30, label='BASE')

    #最適解
    for i in range(len(sol)):
        #最適ノード
        cmap = plt.get_cmap("tab10") #色を変化させるために必要
        max_x1, max_x2, max_x3 = sol[i][1], sol[i][2], sol[i][3]
        plt.scatter(max_x1, max_x2, color=cmap(i), s=50, marker="s", label='OPTIMIZE'+str(i+1))
        #ドローンの視野
        tr = pat.Wedge(center=(max_x1, max_x2), r=View_Range,
                 theta1=max_x3-View_Angle/2, theta2=max_x3+View_Angle/2, color="yellow", alpha = 0.3)
        ax.add_patch(tr)
        ax.set_aspect('equal')

    #obstacle
    for i in range(len(obs_nodes)):
        xo1, xo2 = Coordinate_n(i, obs_nodes)
        plt.scatter(xo1, xo2, c='green', s=2)
    plt.scatter(xo1, xo2, c='green', s=5, label='OBSTACLE')
    for k in range(len(obs)):
        ax.fill_between([obs[k][0][1], obs[k][1][1]], obs[k][0][2], obs[k][2][2], facecolor='k',alpha=0.3)

#以下，メイン------------------------
#obstacle生成
global obs
obs = Obstacle(obs_nodes)
#global blind_nodes
blind_nodes = obs_and_blind_delete(obs, blind_nodes) #obs内部にある死角ノードを取り除く
print('blind_nodes', blind_nodes)

#死角ノード
blind_nodes_original = copy.deepcopy(blind_nodes)
sol = []
s = 0
Find_percent = 80 #発見したい死角の割合(%)
while len(blind_nodes) >= int((100-Find_percent)/100*len(blind_nodes_original)):
    # 探索するパラメータと範囲を決める
    xg, sig = Sigma(blind_nodes)
    x1_min_p = xg[1] - sig[1]
    x1_max_p = xg[1] + sig[1]
    x2_min_p = xg[2] - sig[2]
    x2_max_p = xg[2] + sig[2]
    pbounds = {'x1': (x1_min_p, x1_max_p), 'x2': (x2_min_p, x2_max_p),\
                'x3': (x3_min, x3_max)}
    # 探索対象の関数と、探索するパラメータと範囲を渡す
    bo = BayesianOptimization(f=f, pbounds=pbounds)
    # 最大化する
    bo.maximize(init_points=10, n_iter=20)

    max_x1 = bo.max['params']['x1']
    max_x2 = bo.max['params']['x2']
    max_x3 = bo.max['params']['x3']
    max_y = bo.max['target']
    sol.append([s, max_x1, max_x2, max_x3, max_y])
    N, opt_nodes = Number_of_Find_nodes(max_x1, max_x2, max_x3)
    print('opt_nodes', opt_nodes, N ,'個', s+1,'回目')
    for o in opt_nodes:
        for j in range(len(blind_nodes)):
            if blind_nodes[j][0] == o[0]:
                del blind_nodes[j]
                break
    print('残り :', len(blind_nodes) , 'blind_nodes', blind_nodes)
    s += 1
    print('OPTIMIZE:', s,'th END')
    #input('PLEASE Enter IF coutinue')

print('sol = ', sol)
# 結果をグラフに描画する
plot_field(bo, sol, blind_nodes_original, blind_nodes)
# グラフを表示する
plt.legend(bbox_to_anchor=(0, 1), loc='upper left', borderaxespad=0, fontsize=8)
plt.grid()
plt.show()