LQR vs MPC

control에서 MPC와 LQR은 비슷하게 다루어진다. 둘다 quadratic cost function을 다룬 다는 점에서 유사하지만, 세세한 내용에 있어 큰 차이가 존재한다. 이번 블로그 게시글에서는 그러한 차이점에 대한 내용을 자세히 다루어 보고자한다.

quadratic costs function

LQR과 MPC에서는 모두 quadratic cost function을 사용한다. \(J = \sum_{k=0}^{N-1} \big(x_k^T Q x_k + u_k^T R u_k\big) + x_N^T P x_N\)

system dynamic적 관점에서 두 가지 방식 모두 cost function을 minimize하는 방식이다. 단지 차이는 mpc는 제약조건을 추가할수 있다는 점에서 다르다.

LQR

lqr은 infinite horizon최적화 문제를 풀고 이를 static state-feedback gain으로 적용한다. \(u = -Kx\)

강점

• compuation 효율적이다.
• stability가 보장된다.

약점

• constraints를 추가할수 없다.
• 선형 가정에서만 사용가능하다.
• infinite horzon 최적화를 풀기에 우리 시스템이 finite한 상황이어서, flexible하게 설계하기가 어려운 단점이 있다.(예를들어, finite 시간안에 성능 보장하기 등등)

코드

import numpy as np

dt = 0.1
A = np.array([
    [1, 0, dt, 0],
    [0, 1, 0, dt],
    [0, 0, 1, 0 ],
    [0, 0, 0, 1 ]
])
B = np.array([
    [0.5*dt**2, 0],
    [0, 0.5*dt**2],
    [dt, 0],
    [0, dt]
])
Q = np.diag([10, 10, 1, 1])   # state penalty
R = np.diag([1, 1])           # control penalty

def dlqr(A, B, Q, R):
    """Solve the discrete-time LQR controller."""
    # Solve Riccati equation
    P = np.matrix(Q)
    for _ in range(1000):
        P_next = A.T @ P @ A - A.T @ P @ B @ np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A + Q
        if np.allclose(P, P_next, atol=1e-8):
            break
        P = P_next
    # Gain
    K = np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
    return np.asarray(K)

K = dlqr(A, B, Q, R)

# Simulation
x = np.array([5, 5, 0, 0])  # initial state
traj_lqr = [x.copy()]

for _ in range(50):
    u = -K @ x
    x = A @ x + B @ u
    traj_lqr.append(x.copy())

MPC

mpc는 finitie horzion 최적화 문제를 풀고 이를 매 step 반복적으로 푼다. 그 뒤 first control action에 적용한다.

위 수식처럼 dynamic에 constraints가 포함된 cost function이 적용되는데, 여러 horizon중 가장 작은 cost function값이 control에 올라간다.

강점

• constraint를 다룰 수 있다. 그러나 hard constraint는 문제를 해결할수 없게 만들 수 있다.
• collision avoidance와 같은 상황에 유연하게 사용가능하다.
• 비선형으로 확장이 가능하다.

약점

• computation부하가 크다.
• stability에 대한 안정성 보장이 필요하다. terminal cost를 사용하여 Lyapunov function으로 stability를 보장하려고 하지만, infinite horizon을 사용하기 때문에, stability에 대한 보장을 0으로 도착하는 것을 사용하는 것이 일정 terminal region안에 도달하면 stability로 본다.
• 구현에 복잡성이 존재한다.

코드

N = 10  # horizon length

def mpc_control(x0, N=10):
    best_u0 = None
    best_cost = 1e9
  
    for ax in np.linspace(-2, 2, 9):
        for ay in np.linspace(-2, 2, 9):
            x = x0.copy()
            cost = 0
            for _ in range(N):
                u = np.array([ax, ay])
                cost += x.T @ Q @ x + u.T @ R @ u
                x = A @ x + B @ u
            if cost < best_cost:
                best_cost = cost
                best_u0 = np.array([ax, ay])
    return best_u0

x = np.array([5, 5, 0, 0])
traj_mpc = [x.copy()]

for _ in range(50):
    u = mpc_control(x, N)
    x = A @ x + B @ u
    traj_mpc.append(x.copy())