目次
はじめに
先日、
凸最適化のモデリングツールCVXやCVXPY, CVXGENを紹介しましたが、
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
シンプルなモデル予測制御MPCの最適化問題であれば、
モデリングツールを使わずに、MPCの最適化式を
二次凸最適の標準形に変形して、
ソルバーのみで解くことができます。
myenigma.hatenablog.com
モデリングツールは、
任意の凸最適化問題を解くことができるので、
最適化問題を設計している段階では便利ですが、
モデリングコードを解析するために計算時間がかかってしまうので、
最適化システムを高速で回したい場合は、
モデリングの部分を自作実装したほうが良いです。
そこで、今回は非常にシンプルなMPC最適化問題を、
下記の資料をベースに
数式導出し、Pythonで実装してみましたので紹介したいと思います。
シンプルなMPC問題
今回の例では、
下記の標準的なMPC問題を解きたいとします。
x0は初期状態であるとします。
また今回の例題では、
下記の安定なシステムである
MPCパラメータを使うことにします。
A = np.matrix([[0.8, 1.0], [0, 0.9]])
B = np.matrix([[-1.0], [2.0]])
(nx, nu) = B.shape
N = 10
Q = np.eye(nx)
R = np.eye(nu)
入力制約のみのMPCモデリングの数式導出とPythonサンプルコード
数式導出
まず初めに、
入力制約条件のみのMPCモデリングの数式を導出したいと思います。
入力プロファイルを一つのベクトルとすると、
下記のように、線形方程式で状態ベクトルのプロファイルを計算することができます。
そこで、上記の線形方程式の考えから、
上記のMPCの問題は、
下記のような二次凸問題に変形することができます。
上記の式において、入力制約を追加しない場合は、
MPCの制約無しの解となります。
入力の最大値や最小値などの、
入力制約を入れたい場合は、
下記の式のようなに二次凸最適化の不等式制約を入れることで、
入力値の制約条件を追加することができます。
Pythonコード
上記の数式をPythonのnumpyで実装したのが、
下記のコードになります。
それぞれのMPCパラメータと、
入力制約(オプション)を入力すると、
最適なMPCの状態プロファイルと、
入力プロファイルを計算してくれます。
def opt_mpc_with_input_const(A, B, N, Q, R, P, x0, umax=None, umin=None):
"""
optimize model predictive control only with input constraints
(if you want to solve a problem with state constraints, you can use opt_mpc_with_state_const())
return
x: state
u: input
"""
(nx, nu) = B.shape
Ai = A
AA = Ai
for i in range(2, N + 1):
Ai = A * Ai
AA = np.vstack((AA, Ai))
AiB = B
BB = np.kron(np.eye(N), AiB)
for i in range(1, N):
AiB = A * AiB
BB += np.kron(np.diag(np.ones(N - i), -i), AiB)
RR = np.kron(np.eye(N), R)
QQ = scipy.linalg.block_diag(np.kron(np.eye(N - 1), Q), P)
H = (BB.T * QQ * BB + RR)
gx0 = BB.T * QQ * AA * x0
P = matrix(H)
q = matrix(gx0)
if umax is None and umin is None:
sol = cvxopt.solvers.qp(P, q)
else:
G = np.zeros((0, nu * N))
h = np.zeros((0, 1))
if umax is not None:
tG = np.eye(N * nu)
th = np.kron(np.ones((N * nu, 1)), umax)
G = np.vstack([G, tG])
h = np.vstack([h, th])
if umin is not None:
tG = np.eye(N * nu) * -1.0
th = np.kron(np.ones((N * nu, 1)), umin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])
G = matrix(G)
h = matrix(h)
sol = cvxopt.solvers.qp(P, q, G, h)
u = np.matrix(sol["x"])
xx = AA * x0 + BB * u
x = np.vstack((x0.T, xx.reshape(N, nx)))
return x, u
凸二次ソルバーとしては、
以前紹介したcvxoptを使いました。
myenigma.hatenablog.com
下記はモデリングツールcvxpyを使って、
MPCを解いた場合と、
上記の関数を使った場合の結果です。
下記のシミュレーションでは、
下記の入力制約を追加しています。
両方の出力で同じ結果が得られていることがわかります。
状態・入力制約を含んだMPCモデリングの数式導出とPythonサンプルコード
数式導出
前述の方法だと、入力制約は追加できますが、
状態制約が付けられないので、
別の方法で数式導出する必要があります。
まず初めに入力プロファイルと状態プロファイルを一つのベクトルZにまとめた場合、
下記の二次凸最適化の標準形に変形することができます。
入力制約と、状態制約は先程と同じように、
不等式制約とすることで、計算することができます。
Pythonコード
上記の数式をPythonのnumpyで実装したのが、
下記のコードにです。
それぞれのMPCパラメータと、
入力制約, 状態制約(それぞれオプション)を入力すると、
最適なMPCの状態プロファイルと、
入力プロファイルを計算してくれます。
def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=None):
"""
optimize MPC problem with state and (or) input constraints
return
x: state
u: input
"""
(nx, nu) = B.shape
H = scipy.linalg.block_diag(np.kron(np.eye(N), R), np.kron(np.eye(N - 1), Q), np.eye(P.shape[0]))
Aeu = np.kron(np.eye(N), -B)
Aex = scipy.linalg.block_diag(np.eye((N - 1) * nx), P)
Aex -= np.kron(np.diag([1.0] * (N - 1), k=-1), A)
Ae = np.hstack((Aeu, Aex))
be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) * x0
P = matrix(H)
q = matrix(np.zeros((N * nx + N * nu, 1)))
A = matrix(Ae)
b = matrix(be)
if umax is None and umin is None:
sol = cvxopt.solvers.qp(P, q, A=A, b=b)
else:
G, h = generate_inequalities_constraints_mat(N, nx, nu, xmin, xmax, umin, umax)
G = matrix(G)
h = matrix(h)
sol = cvxopt.solvers.qp(P, q, G, h, A=A, b=b)
fx = np.matrix(sol["x"])
u = fx[0:N * nu].reshape(N, nu).T
x = fx[-N * nx:].reshape(N, nx).T
x = np.hstack((x0, x))
return x, u
def generate_inequalities_constraints_mat(N, nx, nu, xmin, xmax, umin, umax):
"""
generate matrices of inequalities constrints
return G, h
"""
G = np.zeros((0, (nx + nu) * N))
h = np.zeros((0, 1))
if umax is not None:
tG = np.hstack([np.eye(N * nu), np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N * nu, 1)), umax)
G = np.vstack([G, tG])
h = np.vstack([h, th])
if umin is not None:
tG = np.hstack([np.eye(N * nu) * -1.0, np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N, 1)), umin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])
if xmax is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx)])
th = np.kron(np.ones((N, 1)), xmax)
G = np.vstack([G, tG])
h = np.vstack([h, th])
if xmin is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx) * -1.0])
th = np.kron(np.ones((N, 1)), xmin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])
return G, h
下記のグラフはcvxpyを使って、計算した結果の比較です。
下記のシミュレーションでは、
下記の入力制約と状態制約を追加しています。
モデリングツールを使った結果と
同じ結果が得られていることがわかります。
MPCのTrackingサンプルコード
上記のMPCのコードは、
すべて状態を0にするように制御する
いわゆるRegulatorのコードでしたが、
ある目標値に状態値を近づけるように制御する
いわゆるMPC Tracking の方法を紹介したいと思います。
数式の導出
Trackingの数式の導出に関しては、
下記の書籍の２章と３章を参考にして下さい。
Python サンプルコード
"""
MPC tracking sample code
author: Atsushi Sakai
"""
import numpy as np
import matplotlib.pyplot as plt
import scipy.linalg
from cvxopt import matrix
import cvxopt
DEBUG_ = False
def get_mat_psi(A, N):
psi = np.matrix(np.zeros((0, A.shape[1])))
for i in range(1, N + 1):
psi = np.vstack((psi, A ** i))
return psi
def get_mat_gamma(A, B, N):
(nx, nu) = B.shape
gamma = np.zeros((nx, nu)) + B
for i in range(1, N):
tmat = (A ** i) * B + gamma[-nx:, :]
gamma = np.vstack((gamma, tmat))
return gamma
def get_mat_theta(A, B, N):
AiB = B
(nx, nu) = B.shape
theta = np.kron(np.eye(N), AiB)
tmat = np.zeros((nx, 0))
for i in range(1, N):
t = np.zeros((nx, nu)) + B
for ii in range(1, i + 1):
t += (A ** ii) * B
tmat = np.hstack((t, tmat))
for i in range(1, N):
theta[i * nx:(i + 1) * nx, :i] += tmat[:, -i:]
return theta
def model_predictive_control(A, B, N, Q, R, T, x0, u0):
(nx, nu) = B.shape
du = np.matrix([0.0] * N).T
psi = get_mat_psi(A, N)
gamma = get_mat_gamma(A, B, N)
theta = get_mat_theta(A, B, N)
QQ = scipy.linalg.block_diag(np.kron(np.eye(N), Q))
RR = scipy.linalg.block_diag(np.kron(np.eye(N), R))
H = theta.T * QQ * theta + RR
g = - theta.T * QQ * (T - psi * x0 - gamma * u0)
P = matrix(H)
q = matrix(g)
sol = cvxopt.solvers.qp(P, q)
du = np.matrix(sol["x"])
fx = psi * x0 + gamma * u0 + theta * du
ffx = fx.reshape(N, nx)
ffx = np.vstack((x0.T, ffx))
u = np.cumsum(du).T + u0
return ffx, u
def test1():
print("start!!")
A = np.matrix([[0.8, 1.0], [0, 0.9]])
B = np.matrix([[-1.0], [2.0]])
(nx, nu) = B.shape
N = 50
Q = np.diag([1.0, 1.0])
R = np.eye(nu)
x0 = np.matrix([2.0, 1.0]).T
u0 = np.matrix([0.0])
T = np.matrix([1.0, 0.25] * N).T
x, u = model_predictive_control(A, B, N, Q, R, T, x0, u0)
tx = x0
rx = x0
for iu in u[:, 0]:
tx = A * tx + B * iu
rx = np.hstack((rx, tx))
if DEBUG_:
plt.plot(x[:, 0])
plt.plot(x[:, 1])
plt.plot(u[:, 0])
plt.grid(True)
plt.plot(rx[0, :].T, "xr")
plt.plot(rx[1, :].T, "xb")
plt.show()
for ii in range(len(x[0, :]) + 1):
for (i, j) in zip(rx[ii, :].T, x[:, ii]):
assert (i - j) <= 0.0001, "Error" + str(i) + "," + str(j)
target = T.reshape(N, nx)
for ii in range(len(x[0, :]) + 1):
assert abs(x[-1, ii] - target[-1, ii]) <= 0.3, "Error"
上記のコードを実行すると、
下記のようにMPCで目標値に追従させる
入力プロファイルを計算することができます。
すべてのPythonサンプルコード
すべてのPythonサンプルコードは、
下記のGithubリポジトリで公開しています
github.com
参考資料
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。
myenigma.hatenablog.com
