실내 자율주행 로봇을 위한 3차원 다층 정밀 지도 구축 및 위치 추정 알고리즘
CopyrightⓒKROS
Abstract
Moving among multiple floors is one of the most challenging tasks for indoor autonomous robots. Most of the previous researches for indoor mapping and localization have focused on singular floor environment. In this paper, we present an algorithm that creates a multi-floor map using 3D point cloud. We implement localization within the multi-floor map using a LiDAR and an IMU. Our algorithm builds a multi-floor map by constructing a single-floor map using a LOAM-based algorithm, and stacking them through global registration that aligns the common sections in the map of each floor. The localization in the multi-floor map was performed by adding the height information to the NDT (Normal Distribution Transform)-based registration method. The mean error of the multi-floor map showed 0.29 m and 0.43 m errors in the x, and y-axis, respectively. In addition, the mean error of yaw was 1.00°, and the error rate of height was 0.063. The real-world test for localization was performed on the third floor. It showed the mean square error of 0.116 m, and the average differential time of 0.01 sec. This study will be able to help indoor autonomous robots to operate on multiple floors.
Keywords:
Autonomous Robot, Multi-floor Mapping, Localization1. 서 론
최근 자율주행 기술발전과 더불어 급격한 비대면 서비스의 증가로 자율로봇 분야의 연구 및 개발이 활발하게 진행되고 있다. 특히 로봇 산업 시장 확대와 실내배달, 서빙, 경비, 방역 등의 서비스를 제공하는 로봇의 수요가 높아지면서, 실내 자율주행 로봇의 필요성 또한 증가하고 있다. 이러한 실내자율주행을 수행하기 위하여 필요한 핵심기술 중 하나는 실내환경 지도 구축(맵핑)과 로봇의 위치 추정이다. 건물 내부의 경우, GPS (global positioning system) 위성 신호를 수신할 수 없고, 라이다(Light Detecting and Ranging) 센서를 이용한 맵핑과 위치 추정 알고리즘인 SLAM (simultaneous localization and mapping)이 활발하게 이용되고 있다.
특히 실내서비스 로봇의 경우에는 건물 내 층간 이동을 위해서 다층 정밀 지도 구축과 해당 정밀 지도 내에서의 로봇의 위치 추정 기술이 필요하다. 지상로봇이 은 승강기를 이용하여 층간 이동을 하므로 로봇이 건물의 층마다 이동하며 3차원 정밀 지도를 연속적으로 구축하는 방법은 층별 생성된 정밀 지도의 높이 정보를 건물 전체 지도에 적용하는 데 있어 어려움이 있다. 이에, 층별로 단일 축 지도를 따로 구축하여 다수의 층별 지도를 개별적으로 사용하는 방법도 있지만, 이는 층마다 다른 원점으로 인하여 층간 이동 시 새로운 층에서의 위치 추정에 초기화 작업이 매번 수행되어야 한다는 단점이 있다.
다층 정밀 지도 구축과 관련해서 기존에 진행된 연구에서는 여러 센서를 결합하는 센서 융합 기술을 활용하여 다층 맵핑을 진행하였다. 레이저 센서와 기압계를 활용한 연구[1,2] 에서는, 레이저 센서를 활용하여 메트릭 맵핑을 진행하고, 기압계를 이용하여 층간 이동을 감지하여 다층 지도를 구축하였다. 라이다 SLAM의 odometry 결과를 이용하여 연속된 다층 지도를 구축한 연구[3]에서는 로봇 주행 환경의 바닥 정보를 추출하여 이를 제약조건으로 활용하여 지도 작성 과정에서 수직 방향의 LiDAR Odometry 정확도를 높였다. 하지만, 승강기를 이용하는 환경에서 맵핑을 할 수 없다는 한계점이 있다.
다층 환경에서 위치 추정과 관련된 연구에서는 와이파이 정보를 활용하여 로봇의 대략적인 위치를 추정한 연구[4], 자이로센서, 가속도계, 자력계, 기압계, 와이파이 및 GPS 수신기를 가진 스마트폰을 이용하여 각 센서의 신호 수신 세기를 통하여 층 정보를 얻은 연구[5]가 있다.
최근에 진행된 연구[6,7]에서는 지도를 확장하는 방법으로 의미론적 지도 합성(semantic map fusion)을 사용하였다. 해당 연구에서는 두 개의 서로 다른 지도의 공통되는 부분을 정합 알고리즘을 활용해 대응시키는 방법을 활용하였지만, 높이가 일정한 환경에서만 실험을 진행했다는 점에서 본 논문과 차이점이 있다.
본 논문에서는 라이다 센서와 IMU 센서만을 활용하여 다층 실내 환경에서 정밀 지도를 구축하고, 위치를 추정하는 연구를 진행하였다. 이 논문에서 제시한 방법은 아래 세 가지의 이점을 가진다.
- • 로봇의 층간 이동이 있는 실내환경에서의 자율주행을 위한 다층 정밀 지도를 구축한다.
- • GPS 값이 부정확한 실내 환경에서 라이다 센서와 IMU 센서만을 활용하여 로봇의 위치를 추정한다.
- • 층마다 동일한 원점을 제공함으로써, 층 이동 시 위치추정 초기화 작업을 필요로 하지 않는다.
단일 층 지도 구축은 논문의 2.1.1 부분에서, 다층 지도 구축은 2.1.2 부분에서 다룰 것이다. 구축된 다층 지도에서의 위치 추정은 2.2에서 살펴볼 것이다. 또한 논문의 3 부분은 카이스트 본원에 위치한 KI빌딩[Fig. 1]에서 진행한 실험 방법을 서술할 것이다. 마지막으로 4 부분에서 다층 정밀 지도 구축 및 위치 추정 실험 결과에 관하여 설명한다.
2. 알고리즘
2.1 다층 정밀 지도 구축
다층 지도 구축은 [Fig. 2]와 같이 크게 단일 층 지도 구축, 층 누적을 통한 다층 지도 구축 두 단계로 나뉘어 진다. 단일 정밀 지도 구축 단계에서는 일반적인 LOAM[8] 계열의 지도 구축 방식과 유사하게, 라이다 센서와 IMU 센서 정보를 활용하여 LiDAR Odometry (LO)를 얻어 지도를 구축한다.
구축된 단일 층 지도들을 수직으로 쌓아 올리는 층 누적 단계에서는 각 층이 이전 층과 동일하게 가지는 특징점(feature)들을 추출하여 전역 정합(global registration) 알고리즘을 활용하여 추가될 층의 변환 행렬(transform matrix)을 구한다. 이때 주어진 초기 위치에서 시작하여 매칭이 이루어질 경우 해당 행렬을 이용하여 층 전체 지도를 이동시켜 건물 전체 지도 위로 쌓아 올린다.
실내환경의 경우, 일정한 바닥 높이를 가지기 때문에, 바닥 면 전체를 개별의 특징점으로 활용하는 LEGO-LOAM[9]을 백본(backbone) 알고리즘으로 활용하였다. LEGO-LOAM은, 라이다 센서로 시간 τ에 들어온 점 구름 중, 평면 분할(plane segmentation)을 통하여 분류된 바닥에 해당하는 점들을 별개의 특징점() 활용한다. 이후, 나머지 점들을 모서리()와 면() 특징점으로 분류한다. 이러한 분류방식은 특징점 추출단계에서 비교적 적은 연산량과 빠른 속도를 가져 지상구동로봇의 정밀 지도 구축에 적합하다[7]. 는 이전 프레임에서 추출된 바닥 특징점()과 이전까지의 LO (Qτ-1)와의 비교를 통하여 현재 상태의 LO (Oτ)의 변환 행렬 (Tτ)에서 z축의 이동()과 pitch (βτ), roll (γτ)에 해당하는 변화량을 계산하는 데 활용되었다. 는 이전 프레임의 모서리 특징점(), Qτ-1와 비교를 통해서 x축 이동(), y축 이동()과 yaw(ατ) 변화량을 계산하는 데 활용되었다. 이때, IMU 센서 정보를 활용하여 Tτ를 보정하여 변환의 정확도를 한 번 더 높였다.
단일 층 지도(M1:τ)는 이전 프레임 지도(M1:τ-1)위에 Tτ를 통하여 변환된 특징점 를 추가하여 만들어졌다. 이때, LO를 누적하여 만든 로봇의 궤도(S1:τ)는 이후 위치 추정 파트(2.2.2)에서 위치에 대한 가이드를 주는 데 활용되었다.
구축된 단일 k층 지도(Mk)를 적층하여 통합된 다층 지도(Mtotal)를 구축하기 위한 방법으로 NDT (Normal Distribution Transform) 기반의 정합(registration) 알고리즘[Algorithm 1]을 사용하였다. Mk를 앞서 만들어진 층간 누적 지도(U1:k-1)에 대응시키기 위한 회전 변환 행렬(Rk)과 평행 이동 벡터(tk)를 찾는 것을 문제의 목표로 한다. 이때, U1:k-1은 이전 층까지의 지도들을 누적하여 만든 다층 지도이다.
(1) |
Mk의 U1:k-1로의 변환은 두 지도에서 동일하게 존재하는 구역을 Mk로부터 추출한 점군와, U1:k-1로부터 추출한 점군간의 변환으로 재정의될 수 있고, 이는 아래 식 (2)와 같이 표현될 수 있다.
(2) |
[10,11]에 따라 식 (2)를 반복적으로 해를 찾는 방법(iteration)으로 해를 구하기 위해서 의 각 점에 대하여 정규 분포 노이즈를 갖도록 분산(σ)을 부여하였다. 반복적으로 해를 찾는 과정 중 d < δ인 Rk와 tk를 찾은 경우, 반복을 멈춘다. 이후 Rk와 tk에 층간 간격(zbias)을 더해 새로운 tτ를 정의하고, 변환행렬로 Mk를 이동시켜 얻은 를 U1:k-1에 누적하여 새로운 다층 지도 U1:k를 정의하였다. 이 과정을 전체 층(n)까지 반복하여 식 (3)과 같이 전체 층에 대한 지도인 Mtotal을 구축한다.
(3) |
이때, 해당 k층의 누적 궤도(Sk)를 동일한 Rk와 를 이용하여 이동시켜 Mtotal에 대한 누적 궤도(Stotal)를 식 (4)와 같이 생성하였다.
(4) |
2.2 위치 추정 알고리즘
다층 정밀 지도에서의 라이다 센서 기반 위치 추정은 현재 k층의 높이(zbias)와 라이다 센서 정보를 이용하여 [Algorithm 2]와 같이 수행할 수 있다. 우선, 현재 층에 해당하는 지도 점군()을 현재 층의 높이(zk)와 하임계점(l1), 상임계점(l2)을 이용하여 아래 식 (5)와 같이 선별한다.
(5) |
라이다 센서 데이터과 대응시키는 NDT 스캔 매칭 기반의 알고리즘을 통하여 현재 로봇의 위치를 추정한다. 이때, 스캔 매칭의 결과는 식 (1)과 동일한 방식으로 구해질 수 있다.
(6) |
여기서 은 각각 지도 내에서의 로봇의 회전 변환 행렬과 평행 이동 벡터를 나타낸다. 한편 NDT 알고리즘 내부에서는 식 (6)과 같이 반복적으로 해를 찾는 과정에서 과의 초기값을 부여하여 해당 위치를 초기값으로 사용하여 위치 추정을 위한 반복 계산이 빠르게 수렴하게 할 수 있다. 또한 스캔 매칭 기반의 위치 추정 알고리즘을 좀 더 강인하게 수행하기 위해 본 연구진이 다른 논문에서 발표한 위치 추정 알고리즘을 활용하였다[12]. 따라서 본 연구에서는 정밀지도 구축 과정에서 생성한 층별 로봇의 궤도 정보 ST을 활용하여 가장 가까운 위치에 해당하는 점의 좌표를 초기값으로 설정하여 해당 층에서 로봇이 위치 추정을 안정적으로 할 수 있도록 하였다.
층을 이동할 경우, 새로운 층(j)에 대해서 새롭게 을 정의하고, 식 (6)의 NDT 스캔 매칭 기법을 이용하여 새로운 초기 위치를 설정하는 것을 반복한다. 이때, 빠른 초기 위치 설정을 위하여 이전 층에서의 과 현재 층에 해당하는 궤도 정보()을 활용한다.
3. 실험 방법
3.1 하드웨어 세팅
지상로봇은 [Fig. 3]과 같이 2개의 구동바퀴와 4개의 보조바퀴로 구성된 differential wheel구성의 로봇을 활용하였다. 라이다 센서는 16개의 채널과 수직 30°, 수평 360°의 시야각, 100 m의 센서 범위를 가지는 3차원 라이다 센서를 사용하였다. IMU 센서는 정밀 지도 구축과 위치 추정에는 Intel® Core™ i7-11700B Processor (24M Cache, up to 4.80 GHz)가 포함된 탑재형 PC가 활용되었다.
3.2 실험환경
다층 정밀 지도 구축 알고리즘은 KAIST 본교 캠퍼스에 위치한 KI빌딩 1, 2, 3층을 대상으로 테스트되었다.
각층의 정밀 지도를 구축하는 단계에서는 Loop closure를 통하여 지도 제작 시 생겨난 슬립 현상을 보정하였다. 각 층의 지도 제작 시작 위치는 층마다 상이하게 하여, 단일 층 정밀 지도의 x, y 원점은 각각 다르게 설정되었다. 다층 정밀 지도는 단일 층 지도를 낮은 층수부터 쌓아 올리는 방식으로 제작하였다. 단일 층 지도와 다층 지도 사이의 공통 특징점은 중앙 복도를 활용하였다.
4. 실험 결과
4.1 다층 정밀 지도 구축
KI 빌딩의 1층부터 3층에서 구축한 단일 층 지도는 [Fig. 4(a-c)]에서 보는 것과 같다. 각 지도의 원점과 x축 y축 방향은 각각 흰색 점과 빨간색 화살표, 파란색 화살표로 표시하였다. 구축된 알고리즘은 [Algorithm 1]과 같은 방법으로 빌딩의 1층부터 3층까지 순서대로 적층한 결과, 구축된 다층 지도의 모습은 [Fig. 4(d-e)], [Fig. 5]와 같다. 각각의 단일 층 지도에서는 서로 다른 원점을 가졌지만, 구축된 다층 지도는 [Fig. 4(a)]와 같이 적층의 기준으로 삼은 1층의 원점으로 통일되었다.
다층 지도의 x, y축과 yaw의 정확도는 [Fig. 6]에서 E1, E2로 표시된 두 개의 승강기의 x, y좌표를 이용하여 측정하였다. [Fig. 4(d-e)]에서 구축된 3차원 다층 지도에서, 각 층의 E1, E2 승강기의 중심점을 구하고, 두 개의 중심점의 연결한 선분의 중점의 좌표를 비교하여 층간 x, y 축의 차이를 계산하였다.
결과는 [Table 1]에서와 같이 평균적으로 0.29 m, 0.43 m의 층간 차이를 보였다. 각 층의 E1과 E2를 연결한 직선의 기울기를 비교하여 yaw의 차이를 구하였는데, 1층과 2층 사이에는 0.43°, 2층과 3층 사이에는 1.58°로 평균적으로 1.00°의 차이를 보였다. z축은, 구축한 3차원 다층 지도에서의 층간 높이 차이와 [Fig. 5(a)]의 도면상 수치(dz12, dz23)와 비교하여 오차와 오차율을 계산하였다. 이때, 3차원 다층 지도에서의 층간 거리는 위층의 바닥과 아래층에 해당하는 점 중에서 xy 거리가 0.3 m이내인 점들을 대응시켜 두 점의 z 값의 차이를 계산하여 구하였다. 1층과 2층 사이의 z축 거리를 구할 때는 총 1454개의 점, 2층과 3층 사이는 총 1892개의 점들을 대응시켜 구한 값을 평균내었다. 그 결과, [Table 2]과같이 1층과 2층 간 z축 거리의 오차는 0.06 m로 0.03 오차율을 보였고, 2층과 3층 사이에는 0.2 m의 오차와 0.09의 오차율을 보였다.
5. 결 론
본 논문에서는 LOAM 기반의 3차원 다층 정밀 지도 구축과 위치 추정 알고리즘을 제안하였다. 점구름으로 이루어진 단일 층 지도를 전역 정합 알고리즘을 이용하여 적층하는 방식으로 구축된 다층 지도의 정확도는 층 간의 x, y좌표 및 yaw의 차이와 설계도면과 비교한 z축의 오차로 평가하였다. 누적 오차가 가장 큰 3층에서 진행한 위치 추정 실험 결과 실시간이 가능한 빠른 속도와 정확성을 보였다.
다층 3층으로 진행한 다층 지도 구축은 층이 높아질수록 오차가 누적되어 정확성이 낮아진다는 단점을 가진다. 이를 해결하기 위한 층 누적 방향과 방법에 대해서 추가적인 연구를 통하여 개선되어야 할 것이다. 또한 현재까지는 층간 동일한 구조를 활용해 각 층의 지도를 정합하기 때문에 층마다 전혀 다른 구조를 가지는 건물의 경우에는 다층 지도를 생성하는데 있어 한계점이 있다. 따라서 건물의 구조의 제약을 받지 않고 다층지도를 구축하는 방법에 대한 연구가 추가적로 이루어져야 한다.
Notation
Parameter : | Description |
: | Floor feature of point cloud평면 분할을 통하여 분류된 바닥에 해당하는 점 |
: | Edge feature of point cloud포인트 클라우드 중 모서리에 해당하는 특징점 |
: | Non-floor plane feature of point cloud포인트 클라우드 중 면에 해당하는 특징점 |
Qτ : | Lidar odometry (LO)라이다 오도매트리 (LO) |
tτ : | Translation vector at time τ시간τ에서의 평행이동 벡터 |
Rτ : | Rotational matrix at time τ (3 × 3)시간τ에서의 회전 변환 행렬(3 × 3) |
M1:τ : | A single-layer map generated until time τ지도가 생성되기 시작한 시점부터 시간τ까지 생성된 단일 층 지도 |
S1:τ : | The trajectory of the robot until time τ지도가 생성되기 시작한 시점부터 시간τ까지 저장된 로봇의 궤도 |
Mk : | A single-floor map of the kth floor구축된 단일 k-층에 대한 지도 |
Mtotal : | Integrated multi-layer map통합된 다층 지도 |
U1:k : | Multi-floor map stacked up to the kth floork-층까지 적층된 다층 지도 |
: | Point cloud extracted from the kth floor map구축된 단일 k-층 중 추출된 점군 |
: | The target point cloud extracted for the kth floor among the point clouds of the previously built multi-floor map앞서 미리 구축된 다층 지도의 점군 중 k-층에 대해 추출된 타겟 점군 |
Rk : | Rotation matrix that matches the kth floor map to the multi-floor map단일 k-층을 다층 지도로 변환하는 회전 변환 행렬 |
tk : | Translation matrix that matches the kth floor map to the multi-floor map단일 k-층을 다층 지도로 변환하는 평행 이동 벡터 |
: | Point cloud transformed through the kth floor transformation matrix단일 k-층을 변환 행렬을 통해 회전/평행 이동한 점군 |
PI : | Lidar sensor data라이다 센서 데이터 |
: | Estimated rotation matrix of the robot within the map지도 내에서 추정된 로봇의 회전 변환 행렬 |
: | Estimated translation vector of the robot within the map지도 내에서 추정된 로봇의 평행 이동 벡터 (로봇의 지도 내 x,y,z위치) |
References
- R. Philippsen, L. Sentis, and O. Khatib, “An open source extensible software package to create whole-body compliant skills in personal mobile manipulators,” 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, CA, USA, 2011. [https://doi.org/10.1109/IROS.2011.6095163]
- Z. Fan, C. Li, Y. Wang, L. Zhao, L. Yao, G. Zhu, Z. Li, H. Xie, and Y. Xiao, “3D Mapping of Multi-floor Buildings Based on Sensor Fusion,” 2017 International Conference on Industrial Informatics - Computing Technology, Intelligent Technology, Industrial Information Integration (ICIICII), Wuhan, China, 2017. [https://doi.org/10.1109/ICIICII.2017.10]
- X. Wei, J. Lv, J. Sun, and S. Pu, “Ground-SLAM: Ground Constrained LiDAR SLAM for Structured Multi-Floor Environments,” arXiv:2103.03713, , 2021.
- K. S. Kim, S. Lee, and K. Huang, “A scalable deep neural network architecture for multi-building and multi-floor indoor localization based on Wi-Fi fingerprinting,” Big Data Analytics, vol. 3, no. 4, 2018. [https://doi.org/10.1186/s41044-018-0031-2]
- Y. Li, Z. Gao, Z. He, P. Zhang, R. Chen, and N. El-Sheimy, “Multi-sensor multi-floor 3D localization with robust floor detection,” IEEE Access, vol. 6, 2018. [https://doi.org/10.1109/ACCESS.2018.2883869]
- Q. Zhang, M. Wang, and Y. Yue, “Robust Semantic Map Matching Algorithm Based on Probabilistic Registration Model,” 2021 IEEE International Conference on Robotics and Automation (ICRA), Xi’an, China, 2021. [https://doi.org/10.1109/ICRA48506.2021.9561176]
- Y. Yue, C. Zhao, Z. Wu, C. Yang, Y. Wang, and D. Wang, “Collaborative semantic understanding and mapping framework for autonomous systems,” IEEE/ASME Transactions on Mechatronics, vol. 26, no. 2, 2020. [https://doi.org/10.1109/TMECH.2020.3015054]
- J. Zhang and S. Sanjiv, “LOAM: Lidar Odometry and Mapping in Real-time,” Robotics: Science and Systems, vol. 2, no. 9, 2014, [Online], https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf, .
- T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018. [https://doi.org/10.1109/IROS.2018.8594299]
- P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453), Las Vegas, NV, USA, 2003. [https://doi.org/10.1109/IROS.2003.1249285]
- C. Ulaş and H. Temeltaş, “3D multi-layered normal distribution transform for fast and long range scan matching,” Journal of Intelligent & Robotic Systems, vol. 71, 2013. [https://doi.org/10.1007/s10846-012-9780-8]
- D. Lee, G. Kang, B. Kim, and D. H. Shim, “Assistive Delivery Robot Application for Real-World Postal Services,” IEEE Access, vol. 9, 2021. [https://doi.org/10.1109/ACCESS.2021.3120618]
2020 성균관대학교 기계공학부(학사)
2020~현재 KAIST 로봇공학학제전공 석사과정
관심분야: 자율로봇, 자율 시스템, 로보틱스
2018 국민대학교 자동차공학(학사)
2020 KAIST 미래자동차학제전공(석사)
2020~현재 KAIST 전기 및 전자공학부 박사과정
관심분야: 로보틱스, 항법
1991 서울대학교 기계공학(학사)
1993 서울대학교 기계공학(석사)
2000 University of California, Berkeley Mechanical Engineering(박사)
관심분야: 로보틱스, 드론, 자율로봇, 자율주행차, 무인 항공기