7.151 TEB(Timed Elastic Band) 최적화

1. TEB의 개념

TEB(Timed Elastic Band)는 로봇의 국소 경로 계획을 위한 최적화 기반 방법으로, 뢰슈만(Rösmann) 등이 제안하였다. 탄성 밴드(elastic band)의 개념을 시간 매개변수화로 확장한 것이며, 경로의 기하학적 형상과 시간 정보를 동시에 최적화한다.

탄성 밴드는 경유점의 연쇄 $\{(\mathbf{x}_i, \Delta T_i)\}_{i=0}^{n}$ 으로 표현되며, 여기서 $\mathbf{x}_i = (x_i, y_i, \theta_i)$ 는 $i$ 번째 자세(위치와 방향), $\Delta T_i > 0$ 은 $i$ 번째 구간의 시간 간격이다. 이 경유점들은 “탄성적으로” 변형되어 다양한 비용과 제약을 만족하도록 최적화된다.

2. 최적화 문제의 정식화

TEB 최적화는 다수의 목적 항(비용 항)의 가중 합을 최소화한다.

$\min_{\{\mathbf{x}_i, \Delta T_i\}} \quad \sum_k w_k f_k(\{\mathbf{x}_i, \Delta T_i\})$

주요 비용 항은 다음과 같다.

2.1 시간 최적 비용

총 이동 시간을 최소화한다.

$f_{time} = \sum_{i=0}^{n-1} \Delta T_i$

2.2 장애물 회피 비용

각 경유점에서 장애물까지의 거리가 안전 마진 이하일 때 벌점을 부과한다.

$f_{obs} = \sum_i \sum_j \text{pen}(d_{safe} - d(\mathbf{x}_i, \mathcal{O}_j))$

여기서 $\text{pen}(x) = \max(0, x)^2$ 은 벌점 함수이다.

2.3 속도 제약 비용

속도가 허용 범위를 초과하면 벌점을 부과한다.

$f_{vel} = \sum_i \text{pen}\left(\frac{\lVert \mathbf{x}_{i+1} - \mathbf{x}_i \rVert}{\Delta T_i} - v_{\max}\right)$

2.4 가속도 제약 비용

$f_{acc} = \sum_i \text{pen}\left(\left\lVert \frac{\mathbf{v}_{i+1} - \mathbf{v}_i}{\frac{1}{2}(\Delta T_i + \Delta T_{i+1})}\right\rVert - a_{\max}\right)$

2.5 비홀로노믹 제약 비용

차동 구동(differential drive) 로봇이나 자동차형 로봇의 비홀로노믹(nonholonomic) 구속을 소프트 제약으로 부과한다.

$f_{kin} = \sum_i \left(\dot{y}_i\cos\theta_i - \dot{x}_i\sin\theta_i\right)^2$

3. 그래프 기반 최적화

TEB의 최적화 문제는 하이퍼그래프(hypergraph) 기반의 구조로 표현된다. 각 경유점과 시간 간격이 그래프의 노드(정점)이고, 각 비용/제약 항이 관련 노드들을 연결하는 에지(간선)이다. 이 구조를 활용하여 희소 야코비안을 효율적으로 구성하고, 가우스-뉴턴법 또는 레벤버그-마쿼트 알고리즘으로 최적화한다.

g2o(General Graph Optimization) 프레임워크가 TEB의 표준 최적화 엔진으로 사용되며, 희소 선형 시스템의 효율적 해법을 제공한다.

4. 경유점의 적응적 삽입과 제거

TEB에서 경유점의 수는 고정이 아니라 적응적으로 조절된다.

삽입: 인접 경유점 간의 거리가 최대 허용 간격을 초과하면 중간에 새 경유점을 삽입한다.

제거: 인접 경유점 간의 거리가 최소 허용 간격 이하이면 경유점을 제거한다.

이 적응적 조절에 의해 궤적의 해상도가 필요에 따라 자동으로 조정된다.

5. 동적 장애물 회피

TEB의 시간 정보( $\Delta T_i$ )를 활용하면 동적 장애물의 예측 궤적을 고려한 시공간(space-time) 충돌 회피가 가능하다. $i$ 번째 경유점의 도달 시각 $t_i = t_0 + \sum_{j=0}^{i-1}\Delta T_j$ 에서 장애물의 예측 위치 $\mathbf{o}_j(t_i)$ 를 사용하여 거리를 계산한다.

$d(\mathbf{x}_i, \mathcal{O}_j(t_i)) \geq d_{safe}$

6. 다중 위상 경로(Multi-Topology Planning)

장애물의 좌우 통과와 같이 위상적으로 다른 경로의 후보가 존재할 때, TEB는 다수의 후보 밴드를 동시에 유지하고 각각을 독립적으로 최적화한다. 최종적으로 비용이 가장 낮은 밴드를 선택하여 실행한다. 이는 국소 최적해에 갇히는 문제를 완화한다.

7. ROS에서의 구현

TEB는 ROS(Robot Operating System)의 navigation 스택에서 국소 계획기 플러그인(teb_local_planner)으로 구현되어 있으며, 이동 로봇의 국소 경로 계획에서 널리 사용된다. 차동 구동, 자동차형(Ackermann), 전방향(holonomic) 등 다양한 로봇 모델을 지원한다.

8. 참고 문헌

Rösmann, C., Feiten, W., Wösch, T., Hoffmann, F., & Bertram, T. (2012). “Trajectory Modification Considering Dynamic Constraints of Autonomous Robots.” Proceedings of ROBOTIK, 1–6.
Rösmann, C., Hoffmann, F., & Bertram, T. (2017). “Kinodynamic Trajectory Optimization and Control for Car-Like Robots.” Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 5681–5686.
Kümmerle, R., et al. (2011). “g2o: A General Framework for Graph Optimization.” Proceedings of ICRA, 3607–3613.

version: 1.0