[MPC] 4. Optimal Control(2) - Taylor Series 적용, Algebraic Riccati Equation(ARE) 구하기

LQR에 적용

$$V^{*}(x(t), t) = \underset{u[t, t+\Delta t]}{min} \{
\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)$ 부분을 위 Taylor Series로 x와 t에 대해서 정리해보자.
• $x = (x(t), t), v = \Delta t$ 라고 생각하자.
• 정리하면 아래와 같다.
  $$V^{*}(x + v) = V^{*}(x) + f'(x)v + f(x)v' + \frac 12 f''(x)v^{2}+ \frac12 f(x)\cdots$$
• 여기에 $x = (x(t), t), v = \Delta t$ 를 적용하면
  $$V^{*}(x(t + \Delta t), t+\Delta t)
  = V^{*}(x(t), t)[\frac{\partial V^{*}}{\partial x}]^{T} \cdot (x(t), t) \cdot \Delta t \cdot \frac{dx(t)}{dt} \frac{\partial V^{*}}{\partial t} \cdot (x(t), t) \cdot \Delta t \cdot 1 + H.O.T$$
  • 편미분 할 때 $x(t)$를 기준으로 하면 $[\frac{\partial V^{*}}{\partial x}]^{T}$ 는 거리($x$) 기준이므로 기울기가 되고, 때문에 gradient 연산을 위해서 전치행렬로 처리해주었다.
  • $x(t)$ 에 대한 미분값으로 $\frac{dV^{*}}{dt}$ 를 곱해주었다.
  • 편미분 할 때 $\Delta t$를 기준으로 하면 $\frac{\partial V^{*}}{\partial t}$ 는 시간($t$) 기준이므로 그대로 계산한다.
  • $t$는 상수이므로 이에 대한 미분값으로 $1$ 을 곱해주었다.
• 이다. $H.O.T$ 는 고차항이라고 보면 된다. 작아서 뒤에서 날린다.
• 총 식은 아래와 같게 된다.
  $$\begin{matrix}V^{*}(x(t), t) &=& \underset{u[t, t+\Delta t]}{min} \{ 
  \Delta t \cdot l[x(t +  \alpha \Delta t), u(t + \alpha \Delta t), t + \alpha \Delta t] \\ 
  && + V^{*}(x(t), t)  \\
  + & &[\frac{\partial V^{*}}{\partial x}]^{T} \cdot (x(t), t) \cdot \Delta t \cdot \frac{dx(t)}{dt}\\ 
  + & &\frac{\partial V^{*}}{\partial t} \cdot (x(t), t) \cdot \Delta t + H.O.T
   \} \end{matrix}$$
• $H.O.T$는 작은 값이므로 무시하고, 양 변에서 $V^{*}(x(t), t)$를 제거, $\Delta t$로 나눠주면 아래와 같다.

$$\frac{\partial V^{*}}{\partial t}(x(t),t)=−\underset{u(t)}{min}{l[x(t),u(t),t]+[\frac{\partial V^{*}}{\partial x}]^{T}f(x,u,t)}$$

선형 시스템 적용

• system : $\dot x = Ax + Bu, x(t_{0}) = x_{0}$
• cost function : $$V(x(t_{0}), y, t_{0}) = \int_{t_{0}}^{T_{f}}(u^{T}Ru + x^{T}Qx)dt + x^{T}(T_{f})Qx(T_{f})$$
• cost function을 최적화하는 $u^{*}(t), t_{0} \le t \le T_{f}$ 를 찾아보자. $t_{0}$에서 cost function의 최적 함수는 아래와 같다.
  $$\begin{matrix} V^{\*}(x(t_{0}), y, t_{0}) = x^{T}(t)Px(t), & P =P^{T} \end{matrix}$$
• 원본 cost function을 hamilton-jacobi equation에 적용해보면 $l, f, V^{*}$는 아래와 같이 얻을 수 있다.
  $$\begin{matrix} l=u^{T}Ru + x^{T}Qx & f=Ax+Bu & V^{*} = x^{T}Px \end{matrix}$$
• 이것을 적용하면 아래와 같은 식을 구할 수 있다.
  $$\frac{\partial V^{*}}{\partial t} = 0 = -\underset{u(t)}{min} [u^{T}Ru+x^{T}Qx + 2x^{T} P(Ax + Bu)]$$
• 이 내용을 quadratic form으로 정리한다.
  $$u^{T}Ru+x^{T}Qx+2x^{T}PBu + 2x^{T}PAx=0$$
• 뭐야이게
  잘 정리하면
  $$0=-[(u+R^{-1}B^{T}Px)R(u+R^{-1}B^{T}Px) + x^{T}(Q + PA + A^{T}P - PBR^{-1}B^{T}P)x]$$
• 따라서 위 식을 최소화하는 $u^{*}$는
  $$u^{*}= -R^{-1}B^{T}Px$$
• 이고, 아래 조건을 만족하는 P
  $$Q + PA + A^{T}P - PBR^{-1}B^{T}P = 0$$
• 을 구하면 이 식이 Algebraic Riccati Equation(ARE) 다.
  • $Q, R$은 Design factor이며 $P$는 이 값에 따라 얻어지는 값