MPC 란? (Model Predictive Control) 4. Optimal control

참고: 제어조교님 유투브 강의 영상

MPC는 Optimal Control의 한 방법인데 로보틱스의 planning및 제어에 많이 활용되고 있다. MPC를 사용하면 로봇의 속도 및 가속력과 같은 dynamics와 주변 환경 조건을 cost function으로 넣어 상황에 맞는 최적화된 제어 명령을 생성할 수 있고 이를 통해 안정적으로 로봇의 자율항법이 가능하다. 아주 오래전부터 해보고 싶었던 분야 인데, 이 글을 작성하면서 공부하고 실제 드론에 탑재해서 실험까지 수행해보고자 한다.

이전글에서 MPC의 미래 state variable과 출력에 대해 알아보고 이를 통해 현재 정보를 알고 있을 때 미래의 출력을 따라가도록 하는 입력을 구할 수 있다는 것을 알았다. 이제 이러한 배경지식을 가지고 controller를 design하기 전에 필요한 optimal control에 관한 지식을 습득하고 넘어가도록 한다. 

이번글에서는 optimal control의 기초인 LQR(linear quadratic regulator)에 대해서 배워보도록 한다. 

다음과 같은 시스템과 초기 조건이 있을때,
$\dot{x} = f(x,u,t)$.    $x(t_0) = x_0$

다음과 같은 cost function
$V(x(t_0),u,t_0) = \int_{t_0}^T l[x(\tau), u(\tau), \tau]d\tau + m(x(T))$                      (1)

을 최소화하는 입력 $u^*(t)$,    $t_0 \leq t \leq T$ 를 찾는 것이 optimal control의 목적이다.

이때 principle of optimality에 따라 한문제에 대한 해가 최적이면 그 문제를 이루는 sub-problem들의 해도 최적이기 때문에 식(1)을 다음과 같이 $t_0 < t < t_1 < T$ 인 $t_1$을 추가하여 나누어 쓸 수 있다.

$V^*(x(t),t) = \min\limits_{u[t,T]}\{\int_t^{t_1} l[x(\tau),u(\tau),\tau]d\tau + \min\limits_{u[t_1,T]}[\int_{t_1}^T l[x(\tau),u(\tau),\tau]d\tau + m(x(T))]\}$
$\qquad \qquad \quad= \min\limits_{u[t,t_1]}\{\int_t^{t_1} l[x(\tau),u(\tau),\tau]d\tau + V^*(x(t_1),t_1)\}$                      (2)

$\because V^*(x(t_1),t_1) = \min\limits_{u[t_1,T]}[\int_{t_1}^T l[x(\tau),u(\tau),\tau]d\tau + m(x(T))]$

이제 이를 바탕으로 $t_1 = t + \Delta t$ 이며, $\Delta t$가 매우 작은 값인 경우를 살펴보자, 먼저 식(2)로부터

$V^*(x(t),t) = \min\limits_{u[t,t+\Delta t]}\{\int_t^{t+\Delta t} l[x(\tau),u(\tau),\tau]d\tau + V^*(x(t+\Delta t),t+\Delta t) $

가 된다. 이때, 적분 term의 경우 $t$부터 $\Delta t$ 까지의 사다리꼴 면적으로 생각할 수 있고, 사다리꼴의 면적 계산은 사각형면적 계산으로 근사할 수 있다. 즉 밑변 ($\Delta t$)과 높이 ($l[x(t+\alpha\Delta t),u(t+\alpha\Delta t),t+\alpha\Delta t])$ 의 곱으로 계산할 수 있다.
(여기서, $0 < \alpha < 1$)

그러므로,

$V^*(x(t),t) = \min\limits_{u[t,t+\Delta t]}\{\Delta t \cdot l[x(t+\alpha\Delta t),u(t+\alpha\Delta t),t+\alpha\Delta t] + V^*(x(t+\Delta t),t+\Delta t) $

가 된다. 마지막으로 $V^*(x(t+\Delta t),t+\Delta t)$ 를 각각 $x$와 $t$에 대해서 taylor series로 표현하면,

$V^*(x(t),t) = \min\limits_{u[t,t+\Delta t]}\{\Delta t \cdot l[x(t+\alpha\Delta t),u(t+\alpha\Delta t),t+\alpha\Delta t]$
$\qquad \qquad \quad+ V^*(x(t),t)+ [\frac{\partial V^*}{\partial x}]^T(x(t),t)\Delta t \frac{dx(t)}{dt} + \frac{\partial V^*}{\partial t}(x(t),t)\Delta t + H.O.T $

여기서 이제 $V^*(x(t),t)$ 가 양변에 존재하므로 소거 되고, H.O.T은 매우 작은 값이므로 무시한 후, 양변을 $\Delta t$로 나누면.

$0 = \min\limits_{u[t,t+\Delta t]}\{l[x(t+\alpha\Delta t),u(t+\alpha\Delta t),t+\alpha\Delta t] + [\frac{\partial V^*}{\partial x}]^T(x(t),t)\frac{dx(t)}{dt} + \frac{\partial V^*}{\partial t}(x(t),t)$

가 되고, 앞에서 $\Delta t$는 매우 작은 값이라고 하였으므로 0극한으로 보내면 다음의 Hamilton-Jacobi Equation을 얻을 수 있다.

$\frac{\partial V^*}{\partial t}(x(t),t) = -\min\limits_{u(t)}\{l[x(t),u(t),t] + [\frac{\partial V^*}{\partial x}]^Tf(x,u,t)\}$                  (3)

이를 선형 시스템에 적용해보자

먼저 system은 다음과 같고,

$\dot{x} = Ax + Bu$,      $x(t_0) = x_0$

cost function

$V(x(t_0),u,t_0) = \int_{t_0}^{T_f}(u^TRu + x^TQx)dt + x^T(T_f)Qx(T_f)$

을 최적화하는 $u^*(t)$,    $t_0 \leq t \leq T_f$ 를 찾아 보자. 이때,

$V^*(x(t_0),u,t_0) = x^T(t)Px(t)$ 이고 P는 symmetric matrix이다 $(P = P^T)$

먼저. 식(3) Hamilton-Jacobi Equation에 대입하면 위 시스템은 아래와 같으므로,

$l = u^TRu + x^TQx$     $f = Ax + Bu$     $V^* = x^TPx$ 

$\frac{\partial V^*}{\partial t} = 0 = -min\limits_{u(t)}[u^TRu+x^TQx + 2x^TP(Ax + Bu)]$

가 되고, 이를 quadratic form으로 만들면,

$0 = -[(u+R^{-1}B^TPx)^TR(u+R^{-1}B^TPx) + x^T(Q+PA+A^TP-PBR^{-1}B^TP)x]$

가 된다. 그러므로 위 식을 최소화 하는 $u^*$는

$u^* = -R^{-1}B^TPx$

가 되고, 

$ Q+PA+A^TP-PBR^{-1}B^TP = 0$

인 P 를 구하면 된다. 이 식이 바로 Algebraic Riccati Equation(ARE) 이다.

여기서 Q와 R은 Design factor이고, P는 이 값들에 따라 ARE를 풀어서 얻어지는 값이다.