MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

シンプルなMPC最適化モデリングの数式導出とPythonサンプルコード

Model Predictive Control: Classical, Robust and Stochastic (Advanced Textbooks in Control and Signal Processing)

Model Predictive Control: Classical, Robust and Stochastic (Advanced Textbooks in Control and Signal Processing)

目次

はじめに

先日、

凸最適化のモデリングツールCVXやCVXPY, CVXGENを紹介しましたが、

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

シンプルなモデル予測制御MPCの最適化問題であれば、

モデリングツールを使わずに、MPCの最適化式を

二次凸最適の標準形に変形して、

ソルバーのみで解くことができます。

myenigma.hatenablog.com

 

モデリングツールは、

任意の凸最適化問題を解くことができるので、

最適化問題を設計している段階では便利ですが、

モデリングコードを解析するために計算時間がかかってしまうので、

最適化システムを高速で回したい場合は、

モデリングの部分を自作実装したほうが良いです。

 

そこで、今回は非常にシンプルなMPC最適化問題を、

下記の資料をベースに

数式導出し、Pythonで実装してみましたので紹介したいと思います。

 

シンプルなMPC問題

今回の例では、

下記の標準的なMPC問題を解きたいとします。

f:id:meison_amsl:20170205052109p:plain

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  # number of horizon
Q = np.eye(nx)
R = np.eye(nu)

 

入力制約のみのMPCモデリングの数式導出とPythonサンプルコード

数式導出

まず初めに、

入力制約条件のみのMPCモデリングの数式を導出したいと思います。

入力プロファイルを一つのベクトルとすると、

下記のように、線形方程式で状態ベクトルのプロファイルを計算することができます。

f:id:meison_amsl:20170207063125p:plain

 

そこで、上記の線形方程式の考えから、

上記のMPCの問題は、

下記のような二次凸問題に変形することができます。

f:id:meison_amsl:20170207063349p:plain

 

上記の式において、入力制約を追加しない場合は、

MPCの制約無しの解となります。

入力の最大値や最小値などの、

入力制約を入れたい場合は、

下記の式のようなに二次凸最適化の不等式制約を入れることで、

入力値の制約条件を追加することができます。

f:id:meison_amsl:20170207072420p:plain

f:id:meison_amsl:20170207072427p:plain

f:id:meison_amsl:20170207072438p:plain

 

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

    # calc AA
    Ai = A
    AA = Ai
    for i in range(2, N + 1):
        Ai = A * Ai
        AA = np.vstack((AA, Ai))

    # calc BB
    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"])

    # recover x
    xx = AA * x0 + BB * u
    x = np.vstack((x0.T, xx.reshape(N, nx)))

    return x, u

 

凸二次ソルバーとしては、

以前紹介したcvxoptを使いました。

myenigma.hatenablog.com

 

下記はモデリングツールcvxpyを使って、

MPCを解いた場合と、

上記の関数を使った場合の結果です。

 

下記のシミュレーションでは、

下記の入力制約を追加しています。

f:id:meison_amsl:20170207081844p:plain

 

両方の出力で同じ結果が得られていることがわかります。

f:id:meison_amsl:20170207073417p:plain

 

状態・入力制約を含んだMPCモデリングの数式導出とPythonサンプルコード

数式導出

前述の方法だと、入力制約は追加できますが、

状態制約が付けられないので、

別の方法で数式導出する必要があります。

 

まず初めに入力プロファイルと状態プロファイルを一つのベクトルZにまとめた場合、

下記の二次凸最適化の標準形に変形することができます。

f:id:meison_amsl:20170207075337p:plain

f:id:meison_amsl:20170207075705p:plain

 

入力制約と、状態制約は先程と同じように、

不等式制約とすることで、計算することができます。

f:id:meison_amsl:20170207072420p:plain

f:id:meison_amsl:20170207080959p:plain

f:id:meison_amsl:20170207080207p:plain

 

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]))

    # calc Ae
    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))

    # calc be
    be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) * x0
    #  print(be)

    # === optimization ===
    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を使って、計算した結果の比較です。

下記のシミュレーションでは、

下記の入力制約と状態制約を追加しています。

f:id:meison_amsl:20170207081754p:plain

 

モデリングツールを使った結果と

同じ結果が得られていることがわかります。

f:id:meison_amsl:20170207081429p:plain

 

MPCのTrackingサンプルコード

上記のMPCのコードは、

すべて状態を0にするように制御する

いわゆるRegulatorのコードでしたが、

ある目標値に状態値を近づけるように制御する

いわゆるMPC Tracking の方法を紹介したいと思います。

数式の導出

Trackingの数式の導出に関しては、

下記の書籍の2章と3章を参考にして下さい。

モデル予測制御―制約のもとでの最適制御

モデル予測制御―制約のもとでの最適制御

Predictive Control with Constraints

Predictive Control with Constraints

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  # number of horizon
    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)

    # test
    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で目標値に追従させる

入力プロファイルを計算することができます。

f:id:meison_amsl:20170217055226p:plain

 

すべてのPythonサンプルコード

すべてのPythonサンプルコードは、

下記のGithubリポジトリで公開しています

github.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Model Predictive Control: Classical, Robust and Stochastic (Advanced Textbooks in Control and Signal Processing)

Model Predictive Control: Classical, Robust and Stochastic (Advanced Textbooks in Control and Signal Processing)