KR100809352B1

KR100809352B1 - 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치

Info

Publication number: KR100809352B1
Application number: KR1020060113440A
Authority: KR
Inventors: 명현; 방석원; 공동건; 이수진
Original assignee: 삼성전자주식회사
Priority date: 2006-11-16
Filing date: 2006-11-16
Publication date: 2008-03-05
Also published as: JP2008126401A; EP1923763B1; JP5219467B2; US20120029698A1; US20080119961A1; US8630735B2; CN101183266A; CN101183266B; US8055382B2; EP1923763A1

Abstract

본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 방법은 이동 로봇의 자세 변화량을 감지하여 이전 파티클에 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 단계, 센서로부터 획득한 레인지 데이터(range data)와 맵 정보로부터 현재 파티클의 자세 확률 예측 및 가중치를 구하는 단계, 가중치를 기초로 현재 파티클을 리샘플링(resampling)하는 단계 및 센서의 오차를 고려하여 가중치를 조정하는 단계를 포함한다.

슬램(SLAM), 파티클 필터(Particle filter), Map consistency, hill climbing

Description

파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치{Method and apparatus of pose estimation in a mobile robot based on particle filter}

도 1은 본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 방법을 보여주는 흐름도이다.

도 2는 절대 좌표계를 기준으로 스텝의 크기가 동일한 종래의 힐 클라이밍(hill climbing) 방법(a)과 로봇의 방향각에 따른 상대 좌표계를 기준으로 스텝의 크기가 다른 수정된 힐 클라이밍 방법(b)의 좌표축을 도시한 도면이다.

도 3은 도 2의 (a), (b) 방법에 따라 각 파티클에 대하여 힐 클라이밍을 하는 모습을 보여주는 도면이다.

도 4는 주행계와 센서의 정확도 표준편차가 30:1인 경우(a)와 10:1인 경우(b)에 주행계와 센서에 의한 로봇의 자세 확률값을 보여주는 도면이다.

도 5는 주행계와 센서의 정확도 표준편차가 30:1인 경우(a)와 10:1인 경우(b)에, [식 3]에 의한 정확한 식에 의한 자세 확률값과 [식 4]에 의한 근사화한 식에 의한 자세 확률값의 일 예를 보여주며 정확한 식과 근사한 식의 오차 비(r(x))를 보여주는 도면이다.

도 6은 본 발명의 일 실시예에 따른 센서의 오차 특성에 따라 가중치를 재조정하는 함수를 나타내는 P(x)를 도시한 그래프이다.

도 7은 본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 장치의 블럭도이다.

<도면의 주요 부분에 대한 설명>

600: 주행계(odomery)

610: 파티클 자세 계산부

620: 센서

630: 파티클 필터부

640: 샘플링부

650: 가중치 조정부

660: 맵 생성부

본 발명은 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치로, 더욱 상세하게는 파티클 필터를 이용한 격자 기반(Grid-based) 슬램(SLAM; Simultaneous Localization And Map Building) 알고리즘에 센서의 오차를 반영하여 가중치를 조정하여 이동 로봇의 자세를 상대적으로 정확하게 추정하는 방법 및 장치에 관한 것이다.

최근에는 산업체에서 이용되는 산업용 로봇뿐만 아니라 일반 가정이나 사무실 등에서 가사일이나 사무 보조로서 로봇이 실용화되고 있다. 이에 해당하는 대표 적인 예로서 청소용 로봇, 안내 로봇, 방범 로봇 등을 들 수 있다.

이러한 로봇은 기본적으로 주어진 공간내에서 이동을 하면서 로봇 자신의 고유한 기능을 수행한다. 로봇이 주어진 환경속에서 주행과 기능 수행을 위해서는 주변 공간에 대한 맵의 작성과 로봇의 위치 및 방향각(Heading angle)에 대한 정보를 필요로 한다. 이러한 맵 작성과 함께 자신의 위치를 인식하는 방법으로 슬램(SLAM; Simultaneous Localization And Mapping) 알고리즘이 사용될 수 있다.

슬램 알고리즘은 어떤 위치에서 주변 환경의 맵(map)을 작성하고, 작성된 맵을 바탕으로 다시 움직인 로봇의 위치를 알아내는 반복을 통해 로봇의 위치와 주변 환경의 맵을 동시에 추정할 수 있다.

슬램 알고리즘에서는 로봇 자신의 위치를 추정하기 위해서 파티클 필터(Particle filter) 기법이 사용될 수 있다. 파티클 필터 기법이란 로봇의 위치 및 방향각에 대한 예측치를 가진 복수개의 샘플을 추출하고, 각 샘플이 실제 로봇의 위치 및 방향각일 확률을 이용하여 로봇의 최적의 자세를 추정하는 방법이다. 본 발명에서 언급하는 '자세(pose)'는 이동 로봇의 평면 상의 2차원 좌표계 위치(position) 및 방향각을 의미한다.

레인지 데이터(range data)를 이용한 격자 기반(Grid-based) 슬램에서는 특징점(Feature points)을 추출할 필요가 없이 모든 레인지 데이터를 이용하여 맵을 작성할 수 있다. 하지만, 종래에서 레인지 데이터를 얻기 위해서는 정확도가 우수한 거리 측정기(Range finder)(이하, 센서하고 함)를 사용하여야 양질의 맵을 얻을 수 있어 센서의 정확도에 크게 의존하는 문제가 있었다.

일반적으로 상용화되는 청소용 로봇, 방범 로봇 등과 같은 사무용 또는 가정용 로봇에 있어서는 저가이면서 센서의 정확도가 상대적으로 떨어지는 경우에도 맵 작성과 로봇의 위치를 정확하게 예측하는 것이 필요하다.

본 발명은 상기와 같은 문제점을 고려하여 안출된 것으로서, 맵 일관성(Map consistency) 검사를 통하여 파티클 필터에서 구한 파티클의 가중치를 조정함으로써 이동 로봇의 자세 추정을 정확도를 높이는 것을 목적으로 한다.

이와 함께, 로봇의 방향각에 따른 힐 클라이밍(Hill-climbing)을 함으로써 효율적으로 이동 로봇의 자세를 추정할 수 있는 것을 목적으로 한다.

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

상술한 목적을 달성하기 위하여 본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 방법은 (a) 이동 로봇의 자세 변화량을 감지하여 이전 파티클에 상기 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 단계; (b) 센서로부터 획득한 레인지 데이터(range data)와 맵 정보로부터 상기 현재 파티클의 자세 확률 예측 및 가중치를 구하는 단계; (c) 상기 가중치를 기초로 상기 현재 파티클을 리샘플링(resampling)하는 단계; 및 (d) 상기 센서의 오차를 고려하여 상기 가중치를 조정하는 단계를 포함한다.

상술한 목적을 달성하기 위하여 본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 장치는 이동 로봇의 자세 변화량을 감지하는 주행계(odometry); 이전 파티클에 상기 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 파티클 자세 계산부; 레인지 데이터를 획득하는 센서; 상기 레인지 데이터(range data)와 맵 정보로부터 상기 현재 파티클의 자세 확률 예측 및 가중치를 구하는 파티클 필터부; 및 상기 센서의 오차를 고려하여 상기 가중치를 조정하는 가중치 조정부를 포함한다.

기타 실시예들의 구체적인 사항들은 상세한 설명 및 도면들에 포함되어 있다. 본 발명의 이점 및 특징, 그리고 그것들을 달성하는 방법은 첨부되는 도면과 함께 상세하게 후술되어 있는 실시예들을 참조하면 명확해질 것이다. 그러나 본 발명은 이하에서 개시되는 실시예들에 한정되는 것이 아니라 서로 다른 다양한 형태로 구현될 수 있으며, 단지 본 실시예들은 본 발명의 개시가 완전하도록 하고, 본 발명이 속하는 기술분야에서 통상의 지식을 가진 자에게 발명의 범주를 완전하게 알려주기 위해 제공되는 것이며, 본 발명은 청구항의 범주에 의해 정의될 뿐이다. 명세서 전체에 걸쳐 동일 참조 부호는 동일 구성 요소를 지칭한다.

이하, 본 발명의 바람직한 실시예에 대해 첨부된 도면을 참조하여 보다 상세히 설명한다.

본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 방 법은 이동 로봇의 자세 변화량을 감지하여 이전 파티클에 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 단계(S100), 힐 클라이밍(hill-climbing) 기법에 의해 파티클의 자세를 갱신하는 단계(S110), 센서(620)로부터 획득한 레인지 데이터와 맵 정보로부터 현재 파티클의 자세 확률 예측 및 가중치를 구하는 단계(S120), 가중치를 기초로 현재 파티클을 리샘플링하는 단계(S130) 및 센서(620)의 오차를 고려하여 가중치를 조정하는 단계(S140)를 포함한다.

본 발명의 일 실시예에 따른 파티클 필터 기반의 이동 로봇의 자세 추정 방법을 실시하기 위해 이동 로봇이 주행하면서 맵을 작성하고 있는 상황을 살펴 보기로 하자.

먼저, 이동 로봇이 출발하는 지점의 위치 및 이동 로봇이 향하는 방향인 방향각이 주어진다. 또는 이동 로봇이 출발하는 위치 및 출발시 방향각을 기준값으로 설정할 수 있다. '자세'는 이동 로봇의 2차원 평면 상에서의 위치 및 방향각을 의미하므로 이동 로봇의 자세를 추정한다는 것은 이동 로봇의 위치 및 방향각을 추정함을 의미한다.

주어진 출발점에서 이동 로봇이 소정의 거리를 이동할 수 있다. 로봇에 장착되어 있는 로봇의 이동량 및 회전각을 감지할 수 있는 주행계(odometry)(600)에 의해 이동 로봇이 움직임에 의한 자세 변화량을 감지할 수 있다. 주행계(600)의 예로서 엔코더 및 자이로를 들 수 있다. 예를 들어 엔코드에 의하여 움직인 거리 및 방향을 적분함으로써 현재 이동 로봇의 자세를 알 수 있다. 만일 적분 오차가 없다면 엔코더 만으로도 로봇의 자세 추정이 가능하겠지만, 엔코더는 짧은 구간에는 비교 적 정확하나 숫자를 계속 적분함으로써 오차가 누적되는 문제가 있다. 또한, 이동 로봇은 이동 중에 미끄러지거나 장애물을 만날 수 있으므로 엔코더 값에 의한 로봇의 위치와 실제 위치 사이에는 오차가 발생할 수 있다.

따라서 로봇의 자세를 보다 정확하게 추정하기 위하여 파티클 필터를 이용한 슬램 알고리즘을 이용한다.

샘플링부(640)는 최초에 N개의 파티클을 무작위로 샘플링한다. 이동 로봇의 초기 위치 근처에 N개의 임의의 지점 즉 다음식과 같이 N개의 파티클을 선택한다.

[식 1]

여기서,

및

는 특정 인덱스 j의 파티클에 대하여 이동 로봇의 초기 위치 및 초기 가중치를 각각 나타낸다. 상기

는 인덱스 j와 상관없이 모두 1/N으로 동일하게 설정할 수 있다. 가중치는 현재 파티클의 위치가 이동 로봇의 위치가 될 확률을 의미한다.

로봇의 소정의 거리를 이동한 때에는 소정의 시간이 경과되므로, 시간의 경과에 따른 파티클의 자세를 갱신할 수 있다.

파티클 자세 계산부(610)는 전술한 이동 로봇에 구비된 주행계(Odometry) (600)에 의해 감지된 자세 변화량을 파티클에 적용하여 파티클의 자세를 구한다(S100).

이때, 파티클 자세 계산부(610)는 파티클 확률의 정확도를 높이기 위해 힐 클라이밍 방식을 도입하여 각 파티클의 자세를 갱신할 수 있다(S110).

도 2는 절대 좌표계를 기준으로 스텝의 크기가 동일한 종래의 힐 클라이밍 방법(a)과 로봇의 방향각에 따른 상대 좌표계를 기준으로 스텝의 크기가 다른 수정된 힐 클라이밍 방법(b)의 좌표축을 도시한 도면이고, 도 3은 도 2의 (a), (b) 방법에 따라 각 파티클에 대하여 힐 클라이밍을 하는 모습을 보여주는 도면이다.

도 2(a)와 같이 종래의 힐 클라이밍 방식은 절대 좌표축을 기초로 4방향의 직선 이동에 의한 변화(Δx, -Δx, Δy, -Δy)와 2방향의 회전 이동에 의한 변화(Δθ, -Δθ)를 상기 구한 파티클의 자세에 각각 더하였다. 다시 말해서, 파티클이 지닌 로봇의 자세에 상기 6개의 변화를 각각 더하여서 파티클의 자세를 약간 변화시켜 로봇의 위치 확률이 증가되는 방향으로 로봇의 위치를 변화시키는 방식이다. 종래의 힐 클라이밍 방식은 G. Grisetti, C. Stachniss, and W. Burgard, Improving grid-based slam with Rao-Blackwellized particle filters by adaptive proposals and selective resampling," In Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), pp.2443-2448, Barcelona, Spain, 2005 와 D. Hahnel, D. Schulz, and W. Burgard, "Map building with mobile robots in populated environments", In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems(IROS), 2002에 상술되어 있으므로 자세한 설명은 생략하기로 한다. 그리고, 로봇 위치의 확률을 계산하는 과정은 후술하기로 한다.

본 발명에서 새롭게 제안하는 수정된 힐 클라이밍 방식은 로봇의 방향각을 주축으로 하여 각 파티클의 자세를 갱신할 수 있다. 따라서 도 2(b)에서와 같이 절대좌표계에 따른 힐 클라이밍이 아닌 로봇의 방향각에 따른 상대 좌표계인 x' 축과 그에 수직인 y' 축을 두 축으로 하여 힐 클라이밍을 수행한다.

이와 함께 힐 클라이밍 스텝 크기(Step size) (종래의 Δx, Δy에 해당)를 상수로 두지 않고 주행계(600) 모델의 오차 공분산 타원(Error covariance ellipse)에 따라서 적응적으로 변환하도록 할 수 있다. 이는 로봇의 이동 방향으로 주행계(600)의 오차가 더 크게 발생할 가능성이 높은 것을 반영한 것으로, 힐 클라이밍 수행 시간과 파티클의 위치 오차를 감소시킬 수 있다. 이는 아래의 [식 2]와 같이 정의될 수 있다.

[식 2]

여기서 (x, y)는 절대 좌표계를 기준으로 하는 힐 클라이밍 방식의 축이며, (x', y')는 로봇의 방향각에 따른 상대 좌표계를 기준으로 하는 힐 클라이밍 방식의 축을 나타낸다.

도 3의 (a)와 (b)는 전술한 두 가지의 힐 클라이밍 방법에 의해 각 파티클(210)의 자세를 갱신하는 것을 보여주는 도면인데, 도 3(a)에서는 절대 좌표축을 기준으로 로봇(200)의 방향각과 무관하게 ±Δx, ±Δy, ±Δθ만큼 파티클(210)의 위치를 옮기면서 위치 확률이 증가되는 방향으로 위치를 옮겨간다. 반면에 도 3(b)에서는 로봇(200)의 이동 방향에 해당하는 방향각에 따른 상대 좌표축을 기준으로 ±Δx', ±Δy', ±Δθ파티클(210)의 위치를 옮기면서 힐 클라이밍 기법을 수행한다. 이때, 스텝의 간격은 로봇(200)의 진행 방향인 ±Δx'방향으로는 ±Δy'에 비하여 큼을 알 수 있다.

따라서 x축 이동(±Δx)과 y(±Δy)축 이동 및 회전 이동(±Δθ)(바람직하게는, 상대 좌표계에 따른 x'축 이동(±Δx')과 y'(±Δy')축 이동 및 회전 이동(±Δθ)의 6가지 경우에 대하여 힐 클라이밍을 적용하여 각각에 대하여 후술할 식 5에 의해 스코어(score)를 계산할 수 있다. 스코어는 힐 클라이밍에 의해 산출되는 가중치로서 힐 클라이밍이 종료된 후에 가장 스코어가 높은 값이 해당 파티클의 가중치가 되고, 스코어가 가장 높은 값의 위치 및 방향각이 해당 파티클의 자세가 된다.

상기의 힐 클라이밍은 수회에 걸쳐 수행될 수 있다. 다시 말해서, 6가지의 경우에 대하여 힐 클라이밍을 수행하여 가장 높은 스코어에 해당하는 값을 해당 파티클의 자세로 하고 또 다시 동일한 과정을 반복하여 가장 높은 스코어의 값을 찾는 시도를 한다. 그리하여 시도한 경우 중에서 가장 높은 스코어를 가진 값을 해당 파티클의 가중치로 갱신하고, 그때의 로봇의 위치 및 방향각이 해당 파티클의 자세로 갱신한다.

이동 로봇이 소정의 거리를 이동한 후 센서(620)에 의해 새로운 관찰 정보인 레인지 데이터(range data)를 얻고, 이러한 레인지 데이터와 기존의 맵 정보에 의해 파티클 필터부(630)는 파티클의 자세 확률을 예측하고 가중치를 계산할 수 있다(S120).

가중치는 파티클의 자세가 추정되는 이동 로봇의 현재 자세와 유사할수록 커지게 되며, 따라서 가중치가 높을수록 이동 로봇이 그러한 가중치를 가지는 파티클의 자세를 가질 가능성이 높다고 할 수 있다. 이와 함께, 관찰 정보는 이동 로봇에 구비된 주위 정보 특히 주변 장애물과의 거리를 인식할 수 있는 센서(620)에 의해 얻어질 수 있다. 이러한 센서(620)는 초음파 또는 적외선 센서(620)를 이용하거나 스트럭쳐 라이트(Structured light) 모듈(Module)을 이용하여 로봇의 주위의 장애물과의 거리를 감지할 수 있다.

파티클 필터를 이용하여 파티클의 자세 확률을 예측하고 가중치를 계산하는 방법에 대해 살펴보기로 한다.

이동 로봇의 위치를 확률 분포로서 나타낼 수 있고, 이와 같은 확률 분포(p)는 이전 스텝(시간 t-1)과 현재 스텝(시간 t)의 데이터를 이용하는 다음의 [식 3]에 의해 나타낼 수 있다.

[식 3]

여기서, x_t _-1은 시간 t-1에서의 자세이고, u_t는 시간 t(현재)에서의 주행계(600)에 의해 감지된 데이터이고, x_t 는 시간 t에서의 자세이다. m_t _-1은 시간 t-1에서의 맵 상의 데이터이며, z_t 는 시간 t에서의 센서(620)에 얻어진 데이터이다. 상기 확률 분포는 로봇이 동적 잡음이나 시스템에 존재하는 다른 불확실한 인자를 가진 채 x_t _-1에서 x_t까지 움직였을 때, 시간 t에서의 로봇 자세에 대한 확률 분포를 의미한다.

종래의 Grid-based SLAM 알고리즘은 식 3을 근사화시켜 계산하였다. 센서(620)의 정확도가 주행계(600)에 비하여 매우 높은 경우에, 예를 들어 도 4(a)와 같이 주행계(600)와 센서(620)의 정확도의 표준편차 비가 30:1인 경우에는 관찰 영역(

)에서는 센서(620)에 의한 로봇의 자세 확률값(

)은 급격히 변하지만, 주행계(600)에 의한 로봇의 자세 확률값(

)은 상수(Constant; k)로 근사화 될 수 있다. 따라서 식 3은 다음과 같이 [식 4]로 근사화 될 수 있다.

[식 4]

현재 파티클의 가중치는 다음 [식 5]에 의해 근사화 시켜 계산할 수 있다.

[식 5]

전술한 [식 3], [식 4], [식 5]에 의한 파티클의 자세를 예측하고 가중치를 구하는 과정은 G. Grisetti, C. Stachniss, and W. Burgard, "Improving grid-based slam with Rao-Blackwellized particle filters by adaptive proposals and selective resampling," In Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), pp.2443-2448, Barcelona, Spain, 2005에 상술되어 있다.

파티클 필터부(630)가 전술한 [식 3], [식 4], [식 5]에 따라 파티클의 자세 확률을 예측하고 가중치를 계산한 후에 샘플링부(640)는 가중치를 기초로 현재 파티클을 리샘플링(resampling)한다(S130). 초기에 N개의 파티클을 가지고 출발하여 시간 카운트 t가 1 증가하였을때, 상기 N개의 파티클 중 가중치가 낮은 파티클을 탈락시키고, 가중치가 높은 파티클을 중심으로 샘플링하여 다시 N개의 파티클을 유지하도록 한다. 이를 리샘플링이라고 한다.

다음, 맵 평가(map estimation)를 통한 그리드맵(gridmap)을 갱신할 때에 가중치 조정부(650)에서는 센서(620)의 오차를 고려하여 가중치를 조정한다(S140).

본 발명의 일 실시예에서는 센서(620)의 정확도가 우수하지 아니하여 주행 계(600)에 의한 로봇의 자세 확률값(

)을 상수로 적용하지 아니하고 가중치를 계산하는 방법을 제시한다. 즉, 전술한 방법으로 구한 가중치를 리샘플링한 후에 후술할 방법으로 조정을 한다.

센서(620)의 정확도가 우수하지 않는 경우에는 주행계(600)에 의한 자세 확률값을 상수로 근사화 할 수 없다. 즉, 도 4(b)와 같이 주행계(600)와 센서(620)의 정확도의 표준편차 비가 10:1인 경우에는 관찰 영역(

)에서 주행계(600)에 의한 로봇의 자세 확률값(

)은 상수(Constant; k)로 근사화 될 수 없다.

도 5는 주행계(600)와 센서(620)의 정확도 표준편차가 30:1인 경우(a)와 10:1인 경우(b)에 [식 3]에 의한 정확한 식에 의한 자세 확률값과 [식 4]에 의한 근사화한 식에 의한 자세 확률값을 보여주며 정확한 식과 근사한 식의 오차비(r(x))를 보여주는 도면이다.

30:1인 경우에는 주행계(600)에 의한 로봇의 자세 확률값을 상수로 하더라도 실제로 적분을 하는 구간인 로봇의 추정 위치 부근(도 5(a)의 A부분)에서는 거의 오차가 없어 오차 비 r(x)가 1에 가까운 것을 알 수 있다. 이와 대조적으로 10:1인 경우에는 센서(620)의 정확도가 상대적으로 높지 않아서 오차가 커지게 된다. 이는 도 5(b)에서 분포되는 오차 비 r(x)는 로봇 위치로 추정되는 영역(도 5(b)의 A영역)에서 상대적으로 크게 변화하는 것으로부터 알 수 있다.

따라서 도 5(b)에서 도시되는 오차 비 r(x)를 확률을 계산하는 데에 고려함 으로써 주행계(600)에 의한 로봇의 자세 확률값을 상수로 하지 않고 변화하는 양으로 계산할 수 있다.

오차비율 r(x)를 고려하면 다음과 같은 가중치 조정식을 얻을 수 있다.

[식 6]

가중치 조정을 효율적으로 하기 위해 로그(log) 식으로 변환하면 다음 식 7과 같다.

[식 7]

식 5를 로그 식으로 변환 후 식 7과 비교해 보면 식 7에서 R(x) 크기를 빼주는 것이 다르다. 이는 센서(620)의 오차가 비교적 큰 경우에 오차에 의해 생겨나는 양으로써 각 센서(620)의 오차 특성에 따라 그 형태와 크기가 달라질 수 있다.

이때, 식 7의 R(x)는 P(x)와 격자 셀 점유 확률 (Grid cell occupancy) c_j의 곱으로 나타낼 수 있다. 아래의 식 8에 의하여 나타나는 P(x)의 형태는 센서(620)와 센서(620)로부터 감지된 영역을 잇는 선인 그리드선(gridline)을 따라 포물선 형태로 도 6에 도시된다. 식 8의 P(x)를 도시한 도 6의 그래프는 센서(620)에 따라서 그 형태와 값이 다양하게 바뀜은 물론이다.

[식 8]

식 8에서 m은 센서(620)의 정확도에 따라 좌우되는 값이고, c_j는 그리드 셀 점유 확률 값으로 0과 1 사이의 값이다. 또한, x는 각 파티클의 자세 상에서 가지고 있는 맵 상의 격자점(Grid point)까지의 거리이고, D는 현재 이동 로봇의 센서(620)에 의해 측정된 격자점까지의 거리이다. 따라서 현재 이동 로봇의 센서(620)에 의해 측정된 격자점 데이터가 옳고 각 파티클의 자세가 현재 이동 로봇의 실제 자세와 근접하다면 x와 D의 값에 차이가 거의 없이 결과적으로 P(x)와 R(x)의 값이 작아진다. 하지만 반대로 파티클에서 가진 자세가 현재 이동 로봇의 실제 자세와 유사하지 아니하면 x와 D 값에 차이가 크게 발생하여 P(x)와 R(x)의 값이 커진다. 이를 맵 일관성 검사(Map consistency check)라고 한다.

따라서 위와 같이 모든 파티클에 대하여 맵 일관성 검사에 의하여 R(x)를 검사하여 각 파티클의 가중치를 조정할 수 있다. 이러한 각 파티클에 대한 가중치 조정은 그리드맵을 갱신할 때 이루어지므로 추가적인 계산시간의 증가가 거의 없다.

로봇은 전술한 단계를 반복하면서 이동 중인 로봇의 자세를 추정할 수 있다.

본 발명의 일 실시예에 파티클 필터 기반의 따른 이동 로봇의 자세 추정 장치는 주행계(600), 파티클 자세 계산부(610), 센서(620), 파티클 필터부(630), 가중치 조정부(650), 샘플링부(640), 맴 생성부(660)를 포함할 수 있다.

이 때, 본 실시예에서 사용되는 '~부'라는 용어는 소프트웨어 또는 FPGA또는 ASIC과 같은 하드웨어 구성요소를 의미하며, '~부'는 어떤 역할들을 수행한다. 그렇지만 '~부'는 소프트웨어 또는 하드웨어에 한정되는 의미는 아니다. '~부'는 어드레싱할 수 있는 저장 매체에 있도록 구성될 수도 있고 하나 또는 그 이상의 프로세서들을 재생시키도록 구성될 수도 있다. 따라서, 일 예로서 '~부'는 소프트웨어 구성요소들, 객체지향 소프트웨어 구성요소들, 클래스 구성요소들 및 태스크 구성요소들과 같은 구성요소들과, 프로세스들, 함수들, 속성들, 프로시저들, 서브루틴들, 프로그램 코드의 세그먼트들, 드라버들, 펌웨어, 마이크로코드, 회로, 데이터, 데이터베이스, 데이터 구조들, 테이블들, 어레이들, 및 변수들을 포함한다. 구성요소들과 '~부'들 안에서 제공되는 기능은 더 작은 수의 구성요소들 및 '~부'들로 결합되거나 추가적인 구성요소들과 '~부'들로 더 분리될 수 있다.

주행계(600)는 이동 로봇의 자세 변화량을 감지하는 것으로, 주행계(600)의 일 예로 인코더 또는 자이로를 들 수 있다.

파티클 자세 계산부(610)는 주행계(600)에 의해 감지된 자세 변화량을 기초로 각 파티클의 자세를 구한다. 이때, 종래의 힐 클라이밍 기법 또는 로봇의 방향 각에 따라 좌표축의 방향과 스텝의 크기를 달리하는 힐 클라이밍 기법에 의해 파티클의 위치 확률이 증가되는 방향으로 파티클의 자세를 갱신함이 바람직하다.

센서(620)는 이동 로봇의 외부에 구비되어 관찰 정보 특히 주변 장애물과의 거리를 인식하여 레인지 데이터를 획득한다. 본 발명에 있어서는 비교적 센서(620)의 정확도가 떨어지는 스트럭쳐드 라이트(Structured light) 센서를 사용할 수 있다.

파티클 필터부(630)는 센서(620)로부터 획득한 레인지 데이터와 맵 정보로부터 현재 파티클의 자세 확률을 예측하고 가중치를 구한다.

가중치 조정부(650)는 센서(620)의 오차를 고려하여 가중치 계산부에서 구한 가중치를 조정한다. 이때, 현재 파티클 위치에서 맵 상의 거리와 센서(620)로부터 측정한 거리에 따라 센서(620)에 따라 미리 정의되는 오차 특성 값으로 가중치를 조정할 수 있다.

샘플링부(640)는 이동 로봇의 최초 위치를 바탕으로 파티클을 샘플링하고, 로봇이 이동한 후에 파티클 필터부(630)에서 구한 가중치를 기초로 파티클을 리샘플링한다. 이때, 리샘플링은 가중치가 낮은 파티클은 탈락시키고 가중치가 높은 파티클을 중심으로 샘플링을 수행할 수 있다.

맵 생성부(660)는 로봇이 이동하면서 센서(620)로부터 획득한 레인지 데이터를 기초로 맵을 생성하며 갱신한다.

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

상기한 바와 같은 본 발명의 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치에 따르면 다음과 같은 효과가 하나 혹은 그 이상 있다.

첫째, 비교적 정확도가 떨어지는 저가형 센서를 사용하여도 파티클 필터를 이용한 격자 기반(Grid-based) 슬램 알고리즘에 의해 정확도가 높게 이동 로봇의 자세를 추정할 수 있다는 장점이 있다.

둘째, 로봇의 방향각을 고려하여 힐 클라이밍 기법을 수행함으로써 계산시간과 위치오차를 감소시킬 수 있다는 장점도 있다.

Claims

(a) 이동 로봇의 자세(pose) 변화량을 감지하여 이전 파티클에 상기 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 단계;

(b) 센서로부터 획득한 레인지 데이터(range data)와 맵 정보로부터 상기 현재 파티클의 자세 확률 예측 및 가중치를 구하는 단계;

(c) 상기 가중치를 기초로 상기 현재 파티클을 리샘플링(resampling)하는 단계; 및

(d) 상기 센서의 오차를 고려하여 상기 가중치를 조정하는 단계를 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 (a) 단계에서,

힐 클라이밍(hill climbing) 방법을 이용하여 위치 확률이 증가되는 방향으로 상기 현재 파티클의 자세를 갱신하는 단계를 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 (a) 단계에서,

상기 이동 로봇의 방향각을 주축으로 하는 상대 좌표계를 기준으로 힐 클라 이밍 방법을 이용하여 위치 확률이 증가되는 방향으로 상기 현재 파티클의 자세를 갱신하는 단계를 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 3 항에 있어서,

상기 힐 클라이밍 방법은 스텝 크기를 상기 로봇의 진행 방향에 대하여 커지도록 상기 자세 변화량을 감지하는 주행계(odometry) 모델의 오차 공분산 타원(error covariance ellipse)에 따라서 적응적으로 변하도록 하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 (d) 단계에서,

상기 가중치는 상기 현재 파티클의 위치에서 맵 상의 거리와 상기 센서에 의해 측정된 거리를 기초로 상기 센서에 따라 정의되는 오차 특성 값으로 조정되는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 자세 변화량은 엔코더 또는 자이로에 의해 감지되는 것을 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 자세는 평면 상의 2차원 좌표계의 각 좌표값 및 방향각을 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 센서는 스트럭쳐드 라이트(structured light) 센서를 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
제 1 항에 있어서,

상기 리샘플링은 설정된 값을 기준으로 상기 가중치가 낮은 파티클은 탈락시키고, 상기 가중치가 높은 파티클을 중심으로 샘플링을 하는 파티클 필터 기반의 이동 로봇의 자세 추정 방법.
이동 로봇의 자세(pose) 변화량을 감지하는 주행계(odometry);

이전 파티클에 상기 감지된 자세 변화량을 적용하여 현재 파티클의 자세를 구하는 파티클 자세 계산부;

레인지 데이터를 획득하는 센서;

상기 레인지 데이터(range data)와 맵 정보로부터 상기 현재 파티클의 자세 확률 예측 및 가중치를 구하는 파티클 필터부; 및

상기 센서의 오차를 고려하여 상기 가중치를 조정하는 가중치 조정부를 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 파티클 자세 계산부는 힐 클라이밍(hill climbing) 방법을 이용하여 위치 확률이 증가되는 방향으로 상기 현재 파티클의 자세를 갱신하는 것을 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 파티클 자세 계산부는 상기 이동 로봇의 방향각을 주축으로 하는 상대 좌표계를 기준으로 힐 클라이밍 방법을 이용하여 위치 확률이 증가되는 방향으로 상기 현재 파티클의 자세를 갱신하는 것을 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 12 항에 있어서,

상기 힐 클라이밍 방법은 스텝 크기를 상기 로봇의 진행 방향에 대하여 커지도록 상기 주행계(odometry) 모델의 오차 공분산 타원(error covariance ellipse)에 따라서 적응적으로 변하도록 하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 센서는 스트럭쳐드 라이트(structured light) 센서를 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 가중치 조정부는 상기 현재 파티클의 위치에서 맵 상의 거리와 상기 센서에 의해 측정된 거리를 기초로 상기 센서에 따라 정의되는 오차 특성 값으로 상기 가중치를 조정하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 주행계는 엔코더 또는 자이로를 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

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

상기 레인지 데이터로부터 상기 맵을 생성하는 맵 생성부를 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 10 항에 있어서,

상기 이동 로봇의 최초 위치를 바탕으로 상기 파티클을 샘플링하고, 상기 파티클 필터부에서 구한 가중치를 기초로 파티클을 리샘플링하는 샘플링부를 더 포함하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.
제 19 항에 있어서,

상기 리샘플링은 설정된 값을 기준으로 상기 가중치가 낮은 파티클은 탈락시키고, 상기 가중치가 높은 파티클을 중심으로 샘플링을 하는 파티클 필터 기반의 이동 로봇의 자세 추정 장치.