摩擦、空気抵抗のある最急降下曲線を求めようとしてまあまあできた

前回 (https://qiita.com/drafts/9a8456b511d7ae0277cc/edit) の続き

$\{a_n\}, \{b_n\}$ の最適化にTensorflowを使ってみようと思って勉強中。
今簡単にできる最適化として、ベイズ最適化(参考：https://www.kumilog.net/entry/bayesian-optimization )をやってみた。

#ソースコード

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint 
from skopt import gp_minimize
from skopt.plots import plot_convergence

%matplotlib inline
%config InlineBackend.figure_format='retina'
plt.style.use('seaborn-darkgrid')
c_cycle=("#ea5415","#005192","#ffdf00","#1d7a21","#88593b","#737061")

前回と違うのはskoptをimportしているだけ。

spaces = [
    (-1.0, 1.0), #s0
    (-1.0, 1.0), #s1
    (-1.0, 1.0), #s2
    (-1.0, 1.0), #c0
    (-1.0, 1.0), #c1
    (-1.0, 1.0) #c2
]

最適化したいパラメタの取りうる値。とりあえずcos,sinともに3次まで最適化することにする。(ただし、ゴールにたどり着く拘束条件によりモードの数は増える。)この範囲に狭める必要はないし、もっと効率的に探索する方法はあるだろうが、何も考えずに設定した。

def f(coeffs):

    def sin_series(coeffs, period, x):
        value_at_x = 0
        for i in range(len(coeffs)):
            value_at_x += coeffs[i]*np.sin(np.pi*(i+1)*x/period)
        return value_at_x

    def cos_series(coeffs, period, x):
        value_at_x = 0
        for i in range(len(coeffs)):
            value_at_x += coeffs[i]*np.cos(np.pi*(i+1)*x/period)
        return value_at_x

    # a curve through (0,0) and (goal_x, goal_y)
    def func(sin_coeff, cos_coeff, goal_x, goal_y, x):
        return goal_y/goal_x*x + sin_series(sin_coeff, goal_x, x) + cos_series(cos_coeff, goal_x, x)

    # 1st-derivertive of func
    def d_func(sin_coeff, cos_coeff, goal_x, goal_y, x):
        new_sin_coeff = [] 
        new_cos_coeff = [] 
        for i in range(len(sin_coeff)):
            new_sin_coeff.append(sin_coeff[i]*(i+1)*np.pi/goal_x)
        for i in range(len(cos_coeff)):
            new_cos_coeff.append(-cos_coeff[i]*(i+1)*np.pi/goal_x)
        return goal_y/goal_x + cos_series(np.array(new_sin_coeff),goal_x,x) + sin_series(np.array(new_cos_coeff),goal_x,x)

    #2nd-derivertive of func
    def d2_func(sin_coeff, cos_coeff, goal_x, goal_y, x):
        new_sin_coeff = [] 
        new_cos_coeff = [] 
        for i in range(len(sin_coeff)):
            new_sin_coeff.append(-sin_coeff[i]*(i+1)*(i+1)*np.pi*np.pi/(goal_x*goal_x))
        for i in range(len(cos_coeff)):
            new_cos_coeff.append(-cos_coeff[i]*(i+1)*(i+1)*np.pi*np.pi/(goal_x*goal_x))
        return sin_series(np.array(new_sin_coeff),goal_x,x) + cos_series(np.array(new_cos_coeff),goal_x,x)

    def extend_cos_coeff(cos_coeff):
        cos_coeff_odd  = sum(cos_coeff[0::2])
        cos_coeff_even = sum(cos_coeff[1::2])    
        if len(cos_coeff)%2==0:
            cos_coeff.append(-cos_coeff_odd)
            cos_coeff.append(-cos_coeff_even)
        else:
            cos_coeff.append(-cos_coeff_even)
            cos_coeff.append(-cos_coeff_odd)
        return np.array(cos_coeff)

    def equation_of_motion(state, simulation_time, sin_coeff, cos_coeff, x1, y1, m, mu, k, g):
        x, v = state
        fp   = d_func(sin_coeff, cos_coeff,x1,y1,x)
        f2p  = d2_func(sin_coeff, cos_coeff,x1,y1,x)
        temp = 1.0/(1+fp*fp)
        kp   = k/m
        mufp = mu+fp

        dxdt = [v, - temp*mufp*f2p*v*v - temp*kp*(1+mufp*fp)*v - temp*mufp*g]
        return dxdt

    def get_result(anser, goal_x, goal_y, sin_coeff, cos_coeff, simulation_time):
        temp   = anser[anser[:,0]<goal_x]
        trajectory_x = temp[:,0]
        trajectory_y = np.array( list(map(lambda u: func(sin_coeff, cos_coeff, goal_x, goal_y, u), trajectory_x)) )
        time = simulation_time[:len(temp)]
        return trajectory_x, trajectory_y, time
    
    #parameters
    g      = 9.8 # gravity constant [m/s^2]
    k      = 0.   # air resistance coefficient
    mu     = 0.   # friction constant coefficient
    mass   = 1   # mass [kg]
    initial_state = [0.0, 0.0] #x0,v0

    simulation_time = np.linspace(0, 2, (2*1000)+1) # time array

    #define slope
    goal_x  = 1
    goal_y  = -1

    sin_coeff = coeffs[:int(len(coeffs)/2)]
    cos_coeff = coeffs[int(len(coeffs)/2):]
    cos_coeff = extend_cos_coeff(cos_coeff)

    #solve equation of motion
    phase_space      = odeint(equation_of_motion, initial_state, simulation_time, args=(sin_coeff, cos_coeff, goal_x, goal_y, mass, mu, k, g))
    xpos, ypos, tpos = get_result(phase_space, goal_x, goal_y, sin_coeff, cos_coeff, simulation_time)
    return tpos[-1]


res = gp_minimize(f, spaces, acq_func="gp_hedge", n_calls=300, random_state=1)
print(f'res.x: {res.x}')
print(f'res.fun: {res.fun}')
plt.rcParams["font.size"] = 12
plot_convergence(res)  

関数 f に最小化したい値を計算している。今回は目的地に到着した時間。
f内で使う関数、変数はfの中で定義しないといけないみたい？
res.xで最適化後の目的変数を返し、res.funでその時のパラメタをみることができる。
実行結果は

res.x: [-0.24776017762578972, -0.18317989330606743, -0.042950234171557855, 0.09064376170968291, 0.018254807238455628, -0.0739564962501097]
res.fun: 0.586

いい感じで最適化されている。

#最急降下曲線と最適化曲線の比較
とりあえず摩擦も空気抵抗も0にして、最急降下曲線らしきものが出るか試してみた。
恥ずかしながら、サイクロイドになるということは知っていても、サイクロイドのどういう部分を使った形状になるのかは全く知らなかった…

Googleさんに聞いてみて、
スタート地点とゴール地点が決まったときの最急降下曲線の形は
http://www.math.u-ryukyu.ac.jp/~tsukuda/lecturenotes/kaihoussh.pdf
の12ページあたりを参考に決めた。

最急降下曲線の具体的な形

リンク先のpdfから

与えられた２点 A,B をとおる最速降下線を作図する方法を見ておきましょう．
まず A,B を結ぶ直線をひきます．次に点 A を始点とする適当な大きさのサイクロイ
ドを描き，直線 AB との交点を C とします

とあるので、まず交点を求める。

from scipy import optimize
def cycloid(x):
    return y1*(x-np.sin(x))/x1-(np.cos(x)-1.0)

t = optimize.fsolve(cycloid,np.pi)[0]
val_x = t-np.sin(t)
val_y = -(y1/x1)*val_x

(x1,y1)がゴールの座標。ここでx座標の値と媒介変数の $\theta$ を混同していて少しはまった。

ac = np.sqrt(val_x**2+val_y**2)
ab = np.sqrt(x1**2+y1**2)
coef = ab/ac
theta = np.linspace(0,2*np.pi,100)
cx = coef*(theta-np.sin(theta))
cy = coef*(np.cos(theta)-1.0)

求める最急降下曲線が $\theta$ を媒介変数としてcx, cyとして得られた。

##最適化した斜面と最急降下曲線の比較

coef = list(res.x)
sn = np.array(res[:int(len(coef)/2)])
cn = np.array(res[int(len(coef)/2):])
cn = extend_cos_coeff(list(cn))
x  = np.linspace(0,x1,200)
f  = np.array( list(map(lambda u: func(sn,cn,x1,y1,u),    x)) )
plt.plot(cx,cy,label='steepest')
plt.plot(x, f, label="opt-slope")
plt.legend(loc='best')

まずまず近い関数形が得られた。