7.152 CHOMP와 경사 기반 경로 최적화

1. CHOMP의 개요

CHOMP(Covariant Hamiltonian Optimization for Motion Planning)는 래틀리프(Ratliff) 등이 2009년에 제안한 경사 기반 궤적 최적화 방법이다. 초기 궤적(통상 시작점과 목표점을 잇는 직선)에서 출발하여, 매끄러움 비용과 장애물 비용의 가중 합을 경사 하강법으로 반복적으로 최소화하여 충돌 없는 매끄러운 궤적을 생성한다.

2. 비용 함수의 구성

CHOMP의 비용 함수는 두 항의 합으로 구성된다.

$\mathcal{U}[\boldsymbol{\xi}] = f_{smooth}[\boldsymbol{\xi}] + \lambda f_{obs}[\boldsymbol{\xi}]$

2.1 매끄러움 비용

궤적의 동역학적 매끄러움을 측정한다. 가속도 노름의 적분이 통상적이다.

$f_{smooth}[\boldsymbol{\xi}] = \frac{1}{2}\int_0^T \lVert \ddot{\boldsymbol{\xi}}(t) \rVert^2 dt$

이산화하면 $f_{smooth} = \frac{1}{2}\boldsymbol{\xi}^T\mathbf{A}\boldsymbol{\xi}$ 로 궤적 벡터 $\boldsymbol{\xi} = [\boldsymbol{\xi}_1^T, \ldots, \boldsymbol{\xi}_N^T]^T$ 에 대한 이차 형식이 된다. $\mathbf{A}$ 는 유한 차분에 의한 가속도 행렬의 그램(Gram) 행렬이다.

2.2 장애물 비용

작업 공간에서 로봇 표면의 각 점이 장애물에 의한 비용 필드 $c(\mathbf{x})$ 를 통과하면서 축적하는 비용이다.

$f_{obs}[\boldsymbol{\xi}] = \int_0^T \int_{\mathcal{B}} c(\mathbf{x}(\boldsymbol{\xi}(t), u)) \lVert \dot{\mathbf{x}}(\boldsymbol{\xi}(t), u) \rVert \, du \, dt$

여기서 $\mathcal{B}$ 는 로봇 표면의 매개변수 공간, $\mathbf{x}(\boldsymbol{\xi}(t), u)$ 는 관절 자세 $\boldsymbol{\xi}(t)$ 에서 표면 점 $u$ 의 작업 공간 좌표이다. $\lVert \dot{\mathbf{x}} \rVert$ 항은 속도에 비례하는 가중으로, 빠르게 이동하는 구간에서 장애물 비용을 증가시킨다.

비용 필드 $c(\mathbf{x})$ 는 부호 거리 함수(SDF)로부터 구성된다.

$c(\mathbf{x}) = \begin{cases} -d(\mathbf{x}) + \frac{1}{2}\epsilon & d(\mathbf{x}) < 0 \\ \frac{1}{2\epsilon}(d(\mathbf{x}) - \epsilon)^2 & 0 \leq d(\mathbf{x}) \leq \epsilon \\ 0 & d(\mathbf{x}) > \epsilon \end{cases}$

3. 공변 경사 하강법

3.1 함수 공간에서의 그래디언트

CHOMP의 핵심 기여는 궤적 공간에서의 경사 하강이 매끄러움 메트릭(metric)에 의해 사전 조건화되어야 함을 인식한 것이다. 유클리드 메트릭에서의 그래디언트 $\nabla \mathcal{U}$ 는 궤적의 매끄러움을 해치는 고주파 갱신을 산출하지만, 매끄러움 메트릭 $\mathbf{A}$ 에 의한 공변 그래디언트(covariant gradient) $\mathbf{A}^{-1}\nabla \mathcal{U}$ 는 매끄러운 갱신을 산출한다.

$\boldsymbol{\xi}_{k+1} = \boldsymbol{\xi}_k - \eta \mathbf{A}^{-1}\nabla \mathcal{U}[\boldsymbol{\xi}_k]$

$\mathbf{A}^{-1}$ 의 적용은 저역 통과 필터의 역할을 수행하여, 갱신이 매끄러운 궤적 변형에 해당하도록 보장한다.

3.2 해밀토니안 몬테카를로와의 관계

CHOMP의 해밀토니안 동역학 변형은 운동 에너지 항을 추가하여 모멘텀 기반 가속을 달성한다. 이는 네스테로프 가속법의 궤적 공간 확장으로 해석된다.

4. 장애물 비용의 그래디언트 계산

장애물 비용의 관절 공간 그래디언트는 연쇄 법칙에 의해 계산된다.

$\nabla_{\boldsymbol{\xi}} f_{obs} = \sum_u \mathbf{J}_u^T(\boldsymbol{\xi}) \nabla_{\mathbf{x}} c(\mathbf{x}(\boldsymbol{\xi}, u)) \lVert \dot{\mathbf{x}} \rVert$

여기서 $\mathbf{J}_u$ 는 표면 점 $u$ 의 야코비안이다. SDF의 그래디언트 $\nabla_{\mathbf{x}} c$ 는 장애물 표면의 법선 방향으로 향하며, 3차원 격자에 사전 계산되어 빠른 조회가 가능하다.

5. CHOMP의 알고리즘

초기 궤적 $\boldsymbol{\xi}_0$ (시작-목표 직선 보간) 설정
SDF와 비용 필드 $c(\mathbf{x})$ 를 환경으로부터 구성
$k = 0, 1, \ldots$ 에 대해:
$\quad$ 장애물 비용의 그래디언트 $\nabla f_{obs}$ 계산
$\quad$ 매끄러움 비용의 그래디언트 $\nabla f_{smooth} = \mathbf{A}\boldsymbol{\xi}_k$ 계산
$\quad$ 공변 갱신: $\boldsymbol{\xi}_{k+1} = \boldsymbol{\xi}_k - \eta\mathbf{A}^{-1}(\mathbf{A}\boldsymbol{\xi}_k + \lambda\nabla f_{obs})$
$\quad$ 수렴 판정

6. 장점과 한계

장점: 구현이 상대적으로 단순하고, SDF의 사전 계산으로 장애물 비용의 평가가 빠르다. 공변 그래디언트에 의한 매끄러운 갱신이 물리적으로 합리적인 궤적을 산출한다.

한계: 비볼록 장애물 환경에서 국소 최적해(잘못된 위상의 경로)에 갇힐 수 있다. 초기 궤적이 장애물 내부를 관통하면 탈출이 어려운 경우가 있다. 제약이 소프트 벌점으로 처리되므로 미소한 충돌이 잔존할 수 있다.

7. STOMP와의 비교

STOMP(Stochastic Trajectory Optimization for Motion Planning)는 CHOMP의 확률적 변형으로, 그래디언트 대신 확률적 궤적 샘플링에 의해 갱신을 수행한다. 목적 함수의 미분 가능성을 요구하지 않으므로, 비매끄러운 비용 함수에도 적용 가능하다. 그러나 수렴이 CHOMP보다 느릴 수 있다.

8. 참고 문헌

Ratliff, N., Zucker, M., Bagnell, J. A., & Srinivasa, S. (2009). “CHOMP: Gradient Optimization Techniques for Efficient Motion Planning.” Proceedings of ICRA, 489–494.
Zucker, M., et al. (2013). “CHOMP: Covariant Hamiltonian Optimization for Motion Planning.” IJRR, 32(9-10), 1164–1193.
Dragan, A. D., Ratliff, N., & Srinivasa, S. (2011). “Manipulation Planning with Goal Sets Using Constrained Trajectory Optimization.” Proceedings of ICRA, 4582–4588.

version: 1.0