KR20080024407A

KR20080024407A - 이동 로봇의 자세 추정 장치 및 방법

Info

Publication number: KR20080024407A
Application number: KR1020060088726A
Authority: KR
Inventors: 홍빙롱; 루오롱화; 리마오하이; 방석원; 김연호; 명현
Original assignee: 삼성전자주식회사; 하얼빈 인스티튜트 오브 테크놀로지
Priority date: 2006-09-13
Filing date: 2006-09-13
Publication date: 2008-03-18
Also published as: EP1901152B1; EP1901152A2; US8543241B2; JP2008071352A; KR100855469B1; JP5001101B2; US20080065267A1; EP1901152A3

Abstract

본 발명은 이동 로봇에 적용되는 SLAM 알고리즘의 연산 복잡성을 감소시키는 방법 및 장치에 관한 것이다.

복수의 파티클을 이용하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치는, 이동 로봇의 자세 변화량을 감지하는 센서와, 이전 파티클들에 상기 감지된 자세 변화량을 적용하여 현재 파티클들의 자세 및 가중치를 구하는 파티클 필터부와, 상기 구한 자세 및 가중치에 진화 알고리즘을 적용하여 상기 현재 파티클들의 자세 및 가중치를 갱신하는 진화 적용부를 포함한다.

이동 로봇, 파티클 필터, 진화 알고리즘, SLAM

Description

이동 로봇의 자세 추정 장치 및 방법{Pose estimation apparatus used in a mobile robot and method thereof}

도 1은 본 발명의 일 실시예에 따른 이동 로봇 및 자세 추정부의 구성을 도시하는 블록도.

도 2는 자세 추정부(200)의 세부적인 구성을 도시하는 블록도.

도 3은 특징점들의 그레이 그래디언트 벡터를 계산하는 과정을 보여주는 도면.

도 4는 시작점 S로부터 최종점 E까지 각각의 중간 단계에서 이동 로봇의 자세에 대한 확률 분포를 보여주는 도면.

도 5는 관측치와 복수의 예측치 간의 거리를 나타내는 도면.

도 6은 교배 알고리즘의 개념을 보여주는 도면.

도 7은 돌연변이 알고리즘의 개념을 보여주는 도면.

도 8a는 종래의 파티클 필터 기법에 따른 로컬라이제이션 및 맵 생성 결과를 보여주는 도면.

도 8a는 본 발명에 따른 로컬라이제이션 및 맵 생성 결과를 보여주는 도면.

(도면의 주요부분에 대한 부호 설명)

100 : 이동 로봇 110 : 로봇 제어부

120 : 주행부 130 : 엔코더

140 : 카메라 200 : 자세 추정부

210 : 파티클 필터부 211 : 샘플링부

212 : 자세 예측부 213 : 가중치 계산부

220 : 특징점 처리부 221 : 특징점 추출부

222 : 특징점 변환부 223 : 특징적 매칭부

230 : 진화 적용부 231 : 초기화부

232 : 교배 적용부 233 : 돌연변이 적용부

240 : 메모리

본 발명은 이동 로봇에 관한 것으로, 보다 상세하게는 이동 로봇에 적용되는 SLAM(Simultaneous Localization And Mapping) 알고리즘의 연산 복잡성(computational complexity)을 감소시키는 방법 및 장치에 관한 것이다.

일반적으로 이동 로봇(mobile robot)은 산업용으로 개발되어 공장자동화의 일환으로 사용되거나, 인간이 견딜 수 없는 극한의 환경에서 인간을 대신하여 작업을 수행하는 데 사용되어 왔다. 이러한 이동 로봇 공학분야는 근래에 들어 최첨단의 우주개발산업에 사용되면서 발전을 거듭하여 최근에 들어서는 인간친화적인 가정용 이동 로봇의 개발에까지 이르렀다. 덧붙여, 이동 로봇은 의료용 기기를 대체 하여 인간 생체 내에 투입됨으로써, 기존의 의료용 기기로는 치료가 불가능했던 미세한 인간 생체조직의 치료에까지 이용된다. 이와 같은 눈부신 이동 로봇공학의 발전은 인터넷에 의한 정보혁명과 이를 뒤이어 유행하고 있는 생명공학분야를 대체하여 새롭게 등장할 최첨단 분야로서 각광받고 있다. 특히, 최근에는 청소용 이동 로봇과 같은 가정용 이동 로봇과 같이 인간의 생활 환경 주변에서 쉽게 접할 수 있는 형태의 이동 로봇이 대중화되고 있다.

이동 로봇의 이동에 있어서 필수적이라 할 수 있는 로컬라이제이션 기술은 이동 로봇이 주변 환경을 기준으로 한 자신의 위치를 결정하는 과정이다. 예를 들어, 종래의 로컬라이제이션 시스템에서, 이동 로봇은 그 주변 환경의 맵을 제공받는다. 이러한 종래의 로컬라이제이션 시스템은, 수동으로 통하여 맵을 생성하는 것이 상대적으로 어렵고 전문적인 작업을 요한다는 측면에서 불리한 점이 있다. 더욱이, 일반적으로 다양한 환경은 고정적이지 않다. 예를 들어, 방에서 가구를 이동시키는 것은 기존의 맵을 쓸모 없게 만든다. 결국, 종래의 로컬라이제이션 시스템에서, 정확한 주변 환경을 나타내기 위하여, 맵은 상대적으로 자주 그리고, 비싼 비용으로 갱신되어야 한다.

따라서, 이동 로봇이 주변의 공간적 정보가 전혀 없는 지역에서 자신의 상대적 위치 이동 값과 주변환경의 특징점들을 이용하여, 자신의 공간상 절대 위치를 예측하여 이동 경로를 계획하고 동적으로 맵을 작성하는 기술이 요구된다.

이와 같이, 이동 로봇이 상대 위치 값과 주변 환경을 이용하여 자신의 공간상 절대 위치를 예측하기 위하여 사용되는 가장 대표적인 방법으로 SLAM(Simultaneous Localization And Mapping) 알고리즘이 알려져 있다. 이에 따르면, 이동 로봇은 주변의 공간적 정보, 즉 맵 정보를 사전에 알고 있지 않더라도, 주변 정보를 초음파, 레이저 등을 이용하여 얻은 거리정보나 카메라를 이용하여 얻은 영상정보를 활용하여 특징점을 추출하고 맵 정보를 구성한다. 그리고, 이동 로봇은 상기 맵 정보를 이용하여 자신의 위치를 동시에 알아낸다.

로봇의 현재 위치에서 특징점을 이용하여 자신의 위치를 예측하기 위하여 일반적으로 칼만 필터(Kalman Filter)가 많이 사용된다. 그러나, 칼만 필터를 이용하는 방법은 예상 위치 오차의 분포가 가우시안 분포를 따르는 경우에만 유효하다는 단점이 있다. 이를 극복하기 위하여 오차의 분포가 가우시안 분포를 따르지 않는 경우 위치 예측치의 샘플을 추출하고 각 샘플의 값이 참일 확률을 이용하여 최적의 위치를 예측하는 방법, 즉 파티클 필터 기법도 알려져 있다.

파티클 필터 기법과 관련한 종래 기술로 Evolution Robotics사의 미국 특허 7015831호(이하 '831 특허라고 함)가 있다. 상기 '831 특허는 영상정보를 이용한 SLAM기술에 의한 이동체의 위치를 연속적으로 계산하는 시스템 및 방법에 관한 것이다. 특히, 상기 '831 특허는 파티클 필터를 이용한 SLAM 방식으로서, 두 개의 파티클 세트를 이용하여 복수의 센서(odometry와 카메라)를 접합(fusion)하는 기술을 개시한다.

　그러나, 일반적으로 파티클 필터를 이용한 SLAM 기법은 일반적으로 많은 샘플들이 요구되기 때문에 연산량이 증가되어, 이동 로봇이 이동 중에 실시간으로 자신의 위치를 예측하는 것이 어렵게 된다. 따라서, 파티클 필터를 이용한 SLAM 알고 리즘에서, 실시간으로 로봇 자신의 위치를 예측할 수 있도록 연산량을 감소시킬 수 있는 기술이 요구된다.

본 발명이 이루고자 하는 기술적 과제는, 파티클 필터에 기반한 SLAM 기법을 사용하는 이동 로봇에 있어서 연산 복잡성을 낮춤으로써 실시간 로컬라이제이션 및 맵 작성이 가능하도록 하는 방법 및 장치를 제공하는 것이다.

본 발명의 기술적 과제들은 이상에서 언급한 기술적 과제로 제한되지 않으며, 언급되지 않은 또 다른 기술적 과제들은 아래의 기재로부터 당업자에게 명확하게 이해될 수 있을 것이다.

상기한 기술적 과제를 달성하기 위하여, 복수의 파티클을 이용하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치로서, 이동 로봇의 자세 변화량을 감지하는 센서; 이전 파티클들에 상기 감지된 자세 변화량을 적용하여 현재 파티클들의 자세 및 가중치를 구하는 파티클 필터부; 및 상기 구한 자세 및 가중치에 진화 알고리즘을 적용하여 상기 현재 파티클들의 자세 및 가중치를 갱신하는 진화 적용부를 포함한다.

상기한 기술적 과제를 달성하기 위하여, 복수의 파티클을 이용하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법으로서, 이동 로봇의 자세 변화량을 감지하는 단계; 이전 파티클들에 상기 감지된 자세 변화량을 적용하여 현재 파티클들의 자세 및 가중치를 구하는 단계; 및 상기 구한 자세 및 가중치에 진화 알고리즘을 적용하여 상기 현재 파티클들의 자세 및 가중치를 갱신하는 단계를 포함한다.

이하, 첨부된 도면들을 참조하여 본 발명의 일 실시예를 상세히 설명한다.

본 발명은, 기존의 칼만 필터 기반의 SLAM 기법에서 발생하는 여러 문제들, 예를 들어, 비 가우시안 분포에 대한 대처를 못하는 문제, 다중 가정(Multiple hypothesis)으로 인한 문제 등을 해결하기 위해 기본적으로는 파티클 필터 알고리즘을 채택한다. 상기 다중 가정으로 인한 문제란 여러 개의 특징점 매칭 가정이 발생할 때 한번 잘못된 가정을 채용하면 다시는 이 문제를 해결할 수 없게 되는 것을 의미한다.

또한, 본 발명은 상기 파티클 필터에서 적은 수의 파티클을 이용하여 예측된 특징점의 위치와 관측된 특징점과의 매칭을 통하여 이동 로봇의 최적의 자세(pose)를 추정하는데, 상기 최적 추정을 위하여 유전자 알고리즘(Genetic Algorithms)과 같은 진화 알고리즘(Evolutionary Algorithm)이 사용된다. 상기 추정된 최적의 위치 및 특징점 매칭을 통하여 특징점의 최근 최적의 위치가 계산된다.

상술한 바와 같이, 본 발명의 목적은 기존의 파티클 필터 기법에서 보다 적은 수의 파티클을 이용해서 연산 복잡성을 낮추면서도 기존과 동일하거나 우수한 성능을 내는 것이다. 여기서, 우수한 성능이란 위치 오차의 감소를 의미한다. 파티클 필터에 상기 유전자 알고리즘을 추가적으로 적용함에 따라서 연산 복잡성이 다소 증가하게 되지만, 파티클의 수가 감소됨에 따른 연산 복잡성의 감소의 크기가 보다 크다면, 전체적인 연산 복잡성은 낮아질 수 있는 것이다.

도 1은 본 발명의 일 실시예에 따른 이동 로봇(100) 및 자세 추정부(200)의 구성을 도시하는 블록도이다. 이동 로봇(100)은 로봇 제어부(110), 주행부(120), 엔코더(130), 카메라(140) 및 자세 추정부(200)를 포함한다.

주행부(120)는 이동 로봇(100)이 이동할 수 있도록 동력을 제공한다. 주행부(140)는 일반적으로 복수의 바퀴와, 방향 제어 장치 등을 포함하지만, 이동 로봇(100)의 이동이 가능한 한 다른 공지의 주행 수단으로 이루어져도 무방하다.

엔코더(130)는 주행부(120)에 포함되는 주행 바퀴의 회전 속도를 감지한다. 이를 통하여, 이동 로봇(100)의 이전 위치와 현재 위치 사이의 위치 변화 및 방향 변화를 측정한다. 엔코더(130)는 일반적으로 로봇의 위치 이동 또는 방향 변화 지령을 내리고 로봇이 이에 추종하여 움직일 때 그 움직임을 제어할 목적으로 설치된다. 엔코더(130)를 이용하면 움직인 거리 및 방향을 적분하여 현재 이동 로봇의 위치를 알 수 있다. 만일 적분 오차가 없다면 엔코더(130) 만으로도 로봇의 로컬라이제이션이 가능하겠지만, 엔코더는 짧은 기간에는 비교적 정확하나 샘플링시마다 오차가 누적되는 단점이 있다. 이동 로봇(100)의 움직임을 감지(sensing)하는 센서로는, 상기 엔코더(130) 대신에 자이로(Gyroscope) 등의 관성 센서가 사용되어도 무방하다.

카메라(140)는 외부의 이미지를 포착(capture)하여 이를 디지털 신호로 변환하여 주는 장치이다. 일반적으로 카메라(140)는 CCD(Charge Coupled Device) 모듈이나 CMOS (complementary metal oxide semiconductor) 모듈을 포함하여 구성된다.

로봇 제어부(110)는 이동 로봇(100)의 주행부(120), 카메라(140) 등을 기능을 제어한다. 즉, 로봇 제어부(110)는 이동 로봇(100)이 사용자가 명령한 위치로 이동하도록 제어하거나, 카메라(140)의 영상 포착 동작을 제어할 수 있다.

자세 추정부(200)는 파티클 필터에 진화 알고리즘을 적용하여, 이동 로봇(100)의 위치 추정을 수행하는 장치이다. 아울러, 자세 추정부(200)는 상기 위치 추정과 더불어 맵 작성 동작을 수행할 수도 있다.

자세 추정부(200)는 파티클 필터부(210), 특징점 처리부(220), 진화 적용부(230) 및 메모리(240)를 포함하여 구성될 수 있다.

특징점 처리부(220)는 카메라(140)에 포착된 영상을 처리하여 특징점을 추출하고 추출된 특징점에 근거하여 메모리(240)에 기 저장된 특징점들을 갱신(변경 또는 추가)한다.

파티클 필터부(210)는 파티클 필터를 적용한다. 즉, 복수의 파티클을 선택하고 이동 로봇(100)의 각 이동 단계 별로 상기 선택된 파티클의 가중치를 변화시키고, 상기 변화된 가중치를 기반으로 상기 선택된 파티클을 리샘플링한다.

진화 적용부(230)는 상기 선택된 각각의 파티클에 대하여 진화 알고리즘을 적용하여 진화된 파티클 및 상기 진화된 파티클에 대한 가중치(진화된 가중치)를 구한다.

메모리(240)는 현재까지 포착된 다수의 특징점을 저장하고 있으며, 이러한 특징점들을 하나의 완성된 맵을 구성할 수 있다.

도 2는 자세 추정부(200)의 세부적인 구성을 도시하는 블록도이다.

먼저, 특징점 처리부(220)는 특징점 추출부(221), 특징점 변환부(222) 및 특징점 매칭부(223)를 포함하여 구성될 수 있다.

특징점 추출부(221)는 카메라(140)에 의하여 포착된 이미지로부터 스케일-불변(scale-invariant)하고 회전-불변(rotation-invariant)한 특징점들을 추출한다. SLAM의 과정에서, 이러한 특징점들은 맵 생성에 기초가 되는데, 소위 랜드마크라고도 불린다. 로봇이 이동할 때, 특징점들은 커지기도 하고 작아지기도 하며, 기존의 특징점들에 대하여 회전하기도 한다. 이러한 종류의 변화가 발생한 후에 특징점들을 추출하기 위해서는, 우리는 스케일 및 회전에 불변한 특징점들을 골라서 랜드마크로 삼아야 한다. 특징점들을 추출하는 과정은 다음의 (a) 내지 (e) 단계의 세부 단계로 이루어질 수 있다.

(a) 원 이미지를 얻는 단계

원 이미지는 CCD 카메라로부터 샘플링된 영상으로부터 디지털화된 영상을 의미한다.

(b) 가우시안 블러링된(Gaussian blurred) 이미지를 계산하는 단계

N회의 가우시안 블러를 원 이미지에 적용하여 N 가우시안 블러링된 이미지를 얻는다. 이러한 블러 과정을 통하여, 서로 다른 스케일의 이미지를 얻을 수 있다. 더욱 블러링된다는 것은 관측이 멀어진다는 것을 의미한다. 이것은 먼 관찰 거리는 더욱 블러링된 장면과 대응된다는 지각적 특성에 근거한 것이다.

(c) 가우시안 이미지의 차이를 계산하는 단계

인접한 가우시안 블러링된 이미지 간의 차이(DoG)를 계산한다. 두 개의 블러링된 이미지 I₁및 I₂가 있다고 가정하면, 그들의 거리 D₁은 다음의 수학식 1에 따라 구해질 수 있다. 여기서, x, y는 이미지의 x 좌표 성분 및 y 좌표 성분을 각각 의미한다.

따라서, N개의 가우시안 블러링된 이미지로부터는 N-1개의 DoG가 계산될 수 있다.

(d) DoG 이미지의 극점(extrema)들을 감지하는 단계

상기 극점이란 그 그레이 값이 주변의 모든 그레이 값보다 크거나 작은 픽셀을 의미한다. 상기 픽셀은 블러링된 후 선명하게 변화하는 픽셀이다. 이러한 픽셀은 스케일 또는 회전 변화 후에도 매우 고정적(stabile)이다.

(e) 특징점들을 얻기 위하여 극점들을 선택하는 단계

DoG에 있는 극점은 후보 키 포인트라고도 불린다. 이 단계는 특징점들의 안정성을 향상시키기 위해서는 에지를 따라서 잘못 선택된 점들은 제거되어야 하기 때문에 필요하다.

특징점 변환부(222)는 현재 추정된 위치로부터 추출된 특징점들에 대해 정확한 위치와 방향 및 밝기의 변화율의 방향을 구한다. 이를 위하여 특징점 변환부(222)는 상기 키 포인트 주변에 로컬 이미지를 샘플링하고, 모든 특징점들의 위치 및 방향을 계산하고, 그레이 그래디언트(gray gradient)의 관점에서 특징점을 기술하는 벡터를 계산한다. 특징점 변환부(222)는 세부적으로 다음의 2 가지 단 계((a) 및 (b))을 수행한다.

(a) 특징점들의 정확한 위치 및 방향을 계산하는 단계

특징점 변환부(222)는 키 포인트의 현재 위치를 픽셀 단위로 표현된다. 정밀도 향상을 위하여, 상기 픽셀 좌표들은 보간될 필요가 있다. 특징점 변환부(222)는 키 포인트 주변의 64x64 픽셀 이미지 영역의 그레이 그래디언트에 따라서 각각의 키 포인트에 고정된 방향을 부여함으로써 회전-불변성을 얻는다.

(b) 특징점들의 그레이 그래디언트 벡터를 계산하는 단계

도 3를 참조하면, 특징점 변환부(222)는 각각의 특징점들에 대하여 4개의 부 영역을 부여한다. 각각의 부 영역은 8개의 그레이 그래디언트 벡터에 대응된다. 결국, 도 3에 도시된 바와 같이, 각각의 특징점에 대하여 32개의 그레이 그래디언트 벡터가 존재한다. 도 3에서 각각의 화살표는 그 방향으로의 그레이 그래디언트 벡터의 크기를 나타낸다. 즉, 화살표가 더 길수록 그레이 그래디언트도 더 커진다는 것을 의미한다.

특징점 매칭부(223)은 현재 순간에서 감지된 특징점(현재 특징점)의 그레이 그래디언트의 벡터를 맵 메모리(240)에 저장된 특징점들의 그레이 그래디언트 벡터들과 비교하고, 서로 유사한 특징점들을 찾는다. 두 특징점들의 벡터의 차이가 문턱값보다 작으면 상기 두 개의 특징점들은 서로 매치되는 것으로 간주된다. 현재 특징점과 매칭되는 특징점들이 메모리(240)에 복수 개가 존재한다면, 현재 특징점과 가장 가까운 특징점이 매치되는 것으로 선택된다. 메모리(240) 내에 매치되는 특징점이 없으면 현재 특징점은 새로운 특징점으로서 메모리(240)에 저장된다.

다시 도 2를 참조하면, 파티클 필터부(210)는 샘플링부(211), 자세 예측부(212) 및 가중치 계산부(213)를 포함하여 구성될 수 있다. 파티클 필터부(210)는 특징점 처리부(220)로부터 제공된 특징점들의 특성을 이용하여 추정 가능한 로봇의 위치 및 방향, 즉 자세의 표본값들의 가중치를 계산하고 이들로부터 최적의 자세를 추정한다. 상기 가중치는 참일 확률을 의미한다.

자세 예측부(212)는 이동 로봇(100)의 이동에 따라 이동하는 이동 로봇(100)의 위치를 확률 분포로서 나타낸다. 상기 확률 분포는 조건부 확률 p(x_t| x_t-1, u_t-1) 로 나타낼 수 있다. 여기서, x_t-1는 시간 t-1에서의 자세이고, u_t-1는 엔코더에서 측정된 데이터이며, x_t는 시간 t(현재)에서의 자세이다. 상기 자세는, 2차원 직교 좌표계에서의 두 좌표 값 및 이동 로봇(100)의 향하는 방향각을 포함하여 3개의 성분으로 이루어진다. 상기 확률 분포는 로봇이 동적 잡음이나 시스템에 존재하는 다른 불확실한 인자를 가진 채 x_t-1에서 x_t까지 움직였을 때, 시간 t에서의 로봇 자세에 대한 확률 분포를 의미한다.

도 4는 시작점 S로부터 최종점 E까지 각각의 중간 단계에서 이동 로봇(100)의 자세에 대한 확률 분포를 보여준다. 역기서, 시작점으로부터 멀어질수록 파티클이 보다 많이 분산되는 것을 알 수 있다. 이것은 인코더의 에러가 로봇의 시작점으로부터 경로의 거리에 따라 커지기 때문이다.

파티클 필터에서의 파티클 수를 N으로 가정하면 모든 파티클은 (x_t ⁽ⁱ⁾, w_t ⁽ⁱ⁾) 로서 표현된다. 여기서, i는 1 내지 N까지의 정수 인덱스 이고, x_t ⁽ⁱ⁾는 시간 t에서의 적절한(feasible) 자세이며, w_t ⁽ⁱ⁾는 자세가 x_t ⁽ⁱ⁾가 될 확률 또는 가중치(weight)이다. 본 자세 예측부(212)는 매 파티클마다 확률 모델 p(x_t| x_t-1, u_t-1) 에 따라서 x_t ⁽ⁱ⁾를 계산한다.

샘플링부(211)는 최초 N개의 파티클을 랜덤 샘플링한다. 그리고, 시간 카운트 t가 1 증가하였을 때, 상기 N개의 파티클 중 일부의 파티클(가중치가 낮은 파티클)을 탈락시키고, 일부의 파티클(가중치가 높은 파티클)을 중복 샘플링하여 다시 N개의 파티클을 유지한다. 이를 리샘플링이라고 한다. 즉, 샘플링부(211)는 최초 1회의 샘플링 과정과 그 이후 반복되는 복수의 리샘플링 과정을 수행한다.

최초 랜덤 샘플링은 이동 로봇(100)의 최초 위치 근처에 N개의 임의의 지점 즉 N개의 파티클을 선택하는 과정이다. 상기 과정은 다음의 수학식 2와 같이 표시될 수 있다.

상기 x₀ ^(j) 및 w₀ ^(j)는 특정 인덱스 j의 파티클에 대하여, 이동 로봇(100)의 최초 위치 및 최초 가중치를 각각 나타낸다. 상기 w₀ ^(j)는 인덱스 j와 상관없이 모두 1/N으로 동일하게 설정될 수 있다.

한편, 샘플링부(211)는 시간 카운트가 증가하였을 때, 기존의 파티클 세트 로부터 N개의 무작위 샘플을 리샘플한다. 그럼으로써 새로운 샘플 세트를 가중치 w_t-1 ^(j)에 근거하여 구한다.

가중치 계산부(213)은 시간 카운트 t가 증가할 때마다, 각각의 파티클에 대한 가중치 w_t ^(j) 를 계산하고 상기 계산된 값으로 해당 가중치를 갱신한다.

시간 t에서의 파티클 정보 (x_t ^(j), w_t ^(j)) 중에서 가중치 w_t ^(j) 는 로봇의 포즈가 x_t ^(j)인 확률을 나타낸다.

이동 로봇(100)이 새로운 관찰 정보를 얻을 때, 가중치 계산부(213)는 상기 새로운 정보에 의거하여 파티클의 가중치를 다시 계산한다. 여기서, 가중치의 계산이란 확률을 수정하고 로봇을 다시 로컬라이제이션하는 것을 의미한다. 엔코더(130)는 누적 오차가 발생하기 때문에, 특징점 매칭부(223)에서의 특징점 매칭 후에 파티클들의 가중치를 재계산하는 것은 매우 중요하다.

새로이 갱신되는 가중치 w_t ^(j)는 다음의 수학식 3에 따라 계산될 수 있다.

여기서, y_t는 시간 t에서 실제 관측된 이동 로봇(100)의 위치를 의미한다. 수학식 3에 따르면, 결국 w_t ^(j)는 각각의 파티클의 예측된 위치 x_t ^(j)가 실제 관측된 값과 일치할 확률의 의미하는 것이다. 상기 관측된 값은 특징점 매칭부(223)에서의 매칭된 특징점을 기준으로 하여 계산될 수 있는 값이다.

예를 들어, w_t ^(j)는 도 5와 같이, N개의 파티클의 예측치가 있다고 할 때, 각각의 예측치와 관측치 간의 거리(d₁, d₂, ... , d_N)의 역수에 비례하는 값으로 계산될 수 있다. 즉, 관측치와의 거리가 가까울수록 높은 가중치가 부여되는 것이다.

다시 도 2를 참조하면, 진화 적용부(230)는 진화 레벨 설정부(231), 교배 적용부(232) 및 돌연변이 적용부(233)를 포함한다. 진화 적용부(230)는 진화 알고리즘을 사용하여 상태 파라미터 x_t ^(j) 및 w_t ^(j)를 최적화한다. 상기 x_t ^(j)는 자세 예측부(212)에서 계산되는 현재 각 파티클들의 예측 위치를 의미하고, w_t ^(j)는 가중치 계산부(213)에서 계산되어 갱신된 가중치를 의미한다.

상기 예측 및 가중치 계산 후에는, N개의 가능한 포즈 및 그들의 확률을 얻을 수 있지만, 실제 로봇의 포즈는 N 개의 가능한 포즈 중 하나에 해당되지 않는 경우가 많다. 더욱 정확한 포즈를 얻기 위해서는, 파티클 필터를 위한 많은 파티클들이 필요하지만, 파티클 수의 증가는 연산량 및 요구 메모리의 양을 급격히 증대시킨다. 따라서 진화 적용부(230)는 로봇의 포즈가 실제 포즈에 근접하도록 최적화하는데 사용된다. 이러한 최적화의 결과로서, 로봇 로컬라이제이션을 위하여 훨씬 적은 파티클들이 소요되도록 할 수 있다.

진화 적용부(230)에 적용되는 진화 알고리즘으로서는 교배(crossover), 돌연변이(mutation) 기타 알려진 진화 다양한 알고리즘 또는 이들의 조합이 적용될 수 있다. 다만, 본 발명에서는 그 중에서 효과가 좋을 것으로 예측되는 교배 및/또는 돌연변이 알고리즘을 예로 들어서 설명할 것이다.

초기화부(231)는 진화 알고리즘에 적용될 진화 레벨(G) 및 진화 샘플 수(M)를 결정한다. 상기 진화 레벨 및 진화 샘플 수는 사용자로부터 입력 받은 값일 수도 있고, 자세 추정부(200)에 미리 설정된 디폴트 값일수도 있다. 상기 진화 레벨을 많이 진행할수록 정확도가 향상될 수 있지만, 연산량이 다소 증가될 수 있기 때문에, 상기 진화 레벨로서 적절한 값이 선택될 필요가 있다. 자세 추정부(200)의 컴퓨팅 파워에 따라서 달라질 수 있으나, 진화 레벨은 대략 3 내지 5 정도로 결정하는 것이 적당하다.

상기 진화 샘플 수(M)도 많을수록 정확도가 향상될 수 있지만 연산량이 증가되므로 적절한 수(예를 들어 10개 내외)가 선택되어야 한다. 상기 진화 샘플 수는 각 파티클 별로 선택될 수도 있고, 모든 파티클에 대하여 같은 수를 사용할 수도 있다.

교배 적용부(232)는 하나의 파티클에 대하여 선택된 M개의 진화 샘플을 중에서, 무작위로 선택된 두 개의 샘플 x_t ^(A), x_t ^(B)를 추출하여 교배 알고리즘을 적용한다. 교배 알고리즘은 다음의 수학식 4와 같이 표현될 수 있다. 즉, 자식 샘플 x_t ^(Ac), x_t ^(Bc)는 부모 샘플 x_t ^(A), x_t ^(B)의 가중합으로 표현된다. 여기서, ξ는 0과 1사이의 랜덤 유니폼(random uniform) 분포에서 선택되는 임의의 실수이다. 그리고, w_t ^(Ac) 및 w_t ^(Bc)는 상기 자식 샘플에 대한 가중치이다. 상기 자식 샘플에 대한 가중치는 해당 샘플의 값이 관측 값과 일치할 확률을 의미한다.

교배 적용부(232)는, 도 6에 도시된 바와 같이, 2개의 부모 샘플 및 2개의 자식 샘플, 즉 중에서 가장 큰 가중치를 가지는 두 개의 샘플을 선택하여, ,를 상기 선택된 값으로 갱신한다. 이와 같은 교배 과정은 상기 M개의 진화 샘플 중 임의의 2개에 대하여 적용될 수 있다.

돌연변이 적용부(233)는 상기 교배 적용부(232)에 의하여 교배 알고리즘이 적용된 M개의 진화 샘플 중에서 하나의 샘플 x_t ^(A)을 추출하여 돌연변이 알고리즘을 적용한다. 단, 상기 샘플 x_t ^(A)의 가중치 w_t ^(A)가 소정의 임계치 이상인 경우에 한하여 돌연변이 알고리즘을 적용한다. 가중치가 낮은 샘플에 대하여 돌연변이를 적용하면 오차가 더 커질 수도 있기 때문이다.

돌연변이 알고리즘은 다음의 수학식 5와 같이 표현될 수 있다. 여기서 τ는 랜덤 노이즈(random noise)로서, 가우시안 분포 N(0,σ²)를 따르는 3차원 벡터이다. 상기 σ는 상기 가우시안 분포의 표준편차이다. 상기 x_t ^(A)는 부모 샘플이고, x_t ^(Am)는 이로부터 돌연변이된 자식 샘플이다. 그리고, w_t ^(Am)는 상기 자식 샘플에 대한 가중치이다.

돌연변이 적용부(233)는, 도 7에 도시된 바와 같이, 1개의 부모 샘플 및 1개의 자식 샘플, 즉 x_t ^(A) 및 x_t ^(Am)는 중에서 큰 가중치를 가지는 샘플을 선택하여, x_t ^(A)를 상기 선택된 값으로 갱신한다.

M개의 진화 샘플에 대하여, 1회의 교배 및 돌연변이를 적용하면 진화 레벨은 하나 상승한다. 이와 같은 과정은, 진화 레벨이 초기화부(231)에서 정한 진화 레벨 G와 같아질 때까지 반복된다.

이상에서는 교배 알고리즘을 적용한 후 돌연변이 알고리즘을 적용하는 것으로 예를 들었다. 그러나, 이상의 교배 알고리즘 및 돌연변이 알고리즘은 역순으로 이루어질 수도 있고, 이들 알고리즘 중 하나만 적용될 수도 있음은 당업자라면 충분히 이해할 수 있을 것이다.

상기 진화 적용부(230)를 통하여 최적화된 파티클들에 대한 가중치는 다음의 수학식 6에 따라서 정규화된다. 이 경우 전체 파티클의 가중치의 합은 1이 된다.

이상의 과정을 거쳐서 최적화된 파라미터 세트 는 다시 샘플링부(211)에 제공된다. 이후, 시간 카운트가 1 증가하면, 샘플링부(211)는 상기 파라미터 세트를 갖는 N개의 파티클을 기초로 리샘플링하고, 자세 예측부(212) 및 가중치 계산부(213)는 상기 리샘플링 N개의 파티클에 대하여 전술한 바와 마찬가지로, 위치 예측 및 가중치 계산을 수행하며, 진화 적용부(230)는 진화 알고리즘을 적용한다. 이와 같은 과정은 상기 시간 카운트가 종료 지점(도 4의 E)가 될 때까지 반복된다.

진화 적용부(230)에서 최적화된 파티클들 중 가중치가 가장 높은 파티클의 자세를 이동 로봇(100)의 현재 자세로 추정될 수 있다. 상기 추정된 자세는 로봇 제어부(110)에 제공되는데, 로봇 제어부(110)는 이 자세 정보를 이용하여 이동 로봇(100)을 이동시키고 청소 등의 작업을 수행할 수 있다.

지금까지 도 1 및 도 2의 각 구성요소들은 메모리 상의 소정 영역에서 수행되는 태스크, 클래스, 서브 루틴, 프로세스, 오브젝트, 실행 쓰레드, 프로그램과 같은 소프트웨어(software)나, FPGA(field-programmable gate array)나 ASIC(application-specific integrated circuit)과 같은 하드웨어(hardware)로 구현될 수 있으며, 또한 상기 소프트웨어 및 하드웨어의 조합으로 이루어질 수도 있다. 상기 구성요소들은 컴퓨터로 판독 가능한 저장 매체에 포함되어 있을 수도 있고, 복수의 컴퓨터에 그 일부가 분산되어 분포될 수도 있다.

도 8a 및 도 8b는 종래 기술과 본 발명의 결과를 비교하기 위한 도면이다. 이 중에서 도 8a는 종래의 파티클 필터 기법에 따른 로컬라이제이션 및 맵 생성(특징점 표시) 결과를, 도 8b는 본 발명에 따른 로컬라이제이션 및 맵 생성 결과를 보여준다. 도 8a에서 사용된 파티클의 수는 100개이고 도 8b에서 사용된 파티클의 수는 50개(진화 레벨은 5)이다. 도 8a 및 8b를 비교하면, 본 발명은 기존의 파티클 필터 기법에 비하여 더 작은 파티클의 수로도 더 좋은 맵을 생성할 수 있음을 알 수 있다.

이 경우, SLAM 수행 시간(연산에 소요되는 시간)을 비교하면, 도 8a의 경우 474.7ms가 소요되었으나, 도 8b의 경우 443.5ms가 소요되었다. 즉, 동등 또는 그 이하의 연산 시간으로 훨씬 좋은 결과를 나타냄을 알 수 있다. 이는 다시 말하면, 동일한 결과를 나타내는데 사용되는 연산 시간이 훨씬 감소될 수 있음을 의미한다.

이상 첨부된 도면을 참조하여 본 발명의 실시예를 설명하였지만, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자는 본 발명이 그 기술적 사상이나 필수적인 특징을 변경하지 않고서 다른 구체적인 형태로 실시될 수 있다는 것을 이해할 수 있을 것이다. 그러므로 이상에서 기술한 실시예들은 모든 면에서 예시적인 것이며 한정적이 아닌 것으로 이해해야 한다.

상기 본 발명에 따르면, 이동 로봇의 자세를 추정하기 위한 파티클 필터 기법에 진화 알고리즘을 적용함으로써 요구되는 파티클의 수를 감소시킬 수 있다. 이로 인하여, SLAM 수행을 위한 연산 속도가 향상되고 필요한 메모리의 크기가 감소하는 효과가 있다.

Claims

복수의 파티클을 이용하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치로서,

이동 로봇의 자세 변화량을 감지하는 센서;

이전 파티클들에 상기 감지된 자세 변화량을 적용하여 현재 파티클들의 자세 및 가중치를 구하는 파티클 필터부; 및

상기 구한 자세 및 가중치에 진화 알고리즘을 적용하여 상기 현재 파티클들의 자세 및 가중치를 갱신하는 진화 적용부를 포함하는 자세 추정 장치.
제1항에 있어서,

외부 이미지를 포착하는 카메라; 및

상기 포착된 이미지로부터 특징점을 추출하고 상기 추출된 특징점을 가공하여 맵을 생성하는 특징점 처리부를 더 포함하는 자세 추정 장치.
제1항에 있어서, 상기 센서는

엔코더 및 자이로 중 적어도 하나를 포함하는 자세 추정 장치.
제1항에 있어서, 상기 자세는

평면 상의 2차원 좌표계의 각 좌표값 및 방향각을 포함하는 자세 추정 장치.
제2항에 있어서, 상기 파티클 필터부는

상기 파티클들에 상기 감지된 자세 변화량을 적용하여 상기 현재 파티클들의 자세를 구하는 자세 예측부; 및

상기 추출된 특징점을 기반으로 관측된 값과 상기 구한 자세를 비교하여 유사한 파티클에 보다 높은 가중치를 부여하는 가중치 계산부를 포함하는 자세 추정 장치.
제5항에 있어서, 상기 파티클 필터부는

상기 복수의 파티클들을 상기 이동 로봇의 최초의 위치를 바탕으로 샘플링하고, 상기 갱신된 가중치를 기반으로 상기 복수의 파티클들을 리샘플링하는 샘플링부를 더 포함하는 자세 추정 장치.
제2항에 있어서, 상기 진화 적용부는

상기 파티클 중 하나의 파티클로부터 랜덤 샘플링되는 복수의 진화 샘플 중 2 이상의 부모 샘플을 선택하여 이들의 가중합을 계산하고, 상기 가중합으로부터 생성되는 하나 이상의 자식 샘플과 상기 부모 샘플 중 일부의 샘플을 선택하는 교배 적용부를 포함하는 자세 추정 장치.
제7항에 있어서, 상기 일부의 샘플은

상기 자식 샘플과 상기 부모 샘플 중에서 가중치가 큰 샘플인 자세 추정 장치.
제2항에 있어서, 상기 진화 적용부는

상기 파티클 중 하나의 파티클로부터 랜덤 샘플링되는 복수의 진화 샘플 중 하나의 부모 샘플을 선택하여 이것에 랜덤 노이즈를 가하여 자식 샘플을 생성하고, 상기 부모 샘플 및 자식 샘플 중에서 가중치가 큰 샘플을 선택하는 돌연변이 적용부를 포함하는 자세 추정 장치.
제9항에 있어서, 상기 부모 샘플은

상기 복수의 진화 샘플 중 그 가중치가 소정의 임계치를 넘는 샘플 중에서 선택되는 자세 추정 장치.
제9항에 있어서, 상기 랜덤 노이즈는

가우시안 분포를 따르는 자세 추정 장치.
복수의 파티클을 이용하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법으로서,

이동 로봇의 자세 변화량을 감지하는 단계;

이전 파티클들에 상기 감지된 자세 변화량을 적용하여 현재 파티클들의 자세 및 가중치를 구하는 단계; 및

상기 구한 자세 및 가중치에 진화 알고리즘을 적용하여 상기 현재 파티클들의 자세 및 가중치를 갱신하는 단계를 포함하는 자세 추정 방법.
제12항에 있어서,

외부 이미지를 포착하는 단계; 및

상기 포착된 이미지로부터 특징점을 추출하고 상기 추출된 특징점을 가공하여 맵을 생성하는 단계를 더 포함하는 자세 추정 방법.
제12항에 있어서, 상기 감지하는 단계는

엔코더 및 자이로 중 적어도 하나를 이용하여 이루어지는 자세 추정 방법.
제12항에 있어서, 상기 자세는

평면 상의 2차원 좌표계의 각 좌표값 및 방향각을 포함하는 자세 추정 방법.
제13항에 있어서, 상기 자세 및 가중치를 구하는 단계는

상기 파티클들에 상기 감지된 자세 변화량을 적용하여 상기 현재 파티클들의 자세를 구하는 단계; 및

상기 추출된 특징점을 기반으로 관측된 값과 상기 구한 자세를 비교하여 유사한 파티클에 보다 높은 가중치를 부여하는 단계를 포함하는 자세 추정 방법.
제16항에 있어서, 상기 자세 및 가중치를 구하는 단계는

상기 복수의 파티클들을 상기 이동 로봇의 최초의 위치를 바탕으로 샘플링하는 단계; 및

상기 갱신된 가중치를 기반으로 상기 복수의 파티클들을 리샘플링하는 단계를 더 포함하는 자세 추정 방법.
제13항에 있어서, 상기 자세 및 가중치를 갱신하는 단계는

상기 파티클 중 하나의 파티클로부터 랜덤 샘플링되는 복수의 진화 샘플 중 2 이상의 부모 샘플을 선택하는 단계;

상기 부모 샘플의 가중합을 계산하는 단계; 및

상기 가중합으로부터 생성되는 하나 이상의 자식 샘플과 상기 부모 샘플 중 일부의 샘플을 선택하는 단계를 포함하는 자세 추정 방법.
제18항에 있어서, 상기 일부의 샘플은

상기 자식 샘플과 상기 부모 샘플 중에서 가중치가 큰 샘플인 자세 추정 방법.
제13항에 있어서, 상기 자세 및 가중치를 갱신하는 단계는

상기 파티클 중 하나의 파티클로부터 랜덤 샘플링되는 복수의 진화 샘플 중 하나의 부모 샘플을 선택하는 단계;

상기 부모 샘플에 랜덤 노이즈를 가하여 자식 샘플을 생성하는 단계; 및

상기 부모 샘플 및 자식 샘플 중에서 가중치가 큰 샘플을 선택하는 단계를 포함하는 자세 추정 방법.
제20항에 있어서, 상기 부모 샘플은

상기 복수의 진화 샘플 중 그 가중치가 소정의 임계치를 넘는 샘플 중에서 선택되는 자세 추정 방법.
제20항에 있어서, 상기 랜덤 노이즈는

가우시안 분포를 따르는 자세 추정 방법.