(AD) Obstacle Avoidance

Time Elastic Band

개요

Time Elastic Band(TEB)는 로봇의 지역 경로 계획(Local Path Planning) 및 충돌 회피를 위한 최적화 기반 알고리즘으로, DWA와 마찬가지로 로봇의 현재 위치에서 목표 지점까지의 최적 궤적(Trajectory) 을 실시간으로 생성한다.
TEB는 로봇의 궤적을 시간적으로 연결된 일련의 포즈(poses)들로 구성된 “탄성 밴드(Elastic Band)“로 표현하고, 이 밴드를 최적화하여 로봇의 동적 제약, 장애물 회피, 목표 지점 도달, 그리고 시간 효율성을 동시에 고려한 최적의 궤적을 찾는다.
특히, 궤적의 각 포즈 사이의 시간 간격까지 최적화함으로써, 단순히 공간적인 경로뿐만 아니라 시간적인 측면까지 고려한 동적 궤적을 생성하는 것이 특징이다.

주요특징

APF : 힘(Potential)을 이용하여 장애물을 회피
DWA : 속도 공간(Velocity Space)에서 안전한 속도를 선택
TEB : 위치와 시간을 고려하여 Trajectory 자체를 최적화
- TEB는 예측 구간 전체를 최적화 하므로 DWA에 비해 상대적으로 계산량이 크다.
- Global Path를 초기 Trajectory(초기 추정값)로 사용하면서 Cost Function에도 반영한다.

TEB의 궤적 표현 (Trajectory Representation)

TEB는 이동 로봇의 이동 경로를 단순한 좌표의 나열이 아니라 시간 정보가 포함된 궤적(Timed Trajectory) 으로 표현한다.
일반적인 경로는 $P$ 처럼 표현하지만, TEB에서 하나의 Trajectory $B$ 는 다음과 같이 정의된다.

P = {p_{0}, p_{1}, \dots, p_{n}}

B = (P_{0}, Δ T_{0}, P_{1}, Δ T_{1}, \dots, P_{n})

$P_{i}$ : $i$ 번째 Pose
$Δ T$ : 두 Pose 사이의 이동시간
즉, 위치와 시간 ${P_{i}, Δ T_{i}}$ 을 동시에 최적화 하는 것이 TEB 방법의 핵심이다.

+full

Pose Sequence

차량(이동 로봇)의 상태는 일반적으로 아래와 같이 표현된다.

P_{i} = (x_{i}, y_{i}, θ_{i})

Pose Sequence : P_{0} \to P_{1} \to \dots \to P_{n}

$x_{i}, y_{i}$ : 위치
$θ_{i}$ : 헤딩 각도
TEB에서 Trajectory는 이러한 Pose Sequence로 구성되며, Global Planner가 생성한 Global Path가 초기 Pose Sequence가 된다.
이후 최적화 과정에서는 각 Pose가 자유롭게 이동하며 더 좋은 Trajectory를 찾는다.

Time Intervals

Pose 사이에는 시간 간격 $Δ T$ 가 존재하며 아래와 같이 표현한다.

Δ T_{i} = t_{t + 1} - t_{i}

따라서 선속도 $v$ 와 각속도 $ω$ 는 다음과 같이 계산할 수 있다.

v_{i} = \frac{∣∣ P _{i + 1} - P _{i} ∣∣}{Δ T _{i}}

ω_{i} = \frac{θ _{i + 1} - θ _{i}}{Δ T _{i}}

TEB는 모든 로봇이 궤적을 완료하는 데 걸리는 총 시간 $T_{a ll} = \sum_{i = 0}^{N - 1} Δ T_{i}$ 을 최소화하는 방향으로 궤적을 최적화한다.

TEB 최적화 (Optimization)

궤적을 구성하는 포즈 시퀀스와 시간 간격들을 조정하여, 여러 목적 함수(Objective Function)를 최소화하고 동시에 다양한 제약 조건(Constraints)을 만족시키는 최적의 궤적을 찾는 과정이다.

목적 함수 (Objective Function) 정의

TEB의 목적 함수는 아래와 같이 여러개의 목표를 최소화하도록 구성되어 있다.

F (B) = w_{t} J_{t} + w_{o} J_{o} + w_{v} J_{v} + w_{a} J_{a} + w_{k} J_{k} + w_{g} J_{g} + \dots

여기서 $J ()$ 는 각 평가 항목에 대한 비용 함수(Cost Function)이며, $w$ 는 각 항목의 가중치를 나타낸다.
이러한 개별 목적 함수들과 가중치를 합산하여 최종 목적 함수 $F (B)$ 를 구성하며, $F (B)$ 를 최소화하는 궤적 밴드 $B$ 를 찾는 것을 목표로 한다.

목적 함수 구성요소

$J_{t} (B)$ : 시간 최소화

로봇이 전체 궤적을 완료하는 데 걸리는 총 시간을 최소화하여, 로봇이 가능한 빠르게 목표 지점에 도달하도록 유도한다.

J_{t} (B) = i = 0 \sum N - 1 Δ T_{i}

$J_{o} (B)$ : 장애물 회피

궤적이 주변 장애물과 안전 거리를 유지하도록 하여, 궤적이 장애물에 가까워질수록 높은 페널티를 부과한다.

J_{o} (B) = i = 0 \sum N - 1 f_{o b s} (distnace (P_{i}, Obstacles))

f_{o b s} (d) = {(d_{s a f e} - distance)^{2}, 0, distance < d_{s a f e} distance \geq d_{s a f e}

$distance (P_{i}, Obstacle)$ : 궤적의 포즈 $P_{i}$ 에서 가장 가까운 장애물까지의 거리
$f_{o b s} (d)$ : 특정 안전 거리 $d_{s a f e}$ 보다 작아질 때 급격히 증가하는 페널티 함수

$J_{v} (B)$ : 속도 제약

로봇의 선속도 $v$ 와 각속도 $ω$ 가 물리적인 최대 한계를 초과하지 않도록 한다.

J_{v} B = i = 0 \sum N - 1 (f_{v, l in} (v_{i}) + f_{v, an g} (ω_{i}))

f_{v, l in} (v) = (max (0, ∣ v ∣ - V_{ma x}))^{2}

f_{v, an g} (ω) = (max (0, ∣ ω ∣ - W_{ma x}))^{2}

$v_{i}, ω_{i}$ : 포즈 $P_{i}$ 와 $P_{i + 1}$ 사이의 구간에서 추정된 선속도와 각속도
$f_{v, l in} (v), f_{v, an g} (ω)$ : 속도가 최대 한계( $V_{ma x}, W_{ma x}$ )를 초과할 때 페널티를 부여하는 함수

$J_{a} (B)$ : 가속도 제약

로봇의 선가속도 $a$ 와 각가속도 $\overset{ω}{˙}$ 가 물리적인 최대 한계를 초과하지 않도록 하여 궤적의 부드러움과 로봇의 안정적인 움직임에 기여한다.

J_{a} B = i = 0 \sum N - 1 (f_{a, l in} (a_{i}) + f_{a, an g} (\overset{ω}{˙}_{i}))

f_{a, l in} (a) = (max (0, ∣ a ∣ - A_{ma x}))^{2}

f_{a, an g} (\overset{ω}{˙}) = (max (0, ∣ \overset{ω}{˙} ∣ - \dot{W}_{ma x}))^{2}

$a_{i}, \overset{ω}{˙}_{i}$ : 두 구간의 속도 변화로부터 추정된 선가속도, 각가속도
$f_{a, l in} (a), f_{a, an g} (\overset{ω}{˙})$ : 가속도가 최대 한계( $A_{ma x}, \dot{W}_{ma x}$ )를 초과할 때 페널티를 부여하는 함수

$J_{k} (B)$ : 궤적의 부드러움/곡률 제약

궤적이 급격한 방향 전환이나 불연속적인 움직임을 피하고 부드럽게(Smoothness) 이어지도록 하여 로봇의 안정적인 주행과 에너지 효율성에 기여한다.

J_{k} (B) = i = 0 \sum N - 2 (ω_{i + 1} - ω_{i})^{2}

연속된 구간의 각속도 변화를 최소화하여 궤적의 각가속도를 줄이고 부드러움을 유도한다.
다른 형태로는 궤적의 곡률(curvature) $k$ 변화를 최소화하는 항을 사용할 수도 있다.

$J_{g} (B)$ : 전역 경로 추종

TEB가 전역 플래너(Global Path Planner)가 제공한 대략적인 전역 경로에서 너무 멀리 벗어 나지 않도록 한다.

J_{g} (B) = i = 0 \sum N - 1 distance (P_{i}, GlobalPath)^{2}

$distance (P_{i}, GlobalPath)$ : 궤적의 포즈 $P_{i}$ 에서 가장 가까운 전역 경로상의 점까지의 거리

제약 조건 (Constraints)

제약 조건은 최적화 과정에서 반드시 만족되어야 하는 조건들로 TEB에서는 이러한 제약 조건들을 목적 함수에 페널티 항으로 포함시켜 Soft Constraint 형태로 최적화하는 경우가 많지만, 일부는 Hard Constraint 으로 처리될 수도 있다.

시작 및 목표 포즈 고정

궤적의 시작점은 현재 로봇의 위치로, 끝점은 전역 경로에서 주어진 다음 목표 지점으로 고정된다.

P_{0} = P_{c u rre n t}, P_{N - 1} = P_{g o a l}

시간 간격 양수

각 포즈 사이의 시간 간격은 물리적으로 항상 양수어야 한다.

Δ T_{i} > 0

로봇의 물리적 한계

로봇의 최대 속도, 가속도, 각속도 등은 일반적으로 목적 함수의 페널티 항으로 처리되지만, Hard Constraint로 설정될 수도 있다.

∣ v_{i} ∣ \leq V_{ma x}

∣ ω_{i} ∣ \leq W_{ma x}

∣ a_{i} ∣ \leq A_{ma x}

또한 TEB는 궤적의 각 포즈 $P_{i} = (x_{i}, y_{i}, θ_{i})$ 에서 방향 정보 $θ$ 를 포함하는 구조이므로 Holonomic 및 Non-holonomic 로봇에 대해 각 종류에 맞는 적절한 제약 조건 및 페널티 항을 설정하여 최적의 궤적을 생성할 수 있다.

충돌 회피

궤적의 어떤 부분도 장애물과 충돌하면 안되기 때문에, $J_{o}$ 페널티 항에서 처리되지만 직접적인 비선형 제약으로 추가될 수도 있다.

distance (P_{i}, Obstacle) \geq d_{s a f e}

최적화 과정

TEB는 앞서 정의한 목적 함수 $F (B)$ 를 최소화하는 궤적 $B$ 를 찾기 위해 비선형 최적화 문제 (Non-linear Optimization) 로 정의하고, 그래프 기반(Graph-based) 최적화를 통해 반복적으로 수정하여 최적 궤적을 계산한다.
TEB는 기본적으로 Graph-based Nonlinear Least Squares Optimization 방법을 통해 목적 함수를 최소화한다.

초기 궤적 생성 (Trajectory Inirialization)

Global Planner로부터 전달받은 경로를 초기 궤적으로 사용한다.
전역 경로가 없거나 매우 단순한 경우, 목표 지점까지의 직선 경로를 초기 궤적으로 사용할 수 있다.
일반적으로 Global Path에는 $Δ T$ 가 존재하지 않으므로, 현재 속도 또는 기준 속도를 이용하여 직접 초기화 한다.

B_{0} = (P_{0}, Δ T_{0}, P_{1}, Δ T_{1}, \dots, P_{N})

P_{i} = (x_{i}, y_{i}, θ_{i})

반복 최적화

초기 궤적이 생성되면, Graph-based Nonlinear Least Squares를 통해 목적 함수 $F (B)$ 를 최적화 한다.
목적 함수 $F (B)$ 는 여러 개의 개별 비용 함수 $J_{(-)} (B)$ 들의 가중치 합으로 구성되며, 각 비용 함수는 일반적으로 제곱 오차의 형태로 정의된다.

F (B) = k \sum w_{k} \cdot (erro r_{k} (B))^{2}

이러한 형태의 목적 함수는 비선형 최소 제곱 문제로 간주될 수 있으며, 그래프 기반 최적화를 통해 문제를 해결할 수 있다.

그래프 생성

TEB에서는 궤적을 그래프로 표현하여 계산 구조를 단순화 시킨다.

노드 (Nodes)

궤적을 구성하는 최적화 변수들로, 각 포즈 $P_{i} = (x_{i}, y_{i}, θ_{i})$ 와 각 시간 간격 $Δ t_{i}$ 가 그래프의 노드가 된다.

엣지 (Edges)

목적 함수를 구성하는 각 비용 함수 $J_{(-)} (B)$ 항들이 그래프의 엣지가 된다.
각 엣지는 하나 이상의 노드(관련된 포즈나 시간 간격)에 연결되어 해당 노드들의 값에 따라 비용을 발생시킨다.
예를 들어, $J_{t} (B) = \sum Δ T_{i}$ 는 모든 $Δ T_{i}$ 노드에 연결된 엣지로 볼 수 있다.
장애물 회피 함수 $J_{o} (B)$ 는 각 $P_{i}$ 노드와 장애물 정보에 연결된 엣지이며, 속도 제약 함수 $J_{v} (B)$ 는 $P_{i}, P_{i + 1}, Δ T_{i}$ 노드에 연결된 엣지이다.

변수 업데이트

현재 궤적 $B$ 에 대해 목적 함수를 구성하는 각 비용함수의 오차(비용)을 계산한다.
각 비용 함수가 최적화 변수( $P_{i}, Δ T_{i}$ )에 대해 얼마나 민감하게 변하는지에 대한 기울기(gradient) 정보인 자코비안(Jacobian) 행렬을 계산한다.
계산된 오차와 기울기를 바탕으로, 최적화 알고리즘 (Gauss-Newton, Levenberg-Marquardt)은 궤적을 구성하는 변수( $P_{i}, Δ T_{i}$ )를 업데이트하여 목적 함수의 비용을 감소시킨다.

수렴 및 종료

목적 함수의 값이 특정 임계값 이하로 떨어지거나, 미리 설정된 최대 반복 횟수에 도달했을 때 반복 업데이트를 종료한다.

실시간 재계획

TEB는 매 제어 주기마다 최적화 과정을 반복하여, 동적인 환경 변화에 유연하게 대응한다.
로봇이 한 스텝 이동하고 나면, 새로운 로봇 위치를 시작 포즈로 하여 다시 초기 궤적을 생성하고 최적화를 수행한다.

TEB의 장점 및 한계

장점

위치와 시간을 동시에 최적화 - Pose와 시간 간격 $Δ T$ 을 함께 최적화하므로, 최단 경로뿐만 아니라 이동 시간, 속도, 가속도까지 동시에 고려한 시간적으로 실행 가능한(Timed Trajectory) 궤적을 생성할 수 있다.
Graph Optimization을 이용한 높은 계산 효율 - 모든 변수들이 서로 연결되어 있지 않고, 각 비용이 일부 변수에만 영향을 미치므로, Jacobian과 Hessian이 희소 행렬(Sparse Matrix) 형태를 갖는다. 이를 이용한 Graph Optimization(g2o)은 연산량과 메모리 사용량을 크게 줄여, 전체 Trajectory를 실시간으로 반복 최적화할 수 있다.
-유연한 목적 함수 구성 (Flexible Objective Function): 다양한 평가 항목(시간, 장애물, 속도, 가속도, 부드러움, 전역 경로 추종 등)을 가중치와 함께 목적 함수에 유연하게 추가할 수 있어, 로봇의 특성이나 주행 목적에 맞춰 알고리즘을 쉽게 조정할 수 있다.

한계

비선형 최적화로 인한 높은 계산 비용 - 매 제어 주기마다 비선형 최소제곱 문제를 반복적으로 해결해야 하므로 DWA보다 계산량이 크며, 실시간 성능은 하드웨어 성능과 최적화 변수의 개수에 영향을 받는다.
목적함수의 가중치(Parameter Tuning)에 민감 - 장애물 회피, 이동 시간, 전역 경로 추종 등의 비용 항은 가중치 $w_{i}$ 에 의해 균형이 결정되므로, 환경에 맞는 파라미터를 적절히 조정하지 않으면 원하는 주행 성능을 얻기 어렵다.
지역 최적해(Local Optimum) 문제 - Gradient 기반 최적화의 특성상 복잡한 환경에서는 전역 최적해가 아닌 지역 최적해에 수렴할 수 있다. 특히 매우 복잡하거나 좁은 환경에서는 전역적으로 최적의 경로를 찾지 못하고, 비효율적인 경로를 선택하거나 아예 경로를 찾지 못할 수도 있다.

TEB 시뮬레이션 개발

(AD) Obstacle Avoidance Simulation

TEB

원본 링크

탐색기

최근 게시글

(AD) Obstacle Avoidance - TEB

Time Elastic Band

개요

주요특징

TEB의 궤적 표현 (Trajectory Representation)

Pose Sequence

Time Intervals

TEB 최적화 (Optimization)

목적 함수 (Objective Function) 정의

목적 함수 구성요소

Jt​(B) : 시간 최소화

Jo​(B) : 장애물 회피

Jv​(B) : 속도 제약

Ja​(B) : 가속도 제약

Jk​(B) : 궤적의 부드러움/곡률 제약

Jg​(B) : 전역 경로 추종

제약 조건 (Constraints)

시작 및 목표 포즈 고정

시간 간격 양수

로봇의 물리적 한계

충돌 회피

최적화 과정

초기 궤적 생성 (Trajectory Inirialization)

반복 최적화

그래프 생성

노드 (Nodes)

엣지 (Edges)

변수 업데이트

수렴 및 종료

실시간 재계획

TEB의 장점 및 한계

장점

한계

TEB 시뮬레이션 개발

TEB

참고

그래프 뷰

목차

백링크

최근 게시글

$J_{t} (B)$ : 시간 최소화

$J_{o} (B)$ : 장애물 회피

$J_{v} (B)$ : 속도 제약

$J_{a} (B)$ : 가속도 제약

$J_{k} (B)$ : 궤적의 부드러움/곡률 제약

$J_{g} (B)$ : 전역 경로 추종