1. 논문 목적

mobile navigation의 핵심인 장애물 회피는 mobile robot의 대표적인 과제로, 처리해야 할 많은 환경 정보량으로 인해 실시간 인식이 주요 문제이다.

이 논문에서는 장애물의 종류와 모양에 관계없이 단일 2d lidar를 이용해 동적 장애물을 감지하고 추적하기 위한 프레임워크를 제공하고, 이후 TEB local planner를 이용해 장애물 회피를 구현한다.

감지한 장애물 정보를 원과 선으로 근사하여 표현한다. 표현되는 방식은 다음과 같다.(장애물 인식이 아닌 회피가 목적이기 때문에 적당히 근사하여 사용하는 것으로 보임)

• 긴 벽과 같은 큰 장애물은 선으로 표현
• 책상 다리나 이동 중인 사람과 같은 작은 장애물은 cylinder 형태로 표현

중간값 필터를 사용하여 노이즈를 제거하고, 2d point cloud 데이터를 분할-병합(clustering) 과정을 거쳐 장애물을 선형, 원형으로 모델링한다.

마지막으로 칼만 필터에 기반한 전략을 활용하여 장애물의 움직임을 추정한다.

2. Method

A. Obstacle Detection

1) preprocessing(전처리)

• 2d lidar scan을 통해 N개의 측정 포인트가 있는 집합 p를 다음과 같이 극좌표 형태로 표현

$\boldsymbol{p} \triangleq\left\{p_i=\left(R_i, \theta_i\right)\right\}, i \in[1, N]$

• 노이즈 필터링을 위해 다음의 3X3 중앙값 필터를 사용하여 NaN으로 표시되거나 범위를 초과하는 잘못된 값을 가진 포인트는 제거한다.

$\left[\begin{array}{ccc}R_{(t-1, i-1)} & R_{(t-1, i)} & R_{(t-1, i+1)} \\ R_{(t, i-1)} & R_{(t, i)} & R_{(t, i+1)} \\ R_{(t+1, i-1)} & R_{(t+1, i)} & R_{(t+1, i+1)}\end{array}\right]$

이 때 R(t,i) : scan에서 현재 시간 t에서 점 i까지의 거리 거리

• (그림 (a) 참고) 노이즈 필터링 후, 대형 point cloud를 순서대로 독립적인 작은 point cloud로 분할한다. 이 때 각 point cloud 블록의 starting point, ending point 규칙이 있는데, 다음과 같다.
  (1) starting point index : $R_{i-1}=0$ and $R_i \neq 0$.
  (2) ending point index : $R_i \neq 0$ and $R_{i+1}=0$
  (잘 이해는 안 가지만, 그림을 보아 pointcloud가 끊기는 지점에 따라 분할하는 것으로 보임)

2) Segmentation and Merging(분할 및 병합)

• 실제 환경에서는 벽과 같은 일부 큰 요소가 작은 간격으로 절단되어 두 개 이상의 분할 요소로 포함되는 잘못된 경우가 발생하곤 한다. 여기서는 분할을 위해 작은 구간의 두 끝점 거리를 사용한다.

• 데이터 분할을 위해 극좌표 추정 포인트 $R_i$ 와 $R_{i+1}$ 사이 서리 d를 결정

  $d\left(R_i, R_{i+1}\right)=\sqrt{R_i^2+R_{i+1}^2-2 R_i R_{i+1} \cos \Delta \alpha}$

• (그림 (b) 참고) 여기서 $d$ 가 $kW$ 보다 크면 point cloud가 두 블록으로 분할된다.

  $W$ : 로봇의 폭
  $k$ : 다음과 같이 설정된 동적 증폭 계수를 의미한다.

  $\left\{\begin{array}{l}k=\frac{W \cdot R_{\imath} \cdot R_{u+1}}{100\left(R_l+R_{u+1}\right)}, \frac{R_{\imath}+R_{u+1}}{2}>T_d \\ k=0.15,0<\frac{R_{\imath}+R_{u+1}}{2} \leq T_d\end{array}\right.$

  $T_d = 10$ : 측정 거리의 임계값. 임계값을 높이거나 로봇 폭이 크면 분할 개수가 줄어든다.

• 병합 과정은 다음과 같다. 두 point cloud 블록 사이 거리 L을 구한 후, 로봇 폭 W가 L보다 크면 로봇이 블록 사이를 통과할 수 없으므로 병합 처리한다. 그리고, 병합 시 두 블록을 연결된 것처럼 보이게 하기 위해 두 점 p, q 사이에 값을 보상한다.

• 보상할 점의 개수 N을 결정하기 위해 두 point cloud 블록의 인접 점 사이의 평균 거리 $d_{aver}$ 를 계산한다.

• (그림 (c) 참고) 이로 인해 보상된 점들이 두 point cloud 블록 사이에서 균일하게 분포된다.

  $N=\frac{L}{d_{\text {aver }}}$

  $R_i=R_p+i \cdot \frac{R q-R_p}{N}$

3) Classification(분류)

• 분할 및 병합 과정을 거친 point cloud 블록들을 모두 단순한 도형 집합으로 표현하는 과정. 이로 인해 계산 단순화와 측정 noise에 대한 견고성을 제공

• 로봇의 이동 공간이 줄어들 수 있으나, 장애물에 대한 정확한 표현이 아닌 회피가 주된 목적이므로 이 정도의 허용 가능한 범위 내 근사는 가능한 것으로 보인다.

• 먼저, 장애물 집합을 다음과 같이 정의한다.

  $\mathbb{O} \triangleq\{\mathbb{L}, \mathbb{C}\}$

  $\mathbb{L}$ : 선형 장애물 집합
  $\mathbb{C}$ : 원형 장애물 집합

• 만약 블록 set의 point 개수가 미리 정의된 threshold인 $T_N$(=10) 이상일 경우, 시작점 p와 끝점 q를 잇는 선분을 싱성하고 각 점에서 선분까지의 최대 거리 $D_{max}$를 계산한다.

  $D_{max} < 0.2|S|$ : 두 끝점 p와 q로 표현되는 선분으로 point cloud 블록 근사(그림 (A) 참고)

  $D_{max} > 0.2|S|$ : Algorithm 1을 따라 point cloud 블록을 더 세분화. 해당 과정을 재귀적으로 수행하며, 새로 생성된 블록의 점 개수가 $T_N$ 미만일 때 세분화가 중지됨.

• 블록 set의 point 개수가 $T_N$(=10) 미만일 경우, 이러한 작은 크기의 point cloud 집합은 원의 형태로 표현된다.

• 원의 반지름은 다음과 같이 계산된다.

  $D_{max} < 0.2|S|$ : 두 끝점을 연결하는 선분의 중점을 중심으로 설정하고, 중점에서 가장 먼 점까지의 거리를 반지름으로 사용(그림 B 참고)

  $D_{max} > 0.2|S|$ : 점 구름 블록의 볼록성(convexity) 또는 오목성(concavity)을 판단해야 함. 이를 위해 벡터 외적(cross product)을 활용하는 효율적인 알고리즘(Algorithm 2)을 사용.

• 로봇과 장애물 사이의 충돌을 방지하기 위해, 원의 반지름은 안전 거리 여윳값을 추가하여 확장된다.(파란색 원)

오목한 경우: 선분의 두 끝점과 라이다 원점을 연결한 삼각형 영역은 안전 영역입니다(그림 (C) 참고)

볼록한 경우: 라이다와 선분 사이의 가장 가까운 점을 기준으로 생성된 영역이 안전 영역입니다(그림 (D) 참고)

B. Obstacle Tracking

• 제안된 시스템에 KF를 구현하여 원형 장애물 추적

• t-1에서 감지된 장애물이 t에서 감지된 장애물과 일치하는지 파악

• 특정 point cloud 집합이 미리 정의된 횟수 이상 지속적으로 감지되면 이를 동적 장애물 집합에 추가, 그렇지 않으면 해당 정보 삭제

3. 실험