Pythonの最適化モジュールの1つであるcvxpyを用いて、モデル予測制御を行ってみました。題材としては、制御系のHello,World!と言っても過言ではない倒立振子を選んでみました。
モデル予測制御と倒立振子
基本的には以下のサイトにあるサンプルコードがメインになっています。
ちなみに、今回扱う倒立振子はWikipediaの倒立振子の中に書かれている台車駆動型倒立振子というものを仮定します。運動方程式は以下の通りです。各変数は基本はWikipediaの記事と統一してありますが、Fとuだけが違っています。
(M + m)\ddot{x} - ml\ddot{\theta}cos\theta + ml\dot{\theta}^2sin\theta = u \\
l\ddot{\theta} - gsin\theta = \ddot{x}cos\theta
θは0に近いという仮定をもとに、近似を行い、モデルを線形方程式で表します。
\frac{d}{dt}
\begin{pmatrix}
x \\
\dot{x} \\
\theta \\
\dot{\theta}
\end{pmatrix}
=
\begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & 0 & \frac{mg}{M} & 0 \\
0 & 0 & 0 & 1 \\
0 & 0 & \frac{(M + m)g}{lM} & 0
\end{pmatrix}
\begin{pmatrix}
x \\
\dot{x} \\
\theta \\
\dot{\theta}
\end{pmatrix}
+
\begin{pmatrix}
0 \\
\frac{1}{M} \\
0 \\
\frac{1}{lM}
\end{pmatrix}
u
A =
\begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & 0 & \frac{mg}{M} & 0 \\
0 & 0 & 0 & 1 \\
0 & 0 & \frac{(M + m)g}{lM} & 0
\end{pmatrix}
Δt
+
\begin{pmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{pmatrix} \\
B =
\begin{pmatrix}
0 \\
\frac{1}{M} \\
0 \\
\frac{1}{lM}
\end{pmatrix}
Δt
{\bf x}_{t+1} = A{\bf x}_{t} + Bu
Δtは制御周期です。Δtが0.1[s]ならば0.1秒毎に現在の状態に基づいて制御入力を変えるということになります。
コード
以下のように、線形方程式を定義します。
mpc.py
import time
from cvxpy import *
import numpy as np
import matplotlib.pyplot as plt
n_state = 4 # 状態の数
m_state = 1 # 制御入力の数
T = 100 # 何ステップ先まで予測するかを決める
#simulation parameter
delta_t = 0.01
M = 1.0 # [kg]
m = 0.3 # [kg]
g = 9.8 # [m/s^2]
l = 0.6 # [m]
# Model Parameter
A = np.array([
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, m * g / M, 0.0],
[0.0, 0.0, 0.0, 1.0],
[0.0, 0.0, g * (M + m) / (l * M), 0.0]
])
A = np.eye(n_state) + delta_t * A
B = np.array([
[0.0],
[1.0 / M],
[0.0],
[1.0 / (l * M)]
])
B = delta_t * B
# 倒立振子の初期状態
# 今回はすべてが0に収束するよう目指す
x_0 = np.array([
[-0.02],
[0.0],
[0.02],
[0.0]
])
x = Variable(n_state, T+1)
u = Variable(m_state, T)
実際にコスト関数を定義し、最適化を行うのが以下のコードです。cost_arrを用いて各状態に対する重みを調整します。ここら辺のコードはほぼモデル予測制御とサンプルコードからコピーしてきただけです。
mpc.py
cost_arr = np.array([
[1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 0.1, 0.0],
[0.0, 0.0, 0.0, 0.1]
])
states = []
for t in range(T):
# コスト関数の値が小さくなるような配列uを求める
cost = sum_squares(cost_arr*x[:,t+1]) + sum_squares(0.1*u[:,t])
# 制約式（線形方程式と制御入力の限界値）を与える
constr = [x[:,t+1] == A*x[:,t] + B*u[:,t],
norm(u[:,t], 'inf') <= 20.0]
states.append( Problem(Minimize(cost), constr) )
# sums problem objectives and concatenates constraints.
prob = sum(states)
# 制約をさらに2つ追加する
# 最後の状態(Tステップ後の状態)はすべてが0、すなわち理想の状態とすること
# そして、現在の状態x0は事実としてあるので、制約となる
prob.constraints += [x[:,T] == 0, x[:,0] == x_0]
start = time.time()
result=prob.solve(verbose=True)
elapsed_time = time.time() - start
print("calc time:{0} [sec]".format(elapsed_time))
# 発散した場合は制御不能として終了
if result == float("inf"):
print("Cannot optimize")
import sys
sys.exit()
ちなみに、上記のコードが制御1回分に相当します。制御は常に行うものなので、上記の処理を繰り返し行うことで、倒立振子は倒れることなく、立った状態を維持できるのです。
繰り返しモデル予測制御を行うコードは以下のようになります。
mpc.py
cnt = 0
# 制御1000回分行う
while cnt < 1000:
states = []
for t in range(T):
cost = sum_squares(cost_arr*x[:,t+1]) + sum_squares(0.1*u[:,t])
constr = [x[:,t+1] == A*x[:,t] + B*u[:,t],
norm(u[:,t], 'inf') <= 20.0]
states.append( Problem(Minimize(cost), constr) )
# sums problem objectives and concatenates constraints.
prob = sum(states)
prob.constraints += [x[:,T] == 0, x[:,0] == x_0]
start = time.time()
result=prob.solve(verbose=False)
elapsed_time = time.time() - start
if result == float("inf"):
print("Cannot optimize")
import sys
sys.exit()
# 最適とされる制御入力の配列を取得
good_u_arr = np.array(u[0,:].value[0,:])[0]
# 制御入力を入れて、次の状態を得る
# 今回はノイズを考えない
x_next = np.dot(A, x_0) + B * good_u_arr[0]
x_0 = x_next
cnt += 1
# 左から順にx(台車の位置), \dot{x}(台車の速度), rad(振子の角度), \dot{rad}(振子の角速度)
print(x_next.reshape(-1))
倒立振子の初期状態によっては制御不能となることが多くあります。
モデル予測制御は制御性能が高い反面、計算量が多いという問題があるようです。私のPCもかなり温まってきました。