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

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は0でOKです.

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

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

また,緑の点(大きいほう)は近づけたい位置x_{base}です.

以上のベイズ最適化を,死角(×)が無くなるまで繰り返します.

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

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

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

したがって,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()

実行結果

上のコードを実行すれば,以下のような結果が得られると思います.

また,上で説明したような方法で「2回目以降のベイズ最適化では範囲を絞る」ことで,計算時間は短くなったようです.

下のグラフは,黒い線が常に同じ範囲を探索,赤い線が範囲を絞って探索した結果です.横軸は設定した数の死角点を見つけるまでに要した一度のベイズ最適化における試行回数,縦軸は設定した数の死角点を見つけるまでに要する計算時間です.

最後に

もっといい方法はたくさんあると思います.ベイズ最適化をどこかに応用してみたかったので今回はこのような方法で行いました.

コードは,後日GitHubに載せ,この記事にリンクを張る予定です.

経路として完成するには,今回求めた各姿勢の位置を結ぶ必要があります.余裕があれば,別の記事で書こうと思います.

コメント

タイトルとURLをコピーしました