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

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

 

 

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

 

 

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

 

$$\begin{array}{r}\text{(1)}& & \dot{x}=V\cos \theta \\ & \dot{y}=V\sin \theta \\ \\ & \dot{\theta }=\frac{a}{V}\end{array}$$

 

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

 

$$\begin{array}{r}\text{(2)}& & \dot{x}\approx V\\ & \dot{y}\approx V\theta \\ \\ & \dot{\theta }=\frac{a}{V}\end{array}$$

 

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

 

$$\begin{array}{r}\text{(3)}& & J=\frac{1}{2}{\int }_0^{t_f}{a}^2(t)dt\\ & y(0)=0,\theta (0)={\theta }_0,y(t_f)=y_f,\theta (t_f)={\theta }_f\end{array}$$

 

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

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

 

$$\begin{array}{}\text{(4)}& \dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}\end{array}$$

여기서

$$\begin{array}{rl}& A=\left[\begin{array}{cc}0& V\\ 0& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 1/V\end{array}\right],\mathbf{u}=a\\ \\ & \mathbf{x}(t)=\left[\begin{array}{c}y\\ \theta \end{array}\right],\mathbf{x}(0)=\left[\begin{array}{c}0\\ {\theta }_0\end{array}\right],{\mathbf{x}}_f=\left[\begin{array}{c}{y}_f\\ {\theta }_f\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(5)}& \mathbf{u}(t)=R^{-1}{B}^T{e}^{A^T(t_f-t)}{G}^{-1}(t)({\mathbf{x}}_f-e^{A(t_f-t)}\mathbf{x}(t))\end{array}$$

 

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

 

$$\begin{array}{}\text{(6)}& e^{A(t_f-t)}=I+A(t_f-t)=\left[\begin{array}{cc}1& V(t_f-t)\\ 0& 1\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(7)}& G(t)& ={\int }_t^{t_f}{e}^{A(t_f-\tau )}B{R}^{-1}{B}^T{e}^{A^T(t_f-\tau )}d\tau \\ & ={\int }_t^{t_f}\left[\begin{array}{cc}1& V(t_f-\tau )\\ 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 1/V\end{array}\right]\left[\begin{array}{cc}0& 1/V\end{array}\right]\left[\begin{array}{cc}1& 0\\ V(t_f-\tau )& 1\end{array}\right]d\tau \\ \\ & ={\left[\begin{array}{cc}-(t_f-\tau {)}^3/3& -(t_f-\tau {)}^2/2V\\ -(t_f-\tau {)}^2/2V& \tau /V^2\end{array}\right]}_t^{t_f}\\ \\ & =\left[\begin{array}{cc}{t}_{go}^3/3& t_{go}^2/2V\\ t_{go}^2/2V& t_{go}/V^2\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(8)}& G^{-1}(t)& =\frac{12V^2}{t_{go}^4}\left[\begin{array}{cc}{t}_{go}/V^2& -t_{go}^2/2V\\ -t_{go}^2/2V& t_{go}^3/3\end{array}\right]\\ & =\left[\begin{array}{cc}12/t_{go}^3& -6V/t_{go}\\ -6V/t_{go}^2& 4V^2/t_{go}\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{r}\text{(9)}& & B^T{e}^{A^T(t_f-t)}{G}^{-1}(t)=\left[\begin{array}{cc}6/t_{go}^2& -2V/t_{go}\end{array}\right]\\ \text{(10)}& & {\mathbf{x}}_f-e^{A(t_f-t)}\mathbf{x}(t)=\left[\begin{array}{c}{y}_f-y(t)-V{t}_{go}\theta (t)\\ {\theta }_f-\theta (t)\end{array}\right]\end{array}$$

 

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

 

$$\begin{array}{}\text{(11)}& a(t)=\frac{6}{t_{go}^2}(y_f-y(t))-\frac{2V}{t_{go}}(2\theta (t)+{\theta }_f)\end{array}$$

 

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