HJB Equation and Continuous LQR

Continuous-Time Dynamic Programming and Riccati Equations

References: Jonathan P. How, 16.323 Principles of Optimal Control, Lecture 4: HJB Equation, MIT OpenCourseWare, Spring 2008.

앞 장에서는 시간, 상태, 제어입력을 이산화한 뒤 Bellman recursion을 뒤로 계산했다. 이 장에서는 시간 간격을 무한히 작게 보내 연속시간 value function이 만족하는 Hamilton-Jacobi-Bellman equation을 유도한다. HJB는 일반적인 비선형 최적제어 문제를 하나의 partial differential equation으로 표현하며, continuous LQR에서는 이 PDE가 differential Riccati equation으로 축약된다.


Continuous-Time Optimal Control Problem

다음의 유한시간 최적제어 문제를 생각하자.

\[\min_{u(\cdot)} J =h\bigl(x(t_f),t_f\bigr) +\int_{t_0}^{t_f}g\bigl(x(\tau),u(\tau),\tau\bigr)\,d\tau\]

subject to

\[\dot{x}(\tau)=a\bigl(x(\tau),u(\tau),\tau\bigr), \qquad x(t_0)=x_0,\] \[u(\tau)\in\mathcal{U}, \qquad m\bigl(x(t_f),t_f\bigr)=0.\]

$h$는 terminal cost, $g$는 running cost, $m=0$은 terminal constraint를 나타낸다. 시간 $t$의 상태가 $x$일 때 남은 구간 $[t,t_f]$에서 발생하는 최적 비용을

\[V(x,t) =\inf_{u(\cdot)} \left[ h\bigl(x(t_f),t_f\bigr) +\int_t^{t_f}g\bigl(x(\tau),u(\tau),\tau\bigr)\,d\tau \right]\]

로 정의한다. 이 $V$가 continuous-time cost-to-go 또는 value function이다. 최적 정책은 각 $(x,t)$에서 이 값을 가장 작게 만드는 제어입력을 선택한다.

종단 상태가 자유라면 boundary condition은

\[V(x,t_f)=h(x,t_f)\]

이다. Terminal constraint가 있으면 이를 만족하는 상태에 대해서만 $h$를 부여하고, 나머지 상태에는 무한대 비용을 둘 수 있다.

\[V(x,t_f)= \begin{cases} h(x,t_f), & m(x,t_f)=0,\\ +\infty, & m(x,t_f)\neq0. \end{cases}\]


Dynamic Programming over a Short Interval

시간 구간 $[t,t_f]$를 짧은 구간 $[t,t+\Delta t]$와 나머지 구간 $[t+\Delta t,t_f]$로 나눈다. 최적성 원리에 따르면 첫 구간 이후에는 새 상태 $x(t+\Delta t)$에서 시작하는 최적 cost-to-go를 따라야 한다. 따라서

\[\begin{aligned} V(x,t) =\inf_{u(\cdot)} \Biggl[ &\int_t^{t+\Delta t} g\bigl(x(\tau),u(\tau),\tau\bigr)\,d\tau\\ &+V\bigl(x(t+\Delta t),t+\Delta t\bigr) \Biggr]. \end{aligned}\]

$\Delta t$가 충분히 작고 $u$가 이 구간에서 거의 일정하다고 보면

\[\int_t^{t+\Delta t} g\bigl(x(\tau),u(\tau),\tau\bigr)\,d\tau =g(x,u,t)\Delta t+o(\Delta t)\]

이고 dynamics로부터

\[x(t+\Delta t) =x+a(x,u,t)\Delta t+o(\Delta t)\]

를 얻는다.

Value function이 충분히 매끄럽다고 가정하여 $(x,t)$ 주위에서 Taylor expansion을 하면

\[\begin{aligned} V\bigl(x(t+\Delta t),t+\Delta t\bigr) =V(x,t) &+V_t(x,t)\Delta t\\ &+\nabla_xV(x,t)^T a(x,u,t)\Delta t +o(\Delta t). \end{aligned}\]

이를 dynamic programming 식에 대입하고 양변의 $V(x,t)$를 소거하면

\[0 =\inf_{u\in\mathcal{U}} \left\{ g(x,u,t) +V_t(x,t) +\nabla_xV(x,t)^Ta(x,u,t) \right\}\Delta t +o(\Delta t).\]

$\Delta t$로 나눈 뒤 $\Delta t\rightarrow0$으로 보내면 continuous-time Bellman equation을 얻는다.

\[0 =V_t(x,t) +\inf_{u\in\mathcal{U}} \left\{ g(x,u,t) +\nabla_xV(x,t)^Ta(x,u,t) \right\}.\]


Hamilton-Jacobi-Bellman Equation

Costate와 같은 역할을 하는 벡터 $p$를 사용해 Hamiltonian을

\[\mathcal{H}(x,u,p,t) =g(x,u,t)+p^Ta(x,u,t)\]

로 정의하자. $p=\nabla_xV(x,t)$를 대입하면 HJB equation은

\[-V_t(x,t) =\inf_{u\in\mathcal{U}} \mathcal{H}\bigl(x,u,\nabla_xV(x,t),t\bigr)\]

로 쓸 수 있다. 이에 대응하는 optimal feedback policy는

\[\pi^\star(x,t) \in \underset{u\in\mathcal{U}}{\operatorname{argmin}} \mathcal{H}\bigl(x,u,\nabla_xV(x,t),t\bigr)\]

이다.

HJB는 최적 궤적 하나만을 구하는 식이 아니다. 전체 상태-시간 영역에서 $V(x,t)$를 구하고, 각 점에서 Hamiltonian을 최소화하여 feedback policy를 만든다. 이 때문에 초기 상태가 바뀌어도 같은 value function과 policy를 사용할 수 있다.

Classical solution이 존재하고 필요한 regularity condition을 만족하면 HJB와 terminal condition은 optimality를 검증하는 충분조건을 제공한다. 그러나 일반적인 최적제어 문제의 value function은 obstacle, state constraint, 여러 최적 경로의 교차 때문에 미분 불가능할 수 있다. 이 경우 HJB는 classical derivative가 아니라 viscosity solution의 의미로 다룬다.


Minimum-Time Example

다음 시스템을 원점으로 보내는 최소시간 문제를 생각하자.

\[\dot{x}=Ax+u, \qquad \lVert u\rVert\leq1,\]

여기서 $A+A^T=0$이라고 하자. Running cost가 $g=1$이고 terminal cost가 $0$이므로 전체 비용은 도달시간 $t_f$이다. Time-independent value function을 가정하면 HJB는

\[0 =\min_{\lVert u\rVert\leq1} \left\{ 1+\nabla V(x)^T(Ax+u) \right\}\]

이 된다.

고정된 $\nabla V$에 대해 내적 $\nabla V^Tu$를 unit ball 위에서 최소화하면

\[u^\star =-\frac{\nabla V(x)}{\lVert\nabla V(x)\rVert}\]

이다. Candidate value function으로

\[V(x)=\lVert x\rVert\]

를 택하면 $x\neq0$에서

\[\nabla V(x)=\frac{x}{\lVert x\rVert}.\]

또한 $A$가 skew-symmetric이므로

\[x^TAx =\frac{1}{2}x^T(A+A^T)x =0.\]

따라서 candidate와 optimal control

\[u^\star=-\frac{x}{\lVert x\rVert}\]

를 HJB에 대입하면

\[1 +\frac{x^TAx}{\lVert x\rVert} -\frac{x^Tx}{\lVert x\rVert^2} =1+0-1 =0\]

이 된다. 즉 $V(x)=\lVert x\rVert$는 원점까지의 최소시간이고, 최적 입력은 현재 상태의 반대 방향으로 최대 크기의 제어를 가한다. 원점에서는 $V$가 미분 불가능하지만 boundary condition $V(0)=0$을 만족한다.


Continuous Linear Quadratic Regulator

Continuous LQR은 linear dynamics와 quadratic cost를 갖는 문제이다.

\[\dot{x}(t)=A(t)x(t)+B(t)u(t),\] \[\begin{aligned} J =\frac{1}{2}x(t_f)^THx(t_f) +\frac{1}{2}\int_{t_0}^{t_f} \Bigl[ &x(t)^TR_{xx}(t)x(t)\\ &+u(t)^TR_{uu}(t)u(t) \Bigr]\,dt. \end{aligned}\]

다음을 가정한다.

\[H=H^T\succeq0, \qquad R_{xx}(t)=R_{xx}(t)^T\succeq0, \qquad R_{uu}(t)=R_{uu}(t)^T\succ0.\]

Hamiltonian은

\[\begin{aligned} \mathcal{H}(x,u,\nabla_xV,t) =\frac{1}{2} \left(x^TR_{xx}x+u^TR_{uu}u\right) +\nabla_xV^T(Ax+Bu) \end{aligned}\]

이다. 제어입력에 대한 gradient를 $0$으로 두면

\[\frac{\partial\mathcal{H}}{\partial u} =R_{uu}u+B^T\nabla_xV =0\]

이고, $R_{uu}\succ0$이므로 이 stationary point는 유일한 global minimizer이다.

\[u^\star =-R_{uu}^{-1}B^T\nabla_xV.\]

이 식은 value function의 gradient가 최적 제어 방향을 결정한다는 사실을 보여 준다. 다음 단계는 $\nabla_xV$를 실제로 계산할 수 있는 형태로 만드는 것이다.


Quadratic Value Function

LQR의 terminal condition은

\[V(x,t_f)=\frac{1}{2}x^THx\]

이므로 전체 시간 구간에서도 value function이 quadratic form을 유지한다고 가정한다.

\[V(x,t)=\frac{1}{2}x^TP(t)x, \qquad P(t)=P(t)^T.\]

상태와 시간에 대한 partial derivative는

\[\nabla_xV(x,t)=P(t)x,\] \[V_t(x,t)=\frac{1}{2}x^T\dot{P}(t)x.\]

따라서 optimal control은 linear state feedback이 된다.

\[u^\star(t) =-R_{uu}(t)^{-1}B(t)^TP(t)x(t) =-F(t)x(t),\] \[F(t)=R_{uu}(t)^{-1}B(t)^TP(t).\]

$P(t)$와 $F(t)$는 상태 $x(t)$에 의존하지 않으므로 미리 offline에서 계산할 수 있다.


Differential Riccati Equation

Quadratic value function과 optimal control을 HJB equation에 대입하면

\[\begin{aligned} -\frac{1}{2}x^T\dot{P}x =\frac{1}{2}x^TR_{xx}x &+\frac{1}{2}x^T(PA+A^TP)x\\ &-\frac{1}{2}x^TPBR_{uu}^{-1}B^TPx. \end{aligned}\]

이 식이 모든 $x$에 대해 성립해야 하므로 행렬 계수는

\[-\dot{P}(t) =P(t)A(t)+A(t)^TP(t)+R_{xx}(t) -P(t)B(t)R_{uu}(t)^{-1}B(t)^TP(t)\]

를 만족해야 한다. 이것이 continuous-time differential Riccati equation이다. Terminal condition은

\[P(t_f)=H\]

이므로 $P(t)$는 $t_f$에서 $t_0$ 방향으로 backward integration한다. $P$를 구한 뒤에는

\[u^\star(t)=-R_{uu}(t)^{-1}B(t)^TP(t)x(t)\]

를 적용하여 상태를 앞으로 적분한다. 최적 value는

\[V(x(t),t)=\frac{1}{2}x(t)^TP(t)x(t)\]

이다.

Discrete LQR에서는 Riccati difference equation을 뒤로 반복했다. Continuous LQR에서는 같은 optimal-substructure가 matrix differential equation으로 나타난다.


Evaluating a Given Feedback Gain

최적 gain 대신 임의의 time-varying feedback

\[u(t)=-G(t)x(t)\]

를 적용한다고 하자. Closed-loop dynamics는

\[\dot{x}(t)=\bigl(A(t)-B(t)G(t)\bigr)x(t)\]

이다. 이 controller의 cost를

\[J_G(x,t)=\frac{1}{2}x^TS(t)x\]

로 쓰면 $S(t)$는 다음 differential Lyapunov equation을 만족한다.

\[\begin{aligned} -\dot{S} =&(A-BG)^TS+S(A-BG)\\ &+R_{xx}+G^TR_{uu}G, \qquad S(t_f)=H. \end{aligned}\]

최적 gain $G=R_{uu}^{-1}B^TS$를 대입하면 이 식은 Riccati equation으로 환원된다. 따라서 $S(t_0)$와 최적 $P(t_0)$를 비교하면 주어진 feedback law의 suboptimality를 초기 상태별로 평가할 수 있다.


Infinite-Horizon Limit and CARE

$A$, $B$, $R_{xx}$, $R_{uu}$가 일정한 LTI problem에서 horizon을 무한히 길게 하면, 적절한 조건 아래 $P(t)$는 terminal time에서 멀리 떨어진 구간에서 일정한 행렬 $P_{\mathrm{ss}}$로 수렴한다. $\dot{P}=0$을 DRE에 대입하면 control algebraic Riccati equation을 얻는다.

\[P_{\mathrm{ss}}A +A^TP_{\mathrm{ss}} +R_{xx} -P_{\mathrm{ss}}BR_{uu}^{-1}B^TP_{\mathrm{ss}} =0.\]

정상상태 feedback gain은

\[F_{\mathrm{ss}} =R_{uu}^{-1}B^TP_{\mathrm{ss}}\]

이고 closed-loop dynamics는

\[\dot{x} =(A-BF_{\mathrm{ss}})x\]

이다.

$R_{xx}=C_z^TR_{zz}C_z$이고 $R_{zz}\succ0$라고 하자. $(A,B)$가 stabilizable이고 $(A,C_z)$가 detectable이면 CARE는 유일한 stabilizing positive-semidefinite solution을 가지며, $A-BF_{\mathrm{ss}}$는 Hurwitz matrix가 된다. 즉

\[\operatorname{Re}\lambda_i(A-BF_{\mathrm{ss}})<0\]

가 모든 eigenvalue에 대해 성립한다. 이 조건은 불안정한 mode가 제어 가능하고 cost에서 관측 가능해야 함을 뜻한다.

유한시간 gain $F(t)$는 terminal condition 근처에서 크게 변할 수 있지만, horizon이 충분히 길면 대부분의 구간에서 $F_{\mathrm{ss}}$와 가까워진다. 시스템의 transient가 gain이 변하기 전에 끝난다면 constant steady-state gain도 time-varying optimal gain과 거의 같은 응답을 낼 수 있다. 반대로 짧은 horizon이나 중요한 terminal penalty가 있는 문제에서는 time-varying gain이 필요하다.


Scalar LQR Example

Scalar system

\[\dot{x}=ax+bu\]

와 infinite-horizon cost

\[J =\int_0^\infty \left(R_{xx}x(t)^2+R_{uu}u(t)^2\right)\,dt, \qquad R_{xx}>0,\quad R_{uu}>0\]

를 생각하자. Scalar CARE는

\[2aP_{\mathrm{ss}} +R_{xx} -\frac{b^2}{R_{uu}}P_{\mathrm{ss}}^2 =0\]

이다. 다음과 같이 두면

\[\beta =\sqrt{a^2+\frac{b^2R_{xx}}{R_{uu}}},\]

stabilizing solution은

\[P_{\mathrm{ss}} =\frac{R_{uu}}{b^2}(a+\beta) =\frac{R_{xx}}{-a+\beta} >0\]

이다. 정상상태 gain과 closed-loop pole은 각각

\[K_{\mathrm{ss}} =R_{uu}^{-1}bP_{\mathrm{ss}} =\frac{a+\beta}{b},\] \[A_{\mathrm{cl}} =a-bK_{\mathrm{ss}} =-\beta <0\]

이므로 closed-loop system은 안정하다.

Finite-horizon analytic solution

$\tau=t_f-t$와 $P_f=P(t_f)$를 사용하면 scalar DRE의 해는

\[P(\tau) = \frac{ (aP_f+R_{xx})\sinh(\beta\tau) +\beta P_f\cosh(\beta\tau) }{ \left(b^2P_f/R_{uu}-a\right)\sinh(\beta\tau) +\beta\cosh(\beta\tau) }.\]

$P_f=0$이면

\[P(\tau) = \frac{R_{xx}\sinh(\beta\tau)} {-a\sinh(\beta\tau)+\beta\cosh(\beta\tau)}.\]

$\tau\rightarrow\infty$에서 $\sinh(\beta\tau)$와 $\cosh(\beta\tau)$가 같은 지수적 크기를 가지므로 $P(\tau)\rightarrow P_{\mathrm{ss}}$이다. 이 식은 terminal condition의 영향이 backward time $\tau$가 커질수록 사라지고 steady-state solution으로 수렴하는 모습을 직접 보여 준다.

Cheap and expensive control

$R_{xx}/R_{uu}$가 크면 상태 오차에 비해 제어입력이 저렴한 cheap-control regime이다. 이때 $\beta$와 gain이 커져 상태를 빠르게 줄이는 적극적인 controller를 얻는다.

$R_{xx}/R_{uu}$가 작으면 제어입력이 상대적으로 비싼 expensive-control regime이다.

  • $a<0$이면 open-loop system이 이미 안정하므로 $K_{\mathrm{ss}}\rightarrow0$이다.
  • $a>0$이면 안정화를 위해 최소한의 feedback이 필요하며 $K_{\mathrm{ss}}\rightarrow2a/b$이고 closed-loop pole은 $-a$에 접근한다.

즉 제어가 비싸더라도 open-loop unstable mode를 방치할 수는 없으며, 최적 controller는 필요한 안정화와 제어 노력 사이의 절충을 선택한다.


Numerical Integration of the DRE

DRE는 terminal condition에서 시작하므로 원래 시간 $t$에 대해서는 뒤로 적분해야 한다. 새로운 변수

\[\tau=t_f-t\]

를 도입하면 $d\tau=-dt$이고, terminal time은 $\tau=0$이 된다. 따라서

\[\frac{dP}{d\tau} =PA+A^TP+R_{xx}-PBR_{uu}^{-1}B^TP, \qquad P(0)=H\]

를 $\tau$의 증가 방향으로 적분할 수 있다.

$P$는 symmetric matrix이므로 독립적인 원소는 $n(n+1)/2$개뿐이다. Upper-triangular entries를 하나의 벡터

\[y=\operatorname{vecs}(P)\]

로 쌓고, ODE solver 내부에서 $P=\operatorname{unvecs}(y)$로 복원하면 일반적인 vector ODE integrator를 사용할 수 있다. 매 step에서 대칭성을 보존하고 $P(t)$의 eigenvalue 및 residual을 확인하는 것이 중요하다.

시간 순서를 다시 $t=t_f-\tau$로 변환한 뒤

\[F(t)=R_{uu}^{-1}B^TP(t)\]

를 계산한다. 이후 closed-loop dynamics $\dot{x}=(A-BF(t))x$를 초기 상태에서 앞으로 적분한다.


Weighting Matrix Selection

$R_{xx}$와 $R_{uu}$는 단위와 허용 범위가 다른 신호들을 직접 비교하므로 먼저 normalization하는 것이 유용하다. 각 상태와 입력의 허용 또는 기대 최대 크기를 $(x_i){\max}$와 $(u_j){\max}$라 하면 다음과 같은 diagonal weighting을 사용할 수 있다.

\[R_{xx} =\operatorname{diag} \left( \frac{\alpha_1^2}{(x_1)_{\max}^2}, \ldots, \frac{\alpha_n^2}{(x_n)_{\max}^2} \right),\] \[R_{uu} =\rho\, \operatorname{diag} \left( \frac{\beta_1^2}{(u_1)_{\max}^2}, \ldots, \frac{\beta_m^2}{(u_m)_{\max}^2} \right),\]

where

\[\sum_{i=1}^n\alpha_i^2=1, \qquad \sum_{j=1}^m\beta_j^2=1.\]

$\alpha_i$는 상태 성분 사이의 상대적 중요도를, $\beta_j$는 actuator 사이의 상대적 중요도를 정한다. Scalar $\rho$는 제어 노력과 상태 오차 사이의 전체적인 절충을 조절한다. $\rho$가 크면 입력이 비싸지고, 작으면 더 적극적인 제어를 허용한다.

이 normalization은 물리 단위가 다른 위치, 속도, 각도, 에너지 및 여러 actuator를 한 비용함수에 넣을 때 특히 중요하다. Weight는 단순한 tuning parameter가 아니라 어떤 오차와 입력을 어느 규모까지 허용할 것인지 표현하는 설계 변수이다.


Connection to Continuous Path Planning

환경장 $e(x,t)$ 속에서 움직이는 vehicle의 dynamics와 running cost를

\[\dot{x}=a\bigl(x,u,e(x,t),t\bigr),\] \[g=g\bigl(x,u,e(x,t),t\bigr)\]

로 쓰면 path-planning value function은

\[-V_t(x,t) =\inf_{u\in\mathcal{U}} \left\{ g\bigl(x,u,e(x,t),t\bigr) +\nabla_xV(x,t)^T a\bigl(x,u,e(x,t),t\bigr) \right\}\]

을 만족한다. 목표 지점이나 목표 집합에서 terminal 또는 boundary cost를 정하고 HJB를 뒤로 풀면, $V(x,t)$는 각 상태에서 목표까지의 최적 비용을 나타낸다.

최적 policy는

\[\pi^\star(x,t) \in \underset{u\in\mathcal{U}}{\operatorname{argmin}} \left\{ g+\nabla_xV^Ta \right\}\]

로 복원한다. 따라서 HJB는 연속 공간 전체에 cost-to-go field와 feedback direction field를 만든다. Continuous LQR은 이 일반적인 HJB 구조가 quadratic value function 때문에 Riccati equation으로 정확히 축약되는 특별한 경우이다.


Summary

Continuous-time dynamic programming은 짧은 구간의 stage cost와 다음 상태의 value를 결합한 뒤 $\Delta t\rightarrow0$ 극한을 취해 HJB equation을 만든다. HJB는 value function의 시간 변화와 Hamiltonian의 pointwise minimum을 연결하며, 그 minimizer가 optimal feedback policy를 정의한다. Continuous LQR에서는 quadratic value function을 대입하여 differential Riccati equation과 linear feedback $u^\star=-R_{uu}^{-1}B^TPx$를 얻는다. 긴 LTI horizon에서는 DRE가 CARE의 stabilizing solution으로 수렴하며, 실제 설계에서는 신호의 허용 크기를 기준으로 $R_{xx}$와 $R_{uu}$를 normalization하고 상태 성능과 제어 노력의 상대적 비중을 조절한다.