[Continuous-Time] LQR 예제 : 비례항법유도 법칙

이전 포스트(https://pasus.tistory.com/259)와 동일한 문제를 풀어본다. 다만 최종시간에서 \(y(t_f )\) 는 주어지지만 \(\theta (t_f )\) 에 관한 제약조건은 없는 경우이다.

 

 

편의상 비행체의 선형화된 운동 방정식을 다시 쓴다.

 

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

 

여기서 \(a\) 는 비행체의 가속도로서 제어변수, \(\theta\) 는 x-축과 비행체의 속도벡터 사이의 비행 방향각으로서 매우 작다고 가정한 것이다. 비용함수와 제약조건은 다음과 같다.

 

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

 

제어 목적은 출발지에서 출발하여 비행 시간 \(t_f\) 가 경과한 후 목적지에 최소의 에너지를 사용하여 비행체를 목적지 \( (x_f, \ y_f)\)에 도착시키는 것이다. 그림에 비행체와 목적지, 출발지 간의 기하학적인 관계가 나와 있다.

 

 

이 문제는 최종상태가 설정된 LQR 문제 (https://pasus.tistory.com/257)의 해를 이용하여 풀 수 있다. 우선 시스템 운동 방정식 (1)을 LQR 문제에 맞게 상태변수 방정식으로 바꾼다.

 

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

여기서

\[ \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} \end{align} \]

 

이다. 최종 상태변수의 제약조건은 다음과 같다.

 

\[ \begin{align} & C \mathbf{x}(t_f )=\mathbf{r}_f=y_f \tag{4} \\ \\ & C= \begin{bmatrix} 1 & 0 \end{bmatrix} \end{align} \]

 

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

 

\[ \mathbf{u}(t)=-B^T S(t) \mathbf{x}(t)-B^T \mathbf{V}(t) P^{-1} (t) \left( \mathbf{r}_f-\mathbf{V}^T (t) \mathbf{x}(t) \right) \tag{5} \]

여기서

\[ \begin{align} S(t) &=0 \tag{6} \\ \\ -\dot{\mathbf{V}}(t) &= A^T \mathbf{V}, \ \ \ \ t \le t_f, \ \ \mathbf{V}(t_f )=C^T \tag{7} \\ \\ \dot{P}(t) &= \mathbf{V}^T BB^T \mathbf{V}, \ \ \ \ t \le t_f, \ \ P(t_f )=0 \tag{8} \end{align} \]

 

이다. 이제 식 (5)를 구성하는 행렬을 차례로 계산해 보자. 먼저 식 (7)을 계산한다.

 

\[ -\begin{bmatrix} \dot{v}_1 \\ \dot{v}_2 \end{bmatrix} = \begin{bmatrix} 0 & 0 \\ V & 0 \end{bmatrix} \begin{bmatrix} v_1 \\ v_2 \end{bmatrix}, \ \ \ \begin{bmatrix} v_1(t_f) \\ v_2(t_f) \end{bmatrix}=\begin{bmatrix} 1 \\ 0 \end{bmatrix} \tag{9} \]

 

여기서 \(\mathbf{V}= [v_1 \ \ v_2 ]^T\) 이다. 위 식을 전개하면 다음과 같다.

 

\[ \begin{align} & \dot{v}_1=0, \ \ \ v_1 (t_f )=1 \tag{10} \\ \\ & \dot{v}_2=-v_1 V, \ \ \ v_2 (t_f )=0 \end{align} \]

위 식을 풀면,

\[ \begin{align} & v_1 (t)=1 \tag{11} \\ \\ & v_2 (t)=V(t_f-t) \end{align} \]

 

이 된다. 다음으로 식 (8)을 계산한다.

 

\[ \begin{align} \dot{P} &= [v_1 \ \ \ v_2] \begin{bmatrix} 0 & 0 \\ 0 & 1/V^2 \end{bmatrix} \begin{bmatrix} v_1 \\ v_2 \end{bmatrix} \tag{12} \\ \\ &= \frac{v_2^2}{V^2} =(t_f-t)^2 , \ \ \ P(t_f )=0 \end{align} \]

위 식을 풀면,

\[ P(t)=- \frac{(t_f-t)^3}{3} \tag{13} \]

 

이 된다. 식 (11)과 (13)을 식 (5)에 대입하면 최적제어는 다음과 같다.

 

\[ \begin{align} a(t) &= [0 \ \ \ 1/V] \begin{bmatrix} 1 \\ V t_{go} \end{bmatrix} \frac{3}{t_{go}^3} (y_f-(y+Vt_{go} \theta)) \tag{14} \\ \\ &= \frac{3}{t_{go}^2 } (y_f-y)- \frac{3V}{t_{go}} \theta \end{align} \]

여기서 \(t_{go}=t_f-t \) 로서 잔여비행시간(time-to-go)이다.

 

 

식 (14)는 미사일의 유도법칙으로 사용될 수 있다. 미사일 유도법칙에서 사용되는 변수로 식 (14)를 변형하여 보자.

 

 

위 그림에서 \(\beta\) 는 시선각(line-of-sight angle)이다. 시선각은 다음과 같이 계산할 수 있다.

 

\[ \beta (t)= \sin^{-1} \frac{⁡(y_f-y)}{R} \approx \frac{(y_f-y)}{R} \approx \frac{(y_f-y)}{Vt_{go}} \tag{15} \]

여기서 \(R\) 은 표적까지의 거리로서 \(Vt_{go}\) 로 근사할 수 있다. 식 (15)를 식 (14)에 대입하면 최적제어는 다음과 같이 된다.

 

\[ a(t)= \frac{3V}{t_{go}} \beta - \frac{3V}{t_{go}} \theta \tag{16} \]

식 (15)의 시선각을 미분하면 다음과 같이 된다.

 

\[ \dot{\beta} (t)= \frac{-\dot{y}Vt_{go}+V(y_f-y)}{V^2 t_{go}^2 }= - \frac{\dot{y}}{Vt_{go}}+ \frac{\beta}{t_{go}} \tag{17} \]

위 식에 식 (1)을 대입하면,

 

\[ \dot{\beta} (t) \approx - \frac{V \theta}{Vt_{go}} + \frac{\beta}{t_{go}} = \frac{1}{t_{go}} ( \beta- \theta) \tag{18} \]

이 되므로 시선각을 시선각의 변화율과 비행 방향각의 함수로 표현할 수 있다.

 

\[ \beta (t)= \theta (t)+ t_{go} \dot{\beta} (t) \tag{19} \]

 

식 (19)를 식 (16)에 대입하면 최종적으로 최적제어 또는 미사일의 유도법칙은 다음과 같이 된다.

 

\[ \begin{align} a(t) &= \frac{3V}{t_{go}} \left( \theta +t_{go} \dot{\beta}(t) \right)- \frac{3V}{t_{go}} \theta \tag{20} \\ \\ &= 3V \dot{\beta} (t) \end{align} \]

식 (20)에 의하면 미사일의 최적 유도법칙은 항법상수가 \(3\) 인 비례항법유도(PNG)법칙임을 알 수 있다.