Masse Feder System, Reglersynthese durch LQR (in der Notation des bestärkenden Lernens)

Kontrolltheorie
Bestärkendes Lernen
LQR
Autor:in

Johannes Kaisinger

Veröffentlichungsdatum

15. September 2021

In diesem Blogeintrag wollen wir die Notation des bestärkenden Lernens einführen. Dazu wiederholen wir die dynamische Programmierung anhand des LQR Problems.

Dynamische Programmierung - bestärkendes Lernen

Nun soll die dynamische Programmierung aus der Sichtweise des bestärkenden Lernens betrachtet werden. Hier wird eine andere Notation für die dynamische Programmierung verwendet. Anstatt \(J_i(x_i)\) spricht man von der Zustand-Werte-Funktion (engl. state-value function) \(\mathcal{V}_i(x_i)\), anstatt \(J_i(x_i,u_i)\) Zustand-Aktion-Werte-Funktion (engl. state-action-value function) \(\mathcal{Q}_i(x_i,u_i)\), oder \(\mathcal{Q}\)-Funktion (engl. q function). Diese etwas differenzierte Notation ist hilfreich für das Niederschreiben von Algorithmen. Besonders die \(\mathcal{Q}\)-Funktion \(\mathcal{Q}_i(x_i,u_i)\) ist im Feld des bestärkenden Lernens von besonderer Bedeutung.

Dynamische Programmierung für nichtlinear Systeme

In der neuen Notation können nun folgende Beziehungen angegeben werden:

Die optimale Bellman Gleichung für die Zustand-Werte-Funktion:

\[ \mathcal{V}_k^{*}(x_k) = \underset{u_k}{ min} \left[ c(x_k,u_k)+\mathcal{V}^{*}(x_{k+1}) \right] \]

Die optimale Bellman Gleichung für die Zustand-Aktion-Werte-Funktion:

\[ \mathcal{Q}_k^{*}(x_k,u_k) = \underset{u_{k+1}}{ min} \left[ c(x_k,u_k)+\mathcal{Q}^{*}(x_{k+1},u_{k+1}) \right] \]

In beide Funktionen kann das dynamische System \(x_{k+1}=f_k(x_k,u_k)\) eingesetzt werden:

\[ \mathcal{V}_k^{*}(x_k) = \underset{u_k}{ min} \left[ c(x_k,u_k)+\mathcal{V}_k^{*}(f(x_k,u_k)) \right] \]

\[ \mathcal{Q}_k^{*}(x_k,u_k) = \underset{u_{k+1}}{ min} \left[ c(x_k,u_k)+\mathcal{Q}^{*}(f(x_k,u_k),u_{k+1}) \right] \]

Berechnen von \(\mathcal{V}^{*}\) aus \(\mathcal{Q}^{*}\):

\[ \mathcal{V}_k^{*}(x_k) = \underset{u_k}{\text{ min }} \mathcal{Q}^{*}(x_k,u_k) \]

Berechnen von \(\mathcal{Q}^{*}\) aus \(\mathcal{V}^{*}\):

\[ \mathcal{Q}^{*}(x_k,u_k) = c(x_k,u_k) + \mathcal{V}_k^{*}(f(x_k,u_k)) \]

Bestimmung der optimal Aktion aus \(\mathcal{Q}^{*}\):

\[ u_k^{*} = \underset{u_k}{\text{ arg min }} Q^{*}(x_k,u_k) \]

Wörterbuch

Symbol Deutscher Name (Kurz) Englische Name (Short)
\(V(x)\) Zustand-Werte Funktion (Werte Funktion) state-value function (value function)
\(Q(x,u)\) Zustand-Aktion-Werte-Funktion (Aktion-Werte-Funktion) state-action function (action-value function)

Für eine besser Unterscheidung sprechen wir hier in Zukunft nur von der Werte Funktion \(\mathcal{V}(x)\) und von der \(\mathcal{Q}\)-Funktion \(\mathcal{Q}(x,u)\) (aka dem \(\mathcal{Q}\)-Faktor).

Dynamische Programmierung für lineare Systeme

Die ausgerollte Version des oben genannten dynamischen Optimierungsproblem kann mit

\[ \underset{u_0, u_N}{\text{ min }} c(x_0, u_0)+c(f_k(x_0,u_0), u_1)+\dots+c(f_k(f_k(\dots),\dots),u_N) \]

angegeben werden. Für den LQR-Fall vereinfacht sich das Problem zu

\[ f_k(x_k,u_k) = F_k \begin{bmatrix} x_k \\ u_k \end{bmatrix}, \quad c_k(x_k,u_k) = \begin{bmatrix} x_k \\ u_k \end{bmatrix}^T C_k \begin{bmatrix} x_k \\ u_k \end{bmatrix}, \quad C_k = \begin{bmatrix} C_{x_k,x_k} & C_{x_k,u_k} \\ C_{u_k,x_k} & C_{u_k,u_k} \end{bmatrix}, \]

wobei für \(F_k=\begin{bmatrix}A & B\end{bmatrix}\) und für \(C_k=\begin{bmatrix}Q & 0\\0 & R\end{bmatrix}\) verwendet wird.

Aus der optimalen \(Q\)-Funktion

\[ \mathcal{Q}_N^{*}(x_N,u_N) = \frac{1}{2} \begin{bmatrix} x_k \\ u_k \end{bmatrix}^T C_N \begin{bmatrix} x_k \\ u_k \end{bmatrix} \]

ergibt sich durch die Ableitung und das Nullsetzen

\[ \partial_{U_N} \mathcal{Q}_N^{*}(x_N,u_N) = C_{u_N,x_N} x_N + C_{u_N,u_N} u_N = 0 \]

die optimale Aktion mit

\[ u_N^{*} = - C_{u_N,u_N}^{-1}C_{u_N,x_N} x_N \quad \left( u_N^{*} = - K_N^{*} x_N \quad K_N^{*} = C_{u_N,u_N}^{-1}C_{u_N,x_N} \right). \]

Durch das Einsetzen erhalten wir die optimale Werte-Funktion zu

\[ \mathcal{V}_N^{*}(x_N) = \frac{1}{2} \begin{bmatrix} x_N \\ - K_N^{*} x_N \end{bmatrix}^T C_N \begin{bmatrix} x_N \\ - K_N^{*} x_N \end{bmatrix}. \]

Durch das Rausziehen von \(x_N\) aus

\[ \mathcal{V}_N^{*}(x_N) = \frac{1}{2} x_N^T C_{x_N,x_N} x_N - \frac{1}{2} x_N^T C_{x_N,u_N} K_N u_N - \frac{1}{2} x_N^T K_N^T C_{u_N,x_N} x_N - \frac{1}{2} x_N^T K_N^T C_{u_N,u_N} K_N x_N \]

kann

\[ P_N = C_{x_N,x_N} - C_{x_N,u_N}K_N - K_N^TC_{u_N,x_N} + K_N^T C_{u_N,u_N} K_N \]

berechnet und somit die optimale Werte-Funktion in einer verkürzten Schreibweise

\[ \mathcal{V}_N^{*}(x_N) = \frac{1}{2} x_N^T P_N x_N \]

angegeben werden.

Die neue \(\mathcal{Q}\)-Funktion erhalten wir aus der Beziehung

\[ \mathcal{Q}_{N-1}^{*}(x_{T-1},u_{T-1}) = \frac{1}{2} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix}^T C_{N-1} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix} + \mathcal{V}_N^{*}(f_{N-1}(x_{T-1},u_{T-1})) \]

mit

\[ x_N = f_{N-1}(x_{N-1},u_{N-1}) = F_{N-1} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix}, \]

und

\[ \mathcal{V}_N^{*}(f_{N-1}(x_{N-1},u_{N-1})) = \frac{1}{2} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix}^T F_{N-1}^T P_N F_{N-1} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix}. \]

Auch diese Funktion lässt sich in einer kompakten Schreibweise

\[ \mathcal{Q}_{N-1}^{*}(x_{N-1},u_{N-1}) = \frac{1}{2} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix}^T H_{N-1} \begin{bmatrix} x_{N-1} \\ u_{N-1} \end{bmatrix} \]

mit

\[ H_{N-1} = C_{N-1} + F_{N-1}^T P_N F_{N-1} \]

angegeben.

Ist die optimale \(\mathcal{Q}_{N-1}^{*}(x_{N-1},u_{N-1})\) zum Zeitpunkt \(N-1\) bekannt, kann die obige Vorgehensweise wiederholt angewendet werden.

Mit

\[ \partial_{u_{N-1}} \mathcal{Q}_{N-1}^{*}(x_{N-1},u_{N-1}) = H_{u_{N-1},x_{N-1}} x_{N-1} + H_{u_{N-1},u_{N-1}} = 0 \]

wird wieder die optimale Aktion

\[ u_{N-1}^{*} = - H_{u_{N-1},u_{N-1}}^{-1}H_{u_{N-1},x_{N-1}} x_{N-1} \quad \left( u_{N-1}^{*} = - K_{N-1}^{*} x_{N-1} \quad K_{N-1}^{*} = H_{u_{N-1},u_{N-1}}^{-1}H_{u_{N-1},x_{N-1}} \right) \]

berechnet.

Hinweis

Rückwärts Rekursion

\[ \begin{split} & \text{for } k=N-1 \text{ to } 0: \\ & \quad H_k = C_k + F_k^T P_{k+1} F_k, \quad (\text{final cost } P_{N} = S_N) \\ & \quad K_k = H_{u_k,u_k}^{-1} H_{u_k, x_k} \\ & \quad P_k = H_{x_k,x_k} - H_{x_k, u_k} K_k - K_k H_{u_k, x_k} + K_k^T H_{u_k,u_k} K_k \end{split} \]

Vorwärts Rekursion

\[ \begin{split} & \text{for } k=0 \text{ to } N-1: \\ & \quad u_k = - K_k x_k \\ & \quad x_{k+1} = A x_k + B u_k \end{split} \]

import numpy as np
import scipy
import scipy.signal as signal
import matplotlib.pyplot as plt
m1 = 1
d1 = 0.15
c1 = 0.3

m2 = 1
d2 = 0.15
c2 = 0.3

d3 = 0.15
c3 = 0.3

A = np.array([[0., 0., 1., 0.],
              [0., 0., 0., 1.], 
              [-(c1+c2)/m1, c2/m1, -(d1+d2)/m1, d2/m1],
              [c2/m2, -(c2+c3)/m2, (d2)/m2, -(d2+d3)/m2]])
B = np.array([[0., 0.], [0., 0.], [1./m1, 0.], [0., 1/m2]])
C = np.array([[1., 0., 0., 0], [0., 1., 0., 0.]])
D = np.array([[0., 0.],[0., 0.]])
sys_c = signal.StateSpace(A, B, C, D)
dt = 0.1
sys_d = sys_c.to_discrete(dt)
R = np.eye(2)*1
Q = np.eye(4)*1
A.shape[0]
4
B.shape
(4, 2)
def LQR_DP(A,B,Q,R,N):
    """ executes the dynamic program for the discrete time finite horizon LQR (N-steps)
        returns: final controller, final value-function, controller (every-step), value-function (every-step)
    """
    P = np.zeros(A.shape)
    P_array = np.zeros((N,P.shape[0],P.shape[1]))
    K_array = np.zeros((N,B.shape[1],B.shape[0]))
    for k in range(N-1,-1,-1):
        #print(k)
        P = Q+A.T@P@A-A.T@P@B@np.linalg.inv(R+B.T@P@B)@B.T@P@A
        P_array[k,:,:] = P
        K = np.linalg.inv(R+B.T@P@B)@B.T@P@A
        K_array[k,:,:] = K
    return K, P, K_array, P_array
def LQR_DP_Q(A,B,Q,R,N):
    """ executes the dynamic program for the discrete time finite horizon LQR (N-steps)
        ît uses the RL-notation
        returns: final controller, final value-function, controller (every-step), value-function (every-step)
    """
    n = B.shape[0]
    m = B.shape[1]
    H = np.zeros((n+m,n+m))
    P = np.zeros((n,n))
    C = np.zeros((n+m,n+m))
    F = np.zeros((n,n+m))
    H_array = np.zeros((N,H.shape[0],H.shape[1]))
    P_array = np.zeros((N,P.shape[0],P.shape[1]))
    K_array = np.zeros((N,B.shape[1],B.shape[0]))
    C[:n,:n] = Q
    C[n:,n:] = R
    F[:,:n] = A
    F[:,n:] = B
    
    for k in range(N-1,-1,-1):
        #print(k)
        H = C+F.T@P@F
        H_array[k,:,:] = H
        K = np.linalg.inv(H[n:,n:])@H[n:,:n]
        K_array[k,:,:] = K

        P = H[:n,:n]-H[:n,n:]@K-K.T@H[n:,:n]+K.T@H[n:,n:]@K
        P_array[k,:,:] = P
        
    return K, P, K_array, P_array
N = 100
K, P, K_array, P_array = LQR_DP(sys_d.A,sys_d.B,Q,R,N)
K
array([[0.525914  , 0.14589593, 1.15336293, 0.20646155],
       [0.14589593, 0.525914  , 0.20646155, 1.15336293]])
N = 100
K2, P2, K2_array, P2_array = LQR_DP_Q(sys_d.A,sys_d.B,Q,R,N)
K2
array([[0.52591394, 0.14589594, 1.15336281, 0.2064616 ],
       [0.14589594, 0.52591394, 0.2064616 , 1.15336281]])
np.allclose(K,K2)
True

Unendlicher Zeithorizont

In diesem Abschnitt werden wir das Problem

\[ \mathcal{V}(x_0) = \frac{1}{2} \sum_{k=0}^{\infty} (x_k^T Q x_k + u_k^T R u_k) \]

mit unendlichem Zeithorizont besprechen. Für die Herleitung der Riccati Gleichung versuchen wir den Ansatz

\[ \mathcal{V}^{*}(x_k) = \sum_{i=k}^{\infty} x_i^T \left(Q+K_{*}^TRK_{*}\right) x_i = x_k^TP_{*}x_k \]

für die Werte Funktion.

Für das LQR Problem gilt die Beziehung zwischen der Wertefunktion und dem \(\mathcal{Q}\)-Faktor

\[ \mathcal{Q}^{*}(x_k,u_k) = x_k^{T} Q x_k + u_k^{T} R u_k + \mathcal{V}^{*}(Ax_k+Bu_k). \]

Angenommen die optimale Wertefunktion lautet \(\mathcal{V}^{*}(x_k) = x_k^T P_{*} x_k\), wobei \(P_{*}\) die Lösung der Riccati Gleichung ist. Eingesetzt erhalten wir nun den optimal \(\mathcal{Q}\)-Faktor

\[ \begin{split} Q^{*}(x_k,u_k) &= x_k^{T} Q x_k + u_k^{T} R u_k + (Ax_k+Bu_k)^T P_{*} (Ax_k+Bu_k) \\ &= \begin{bmatrix} x_k \\ u_k \end{bmatrix}^T \begin{bmatrix} Q+A^TP_{*}A & A^TP_{*}B \\ B^TP_{*}A & R+B^TP_{*}B \end{bmatrix} \begin{bmatrix} x_k \\ u_k \end{bmatrix} \end{split} \]

Angenommen wir haben eine positive semidefinite quadratische optimale \(\mathcal{Q}\)-Funktion:

\[ \mathcal{Q}^{*}(x_k,u_k) = \begin{bmatrix} x_k \\ u_k \end{bmatrix}^T \begin{bmatrix} H_{xx}^{*} & H_{xu}^{*} \\ H_{ux}^{*} & H_{uu}^{*} \end{bmatrix} \begin{bmatrix} x_k \\ u_k \end{bmatrix} \]

Minimieren wir diese quadratische Funktion nach \(u_k\) ergibt sich die optimal Aktion \(u_k = - (H_{uu}^{*})^{-1}H_{ux}^{*} x_k\). Nun ist klar ersichtlich, dass sich der optimale Regler als \(K_{*}=(H_{uu}^{*})^{-1}H_{ux}^{*}\) angeben lässt.

Diese Darstellung wird sich noch als sehr hilfreich erweisen. Wenn es uns gelingt, den optimalen \(\mathcal{Q}\)-Faktor direkt aus Daten zu lernen, kann durch \(K_{*}=(H_{uu}^{*})^{-1}H_{ux}^{*}\) ein Regler, ohne der Kenntnis der Systemdynamik synthetisiert werden.

Fazit

In diesem Blogeintrag haben wir die dynamische Programmierung in der Notation des bestärkenden Lernens besprochen. Vor allen der Q-Faktor wird sich als hilfreich erweisen optimale Regler ohne Kenntnis der Systemdynamik zu synthetisieren.

Vorher wollen wir aber zwei modellbasierte Methoden zeigen, welche ohne das Lösen der Riccati Gleichung auskommen.