KR100926783B1

KR100926783B1 - 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를바탕으로 한 로봇의 자기 위치 추정 방법

Info

Publication number: KR100926783B1
Application number: KR1020080013832A
Authority: KR
Inventors: 박성기; 박순용
Original assignee: 한국과학기술연구원
Priority date: 2008-02-15
Filing date: 2008-02-15
Publication date: 2009-11-13
Also published as: JP4942733B2; KR20090088516A; US20090210092A1; JP2009190164A; US8024072B2

Abstract

본 발명은 이동 로봇이 기준좌표가 주어진 동작 환경내에서 로봇 스스로 자기의 위치를 추정(estimation)하는 방법에 관한 것으로, 좀더 자세하게는 이동 로봇이 환경내의 개별 물체들을 비전 센서에 의해 인식을 하고, 인식된 물체와 인식된 물체를 포함하는 주변 환경(의 3차원) 정보를 주어진 지도(맵)를 기반으로 분석하여 자기의 위치를 추정(estimation)하는 방법에 관한 것이다.

본 발명에서는, 스테레오 영상장치로 형성된 카메라부와, 로봇 이동 경로 주위의 지도가 저장된 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서, 상기 카메라부에서 로봇 주위의 영상을 획득하는 1 단계와; 상기 위치 연산부에서 상기 카메라부가 획득한 영상 내의 개별 물체를 인식하고, 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와; 상기 위치 연산부에서 상기 데이터베이스부에 저장되어 있는 지도 및 상기 2 단계의 결과를 바탕으로 로봇의 위치를 추정하는 3 단계를 포함하는 로봇의 자기 위치 추정 방법이 제시된다.

스테레오 영상 입력 장치, 파티클 필터링 기법, 위상학적 지도, 로봇

Description

물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기 위치 추정 방법 { Method for self-localization of a robot based on object recognition and environment information around the recognized object }

본 발명은 이동 로봇이 기준좌표가 주어진 동작 환경내에서 로봇 스스로 자기의 위치를 추정(estimation)하는 방법에 관한 것으로, 이동 로봇이 환경 내의 개별 물체들을 비전 센서에 의해 인식을 하고, 인식된 물체와 인식된 물체를 포함하는 주변 환경 정보를 주어진 지도(맵)를 기반으로 분석하여 자기의 위치를 추정(estimation)하는 방법에 관한 것이다.

산업용 로봇에서 시작한 로봇은 최근에는 임의의 환경에서도 동작할 수 있는 지능로봇의 형태로 그 응용 영역을 넓혀가고 있다. 이러한 지능로봇은 청소로봇을 비롯하여, 경비로봇, 위험물 탐지 로봇, 가사 도우미 로봇 및 교육 로봇 등이 있을 수가 있다. 이러한 로봇이 주어진 환경에서 사람에게 서비스를 제공하기 위해서는 로봇 주행기술의 확보는 필수적이며, 그 중에서 로봇이 자기 위치를 추정해야 하는 자기위치추정(self-localization) 기술은 핵심적 요소 기술이 된다.

이러한 로봇의 자기 위치 추정 기술은 기본적으로 지도를 바탕으로 이루어지 기 때문에 로봇이 이용하는 지도 표상(representation)이 어떤 방식으로 구성되어져 있느냐와 이용하는 센서에 따라 다양하게 변화할 수 있다. 최근의 기술의 전개는 지도(맵)는 통상 격자지도와 같은 수치적 지도위에 위상적(topological) 지도를 결합하여 사용을 하며, 센서로는 레이저 및 초음파센서에서 비전 센서로 발전을 이루고 있는데, 이는 비전 센서가 거리 정보뿐만 아니라 영상에 기반하여 색상, 물체식별 및 사람식별 등과 같은 풍부한 추가 정보를 제공할 수도 있어 그 사용의 확대가 이루어지고 있다.

이러한 비전 센서를 기반으로 물체 인식 기술을 바탕으로 하여 로봇의 자기 위치를 추정(estimation)하고자 한 종래의 기술은 다음과 같다.

국내의 특허로는 물체에 액티브 태그를 붙여 이를 비전 센서로 인식하고 이를 물체와 로봇 사이의 위치를 알고자 한 특허(공개번호 10-2006-0102389)와 비전기반의 이동로봇의 위치 제어 시스템 특허(공개 번호 10-2005-0062713)가 있다. 첫 번째 특허는 로봇이 이동하는 경로에 있는 물체에 인공적 태그를 부착하여야 하는 불편함이 있으며, 두 번째 특허는 비전 센서가 로봇에 부착되지 아니하고 환경에 부착되어 단순히 로봇만을 인식하여 그 위치를 추정하는 방식이어서 환경에 따라 비전 센서가 필요하므로 다수의 비전 센서를 설치해야 한다는 단점이 있었다.

외국의 특허로는 미국의 비전 센서에 의해 자기위치 추정과 맵 구현을 동시에 수행하는 특허(미국 특허 US 7015831) 기술이 대표적이다. 그러나, 이 특허의 기술도 환경에서 비전 센서를 이용하여 물체 단위로 인식을 수행하지 아니하고 3차원상의 국소적 특징점들을 추출하여 이를 바탕으로 자기위치를 추정함으로써 향후 환경 변화의 요인에 대처할 수 있는 수단을 상실하여 기본적으로 레이저 센서를 이용하는 것과 별반 차이가 없다는 단점이 있었다.

그리고, 공개된 논문들 중에서 본 특허와 유사한 기술은 다음과 같다.

일본 Yuta등이 물체인식을 바탕으로 물체 중심의 위상적 지도를 구성하고 로봇의 위치는 그러한 물체를 1개씩 인식함과 동시에 로봇이 움직여 가면서 또 다른 물체를 인식하여 로봇의 위치를 추정하는 방법을 제시하였다 (M. Tomoyo and S. Yuta, "Object-based localization and mapping using loop constraints and geometric prior knowledge," International Conference on Robotics and Automation, pp. 862~867. 2003.). 이러한 방법은 로봇의 자기위치추정의 방법에 있어서 국소적 위치 추적(local localization tracking)에 해당하는 방법으로서, 로봇이 임의의 위치로 갑자기 움직여졌거나, 국소적 위치 추정의 실패로 새로 로봇의 위치를 추정하고자 하는 전역적 자기위치 추정에는 적용이 어렵다는 문제점이 있었다.

한국 박순용 등이 실내 환경을 국소 영역 중심의 위상적 지도로 표상하고 각 국소 영역에 포함된 물체를 인식하여 로봇이 위치한 국소 영역을 알아내고 그 국소 영역에 정의된 국소 좌표계에 대한 로봇의 위치를 계산하는 방법을 제시하였다 (S. Park, M. Park, and S.-K. Park, "Vision-based Global Localization for Mobile Robots with an Object Entity-based Hybrid Map," International Conference on Advanced Robotics, pp.761~766, 2007.). 이러한 방법은 로봇의 위치가 인식된 물체의 3차원 정보만을 사용하여 계산되므로 인식된 물체의 3차원 정보의 개수가 적 을 경우 계산된 위치가 부정확할 수 있다는 문제점이 있었다.

본 발명은 상기한 바와 같은 종래 기술의 불편함을 해결하기 위하여 안출된 것으로서,

본 발명의 목적은 로봇의 환경 인식을 위한 물체와 물체를 포함하는 환경 정보는 미리 위상학적 지도 형태로 3차원 정보와 함께 저장이 된 상태에서 비전 센서에 의하여 환경 내의 물체 인식을 수행하고 물체가 인식되는 경우 물체와 물체를 포함하는 환경의 3차원 정보를 사용하여 로봇의 자기위치를 추정하는 방법을 제공하는 것이다.

또한 본 발명의 다른 목적은 비전 센서에 의한 물체 인식 시간이 로봇이 주행을 하면서 위치 추정에 적용하기에는 어렵다는 전제 아래, 로봇이 환경의 개별적 물체를 인식하고 그를 바탕으로 위치를 추정(estimation)하도록 하여 로봇이 정지하여 있는 위치에서 전역적 위치 추정하는 로봇의 자기위치 추정 방법을 제공하는 것이다.

상기한 목적을 달성하기 위하여 본 발명에서는,

제1 측면으로서, 스테레오 영상장치로 형성된 카메라부와, 로봇 이동 경로 주위의 지도가 저장된 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서, 상기 카메라부에서 로봇 주위의 영상을 획득하는 1 단계와; 상기 위치 연산부에서 상기 카메라부가 획득한 영상 내의 개별 물체를 인식하고, 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와; 상기 위치 연산부에서 상기 데이터베이스부에 저장되어 있는 지도 및 상기 2 단계의 결과를 바탕으로 로봇의 위치를 추정하는 3 단계를 포함하는 로봇의 자기 위치 추정 방법이 제시되며,

제1 측면에 있어서, 상기 3단계는, 상기 데이터베이스에 저장된 지도로부터 상기 2단계에서 인식한 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 후보노드 선정 단계와; 상기 2 단계에서 생성된 위치값을 이용하여 선정된 모든 후보 노드에 대한 로봇의 잠정 위치를 계산하는 잠정 위치 계산 단계와; 계산된 잠정 위치를 초기 위치값으로 하여 선정된 모든 후보 노드에 파티클 필터링 기법을 적용하여 로봇의 위치를 추정하는 위치 추정 단계를 포함하여 이루어지는 것을 특징으로 하고,

제1 측면에 있어서, 상기 3 단계는, 상기 데이터베이스에 저장된 지도로부터 상기 2단계에서 인식한 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 후보노드 선정 단계와; 상기 2 단계에서 생성된 위치값을 이용하여 선정된 모든 후보 노드에 대한 로봇의 잠정 위치를 계산하는 잠정 위치 계산 단계와; 후보 노드 중에서 최종 후보 노드를 선택하는 최종 후보 노드 선택 단계와; 최종 후보 노드에 대한 로봇의 잠정 위치를 초기 위치값으로 하여 최종 후보 노드에 파티클 필터링 기법을 적용하여 로봇의 위치를 추정하는 위치 추정 단계를 포함하여 이루어지는 것을 특징으로 하고,

제2 측면으로서, 360도보다 작은 시야각을 갖고 스테레오 영상장치로 구성된 카메라부와, 상기 카메라부를 일정 각도씩 회전시킬 수 있는 회전 장치와, 로봇 이동 경로 주위의 지도를 저장하고 있는 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서,

상기 카메라부가 일정 방향의 로봇 주위의 영상을 획득하는 1 단계와; 상기 위치 연산부에서 상기 카메라부가 획득한 영상 내에서 인식한 물체가 없는 경우에는 상기 회전 장치를 구동하여 카메라를 회전시킨 후 다른 방향의 영상을 획득하는 과정을 획득된 영상에서 인식한 개별물체가 있을 때까지 반복하고, 인식한 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와; 상기 위치 연산부에서 상기 데이터베이스부에 저장된 지도로부터 상기 인식된 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 3 단계와; 상기 위치 연산부에서 후보 노드들의 우선 순위를 결정하는 4 단계와; 상기 위치 연산부에서 후보 노드에 포함된 물체와 로봇 간의 거리 및 최우선 순위 물체를 계산하는 5 단계와; 상기 위치 연산부에서 상기 회전 장치로 카메라부를 회전시켜 후보 노드의 최우선 순위 물체를 확인하는 6 단계와, 상기 위치 연산부에서 상기 6 단계에서 최우선 물체가 확인된 노드에 파티클 필터링 기법을 사용해서 로봇의 위치를 추정하는 7 단계를 포함하는 것을 특징으로 하는 로봇의 자기위치 추정 방법이 제시되고,

제2 측면에 있어서, 상기 6 단계는 후보 노드의 최우선 물체를 인식하지 못 하면 상기 회전 장치로 카메라부를 회전시켜 상기 4단계에서의 해당 후보 노드 중 다음 우선 순위의 후보 노드의 최우선 물체를 확인하는 것을 특징으로 한다.

본 발명에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법은 환경 내의 개별 물체들을 스테레오 비전 센서에 의해 인식을 하고 그에 대한 3차원 정보뿐만 아니라 그 주변 환경의 3차원 정보를 함께 사용함으로써 로봇의 정확한 위치를 추정할 수 있다. 또한 임의로 위치가 변하는 물체(예를 들면, 사람, 의자)가 많은 일반적인 환경에서 더욱 강인한 위치 추정을 할 수 있는 효과가 있다.

이하, 실시예를 통하여 본 발명에 따른 물체인식 및 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 시스템과 방법을 보다 구체적으로 살펴본다.

도1은 본 발명에 따른 물체인식 및 주변 환경 정보를 바탕으로 자기위치를 추정하는 로봇의 구조도이다.

본 발명에 따른 물체인식 및 주변 환경 정보를 바탕으로 자기위치를 추정하는 로봇은 도1에 도시된바와 같이 카메라부(100)와 로봇 본체(200)와 로봇 이동부(300)를 포함하여 구성된다.

상기 카메라부(100)는 로봇의 위치를 추정하기 위한 수단으로서 스테레오 영상 입력 장치로 구성된다.

상기 카메라부(100)는 일정 범위의 시야각을 갖는 카메라를 사용할 수도 있 고, 전방향의 시야각을 갖는 카메라를 사용할 수도 있다. 다만 일정 범위의 시야각을 갖는 카메라를 사용할 경우에는 도1에 도시된바와 같이 카메라를 일정 각도씩 회전시킬 수 있는 팬/틸트 장치(110)를 추가하여 카메라를 회전시킴으로써 전방향의 영상을 입력받을 수 있다.

상기 팬/틸트 장치(110)는 수평으로의 좌우 회전하는 패닝(Panning)과 상하의 끄덕이는 동작인 틸팅(tilting)이 가능한 회전 장치이다.

로봇 본체(200)는 외부 동작을 하기 위한 기계 팔인 머니퓨레이터(210)가 외부에 장착되고, 내부에 로봇 이동 경로 주위의 지도가 저장된 데이터베이스부(미도시)와 상기 카메라부(100)에서 입력되는 영상 정보를 상기 데이터베이스부에 저장된 지도와 대조하여 물체를 인식하고 로봇의 자기 위치를 추정하는 위치 연산부(미도시)를 포함한다.

상기 데이터베이스부에 저장된 지도는 미리 위상학적 지도 형태로 제작되며 지도상의 각 물체들의 3차원 위치 및 물체를 포함하는 주변 환경의 국소 특징점들의 3차원 위치와 함께 저장된다.

로봇 이동부(300)는 로봇을 이동 경로에 따라 이동시키는 구성으로 바퀴나 캐터필러를 비롯하여 로봇의 본체를 이동시킬 수 있는 모든 장치를 사용할 수 있다.

도2는 본 발명에 따른 물체인식 및 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 기본 순서도이다.

본 발명에 따른 물체인식 및 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법은 도2에 도시된 바와 같이 카메라부(100)로부터 영상을 입력을 받는 영상 입력 단계(S110)와, 상기 영상 입력 단계(S110)에서 입력된 영상 안의 개별 물체들을 인식하고 또한 개별 물체들의 3차원 위치 및 물체를 포함하는 주변 환경의 국소 특징점들의 3차원 위치 정보를 생성하는 물체인식 및 정보 추출 단계(S120)와, 로봇의 위치 추정 수단이 상기 물체인식 및 정보 추출 단계(S120)에서 추출된 정보를 이용하여 미리 등록되어 있는 물체 기반의 지도를 바탕으로 로봇의 위치를 추정하는 로봇 위치 추정단계(S130)를 포함한다. 따라서, 본 발명에서는 기본적으로 먼저 환경 내의 개별 물체를 인식하고, 이를 바탕으로 물체와 물체를 포함하는 주변 환경의 3차원 거리 정보를 추출하여 로봇의 위치를 추정한다고 할 수 있다.

본 발명에서는 국소 특징점들의 집합으로 물체 인식을 수행하는데, 카메라부(100)를 이용하여 획득한 환경 영상에서 국소 특징점들을 추출하고 이 특징점들과 상기 데이터베이스부에 저장된 물체 모델의 국소 특징점들과 비교하는 방법을 사용한다.

도 3은 로봇이 인식한 물체(410)와 인식된 물체를 포함하는 주변 환경(400)을 도시한다.

는 도 4에 도시된 바와 같이 물체와 물체를 포함하는 주변 환경의 국소 특징점들과 카메라부(100)간의 3차원 거리값이고,

는 도 5에 도시된 바와 같이 도 4에서의 물체 및 물체를 포함하는 주변 환경의 국소 특징점들과 환경 지도 중심 좌표계 간의 3차원 거리값이며,

과

의 관계는 도 6에 도시된 바와 같다.

이러한 두 거리값들의 동차 변환행렬은 하기된 수학식 1에서와 같이

로 나타낼 수 있으며, 이들 사이의 관계식은 하기된 수학식 2와 같다.

따라서, 로봇이 임의의 위치에서

를 계산할 수 있고, 또한 환경지도 중심으로 인식된 물체와 물체를 포함하는 주변 환경의 거리 정보

를 알고 있으면, 수학식 2를 통해

를 추정할 수 있고, 그로부터 로봇의 위치를 수학식 1에 따라 추정할 수 있게 된다. 3차원 공간상의 임의의 위치라면 그 로봇 위치는

로 나타낼 수 있으며, 일정한 평면상에서 동작하는 로봇이라면, 이

되어 로봇 위치를 결정하는 변수는

가 된다. 따라서, 대응되는 국소 특징점들이 3점 이상이면 수학식 2에서

를 결정할 수 있게 된다.

,

여기서, 벡터 [ r₁₁ r₂₁ r₃₁ ]^T는 카메라좌표계의 x축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₂ r₂₂ r₃₂ ]^T는 카메라좌표계의 y축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₃ r₂₃ r₃₃ ]^T는 카메라좌표계의 z축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ t_x t_y t_z ]^T는 카메라좌표계의 위치좌표를 환경지도중심좌표계에 대해 표시한 것이고,

는 arc의 a를 의미하고, atan2(f,g)는 tan^-1(f/g)를 나타낸다.

상기 로봇 위치 추정단계(S130)에서 로봇의 위치를 추정하는데 사용되는 지도는 로봇 본체(200)의 데이터베이스부에 저장된 지도로서 도 7에 도시된 바와 같이 로봇의 주행 경로를 중심으로 한 위상학적 지도(Topological map)이다.

도 7에서 A, B, C 등이 노드(node)에 해당하고, 각 노드는 독립적인 기준 좌표계를 갖고 있으며, 각 노드를 연결하는 에지(edge)는 각 노드 좌표계의 상대적인 거리 정보를 가지고 있다. 그런데, 이러한 환경 지도를 고려해 보면 로봇이 물체 하나만을 인식하여 그 물체 내부의 국소 특징점과 그 물체 주변 환경의 국소 특징점만을 가지고 로봇 위치를 추정하게 되면, 2개 이상의 물체를 기반으로 위치를 추정하는 것보다는 부정확할 수밖에 없음을 알 수 있으며, 특히 로봇의 회전 각도의 오차가 클 수밖에 없음을 알 수 있다. 따라서, 본 발명에서는 복수개의 물체 인식을 바탕으로 로봇의 위치 추정을 하고자 한다.

또한, 위상학적 지도 기반과 물체인식 중심의 위치 추정을 하게 되는 경우, 도 7에 도시된 바와 같이 특정의 물체가 특정의 노드에서만 관측되는 것이 아니다. 예를 들어 물체-a, 물체-b 및 물체-c등은 노드-L과 노드-N에서 모두 관측이 가능하 다.

즉, 이는 물체 관측이 이루어진다고 해서 로봇이 어느 노드 근방에 있다는 것을 알려주는 것이 아니기 때문에 물체 위치를 특정시켜 주는 기준 노드를 결정해 주어야 하는 문제도 있다. 본 발명에서는 이러한 위치 추정의 방법을 아래와 같은 파티클 필터링 (particle filtering, M. Isard and A. Blake, "Condensation - conditional density propagation for visual tracking," International Journal of Computer Vision 29(1), pp. 5-28, 1998) 기법을 적용하여 해결한다.

도 8은 파티클 필터링에 기반한 위치 추정 방법의 순서도이다.

이러한 파티클 필터링에 기반한 위치 추정 방법은 도 8에 도시된 바와 같이 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)와 반복 조건 확인 단계(S240)와 위치 추정 단계(S250)를 포함한다.

상기 샘플링 단계(S210)는 특정 물체가 관측되면 해당 물체를 관측할 수 있는 곳에 로봇이 있는 것으로 추정되므로 해당 물체를 관측할 수 있는 로봇 위치들에 대하여 이를 랜덤 샘플링(random sampling)하여 생성하는 단계이다.

상기 샘플링 단계(S210)에서 반복 스텝을 t, 랜덤샘플의 번호는 n이라고 하면,

- t = 1 인 경우 (시작 초기)

여기서, n = 1 ~

이다.

는 랜덤 샘플의 총 개수를 의미한다.

- t > 1 인 경우

바로 전 스텝 step t-1의 샘플집합

으로부터 현 반복스텝 t 의 새로운 샘플집합

을 다음과 같이 선택한다. 여기서

은 샘플

의 반복스텝 t에서의 가중치이고,

는 누적 가중치이다.

(a) 균일 확률 분포에 의한 임의의 수

을 생성한다.

(b)

에 대한 가장 작은 j를 찾는다.

(c)

로 설정하고, 하기된 수학식4와 같이 정의한다.

상기 오차계산 단계(S220)는 상기 샘플링 단계(S210)에서 생성된 위치 샘플에 로봇의 이동에 따른 동작 오차와 비전 센서의 측정 오차 및 물체 인식의 오류등으로 인해 발생하는 오차 범위를 하기된 수학식 5를 사용하여 적용하는 단계이다.

여기서,

는 로봇의 동적 모델을 나타낸다. 그러나 로봇은 정지 상태에서 위치 추정을 수행하므로

로 설정한다.

는 비전 센서의 측정 오차와 물체 인식의 오류 등으로 인해 발생하는 로봇 위치 측정 오차이다.

상기 가중치 계산 단계(S230)는 상기 오차계산 단계(S220)에서 예측한 위치에 대한 무작위 샘플들의 가중치를 하기된 수학식 6에 준하여 구하는 단계이다.

여기서, 식의 우변은 가정된 샘플 위치를 정확한 위치라고 가정하고 환경 지도로부터 얻은 물체들의 위치와 센서에 의하여 얻어진 측정값들과의 유사측도를 설정하여 얻을 수 있으며, 그 값은

이 되도록 정규화할 수 있다.

이와 함께 누적가중치

을 계산한다.

상기 반복 조건 확인 단계(S240)는 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)가 일정 조건을 만족하는 동안 반복 수행되었나를 확인하는 단계로 반복 조건은 균일한 일정 횟수(T)가 될 수도 있고, 상기 수학식 6에서 샘플들중 제일 큰 가중치 값을 갖는 샘플의 가중치 값이 어느 문턱값(예를 들면, 정규화된 경우 0.9)이하라는 조건을 사용할 수도 있으며, 또한 상기 2가 지 방법을 OR로 조합하여 T회 반복하였거나 제일 큰 가중치 값을 갖는 샘플의 가중치 값이 어느 문턱값(예를 들면, 정규화된 경우 0.9)이상이면 반복수행을 정지하게도 할 수 있다.

상기 위치 추정 단계(S250)는 샘플들은 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)를 T회 반복하게 되면 샘플들이 하나의 후보 위치로 수렴하므로, 가중평균을 이용하여 그 위치를 하기된 수학식 8과 같이 추정하는 단계이다.

여기서, [S]는 추정된 로봇의 위치이고,

는 가정할 수 있는 로봇 위치의 샘플이고,

은 샘플

의 반복스텝 t에서의 가중치이다.

도 9는 본 발명의 제1 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

본 발명의 1실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법은 카메라부(100)에 장착되는 센서가 전방향 스테레오 비전 센서인 경우의 실시예로 도 9에 도시된 바와 같이 전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S310)와, 물체 인식 단계(S320)와, 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S330)와, 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)와, 모든 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S350)와, 최적 노드 및 최적 노드에 대한 로봇의 2차원 위치 확정 단계(S360)를 포함한다.

상기 전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S310)는 카메라부(100)가 로봇 주위의 전방향 환경 영상 및 그에 대하여 스테레오 비전 기반하에 거리 정보를 획득하는 단계이다.

상기 물체 인식 단계(S320)는 위치 연산부가 상기 전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S310)에서 획득한 전방향 환경 영상에서 물체 및 물체를 포함하는 주변 환경의 국소 특징점을 추출하고, 추출된 국소 특징점의 3차원 위치값(카메라 좌표계 기준)을 스테레오 거리 정보를 이용하여 구하고, 로봇 본체(200)의 데이터베이스부에 저장되어 있는 물체 모델의 국소 특징점들과 비교하여 물체를 인식하는 단계이다.

상기 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S330)는 위치 연산부가 데이터베이스부에 저장된 지도에서 상기 물체 인식 단계(S320)에서 인식된 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 단계이다.

상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)는 위치 연산부가 모든 후보 노드 좌표계에 대한 로봇의 잠정 위치를 계산하고, 계산된 로봇의 위치를 사용하여 노드 근접 가중치를 계산하는 단계이다.

상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)에서 계산되 는 로봇의 잠정 위치는 상기 수학식 1과 수학식 2로부터 구할 수 있으며, 매칭된 국소적 특징점들이 3점 이상이면 최소자승법을 이용하여 계산을 수행한다.

이때 수학식 2의

는 노드 좌표계 {N}에 대한 로봇 좌표계 {R}의 pose 관계

를 나타내는 기호가 된다.

이렇게 얻어진 잠정위치는 다음 단계의 파티클 필터링에 의하여 최종적으로 정확한 위치를 추정하기 위한 초기 위치값으로 사용된다.

상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)에서 위치 연산부는 수학식 6에 해당하는 가중치 중에서 기본적으로 인식된 물체와 물체를 포함하는 주변 환경을 기준으로 로봇이 어느 노드에 가까이 있을지를 나타내 주는 노드 근접 가중치

를 계산한다.

상기 노드 근접 가중치는 물체 존재 가중치, 물체-환경 특징 가중치, 및 물체-환경 거리 가중치를 사용하여 계산되는 값이며, 이 중 물체-환경 거리 가중치를 계산하는 데에 각 후보 노드에 대한 로봇의 3차원 위치가 사용된다.

상기 물체 존재 가중치는 후보 노드에 포함된 물체 중 로봇이 인식한 물체와 겹치는 수에 따라 변화하는 가중치로 하기된 수학식 9에 따라 계산된다.

여기서,

는 후보 노드

의 물체 존재 가중치이고,

는 후보 노드의 개수이다.

은 로봇이 인식한 물체의 집합,

는 후보 노드

에 존재하는 물체의 집합이고,

는 로봇이 인식한 물체와 후보 노드

에 존재하는 물체 중 동일한 물체의 개수이다.

상기 물체-환경 특징 가중치는 후보 노드에 저장된 물체 모델의 국소 특징점과 그 물체를 포함하는 주변 환경에서 추출한 환경 모델의 국소 특징점을 로봇이 인식한 물체와 그 물체를 포함하는 주변환경의 국소적 특징점의 매칭 정도( 매칭된 국소적 특징점의 개수 등)에 따른 가중치로 하기된 수학식10에 따라 계산된다.

여기서,

는 후보 노드

의 물체-환경 특징 가중치이고,

은 로봇이 인식한 모든 물체들의 국소적 특징점과 그 물체들을 포함하는 주변 환경의 국소적 특징점의 집합이고,

는 후보 노드

에 포함된 물체들의 국소적 특징점과 그 물체들을 포함하는 주변환경의 국소적 특징점의 집합이다.

는 로봇이 인식한 모든 물체의 국소적 특징점과 그 물체들을 포함하는 주변 환경의 국소적 특징점의 총 개수,

는 로봇이 인식한 모든 물체 및 그 물체를 포함하는 주변환경 의 국소적 특징점과 후보 노드

에 존재하는 물체 중 로봇이 인식한 물체와 동일한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점 간에 매칭된 국소적 특징점의 개수이다.

상기 물체-환경 거리 가중치는 로봇이 인식한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점의 3차원값을 후보 노드의 동일 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점의 3차원 값으로 보간(fitting)하고, 보간된 국소적 특징점의 3차원 값과 후보 노드의 동일 국소적 특징점 3차원 값들 간의 거리 차이로 하기된 수학식 11에 따라 계산된다.

여기서,

는 후보 노드

의 물체-환경 거리 가중치이고,

는 보간(fitting)된 국소적 특징점의 3차원값과 후보 노드의 동일 국소적 특징점의 3차원값 간의 거리 차이다.
또한,

는 로봇이 인식한 모든 물체 및 그 물체를 포함하는 주변환경의 국소적 특징점과 노드

에 존재하는 물체 중 로봇이 인식한 물체와 동일한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점 간에 매칭된 국소적 특징점 각각에 대한

의 평균값이다.

상기 노드 근접 가중치는 로봇의 그 후보 노드에 대한 근접 정도를 나타내는 가중치로 상기된 수학식 9,10,11에 따라 계산된 물체 존재 가중치, 물체-환경 특징 가중치, 물체-환경 거리 가중치를 통합하여 하기된 수학식 12에 따라 계산된다.

여기서,

는 노드

에 대한 로봇의 노드 근접 가중치이다.

모든 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S350)는 위치 연산부가 상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)에서 계산된 잠정 위치에 대하여 도7에 도시된 파티클 필터링 알고리즘 중 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)와 반복 조건 확인 단계(S240)를 수행하고, 정확한 위치 추정에 필요한 최종 가중치를 계산하는 단계이다.

상기 모든 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S350)에서 파티클 필터링 방법을 적용하는데 있어서 상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)에서 계산된 노드 근접 가중치

와 로봇이 인식한 물체 및 그 물체를 포함하는 환경 영상의 3차원 특징점과 후보 노드에 존재하는 동일한 물체 및 그 물체를 포함하는 환경 영상의 3차원 특징점 간의 거리 차이에 의한 샘플

의 3차원 특징점 가중치

를 하기된 수학식 13와 같이 통합하여 샘플

의 최종 가중치

을 구한다.

상기 3차원 특징점 가중치를 구하기 위해서는 일단 후보 노드

에 대한 랜덤 샘플

의 위치를 다음과 같이 3차원 동차변환 행렬(3D homogeneous transformation matrix) 형태로 나타낸다.

그리고, 로봇이 인식한 물체 및 그 물체를 포함하는 환경 영상의 3차원 특징점

를 하기된 수학식 14를 이용하여 다음과 같이 후보 노드

에 대한 값으로 변환한다.

그 다음에 상기 수학식 14와 같이 변환된 3차원 특징점

와 후보 노드

에 존재하는 동일 물체 및 그 물체를 포함하는 환경 영상의 3차원 특징점

간의 거리 차이를 하기된 수학식 15에 따라 계산한다.

로봇이 인식한 물체 및 그 물체를 포함하는 주변환경의 3차원 특정점과 후보 노드에 존재하는 동일한 물체 및 그 물체를 포함하는 주변 환경의 3차원 특징점 간의 거리 차이에 의한 샘플

의 3차원 특징점 가중치

를 하기된 수학식 16에 따라 계산한다.

여기서,

는 로봇이 인식한 모든 물체 및 그 물체를 포함하는 주변환경의 국소적 특징점과 후보 노드

에 존재하는 물체 중 로봇이 인식한 물체와 동일한 물체 및 그 물체를 포함하는 주변환경의 국소적 특징점 간에 매칭된 국소적 특징점의 개수이다.

상기 최적 노드 및 최적 노드에 대한 로봇의 2차원위치 확정 단계(S360)는 도8에 도시된 위치 추정 단계(S250)와 동일한 단계로 샘플들이 제일 많이 수렴된 노드의 샘플만으로 상기 수학식 8을 수행하여 로봇의 위치를 추정하는 단계이다.

도 10은 본 발명의 제2 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

본 발명의 2실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법은 카메라부(100)에 장착되는 센서가 전방향 스테레오 비전 센서이며 로봇이 속하는 최종 노드를 중간에서 미리 정하는 경우의 실시예로 도10에 도시된 바와 같이 전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S410)와, 물체 인식 단계(S420)와, 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S430)와, 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S440)와, 최종 후보 노드 선정 단계(S450)와, 최종 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S460)와, 최적 노드 및 최적 노드에 대한 로봇의 2차원위치 확정 단계(S470)를 포함한다.

전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S410)와, 물체 인식 단계(S420)와, 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S430)와, 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S440)는 상기 제1실시예의 전방향 환경 이미지 및 스테레오 깊이 정보 획득 단계(S310)와, 물체 인식 단계(S320)와, 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S330)와, 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)와 동일하므로 상세한 설명은 생략한다.

최종 후보 노드 선정 단계(S450)는 상기 모든 후보 노드에 대한 로봇의 3차 원 위치 계산 단계(S440)에서 계산된 노드 근접 가중치 값이 가장 큰 노드를 최종 후보 노드로 선정하는 단계이다.

상기 최종 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S460)는 위치 연산부가 상기 최종 후보 노드 선정 단계(S450)에서 선택된 최종 후보 노드에 도8에 도시된 파티클 필터링 알고리즘중 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)와 반복 조건 확인 단계(S240)를 수행하고, 정확한 위치를 추정에 필요한 최종 가중치를 계산하는 단계이다.

상기 최적 노드 및 최적 노드에 대한 로봇의 2차원위치 확정 단계(S470)는 상기 최종 후보 노드 선정 단계(S450)에서 선택되고 상기 최종 후보 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S460)에서 최종 가중치가 계산된 노드들에 상기 수학식 8을 수행하여 로봇의 위치를 추정하는 단계이다.

도 11은 본 발명의 제3 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

본 발명의 3실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법은 카메라부(100)에 장착되는 센서가 스테레오 카메라가 360도가 아닌 일정의 시야각을 갖는 스테레오 카메라인 경우에 로봇의 위치 추정 시간을 단축하기 위하여 환경에서 2개의 물체를 인식하는 방법을 바탕으로 로봇의 위치를 추정하는 실시예로 도 11에 도시된 바와 같이 임의의 한 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S510)와, 물체 인식 단계(S515)와, 인식된 물체가 있는가 확인 단계(S520)와, 인식된 물체가 1개 이상 존 재하는 후보 노드 선정 단계(S525)와, 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S530)와, 후보 노드의 우선 순위 계산 단계(S535)와, 후보노드에 포함된 물체들의 로봇에 대한 위치 계산 및 우선 순위 계산 단계(S540)와, 최우선 순위 후보 노드의 최우선 순위 물체를 바라보는 단계(S545)와, 그 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S550)와, 물체 인식 단계(S555)와, 인식된 물체가 최우선 순위 물체와 같은가 확인 단계(S560)와, 다음 우선 순위 후보 노드의 최우선 순위 물체를 바라보는 단계(S562)와, 최종 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S565)와, 최종 노드에 대한 로봇의 2차원위치 확정 단계(S570)를 포함한다.

상기 임의의 한 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S510)는 카메라부(100)가 향하고 있는 방향의 환경 영상 및 그에 대하여 스테레오 비전 기반하에 거리 정보를 획득하는 단계이다.

상기 물체 인식 단계(S515)는 위치 연산부가 상기 임의의 한 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S510)에서 획득한 환경 영상에서 물체 및 물체를 포함하는 주변 환경의 국소적 특징점을 추출하고, 추출된 국소 특징점에 대한 3차원 위치값(카메라 좌표계 기준)을 스테레오 거리 정보를 이용하여 구하고, 로봇 본체(200)의 데이터베이스부에 저장된 물체 모델의 국소 특징점들과 비교하여 물체를 인식하는 단계이다.

상기 인식된 물체가 있는가 확인 단계(S520)는 위치 연산부가 상기 물체 인식 단계(S515)에서 인식된 물체가 있는가를 확인하고 인식된 물체가 없으면 팬/틸 트 장치(110)를 동작하여 카메라부(100)를 회전(panning)시켜 다른 방향을 향하도록 하는 단계이다.

상기 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S525)는 위치 연산부가 상기 데이터베이스부에 저장된 지도에서 상기 물체 인식 단계(S515)에서 인식된 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 단계이다.

상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S530)는 상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S340)와 동일하므로 상세한 설명은 생략한다.

상기 후보 노드의 우선 순위 계산 단계(S535)는 상기 모든 후보 노드에 대한 로봇의 3차원 위치 계산 단계(S530)에서 계산된 노드 근접 가중치 값의 값에 따라 각각 노드의 우선 순위를 계산하는 단계이다.

상기 후보노드에 포함된 물체들의 로봇에 대한 위치 계산 및 우선 순위 계산 단계(S540)는 위치 연산부가 로봇이 인식한 물체를 제외하고, 상기 인식된 물체가 1개 이상 존재하는 후보 노드 선정 단계(S525)에서 선택된 후보 노드들에 포함된 물체들의 로봇에 대한 위치를 계산하여 로봇의 잠정 위치로부터 제일 가까운 물체를 검증 물체로 정하는 단계이다.

상기 최우선 순위 후보 노드의 최우선 순위 물체를 바라보는 단계(S545)는 위치 연산부가 팬/틸트 장치(110)를 동작하여 상기 후보노드에 포함된 물체들의 로봇에 대한 위치 계산 및 우선 순위 계산 단계(S540)에서 선택된 검증 물체가 있을 것이라고 예측되는 방향으로 카메라부(100)를 회전시키는 단계이다.

상기 최우선 순위 후보 노드의 최우선 순위 물체를 바라보는 단계(S545)는 위치 추정부는 팬/틸트 장치(110)를 사용하여 카메라를 수평으로의 좌우 회전하는 거나 상하의 끄덕이는 동작을 할 수 있다.

상기 그 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S550)는 카메라부(100)가 향하고 있는 방향의 환경 영상 및 그에 대하여 스테레오 비전 기반하에 거리 정보를 획득하는 단계이다.

상기 물체 인식 단계(S555)는 위치 연산부가 상기 그 방향의 환경 이미지 및 스테레오 깊이 정보 획득 단계(S550)에서 획득한 환경 영상에서 국소적 특징점을 추출하고, 로봇 본체(200)의 데이터베이스부에 저장된 최우선 순위 물체의 물체 모델의 국소 특징점들과 비교하여 물체를 인식하는 단계이다.

상기 인식된 물체가 최우선 순위 물체와 같은가 확인 단계(S560)는 위치 연산부가 상기 물체 인식 단계(S555)에서 인식된 물체와 상기 후보노드에 포함된 물체들의 로봇에 대한 위치 계산 및 우선 순위 계산 단계(S540)에서 선택된 검증 물체를 비교하여 같으면 그 후보 노드를 최종 노드로 선정하는 단계이다.

다음 우선 순위 후보 노드의 최우선 순위 물체를 바라보는 단계(S562)는 위치 연산부가 상기 인식된 물체가 최우선 순위 물체와 같은가 확인 단계(S560)에서 인식된 물체와 최우선 순위 물체가 다르면 상기 후보 노드의 우선 순위 계산 단계(S535)에서 지금 카메라부(100)가 향하고 있는 노드의 다음 순위의 우선 순위를 받은 후보 노드의 상기 후보노드에 포함된 물체들의 로봇에 대한 위치 계산 및 우 선 순위 계산 단계(S540)에서 계산된 검증 물체로 카메라부(100)를 이동시키는 단계이다.

즉, 예를 들어, 후보 노드가 A, B 2개 이고 우선순위가 A, B 인 경우, 최우선 순위 노드는 A가 된다. 이미 인식된 물체를 제외한 노드 A에 속한 물체가 a1, b1, c1이고 물체의 우선순위가 a1, b1, c1이면, 로봇은 a1이 있을 거라 예측한 방향으로 고개를 돌리고 물체 인식을 수행한다. 만약 물체 a1이 인식되지 않았을 경우, 그 다음 우선순위인 후보 B에 속한 최우선 순위 물체를 바라보도록 고개를 돌리고 물체 인식을 수행하도록 한다.

상기 최종 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S565)는 위치 연산부가 상기 인식된 물체가 최우선 순위 물체와 같은가 확인 단계(S560)에서 선정된 최종 노드에 도8에 도시된 파티클 필터링 알고리즘 중 샘플링 단계(S210)와 오차계산 단계(S220)와 가중치 계산 단계(S230)와 반복 조건 확인 단계(S240)를 수행하고, 정확한 위치를 추정에 필요한 최종 가중치를 계산하는 단계이다.

상기 최종 노드에 대한 로봇의 2차원위치 확정 단계(S570)는 상기 인식된 물체가 최우선 순위 물체와 같은가 확인 단계(S560)에서 선택되고 상기 최종 노드에 샘플링하여 파티클 필터링 알고리즘을 수행하는 단계(S565)에서 최종 가중치가 계산된 노드들에 상기 수학식 8을 수행하여 로봇의 위치를 추정하는 단계이다.

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

도2는 본 발명에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 기본 순서도이다.

도3은 본 발명에 따른 로봇이 인식한 물체와 인식된 물체를 포함하는 주변 환경 개념도이다.

도4는 본 발명에 따른 로봇 중심의 물체 인식 및 물체 위치, 인식된 물체를 포함하는 주변 환경 위치 개념도이다.

도5는 본 발명에 따른 환경 중심의 물체 위치 및 그 물체를 포함하는 주변 환경 위치 개념도이다.

도6은 본 발명에 따른 로봇 중심의 물체 및 인식된 물체를 포함하는 주변 환경 위치와 환경 중심의 물체 및 그 물체를 포함하는 주변 환경 위치간의 관계도이다.

도7은 본 발명에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법에서 사용되는 위상학적 지도의 예시도이다.

도8은 파티클 필터링에 기반한 위치 추정 방법의 순서도이다.

도9는 본 발명의 제1 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

도10은 본 발명의 제2 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

도11은 본 발명의 제3 실시예에 따른 물체인식 및 인식된 물체를 포함하는 주변 환경 정보를 바탕으로 한 로봇의 자기위치 추정 방법의 순서도이다.

Claims

삭제
스테레오 영상장치로 형성된 카메라부와, 로봇 이동 경로 주위의 지도가 저장된 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서,

상기 카메라부에서 로봇 주위의 영상을 획득하는 1 단계와;

상기 위치 연산부에서 상기 카메라부가 획득한 영상 내의 개별 물체를 인식하고, 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와;

상기 위치 연산부에서 상기 데이터베이스부에 저장되어 있는 지도 및 상기 2 단계의 결과를 바탕으로 로봇의 위치를 추정하는 3 단계를 포함하고,

상기 3단계는,

상기 데이터베이스에 저장된 지도로부터 상기 2단계에서 인식한 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 후보노드 선정 단계와;

상기 2 단계에서 생성된 위치값을 이용하여 선정된 모든 후보 노드에 대한 로봇의 잠정 위치를 계산하는 잠정 위치 계산 단계와;

계산된 잠정 위치를 초기 위치값으로 하여 선정된 모든 후보 노드에 파티클 필터링 기법을 적용하여 로봇의 위치를 추정하는 위치 추정 단계를 포함하여 이루어지는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.
청구항 2에 있어서,

상기 잠정 위치 계산 단계에서는 아래의 수학식 1과 2를 사용해서 로봇의 잠정위치를 계산하고,

상기 위치 추정 단계에서는 최종 가중치를 계산하여 파티클 필터링 기법에 적용하고, 샘플들이 제일 많이 수렴된 노드의 샘플만으로 아래의 수학식 8을 수행하여 로봇의 위치를 추정하는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 1 >

< 수학식 2>

( 여기서,
와
는 각각 동일물체 및 동일물체를 포함하는 주변 환경의 국소 특징점들에 대한 카메라 좌표계 위치값, 환경 지도 중심 좌표계 위치값이고,
는 이 두 위치값들의 동차 변환 행렬이며, 로봇 위치는
로 나타낼 수 있고,

벡터 [ r₁₁ r₂₁ r₃₁ ]^T는 카메라좌표계의 x축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₂ r₂₂ r₃₂ ]^T는 카메라좌표계의 y축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₃ r₂₃ r₃₃ ]^T는 카메라좌표계의 z축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ t_x t_y t_z ]^T는 카메라좌표계의 위치좌표를 환경지도중심좌표계에 대해 표시한 것이고,

는 arc의 a를 의미하고, atan2(f,g)는 tan^-1(f/g)를 나타낸다.)

< 수학식 8 >

( 여기서, [S]는 추정된 로봇의 위치이고,
는 가정할 수 있는 로봇 위치의 샘플이고,
은 샘플
의 반복스텝 t에서의 가중치이고,
는 랜덤 샘플의 총 개수를 의미한다.)
스테레오 영상장치로 형성된 카메라부와, 로봇 이동 경로 주위의 지도가 저장된 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서,

상기 카메라부에서 로봇 주위의 영상을 획득하는 1 단계와;

상기 위치 연산부에서 상기 카메라부가 획득한 영상 내의 개별 물체를 인식하고, 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와;

상기 위치 연산부에서 상기 데이터베이스부에 저장되어 있는 지도 및 상기 2 단계의 결과를 바탕으로 로봇의 위치를 추정하는 3 단계를 포함하고,

상기 3 단계는,

상기 데이터베이스에 저장된 지도로부터 상기 2단계에서 인식한 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 후보노드 선정 단계와;

상기 2 단계에서 생성된 위치값을 이용하여 선정된 모든 후보 노드에 대한 로봇의 잠정 위치를 계산하는 잠정 위치 계산 단계와;

후보 노드 중에서 최종 후보 노드를 선택하는 최종 후보 노드 선택 단계와;

최종 후보 노드에 대한 로봇의 잠정 위치를 초기 위치값으로 하여 최종 후보 노드에 파티클 필터링 기법을 적용하여 로봇의 위치를 추정하는 위치 추정 단계를 포함하여 이루어지는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.
청구항 4에 있어서,

상기 잠정 위치 계산 단계에서는 아래의 수학식 1과 2를 사용해서 로봇의 잠정위치를 계산하고,

상기 위치 추정 단계에서는 최종 가중치를 계산하여 파티클 필터링 기법에 적용하고, 아래의 수학식 8을 수행하여 로봇의 위치를 추정하는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 1 >

< 수학식 2>

( 여기서,
와
는 각각 동일물체 및 동일물체를 포함하는 주변 환경의 국소 특징점들에 대한 카메라 좌표계 위치값, 환경 지도 중심 좌표계 위치값이고,
는 이 두 위치값들의 동차 변환 행렬이며, 로봇 위치는
로 나타낼 수 있고,

벡터 [ r₁₁ r₂₁ r₃₁ ]^T는 카메라좌표계의 x축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₂ r₂₂ r₃₂ ]^T는 카메라좌표계의 y축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ r₁₃ r₂₃ r₃₃ ]^T는 카메라좌표계의 z축의 방향벡터를 환경지도중심좌표계에 대해 표시한 것이고, 벡터 [ t_x t_y t_z ]^T는 카메라좌표계의 위치좌표를 환경지도중심좌표계에 대해 표시한 것이고,

는 arc의 a를 의미하고, atan2(f,g)는 tan^-1(f/g)를 나타낸다.)

< 수학식 8 >

( 여기서, [S]는 추정된 로봇의 위치이고,
는 가정할 수 있는 로봇 위치의 샘플이고,
은 샘플
의 반복스텝 t에서의 가중치이고,
는 랜덤 샘플의 총 개수를 의미한다.)
청구항 4에 있어서,

상기 최종 후보 노드는, 후보 노드들에 대하여 아래의 수학식 12에 의해 구해진 노드 근접 가중치 값 중 가장 큰 값을 갖는 노드인 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 12 >

( 여기서,
는 노드
에 대한 로봇의 노드 근접 가중치이고,
는 노드
의 물체 존재 가중치이고,
는 노드
의 물체-환경 특징 가중치이 고,
는 노드
의 물체-환경 거리 가중치이고,
는 후보 노드의 개수이다.)
청구항 3 또는 5에 있어서,

상기 최종 가중치는 로봇이 어느 노드에 가까이 있을지를 나타내 주는 노드 근접 가중치
와, 로봇이 인식한 물체 및 그 물체를 포함하는 주변 환경의 3차원 특징점과 노드에 존재하는 동일한 물체 및 그 물체를 포함하는 주변 환경의 3차원 특징점 간의 거리 차이에 의한 샘플
의 3차원 특징점 가중치
를 아래의 수학식 13와 같이 통합하여 계산되는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 13 >

(여기서,
은 최종 가중치이고,
는 후보 노드의 개수이다.)
청구항 7에 있어서,

상기 노드 근접 가중치
는 아래의 수학식 12에 의하여 결정되는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 12 >

( 여기서,
는 노드
에 대한 로봇의 노드 근접 가중치이고,
는 노드
의 물체 존재 가중치이고,
는 노드
의 물체-환경 특징 가중치이고,
는 노드
의 물체-환경 거리 가중치이고,
는 후보 노드의 개수이다.)
청구항 8에 있어서,

상기 물체 존재 가중치
는 아래의 수학식 9에 따라 계산되는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 9 >

(여기서, 여기서,
는 노드
의 물체 존재 가중치이고,
은 로봇이 인식한 물체의 집합,
는 노드
에 존재하는 물체의 집합이고,
는 로봇이 인식한 물체와 노드
에 존재하는 물체 중 동일한 물체의 개수이다.)
청구항 8에 있어서,

상기 물체-환경 특징 가중치
는 아래의 수학식 10에 따라 계산되는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 10 >

( 여기서,
는 노드
의 물체-환경 특징 가중치이고,
은 로봇이 인식한 모든 물체들의 국소적 특징점과 그 물체들을 포함하는 주변 환경의 국소적 특징점의 집합이고,
는 노드
에 포함된 물체들의 국소적 특징점과 그 물체들을 포함하는 주변환경의 국소적 특징점의 집합이고,
는 로봇이 인식한 모든 물체 및 그 물체를 포함하는 주변환경의 국소적 특징점과 노드
에 존재하는 물체 중 로봇이 인식한 물체와 동일한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점 간에 매칭된 국소적 특징점의 개수이다.)
청구항 8에 있어서,

상기 물체-환경 거리 가중치
는 아래의 수학식 11에 따라 계산되는 것을 특징으로 하는 로봇의 자기 위치 추정 방법.

< 수학식 11 >

(여기서,
는 노드
의 물체-환경 거리 가중치이고,
는 로봇이 인식한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점의 3차원값을 후보 노드의 동일 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점의 3차원 값으로 보간(fitting)하고 보간(fitting)된 국소적 특징점의 3차원값과 후보 노드의 동일 국소적 특징점의 3차원값 간의 거리 차이고,

는 로봇이 인식한 모든 물체 및 그 물체를 포함하는 주변환경의 국소적 특징점과 노드
에 존재하는 물체 중 로봇이 인식한 물체와 동일한 물체 및 그 물체를 포함하는 주변 환경의 국소적 특징점 간에 매칭된 국소적 특징점 각각에 대한
의 평균값이다.)
360도보다 작은 시야각을 갖고 스테레오 영상장치로 구성된 카메라부와, 상기 카메라부를 일정 각도씩 회전시킬 수 있는 회전 장치와, 로봇 이동 경로 주위의 지도를 저장하고 있는 데이터베이스부와, 상기 카메라부에서 획득한 영상 정보를 이용하여 자기의 위치를 추정하는 위치 연산부를 포함하는 로봇의 자기 위치 추정 방법에 있어서,

상기 카메라부가 일정 방향의 로봇 주위의 영상을 획득하는 1 단계와;

상기 위치 연산부에서 상기 카메라부가 획득한 영상 내에서 인식한 물체가 없는 경우에는 상기 회전 장치를 구동하여 카메라를 회전시킨 후 다른 방향의 영상을 획득하는 과정을 획득된 영상에서 인식한 개별물체가 있을 때까지 반복하고, 인식한 개별 물체의 국소 특징점과 개별 물체를 포함하는 주변 환경의 국소 특징점의 카메라 좌표계 위치값을 생성하는 2 단계와;

상기 위치 연산부에서 상기 데이터베이스부에 저장된 지도로부터 상기 인식된 물체가 1개 이상 포함된 노드를 로봇이 위치할 수 있는 후보 노드로 선정하는 3 단계와;

상기 위치 연산부에서 후보 노드들의 우선 순위를 결정하는 4 단계와;

상기 위치 연산부에서 후보 노드에 포함된 물체와 로봇 간의 거리 및 최우선 순위 물체를 계산하는 5 단계와;

상기 위치 연산부에서 상기 회전 장치로 카메라부를 회전시켜 후보 노드의 최우선 순위 물체를 확인하는 6 단계와,

상기 위치 연산부에서 상기 6 단계에서 최우선 물체가 확인된 노드에 파티클 필터링 기법을 사용해서 로봇의 위치를 추정하는 7 단계를 포함하는 것을 특징으로 하는 로봇의 자기위치 추정 방법.
청구항 12에 있어서,

상기 6 단계는 후보 노드의 최우선 물체를 인식하지 못하면 상기 회전 장치 로 카메라부를 회전시켜 상기 4단계에서의 해당 후보 노드 중 다음 우선 순위의 후보 노드의 최우선 물체를 확인하는 것을 특징으로 하는 로봇의 자기위치 추정 방법.