Journal of Institute of Control, Robotics and Systems (2014) 20(11):1153-1163 http://dx.doi.org/10.5302/J.ICROS.2014.14.0027 ISSN:1976-5622 eISSN:2233-4335
2 차원 레이저 거리계를 이용한 수직/수평 다각평면 기반의 위치인식 및 3 차원 지도제작
3D Simultaneous Localization and Map Building (SLAM) using a 2D Laser Range Finder based on Vertical/Horizontal Planar Polygons
이 승 은*, 김 병 국 (Seungeun Lee1,* and Byung-Kook Kim1)
1Department of Electrical Engineering, KAIST (Korea Advanced Institute of Science and Technology)
Abstract: An efficient 3D SLAM (Simultaneous Localization and Map Building) method is developed for urban building environments using a tilted 2D LRF (Laser Range Finder), in which a 3D map is composed of perpendicular/horizontal planar polygons. While the mobile robot is moving, from the LRF scan distance data in each scan period, line segments on the scan plane are successively extracted. We propose an “expected line segment” concept for matching: to add each of these scan line segments to the most suitable line segment group for each perpendicular/horizontal planar polygon in the 3D map. After performing 2D localization to determine the pose of the mobile robot, we construct updated perpendicular/horizontal infinite planes and then determine their boundaries to obtain the perpendicular/horizontal planar polygons which constitute our 3D map. Finally, the proposed SLAM algorithm is validated via extensive simulations and experiments.
Keywords: 3D map building, mobile robot, SLAM, LRF, vertical/horizontal planar polygon
I. 서론
이동 로봇의 위치인식 및 지도작성은 중차대한 과제이다.
이동 로봇은 지도와 같은 주변 환경에 대한 정보가 주어지지 않더라도 자신의 임무를 수행할 수 있어야 한다. 이때 이동 로봇은 자신의 위치를 추정하는 동시에 주변 환경에 대한 정 보를 수집하여 지도를 작성하여야 하는데 이와 같은 문제는 SLAM (Simultaneous Localization and Mapping) 혹은 CML (Concurrent Mapping and Localization)이라 불리고 있다. 위치 추정을 위해서는 지도가 필요하고 지도작성을 위해서는 위 치 추정이 필요하게 되는데 그 특성이 닭이 먼저냐 달걀이 먼저냐의 문제와 비슷하여 SLAM 문제는 도전적인 연구분야 중 하나로 꼽힌다.
보다 정확한 환경의 표현을 위하여 적절한 센서의 선택과 3차원(3D) 지도가 필요하다. 2차원(2D) 지도의 경우 그림 1(a) 와 같이 탁자위에 놓인 물체에 대하여 접근불가 영역으로 인 식할 수 밖에 없지만, 그림 1(b)와 같이 3D 지도에서는 일정 높이를 가지지만 접근 가능한 물체로 인식할 수 있다. 그러 므로 보다 성공적인 업무수행을 위해서는 3D 지도가 필요하 게 된다.
3D 지도작성을 위해 비전(vision) 센서(sensor), 적외선 거리 센서 등 여러 센서가 이용되고 있으나 그 중 레이저 거리계 (laser range finder) 센서는 높은 정확도의 데이터(data), 짧은 데 이터 획득 주기, 그리고 광범위한 스캔(scan) 영역 등의 장점 으로 인해 널리 이용되고 있다. 그러나 3D 레이저 거리계의
경우 가격이 매우 비싸다는 단점이 있어 2D 레이저 거리계 를 이용한 3D 지도작성 연구가 많이 시도되고 있다[1-3]. Besl [4]에 의해 제안된 ICP (Iterative Closest Points) 알고리즘은 로 봇이 이동하면서 여러 장소에서 획득한 3D 점(point) 데이터 를 오차를 최소화하면서 전역 좌표계에 등록하는 데 사용된 다. ICP 알고리즘은 높은 정확도로 인해 현재까지도 많이 사 용되고 있으나, 반복적으로 스캔 데이터를 매칭(matching) 하 고 쌍(pair)을 이루는 데 수행시간이 오래 걸리는 단점이 있 어서 이를 극복하기 위하여 ICL (Iterative Closest Line)과 같은 향상된 알고리즘들이 제안되었다[5]. Magnusson [6]은 3D 스캔 데이터 매칭 및 전역 좌표계 등록을 위해 복셀(voxel) 기반의 3D-NDT (Normal Distributions Transform)를 사용하였다. 점이나 복셀 기반의 방법들은 환경에서 어떠한 특징도 가정하지 않 아 다양한 환경에서 적용 가능하다는 장점이 있지만, 연산시 간이 길다는 단점이 있다.
보다 효율적인 평면 기반의 지도, 즉 3D 스캔 데이터로부 터 평면을 추출하고 추출된 평면을 기반으로 스캔 데이터를 매칭 및 전역 좌표계에 등록하기 위한 시도 또한 이루어졌다 [7,8]. 이 경우 연산시간을 획기적으로 줄일 수 있었고 지도
Copyright© ICROS 2014
* Corresponding Author
Manuscript received March 14, 2014 / revised July 19, 2014 / accepted July 21, 2014
이승은, 김병국: 한국과학기술원 전기 및 전자공학과 ([email protected]/[email protected])
(a) (b) 그림 1. 3D 지도의 필요성.
Fig. 1. Necessity of 3D map.
이 승 은, 김 병 국 1154
또한 다각 평면의 형태로 데이터의 양을 획기적으로 줄여 메 모리(memory) 효율적인 지도제작이 가능하였다. 하지만 이들 은 거리영상(range image)으로부터 평면을 추출하여 로봇의 stop-and-go 이동 방식이 불가피하였다. Stop-and-go 이동 방식 을 극복하고자 Cole [9]은 로봇의 이동 중 획득된 3D 데이터 스트림(stream)을 이동한 거리에 따라 구간으로 나누어 처리 하였다. 하지만 로봇이 급격하게 회전하는 경우에 획득된 데 이터를 버려야 하는 단점이 있고, 지도 또한 점군(point cloud) 의 형태로 많은 메모리 사용이 불가피하였다. Hwang [10] 또 한 stop-and-go 이동방식을 사용하지 않고 로봇이 이동중에 지도제작을 수행하였으나 로봇이 이동시 위치추정을 수행하 지 않아 장거리 주행시 큰 오차가 누적될 것으로 예상되고 지도 또한 점군의 형태로 많은 메모리 사용이 불가피하다.
Huhle [11]은 레이저 거리계 센서 중심점과 이웃하는 두 개의 점 데이터에 의해 생성되는 삼각형을 빈 공간(triangle of free space)으로 간주하고 이 삼각형과 다른 점들에 의해 생성된 삼 각형이 최대한 겹치지 않게끔 하는 방식으로 연속적인 스캔 데이터를 등록하고자 하였다. 하지만 충분한 영역의 삼각형 생성을 위해서는 스캔 데이터의 샘플링(sampling)이 필요하다 는 단점이 있고 연산시간 또한 길다. 즉 기존에 진행되었던 연구들은 긴 연산시간 혹은 알고리즘의 특성상 스캔 데이터 매칭을 위하여 stop-and-go 이동 방식을 취하여 로봇의 이동 성에 제약을 주거나 스캔 데이터의 손실이 불가피하였다.
본 논문에서는 로봇의 이동과 동시에 빠른 속도로 스캔 데이터를 매칭하는 방법을 제안하고, 로봇의 위치추정과 3D 수직/수평 다각평면 기반의 지도제작 방법을 제안한다. 로봇 의 이동과 동시에 LRF 스캔 데이터로부터, 효율적으로 스캔 선분을 추출하고, 이들 각각을 기존의 선분 그룹(group)들과 의 매칭을 “예측 선분” 개념을 활용하여 수행하고, 로봇의 위치추정을 수행한 후, 점군이나 복셀의 형태가 아닌 수직/수 평 무한평면의 갱신, 꼭지점 추출을 통한 수직/수평 다각평면 의 갱신을 통하여, 수직/수평 다각평면들로 구성된 3D 지도 를 구축한다. 제안한 방법들을 검증하기 위하여 MATLAB을 사용한 시뮬레이션을 수행하였고, 전동 스쿠터를 개조한 이 동 로봇을 사용하여 ROS (Robot Operating System) 기반의 실 험 또한 수행하였다.
본 논문은 다음과 같이 구성되었다. 먼저 II장에서는 3D 지도의 정의 및 지도제작 전체 알고리즘에 대해서 소개를 한 다. III장에서는 “예측 선분” 개념과 스캔 데이터를 매칭하는 방법에 대해서 설명하고, IV장에서는 수직/수평 무한평면 추 출에 대해서 설명한다. V장에서는 다각평면을 구성하는 방법 에 대해서 설명하고 VI장에서는 지도제작 알고리즘의 시뮬 레이션 및 실험 결과에 대해서 설명한다. 마지막으로 VII장 에서는 결론에 대해서 다룬다.
II. 지도 및 지도제작 알고리즘의 전체적인 소개 1. 기울어진 2D 레이저 거리계
본 논문에서는 건물 벽면 및 바닥면과 같이 도심 건물 환 경에서 흔히 볼 수 있는 수직/수평 다각평면을 지도의 구성 요소로 선택하였다. 그리고 2D 레이저 거리계를 가지고 3D 데이터를 얻기 위하여 그림 2와 같이 2D 레이저 거리계가 지면을 향하여 기울어지게끔 설치하였다.
그림 2. 지면을 향하여 기울어진 레이저 거리계.
Fig. 2. LRF tilted toward the ground.
2D 레이저 거리계를 가지고 단 한 번의 스캔만으로는 수 직/수평 다각평면을 추출할 수 없지만 스캔 평면상에서 선분 으로서는 검출할 수 있다. 검출된 선분이 실제로 수직/수평 다각평면으로부터 온 것이라면 로봇의 이동후의 연속적인 스캔에서도 선분들로 추출될 것이다. 본 연구는 이에 착안하 여 연속적인 스캔 데이터에서 추출된 선분들을 모아 수직/수 평 다각평면을 구성하도록 하였다.
2. 3D 환경지도의 정의
본 논문에서의 3D 환경지도는 수직/수평 다각평면들을 구 성요소로 가진다. k 번째 시간 단계(time step)에서 N 개의 수kv
직 다각평면들로 구성된 수직 다각평면 집합 Vk와 N 개의 kh
수평 다각평면들로 구성된 수평 다각평면 집합 Hk는 다음 과 같이 나타낸다.
, ,
{ | 1,..., },v { | 1,..., }h
k k i k k k i k
V = v i= N H = h i= N (1) 그리고 각각의 수직/수평 다각평면은 다각평면 꼭지점들의 전역좌표들로 표현된다. 즉, vc,
N 개의 꼭지점들로 이루어진 k i
하나의 수직 다각평면 v 와, k i, hc,
N 개의 꼭지점들로 이루어k i
진 하나의 수평 다각평면 h 는 다음과 같이 나타낸다. k i,
, ,
, ,
{( , , ) | 1,..., } {( , , ) | 1,..., }
vc
k i j j j k i
k i j j j k ihc
v x y z j N
h x y z j N
= =
= = (2)
수직 다각평면 vc,
N 개의 꼭지점들은 다음 그림과 같이 k i
, cos , sin ,
v v v
k i x k i y k i
ρ = θ + θ 로 표현되는 수직 무한평면상에 존 재하고 수평 다각평면 hc,
N 개의 꼭지점들은 k i h,
z ρ= k i로 표현 되는 수평 무한평면상에 존재하게 된다.
그림 3. 수직/수평 무한평면의 표현.
Fig. 3. Representation of vertical/horizontal infinite plane.
Seungeun Lee and Byung-Kook Kim
2 차원 레이저 거리계를 이용한 수직/수평 다각평면 기반의 위치인식 및 3 차원 지도제작 1155
LRF에 의하여 k 번째 시간 단계에 스캔 평면상에서 추출 되는 N 개의 2D 스캔 선분들 ks k { , | 1,..., }
L L s
k i k
S = s i= N 를 전 역 좌표계로 변환하면 3D 스캔 선분들 Sk={sk i, |i=1,...,
s}
N 를 얻게된다. 그리고 각각의 3D 스캔 선분 k s 는 선분k i,
의 양 끝점 (p pbk i,, k ie,), 선분의 길이 lk i,, 선분을 구성하는 점들의 개수 Nk ip,, 점들의 전역 좌표 집합 Pk i, ={pk i j, , |pk i j, ,
( , , ),x y z jj j j 1,...,Nk ip,},
= = 그리고 점들에서 선분까지 수직
거리들의 표준편차 σ 로 표현된다. k i,
, , , , , , ,
, , 3 , , ,
{( , ), , , , |
, , , , }
b e p
k i k i k i k i k i k i k i
b e p
k i k i k i k i k i
s p p l N P
p p l N
σ σ
=
∈R ∈R (3)
수직/수평 다각평면을 추출하기 위해 여러 시간 단계에 걸 쳐 추출된 서로 비슷하고 근접한 3D 스캔 선분들을 모아 ‘선 분 그룹’을 형성하게 된다. 어떤 시간 단계에서든지 새로운 수 직/수평 다각평면이 추출되기 위해서는 선분 그룹이 먼저 형성 되어야 하며, 수직/수평 다각평면을 추출하기 위한 조건들이 만족되었을 때 한 선분 그룹으로부터 한 수직/수평 다각평면 이 추출되는 것이다. k 번째 시간 단계에서 이제까지 누적된
N 개의 선분 그룹들로 이루어진 선분 그룹들의 집합을 g
{ |G ll 1,...,Ng}
Γ = = 라 할 때, N 개의 3D 스캔 선분들로 lgs
이루어진 각각의 선분 그룹 G 은 다음과 같이 표현된다. l
, ,
{ | 1,2,..., , 1,2,..., , mutuallycloseenough}
gs
l a i k
a i
G s a k i N
s
= = =
(4)
3. 전체적인 SLAM 알고리즘
제안된 전체적인 위치인식 및 지도제작 알고리즘에 대해 서 설명한다. 매 스캔마다 아래의 단계들을 수행한다.
단계 1: 3D 스캔 선분 추출에서는 레이저 거리계로 획득한 거리 데이터로부터 스캔 평면상에서 양방향 선분 추출 방법 (method and apparatus for bidirectional line segmentation of scan distance data) [12]을 이용하여 선분들을 추출하고 전역 좌표의 3D 스캔 선분들 S 로 변환시킨다. k
단계 2: 매칭에서는 3D 스캔 선분들이 어떤 평면으로부터 추출된 것인지 판단하기 위하여 수직/수평 다각평면과의 매 칭을 진행하여, <3D 스캔 선분, 수직 다각평면>, <3D 스캔 선 분, 수평 다각평면>쌍들을 결정한다. 초기에는 수직/수평 다 각평면이 존재하지 않으므로, <3D 스캔 선분, 선분 그룹>쌍 들을 결정한다.
단계 3: 위치 추정 및 스캔 선분들의 좌표 변환에서는 매 칭된 <3D 스캔 선분, 수직 다각평면> 쌍들을 가지고 이동 로봇의 포즈(pose) (xrobot,yrobot,θrobot)를 추정한다. 그리고 3D 스캔 선분들이 정확한 전역 좌표를 갖도록 추정한 포즈 정보 를 이용하여 다시 한번 좌표 변환을 해 준다.
단계 4/5: 수직/수평 무한평면 추출에서는 높이(height)가 일정 값 이상/미만인 매칭된 <3D 스캔 선분, 선분 그룹> 쌍 들에 대해 3D 스캔 선분을 선분 그룹에 포함시키고 이 선분 그룹으로부터 수직/수평 무한평면을 추출한다. 그리고 매칭 된 <3D 스캔 선분, 수직/수평 다각평면> 쌍들에 대해서도
3D 스캔 선분이 수직/수평 다각평면에 포함되도록 수직/수평 다각평면의 무한평면 정보 ρk iv,, θk iv,, h,
ρ 를 갱신한다. k i
단계 6/7: 수직/수평 다각평면 추출에서는 선분 그룹으로 부터 추출된 수직/수평 무한평면의 외곽선을 결정지어 수직/
수평 다각평면을 구성한다. 그리고 무한평면의 정보가 갱신 된 수직/수평 다각평면에 대해서도 추가된 3D 스캔 선분을 아우르도록 다각형을 갱신하므로써 수직/수평 다각평면 집합
k,
V Hk를 구성한다.
전체적인 알고리즘의 단계 및 입출력을 흐름도로 나타내 면 그림 4와 같이 나타낼 수 있다.
III. 스캔 데이터 매칭
스캔 데이터 매칭을 진행하기에 앞서, 단계 1에서 스캔 평 면상에서 양방향 선분 추출 방법을 이용하여 2D 스캔 선분 들 LS 를 추출한다. 이때 추출 되는 2D 스캔 선분 k L ,
s 의 k i
직선(line) 식은 다음과 같이 나타낼 수 있다. 선분에 포함되 는 p,
N 개의 2D 점 데이터들 k i LPk i, ={Lpk i j, , |Lpk i j, , =(Lxj, ),
Lyj j=1,...,Nk ip,}의 제곱오차(squared error)가 최소가 되는 직선(line: Lx⋅cosφk i, +Ly⋅sinφk i, =ηk i,)은 다음 식과 같이 구한다.
, , , , ,
2 2
, , , , ,
, , , , ,
1 atan2( 2 ( ),
2
( ) ( )),
cos sin ,
L xy p L L
k i k i k i k i k i
L yy L xx p L L
k i k i k i k i k i
L L
k i k i k i k i k i
S N x y
S S N y x
x y
φ
η φ φ
= − ⋅ − ⋅ ⋅
− − ⋅ −
= ⋅ + ⋅
(5)
여기서
그림 4. 지도제작 전체 알고리즘 흐름.
Fig. 4. Overal algorithm flow of map building.
3D Simultaneous Localization and Map Building (SLAM) using a 2D Laser Range Finder based on Vertical/Horizontal Planar Polygons
이 승 은, 김 병 국 1156
, , ,
, ,
2 2
, , ,
1 1 1
, ,
1 1
, ,
, , ,
1 , 1
p p p
k i k i k i
p p
k i k i
N N N
L xx L L yy L L xy L L
k i j k i j k i j j
j j j
N N
L L L L
k i p j k i p j
j j
k i k i
S x S y S x y
x x y y
N N
= = =
= =
= = = ⋅
= ⋅ = ⋅
∑ ∑ ∑
∑ ∑
그리고 추출한 2D 스캔 선분들을 엔코더 데이터로부터 추정 한 이동 로봇의 포즈 정보를 이용하여 전역좌표의 3D 스캔 선분들로 변환한다.
단계 2의 매칭에서의 기본 아이디어는 k-2 번째 시간 단계 에서 k-1 번째 시간 단계로 넘어갈 때 평면에서 추출된 스캔 선분의 형태가 어떻게 변하는지 보고 같은 변화를 적용하여 k번째 시간 단계에서 추출되는 스캔 선분을 예측하는 것이 다. 그리고 이 예측된 선분과 가장 근접한 스캔 선분을 해당 수직/수평 다각평면 또는 선분 그룹에 매칭시킨다.
이전 시간 단계 k-1에서 스캔 선분의 매칭이 성공했던 수 직/수평 다각평면들, 선분 그룹들 Vk−1'={vk−1,i|i=1,...,Nkv−1'},
' '
1 { 1, | 1,..., h1},
k k i k
H − = h− i= N− Γ ={ |G ll =1,...,Ng} 중에서, 현 재 시간 단계 k에서 추출된 3D 스캔 선분들 중 하나인 sk i,
와 쌍을 이룰 매칭의 대상 vk i,/h G 를 다음과 같은 과정k i, / l
을 통하여 찾는다.
입력: 3D 스캔 선분, 수직/수평 다각평면들, 선분 그룹들 단계 2.1 시작/최종점 인덱스(index) 검사: 3D 스캔 선분 과 인덱스 범위가 겹치는 매칭의 대상을 찾는다. 인덱스 범 위가 겹치는 매칭의 대상이 존재하지 않을 경우 단계 2.5로 이동한다.
단계 2.2 선분 각도 검사: 3D 스캔 선분과 3D 지도 선분 이 이루는 각도가 일정 값보다 작은 매칭의 대상을 찾는다.
각도가 일정 값보다 작은 매칭의 대상이 존재하지 않을 경우 단계 2.5로 이동한다.
단계 2.3 선분 최소 거리 검사: 3D 스캔 선분의 양 끝점에 서 3D 지도 선분으로의 최소 거리 오차를 갖는 매칭의 대상 을 찾는다. 최소 거리 오차가 일정 값보다 작지 않을 경우 단계 2.5로 이동한다.
단계 2.4 매칭성공: 최소 거리 오차를 갖는 매칭의 대상을 현재 3D 스캔 선분과 쌍을 이루는 매칭의 결과로 선택하고 현재 3D 스캔 선분에 대한 매칭의 과정을 종료.
단계 2.5 매칭실패: 매칭이 안된 3D 스캔 선분을 출력으 로 하고 현재 3D 스캔 선분에 대한 매칭의 과정을 종료.
출력: <3D 스캔 선분, 수직 다각평면> 쌍 / <3D 스캔 선분, 수평 다각평면> 쌍 / <3D 스캔 선분, 선분 그룹> 쌍 / 매칭이 되지 않은 3D 스캔 선분
단계 2.1에서는 인덱스 범위가 겹치는 매칭의 대상을 찾는 다. 레이저 거리계는 스캔 방향으로 레이저 광선을 쏘아 차 례로 거리를 측정하기 때문에 획득되는 점 데이터에 인덱스 를 붙일 수 있다. 그리고 이 점 데이터로부터 추출되는 선분 들은 인덱스 범위를 형성하게 된다. 인덱스 범위를 검사하는 이유는 수직/수평 다각평면에서 연속적으로 추출되는 선분들 의 경우 인덱스 범위가 상당히 많은 부분 겹치기 때문이다.
단계 2.2에서 3D 지도 선분을 언급하였는데 3D 지도 선분
은 “예측 선분” 개념이 적용된 수직/수평 다각평면 내부의 선분으로, 다음과 같은 과정에 의해서 구하게 된다. 현재 시 간 단계 k에서 매칭의 과정을 진행한다고 할 때, k-2 번째와 k-1 번째 시간 단계에서 각각 매칭된 선분 sk−2,i, sk−1,i의 양 끝점 (pbk−2,i,pke−2,i), (pkb−1,i,pek−1,i)을 평면에 정사영(orthogonal projection)한다. (만약 3D 지도 선분을 찾고자하는 매칭의 대 상이 선분 그룹이라면 정사영할 평면이 아직 존재하지 않기 때문에 정사영의 과정을 생략한다.) 그리고 k-2 번째 시간 단 계의 정사영 선분 양 끝점에서 k-1 번째 시간 단계의 정사영 선분 양 끝점을 향하는 변위 벡터 d d 1, 2
를 구한다. k-1 번째 시간 단계의 정사영 선분 양 끝점에 앞서 구한 변위 벡터 각 각을 더하여 k번째 시간 단계에서 추출될 스캔 선분을 예측 할 수 있다.
3D 스캔 선분의 벡터를 a 위와 같은 과정을 통해 구한 , 3D 지도 선분의 벡터를 b
라 할 때 단계 2.2에서 다음 식을 만족할 경우에만 다음 단계로 이동하게 된다.
atan2(a b a b × , ⋅ ≤) thresholdangle
(6) 단계 2.3의 선분 최소 거리 검사에서 sqrd_dist는 3D 스캔
선분의 양 끝점에서 3D 지도 선분으로의 수직거리 각각을 제곱하여 더한 값을 나타낸다. 단계 2.1과 단계 2.2를 통과하 여 단계 2.3에 도달한 매칭의 대상들을 M ={ |m ii =1,...,
m}
N 이라 하고 이들 각각의 sqrd_dist를 {sqrd dist i = _ i| 1, ...,Nm}이라 할 때 arg min(sqrd dist_ i)를 3D 스캔 선분과 쌍을 이룰 최종 매칭 후보로 삼는다. 그리고 마지막으로
_ i
sqrd dist가 일정 값보다 작은지 확인해 줌으로써 3D 스캔 선분이 큰 값의 sqrd dist_ i를 갖는 수직/수평 다각평면(또는 선분 그룹)과 매칭되는 것을 막는다. 이를 간단히 나타내면 다음과 같다.
min( _ ) _ ,
arg min( _ )
i sqrd dist
i
if sqrd dist threshold sqrd dist is matched
≤ (7)
앞서 설명한 매칭을 위한 일련의 과정을 k 번째 시간 단계 에 추출된 모든 3D 스캔 선분들에 대해서 진행하고 매칭되 는 수직/수평 다각평면 또는 선분 그룹을 찾는다.
매칭의 결과 <3D 스캔 선분, 수직 다각평면>, <3D 스캔 선 분, 수평 다각평면>, <3D 스캔 선분, 선분 그룹> 3가지 종류 그림 5. 변위벡터를 이용하여 구한 예측 선분.
Fig. 5. Expected line segment calculated using displacement vectors.
2 차원 레이저 거리계를 이용한 수직/수평 다각평면 기반의 위치인식 및 3 차원 지도제작 1157
의 쌍들을 얻게 된다. 이들 중 <3D 스캔 선분, 수직 다각평 면> 쌍들을 3 자유도의 포즈 (xrobot,yrobot,θrobot)를 갖는 이동 로봇의 위치 추정에 사용하게 된다.
단계 3의 이동 로봇의 위치 추정을 위해 본 연구에서는 Sohn [13]의 2D 위치 추정방법을 사용하였다. Sohn의 위치 추 정방법의 아이디어는 모든 <3D 스캔 선분, 수직 다각평면>
쌍들에 대하여 3D 스캔 선분의 양 끝점에서 매칭된 수직 다 각평면으로의 수직 거리 각각의 제곱의 합이 최소가 되는 이 동 로봇의 최적의 포즈를 구하는 것이다. 위치 추정의 과정 을 거처 이동 로봇의 정확한 포즈 정보를 획득한 이후에 3D 스캔 선분들이 정확한 전역 좌표를 갖도록 다시 한번 좌표 변환을 해 준다.
IV. 수직/수평 무한평면 추출
이 장에서는 여러 시간단계에 걸처 스캔 선분들이 매칭 되 어 선분 그룹을 이루었을 때 수직/수평 무한평면을 추출하는 단계 4/5의 방법에 대해서 다룬다. 앞 장에서 설명한 방법으 로 매칭의 과정을 진행하면 형태가 비슷하고 서로 근접한 선 분들의 그룹들 Γ 를 생성할 수 있다. 이 선분 그룹들 중 다 음 두 가지 조건을 만족하는 선분 그룹 Gl에 대해 단계 4/5 의 수직/수평 무한평면 추출의 과정을 진행한다.
1. _
2.
lgs line num
distFromStart
N threshold
distFromStart threshold
≥
≥ (8)
첫 번째 조건은 선분 그룹 Gl에 포함된 스캔 선분들의 개 수 N 가 일정 개수보다 많아야 한다는 것인데, 이는 적은 lgs
데이터로부터 무한평면이 추출되는 것을 막기 위함이다. 두 번째 조건을 확인하기 위하여 선분 그룹을 이루는 스캔 선분 들 중 가장 최근에 포함된 선분 s 의 양 끝점 k i, (p pk ib,, k ie,)에 서 가장 처음에 포함된 선분 sk j i j N− +1, = lgs 으로의 수직거리를 계산하고 각각을 제곱하여 더한다. 그리고 이 값 (distFromStart)이 일정 값보다 클 경우에만 수직/수평 무한평 면 추출의 과정을 거치게 된다. 이동 로봇이 정지해 있는 경 우에는 많은 수의 선분들이 그룹으로 모였다 하더라도 단순 히 같은 위치에서 같은 형태의 선분들이 누적된 꼴이므로 이 러한 경우를 피하기 위하여 distFromStart가 일정 값보다 클 경우에만 무한평면 추출의 과정을 진행한다.
선분 그룹이 위 두 조건을 만족하였다면 앞으로 설명하는 수직/수평 무한평면 추출의 과정을 진행한다. 선분 그룹을 형 성한 스캔 선분들의 양 끝점의 (전역 좌표) z 성분을 확인하 여 최대 z 값과 최소 z 값을 구하고 이 둘의 차의 절댓값을 이용하여 선분 그룹의 높이를 구한다. 그리고 높이 값이 일 정 값보다 클 경우에는 단계 4의 수직 무한평면 추출, 그렇 지 않을 경우 단계 5의 수평 무한평면 추출을 수행한다.
1. 단계 4: 수직 무한평면 추출
단계 4의 수직 무한평면 추출을 위해서는 선분 그룹에 포 함된 스캔 선분들을 구성하는 모든 점들로부터 제곱오차가 최소가 되는 수직 무한평면을 찾는다. 수직 무한평면은 전역 좌표계의 z+에서 x-y 평면을 향하여 내려다봤을 때 직선으로
나타낼 수 있으므로 제곱 오차가 최소가 되는 수직 무한평면 을 구하는 작업은 모든 점들을 x-y 평면으로 정사영한 뒤 이 들의 제곱오차가 최소가 되는 직선을 찾는 작업, 즉 최소 제 곱오차 직선 근사(least-squares line fitting)의 과정과 같게 된다.
재귀적 최소 제곱오차 직선 추정(recursive least-squares line estimation)방법[13]을 이용하여 선분 그룹 Gl에 포함된 선분
들을 구성하는 , 1,
1
( )
lgs N
gp p
k l k j i
j
N N − +
=
=∑ 개의 점 데이터 P = lg
, ,
{pl jg |pl jg =( , , ),x y zj j j j=1,...,Nk lgp,}로부터 제곱오차가 최소 가 되는 수직 무한평면을 찾기 위해서는 전역 좌표로 표현된 다음 식의 값들이 필요하게 된다.
, , ,
, ,
' 2 ' 2 '
, , ,
1 1 1
' '
, ,
1 1
, ,
, , ,
1 , 1
gp gp gp
k l k l k l
gp gp
k l k l
N N N
xx yy xy
k l j k l j k l j j
j j j
N N
k l gp j k l gp j
j j
k l k l
S x S y S x y
x x y y
N N
= = =
= =
= = = ⋅
= ⋅ = ⋅
∑ ∑ ∑
∑ ∑
(9)
식 (9)와 같은 값들을 앞으로 직선 매개변수들(line parameters) 이라 부르도록 하겠다. 직선 매개변수들을 구하기 위해 그룹 내 스캔 선분들을 구성하는 모든 점들에 대해서 일일이 식 (9)의 연산을 진행하는 것은 비효율적이다. 스캔 평면상에서 선분들을 추출할 때 이미 식 (5)와 같이 직선 매개변수들을 구했으므로 이들을 전역좌표로 변환하여 사용하면 된다. 스 캔 선분이 선분 그룹에 추가 될 때의 각 시간 단계에 스캔 평면 좌표에서 전역 좌표로의 변환행렬을 Ts g2 라 할 때 스 캔 선분 s 의 직선 매개변수들은 다음과 같이 변환된다. k i,
2 2 2
, 1,1 , 1,2 , , 1,4 1,1 1,2 ,
, 1,2 1,4 , , 1,1 1,4 ,
2 2 2
, 2,1 , 2,2 , , 2,4 2,1 2,2 ,
, 2,2 2,4
2
2 2 ,
2 2
xx L xx L yy p L xy
k i k i k i k i k i
p L p L
k i k i k i k i
yy L xx L yy p L xy
k i k i k i k i k i
p k i
S u S u S N u u u S
N u u y N u u x
S u S u S N u u u S
N u u
= ⋅ + ⋅ + ⋅ + ⋅ ⋅ ⋅
+ ⋅ ⋅ ⋅ ⋅ + ⋅ ⋅ ⋅ ⋅
= ⋅ + ⋅ + ⋅ + ⋅ ⋅ ⋅
+ ⋅ ⋅ ⋅ ⋅ , , 2,1 2,4 ,
, 1,1 2,1 , 1,1 2,2 1,2 2,1 ,
1,2 2,2 , 1,1 2,4 2,1 1,4 , ,
1,2 2,4 2,2 1,4 , , , 1,4 2,4
, 1,1
2 ,
( )
( )
( ) ,
L p L
k i k i k i
xy L xx L xy
k i k i k i
L yy p L
k i k i k i
p L p
k i k i k i
k i L k
y N u u x
S u u S u u u u S
u u S u u u u N x
u u u u N y N u u
x u x
+ ⋅ ⋅ ⋅ ⋅
= ⋅ ⋅ + ⋅ + ⋅ ⋅
+ ⋅ ⋅ + ⋅ + ⋅ ⋅ ⋅
+ ⋅ + ⋅ ⋅ ⋅ + ⋅ ⋅
= ⋅ , 1,2 , 1,4
, 2,1 , 2,2 , 2,4
, ,
i L k i
L L
k i k i k i
u y u
y u x u y u
+ ⋅ +
= ⋅ + ⋅ +
(10)
여기서 u 는 ,i j Ts g2 의 i 번째 행, j 번째 열 원소.
스캔 선분이 선분 그룹에 추가되는 매 시간 단계마다 식 (10)과 같은 변환과정을 거쳐서 전역 좌표계에서의 직선 매 개변수들을 구하며 이들을 누적하여 선분 그룹을 대표하는 직선 매개변수들을 구한다. k-1 번째 시간 단계까지 선분 그 룹 G 을 대표하는 누적된 직선 매개변수들 l (Skxx−1,l',Skyy−1,l',
' ' '
1, , 1,, 1,, 1,)
xy gp
k l k l k l k l
S− x− y− N − 과 k 번째 시간 단계에 새롭게 추가 된 스캔 선분 s 로부터 식 (10)의 과정으로 변환된 직선 매k i,
개변수들 (S S S x y Nk ixx,, k iyy,, k ixy,, k i,, k i,, k ip,)을 누적하여 k 번째 시 간 단계에 선분 그룹 Gl 을 대표하는 직선 매개변수들
' ' ' ' '
, , , , , ,
(Sk lxx,Sk lyy,Sk lxy,xk l,yk l,Nk lgp)을 구하는 방법은 다음과 같다.
이 승 은, 김 병 국 1158
' '
, 1, , , 1, ,
' ' ' '
, 1, , , 1, ,
1, 1,' , ,
,'
,
1, 1,' , ,
,'
,
, ,
, ,
,
gp gp p xx xx xx
k l k l k i k l k l k i
yy yy yy xy xy xy
k l k l k i k l k l k i
gp p
k l k l k i k i
k l gp
k l
gp p
k l k l k i k i
k l gp
k l
N N N S S S
S S S S S S
N x N x
x N
N y N y
y N
− −
− −
− −
− −
= + = +
= + = +
⋅ + ⋅
=
⋅ + ⋅
=
(11)
이렇게 선분 그룹 Gl에 포함된 스캔 선분들을 구성하는 모든 점들에 대해서 직선 매개변수들을 구하였으면 다음 식 을 이용하여 제곱 오차가 최소가 되는 수직 무한평면 정보
,,
k lg
θ g,
ρ 를 구한다. k l
' ' '
, , , , ,
' ' ' 2 ' 2
, , , , ,
' '
, , , , ,
1 atan2( 2 ( ),
2
( ) (( ) ( ) ))
cos sin
g xy gp
k l k l k l k l k l
yy xx gp
k l k l k l k l k l
g g g
k l k l k l k l k l
S N x y
S S N y x
x y
θ
ρ θ θ
= − ⋅ − ⋅ ⋅
− − ⋅ −
= ⋅ + ⋅
(12)
위와 같은 과정을 통하여 찾은 수직 무한평면이 실제 세계 에서의 수직 무한평면이 될만한 지 확인하기 위하여 선분 그 룹 G 내 포함된 스캔 선분들을 구성하는 모든 점들 l
, , ,
{ | ( , , ), 1,..., }
g g g gp
l l j l j j j j k l
P = p p = x y z j= N 이 수직 무한평면으 로부터 얼마의 표준 편차를 갖고 위치하는지 확인하여야 한 다. 만약 매우 작은 값의 표준 편차를 갖는다면 이제서야 비 로소 찾은 수직 무한평면을 실제 세계에서 존재하는 수직 무 한평면으로 판단할 수 있게 된다.
위와 같이 추출된 수직 무한평면은 스캔 선분이 새로 매칭 되어 추가될 때마다 갱신된다. 수직 무한평면에 추가되는 스 캔 선분의 직선 매개변수들은 식 (10)에 의해 전역 좌표로 변환되며 식 (11)에 의해 수직 무한평면을 대표하는 직선 매개 변수들과 합쳐진다. 그리고 이 합쳐진 직선 매개변수들을 이 용하여 식 (12)에 의해 수직 무한평면의 갱신이 이루어진다.
2. 단계 5: 수평 무한평면 추출
수평 무한평면의 경우도 수직 무한평면의 추출과 마찬가 지로 제곱오차가 최소가 되는 수평 무한평면을 찾는다. 수평 무한평면의 추출과정은 단순한데, 선분 그룹에 포함된 스캔 선분들을 구성하는 모든 점들의 (전역 좌표) z 값을 평균 내 면 되기 때문이다. 매 시간 단계마다 선분 그룹 Gl로 포함 되는 스캔 선분 s 에 대해서 z 값의 평균을 다음 식과 같이 k i,
구한다.
, 3,1 L , 3,2 L , 3,4
k i k i k i
z =u ⋅ x +u ⋅ y +u (13) 그리고 k-1 번째 시간 단계까지 선분 그룹 Gl을 대표하는
누적된 z의 평균값 g1,
k l
ρ− 와 k 번째 시간 단계에 새롭게 추가 된 스캔 선분 s 로부터 식 (13)의 과정으로 구한 z의 평균k i,
값 z 를 누적하여 k 번째 시간 단계에 선분 그룹 k i, Gl을 대 표하는 z의 평균값 g,
ρ 을 구하는 방법은 다음 식과 같다. k l
1, 1, , ,
, 1, , ,
,
, kgp l kg l k ip k i
gp gp p g
k l k l k i k l gp
k l
N N z
N N N
N
ρ − ρ −
−
⋅ + ⋅
= + = (14)
그리고 수직 무한평면과 마찬가지로 찾은 수평 무한평면 에 대해서 점들의 표준편차를 구하고 표준편차가 매우 작을 때에만 실제 세계에서의 수평 무한평면으로 판단하고 수평 무한평면 추출의 과정을 마치게 된다.
위와 같이 추출된 수평 무한평면은 스캔 선분이 새로 매칭 되어 추가될 때마다 갱신된다. 수평 무한평면에 추가되는 스 캔 선분의 (전역 좌표) z 값 평균은 식 (13)에 의해 구해지며, 식 (14)에 의해 수평 무한평면을 대표하는 z의 평균값과 합 쳐짐으로써 수평 무한평면의 갱신이 이루어진다.
V. 수직/수평 다각평면 추출
앞서 추출하고 갱신을 진행했던 수직/수평 무한평면은 무 한히 뻗어 나가는 무한 평면이다. 이번 장에서는 추출된 수 직/수평 무한평면을 유한평면화 하기 위하여 평면에 포함된 점들을 아우르는 다각형을 구성하는 방법에 대해 다룬다. 다 각형을 구성하는 방법의 아이디어는 새로운 스캔 선분이 수 직/수평 다각평면에 추가될 때 마다 선분의 양 끝점들을 가 지고 재귀적 최소 제곱오차 직선 추정을 진행하여 다각형의 외곽선을 구성하는 것이다.
1. 단계 6: 수직 다각평면 추출
재귀적 최소 제곱오차 직선 추정을 진행하기 위해서는 2D 좌표가 필요하다. 하지만 현재 데이터들이 3D 전역 좌표로 변환된 상태이므로 재귀적 최소 제곱오차 직선 추정을 진행 하기에 앞서 3D 좌표의 데이터들을 수직 무한평면상의 2D 좌표로 변환하는 과정을 먼저 거처야 한다.
수직 무한평면상에 2D 좌표계를 구성하기 위해, 어떤 점을 원점으로 삼고 어떠한 방향들을 좌표계 두 축의 방향으로 정 할지에 대한 문제가 생기게 된다. 본 연구에서는 수직 다각 평면에 가장 처음 포함되었던 스캔 선분의 인덱스값이 작은 쪽의 끝점(앞으로 인덱스값이 작은 쪽의 끝점을 시작점, 인 덱스 값이 큰 쪽의 끝점을 최종점이라 부르도록 하겠다)을 전역 좌표계 x-y 평면상에 정사영하고 이 점을 수직 무한평 면 좌표계의 원점으로 정하였다. 그리고 전역 좌표계의 z축 방향을 수직 무한평면 좌표계의 z 축 방향으로 삼았고 수직 무한평면을 나타내는 직선인 x⋅cosθv+ ⋅y sinθv=ρv 의 방 향을 수직 무한평면 좌표계의 y축 방향으로 삼았다. 즉, 수직 무한평면 좌표계는 y축과 z축으로 이루어진 2D 좌표계로 다 음 그림과 같이 나타낼수 있다.
그림 6. 수직 무한평면 좌표계.
Fig. 6. Vertical infinite plane coordinate system.
Seungeun Lee and Byung-Kook Kim
