[Continuous-Time] LQR 예제 : 타격각 제어

일정한 속력 \(V\) 로 움직이는 비행체가 있다. 제어 목적은 출발지에서 출발하여 비행 시간 \(t_f\) 가 경과한 후 목적지에 최소의 에너지를 사용하여 특정한 방향각 \(\theta_f\) 로 비행체를 목적지 \((x_f, \ y_f)\) 에 도착시키는 것이다. 비행체가 미사일이라면 \(\theta_f\)를 타격각(impact angle)이라고 한다.

 

 

아래 그림에 비행체와 목적지, 출발지 간의 기하학적인 관계가 나와 있다.

 

 

비행체의 운동 방정식은 다음과 같다.

 

\[ \begin{align} & \dot{x}= V \cos \theta \tag{1} \\ \\ & \dot{y} =V \sin \theta \\ \\ & \dot{\theta}= \frac{a}{V} \end{align} \]

 

여기서 \(a\) 는 비행체의 가속도로서 제어변수, \(\theta\) 는 x-축과 비행체의 속도벡터 사이의 각으로서 비행 방향각을 나타낸다. \(\theta\) 가 매우 작다고 가정한다면 위 식은 다음과 같이 선형화할 수 있다.

 

\[ \begin{align} & \dot{x} \approx V \tag{2} \\ \\ & \dot{y} \approx V \theta \\ \\ & \dot{\theta} = \frac{a}{V} \end{align} \]

 

위 식에서 첫번째 식은 자명한 식이므로 최적제어 문제의 동역학 식에서 제외하도록 한다. 비용함수와 제약조건은 다음과 같다.

 

\[ \begin{align} & J = \frac{1}{2} \int_0^{t_f} a^2 (t) \ dt \tag{3} \\ \\ & y(0)=0, \ \ \theta(0)=\theta_0, \ \ y(t_f )=y_f, \ \ \theta(t_f )=\theta_f \end{align} \]

 

이 문제는 최적제어 예제(https://pasus.tistory.com/232)에서 풀었던 문제와 거의 동일하지만, 이번에는 최종상태가 설정된 간략한 LQR 문제의 해(https://pasus.tistory.com/258)를 이용하여 풀어보도록 한다.

우선 시스템 운동 방정식 (2)를 LQR 문제에 맞게 상태변수 방정식으로 바꾼다.

 

\[ \dot{\mathbf{x}}=A \mathbf{x}+B \mathbf{u} \tag{4} \]

여기서

\[ \begin{align} & A= \begin{bmatrix} 0 & V \\ 0 & 0 \end{bmatrix} , \ \ B=\begin{bmatrix} 0 \\ 1/V \end{bmatrix} , \ \ \mathbf{u}=a \\ \\ & \mathbf{x}(t)= \begin{bmatrix} y \\ \theta \end{bmatrix} , \ \ \mathbf{x}(0 )= \begin{bmatrix} 0 \\ \theta_0 \end{bmatrix} , \ \ \mathbf{x}_f= \begin{bmatrix} y_f \\ \theta_f \end{bmatrix} \end{align} \]

 

이다. 그러면 시스템 모델과 제약조건이 식 (4)로 주어지고 비용함수가 식 (3)으로 주어졌을 때 최적제어는 다음과 같이 계산할 수 있다.

 

\[ \mathbf{u}(t)=R^{-1} B^T e^{A^T (t_f-t) } G^{-1} (t) \left( \mathbf{x}_f-e^{A(t_f-t)} \mathbf{x}(t) \right) \tag{5} \]

 

이제 식 (5)를 구성하는 행렬을 차례로 계산해 보자. 먼저 행렬지수함수는 다음과 같이 계산된다.

 

\[ e^{A(t_f-t)} =I+A(t_f-t) = \begin{bmatrix} 1 & V(t_f-t) \\ 0 & 1 \end{bmatrix} \tag{6} \]

 

행렬 \(G(t)\) 는 정의와 식 (6)에 의해서 다음과 같이 계산된다.

 

\[ \begin{align} G(t) &= \int_t^{t_f} e^{A(t_f-\tau)} BR^{-1} B^T e^{A^T (t_f-\tau) } \ d \tau \tag{7} \\ \\ &= \int_t^{t_f} \begin{bmatrix} 1 & V(t_f-\tau) \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 1/V \end{bmatrix} \begin{bmatrix} 0 & 1/V \end{bmatrix} \begin{bmatrix} 1 & 0 \\ V(t_f-\tau) & 1 \end{bmatrix} \ d\tau \\ \\ &= \begin{bmatrix} -(t_f-\tau)^3/3 & -(t_f-\tau)^2 / 2V \\ -(t_f-\tau)^2/2V & \tau /V^2 \end{bmatrix}_t^{t_f } \\ \\ &= \begin{bmatrix} t_{go}^3 /3 & t_{go}^2/2V \\ t_{go}^2/2V & t_{go} /V^2 \end{bmatrix} \end{align} \]

 

여기서 \(t_{go}=t_f-t\) 는 잔여시간(time-to-go)을 나타낸다. \(G(t)\) 의 역행렬은 다음과 같다.

 

\[ \begin{align} G^{-1} (t) &= \frac{12V^2}{t_{go}^4 } \begin{bmatrix} t_{go}/V^2 & -t_{go}^2/2V \\ -t_{go}^2/2V & t_{go}^3 /3 \end{bmatrix} \tag{8} \\ \\ & = \begin{bmatrix} 12/t_{go}^3 & -6V/t_{go} \\ -6V/t_{go}^2 & 4V^2/t_{go} \end{bmatrix} \end{align} \]

 

식 (6)과 (8)을 이용하면 식 (5)의 구성 부분을 다음과 같이 계산할 수 있다.

 

\[ \begin{align} & B^T e^{A^T (t_f-t) } G^{-1} (t)= \begin{bmatrix} 6/t_{go}^2 & -2V/t_{go} \end{bmatrix} \tag{9} \\ \\ & \mathbf{x}_f-e^{A(t_f-t)} \mathbf{x}(t)= \begin{bmatrix} y_f-y(t)-Vt_{go} \theta(t) \\ \theta_f-\theta (t) \end{bmatrix} \tag{10} \end{align} \]

 

식 (9)와 (10)을 식 (5)에 대입하면 최적제어는 다음과 같다.

 

\[ a(t)=\frac{6}{t_{go}^2 } (y_f- y(t))- \frac{2V}{t_{go}} (2\theta (t) + \theta_f) \tag{11} \]

 

식 (11)은 타격각이 설정된 미사일의 유도법칙으로 응용될 수 있다.