2.1 내부점 미분 동적 프로그래밍

IPDDP는 다음과 같이 제약조건이 있는 최적 제어 문제를 다룬다:

여기서 $x_{t}∊ℝ^{n}$와 $u_{t}∊ℝ^{n}$는 각각 stage $t$에서의 상태변수와 제어변수를 나타내고, $\hat x$는 초기 상태를 나타낸다. $l_t: \mathbb{R}^n \times \mathbb{R}^m \rightarrow \mathbb{R}$와 $l_f: \mathbb{R}^n \rightarrow \mathbb{R}$는 비용 함수이고, $f_t: \mathbb{R}^n \times \mathbb{R}^m \rightarrow \mathbb{R}^n$와 $g_t: \mathbb{R}^n \times \mathbb{R}^m \rightarrow \mathbb{R}^k$는 각각 상태 전이 함수와 제약조건 함수를 나타낸다. $k$는 제약조건의 개수이다. $T$는 stage 개수를 나타낸다. 모든 함수는 적어도 2번 이상 미분 가능하다고 가정한다. 식 (1)은 다음과 같이 가치 함수(value function)에 대한 벨만 방정식(Bellman equation) 형태로 표현될 수 있다:

여기서 $V_t: \mathbb{R}^n \rightarrow \mathbb{R}$은 가치함수이고 $s_{t}=[s_{1},\:...,\:s_{k}]_{t}$는 슬랙변수(slack variable)을 의미한다. 최종 stage $T$에서의 가치함수는 $V_{T}(x_{T})=l_{f}(x_{T})$으로 정의한다. 편의상 앞으로 stage에 대한 아래 첨자 $t$를 생략한다.

로그 장벽(logarithmic barrier)을 포함한 완화된 라그랑지안(relaxed Lagrangian)은 다음과 같이 정의된다:

여기서 $\mu >0$는 장벽 파라미터(barrier parameter)이고 $y$는 라그랑지 승수를 나타낸다. 완화된 가치 함수(relaxed value function)는 다음과 같이 함수 $Q$의 안장점(saddle point)으로 정의된다:

이에 대한 1차 최적 필요 조건(first order necessary condition for optimality)인 Karush-Kuhn-Tucker(KKT) 조건은 다음과 같다:

여기서 이항연산 기호 $\circ$은 아다마르 곱(Hadamard product)을 의미하고, $e=[1,\:...,\:1]∊ℝ^{k}$은 모든 성분이 1인 벡터이다. 아다마르 곱은 대각행렬 $Y=diag(y)$에 대해 행렬-벡터 곱 $y\circ s=Ys$으로 표현될 수 있다.

엄밀히 말하자면, 위의 식 (5)는 로그장벽함수를 사용하는 Central Path 방법의 섭동(perturbed) KKT 조건이며, 장벽 파라미터 $\mu >0$가 0으로 접근하면서 original KKT 조건을 ‘거의’ 만족시키게 된다[Chap. 11.2, 14].

2.1.1 역전달

기존 DDP에서 수행하는 역전달(backward pass)과 마찬가지로, 완화된 라그랑지안 $Q$를 다음과 같이 2차 항까지 절단하는 테일러 전개를 이용한 근사를 사용한다.

(6)

$\delta Q=\left[\begin{array}{l}Q_x \\ Q_u \\ Q_s \\ Q_y\end{array}\right]^{\top}\left[\begin{array}{l}\delta x \\ \delta u \\ \delta s \\ \delta y\end{array}\right]+\frac{1}{2}\left[\begin{array}{l}\delta x \\ \delta u \\ \delta s \\ \delta y\end{array}\right]^{\top}\left[\begin{array}{llll}Q_{x x} & Q_{x u} & Q_{x s} & Q_{x u} \\ Q_{u x} & Q_{u u} & Q_{u s} & Q_{u y} \\ Q_{s x} & Q_{s u} & Q_{s s} & Q_{s y} \\ Q_{y x} & Q_{y u} & Q_{y s} & Q_{s s}\end{array}\right]\left[\begin{array}{l}\delta x \\ \delta u \\ \delta s \\ \delta y\end{array}\right]$ $=Q_x^{\top} \delta x+\frac{1}{2} \delta x^{\top} Q_{x x} \delta x$ $+\delta x^{\top}\left[\begin{array}{lll}Q_{x u} & 0 & Q_{x y}\end{array}\right]\left[\begin{array}{l}\delta u \\ \delta s \\ \delta y\end{array}\right]+\left[\begin{array}{c}Q_u \\ y-\mu S^{-1} e \\ Q_y\end{array}\right]^{\top}\left[\begin{array}{l}\delta u \\ \delta s \\ \delta y\end{array}\right]$

여기서 $S=diag(s)$은 대각행렬이고, central path 방법의 섭동 KKT 조건을 따라서 대각성분이 모두 양수로서 역함수가 존재한다고 가정하였다.

식 (5)의 세 번째 등식에 해당하는 complementary slackness 조건으로부터 $\delta s^{\top}(\mu S^{-2})\delta s=\delta s^{\top}(S^{-1}Y)\delta s$으로 설정하면 다음과 같이 섭동변수 $\delta u,\:\delta s,\:\delta y$에 대한 primal-dual KKT 선형시스템을 얻을 수 있다:

(7)

$\left[\begin{array}{ccc}Q_{u u} & 0 & Q_{u y} \\ 0 & Y & S \\ Q_{u u} & I & 0\end{array}\right]\left[\begin{array}{c}\delta u \\ \delta s \\ \delta y\end{array}\right]=-\left[\begin{array}{c}Q_{u x} \\ 0 \\ Q_{u x}\end{array}\right] \delta x-\left[\begin{array}{c}Q_u \\ S y-\mu e \\ Q_u\end{array}\right]$

위의 KKT 선형시스템의 닫힌 해를 다음과 같이 얻을 수 있다:

(8)

\begin{align*} \delta u=K_{u}\delta x+d_{u},\:\delta s=K_{s}\delta x+d_{s},\:\delta y=K_{y}\delta x+d_{y},\:\\ \\ K_{u}=-\widetilde Q_{uu}^{-1}\widetilde Q_{ux},\:\\ d_{u}=-\widetilde Q_{uu}^{-1}\widetilde Q_{u},\:\\ K_{s}=-(Q_{ys}+Q_{yu}K_{u}),\:\\ d_{s}=-(r_{p}+Q_{yu}d_{u}),\:\\ K_{y}=S^{-1}Y(Q_{yx}+Q_{yu}K_{u}),\:\\ d_{y}=S^{-1}(r+YQ_{yu}d_{u}),\:\\ \\ \widetilde Q_{u}=Q_{u}+Q_{uy}S^{-1}r,\:\\ \widetilde Q_{uu}=Q_{uu}+Q_{uy}S^{-1}YQ_{yu},\:\\ \widetilde Q_{ux}=Q_{ux}+Q_{uy}S^{-1}YQ_{yx},\:\\ r=Yr_{p}-r_{d},\:\\ r_{p}=Q_{y},\:\\ r_{d}=Sy-\mu e \end{align*}

여기서 $r_{p}$와 $r_{d}$는 각각 primal residual과 dual residual을 나타낸다. 위의 식으로부터 $\delta s$와 $\delta y$는 다음과 같이 정리될 수 있다:

(9)

\begin{align*} \delta s=S^{-1}YQ_{yx}\delta x+S^{-1}YQ_{yu}\delta u+S^{-1}r\\ \delta y=-Q_{yx}\delta x-Q_{yu}\delta u-r_{p} \end{align*}

식 (6)에 $\delta s^{\top}(\mu S^{-2})\delta s=\delta s^{\top}(S^{-1}Y)\delta s$를 적용하고 위에서 구한 $\delta s$와 $\delta y$를 대입하면, 식 (6)은 다음과 같이 단순한 표현으로 정리될 수 있다.

(10)

$\delta Q=\frac{1}{2} r_p^{\top} S^{-1} Y r_p-r_d^{\top} S^{-1} r_p+\left[\begin{array}{c}\widetilde{Q}_x \\ \widetilde{Q}_u\end{array}\right]^{\top}\left[\begin{array}{l}\delta x \\ \delta u\end{array}\right]$ $+\frac{1}{2}\left[\begin{array}{l}\delta x \\ \delta u\end{array}\right]^{\top}\left[\begin{array}{cc}\widetilde{Q}_{x x} & \widetilde{Q}_{x u} \\ \widetilde{Q}_{u x} & \widetilde{Q}_{u u}\end{array}\right]\left[\begin{array}{l}\delta x \\ \delta u\end{array}\right]$ $\widetilde{Q}_x=Q_x+Q_{x y} S^{-1} r$ $\widetilde{Q}_{x x}=Q_{x x}+Q_{x u} S^{-1} Y Q_{v x}$

최종적으로 2차 테일러 급수로 근사한 완화된 가치 함수는 다음과 같이 기술된다:

(11)

$\delta V=\min _{u, s} \max _\nu \delta Q=\Delta V+V_x^{\top} \delta x+\frac{1}{2} \delta x^{\top} V_{x x} \delta x$

여기서 $\Delta V,\: V_{x},\: V_{xx}$는 다음과 같다:

(12)

\begin{align*} \Delta V=\widetilde Q_{u}^{\top}d_{u}+\dfrac{1}{2}d_{u}^{\top}\widetilde Q_{uu}d_{u}+\dfrac{1}{2}r_{p}^{\top}S^{-1}Yr_{p}-r_{d}^{\top}S^{-1}r_{p}\\ V_{x}=\widetilde Q_{x}+K_{u}^{\top}\widetilde Q_{u}+\widetilde Q_{ux}^{\top}d_{u}+K_{u}^{\top}\widetilde Q_{uu}d_{u}\\ V_{xx}=\widetilde Q_{xx}+K_{u}^{\top}\widetilde Q_{ux}+\widetilde Q_{ux}^{\top}K_{u}+K_{u}^{\top}\widetilde Q_{uu}K_{u} \end{align*}

그림. 1. IPDDP 순서도

Fig. 1. IPDDP flowchart

2.2 이차 원뿔 제약조건, 로그 장벽 함수 그리고 조르단 스핀 EJA

이번 절에서는 SOC-IPDDP을 설명하기 위해 필요한 이차 원뿔에 대한 배경 지식을 알아본다. 이차 원뿔의 정의는 다음과 같다.

여기서 $s$는 $x$의 스칼라 부분이고 $v$는 $x$의 벡터 부분이다. 식 (3)과 유사하게, 이차 원뿔에 대한 로그 장벽 함수 $B:K arrow ℝ$을 다음과 같이 정의할 수 있다.

장벽 $B$에 대한 그래디언트(gradient)는

와 같이 표현되고, 이차 원뿔의 정의에 따라 $\nabla B(x)$는 $K$의 원소이다.

이차 원뿔 $K$에 속한 임의의 두 벡터 $x_{1}=[s_{1},\:v_{1}]$와 $x_{2}=[s_{2},\:v_{2}]$에 대하여 다음과 같이 정의된 이항 연산 $\star$를 부여하자:

그러면 임의의 벡터 $x∊K$와 로그 장벽 함수 $B(x)$의 그래디언트 사이의 이항 연산에서 다음의 등식이 만족한다:

여기서 $e_{K}=[1,\:0,\:...,\:0]∊ℝ^{p+1}$이다. 사실, $e_{K}$는 $(K,\:\star)$의 항등원이고, $(K,\:\star)$는 조르단 스핀 EJA(Jordan spin EJA)이라 알려져 있다⁽⁸⁾.

식 (17)은 다음과 같이 행렬-벡터 곱으로 표현될 수 있다:

여기서 행렬 부분을 $L(x_{1})$으로 표기하였다.

4.1 문제 설정

최적제어문제 및 SOC-IPDDP 알고리즘에 사용되는 로켓의 2차원 동역학 모델은 다음과 같다:

여기서 $p,\:v∊ℝ^{2}$는 로켓의 위치와 속도를 나타내고, $\theta ,\:w∊ℝ$은 로켓의 각도와 각속도를 나타낸다. $F^{b}=[F_{1}^{b},\:F_{2}^{b}]∊ℝ^{2}$는 동체 프레임(body frame)에 대한 추력 벡터를 의미한다. 추력 벡터는 위 모델의 제어 입력이다. $m,\:l,\:J$는 각각 로켓의 질량, 길이, 회전 관성이고, $g=-[9.81,\:0]$은 중력 가속도 벡터이다. 편의상 로켓의 질량 중심은 로켓의 정중앙에 있다고 가정하였다.

SOC-IPDDP에 사용되는 필수 제약조건은 다음과 같다.

여기서 $p_{1}$은 로켓의 고도를 나타내고, $F_{\max},\: F_{\min}$는 각각 최대, 최소 추력이다. $\alpha$는 동체 프레임에 대한 추력 벡터의 각도를 의미하고, $\alpha_{\max}$는 $\alpha$의 최댓값이다. $K$는 이차 원뿔이다.

IPDDP에 사용될 필수 제약조건에는 이차 원뿔이 포함되면 안 되므로, $F_{1}^{b},\: F_{2}^{b}$에 각각 $F\cos\alpha$와 $F\sin\alpha$를 대입한다.

그림. 2. 2차원 로켓 모델의 도식화

Fig. 2. Planar rocket model

이에 따라, IPDDP에 사용될 2차원 로켓의 동역학 모델은 다음과 같이 기술된다.

IPDDP에 사용될 필수 제약조건은 다음과 같이 기술된다.

로켓이 원점에 착륙하도록 비용 함수를 다음과 같이 설정하였다. 편의상 stage에 대한 첨자를 생략하였다:

식 (27),(29)는 시간 간격 $\Delta t$와 오일러 방법으로 이산화되었다.

그림. 3. SOC-IPDDP와 IPDDP의 수렴 속도 비교

Fig. 3. Comparison of the convergence rate between SOC-IPDDP and IPDDP

4.2 실험 결과

로켓 착륙 문제에 대한 파라미터 값은 표 1에 정리하였고, IPDDP 및 SOC-IPDDP 알고리즘에 대한 모든 파라미터와 초기값은 서로 동일하게 설정하였다. 실험 방법은 식 (30)의 $\alpha$에 대한 페널티 계수 $c$와 초기 위치 $p_{0}$를 변화시키며 진행하였다. 실험은 일반 데스크탑 컴퓨터와 MATLAB을 이용하였다.

그림 2은 경우에 따른 각 알고리즘의 수렴 속도를 나타낸다.

그림. 4. SOC-IPDDP으로 계산한 시작점 (15,10)에서 출발하여 종점 (0,0)에 착륙하는 로켓의 궤적

Fig. 4. Rocket landing trajectory from the starting point (15,10) to the end point (0,0) by SOC-IPDDP

그림. 5. 그림 4의 궤적에 대한 제어 입력

Fig. 5. Control inputs of the trajectory in Fig. 4

$c=4.5$인 경우, SOC-IPDDP의 수렴 속도는 IPDDP와 유사하였고, $c=0.18$인 경우, 모든 초기 위치에서 SOC-IPDDP의 수렴 속도는 IPDDP보다 빨랐다. 이런 현상이 일어나는 이유는 $c$가 작아질수록 SOC-IPDDP와 달리 IPDDP의 문제는 불량 조건 문제(ill-posed problem)에 가까워지기 때문이다. 또한, SOC-IPDDP는 모든 경우에서 30 iterations 안에 수렴하였지만, IPDDP는 40 iterations을 넘는 경우를 관찰할 수 있었다. 종합적으로 평가했을 때, SOC-IPDDP가 IPDDP에 비해 수렴 속도면에서 유사하지만 다룰 수 있는 문제의 범위는 넓다는 결론을 내릴 수 있다.