離散LQ制御の例(初期条件+終端条件固定の場合)

Published

2024-08-28

Modified

2024-08-28

問題設定

状態ベクトルの次元n=2, 入力制御ベクトルの次元m=1とし, 線形システム

x(k+1) = Ax(k) + B u(k)

を考える. ここで A=\begin{bmatrix} 0 & 1\\ -6&-5\end{bmatrix},\quad B=\begin{bmatrix} 0\\1 \end{bmatrix} とし, 境界値は x(0)=\begin{bmatrix} -1\\ 0\end{bmatrix}, \quad x(N)=\begin{bmatrix} 0\\ 0\end{bmatrix}

とする. 評価関数は

J= \sum_{k=1}^{N-1}\dfrac12\left(x^T(k)Qx(k) + u^T(k)Ru(k)\right).

において Q=\begin{bmatrix} 13& 0\\ 0&1\end{bmatrix},\quad R=\begin{bmatrix} 1 \end{bmatrix} とする. すなわち

J=\dfrac12\sum_{k=1}^{N-1}\left(13x_1(k)^2 + x_2(k)^2 +u(k)^2\right)

を最小化する最適制御を考える.

オイラー・ラグランジュ方程式は

\begin{aligned} &x(k+1) = Ax(k) + Bu(k), \quad x(0)=x_0, x(N)=x_N\\ &\lambda(k) =Qx(k)+A^T \lambda(k+1) \\ &u^T(k)R + \lambda^T(k+1)B=0. \end{aligned}

となる. ここで, x(k)と\lambda(N)には無関係な行列S(k),U(k)として関係式\lambda(k)=S(k)x(k)+U(k)\lambda(N)を代入してS(k),U(k)の条件を求める.

まずはk=Nを代入してS(N)=0, U(N)=Iであり, 随伴方程式と第3式から\lambda(k+1)を消去して

u(k) = -(R+B^TS(k+1)B)^{-1}B^T(S(k+1)Ax(k)+U(k+1)\lambda(N))

を得る. これを再び随伴方程式に代入することで,

\begin{aligned} S(k)x(k)+U(k)\lambda(N) &=Qx(k)+A^T(S(k+1)(Ax(k)+Bu(k))+U(k+1)\lambda(N))\\ &=(Q+A^TS(k+1)A^TS(k+1)A - A^TS(k+1)B(R+B^TS(k+1)B)^{-1}B^TS(k+1)A)x(k)\\ &\quad + A^T(1-S(k+1)B(R+B^TS(k+1)B)^{-1}B^T)U(k+1)\lambda(N) \end{aligned}

を得る. x(k)と\lambda(N)について係数比較をすることで,

\begin{aligned} S(k) &=Q+A^TS(k+1)A\\ &\quad - A^TS(k+1)B(R+B^TS(k+1)B)^{-1}B^TS(k+1)A,\quad k=1,cdots, N-1,\\ U(k)&=A^T(1-S(k+1)B(R+B^TS(k+1)B)^{-1}B^T)U(k+1),\quad k=1,cdots, N-1,\\ S(N)&=0, \quad U(N)=I \end{aligned}

を得る. また, \lambda(N)の値は以下のように逐次代入によって求められる. すなわちx(k)と\lambda(N)には無関係な行列V(k),W(k)として関係式x(N)=V(k)x(k)+W(k)\lambda(N)を代入してV(k),W(k)の条件を求める.

まずはk=Nを代入してV(N)=I, W(N)=0であり, 状態方程式と u=の式を代入することで, k=0,\cdots, N-1について

\begin{aligned} V(k)x(k)+W(k)\lambda(N) &=V(k+1)(Ax(k)+Bu(k))+U(k+1)\lambda(N)\\ &=V(k+1)(1-B(R+B^TS(k+1)B)^{-1}B^TS(k+1))A\\ &\quad + W(k+1)\lambda(N)- V(k+1)(R+B^TS(k+1)B)^{-1}B^TU(k+1)\lambda(N) \end{aligned}

を得る. x(k)と\lambda(N)について係数比較をすることで,

\begin{aligned} V(k) &=V(k+1)(1-B(R+B^TS(k+1)B)^{-1}B^TS(k+1))A,\quad k=1,cdots, N-1,\\ W(k)&=W(k+1)- V(k+1)B(R+B^TS(k+1)B)^{-1}B^TU(k+1),\quad k=1,cdots, N-1,\\ V(N)&=I, \quad W(N)=0 \end{aligned}

を得る. これを用いると

\lambda(N)=W(0)^{-1}(x_N-V(0)x_0)

として\lambda(N)を定めることができる.

を得る. U(k)=V^T(k)が成り立つことに注意すると, 以下のステップで最適制御と状態遷移を求めることができる

S(k), U(k)をk=N,N-1,\cdots 0まで求める
V(k)=U^T(k)として, W(k)をk=N,N-1,\cdots 0まで求める
\lambda(N)=W(0)^{-1}(x_N-V(0)x_0)を求める
u(k) = -(R+B^TS(k+1)B)^{-1}B^T(S(k+1)Ax(k)+U(k+1)\lambda(N))から, x(0), S(1) を用いてu(0)を定める
状態方程式x(k+1)=Ax(k)+Bu(k)からx(0), u(0)を用いてx(1)を求める
以上を繰り返して, u(1), x(1), u(2),\cdotsを順次求める

1. S(k), U(k)をk=N,N-1,\cdots 0まで求める

import numpy as np

# 問題設定
dim = 2
A = np.array([[0, 1], [-6, -5]], dtype=float)
B = np.array([[0], [1]], dtype=float)

Q = np.array([[13, 0], [0, 1]], dtype=float)
R = np.array([[1]], dtype=float)

x_0 = np.array([[-1], [0]], dtype=float)
x_N = np.array([[0], [0]], dtype=float)

# 解く区間
N = 10

# リッカチ方程式は時間逆向きに解いてリストに加え, 最後に逆順に並べる
S_sols = [np.zeros((dim, dim), dtype=float)]
U_sols = [np.identity(dim, dtype=float)]
for _ in range(N):
    S_next = S_sols[-1]
    U_next = U_sols[-1]
    Id = np.identity(dim, dtype=float)

    S = Q.copy()
    S += A.T @ S_next @ A
    S -= A.T @ S_next @ B @ np.linalg.inv(R + B.T @ S_next @ B) @ B.T @ S_next @ A
    S_sols.append(S)
    U = A.T @ (Id - S_next @ B @ np.linalg.inv(R + B.T @ S_next @ B) @ B.T) @ U_next
    U_sols.append(U)

2. V(k)=U^T(k)として, W(k)をk=N,N-1,\cdots 0まで求める

W_sols = [np.zeros((dim, dim), dtype=float)]
for k in range(N):
    W_next = W_sols[-1]
    V_next = U_sols[k + 1].T
    S_next = S_sols[k + 1]

    W = W_next.copy()
    W -= V_next @ B @ np.linalg.inv(R + B.T @ S_next @ B) @ B.T @ V_next.T
    W_sols.append(W)

S_sols.reverse()
U_sols.reverse()
W_sols.reverse()

lambda_N = np.linalg.inv(W_sols[0]) @ (x_N - U_sols[0].T @ x_0)

3. k=0,\cdots N-1についてu(k), x(k+1)を求める

リッカチ方程式の解とオイラー・ラグランジュ方程式を用いて最適制御と状態ベクトルを逐次的に求めていく

X = [x_0]
U = []

for k in range(N - 1):
    x_k = X[-1]
    S_k1 = S_sols[k + 1]
    U_k1 = U_sols[k + 1]

    u_k = -np.linalg.inv(R + B.T @ S_k1 @ B) @ B.T @ (S_k1 @ A @ x_k + U_k1 @ lambda_N)
    U.append(u_k)
    X.append(A @ x_k + B * u_k)

4. 可視化

得られた状態ベクトル(左図)と制御入力(右図)をグラフを表示する

左図より, オーバーシュートは発生しているが抑えられていることがわかる
状態ベクトル, 制御入力ともにk=5あたりで0の十分近くに到達している
離散LQ制御の例の場合とほぼ同じ制御ができている

import matplotlib.pyplot as plt

X = np.array(X).reshape(N, 2)
U = np.array(U).reshape(N - 1, 1)

plt.figure(figsize=(12, 4))

plt.subplot(1, 2, 1)
for k in range(2):
    plt.plot(np.arange(0, N), X[:, k], label=f"x_{k}")
plt.axhline(0, color="black", linestyle="--", linewidth=0.7)
plt.legend()

plt.subplot(1, 2, 2)
for k in range(1):
    plt.plot(np.arange(0, N - 1), U[:, k], marker="o", linestyle="--", label=f"u_{k}")
plt.axhline(0, color="black", linestyle="--", linewidth=0.7)
plt.legend()

plt.show()