6.135 로봇 궤적 최적화에서의 행렬 연산

1. 궤적 최적화 문제의 정식화

로봇 궤적 최적화(trajectory optimization)란 로봇이 초기 상태에서 목표 상태로 이동하는 경로를 역학적 제약 조건과 성능 지표를 만족하도록 결정하는 문제이다. 연속 시간에서의 일반적인 궤적 최적화 문제는 다음과 같이 정식화한다.

\begin{aligned} \min_{\mathbf{x}(t), \mathbf{u}(t)} \quad & \Phi(\mathbf{x}(t_f)) + \int_{t_0}^{t_f} L(\mathbf{x}(t), \mathbf{u}(t)) \, dt \\ \text{subject to} \quad & \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)) \\ & \mathbf{x}(t_0) = \mathbf{x}_0 \\ & \mathbf{g}(\mathbf{x}(t), \mathbf{u}(t)) \leq \mathbf{0} \end{aligned}

여기서 \mathbf{x}(t) \in \mathbb{R}^n은 상태 벡터, \mathbf{u}(t) \in \mathbb{R}^m은 제어 입력 벡터, \Phi는 종단 비용(terminal cost), L은 주행 비용(running cost), \mathbf{f}는 시스템 역학, \mathbf{g}는 경로 제약(path constraint)이다.

2. 이산화와 행렬 구조

2.1 직접 전사법(Direct Transcription)

연속 시간 문제를 이산화하면 유한 차원의 비선형 계획 문제(NLP)로 변환된다. 시간 구간을 N개의 이산 시점 t_0, t_1, \dots, t_N으로 나누고, 각 시점에서의 상태와 제어를 결정 변수로 설정한다.

\mathbf{z} = \begin{bmatrix} \mathbf{x}_0 \\ \mathbf{u}_0 \\ \mathbf{x}_1 \\ \mathbf{u}_1 \\ \vdots \\ \mathbf{x}_N \end{bmatrix} \in \mathbb{R}^{N(n+m)+n}

역학 제약을 오일러 방법으로 이산화하면 다음을 얻는다.

\mathbf{x}_{k+1} = \mathbf{x}_k + h \, \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k), \quad k = 0, 1, \dots, N-1

여기서 h = (t_f - t_0)/N은 시간 간격이다. 이 등식 제약을 벡터 형태로 정리하면 다음과 같다.

\mathbf{c}(\mathbf{z}) = \begin{bmatrix} \mathbf{x}_1 - \mathbf{x}_0 - h\,\mathbf{f}(\mathbf{x}_0, \mathbf{u}_0) \\ \mathbf{x}_2 - \mathbf{x}_1 - h\,\mathbf{f}(\mathbf{x}_1, \mathbf{u}_1) \\ \vdots \\ \mathbf{x}_N - \mathbf{x}_{N-1} - h\,\mathbf{f}(\mathbf{x}_{N-1}, \mathbf{u}_{N-1}) \end{bmatrix} = \mathbf{0}

2.2 제약 자코비안의 밴드 구조

제약 함수 \mathbf{c}(\mathbf{z})의 자코비안 \frac{\partial \mathbf{c}}{\partial \mathbf{z}}는 특징적인 밴드 희소 구조(banded sparse structure)를 가진다. k번째 역학 제약은 \mathbf{x}_k, \mathbf{u}_k, \mathbf{x}_{k+1}에만 의존하므로, 자코비안은 다음과 같은 블록 구조를 가진다.

\frac{\partial \mathbf{c}}{\partial \mathbf{z}} = \begin{bmatrix} -\mathbf{I} - h\mathbf{A}_0 & -h\mathbf{B}_0 & \mathbf{I} & & & \\ & & -\mathbf{I} - h\mathbf{A}_1 & -h\mathbf{B}_1 & \mathbf{I} & \\ & & & & \ddots & \\ & & & & -\mathbf{I} - h\mathbf{A}_{N-1} & -h\mathbf{B}_{N-1} & \mathbf{I} \end{bmatrix}

여기서 \mathbf{A}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{x}}\big\vert_k \in \mathbb{R}^{n \times n}, \mathbf{B}_k = \frac{\partial \mathbf{f}}{\partial \mathbf{u}}\big\vert_k \in \mathbb{R}^{n \times m}이다. 전체 자코비안의 비영(nonzero) 원소 수는 O(N \cdot n \cdot (n + m))으로, 전체 크기 O(N^2 n (n+m))에 비하여 매우 희소하다.

3. 이차 계획 부분 문제(QP Subproblem)

순차 이차 계획법(SQP)이나 내점법을 적용하면, 각 반복에서 다음의 이차 계획 문제를 풀어야 한다.

\begin{aligned} \min_{\Delta \mathbf{z}} \quad & \frac{1}{2} \Delta \mathbf{z}^T \mathbf{H} \, \Delta \mathbf{z} + \mathbf{g}^T \Delta \mathbf{z} \\ \text{subject to} \quad & \mathbf{J} \, \Delta \mathbf{z} = -\mathbf{c} \end{aligned}

KKT(Karush-Kuhn-Tucker) 조건을 적용하면 다음의 선형 시스템을 얻는다.

$$
\begin{bmatrix}
\mathbf{H} & \mathbf{J}^T \
\mathbf{J} & \mathbf{0}
\end{bmatrix}
\begin{bmatrix}
\Delta \mathbf{z} \
\boldsymbol{\lambda}
\end

\begin{bmatrix}
-\mathbf{g} \
-\mathbf{c}
\end{bmatrix}
$$

이 KKT 행렬은 안장점 시스템(saddle-point system)이며, 대칭이지만 부정치(indefinite)이다. 궤적 최적화에서 이 행렬은 블록 삼대각(block tridiagonal) 구조를 가지므로, O(N) 복잡도의 효율적인 알고리즘으로 풀 수 있다.

4. 리카티 재귀(Riccati Recursion)

4.1 블록 소거를 이용한 효율적 풀이

KKT 시스템의 블록 삼대각 구조를 이용한 블록 소거(block elimination)는 이산 시간 리카티 방정식의 후방 재귀(backward recursion)와 동치이다. 헤시안 \mathbf{H}가 시간 단계별로 블록 대각이면, 즉 \mathbf{H} = \text{blkdiag}(\mathbf{Q}_0, \mathbf{R}_0, \mathbf{Q}_1, \mathbf{R}_1, \dots, \mathbf{Q}_N)이면, 리카티 재귀는 다음과 같다.

후방 재귀 (k = N-1, N-2, \dots, 0):

\begin{aligned} \mathbf{S}_N &= \mathbf{Q}_N \\ \mathbf{K}_k &= -(\mathbf{R}_k + \mathbf{B}_k^T \mathbf{S}_{k+1} \mathbf{B}_k)^{-1} \mathbf{B}_k^T \mathbf{S}_{k+1} \mathbf{A}_k \\ \mathbf{S}_k &= \mathbf{Q}_k + \mathbf{A}_k^T \mathbf{S}_{k+1} \mathbf{A}_k + \mathbf{A}_k^T \mathbf{S}_{k+1} \mathbf{B}_k \mathbf{K}_k \end{aligned}

전방 재귀 (k = 0, 1, \dots, N-1):

\begin{aligned} \Delta \mathbf{u}_k &= \mathbf{K}_k \Delta \mathbf{x}_k + \mathbf{d}_k \\ \Delta \mathbf{x}_{k+1} &= \mathbf{A}_k \Delta \mathbf{x}_k + \mathbf{B}_k \Delta \mathbf{u}_k + \mathbf{e}_k \end{aligned}

각 시간 단계의 계산 복잡도는 O(n^3 + n^2 m + nm^2 + m^3)이고, 전체 복잡도는 O(N(n+m)^3)으로 N에 선형이다.

4.2 iLQR/DDP 알고리즘

반복 선형 이차 조절기(iterative Linear Quadratic Regulator, iLQR)와 미분 동적 계획법(Differential Dynamic Programming, DDP)은 리카티 재귀를 비선형 궤적 최적화에 확장한 방법이다. 각 반복에서 현재 궤적을 중심으로 역학과 비용을 선형화(iLQR) 또는 이차 근사(DDP)하고, 리카티 재귀로 최적 피드백 이득 \mathbf{K}_k와 피드포워드 항 \mathbf{d}_k를 계산한다.

DDP에서 각 시간 단계의 이차 근사를 위한 Q-함수는 다음과 같이 정의한다.

Q(\delta \mathbf{x}, \delta \mathbf{u}) = \frac{1}{2} \begin{bmatrix} 1 \\ \delta \mathbf{x} \\ \delta \mathbf{u} \end{bmatrix}^T \begin{bmatrix} Q_0 & \mathbf{Q}_{\mathbf{x}}^T & \mathbf{Q}_{\mathbf{u}}^T \\ \mathbf{Q}_{\mathbf{x}} & \mathbf{Q}_{\mathbf{xx}} & \mathbf{Q}_{\mathbf{ux}}^T \\ \mathbf{Q}_{\mathbf{u}} & \mathbf{Q}_{\mathbf{ux}} & \mathbf{Q}_{\mathbf{uu}} \end{bmatrix} \begin{bmatrix} 1 \\ \delta \mathbf{x} \\ \delta \mathbf{u} \end{bmatrix}

최적 제어 수정량은 \mathbf{Q}_{\mathbf{uu}}의 역행렬을 이용하여 계산한다.

\delta \mathbf{u}^* = -\mathbf{Q}_{\mathbf{uu}}^{-1}(\mathbf{Q}_{\mathbf{u}} + \mathbf{Q}_{\mathbf{ux}} \delta \mathbf{x}) = \mathbf{K} \delta \mathbf{x} + \mathbf{d}

여기서 \mathbf{K} = -\mathbf{Q}_{\mathbf{uu}}^{-1}\mathbf{Q}_{\mathbf{ux}}, \mathbf{d} = -\mathbf{Q}_{\mathbf{uu}}^{-1}\mathbf{Q}_{\mathbf{u}}이다.

5. 콜로케이션 방법과 행렬 구조

5.1 직접 콜로케이션(Direct Collocation)

직접 콜로케이션은 상태 궤적을 다항식으로 근사하고, 콜로케이션 점에서 역학 제약을 부과하는 방법이다. 에르미트-심프슨(Hermite-Simpson) 콜로케이션에서는 각 구간의 중점에서의 상태를 추가 결정 변수로 도입한다.

\mathbf{x}_{k+\frac{1}{2}} = \frac{1}{2}(\mathbf{x}_k + \mathbf{x}_{k+1}) + \frac{h}{8}(\mathbf{f}_k - \mathbf{f}_{k+1})

\mathbf{x}_{k+1} - \mathbf{x}_k = \frac{h}{6}(\mathbf{f}_k + 4\mathbf{f}_{k+\frac{1}{2}} + \mathbf{f}_{k+1})

이 방법은 오일러 방법보다 높은 정확도(4차)를 달성하며, 자코비안의 밴드 폭은 약간 증가하지만 여전히 희소 구조를 유지한다.

5.2 의사 스펙트럴 방법(Pseudospectral Method)

가우스-로바토(Gauss-Lobatto) 점이나 체비셰프(Chebyshev) 점을 사용하는 의사 스펙트럴 방법에서는 미분 행렬 \mathbf{D} \in \mathbb{R}^{(N+1) \times (N+1)}이 핵심적인 역할을 한다.

\dot{\mathbf{x}}(t_k) \approx \sum_{j=0}^{N} D_{kj} \mathbf{x}(t_j)

미분 행렬 \mathbf{D}는 일반적으로 밀집(dense) 행렬이므로, 제약 자코비안도 밀집 블록을 포함한다. 그러나 높은 수렴 차수(spectral convergence)를 달성하므로, 적은 수의 노드로도 높은 정확도를 얻을 수 있다.

6. 희소 행렬 솔버의 활용

대규모 궤적 최적화에서는 희소 행렬 연산이 필수적이다. 주요 기법은 다음과 같다.

희소 행렬의 채움(fill-in)을 최소화하기 위하여 근사 최소 차수(Approximate Minimum Degree, AMD) 순서나 중첩 분할(Nested Dissection) 순서를 적용한다. 궤적 최적화의 경우, 시간 방향의 자연스러운 순서가 이미 좋은 희소성을 보장하는 경우가 많다.

7. 참고 문헌

• Betts, J. T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming (2nd ed.). SIAM.
• Tassa, Y., Erez, T., & Todorov, E. (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. IEEE/RSJ International Conference on Intelligent Robots and Systems, 4906–4913.
• Kelly, M. (2017). An introduction to trajectory optimization: How to do your own direct collocation. SIAM Review, 59(4), 849–904.
• Nocedal, J., & Wright, S. J. (2006). Numerical Optimization (2nd ed.). Springer.
• Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.

v 0.1