Home

다중 로봇 SLAM에서 중요한 과정 중 하나는 각 로봇이 수집한 지도 데이터를 하나의 통합된 지도로 병합하는 지도 병합 과정이다. 이 과정은 각 로봇이 생성한 지도 간 변환 행렬[25]을 계산함으로 수행된다. 따라서 3차원 점 군 지도 병합 문제는 아래 수식과 같이 3차원 변환 행렬로 정의할 수 있다.

여기서 t는 이동 벡터 [Δ_x, Δ_y, Δ_θ]^T 이며, R_x(α), R_y(β), R_z(γ)은 각각 x, y, z 축을 중심으로 α, β, γ 만큼 회전하는 행렬을 나타낸다.

하지만 실내 환경에서 지도 병합을 수행할 때, 모든 변수를 계산하는 것은 비효율적 일 수 있다. 따라서 본 논문에서는 3차원 점 군 지도를 2차원 점유 격자 지도로 변환하여 효율적으로 변환 행렬을 계산한다. 따라서 지도 병합 문제를 아래 수식과 2차원 변환 행렬로 축소하여 정의할 수 있다.

3차원 지도 변환 행렬을 계산하는 것은 2차원 지도 변환 행렬을 계산하는 것보다 계산해야 하는 변수의 수와 3차원 지도를 구성하는 점 군의 수에 의해 상당한 계산 비용을 동반한다. 3차원 점군 지도는 수십만에서 수천만 개의 3차원 점을 포함하고 있으므로 ICP와 같은 스캔 매칭 기반의 방법만 사용하여도 엄청난 계산 비용을 발생시키며, 또한 국소 최솟값에 수렴할 수 있어 신뢰할 수 없는 지도 병합이 도출될 수 있다.

이를 해결하기 위해 사전에 SHOT(Signature of Histograms of OrienTations)[26] 혹은 FPFH(Fast Point Feature Histogram)[27]와 같이 점과 점 사이의 관계를 계산하여 특징점을 추출하여 점 군 간 대응 관계(Correspondence)를 명확히 하는 방법을 사용하거나 Quatro++[28] 혹은 Teaser[29]와 같은 전역 정합 기법을 통해 계산된 초기 추정값을 이용하는 방법이 사용될 수 있는데, 이는 추가적인 시간 소요를 발생시켜 실시간 계산이 어렵다.

본 논문은 이러한 이유로 인하여 3차원 점 군 지도를 2차원 점유 격자 지도로 변환하여 지도 병합을 수행함으로, 계산 비용을 감소시키고 신뢰할 수 있는 지도 병합 결과를 나타내는 방법을 연구한다.

본 논문에서 제안하는 방법의 전체 구조는 그림 1과 같다. 서버는 각 로봇이 수집한 데이터를 처리하는 역할을 하며, 독립된 시스템 혹은 다중 로봇 시스템 내의 리더 로봇으로 구성될 수 있다.

Fig. 1. 
Overall system pipeline

먼저, 각 로봇은 독립적으로 3D-SLAM을 수행하여 자신의 위치를 추정함과 동시에 3차원 점 군 지도를 생성한다. 이 과정에서, 로봇들은 자신들의 지역 위치와 지역 점 군 정보를 실시간으로 서버로 전송한다.

서버는 데이터가 수신될 때마다, 지도 투영(Map projection), 특징점 추출 및 매칭(Feature extraction and matching), 이상치 제거(Outlier filtering), 그리고 지도 병합 결정 모듈(Map-Merge decision module)을 순차적으로 반복 진행한다.

지도 투영 과정에서는 3차원 점 군 데이터를 2차원 점유 격자 지도의 형태로 변환하여, 지도 병합 단계에서 처리해야 할 변수의 수를 축소한다.

특징점 추출 및 매칭 과정에서는 점유 격자 지도 간의 변환 행렬을 계산하기 위해 Accelrated-KAZE(AKAZE)[30] 특징점을 추출하고, 각 점유 격자 지도 간의 상응하는 특징점을 매칭한다.

이상치 제거 과정에서는 특징점 매칭 과정에서 발생한 잘못된 매칭으로 지도 병합의 성능이 저하되는 것을 방지하기 위해 RANSAC(Random Sample Consensus)를 이용하여 이상치 제거(Outlier filtering) 수행한다.

최종적으로 남은 정상치는 지도 병합 결정 모듈에서 활용되어 지도 병합 수행 여부를 결정하고, 지도 병합 수행 여부에 따라 지도 병합을 수행한다.

지도 투영은 로봇의 지역 위치(Local pose)와 지역 점 군(Local PointCloud)을 받아 2차원의 점유 격자 지도를 생성하는 과정이다.

먼저, 점 군 정보는 사전에 정의된 높이 정보를 기반으로 천장과 바닥 영역을 제거한다. 천장과 바닥이 포함된 경우 점유 격자 지도 생성 시 자유 공간과 점유 공간을 구분할 수 없고 이는 특징점 매칭에 악영향을 주기 때문이다.

그 후 천장과 바닥 영역이 제거된 점 군과 로봇의 위치는 z축에 해당하는 값을 제거하여 2차원으로 변환하고, 변환 함수를 이용하여 x-y 좌표계에서 이미지 좌표계로 변환된다. 아래의 수식은 x-y 좌표계에서 이미지 좌표계로의 변환과 역변환을 나타낸다.

여기서 x_o,y_o는 지도의 원점, i, j는 점유격지도의 열과 행에 해당하는 인덱스를 나타내고, height, width, resolution은 각각 지도의 높이, 넓이, 해상도이다. 바닥 함수⌊∘⌋는 실숫값을 가장 가까운 작은 정수로 내림하여 정수형으로 형 변환을 나타낸다.

변환이 완료된 로봇의 위치와 점 군의 위치는 브레젠험 직선 알고리즘(Bresenham’s line algorithm)을 활용하여 점유 공간과 자유 공간을 식별하고, 이를 토대로 2차원 점유 격자 지도로 생성한다. 이 과정은 3D-SLAM이 수행되는 동안 지속해서 반복하며, 각 지도를 갱신하게 된다.

지도 투영 과정을 거쳐 생성된 점유 격자 지도는 지도 간 특징점 매칭을 수행하여 변환 행렬을 구하기 위해 사용된다. 그림 2는 특징점 매칭을 위해 각 로봇이 생성한 점유 격자 지도이다.

Fig. 2. 
Occupancy grid map for each robot

본 시스템에서는 특징점 추출을 위해 AKAZE 특징점을 사용한다. AKAZE 특징점은 KAZE[31] 특징점을 기반으로 개선된 알고리즘이다. AKAZE 특징점은 비선형 스케일 공간을 사용하여 기존 KAZE 특징점보다 노이즈에 강건한 특징점 빠르게 추출할 수 있다는 장점이 있다.

특징점들이 추출된 후, 완전 탐색(Brute Force Algorithm)을 이용하여 L2-Norm을 기준으로 가장 가까운 특징점들을 매칭한다. 하지만 매칭 쌍 중에는 지도 투영 과정 중 동적 객체의 영향으로 발생하는 이상치 및 여러 요인으로 발생하는 이상치가 포함될 가능성이 있어, 이를 직접 지도 병합에 사용하기에는 적합하지 않다. 따라서 이러한 이상치들을 제거하기 위해 Random Sample Consensus(RANSAC)을 적용한다.

RANSAC 과정에서는 무작위로 선택된 특징점 쌍을 기반으로 변환 행렬을 계산하고, 변환 행렬을 적용했을 때 정해진 오차 범위 이내에 존재하는 매칭 쌍의 개수를 계산한다. 이 과정을 반복하여 가장 많은 매칭 쌍을 제공하는 변환 행렬을 찾고, 이상치를 제거한다. 최종적으로 추출된 정상치는 지도 병합에 사용된다. 그림 3, 4는 이상치를 포함한 특징점 매칭의 결과와 RANSAC을 통해 이상치를 제거한 결과이다.

Fig. 3. 
AZAZE feature matching result

Fig. 4. 
Outlier filtering with RANSAC

지도 병합 결정 모듈은 사전에 정의된 중복영역비율 임곗값(Overlap ratio threshold)과 연속프레임 임곗값(Overlap frame threshold)을 사용하여, 지도 병합 여부를 결정한다. 앞선 단계에서 RANSAC을 거쳐 이상치가 제거된 정상치라 할지라도 단일 프레임에서의 정상치를 그대로 지도 병합에 사용한다면, 복도와 같은 유사한 구조를 가진 환경에서 오인된 정상치로 인해 부정확한 지도 병합 결과가 발생할 수 있다.

이를 방지하기 위해, 지도 병합 결정 모듈은 연속된 프레임을 활용하여 이런 정상치들이 지도 병합에 적합한지를 판별하기 위해 설계되었다.

우선, 현재 프레임에서 계산된 정상치들의 좌표들을 활용하여 사각형의 내부 영역인 A_curr을 정의한다. 그 후, 이전 프레임에서 계산된 정상치들의 좌표들과 현재 프레임에서 계산된 정상치들의 좌표들의 합집합을 이용해, 해당 집합으로 나타낼 수 있는 사각형의 내부 영역인 A_combined을 정의한다.

계산된 두 영역, A_curr과 A_combined을 활용하여 현재 프레임의 정상치가 이전 프레임의 정상치로부터 얼마나 변했는지를 나타내는 중복영역비율 R_overlap을 계산한다. 중복영역비율이 1에 가까우면 현재 프레임의 정상치가 이전 프레임과 유사함을 의미하며, 0에 가까워질수록 크게 달라짐을 나타낸다. 따라서, 정해진 중복영역비율 임곗값과 연속프레임 임곗값을 만족하는 프레임이 연속될 경우, 이는 신뢰할 수 있는 정상치로 간주하여 지도 병합을 진행하게 된다.

표 2는 지도 병합 검사 모듈의 의사 코드이다. 여기서 Th_ratio와 Th_frame는 중복영역비율 임곗값과 연속프레임 임곗값을 의미하고, B, A, R 은 입력으로 받은 특징점 정상치 I를 포함하여 만들 수 있는 최소한의 사각형의 좌표, 내부 영역의 넓이, 중복 영역의 비율을 의미한다. C는 중복영역비율 임곗값 조건을 만족하는 연속된 프레임의 수를 나타내고 S는 지도 병합 여부를 나타낸다.

본 실험은 크기가 다른 두 가지의 실험 환경에서 진행되었으며, 지도 병합이 수행될 수 있도록 교차 영역을 형성하였다. 그림 5는 사용된 다중 로봇 시스템을 나타내며, 그림 6은 사용된 실험 환경과 각 로봇의 이동 경로를 나타낸다.

Fig. 5. 
Robot configuration

Fig. 6. 
Experimental environment and path for each robot

사용된 로봇은 OMOROBOT 사의 R1 모델이며, 3D LiDAR와 IMU는 각각 Velodyne 사의 VLP-16 모델과 Microstrain 사의 3DM-GX5-AHRS 모델을 사용했다. 로봇 간 통신을 위해 Iptime 사의 A9004M을 Mesh Network로 사용했다.

아래 그림들은 실험 간 지도 병합 검사 모듈을 수행했을 때 취득한 결과이다. 그림 7은 지도 병합 검사 모듈에 입력으로 들어갈 정상치 특징점들이며, 이를 바탕으로 그림 8과 같이 중복 영역을 계산한다. 마지막으로 그림 9과 같이 연속된 프레임 동안 중복 영역의 비율이 사전에 정의한 연속프레임 임곗값 이상이면 지도 병합이 수행된다.

Fig. 7. 
Inliers for map-merge dicision module

Fig. 8. 
Visualization of overlap area

Fig. 9. 
Map-merge-dicision module results by frame

다음은 3차원 지도 병합 결과 및 분석에 관한 설명이다. 지도 병합의 정확도를 측정하는 것은 실내 환경에서의 다중 로봇 시스템의 지도 병합을 위한 공개 데이터의 부재와 정확한 평가 지표를 도출하는 데 어려움이 있다.

본 논문에서는 Loop Closure Detection에서 Loop 발생이 판별되면, 노드 간 변환 관계를 알기 위해 추가로 ICP와 같은 방법이 사용된다는 점과 [16]에서 ICP를 사용하여 지도 간 변환 관계를 계산한 것을 참고하여 ICP와 Voxelized-ICP와 제안된 방법을 비교하였다.

그림 10은 각 실험 환경에서 비교 알고리즘과 본 논문에서 제안한 알고리즘의 지도 병합 결과를 나타낸 것이다. 각 그림에서 파란색의 점 군 지도는 로봇 1이 생성한 지도이며, 초록색의 점 군 지도는 로봇 2가 생성한 지도이다. 사용된 ICP 알고리즘은 Open3D[32] 패키지에 내장된 함수를 사용하였으며 Voxelized-ICP는 ICP 과정 중 제안된 방법과 동일한 해상도를 가지도록 복셀화 과정을 추가하여 사용하였다. 또한 동일한 점 군 개수를 사용하기 위해 모든 방법에서 3.2에서 설명한 2차원에 투영된 점 군을 사용하였다.

Fig. 10. 
Algorithm results in each experimental environment

Top row : ICP(a), Voxelized-ICP(b), Proposed method(c) map-merging result in experimental environment 1, Bottom row : ICP(d), Voxelized-ICP(e), Proposed method(f) map-merging result in experimental environment 2

비교 지표로 지도 병합 과정에서 소요된 시간(Time)과 지도 병합의 성공 여부(Success/Failure)를 측정하였으며 표 3에 정리되어 있다. 지도 병합 성공 여부는 사용된 방법의 수렴 여부를 나타낸 것이다. 표 3에 나타난 것과 같이, 제안된 방법은 시간 소요 측면에서 기존 방법보다 최소 58.68% 단축을 나타내어, 이는 제안된 방법이 실시간 지도 병합에 더 적합하다는 것을 보인다.

또한, 그림 10에서 나타난 것과 같이 ICP와 Voxelized-ICP는 초기 추정값에 대한 민감성으로 인해 지도 병합에 실패하였지만, 제안된 방법은 초기 추정값 없이도 지도 병합에 성공함으로써 신뢰할 수 있는 지도 병합 결과를 제공한다.