はじめに
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$は重力加速度です。
次に、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秒です。また、物体の質量と空気抵抗は適当な値としています。
optsのtfには、微分方程式を実行する時間を指定しています。
最適化問題の定式化
次に、最適化問題を定式化します。
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を使用しています。
ここまでのコードを実行すると、以下のログが表示され、最適化問題を解くことができます。
実行時間は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)
|
実行結果
すなわち、$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()
|
最後に、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()
|
まとめ
PythonとCasADiを使って、斜方投射を例題として、与えられた時刻と位置に物体を到達させるために必要なエネルギー（初速）を最小化しました。手法にはDirect Single Shooting法を用いました。
参考
CasADiの公式リファレンス
