JavaScriptを有効にしてください

PythonとCasADiを使ったDirect Single Shooting法による最適制御

 ·   6 min read

はじめに

Pythonと最適化ライブラリCasADiを使って、Direct Single Shooting法と呼ばれる手法によって最適制御問題を解きました。対象とした例題は斜方投射(物体を斜め方向に上げる)で、指定の時刻・距離に物体を到達させる最小の初速度を求めます。

Direct Single Shooting法は以下の特徴を持つ最適制御手法です。

  • 制御の開始から終了まで、時刻を$t_0, t_1, …, t_f$と(等間隔に)分割する
  • 分割された時刻ごとに、その時間幅内で入力$u(t)$の値は一定とする
  • 時間幅ごとに状態$x(t)$を数値積分により求める

CasADiとDirect Single Shooting法に関する詳細は、それぞれ以下の記事を参考にしてください。

CasADiとIPOPTで非線形計画問題を解く – Helve Tech Blog
PythonとCasADiで常微分方程式を解く – Helve Tech Blog
Direct Single Shooting法による最適制御 – Helve Tech Blog

この記事で使用したPythonとライブラリのバージョンは以下の通りです。

ソフトウェア バージョン
Python 3.8.8
NumPy 1.20.1
CasADi 3.5.5
IPOPT 3.12.3
Matplotlib 3.3.4

CasADiとIPOPTがインストールされていない場合、以下でインストールします。

pip install casadi
pip install ipopt

斜方投射の運動方程式

まず、斜方投射の運動方程式を示します。斜方投射は、物体に初速度を与えて空中に投げ上げる運動のことです。

斜方投射の開始点を原点として、水平方向に$x$軸、鉛直方向に$y$軸をとります。物体の質量を$m$とします。また、物体には速度に比例した空気抵抗が働くとして、その係数を$k(>0)$とします。

このとき、$x$方向と$y$方向の運動方程式は、それぞれ以下のようになります。

$$m \ddot{x} + k \dot{x} = 0 \\ m \ddot{y} + k \dot{y} + mg = 0$$

ただし、$g$は重力加速度です。

projectile motion

次に、CasADiで運動方程式を扱えるように、$\dot{z}=f(z)$という1階微分方程式の形に式を変形します。
ここで、
$$z=[x, y, \dot{x}, \dot{y}]^\top$$
とおくと、運動方程式は以下のようになります。
$$\dot{z} = \left[ \begin{array}{c} \dot{x} \\ \dot{y} \\ -\frac{k}{m}\dot{x} \\ -\frac{k}{m}\dot{y} -g \end{array} \right] $$

斜方投射のPythonコード

斜方投射をCasADiを使って解く、Pythonコードを示します。

斜方投射のパラメータと微分方程式モデル

まず、斜方投射のパラメータと微分方程式モデルを設定します。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
import numpy as np
import casadi as ca
import matplotlib.pyplot as plt

Ts = 1.0 # サンプリング周期[s]
N = 10 # サンプリング点数

m = 1 # 質量[kg]
k = 0.5 # 空気抵抗係数[(N・s)/m]
g = 9.81 # 重力加速度[m/(s^2)]

# 斜方投射を微分方程式で記述する
z = ca.MX.sym('z', 4) # x, y, x_dot, y_dot
zdot = ca.vertcat(z[2], z[3], -k/m*z[2], -k/m*z[3]-g)

dae = {'x':z, 
       'ode':zdot}
opts = {'tf':Ts}
F = ca.integrator('F', 'cvodes', dae, opts)

サンプリング周期を1[s], サンプリング点数をN=10とするので、シミュレーション全体の時間は10秒です。また、物体の質量と空気抵抗は適当な値としています。
optstfには、微分方程式を実行する時間を指定しています。

最適化問題の定式化

次に、最適化問題を定式化します。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
u = ca.MX.sym('U', 2) # x, y方向の初速[m/s]

Xk = ca.vertcat(ca.MX([0, 0]), u)
for i in range(N):
    Fk = F(x0=Xk) # Tsだけ斜方投射をシミュレーションする(微分方程式を解く)
    Xk = Fk['xf']

J = ca.sum1(u**2) # 目的関数
prob = {'f': J, 'x': u, 'g': Xk[:2]}
solver = ca.nlpsol('solver', 'ipopt', prob)
sol = solver(x0=[1, 1], lbx=[0, 0], ubx=[ca.inf, ca.inf], 
             lbg=[100, 0], ubg=[100, 0])

uは最適化問題の未知変数ベクトルで、x, y方向の初速になります。
その次のfor文の内側で、サンプリング周期Tsだけ斜方投射の微分方程式を解くことをN回だけ繰り返します。

目的関数Jは、速度エネルギーを最小化すること、すなわち$\dot{x}_0^2 + \dot{y}_0^2$とします。

また、未知変数ベクトルuの初期値はx0=[1, 1]とし、下限値をlbx=[0, 0], 上限値をubx=[ca.inf, ca.inf]としています。

さらに、最終時刻t=10[s]に物体がx=100[m], y=0[m]の位置にあるものとします。これは制約'g': Xk[:2]として最終時刻の位置をとり、下限値lbg=[100, 0], 上限値ubg=[100, 0]で与えます。

また、最適化問題を解くために非線形ソルバIPOPTを使用しています。

projectile problem

ここまでのコードを実行すると、以下のログが表示され、最適化問題を解くことができます。
実行時間は78.15msでした。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

This is Ipopt version 3.12.3, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:        2
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:        2

Total number of variables............................:        2
                     variables with only lower bounds:        2
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:        2
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0 2.0000000e+000 1.55e+002 1.00e+000  -1.0 0.00e+000    -  0.00e+000 0.00e+000   0
   1 8.7980408e+003 2.78e-005 7.81e+001  -1.0 7.81e+001    -  1.25e-002 1.00e+000h  1
   2 8.7980379e+003 3.86e-010 1.72e-003  -1.0 1.40e-005    -  9.95e-001 1.00e+000h  1

Number of Iterations....: 2

                                   (scaled)                 (unscaled)
Objective...............:  8.7980378964751490e+003   8.7980378964751490e+003
Dual infeasibility......:  1.4210854715202004e-014   1.4210854715202004e-014
Constraint violation....:  3.8573360205111218e-010   3.8573360205111218e-010
Complementarity.........:  0.0000000000000000e+000   0.0000000000000000e+000
Overall NLP error.......:  3.8573360205111218e-010   3.8573360205111218e-010


Number of objective function evaluations             = 3
Number of objective gradient evaluations             = 3
Number of equality constraint evaluations            = 3
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 3
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 2
Total CPU secs in IPOPT (w/o function evaluations)   =      0.016
Total CPU secs in NLP function evaluations           =      0.046

EXIT: Optimal Solution Found.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  |        0 (       0)        0 (       0)         3
       nlp_g  |  16.00ms (  5.33ms)  15.60ms (  5.20ms)         3
  nlp_grad_f  |        0 (       0)        0 (       0)         4
  nlp_hess_l  |  30.00ms ( 15.00ms)  31.33ms ( 15.67ms)         2
   nlp_jac_g  |        0 (       0)        0 (       0)         4
       total  |  78.00ms ( 78.00ms)  78.15ms ( 78.15ms)         1

また、最適解を表示します。

1
2
u_opt = sol['x']
print(u_opt)

実行結果

1
[50.3392, 79.1455]

すなわち、$x, y$軸方向の初速はそれぞれ50.3[m/s], 79.1[m/s]となりました。

最適化結果のプロット

最後に最適化結果をプロットします。

最適化問題を定式化したときと同様に、斜方投射の微分方程式をサンプリング周期Tsだけ解くことをN回繰り返し、位置と速度のベクトル$z$の時刻歴データz_historyを得ます。

1
2
3
4
5
6
7
z_history = [[0, 0] + u_opt.toarray().flatten().tolist()]
for i in range(N):
    Fk = F(x0=z_history[-1])
    z_history += [Fk['xf'].toarray().flatten().tolist()]

z_history = np.array(z_history)
time = np.arange(0, N+Ts, Ts)

まず、物体の位置$x, y$の時刻歴データをプロットします。時刻10[s]で、物体はx=100[m], y=0[m]に到達していることが分かります。

1
2
3
4
5
6
7
fig, ax = plt.subplots(nrows=2, sharex=True)
ax[0].plot(time, z_history[:, 0])
ax[0].set_ylabel('x [m]')
ax[1].plot(time, z_history[:, 1])
ax[1].set_xlabel('time [s]')
ax[1].set_ylabel('y [m]')
plt.show()

projectile solution

最後に、x-y平面に物体の軌跡をプロットします。空気抵抗があるため、綺麗な放物線とはなっていません。

1
2
3
4
5
fig, ax = plt.subplots()
ax.plot(z_history[:, 0], z_history[:, 1])
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
plt.show()

projectile trajectory solution

まとめ

PythonとCasADiを使って、斜方投射を例題として、与えられた時刻と位置に物体を到達させるために必要なエネルギー(初速)を最小化しました。手法にはDirect Single Shooting法を用いました。

参考

CasADiの公式リファレンス

シェアする

Helve
WRITTEN BY
Helve
関西在住、電機メーカ勤務のエンジニア。Twitterで新着記事を配信中です

サイト内検索