Sorry.
You are not permitted to access the full text of articles.
If you have any questions about permissions,
please contact the Society.
죄송합니다.
회원님은 논문 이용 권한이 없습니다.
권한 관련 문의는 학회로 부탁 드립니다.
[ ARTICLE ] | |
The Journal of Korea Robotics Society - Vol. 17, No. 3, pp. 373-380 | |
Abbreviation: J. Korea Robot. Soc. | |
ISSN: 1975-6291 (Print) 2287-3961 (Online) | |
Print publication date 31 Aug 2022 | |
Received 24 Mar 2022 Revised 29 Apr 2022 Accepted 07 May 2022 | |
DOI: https://doi.org/10.7746/jkros.2022.17.3.373 | |
ICP 계산속도 향상을 위한 빠른 Correspondence 매칭 방법 | |
A Fast Correspondence Matching for Iterative Closest Point Algorithm | |
1Undergraduate Student, Department of Electrical Engineering, Inha University, Incheon, 22212, Korea (gunmaplehee@gmail.com) | |
2Undergraduate Student, Department of Mechanical Engineering, Inha University, Incheon, 22212, Korea (jahii@naver.com) | |
Correspondence to : †Assistant Professor, Corresponding author: Department of Electrical Engineering, Inha University, Incheon, 22212, Korea (kwangki.kim@inha.ac.kr) Contributed by footnote: * The authors (G. Shin and J. Choi) have equally contributed to this work | |
CopyrightⓒKROS | |
Funding Information ▼ |
This paper considers a method of fast correspondence matching for iterative closest point (ICP) algorithm. In robotics, the ICP algorithm and its variants have been widely used for pose estimation by finding the translation and rotation that best align two point clouds. In computational perspectives, the main difficulty is to find the correspondence point on the reference point cloud to each observed point. Jump-table-based correspondence matching is one of the methods for reducing computation time. This paper proposes a method that corrects errors in an existing jump-table-based correspondence matching algorithm. The criterion activating the use of jump-table is modified so that the correspondence matching can be applied to the situations, such as point-cloud registration problems with highly curved surfaces, for which the existing correspondence-matching method is non-applicable. For demonstration, both hardware and simulation experiments are performed. In a hardware experiment using Hokuyo-10LX LiDAR sensor, our new algorithm shows 100% correspondence matching accuracy and 88% decrease in computation time. Using the F1TENTH simulator, the proposed algorithm is tested for an autonomous driving scenario with 2D range-bearing point cloud data and also shows 100% correspondence matching accuracy.
Keywords: Scan Matching, Point-Cloud Registration, Iterative Closest Point Methods, Jump Table, LiDAR, Correspondence Matching, Pose Estimation |
자율주행 분야 연구에는 정확하고 실시간에 가까운 측위가 보장되어야 한다. 로봇에 부착된 다양한 센서를 통해서 측위를 수행하는데 mm 수준 오차의 연속적인 거리 데이터를 얻는 라이다를 활용한 측위 기술에는 Iterative Closet Point (ICP)를 이용한 Scan Matching 방법이 많이 사용되어 왔다. 이는 Simultaneous Localization and Mapping (SLAM)의 성능향상[1] 뿐만 아니라 실내외 지도 작성 및 주변 환경 탐지와 추적에도 활용되고 있다[2,3].
Besl과 Mckay가 처음 제안한 ICP방법은 서로 다른 위치(position)와 방향(orientation)에서 측정된 두 개의 포인트 클라우드 집합들을 정합하여 서로 다른 두 시점에서의 상대적인 자세(pose)를 추정하는 기술로서, 이동로봇의 자세 추정에 적용될 수 있다[4][Fig. 1]. 이동로봇이 한 시점의 자세에서 측정하여 얻은 측정 물체 또는 지형의 포인트 클라우드를 ‘기준 포인트 클라우드(Reference Point Cloud, R_PC)’라고 하고, 다음 시점의 다른 자세에서 얻은 포인트 클라우드를 ‘변환 포인트 클라우드(Transformed Point Cloud, T_PC)’라고 할 때, 이동로봇의 자세추정 문제는 변환 포인트 클라우드를 기준 포인트 클라우드에 정합하는 병진이동과 회전이동을 구하는 문제로 정의될 수 있다. 그리고 해당되는 포인트 클라우드 정합 문제는 ICP 또는 그와 유사한 변형 알고리즘들을 사용하여 해를 구할 있다. ICP 과정 중 중요한 부분은 변환 포인트 클라우드의 각 점에 대한 기준 포인트 클라우드 위의 유의미한 부분을 찾는 Correspondence 매칭 단계와 이 결과로부터 얻은 오류 함수를 최소화하는 변환 행렬을 구하는 최적화 단계이다. 두 단계가 반복적으로 수행되며 최적화의 결정변수인 변환 행렬의 변화가 임계 값보다 작으면 ICP를 종료하여 최종의 변환 행렬에서 이동로봇의 자세 변화를 표현하는 병진이동과 회전이동을 정의할 수 있다.
이동로봇의 자세 결정을 위한 측위 기술에서 중요한 성능지표 중에 하나는 연산의 실시간성을 확보하는 것이다. [4]에서 Correspondence 단계의 과정이 ICP 알고리즘의 총 연산 시간에서 약 95%를 차지한다는 조사결과를 발표하였는데, 이는 ICP를 활용한 이동로봇 실시간 자세 결정 또는 추정을 위해서는 Correspondence 단계의 시간을 단축하는 것이 중요하다는 것을 의미한다. Correspondence 매칭 시간을 줄이기 위한 다양한 연구들이 진행되었는데, [5]에서는 변환 포인트 클라우드의 각 점에서 거리를 비교할 기준 포인트 클라우드 점들 중 후보를 선정하는 Jump Table을 이용하였고, [6,7]에서는 kd-trees, [8]에서는 다중 Z-buffer, [9]에서는 계층적 모델을 활용하여 Correspondence 단계의 시간을 단축했다.
Correspondence 매칭 시간 단축을 통한 연산 시간 감소와 함께, 또 다른 중요한 문제는 측정 데이터의 잡음과 Correspondence 매칭 오류에 취약한 최소자승법 기반의 ICP 알고리즘의 단점을 극복하는 것이다. Correspondence 단계의 결과를 바탕으로 최소자승법에 기반한 최적의 변환 행렬 계산은Correspondence 매칭 오류에 해당하는 이상치(outlier)에 취약하다. 그러므로 위의 연구에서 속도 증대와 함께 Correspondence 매칭 오류를 줄이기 위한 매칭 알고리즘의 정확성에 대한 검증이 동반되어야 한다. 여기서 이상치에 강건한 최적화를 위한 이상치 제거 방법(e.g., RANSAC) 및 강건 추정법(e.g., M-estimation)에 대한 연구가 활발하게 진행되고 있기는 하지만, Correspondence 매칭 오류에 의한 이상치 자체를 감소시키는 것도 좋은 방법이 될 수 있다.
본 연구에서는 [5]에서 제시된 희소 탐색(sparse search)을 통하여 Correspondence 단계에서 연산시간을 단축시키는 Jump-Table방법에 대한 정확성 측면에서의 두 가지 오류를 소개하고 이를 보완한 새로운 알고리즘을 제시한다. 제시한 알고리즘의 성능은 Hokuyo-10LX라이다를 이용한 실제 하드웨어 실험과 F1TENTH Simulator[10]를 사용한 시뮬레이션을 통해서 검증하였다.
ICP (Iterative Closest Point)는 서로 다른 위치에서 얻은 두 포인트 클라우드가 있을 때 아래 4단계를 반복하여 두 포인트 클라우드를 정합하고 마지막 단계가 끝날 때마다 새롭게 위치 차이를 추정한다. [Fig. 1]에서 볼 수 있듯이, 상대적 포즈 차이는 두 포인트 클라우드의 기준 좌표계 사이의 강체 변환 행렬(Rigid Body Transformation Matrix)로 나타낸다. 기준 포인트 클라우드의 기준 좌표계가 (X1, Y1), 변환 포인트 클라우드의 기준 좌표계가 (X2, Y2)일 때 두 좌표계 사이의 변환 행렬들의 관계는 식 (1)과 같다.
(1) |
포인트 클라우드는 라이다의 기준 좌표계에서 Z=0 평면에 표현되고 Z축으로 라이다의 물리적 이동이 없다고 가정한다. 4단계를 모두 수행하면 ICP를 한번 수행했다고 하며 ICP가 k번 수행되었을 때 추정한 위치 차이를 qk(Rk, tk)으로 나타낸다. [Fig. 2]의 순서도에 해당하는ICP알고리즘은 다음 네가지 단계를 반복한다.
1. 변환(Transformation): 이전 변환 행렬 qk-1 (Rk-1, tk-1)으로 변환 포인트 클라우드를 변환하여 새로운 포인트 클라우드를 만들고 이를 k번째 ‘중간 포인트 클라우드(Middle Point Cloud, M_PC)’라 한다. 변환 행렬의 초기값(q0)은 ICP 성능에 큰 영향을 주는데 이는 Wheel Odometry 또는 관성 측정 장치(IMU)와 같은 다른 센서를 통해 예측한다. 포인트 클라우드는 라이다의 기준 좌표계에서 Z=0 평면에 나타나므로 포인트 클라우드의 점들에 일정한 규칙으로 순서를 매겼을 때 중간 포인트 클라우드의 i번째 점은
(2) |
2. 대응 관계(Correspondence): k번째 중간 포인트 클라우드의 각 점에 대응하는 기준 포인트 클라우드 위의 유의미한 부분을 찾는다. 여기서 유의미한 부분(Segment)은 유클리드 공간에서 거리가 가장 가까운 점을 기반으로 형성되는데 [2]의 점-점(point-to-point) ICP의 경우 가장 가까운 점이 되며 [5]의 점-선(point-to-line) ICP의 경우 가장 가까운 점 두개를 이은 직선이 된다. 기준 포인트 클라우드 위의 유의미한 부분은 대응하는 변환 포인트 클라우드의 점(
3. 이상치 제거(Outlier rejection): ICP는 correspondence결과를 바탕으로 최소자승법(Least Squares Method)를 통해 해를 구한다. 최소자승법은 이상치에 취약하므로 Correspondence 결과에 가중치를 부여하는 M-estimation 방법을 사용하거나 이상치를 판별하여 제거하는 RANSAC 방법이 많이 사용된다.
4. 오차함수 최소화(Minimizing Error Function): 이상치를 제거한 Correspondence 결과를 이용하여 오차함수를 구한 뒤 최소자승법으로 이를 최소화하는 qk를 구한다. ICP 종류에 따라 오차함수의 형태가 구분된다. [Fig. 3(a)]의 점-점의 경우 강체 변환된
(3) |
(4) |
ICP 알고리즘은 변환 포인트 클라우드의 각 점에 대응하는 기준 포인트 클라우드의 유의미한 부분을 찾고 그 사이의 거리 합을 최소로 만드는 변환 행렬 q(R,t)를 구하는 과정을 반복적으로 수행한다. ICP를 한번 수행했을 때 4단계를 하나의 식으로 표현하면 식 (5)와 같다.
(5) |
A.Censi는 Correspondence의 시간을 Jump Table을 사용하여 단축했다[5]. Correspondence는 중간 포인트 클라우드의 각 점에 대해 기준 포인트 클라우드를 탐색하며 가장 가까운 점을 찾는다. [5]에서는 기준 포인트 클라우드 위의 모든 점을 차례대로 탐색하지 않고 Jump Table을 사용하여 기존 포인트 클라우드의 점들을 점프하며 탐색한다.
Jump Table은 원점(라이다)에서 기준 포인트 클라우드의 각 점까지의 거리 비교를 통해 한 점에 대해 서로 다른 네 개의 점을 설정한다(up_small, up_big, down_small, down_big). 기준 포인트 클라우드 한 점에 대해 반 시계 방향을 위(up), 시계 방향을 아래(down)로 할 때, 최초로 거리가 큰 점을 big, 거리가 작은 점을 small로 설정한다. 예를 들어, 어떤 기준 포인트 클라우드의 20번째 점의 Jump Table은 [Fig. 4(a)]와 같이 설정된다.
중간 포인트 클라우드의 한 점(
아래 방향으로 탐색할 때 새롭게 best_dis가 업데이트 될 수 있는 경우는 기준 포인트 클라우드의 점이 주황색 구역에 존재할 때이다. 위 상황에서 [5]는 Jump Table을 이용하여 아래 방향에 존재하는 점들을 차례대로 탐색하지 않고 주황색 영역에 존재할 가능성이 있는 점들만 Jump하며 탐색한다. 그리고
[Fig. 4(b)]의
(a) | (b) | |||
---|---|---|---|---|
Status | Status | |||
UP | up_small | up_small | ||
UP | up_big | up_big | ||
DOWN | down_small | down_small | ||
DOWN | down_big | down_big |
대부분의 상황에서는 [Table 1(a)]의 기준이 합리적이지만
[5]에서 제안된 Jump Table에서
[5]의 경우,
우리가 새로 고안한 알고리즘의 Pseudo Code는 [Algorithm 1]과 같으며 사용된 기호의 의미는 [Table 2]와 같다.
21 | //update best point if the distance between checking point and |
42 |
Symbol | meaning |
---|---|
Middle point cloud (M_PC) | |
The ith point in M_PC | |
Reference point cloud (R_PC) | |
The closest point in R_PC from current |
|
best_dis | Distance from |
The angle between -x axis and line from origin (LiDAR) to |
|
angle_increment | The unit angle of the point cloud which LiDAR reads |
start_index | The point in R_PC which starts to search |
dist_up/down | The distance from |
up_stopped/ down_stopped |
The index which determines whether to stop searching UP/DOWN direction |
now_up | The index which determines whether to search UP direction |
The point in R_PC which currently searching in UP/DOWN direction | |
Angle between two straight lines. First line from origin to |
|
Distance from origin to |
|
min_dist | Length of the perpendicular from |
Angle between two straight lines. First from |
[Algorithm 1]의 주요 구문의 의미는 다음과 같다:
line 1 : 포인트 클라우드 PM 위의 모든 점에 대해 탐색.
line 9 : up_stopped 과 down_stopped 모두 참이면 탐색 종료.
line 10 : 탐색이 중지된 방향의 반대쪽으로 탐색하도록 설정.
line 12~14 :
line 24 : best_dis가 min_dist보다 크면 더 이상 해당 방향으로 탐색하는 것이 무의미하므로 탐색 종료.
line 26~27 : θcriteria를 코사인 제 2 법칙을 이용하여 구함.
line 30~34 : Table 1(1)를 기준으로 θcriteria의 값에 따라 다음으로 탐색할 점을 결정.
Hokuyo-10LX와 F1TENTH Simulator를 각각 활용하여 두 번의 실험을 진행했다[Fig. 5]. 먼저, 270°의 시야각을 가진 Hokuyo-10LX 라이다를 F1TENTH 경주차에 부착하고 인하대학교 하이테크 건물 6층에서 구동하여 실시간으로 데이터를 받았다. 이 데이터를 우리의 Jump Table 기반 Correspondence, [5]의 Jump Table 기반 Correspondence, 그리고 일반적인 Correspondence 알고리즘(Naïve algorithm)에 동일하게 입력하고 비교했다. 일반적인 Correspondence는
Jump Table이 360°에 대해 유효한 우리의 알고리즘의 성능을 270°시야각의 Hokuyo-10LX으로 확인하기에는 한계가 있어 360°시야각의 라이다가 구현된 F1TENTH Simulator[10]에서 추가로 실험했다. [Fig. 5(b)]와 같이 Simulator에서 제공하는 주행환경에서 차량이 한 쪽 벽면을 따라가며 얻은 라이다 데이터를 이용하여 같은 비교를 진행하였다. 차량의 선속도와 각속도는 각각 1.8 m/s와 0.3 rad/s이며 차량의 가로 길이와 세로 길이는 각각 0.33 m와 0.2 m이다.
두 실험에서 사용된 소스코드는 https://github.com/jahii/F1tenth의 Experiment 브랜치 안에 scan_matching_skeleton/src/correspond.cpp에서 확인할 수 있다. 또한, 실험 영상은 https://youtu.be/VvvXB3yQ_Wg 에서 확인할 수 있다.
Hokuyo-10LX를 이용한 첫번째 실험에서는 대부분의 경우 중간 포인트 클라우드의 각 점에 대한
Our new jump table algorithm(①) |
Naïve correspondence algorithm(②) |
(1-①/②) *100% | |
---|---|---|---|
The number of search points | 14,178 | 1,166,400 | 98.784% |
Computation time [ms] | 70.1417 | 584.008 | 87.990% |
F1TENTH Simulator를 이용한 두 번째 실험에서는
본 연구에서는 [5]의 Jump Table 기반 Correspondence에서 오류를 도출하고 이를 보완하면서 일반적인 Correspondence 방법보다 시간이 크게 단축된 새로운 Jump Table 기반 Correspondence을 제안한다. F1tenth Simulator와 Hokuyo-10LX를 활용한 실험을 통해 성능향상을 확인하였다. 향후, ICP의 Correspondence 시간을 줄이는 기존의 방법들과 본 연구논문에서 제안한 방법의 성능을 직접적으로 비교하는 연구를 진행할 예정이다.
This research was supported by the Basic Science Research Program through the National Research Foundation of Korea (NRF) funded by the Ministry of Education (NRF-2020R1F1A1076404)
1. | D. Hahnel, W. Burgard, D. Fox, and S. Thrun, “An efficient fastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements,” 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453), Las Vegas, NV, USA, 2003.![]() |
2. | J. Minguez, L. Montesano, and L. Montano, “An architecture for sensor-based navigation in realistic dynamic and troublesome scenarios,” 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566), Sendai, Japan, 2004.![]() |
3. | D. Schulz, W. Burgard, D. Fox, and A. B. Cremers, “Tracking multiple moving targets with a mobile robot using particle filters and statistical data association,” 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164), Seoul, South Korea, 2001.![]() |
4. | P. J. Besl and Neil D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239-256, Feb., 1992.![]() |
5. | A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, USA, 2008.![]() |
6. | M. Greenspan and M. Yurick, “Approximate k-d tree search for efficient ICP,” Fourth International Conference on 3-D Digital Imaging and Modeling, 2003, Banff, AB, Canada, 2003.![]() |
7. | W.-S. Choi, Y.-S. Kim, S.-Y. Oh, and J. Lee, “Fast Iterative Closest Point framework for 3D LIDAR data in intelligent vehicle,” 2012 IEEE Intelligent Vehicles Symposium, Madrid, Spain, 2012.![]() |
8. | R. Benjemaa and F. Schmitt, “Fast global registration of 3D sampled surfaces using a multi-z-buffer technique,” Proceedings. International Conference on Recent Advances in 3-D Digital Imaging and Modeling (Cat. No.97TB100134), 1997, pp. 113-120.![]() |
9. | D. Kim and D. Kim, “A Fast ICP Algorithm for 3-D Human Body Motion Tracking,” IEEE Signal Processing Letters, vol. 17, no. 4, pp. 402-405, April 2010.![]() |
10. | F1TENTH Simulator, F1TENTH Racing Community, [Online], https://github.com/f1tenth/f1tenth_gym_ros, Accessed: May 27, 2022. |
2017~현재 인하대학교 전기공학과(학사)
관심분야: 컴퓨터 비전, SLAM, 자율주행
2016~현재 인하대학교 기계공학과(학사)
관심분야: 로보틱스, 경로 추정, 자율주행
2013 일리노이 주립대학교(UIUC) 박사
2013~2015 조지아 공과대학(Georgia Institute of Technology) 박사후연구원
2016~2017 현대자동차 책임연구원
2017~현재 인하대학교 전기공학과 조교수
관심분야: 제어이론, 최적화이론, 최적제어, 계산과학, 로보틱스, 센서퓨전, 공간인지
Copyright © KOREA ROBOTICS SOCIETY. All rights reserved.
#504, The Korea Science and Technology Center, (635-4,Yeoksam-dong)22