7.148 충돌 회피 제약 조건의 통합

7.148 충돌 회피 제약 조건의 통합

1. 충돌 회피 제약의 필요성

로봇 궤적 최적화에서 충돌 회피(collision avoidance)는 가장 핵심적인 제약 중 하나이다. 로봇이 환경의 장애물이나 자기 자신의 다른 링크와 충돌하지 않도록 보장해야 하며, 이는 비볼록 부등식 제약으로 정식화된다.

2. 거리 기반 제약의 수학적 표현

로봇의 기하학적 모델과 장애물 모델 사이의 최소 거리 d(\mathbf{q})를 이용한 제약은 다음과 같다.

d(\mathbf{q}(t)) \geq d_{safe}, \quad \forall t \in [t_0, t_f]

여기서 d_{safe} > 0은 안전 마진이다. 이 연속 시간 제약은 이산화하여 격자점에서 부과하거나, 볼록 포 기반 충분 조건으로 변환한다.

3. 최적화 문제에의 통합 방법

3.1 벌점법(Penalty Approach)

충돌 회피 제약을 벌점 항으로 목적 함수에 가산한다.

f_{obs}(\mathbf{q}) = \sum_{(i,j)} \int_0^T c(\lVert \mathbf{p}_i(\mathbf{q}(t)) - \mathbf{o}_j \rVert) \, dt

여기서 c(d)는 거리 d가 안전 마진 이하일 때 양의 비용을 부과하는 함수이다. CHOMP에서 사용되는 비용 함수의 예는 다음과 같다.

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

벌점 강도를 점진적으로 증가시켜 충돌 없는 궤적으로 수렴시킨다. 장점은 비제약 최적화로 환원되어 구현이 단순하다는 점이며, 단점은 벌점이 유한하면 미소한 제약 위반이 잔존할 수 있다는 점이다.

3.2 제약법(Constraint Approach)

충돌 회피를 명시적 부등식 제약으로 부과한다.

g_k(\mathbf{q}) = d_{safe} - d_k(\mathbf{q}) \leq 0

SQP 또는 내점법에 의해 처리된다. 제약이 비볼록이므로 선형화가 필요하며, 현재 자세에서의 최근접점 정보를 이용한 1차 근사가 사용된다.

3.3 안전 복도법(Safe Corridor Approach)

전역 경로 계획(RRT*, A* 등)에 의해 생성된 경로 주위에 볼록 자유 공간 영역의 연쇄(safe corridor)를 구성하고, 궤적이 각 영역 내에 머물도록 볼록 제약을 부과한다.

\mathbf{r}(t) \in \mathcal{P}_k, \quad t \in [t_k, t_{k+1}]

각 \mathcal{P}_k = \{\mathbf{r} : \mathbf{A}_k\mathbf{r} \leq \mathbf{b}_k\}가 볼록 다면체이면, 제약이 선형이 되어 전체 문제가 볼록(QP 또는 LP)으로 유지된다.

4. 자유 공간의 볼록 분해

4.1 IRIS(Iterative Regional Inflation by Semidefinite programming)

주어진 시드 점(seed point) 주위에서 장애물을 회피하는 최대 체적의 타원체를 반복적으로 팽창시킨 후, 이 타원체를 포함하는 다면체를 구성하는 방법이다. SDP를 이용하여 타원체를 최적화하고, 타원체를 포함하는 반공간의 교집합으로 다면체를 구성한다.

4.2 볼록 분해 트리

GCS(Graphs of Convex Sets) 프레임워크에서 자유 공간을 볼록 영역의 그래프로 표현하고, 궤적이 이 그래프의 경로를 따르도록 정식화한다. 영역 할당과 궤적 최적화를 동시에 수행하는 혼합 정수 볼록 계획으로 정식화할 수 있다.

5. 부호 거리 함수(Signed Distance Function)

부호 거리 함수(SDF)는 점 \mathbf{p}에서 장애물 표면까지의 부호 있는 거리를 반환한다.

\text{SDF}(\mathbf{p}) = \begin{cases} -\min_{\mathbf{o} \in \partial\mathcal{O}} \lVert \mathbf{p} - \mathbf{o} \rVert & \mathbf{p} \in \mathcal{O} \\ \min_{\mathbf{o} \in \partial\mathcal{O}} \lVert \mathbf{p} - \mathbf{o} \rVert & \mathbf{p} \notin \mathcal{O} \end{cases}

SDF는 장애물 내부에서 음수, 외부에서 양수이며, 표면에서 영이다. 3차원 격자(voxel grid)에 사전 계산하여 저장하면, 임의의 점에서의 거리와 그래디언트를 O(1)에 조회할 수 있다.

충돌 회피 제약은 \text{SDF}(\mathbf{p}_i(\mathbf{q})) \geq d_{safe}로 표현되며, SDF의 그래디언트가 자연스럽게 제공되므로 경사 기반 최적화에 직접 활용된다.

6. 연속 시간 충돌 검사

이산 격자점에서만 충돌을 검사하면, 격자점 사이에서 장애물을 관통하는 “터널링(tunneling)” 현상이 발생할 수 있다. 이를 방지하기 위해 연속 시간 충돌 검사(continuous collision checking)가 필요하다.

보수적 전진(conservative advancement): 현재 거리가 이동 거리보다 크면 충돌이 없음이 보장된다. 이를 이용하여 시간 구간을 안전하게 전진시킨다.

스웹 볼륨(swept volume): 시간 구간에서의 로봇 점유 공간의 합집합(swept volume)과 장애물의 교차를 검사한다.

7. 로봇 공학에서의 실용적 고려

계산 비용: 충돌 검사와 거리 계산이 궤적 최적화의 주요 계산 병목이 될 수 있다. 계층적 바운딩 볼륨(BVH), 공간 해시(spatial hash), 옥트리(octree) 등의 가속 구조가 필수적이다.

다중 장애물: 장애물의 수가 증가하면 제약의 수도 증가한다. 근접한 장애물 쌍만을 선별하는 광역 단계(broad phase)와 정밀 거리 계산의 협역 단계(narrow phase)의 2단계 구조가 효율적이다.

동적 장애물: 이동하는 장애물의 경우, 시공간(space-time)에서의 충돌 회피 제약이 부과되어야 하며, 장애물의 예측 궤적에 대한 불확실성이 추가적인 복잡성을 야기한다.

8. 참고 문헌

  • Ratliff, N., Zucker, M., Bagnell, J. A., & Srinivasa, S. (2009). “CHOMP: Gradient Optimization Techniques for Efficient Motion Planning.” Proceedings of ICRA, 489–494.
  • Schulman, J., et al. (2014). “Motion Planning with Sequential Convex Optimization and Convex Collision Checking.” The International Journal of Robotics Research, 33(9), 1251–1270.
  • Deits, R., & Tedrake, R. (2015). “Computing Large Convex Regions of Obstacle-Free Space through Semidefinite Programming.” Proceedings of WAFR, 109–124.
  • Marcucci, T., Petersen, M., von Wrangel, D., & Tedrake, R. (2024). “Motion Planning around Obstacles with Convex Optimization.” Science Robotics, 9(84).

version: 1.0