MPC 란? (Model Predictive Control) 3. Prediction of state and output

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

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

이전글에서 MPC를 위한 기본 모델을 모델링 해보았다. 이번 글에서는 MPC의 목적 즉, 내가 원하는 $N_p$개의 출력 model이 있다면, 그렇게 되도록 만드는 $N_c$개의 제어입력을 구하는 과정에 대해서 알아보도록 하겠다.

$N_p$ : 예측하고 싶은 미래 출력 수 (Prediction horizon)
$N_c$ : 예측하고 싶은 미래 제어 입력 수 (Control horizon)

trajectory tracking의 문제로 예를 들어보면, $N_p$개의 점들로 구성된 reference trajectory가 있을 때, 이를 정확하게 tracking할 수 있도록 design하는 $N_c$개의 제어 입력을 구하는 것이다. 

즉, 예측할 control input은 다음과 같이 $N_c$개의 입력이 된다.
$\Delta u(k), \Delta u(k+1), \Delta u(k+2), \cdots, \Delta u(k+N_c-1)$

또 예측할 output은 다음과 같다.
$y(k), y(k+1), y(k+2), \cdots, y(k+N_p)$ 
이때, $y(k) = Cx(k)$ 이므로, $y(k+1) = Cx(k+1), y(k+2) = Cx(k+2), \cdots$ 로 나타낼 수 있고,
따라서 예측할 state $x(k+1), x(k+2), x(k+N_p)$ 를 구해야 한다.

예측할 state variable 은 다음과 같다.
$x(k+1) = Ax(k) + B\Delta u(k)$
$x(k+2) = Ax(k+1) + B\Delta u(k+1)$
$\qquad \qquad = A^2x(k) + AB\Delta u(k) + B\Delta u(k+1)$
$x(k+3) = Ax(k+2) + B\Delta u(k+2)$
$\qquad \qquad = A^3x(k) + A^2B\Delta u(k) + AB\Delta u(k+1) + B\Delta u(k+2)$
$\therefore x(k+N_p) = A^{N_p}x(k) + A^{N_p-1}B\Delta u(k) +A^{N_p-2}B\Delta u(k+1)+\cdots+A^{N_p-N_c}B\Delta u(k+N_c-1)$

$y(k) = Cx(k)$ 이므로 최종적으로 우리가 얻고 싶은 $N_p$까지의 미래 출력은 위의 예측할 state variable에 C만 곱해주면 된다.
$y(k+1) = CAx(k) + CB\Delta u(k)$
$y(k+2) = CA^2x(k) + CAB\Delta u(k) + CB\Delta u(k+1)$
$x(k+3) = CA^3x(k) + CA^2B\Delta u(k) + CAB\Delta u(k+1) + CB\Delta u(k+2)$
$y(k+N_p) = CA^{N_p}x(k) + CA^{N_p-1}B\Delta u(k) +CA^{N_p-2}B\Delta u(k+1)+\cdots+CA^{N_p-N_c}B\Delta u(k+N_c-1)$

이를 matrix form으로 정리하면 다음과 같다.
$Y = Fx(k) + \Phi\Delta U$
$F = \begin{bmatrix}CA\\CA^2\\\vdots\\CA^{N_p}\end{bmatrix}$   $\Phi = \begin{bmatrix}CB & 0 & 0 & \cdots & 0 \\ CAB & CB & 0 & \cdots & 0 \\ CA^2B & CAB & CB & \cdots & 0 \\ \vdots & \vdots & \vdots & \cdots & \vdots \\ CA^{N_p-1}B & CA^{N_p-2}B & CA^{N_p-3}B & \cdots & CA^{N_p-N_c}B\end{bmatrix}$

이를 통해 알 수 있는 것은 현재 state 정보, $x(k)$, 만 알고 있다면 내가 입력, $\Delta U$, 를 넣었을 때 미래의 출력이 어떻게 될지 알 수 있다는 것이다.

물론 실제 system에 도입하기에는 parameter uncertainty(A,B,C matrix의 부정확도) 및 system nonlinearity로 인해서 바로 도입할 수 는 없지만 기초적인 개념은 ''현재 정보를 갖고 있다면, $N_c$개의 입력을 design해서 $N_p$개의 출력을 확인할 수 있다"는 것이다. 또 이를 거꾸로 말하면, 내가 원하는 미래의 출력 모양이 있다면, 그렇게 되도록 하는 입력을 구할 수 있다는 것.

즉 이를 로봇에 적용하면, 무인 시스템이 특정 궤적(미래의 출력)을 따라가도록 하는 입력을 구할 수 있다는 것!!