7.142 궤적 최적화 문제의 정식화

1. 궤적 최적화의 정의

궤적 최적화(trajectory optimization)는 시스템의 동역학 제약과 물리적 한계를 만족하면서 주어진 성능 지표를 최적화하는 시간 매개변수화된 상태-제어 궤적을 계산하는 문제이다. 최적 제어 문제의 수치적 구현에 해당하며, 연속 시간 문제를 유한 차원 비선형 계획 문제(NLP)로 이산화하여 풀어야 한다.

2. 연속 시간 정식화

로봇 궤적 최적화의 일반적 형태는 다음과 같다.

$\min_{\mathbf{x}(\cdot), \mathbf{u}(\cdot), t_f} \quad \phi(\mathbf{x}(t_f), t_f) + \int_{t_0}^{t_f} L(\mathbf{x}(t), \mathbf{u}(t), t) \, dt$

$\text{s.t.} \quad \dot{\mathbf{x}}(t) = \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t), t)$

$\mathbf{x}(t_0) = \mathbf{x}_0$

$\boldsymbol{\psi}(\mathbf{x}(t_f), t_f) = \mathbf{0}$

$\mathbf{g}(\mathbf{x}(t), \mathbf{u}(t), t) \leq \mathbf{0}, \quad \forall t \in [t_0, t_f]$

여기서 $\mathbf{x}(t) \in \mathbb{R}^{n_x}$ 는 상태, $\mathbf{u}(t) \in \mathbb{R}^{n_u}$ 는 제어 입력, $\mathbf{f}$ 는 동역학, $\boldsymbol{\psi}$ 는 종단 제약, $\mathbf{g}$ 는 경로 제약이다.

3. 이산화 기법

연속 시간 문제를 유한 차원 NLP로 변환하기 위해 시간 축을 $N$ 개의 구간으로 이산화한다.

$t_0 = \tau_0 < \tau_1 < \cdots < \tau_N = t_f$

3.1 직접 사격법(Direct Shooting)

제어 입력을 각 구간에서 매개변수화하고( $\mathbf{u}(t) = \mathbf{u}_k$ , $t \in [\tau_k, \tau_{k+1}]$ ), 상태를 순방향 수치 적분으로 계산한다.

$\mathbf{x}_{k+1} = \Phi(\mathbf{x}_k, \mathbf{u}_k; \tau_k, \tau_{k+1})$

결정 변수: $\mathbf{z} = [\mathbf{u}_0^T, \ldots, \mathbf{u}_{N-1}^T]^T \in \mathbb{R}^{N \cdot n_u}$

장점은 결정 변수의 수가 적고, 동역학이 항상 만족된다는 점이다. 단점은 초기 상태의 오차가 후기로 증폭되는 수치적 불안정성과, 상태 경로 제약의 직접 부과가 어렵다는 점이다.

3.2 직접 배치법(Direct Collocation)

상태와 제어 입력을 모두 결정 변수로 유지하고, 동역학을 배치점(collocation point)에서의 등식 제약으로 부과한다.

결정 변수: $\mathbf{z} = [\mathbf{x}_0^T, \mathbf{u}_0^T, \ldots, \mathbf{x}_N^T, \mathbf{u}_N^T]^T \in \mathbb{R}^{N(n_x + n_u) + n_x}$

에르미트-심프슨(Hermite-Simpson) 배치에서 구간 중점의 결함(defect) 제약은 다음과 같다.

$\mathbf{x}_{k+1} - \mathbf{x}_k - \frac{h_k}{6}(\mathbf{f}_k + 4\mathbf{f}_{k+1/2} + \mathbf{f}_{k+1}) = \mathbf{0}$

장점은 상태 경로 제약의 직접 부과가 용이하고, 수치적으로 안정적이며, NLP 해법기의 희소 구조 활용이 가능하다는 점이다.

3.3 직접 전사법(Direct Transcription)

직접 배치법의 일반적 형태로, 오일러법, 사다리꼴법, 룽게-쿠타법 등 다양한 수치 적분 방식으로 동역학을 이산화한다.

3.4 다중 사격법(Multiple Shooting)

단일 사격법과 직접 배치법의 중간으로, 시간을 여러 구간으로 나누고 각 구간의 초기 상태를 독립 결정 변수로 유지한다. 구간 경계에서의 연속성 조건이 등식 제약으로 부과된다.

$\Phi(\mathbf{x}_k, \mathbf{u}_k; \tau_k, \tau_{k+1}) - \mathbf{x}_{k+1} = \mathbf{0}$

수치적 안정성이 단일 사격법보다 우수하며, 동역학의 병렬 적분이 가능하다.

4. NLP의 구조

이산화된 궤적 최적화 NLP는 다음의 특징적 구조를 갖는다.

시간 밴드 구조: 인접한 시간 단계의 변수만이 동역학 제약으로 결합되므로, 야코비안과 헤시안이 블록 밴드(block-banded) 구조를 갖는다.

대규모 희소성: 결정 변수의 수가 $O(N \cdot (n_x + n_u))$ 로 크지만, 비영 원소의 수는 $O(N \cdot (n_x + n_u)^2)$ 으로 전체 행렬 크기에 비해 매우 적다.

제약의 비선형성: 비선형 동역학에서 유래하는 등식 제약이 비선형이다.

이 구조를 활용한 전용 NLP 해법기(IPOPT, acados)가 효율적인 해법을 제공한다.

5. 시간 할당의 처리

종단 시각 $t_f$ 가 자유인 경우, 시간 할당이 추가적인 최적화 변수가 된다.

고정 격자 + 시간 스케일링: $s \in [0, 1]$ 로 정규화된 시간을 사용하고, 실제 시간 $t = t_0 + s(t_f - t_0)$ 에서 $t_f$ 를 최적화한다.

가변 격자: 각 구간의 시간 길이 $h_k = \tau_{k+1} - \tau_k$ 를 결정 변수로 포함한다.

6. 로봇 궤적 최적화의 전형적 구성

구성 요소	내용
상태 변수	$\mathbf{q}, \dot{\mathbf{q}}$ (관절 위치, 속도)
제어 입력	$\boldsymbol{\tau}$ (관절 토크) 또는 $\ddot{\mathbf{q}}$ (관절 가속도)
동역학	$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \boldsymbol{\tau}$
비용 함수	토크 노름, 저크, 시간, 에너지
경로 제약	관절 한계, 속도 한계, 장애물 회피, 마찰 원추
경계 조건	초기/종단 자세, 속도

7. 참고 문헌

Betts, J. T. (2010). Practical Methods for Optimal Control and Estimation Using Nonlinear Programming (2nd ed.). SIAM.
Kelly, M. (2017). “An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation.” SIAM Review, 59(4), 849–904.
Diehl, M., Bock, H. G., Diedam, H., & Wieber, P.-B. (2006). “Fast Direct Multiple Shooting Algorithms for Optimal Robot Control.” In Fast Motions in Biomechanics and Robotics, Lecture Notes in Control and Information Sciences, 340, 65–93.
Wächter, A., & Biegler, L. T. (2006). “On the Implementation of an Interior-Point Filter Line-Search Algorithm for Large-Scale Nonlinear Programming.” Mathematical Programming, 106(1), 25–57.

version: 1.0