Optimal Reciprocal Collision Avoidance (ORCA) 논문

1. 논문 목적

가정

• 각 로봇을 단순한 모양(원, 볼록한 다각형)으로 가정
• 로봇이 홀로노믹(holonomic)하다고 가정
• 다중 에이전트 상황에서 상호 충돌 회피를 구현

2. Problem Definition

• n개의 로봇이 환경을 공유
• 로봇들은 원 모양, 2차원 평면에서 이동
• external parameter: 다른 로봇이 관찰할 수 있는 정보

  • $\textbf{p}_A$ : robot A 현재 위치
  • $\textbf{v}_A$ : robot A 현재 속도
  • $r_A$ : robot A radius

• internal parameter: 다른 로봇에게 관찰되지 않는 정보

  • $\textbf{v}^{max}_A$ : 최대 속도
  • $\textbf{v}^{pref}_A$ : preferred vel (선호 속도)

• 각 로봇들은 독립적으로, 또한 동시에 새 속도 $\textbf{v}^{new}_A$ 를 선택해야 함

  • 이 때 $\textbf{v}^{new}_A$ 로 이동할 경우 $\tau$ 초 동안 충돌하지 않아야 함
  • 또한, 가능한 한 $\textbf{v}^{new}_A$ 는 $\textbf{v}^{pref}_A$ 와 비슷해야 함

• 로봇은 서로 communication이 불가능하고, 다른 로봇의 위치와 속도만 관찰할 수 있음
• 각 로봇은 다른 로봇들도 자신과 같은 전략을 사용하여 $\textbf{v}^{new}_A$ 를 선택한다고 가정할 수 있음

3. Reciprocal Collision Avoidance

[ VO ]

• $VO_{A|B}^\tau$ : 두 로봇 A, B에 의해 정의되며 시간 $time = \tau$ 후 A와 B가 충돌할 가능성이 있는 속도 공간 영역을 의미함

  • $D(\textbf{p}, r) = \{\textbf{q} \mid \|\textbf{q} - \textbf{p}\| < r\}$
    • $D(\textbf{p}, r)$ : 중심이 $\textbf{p}$ 이고 반지름이 $r$ 인 원
    • $\{\textbf{q} \mid \|\textbf{q} - \textbf{p}\| < r\}$ : 벡터 $\textbf{q}$와 $\textbf{p}$ 사이의 유클리드 거리(벡터의 크기)가 반지름 $r$ 보다 작음
  • $VO_{A|B}^\tau = \{\mathbf{v} \mid \exists t \in [0, \tau] :: t\mathbf{v} \in D(p_B - p_A, r_A + r_B)\}$
    • $[0, \tau]$ 이내 존재하는 $t$ 에 대하여 $t\textbf{v}$ (t초 이후 위치) $\in D(p_B - p_A, r_A + r_B)\}$ 를 만족하는 $\textbf{v}$ 의 set (Fig.1 (b) 참고)

$VO_{A|O}^{\tau} = \{\vec{v} \mid \exists t \in [0, \tau] :: t\vec{v} \in D(P_O - P_A, r_O + r_A)\}$

$\vec{v} = \vec{v}_A-\vec{v}_O$

$\vec{v}_A \notin VO_{A|O} \oplus \vec{v}_O$

$ Cost(x, y) = \begin{cases} A \cdot \exp\left(-\frac{(x)^2}{2\sigmax^2} - \frac{(y)^2}{2\sigma_y^2}\right), & \text{if } \text{dist} \le R{cutoff} \ 0, & \text{if } \text{dist} > R_{cutoff} \end{cases} $

[ RVO ]

• 현재 로봇의 상대 속도가 $\textbf{v}_A - \textbf{v}_B \in VO_{A|B}^\tau$ (VO에 속함) 이면, A와 B는 계속 해당 속도로 이동 시 시간 $\tau$ 내 중돌한다.
• 반대로, $\textbf{v}_A - \textbf{v}_B \notin VO_{A|B}^\tau$ 이면 시간 $\tau$ 안에는 충돌하지 않음을 보장

  • 그렇다면, $\textbf{v}_B \in V_B$ 인 $V_B$ set이 $\textbf{v}_A \notin VO_{A|B}^\tau \oplus V_B$ 이면 A와 B는 시간 $\tau$ 안에는 충돌하지 않음을 보장
  • collision-avoiding velocities 집합 $CA_{A|B}^\tau(V_B) = \{\mathbf{v} \mid \mathbf{v} \notin VO_{A|B}^\tau \oplus V_B\}$

4. Optimal Reciprocal Collision Avoidance

이제 우리는 $V_A = CA_{A|B}^\tau(V_B)$와 $V_B = CA_{B|A}^\tau(V_A)$를 만족하는 A,B에 대한 permitted velocities 집합인 $V_A, V_B$를 결정해야 함

또한, A와 B는 개별 로봇으로, 다른 로봇과의 통신 없이도 permitted velocities 집합을 추론할 수 있어야 함

이를 만족하는 무한한 $V_A, V_B$ set 중에서 최적 속도 $\textbf{v}_A^{opt}, \textbf{v}_B^{opt}$ 에 "근접한" permitted velocities 의 크기가 가장 큰 set을 $ORCA_{A|B}^\tau$ 와 $ORCA_{B|A}^\tau$ 라고 부른다.

[ Definition ]

• $ORCA_{A|B}^\tau$ 와 $ORCA_{B|A}^\tau$ 는 다음과 같이 정의됨

  • $CA_{A|B}^\tau(ORCA_{B|A}^\tau) = ORCA_{A|B}^\tau$ 및 $CA_{B|A}^\tau(ORCA_{A|B}^\tau) = ORCA_{B|A}^\tau$
    (A, B가 서로 동일한 속도를 선택)
  • $V_A, V_B$에 대해 $V_A \subseteq CA_{A|B}^\tau(V_B)$ 및 $V_B \subseteq CA_{B|A}^\tau(V_A)$
  • 모든 $r > 0$ 에 대해 다음을 만족
    (즉, $ORCA_{A|B}^\tau$ 와 $ORCA_{B|A}^\tau$ 는 각각 $\textbf{v}_A^{opt}, \textbf{v}_B^{opt}$ 와 교집합이 가장 많다 ($\textbf{v}_A^{opt}, \textbf{v}_B^{opt}$ 와 가까운 속도를 다른 모든 reciprocally collision-avoiding 집합보다 더 많이 포함한다))

[ geometrically constuction ]

• A, B 가 $\textbf{v}_A^{opt}, \textbf{v}_B^{opt}$ 를 채택하였고, $\textbf{v}_A^{opt} - \textbf{v}_B^{opt} \in VO_{A|B}^\tau$ 에 의해 충돌이 일어난다고 가정

• $\textbf{u}$ 를 $\textbf{v}_A^{opt} - \textbf{v}_B^{opt}$ 에서 VO의 경계의 가장 가까운 지점(the closest point on the boundary of the velocity obstacle)까지의 벡터라고 새롭게 정의

  • $\mathbf{u} = \left( \arg\min_{\textbf{v} \in \partial VO^\tau_{A|B}} \| \textbf{v} - (\textbf{v}_A^{opt} - \textbf{v}_B^{opt}) \| \right) - (\textbf{v}_A^{opt} - \textbf{v}_B^{opt})$
  • $\textbf{n}$ : $VO_{A|B}^\tau$ boundary point인 $(\textbf{v}_A^{opt} - \textbf{v}_B^{opt}) + \mathbf{u}$ 의 바깥 법선 벡터
    (Fig.2 에 표시되어 있음)
    즉, $\mathbf{u}$ 는 시간 $\tau$ 내 A와 B가 충돌을 피하기 위해 필요한 최소한의 상대 속도 변화를 의미

• 공정한 충돌 회피를 위한 "책임 공유" (share the responsibility) : A 와 B 가 자신의 속도를 각각 $\frac{1}{2}\textbf{u}$ 만큼 조정

  • A의 permitted velocity 집합 $ORCA_{A|B}^\tau$ 는 starting point $\textbf{v}_A^{opt} + \frac{1}{2}\textbf{u}$ 에서 법선 $\textbf{n}$ 을 가리키는 half-plane 으로 정의됨

  • $ORCA^\tau_{A|B} = \{ \textbf{v} \mid (\textbf{v} - (\textbf{v}^{opt}_A + \frac{1}{2} \mathbf{u})) \cdot \mathbf{n} \geq 0 \}$ ( $ORCA^\tau_{B|A}$ 와는 대칭 )

  • $\textbf{v}_A^{opt} - \textbf{v}_B^{opt} \notin VO_{A|B}^\tau$ 인 경우에도 적용 가능
    (두 로봇은 충돌 없는 궤적 유지를 위해 각각 절반의 책임을 진다)

5. n-Body Collision Avoidance.

[ Basic Approach ]

각 로봇 A 는 iteration ( time step $\Delta{t}$ ) 마다 sensing 과 acting을 수행 - 다른 로봇들의 정보(위치, $r$, $\textbf{v}^{opt}$ 등) 획득하고 $ORCA^\tau_{A|B}$ 계산

• 모든 로봇에 대해 A에 허용된 속도들의 집합 (허용된 속도: 충돌을 일으키지 않도록 보장된 속도) 은 다음과 같음

  • $ORCA^{\tau}_A = D(\textbf{0}, \textbf{v}^{\max}_A) \cap \bigcap_{B \neq A} ORCA^\tau_{A|B}$
    (각 로봇에 의해 유도된 ORCA half plane들의 교집합, Fig.4 (b))

• 다음으로 로봇 A는 permitted vel 영역 내에서 $\textbf{v}_{pref}^A$ 에 가장 가까운 $\textbf{v}_{new}^A$ 를 선택

  • $\textbf{v}_{\text{new}}^A = \arg\min_{\textbf{v} \in ORCA^\tau_{A|B}} \| \textbf{v} - \textbf{v}^{pref}_A \|$
  • $\textbf{p}^{\text{new}}_A = \textbf{p}^A + \textbf{v}^{\text{new}}_A \Delta t$

[ Choosing the Optimization Velocity ]

로봇들이 서로 통신 없이 half-plane을 결정하려면 $\textbf{v}^{opt}_A$ 를 다른 로봇들이 관찰으로 결정할 수 있어야 함

그렇다면 로봇의 최적화 속도 $\textbf{v}^{opt}_A$ 를 어떻게 설정해야 하는지? 에 대한 몇 가지 방법론

• $\textbf{v}^{opt}_A = 0$ (모든 로봇의 최적화 속도 = 0)

  충돌 회피는 항상 가능하지만, 동작이 부자연스럽고 전역적 교착 상태 발생 가능

• $\textbf{v}^{opt}_A = \textbf{v}^{pref}_A$ (선호 속도로 설정)

  낮은 밀도에서는 효과적이지만, 중간 밀도 이상에서는 충돌 위험 증가.

• $\textbf{v}^{opt}_A = \textbf{v}_A$ (현재 속도로 설정)

  현실적인 절충안이지만, 밀도가 높을 때 충돌을 완전히 피하는 것은 어려움.

따라서, 현재 속도를 최적화 속도로 설정하는 방법이 가장 현실적인 선택이며, 높은 밀도 환경에서는 추가적인 방법(3차원 선형 계획법)을 적용해야 함.

[ Densely Packed Conditions ]

$\textbf{v}^{opt}_A$ 를 현재 속도로 설정한다면 밀도가 매우 높은 상황에서는 선형 프로그램의 모든 제약 조건을 만족하는 단일 속도가 존재하지 않을 수 있다.

대신 ‘safest possible’ velocity ('가장 안전한' 속도) 를 선택
(다른 로봇에 의해 생긴 constraints를 최소한으로 침범하는 속도)

• $d_{A|B}(\textbf v)$ : $ORCA^\tau_{A|B}$ 의 half plane 까지 속도 $\textbf v$ 의 유클리드 거리(부호가 있는)

  • 만약 $\textbf{v} \in ORCA^\tau_{A|B}$ 를 만족하면 $d_{A|B}(\textbf v)$ 는 음수(-)

• 이제 우리는 다른 로봇들에 의해 유도된 $d_{A|B}(\textbf v)$ 들 중 어느 하나라도 $\max d_{A|B}(\textbf v)$ 를 최소화하는 속도를 선택

  • 
  • Geometrically, this can be interpreted as moving the edges of the half-planes $ORCA^\tau_{A|B}$ perpendicularly outward(수직으로 바깥 방향) with equal speed, until exactly one velocity becomes valid(유효하게 될 때까지).

6. Static Obstlacles / Experiments

[ Static Obstlacles ]

일반적인 다중 로봇 환경에서 장애물(정적)을 피할 때

$O$ : line segments
$A$ : $r_A, \mathbf{p}_A$
일 때 장애물 O에 의해 유도된 VO는 다음과 같다.( Fig 6 (a,b) )

• $VO_{A|O}^{\tau} = \left\{ v \mid \exists t \in [0, \tau] :: t \textbf{v} \in O \oplus -D(\mathbf{p}_A, r_A) \right\}$

  • $\mathbf{v}_A \in VO_{A|O}^{\tau}$ 이면 시간 $\tau$ 내 장애물 $O$ 와 로봇 $A$ 는 충돌한다.
  • $\mathbf{v}_A \notin VO_{A|O}^{\tau}$ 이어야 충돌하지 않는데, 이를 위해 로봇 $A$의 장애물 $O$에 대한 permitted vel 집합을 [ $VO_{A|O}^{\tau}$의 경계면에서 $\mathbf{v}_A^{opt}$ 와 가장 가까운 점' ] 에서 $VO_{A|O}^{\tau}$ 의 접선인 half plane $ORCA_{\tau}^{A|O}$ 로 정의 ( Fig.6 (c) )

장애물의 경우 모든 로봇 A에 대해 $\mathbf{v}_A^{opt} = 0$ 으로 선택하면 로봇과 장애물이 충돌하지 않는 유효 속도가 항상 존재하게 됨