KR20070026912A

KR20070026912A - Method for catadioptric vision based localization and mapping in a particle filter framework

Info

Publication number: KR20070026912A
Application number: KR1020050079125A
Authority: KR
Inventors: 장기정; 권인소
Original assignee: 한국과학기술원
Priority date: 2005-08-27
Filing date: 2005-08-27
Publication date: 2007-03-09
Also published as: KR100702663B1

Abstract

A method for catadioptric vision based localization and mapping in particle filter framework is provided to reduce the amount of calculation necessary for a SLAM(Simultaneous Localization And Mapping) process by alternately updating the position states of a moving object and a feature point. An omnidirectional panorama image for exploring environment is obtained at the present position of a moving object, and each position of feature points in the exploring environment is detected as an azimuth from the moving object by using the omnidirectional panorama image(400). The posterior probability distribution of the present moving object position is updated by using the detected azimuth and the prior updated probability distribution of the feature points(410). The posterior probability distribution of the feature points is updated by using the detected azimuth and the posterior probability distribution of the updated moving object position(420).

Description

Method for catadioptric vision based localization and mapping in a particle filter framework}

도 1은 본 발명을 개괄하는 흐름도이다.1 is a flowchart outlining the present invention.

도 2a 내지 도 2b는 전방향 시각 센서의 구조 및 이를 이용하여 촬영한 전방향 파노라마 영상을 나타낸다.2A to 2B illustrate a structure of an omnidirectional visual sensor and an omnidirectional panoramic image photographed using the omnidirectional visual sensor.

도 3a 내지 도 3b는 전방향 경로 파노라마 영상 및 상기 전방향 경로 파노라마의 경계점을 추출한 영상을 나타낸다.3A to 3B illustrate an omnidirectional path panorama image and an image obtained by extracting boundary points of the omnidirectional path panorama.

도 4은 본 발명의 일실시예에 따른 전방향 시각센서 기반 위치 추정 및 매핑을 위한 방법의 동작을 나타내는 흐름도이다.4 is a flowchart illustrating an operation of a method for omnidirectional visual sensor based position estimation and mapping according to an embodiment of the present invention.

도 5는 본 발명의 일실시예에 따른 이동체 파티클에 대한 측정 절차의 동작을 나타내는 흐름도이다.5 is a flowchart illustrating the operation of a measurement procedure for a mobile particle according to an embodiment of the present invention.

도 6는 각 특징점에 대한 경계 밀도를 구하는 과정을 나타내는 개념도이다.6 is a conceptual diagram illustrating a process of obtaining a boundary density for each feature point.

도 7은 하나의 특징점에 대한 가중치를 구하는 과정을 나타내는 개념도이다.7 is a conceptual diagram illustrating a process of obtaining a weight for one feature point.

도 8은 특징점 추적 및 조각 가능한 영역을 나타내는 개념도이다. 8 is a conceptual diagram illustrating a feature point tracking and an engravable area.

도 9은 본 발명에 의한 칼라화 점유 지도를 생성하는 방법의 동작을 나타내는 흐름도이다.9 is a flowchart illustrating the operation of a method for generating a colorized occupancy map according to the present invention.

도 10은 본 발명의 일실시예에 따른 COM 생성 알고리듬의 결과물인 칼라화 점유 지도를 나타낸다. 10 illustrates a colorized occupancy map that is a result of a COM generation algorithm according to an embodiment of the present invention.

도 11은 본 발명의 일실시예에 따른 칼라화 점유 지도를 이용한 위치 추정을 설명하기 위한 개념도이다.11 is a conceptual diagram illustrating position estimation using a colorized occupation map according to an embodiment of the present invention.

본 발명은 이동체의 위치 추정 및 매핑을 동시에 수행하는 장치 및 방법에 관한 것으로, 보다 상세하게는 입자 필터(particle filter) 기반의 효율적인 실시간 SLAM 알고리듬을 제공하고, 이를 통해 칼라화 점유 지도를 생성하며, 상기 생성된 칼라화 점유 지도를 이용하여 이동체의 위치를 추정하는 입자 필터 프레임 워크에서 전방향 시각센서 기반 위치 추정 및 매핑을 위한 장치 및 방법에 관한 것이다.The present invention relates to an apparatus and a method for simultaneously performing the position estimation and the mapping of a moving object, and more particularly, to provide an efficient real-time SLAM algorithm based on a particle filter, thereby generating a colorized occupancy map, The present invention relates to an apparatus and method for omnidirectional visual sensor-based position estimation and mapping in a particle filter framework that estimates the position of a moving object using the generated colorized occupancy map.

로봇이 임의의 사전 지도(a priori map)없이 새로운 환경을 탐사하게 하기 위해서는, 로봇과 환경 간의 기하학적 관계를 실시간으로 추정할 필요가 있다. 즉, 로봇은 주변 환경에 대한 지도 작성과 이 지도상에서의 자신의 위치 추정을 동시에 수행하는 과정이 필요하다. 상기 SLAM을 구현하기 위한 방법으로는 확장 칼만 필터(Extended Kalman Filter : 이하 EKF라 칭함) 기반의 접근 방식이 일반적이며, 최근 Condensation(CONditional DENsity propagATION)으로도 알려져 있는 입자 필터 방식 기반의 접근 방식도 시도되고 있다.In order for the robot to explore a new environment without a priori map, it is necessary to estimate in real time the geometric relationship between the robot and the environment. In other words, the robot needs to simultaneously create a map of its surroundings and estimate its position on the map. As a method for implementing the SLAM, an approach based on an Extended Kalman Filter (hereinafter referred to as EKF) is generally used. Also, an approach based on a particle filter method, also known as Condensation (CONditional DENsity propagation), is also tried. It is becoming.

EKF 방식은 소정의 조건하에, 추정 오차의 공분산을 최소화한다는 관점에서 최적인 예측기-교정기(predictor-corrector) 유형의 추정기이다. 여기서, 예측기-교정기 유형의 추정기란 시스템의 모델에 따라 상태를 예측하는 과정과 상기 예측된 상태를 입력 측정치와의 비교를 통하여 교정하는 과정을 지속적으로 반복함으로써, 오차를 최소화하는 추정기이다. 그러나 상기 소정의 조건, 즉, 실제 환경에서의 모델 확률 분포가 가우시안 분포(gaussian distribution)를 따르지 않는 경우에는 추정에 실패하거나 심각한 오차를 발생시킨다는 문제점이 있다.The EKF scheme is a predictor-corrector type estimator that is optimal in terms of minimizing covariance of estimation errors under certain conditions. Here, the estimator-calibrator type estimator is an estimator that minimizes errors by continuously repeating a process of predicting a state according to a model of the system and correcting the predicted state by comparing the measured state with an input measurement value. However, there is a problem in that estimation fails or a serious error occurs when the predetermined condition, that is, the model probability distribution in the real environment does not follow the Gaussian distribution.

이에 대응하여 입자 필터 방식은 마찬가지로 예측기-교정기 유형의 추정기이지만 비-가우시안 분포(non-Gaussian distribution)까지 표현할 수 있는데, 여기서, 입자 필터란 추정하고자 하는 상태(state)의 확률 분포를 복수의 가설 샘플(hypothesized sample)의 밀도로 표현한 것이다. 상기 가설 샘플들은 입자 필터에서는 파티클로 불린다. 즉, 상기 추정하고자 하는 상태가 위치와 관련되었다면, 확률이 높은 위치는 파티클의 밀도가 높고, 확률이 낮은 위치는 파티클의 밀도가 낮다. 이후 소정의 입력 측정치를 통하여 파티클의 밀도로 표현되는 사후분포를 갱신하고, 이러한 과정을 반복하면서, 정확한 상태를 추정하게 된다.Correspondingly, the particle filter method is a predictor-calibrator type estimator, but can also represent a non-Gaussian distribution, where the particle filter is a plurality of hypothetical samples that represent the probability distribution of states to be estimated. It is expressed as the density of the hypothesized sample. The hypothesis samples are called particles in the particle filter. That is, if the state to be estimated is related to the position, the position of high probability has high particle density, and the position of low probability has low particle density. After that, the post-distribution expressed in terms of particle density is updated through a predetermined input measurement, and the process is repeated, and the correct state is estimated.

여기서, 사후분포 갱신이란 EKF에서의 상태 교정과 비슷한 개념으로, 가정한 분포에서 소정의 측정을 수행한 후, 이 측정치에 따라 파티클을 다시 분포시키는 것이다. 이러한 의미에서 사후분포를 사후확률(posterior probability)이라고도 불린다. 즉, 각각의 파티클에서의 측정치가 높으면 그 파티클 위치에서 파티클 수가 증식되고, 파티클의 측정치가 낮은 경우, 파티클은 소멸할 수 있다. 예를 들어 초 기 조건으로 균일 분포(uniform distribution)를 가정하였다면, 측정을 수행하고, 측정치에 따라 파티클을 다시 분포시키는 사후분포 갱신의 횟수가 늘어남에 따라 파티클들은 점점 추정되는 위치로 집중되게 된다.Here, post-distribution update is a concept similar to state correction in EKF. After performing a predetermined measurement in the assumed distribution, the particles are redistributed according to the measured value. In this sense, posterior distribution is also called posterior probability. In other words, if the measurement value of each particle is high, the particle number is multiplied at the particle position, and if the measurement value of the particle is low, the particle may disappear. For example, assuming a uniform distribution as an initial condition, the particles are concentrated to an estimated position as the number of post-distribution updates that perform the measurement and redistribute the particles according to the measurement increases.

입자 필터는 이와 같이, 확률 분포를 파티클 수의 많고 적음(밀도)으로 표시하기 때문에, 상태의 평균값과 공분산만으로 표현되는 칼만 필터 방식과는 달리 임의의 확률밀도를 표시하는 것이 가능하다. 입자 필터가 이처럼 비-가우시안 분포를 표현할 수 있기 때문에, 서로 다른 두 위치의 확률이 대등하게 높은 경우이거나 동적이고 복잡한 성질의 잡음 환경에서도 적용가능하며, 초기 또는 중간 사후확률이 일시적으로 그릇된 경우에도 갱신 횟수가 증가함에 따라 상태 추정의 회복이 용이하다는 측면에서, 매우 강인하다. 그러나 입자 필터에서는 차원이 높아지면, 적절한 상태표현을 위해 요구되는 파티클의 개수가 지수적으로 증가되는 문제점이 있다. 따라서 추정하고자 하는 상태가 로봇의 위치 추정에 필요한 3차원 정도일 경우, 입자 필터가 매우 강력한 추정기로 인정되었으나, 위치 추정과 지도 생성을 동시에 수행하는 SLAM에서는 지도상에 존재하는 많게는 수백 개의 특징점 각각이 적어도 2차원(수평이동 가정)을 요하므로, 실질적으로 SLAM에서는 입자 필터를 기존 방법 그대로 적용하는 것은 거의 불가능하다.Since the particle filter displays the probability distribution as a large and small number of particles (density) in this manner, it is possible to display an arbitrary probability density unlike the Kalman filter method expressed only by the mean value and the covariance of the states. Because particle filters can represent this non-Gaussian distribution, they can be applied in situations where the probability of two different positions is equally high, or even in dynamic and complex noise environments, and even when the initial or intermediate posterior probability is temporarily wrong. It is very robust in terms of ease of recovery of state estimation as the number of times increases. However, in the particle filter, as the dimension increases, the number of particles required for proper state expression increases exponentially. Therefore, when the state to be estimated is about three-dimensional, which is necessary to estimate the position of the robot, the particle filter is recognized as a very powerful estimator.However, in SLAM, which performs both the position estimation and the map generation at the same time, each of the hundreds of feature points on the map is Since it requires two dimensions (a horizontal shift assumption), it is virtually impossible to apply particle filters as they are in SLAM.

즉, EKF 기반의 접근 방식은 비-가우시안 분포의 시스템에 적용할 수 없으며, 일반적인 입자 필터 기반의 접근 방식은 고차원에서 계산량이 폭주하므로 구현이 비현실적이라는 문제점이 있다.In other words, the EKF-based approach cannot be applied to non-Gaussian distribution systems, and the general particle filter-based approach has a problem that the implementation is unrealistic because of the high computational complexity.

한편, 로봇 등 이동체는 움직일 수 있는 자유공간(free space)에 대한 정보 가 있어야 이동이 용이하다. 이를 위해 종래의 기술은 주로 레이저 거리 탐색기(Laser Range Finder) 및 소나(Sonar)와 같은 능동 센서(active sensor)를 사용하였다. 이러한 능동 센서들은 점유 지도 생성에 직접적인 거리 정보를 제공하지만, 무겁거나 부피가 크며 비용이 많이 든다. 또한, 다차원의 칼라 정보를 제공할 수 없다는 한계가 있다. 반면, 로봇에 대해서 범용 센서로 받아들여지는 시각 센서(vision sensor)는 주변 환경에 대해 다량의 정보를 제공하고, 컴팩트하지만, 이들이 제공하는 데이터는 기하학적으로 해석이 어렵다는 단점이 있다.On the other hand, a moving object such as a robot is easy to move only if there is information about free space to move. To this end, the prior art mainly used active sensors such as a laser range finder and a sonar. These active sensors provide direct distance information for generating occupancy maps, but are heavy, bulky and expensive. In addition, there is a limitation that multi-dimensional color information cannot be provided. On the other hand, vision sensors, which are accepted as general-purpose sensors for robots, provide a large amount of information about the surrounding environment and are compact, but the data they provide is difficult to interpret geometrically.

본 발명이 이루고자 하는 기술적 과제는, 이동체의 자세 및 환경 지도의 특징점에 대한 상태(state)를 분리하고, 각각에 대한 사후분포를 서로를 기반으로 번갈아 갱신시키는 방법을 사용함으로써, SLAM에 입자 필터를 적용할 때 발생하는 계산량 폭주 문제를 해결할 수 있도록 하는 전방향 시각센서 기반 위치 추정 및 매핑을 위한 방법을 제공하는데 있다.The technical problem to be achieved by the present invention is to separate the state of the moving object posture and the feature points of the environmental map, and to use the method of updating the post-distribution for each alternately based on each other, to apply a particle filter to the SLAM The present invention provides a method for omnidirectional visual sensor based position estimation and mapping that can solve the problem of computational congestion.

본 발명이 이루고자 하는 다른 기술적 과제는, 본 발명의 일실시예를 통해 얻어진 정보로부터 칼라화 점유 지도(Colored Occupancy Map : 이하 COM이라 칭함)를 생성함으로써, 보다 다중의 지도 정보를 제공할 수 있도록 하는 전방향 시각센서 기반 매핑을 위한 방법을 제공하는데 있다.Another technical problem to be achieved by the present invention is to generate a colored occupancy map (hereinafter referred to as COM) from the information obtained through an embodiment of the present invention, so that it is possible to provide more map information. It provides a method for omnidirectional visual sensor based mapping.

본 발명이 이루고자 하는 또 다른 기술적 과제는, 획득된 비전 데이터를 이용하여, 본 발명의 일실시예를 통해 얻어진 COM에서의 이동체의 위치를 추정할 수 있도록 하는 전방향 시각센서 기반 위치 추정 방법을 제공하는데 있다.Another technical problem to be achieved by the present invention is to provide an omnidirectional visual sensor-based position estimation method for estimating the position of a moving object in a COM obtained through an embodiment of the present invention by using the acquired vision data. It is.

상기의 기술적 과제를 이루기 위한, 본 발명에 의한 입자 필터 프레임 워크에서 전방향 시각센서 기반 위치 추정 및 매핑을 위한 방법은 (a) 이동체의 현 위치에서, 탐사환경에 대한 전방향 파노라마 영상을 획득하고, 상기 획득된 전방향 파노라마 영상을 이용하여, 상기 탐사 환경에 존재하는 특징점 각각의 위치를 이동체로부터의 방위각으로서 검출하는 단계; (b) 상기 검출된 방위각 및 이전에 갱신된 상기 특징점 위치의 사후 확률 분포를 기반으로, 현재의 이동체 위치의 사후 확률 분포를 갱신하는 단계; 및 (c) 상기 검출된 방위각 및 상기 갱신된 이동체 위치의 사후 확률 분포를 기반으로, 상기 특징점 위치의 사후 확률 분포를 갱신하는 단계를 포함하고, 상기 (a) 단계 내지 상기 (c) 단계를 반복 수행하는 것을 특징으로 한다.In order to achieve the above technical problem, a method for omnidirectional visual sensor-based position estimation and mapping in a particle filter framework according to the present invention includes (a) acquiring an omnidirectional panoramic image of an exploration environment at a current position of a moving object; Detecting a position of each feature point present in the exploration environment as an azimuth from a moving object using the obtained omnidirectional panoramic image; (b) updating the posterior probability distribution of the current moving object position based on the detected azimuth angle and the posterior probability distribution of the feature point position previously updated; And (c) updating the posterior probability distribution of the feature point position based on the detected azimuth angle and the posterior probability distribution of the updated moving object position, repeating steps (a) to (c). It is characterized by performing.

상기 (b) 단계는, 상기 각각의 이동체의 파티클로부터 상기 검출된 방위각으로의 연장선상에 위치하는 특징점 파티클의 수를 기반으로 상기 각각의 이동체 파티클에 대한 가중치를 산출하고, 상기 산출된 각각의 이동체 파티클의 가중치를 기반으로 상기 이동체의 현재의 사후 확률 분포를 갱신하는 것이 바람직하다. 상기 (c) 단계는, 상기 갱신된 이동체의 사후 확률 분포에 따른 이동체 파티클 각각으로부터 상기 검출된 각각의 방위각 방향으로의 연장선을 고려할 때, 상기 특징점의 파티클 각각에 대해, 상기 특징점 파티클을 통과하는 상기 연장선의 수를 기반으로 상기 각각의 특징점 파티클에 대한 가중치를 산출하고, 상기 산출된 특징점 파티클의 가중치를 기반으로 상기 특징점 각각의 사후 확률 분포를 갱신하는 것이 바람직 하다.In the step (b), the weight of each moving object particle is calculated based on the number of feature point particles located on an extension line from the particles of each moving object to the detected azimuth angle, and the calculated moving objects are calculated. It is desirable to update the current posterior probability distribution of the moving object based on the weight of the particles. In the step (c), considering the extension line in each of the detected azimuth directions from each of the moving particles according to the posterior probability distribution of the updated moving object, for each of the particles of the feature point, passing through the feature point particle It is preferable to calculate a weight for each of the feature point particles based on the number of extension lines, and to update the posterior probability distribution of each of the feature points based on the calculated weight of the feature point particles.

상기 방법은, (d) 상기 (a) 단계 내지 상기 (c) 단계를 반복 수행하여 획득한 각 이동 시점의 전방향 파노라마 영상을 적층하여 얻은 전방향 경로 파노라마 영상, 각 이동 시점의 이동체의 위치를 취합한 이동체 항로, 상기 생성된 특징점 지도를 기반으로, 상기 이동체 항로 중 각각의 위치에서의 각 특징점의 가시성을 판단하여 자유 공간 조각(free space carving)을 수행하는 단계; (e) 상기 자유 공간 조각을 거친 후에 조각되지 않은 영역의 표면점을 획득하고, 상기 전방향 경로 파노라마 영상을 이용하여 상기 획득된 표면점에 대해 가시성 조건을 만족시키면서 상기 이동체 항로로부터 가장 가까운 이동체의 위치를 결정하는 단계; 및 (f) 상기 전방향 경로 파노라마 영상으로부터 상기 결정된 이동체의 위치일 때 획득한 전방향 파노라마 영상을 추출하고, 상기 추출된 전방향 파노라마 영상으로부터 상기 획득된 표면점 방향에 대한 칼라를 추출하여, 상기 표면점에 상기 추출된 칼라를 할당하여 칼라화 점유 지도를 생성하는 단계를 더 포함하는 것이 바람직하다. 또한, 상기 방법은 (g) 이동체가 탐사환경을 이동하고, 각 이동 위치에서 전방향 수평 파노라마 영상을 획득하는 단계; (h) 상기 생성된 칼라화 점유 지도로부터 각각의 이동체 파티클 위치의 전방향 수평 파노라마 영상을 생성하고, 상기 생성된 각각의 전방향 수평 파노라마 영상과 상기 획득된 전방향 수평 파노라마 영상을 비교하여, 이동체의 위치에 대한 사후 확률 분포를 갱신하는 단계를 더 포함하고, 상기 (g) 단계 내지 상기 (h) 단계를 반복 수행하는 것이 바람직하다.The method comprises: (d) omnidirectional panoramic panorama images obtained by stacking omnidirectional panoramic images of each moving point obtained by repeating steps (a) to (c), and positions of moving bodies at each moving point. Performing free space carving by determining the visibility of each feature point at each position of the moving path based on the collected moving object route and the generated feature point map; (e) obtaining a surface point of the unsculpted area after passing through the free space fragment, and satisfying a visibility condition with respect to the obtained surface point using the omnidirectional path panoramic image, Determining a location; And (f) extracting an omnidirectional panoramic image obtained at the position of the determined moving object from the omnidirectional panoramic panorama image, and extracting a color of the obtained surface point direction from the extracted omnidirectional panoramic image, The method may further include generating a colorized occupancy map by allocating the extracted color to a surface point. In addition, the method comprises the steps of: (g) moving the exploration environment of the moving object, and obtaining an omnidirectional horizontal panoramic image at each moving position; (h) generating an omnidirectional horizontal panoramic image of each mobile particle position from the generated colorized occupancy map, comparing the generated omnidirectional horizontal panoramic image with the obtained omnidirectional horizontal panoramic image, The method may further include updating a post-probability distribution for the position of, and repeating the steps (g) to (h).

이하, 첨부된 도면들을 참조하여 본 발명에 따른 방법 및 장치에 대해 상세 히 설명한다.Hereinafter, a method and an apparatus according to the present invention will be described in detail with reference to the accompanying drawings.

도 1은 본 발명을 개괄하는 흐름도이다. 본 발명은 도 1과 같이 3 개의 주요 단계로 이루어져 있으며, 100 단계, 110 단계, 120 단계는 본 발명의 일실시예를 각각 이룬다.1 is a flowchart outlining the present invention. The present invention consists of three main steps as shown in Figure 1, step 100, step 110, step 120 forms an embodiment of the present invention, respectively.

먼저, 본 발명의 일실시예인 SLAM 알고리듬을 이용하여 로봇 등 이동체가 미탐사 환경에서 자신의 위치 추정 및 주변 환경의 특징점을 포함하는 지도 생성을 수행한다(100 단계). 즉, 특징점 지도가 실시간으로 생성되며, 이 과정에서 부수적으로 이동체의 항로 및 전방향 경로 파노라마 영상이 얻어진다. 상기 100 단계에서는 상기 이동체의 전체 항로 및 상기 항로 전체에 대한 전방향 파노라마 영상을 저장할 필요는 없지만, 상기 정보들은 상기 110단계에서 요구되므로 상기 110 단계에서 별도로 상기 정보를 획득하지 않는 한 상기 정보들을 저장하는 것이 바람직할 것이다. 즉, SLAM의 목적 달성만을 위해서는 상기 정보들의 저장은 불필요하다.First, using a SLAM algorithm, which is an embodiment of the present invention, a mobile object such as a robot performs a map generation including its location estimation and a feature point of the surrounding environment in an unexplored environment (step 100). That is, the feature point map is generated in real time, and in this process, the moving path and the omnidirectional panoramic panorama image of the moving object are obtained. In the step 100, it is not necessary to store the entire panoramic path of the moving object and the entire path of the moving object, but since the information is required in the step 110, the information is stored unless the information is acquired separately in the step 110. It would be desirable to. In other words, it is not necessary to store the information only for the purpose of SLAM.

다음, 이동체의 항로, 특징점 지도 및 전방향 경로 파노라마 영상을 이용하여 COM을 생성한다(110 단계). 상기 100 단계에서 상기 이동체 항로, 특징점 지도 및 전방향 경로 파노라마 영상이 저장되지 않았다면, 상기 110 단계에서 별도로 이동 중에 SLAM을 수행하면서 상기 이동체의 항로, 특징점 지도 및 전방향 경로 파노라마 영상을 획득하여야 할 것이다. 상기 생성된 COM 자체로서 본 발명의 다른 일실시예의 목적이 달성된다. 다만, 본 발명의 또 다른 일실시예의 목적 즉, 120 단계를 수행하기 위해서는 상기 생성된 COM을 저장한다.Next, a COM is generated using the moving path of the moving object, the feature point map, and the omnidirectional panoramic panorama image (step 110). If the moving object route, the feature point map and the omnidirectional panoramic panorama image are not stored in step 100, the moving object route, the feature point map and the omnidirectional panoramic panorama image should be obtained while performing a SLAM during the movement separately in step 110. . With the generated COM itself, the object of another embodiment of the present invention is achieved. However, for the purpose of another embodiment of the present invention, that is, to perform step 120, the generated COM is stored.

마지막으로, 이동체가 이동하면서 획득하는 전방향 파노라마 영상 및 상기 저장된 COM을 이용하여, 이동체는 상기 COM에서의 자신의 위치를 추정한다(120 단계).Finally, using the omnidirectional panoramic image acquired while the moving object and the stored COM, the moving object estimates its position in the COM (step 120).

이하, 상기 100 단계, 110 단계, 120 단계 즉, 본 발명의 일실시예들을 I, II, III의 섹션으로 나누어 설명한다.Hereinafter, the steps 100, 110, and 120, that is, the embodiments of the present invention will be described by dividing into sections of I, II, and III.

I. 미탐사 환경에서의 본 발명에 의한 SLAM 알고리듬(100 단계)I. SLAM Algorithm According to the Present Invention in Unexplored Environment (Step 100)

본 명세서에서는 넓은 시각(수평 360^o)의 영상을 제공하여 일반적으로 많이 쓰이는 전방향 시각센서 예컨대, 전방향 카메라(omindirectional camera)를 사용하는 것을 전제하여 기술한다.In the present specification, a wide-angle (horizontal 360 ^o ) image is provided, which is described on the premise of using a general omnidirectional vision sensor, for example, an omnidirectional camera.

도 2a는 쌍곡면 거울과 시각 센서를 구비한 시스템이 도시되어 있다. 쌍곡면 거울의 초점에서 P(X,Y,Z)의 좌표 상에 있는 물체를 바라 본 상이 상기 영상 평면에 투시되며, 시각 센서 즉, 카메라의 유효 핀홀을 통과한다. 실제적으로 유효 핀홀 아래에 있는 촬상 소자에 촬상된다. 상기 쌍곡면 거울은 주변 전방향에 대한 상을 반사시켜, 반사된 상을 상기 촬상 소자에 제공하므로 도 2b와 같은 원형의 영상이 얻어진다. 특히, 상기 영상에서 검은 선으로 이어진 원은 상기 쌍곡면 거울의 초점 즉, 가상 시점의 높이에 위치하는 상들의 연속선이다.2A shows a system with a hyperbolic mirror and a vision sensor. An image looking at the object at the coordinates of P (X, Y, Z) at the focal point of the hyperbolic mirror is projected onto the image plane and passes through the visual sensor, ie the effective pinhole of the camera. The image is actually picked up by the image pickup device under the effective pinhole. The hyperbolic mirror reflects the image in the peripheral omnidirectional direction and provides the reflected image to the imaging device, thereby obtaining a circular image as shown in FIG. 2B. In particular, the circle of black lines in the image is a continuous line of images located at the focal point of the hyperbolic mirror, that is, at the height of the virtual viewpoint.

도 3a 내지 도 3b는 이동체가 이동 중에 상기 전방향 시각센서를 통하여 획득한 전방향 영상의 프레임 중 가상시점을 지나는 수평면이 투시되는 원을 시간축 으로 누적한 전방향 경로 파노라마 영상(Omnidirectional Route Panorama : 이하 ORP라 칭함) 및 상기 전방향 경로 파노라마의 경계점을 추출한 영상을 나타낸다. 도 2b와 같은 영상을 각 이동 시점에서 획득하고, 상기 획득된 영상에서 검은 원 위치의 영상을 펴서, 상기 펴진 영상들을 시간축으로 쌓아서 얻은 영상이 상기 도 3a인 것이다. 즉, 도 3a는 평면상에서 로봇이 움직임에 따라 얻어진 수평선들의 연속적인 적층(stacking)이다. y축은 이동 중에 얻은 각각의 프레임에 대한 인덱스 즉, 시간축을 나타내며, x축은 이동체의 위치에서 바라본 0^o내지 360^o의 방위각에 따른 전방향 파노라마 영상을 나타낸다. 상기 도 3a 및 도 3b는 특정 높이의 주변환경을 촬영하여 얻은 것이다.3A to 3B are omnidirectional route panorama images in which a horizontal plane passing through a virtual point of view of an omnidirectional image obtained through the omnidirectional visual sensor is projected through the omnidirectional route sensor in a time axis; ORP) and the boundary point of the forward path panorama are extracted. The image obtained by stacking the unfolded images on the time axis by acquiring an image as shown in FIG. That is, FIG. 3A is a continuous stacking of horizontal lines obtained as the robot moves on a plane. The y-axis represents the index for each frame obtained during the movement, that is, the time axis, and the x-axis represents the omnidirectional panoramic image according to the azimuth of 0 ^o to 360 ^o as viewed from the position of the moving object. 3A and 3B are obtained by photographing a surrounding environment having a specific height.

ORP 상에서 비최대값 억제(nonmaxima suppression)를 하면서 경계선 검출(edge operation)을 수행하면, 특징점 검출이 수월해진다. 즉, 도 3a와 같이 이동 중에 획득한 전방향 경로 파노라마 영상에 대해 경계선 검출을 수행하면 도 3b의 영상이 얻어진다. 이를 통하여 실시간으로 특징점 검출이 가능하다. 도 3b에서, 하얀색 부분이 검출된 특징점이며, 전방향 경로 파노라마 영상에서는 연속된 프레임 영상에 따라 하얀 선이 발생되는 것이다. 특징점 방향의 방위각과 수평선의 특징점 위치 즉, x축은 비례하기 때문에 각 특징점에 대한 각 시점(프레임)의 방위각을 상기 도 3b와 같은 경계점 검출된 전방향 파노라마 영상으로부터 얻을 수 있다.If edge detection is performed while nonmaxima suppression is performed on ORP, feature point detection is facilitated. That is, when boundary detection is performed on the omnidirectional panoramic panorama image acquired during movement as shown in FIG. 3A, the image of FIG. 3B is obtained. This enables detection of feature points in real time. In FIG. 3B, a white portion is a detected feature point, and a white line is generated according to a continuous frame image in the omni-directional path panorama image. Since the azimuth angle in the direction of the feature point and the location of the feature point on the horizontal line, i.e., the x-axis, are proportional, the azimuth angle of each viewpoint (frame) with respect to each feature point can be obtained from the boundary point detected omnidirectional panoramic image as shown in FIG. 3B.

한편, SLAM에 입자 필터를 적용하면 사후 확률 분포는

와 같이 표현되며, 구체적으로는 수학식 1로 정의된다. On the other hand, if you apply a particle filter to SLAM, the posterior probability distribution is

It is expressed as, and specifically defined by the equation (1).

여기서, 모든 측정 정보

는 현재 시간 단계까지 주어지며, 이동체의 이동을 제어하는 제어 입력

은 이전 시간 단계까지 주어진다. 여기서 주된 관심은 상태

의 추정이다. 예측기-교정기 유형의 추정기인 입자필터는 마르코프(Markov) 가정 하에서 예측기의 모션 모델이

와 같은 조건부 확률로 정의되며, 결과적으로

에 대한 예측 확률 분포는

과 같은 적분 형태로 얻어진다. 측정 모델은

의 확률로서 주어지고 베이스(Bayes) 확률 법칙을 이용하여 최종 사후 확률 분포는 예측 확률 분포에 측정치

가 곱해지는 형태를 갖는다.Where all measurement information

Is given up to the current time step, the control input to control the movement of the moving object

Is given up to the previous time step. The main concern here is status

Is estimated. The particle filter, which is an estimator-corrector type estimator, is based on the Markov assumption.

Is defined as a conditional probability such that

The probability distribution for

Obtained in the form of integrals such as Measurement model

Given the probability of, and using the Bayes probability law, the final posterior probability distribution is a measure of the predicted probability distribution.

Has the form of multiplication.

SLAM의 경우 상태

는 이동체의 위치

및 지도의 특징점

이다.

은 이동체의 주변 환경의 특징점 벡터이며, 상기 벡터에는 그 특징점에 해당되는 파티클들이 엘리먼트로서 포함된다. N은 특징점의 개수이고,

은 특징점 전체를 표현하는 벡터이다. 즉, 모든 가능한 특징점 및 모든 가능한 이동체 위치에 대해 공간상의 사후 확률 분포를 직접 동시에 추정하는 것이다. 다시 말하면, 수학식 1의 사후 확률분포를 구하는 것이 다.Status for SLAM

Is the position of the moving object

And feature points of the map

to be.

Is a vector of feature points of the surrounding environment of the moving object, and the particles corresponding to the feature points are included as elements. N is the number of feature points,

Is a vector representing the entire feature point. In other words, the posterior probability distribution in space is directly estimated simultaneously for all possible feature points and all possible moving object positions. In other words, the posterior probability distribution of Equation 1 is obtained.

전술한 바와 같이, 비록 입자 필터가 임의의 확률 분포를 표현할 수 있다는 장점이 있지만,

가 고차원인 환경에서는 파티클 표현이 용이하지 않으며, 계산량이 매우 높아지기 때문에 비현실적인 방법이다.As mentioned above, although the particle filter has the advantage that it can represent any probability distribution,

Particle representation is not easy in a high-dimensional environment, and the computational amount is very high, which is impractical.

그러므로 본 발명의 일실시예는 상태를 이동체 및 특징점 관점에서 분리하도록 수학식 1을 변형함으로써, 입자 필터 적용시의 복잡도를 줄일 수 있다. 이동체와 각 특징점의 사후 확률 분포는 마르코프 가정 하에, 베이스 확률 법칙을 적용하여 재귀적 형태를 번갈아 수행함으로써 얻어진다. 상술한 바에 따라, 이동체의 사후 확률 분포 b(s _t) 및 환경 지도의 특징점 사후 확률 분포 b(M _t)의 갱신 프로세스를 나타내면 다음과 같다.Therefore, an embodiment of the present invention can reduce the complexity in applying the particle filter by modifying Equation 1 to separate the state in terms of moving object and feature point. The posterior probability distribution of the moving object and each feature point is obtained by alternating the recursive form by applying the base probability law under the Markov assumption. As described above, a process of updating the posterior probability distribution b ( s _t ) and the feature point posterior probability distribution b ( M _t ) of the moving object is as follows.

갱신 단계 프로세스 :

Renewal stage process:

즉, 이전 시간 t-1에서 주어진 제어 입력 u _t-1에 대해, 수학식 2로 표현되는 이동체 사후 확률 분포 b(s _t)를 갱신하고, 다음 갱신된 이동체 사후 확률 분포 b(s _t)로부터, 수학식 3으로 표현되는 환경지도상 특징점의 사후 확률 분포 b(M _t)를 갱신하며, 이 과정이 되풀이된다.That is, for the control input u _t-1 given at the previous time t-1, the moving body post probability distribution b ( s _t ) represented by Equation 2 is updated, and from the next updated moving body post probability distribution b ( s _t ), , The posterior probability distribution b ( M _t ) of the feature points on the environment map represented by Equation 3 is updated, and this process is repeated.

만약 지도상의 특징점들 사이의 관계가 독립이라고 가정하면 수학식 2와 수학식 3은 각각 다음과 같이 표현할 수 있다.If the relationship between the feature points on the map is assumed to be independent, Equations 2 and 3 may be expressed as follows.

수학식 4에서 측정치

는 각 특징점에 대한 측정 결과의 곱으로 표현하였는데, 이는 하나의 예시로서, 이 외에 각 특징점 측정 결과의 합이나 가중합(weighted sum) 등도 가능하며, 이에 제한되지 않는다.Measured value in (4)

Is expressed as a product of the measurement results for each feature point, which is an example, and in addition to this, a sum or a weighted sum of each feature point measurement result is possible, but is not limited thereto.

도 4는 본 발명의 일실시예에 따른 전방향 시각센서 기반 위치 추정 및 매핑을 위한 방법의 동작을 나타내는 흐름도이다.4 is a flowchart illustrating an operation of a method for omnidirectional visual sensor based position estimation and mapping according to an embodiment of the present invention.

먼저, 특징점의 위치를 나타내는 방위각 벡터 즉, 측정 벡터

를 검출한다(400 단계). 방위각 측정치를 얻는 방법으로는 도 3a 및 도 3b에서 설명한 바와 같다. 즉, 상기 전방향 경로 파노라마로부터 특징점을 검출한다.First, an azimuth vector representing the position of the feature point, that is, the measurement vector

Is detected (step 400). The method for obtaining the azimuth measurement is as described with reference to FIGS. 3A and 3B. In other words, a feature point is detected from the omni-directional path panorama.

다음, 상기 검출된 방위각 및 특징점에 대한 이전의 사후 확률 분포를 이용하여 이동체의 사후 확률 분포를 갱신한다(410 단계). 여기서, 특징점에 대한 이전의 사후 확률 분포는 현재가 t 시점이라면 t-1 시점의 특징점 파티클의 분포를 의미한다.Next, the posterior probability distribution of the moving object is updated using the previous posterior probability distribution of the detected azimuth and feature points (step 410). Here, the previous posterior probability distribution for the feature point means the distribution of feature point particles at time t-1 if the current is time t.

이동체의 사후 확률 분포는 예측 확률 분포

에 측정치

가 곱해진 형태로 갱신된다. The posterior probability distribution of the moving object is the predictive probability distribution

Measurements on

Is updated to be multiplied.

이동체 예측기의 모션 모델

은 제어 입력

에 따른 이동체 자세의 예측 모델로서, 이동체의 확률 분포를 파티클로 표현할 경우 적분과정 없이 각각의 파티클을 모션모델에 따라 이동시켜 줌으로써 예측 확률 분포를 구할 수 있다.Motion Model of Moving Object Predictor

Silver control input

As a predictive model of a moving object posture, when a probability distribution of a moving object is expressed as particles, a predicted probability distribution can be obtained by moving each particle according to a motion model without an integration process.

각 특징점에 대한 이동체의 측정 모델

의 일예는 다음과 같이 정의될 수 있다.Measurement model of moving object for each feature point

An example of may be defined as follows.

여기서, k는 전체 확률값을 1로 만드는 임의의 상수이다. Where k is an arbitrary constant that makes the overall probability value one.

도 5는 본 발명의 일실시예에 따른 이동체 파티클에 대한 측정 절차(410 단계)의 동작을 나타내는 흐름도이다.5 is a flowchart illustrating the operation of a measurement procedure (step 410) for a mobile particle according to an embodiment of the present invention.

수학식 6과 같은 측정 모델을 채택할 경우, 이동체가 이동한 t 시점에서, 이동체 파티클로부터 상기 검출된 방위각 방향의 연장선에 위치한 각각의 특징점의 경계밀도를 구한다(500 단계). 여기서, 경계밀도의 예로는 상기 연장선상에 위치하는 특징점 파티클을 계수하는 것을 들 수 있다.When adopting the measurement model as shown in Equation 6, at the time t of the moving object, the boundary density of each feature point located on the detected extension line in the azimuth direction is calculated from the moving object particle (step 500). In this case, an example of the boundary density may include counting feature point particles located on the extension line.

도 6은 각 이동체에 대한 측정치를 구하는 과정을 나타내는 개념도이다. 도 6에서 특징점은 3개로서, 흩뿌려져 있는 검은 점들이 특징점 파티클이며, 중심에는 이동체 파티클들이 흩뿌려져 있다. 도 5에서는 이동체 파티클들 중 삼각형 위치의 파티클로부터 상기 검출된 방위각으로 연장선이 있으며, 상기 연장선이 통과되는 특징점 파티클을 계수하는 것을 보여준다. 이 과정을 각각의 이동체 파티클에 대해 적용하여, 3 개의 특징점 경계밀도

,

가 검출된다.6 is a conceptual diagram illustrating a process of obtaining measurement values for each moving object. In FIG. 6, three feature points are scattered, and black dots scattered are feature point particles, and moving particles are scattered in the center. 5 shows an extension line at the detected azimuth angle from particles in a triangular position among moving particles, and counts feature point particles through which the extension line passes. Apply this process to each mobile particle, so that three feature point boundary densities

,

Is detected.

상기 검출된 상기 특징점 경계밀도를 이용하면 수학식 4의 우측 텀(term) 인 측정치

가 산출된다(510 단계). 상기 산출된 측정치는 이동체 파티클의 갱신을 위한 가중치로 사용된다.The measured value which is the right term of Equation 4 using the detected feature point boundary density.

Is calculated (step 510). The calculated measurement value is used as a weight for updating the mobile particle.

다음, 상기 검출된 방위각 및 상기 갱신된 이동체의 사후분포를 이용하여 특징점에 대한 사후 확률 분포를 갱신한다(420 단계).Next, the posterior probability distribution for the feature point is updated using the detected azimuth angle and the posterior distribution of the updated moving object (step 420).

특징점의 사후 확률 분포도 마찬가지로 예측 확률 분포

에 측정치

가 곱해진 형태로 갱신되며, 특징점 예측기의 모션 모델

은 특징점이 지도상에서 스스로 움직이지 않는다는 가정을 할 경우 정적(static)으로 모델링된다.The posterior probability distribution of feature points is similarly predicted probability distribution

Measurements on

Is updated to be multiplied and the motion model of the feature point predictor

Is modeled static if we assume that the feature points do not move themselves on the map.

각 특징점의 측정 모델은 수학식 6과 마찬가지로 정의될 수 있다. 각 특징점에 대한 측정치

를 계산하는 방법의 예로는, 상기 갱신된 이동체 파티클 각각으로부터 각 특징점에 해당되는 상기 검출된 방위각 방향으로의 연장선을 고려할 때, 상기 특징점 파티클 각각에 대해, 상기 특징점 파티클을 통과하는 상기 연장선의 수를 계수하여 각각의 특징점 파티클에 대한 가중치(weight)를 구할 수 있다. 다만, 상기의 방법은 일예에 불과하므로 이에 제한되지 않는다. 상술한 과정을 모든 특징점의 모든 파티클에 대해 수행되면 상기 420 단계는 완료된다.The measurement model of each feature point may be defined as in Equation 6. Measurement for each feature point

As an example of calculating a method, for each of the feature point particles, the number of extension lines passing through the feature point particle is determined for each of the feature point particles in consideration of the extension line in each of the updated moving object particles in the detected azimuth direction corresponding to each feature point. By counting, a weight for each feature point particle may be obtained. However, the above method is only an example and is not limited thereto. If the above-described process is performed for all particles of all the feature points, step 420 is completed.

도 7은 하나의 특징점에 대한 가중치를 구하는 과정을 나타내는 개념도이다. 상기 특징점의 파티클들 중의 하나에 대한 가중치가 표기되어 있다.7 is a conceptual diagram illustrating a process of obtaining a weight for one feature point. The weight for one of the particles of the feature point is indicated.

이와 같은 과정을 이동체가 이동하면서 상기 400 단계 내지 420 단계를 반복한다. The process repeats the steps 400 to 420 while the moving body moves.

한편, 상기 400 단계 내지 420 단계의 수행과정에서 특징점 지도뿐만 아니라, 이동체의 항로 및 전방향 경로 파노라마 영상이 얻어지는데, 본 발명의 다른 일실시예에 따른 칼라화 점유 지도를 생성(110 단계)하기 위해서는 상기 정보들을 저장하는 것이 바람직하다.On the other hand, in the process of performing the steps 400 to 420, as well as the feature point map, the moving path and the omni-directional path panorama image of the moving object is obtained, generating a colorized occupancy map according to another embodiment of the present invention (step 110). In order to store the information, it is desirable.

II. 칼라화 점유 지도 생성 알고리듬(110 단계)II. Colorized occupancy map generation algorithm (step 110)

다음으로, 본 발명의 다른 일 실시예 중의 하나인 칼라화 점유 지도를 생성하는 단계(110)에 대해 기술한다. 상기 지도를 생성하는 방법은 크게 자유 공간 조각(space carving) 과정과 칼라 매핑(color mapping) 과정으로 이루어진다.Next, a step 110 of generating a colorized occupancy map, which is one embodiment of the present invention, is described. The method for generating the map is largely composed of a free space carving process and a color mapping process.

여기서, 공간 조각은 "특징점이 움직이는 이동체에 의해 짧은 주기로 추적된다면, 세 점(두 개의 시공간 상에 연속하는 이동체 위치와 여기서 추적된 특징점 위치)으로 생성된 삼각형은 비어있다"는 원리를 이용한다. 그리고 칼라 매핑 방법으로는, 자유 공간 조각이 완료된 후 조각되지 않은 영역의 표면을 이루는 각 점에 대해 가시성 제한(visibility constraint)을 만족시키는 가장 가까운 이동체 위치로부터 관찰된 칼라를 할당하는 방법을 예로 들 수 있다.Here, the space fragment uses the principle "If a feature is tracked in a short period by a moving object, the triangle created by three points (moving object position on two space-times and the tracked feature point position here) is empty". For example, the color mapping method assigns the observed color from the nearest moving object position that satisfies the visibility constraint for each point that forms the surface of the unsculpted area after the free space piece is completed. have.

도 8은 특징점 추적(tracking) 및 조각 가능한 영역을 나타내는 개념도이다. 이동체는 이동하기 때문에 t시점, t+

시점, t+

시점에서의 위치가 다르다. 또한, 도 8에서는 특징점은 1, 2로서 A, B의 위치에 있다. t 시점의 이동체 위치 및 t+

시점의 이동체 위치에서 모두 특징점 1이 보인다면 삼각형 BDE는 비어 있는 공간으로 볼 수 있어 자유 공간 조각이 된다. 마찬가지로 t+

시점의 이동체 위치 및 t+

시점의 이동체 위치에서 특징점 2가 보인다면 삼각형 ACD가 자유 공간 조각이 된다. 만약, 특징점1, 2가 모두 모든 시점에서 관찰 가능하다면 결국, 회색 영역이 자유 공간 조각이 되는 영역이다.8 is a conceptual diagram illustrating a feature point tracking and engravable area. Since the moving object moves at time t, t +

Time point, t +

The position at the time point is different. In FIG. 8, the feature points are 1 and 2, which are at positions A and B. In FIG. Moving object position at time t and t +

If feature point 1 is seen at the mobile object's position at all, the triangle BDE can be regarded as an empty space, resulting in a free space fragment. Similarly t +

Moving object position at time t +

If feature point 2 is visible at the point of view of the moving object, then the triangular ACD becomes a free space fragment. If the feature points 1 and 2 are both observable at all time points, the gray area is the area where the free space pieces become.

도 9는 본 발명에 의한 칼라화 점유 지도를 생성하는 방법의 동작을 나타내는 흐름도이다.9 is a flowchart illustrating the operation of a method for generating a colorized occupation map according to the present invention.

먼저, 이동체의 항로, 특징점 지도, 이동체의 항로에 따른 전방향 경로 파노라마 영상을 획득한다(900단계). 이는 상기 본 발명에 의한 SLAM(100단계)를 거치면 얻을 수 있는 정보이며, 상기 SLAM(100단계)에서 얻은 정보와는 별도로, 이동체가 다시 항해하여 전술한 바와 같이 이동체의 항로, 특징점 지도 및 전방향 경로 파노라마를 획득할 수 있다.First, an omnidirectional panoramic panorama image according to a moving path of a moving object, a feature point map, and a moving path of a moving object is acquired (step 900). This is information that can be obtained by going through the SLAM (step 100) according to the present invention, and apart from the information obtained in the SLAM (step 100), the moving body sails again, and the course of the moving object, the feature point map and the omni-directional direction as described above. A path panorama can be obtained.

상기 900단계에서 얻은 특징점 지도 및 이동체의 항로 정보를 이용하여 특징점을 추적하고, 상기 추적된 특징점의 가시성을 확인하면서 자유 공간 조각을 수행한다(910단계). 즉, 이동체의 항로 각각의 위치에서 각 특징점이 보이는지를 확인하면서 자유 공간 조각을 하는 것이다.The feature point is tracked using the feature point map obtained in step 900 and the route information of the moving object, and the free space fragment is performed while checking the visibility of the tracked feature point (step 910). That is, pieces of free space are made while checking whether each feature is visible at each position of the moving object.

조각되지 않은 영역 즉, 자유 공간 조각이 완료된 영역의 표면점을 획득하고, 상기 획득된 표면점 각각에 대해 가시성 제한을 만족시키면서 이동체 경로로부터 가장 가까운 이동체의 위치를 결정한다(920단계). 도 8에서 조각되지 않은 영역은 A, F, B로 연결된 면이다.A surface point of the unsculpted area, that is, the area where the free space piece is completed, is obtained, and the position of the moving object closest to the moving object path is determined (step 920) while satisfying the visibility limit for each of the obtained surface points. In FIG. 8, the non-sculpted areas are planes connected by A, F and B.

상기 결정된 각각의 이동체 위치로부터, 대응하는 상기 획득된 각각의 표면점 방향으로의 ORP상 칼라를 추출하여 상기 표면점에 할당한다(930 단계). 즉, A, F, B로 연결된 면에 획득된 칼라를 할당한다.From the determined respective moving object positions, a color on the corresponding ORP in the direction of each obtained surface point is extracted and assigned to the surface point (step 930). That is, the acquired color is assigned to the surface connected by A, F, and B.

도 10은 본 발명의 일실시예에 따른 COM 생성 알고리듬의 결과물인 칼라 지도를 나타낸다. 자유 공간 조각을 수행함으로써, 다각형의 형상이 보이며, 각 면마 다 칼라가 입혀졌음을 알 수 있다. 한편, 도 10에서 동그란 점들은 특징점들이다. 상기 도 10과 같은 COM 영상은 본 발명의 또 다른 일 실시예인 칼라맵을 이용한 위치 추정 알고리듬(120 단계)에 사용된다. 10 shows a color map resulting from a COM generation algorithm in accordance with an embodiment of the present invention. By performing a free space piece, it is possible to see the shape of the polygon and to see that the color is coated on each side. Meanwhile, the circular points in FIG. 10 are feature points. The COM image as shown in FIG. 10 is used for a position estimation algorithm using a color map (step 120), which is another embodiment of the present invention.

III. 칼라 맵을 이용한 위치 추정 알고리듬(120 단계)III. Position Estimation Algorithm Using Color Map (120 steps)

본 발명의 일실시예인 상기 120 단계는 전방향 시각 센서를 이용하여 주위 환경에 대한 수평 전방향 영상을 획득하고, 상기 획득된 수평 전방향 영상을 COM으로부터 생성된 각각의 가설 수평 전방향 영상과 비교함으로써 이루어진다. 여기서, 수평 전방향 영상이란 상기 생성된 COM(칼라 맵)이 표현하는 특정 높이에 대한 전방향 영상이다.In step 120, an embodiment of the present invention acquires a horizontal omnidirectional image of the surrounding environment by using an omnidirectional visual sensor, and compares the acquired horizontal omnidirectional image with each hypothetical horizontal omnidirectional image generated from COM. By doing so. Here, the horizontal omnidirectional image is an omnidirectional image of a specific height represented by the generated COM (color map).

이동체 위치 추정 문제에서, 현재까지 획득된 지식을 가지고 현재의 이동체 위치를 추정하게 된다. 획득된 지식은, 현재 시간 단계까지의 이동체로부터 전방향으로 획득되는 칼라 정보

, 이전 시간 단계까지의 제어 입력

, 칼라 점유 지도

이며, 이것이 주어졌을 때 이동체 위치 추정의 문제는 베이시안(Bayesian) 필터링의 일예로서, 우리가 관심 있는 것은 모든 측정치를 조건으로 현재 상태의 사후 확률 밀도

를 검출하는 것이다. 여기서,

는

로서, 이동체의 수평 좌표 및 이동체가 향한 방향을 의미한다. 상기 확률 밀도 함수는 가우시안 분포가 아닌 다수의 국부 최대치(local maximum)를 갖는 경향이 있으며, 특히 글로벌 위치 추정 단계인 초기 위치 추정의 경우는 더욱 그러하다. In the moving object estimation problem, the current moving object position is estimated with the knowledge acquired to date. The acquired knowledge is color information obtained in all directions from the moving object up to the current time step.

Control input up to the previous time step

Color occupancy map

Given this, the problem of mobile position estimation is an example of Bayesian filtering, which we are interested in is the post-probability density of the current state, subject to all measurements.

Will be detected. here,

Is

As the horizontal coordinates of the moving object and the direction toward which the moving object is directed. The probability density function tends to have a large number of local maximums that are not Gaussian distributions, especially for initial position estimation, which is a global position estimation step.

여기서,

를 구하기 위한 확률 전파 모델은 수학식 7과 같으며 이는 수학식 1에서 지도

이 주어질 경우, 상태

를 이동체의 위치와 방향으로 정의한 것과 동일하다. 이 경우 추정하고자 하는 상태의 차원이 3차원으로 비교적 낮으므로 상기 입자 필터의 직접 적용이 용이하다.here,

The probability propagation model to find is

Given this, state

Is defined as the position and direction of the moving object. In this case, since the dimension of the state to be estimated is relatively low in three dimensions, it is easy to directly apply the particle filter.

전술한 바와 같이, 입자 필터는 파티클의 밀도로서 확률 분포를 표시되며, 사후 확률 분포는 '이동 갱신' 및 '측정치 갱신'을 번갈아 수행하는 것으로 변형된다. 측정치로부터 생성된 가중치(weight scale)는 단일 파티클의 존속 및 증식 확률을 결정한다. 상기 수학식 7로 표현되는 사후 확률 분포를 구하기 위해서, 수학식 8로 표현되는 이동 갱신과, 수학식 9로 표현되는 측정치 갱신이 번갈아 수행된다.As mentioned above, the particle filter displays a probability distribution as the particle's density, and the post probability distribution is modified to alternately perform 'movement update' and 'measurement update'. The weight scale generated from the measurements determines the survival and propagation probability of a single particle. In order to obtain the posterior probability distribution represented by Equation 7, the shift update represented by Equation 8 and the measured value update represented by Equation 9 are alternately performed.

첫째 단계에서, 로봇의 현재 상태를 예측하기 위한 조건부 확률

로 특정되는 이동 모델을 사용한다. 마르코프 가정을 사용하면,

는 이전의 상태

와 알려진 제어 입력

에만 의존적이다.In the first step, conditional probabilities to predict the current state of the robot

Use the movement model specified by. Using Markov homes,

Is the previous state

And known control inputs

Only depends.

둘째 단계에서, 사후 확률 밀도

를 계산하기 위해, 측정치

는

가 주어진 조건에서 과거의 측정치

와는 조건적 독립(conditionally independent)이라고 가정한다.In the second stage, post probability density

To calculate, measure

Is

Past measurements under given conditions

Is assumed to be conditionally independent.

본 발명의 일실시예에 따르면 측정치 정보는 도 3a의 각 시점 즉, 특정 프레임에 대한 영상과 비슷한 형태의 1차원의 전방향 칼라 영상이다. 또한, 사용되는 칼라 맵은 도 10의 형태일 것이나, 개념적으로 명확히 설명하기 위해 도 11과 같이 각 면이 균일한 형태를 이루는 칼라 맵을 이용하여 본 발명을 설명한다.According to an embodiment of the present invention, the measured value information is a one-dimensional omnidirectional color image having a form similar to that of each viewpoint of FIG. 3A, that is, a specific frame. In addition, although the color map used will be in the form of FIG. 10, the present invention will be described using a color map in which each surface has a uniform shape as shown in FIG.

수학식 9에서 사용되는 관찰 확률 분포

는, 시간에 따라 변동이 없는 것으로 가정한다.Observation probability distribution used in equation (9)

Assumes that there is no change over time.

는 이동체가 t 시점의 위치에서 바라보는 전방향으로 바라봤을 때 각각의 방위각

에 대해 측정된 칼라 분포를 나타낸다. 칼라의 기본 정보인 RGB성분으로 구성되어 있음을 알 수 있다. 즉,

가 현재 위치

에서 획득한 칼라 영상이다.

Is the azimuth angle when the moving object is seen in the omnidirectional direction from the point t

The color distribution measured for. It can be seen that it is composed of RGB components that are basic information of colors. In other words,

Is your current location

Color image obtained from.

한편, 칼라화 점유 지도로부터 각 이동체 파티클 위치

에 따른 칼라 영상

을 얻을 수 있다. 상기

은

및

에 의한 가설 칼라 분포로 표현될 수 있으므로, 관찰 확률 분포는 수학식 10으로 정의된다.On the other hand, the location of each mobile particle from the colorized map

Color image according to

Can be obtained. remind

silver

And

Since it can be expressed by the hypothesis color distribution by, the observation probability distribution is defined by Equation 10.

여기서,

이며,

는 예측된 잡음 분산(expected variance of noise)으로 볼 수 있다. 이 파라미터는 효과적으로 관찰 확률 분포 함수의 모양을 제어한다.

가 작은 값으로 선택된 경우, 해당 확률 분포는

이 최소가 되게 하는 위치 주변으로 집중되며, 그 값으로부터 벗어나지 않는 모양이 된다. 그러나, 초기에 측정된 칼라와 유사한 가설 칼라를 가진 그릇된 위치의 이동체 파티클이 존재하는 경우에는, 정확한 위치로 회복하기가 어렵다.here,

Is,

Can be seen as the expected variance of noise. This parameter effectively controls the shape of the observed probability distribution function.

If is chosen to be a small value, the probability distribution is

It is concentrated around the position that makes this minimum, and it does not deviate from its value. However, if there is a wrong position of mobile particles with hypothetical collar similar to the initially measured color, it is difficult to recover to the correct position.

반면에

이 큰 값으로 선택된 경우, 글로벌 위치 추정에서 확률 분포 함수의 초기 집중은 느리지만, 발산할 위험은 일반적으로 줄어든다.On the other hand

If this large value is chosen, the initial concentration of the probability distribution function in the global position estimate is slow, but the risk of divergence is generally reduced.

본 발명은 또한 컴퓨터로 읽을 수 있는 기록매체에 컴퓨터가 읽을 수 있는 코드로서 구현하는 것이 가능하다. 컴퓨터가 읽을 수 있는 기록매체는 컴퓨터 시스템에 의해 읽혀질 수 있는 데이터가 저장되는 모든 종류의 기록장치를 포함한다. 컴퓨터가 읽을 수 있는 기록매체의 예로는 ROM, RAM, CD-ROM, 자기 테이프, 플로피 디스크, 광데이터 저장장치 등이 있으며, 또한 케리어 웨이브(예를 들어 인터넷을 통한 전송)의 형태로 구현되는 것도 포함한다. 또한, 컴퓨터가 읽을 수 있는 기록매체는 네트워크로 연결된 컴퓨터 시스템에 분산되어, 분산방식으로 컴퓨터가 읽을 수 있는 코드가 저장되고 실행될 수 있다. 그리고 본 발명을 구현하기 위한 기능적 인(functional) 프로그램, 코드 및 코드 세그먼트들은 본 발명이 속하는 기술분야의 프로그래머들에 의해 용이하게 추론될 수 있다.The invention can also be embodied as computer readable code on a computer readable recording medium. Computer-readable recording media include all kinds of recording devices that store data that can be read by a computer system. Examples of computer-readable recording media include ROM, RAM, CD-ROM, magnetic tape, floppy disks, optical data storage devices, and the like, which are also implemented in the form of carrier waves (for example, transmission over the Internet). Include. The computer readable recording medium can also be distributed over network coupled computer systems so that the computer readable code is stored and executed in a distributed fashion. And functional programs, codes and code segments for implementing the present invention can be easily inferred by programmers in the art to which the present invention belongs.

이러한 본원 발명인 방법 및 장치는 이해를 돕기 위하여 도면에 도시된 실시예를 참고로 설명되었으나, 이는 예시적인 것에 불과하며, 당해 분야에서 통상적 지식을 가진 자라면 이로부터 다양한 변형 및 균등한 타 실시예가 가능하다는 점을 이해할 것이다. 따라서, 본 발명의 진정한 기술적 보호 범위는 첨부된 특허청구범위에 의해 정해져야 할 것이다.Such a method and apparatus of the present invention have been described with reference to the embodiments shown in the drawings for clarity, but these are merely exemplary, and various modifications and equivalent other embodiments are possible to those skilled in the art. Will understand. Therefore, the true technical protection scope of the present invention will be defined by the appended claims.

본 발명에 따르면, SLAM에 입자 필터를 적용할 때 발생하는 복잡도 문제를 극복하기 위하여, 이동체 위치와 특징점 위치에 대한 상태 변수를 하나의 상태 변수로 사용하여 갱신하는 방법 대신에, 상기 상태 변수들을 이동체 위치 상태와 특징점 위치 상태로 각각 분할하고, 상태 변수 갱신 방법도 상기 두 상태 그룹에 대한 갱신을 번갈아 수행함으로써 SLAM 프로세스를 위해 요구되는 계산량을 특징점 개수에 선형적인 크기가 되도록 줄일 수 있다.According to the present invention, in order to overcome the complexity problem of applying the particle filter to the SLAM, instead of updating the state variables for the mobile body position and the feature point position as one state variable, the state variables are moved. By splitting the position state and the feature point position state, respectively, the state variable update method can also reduce the amount of calculation required for the SLAM process to be linear in the number of the feature points by alternately performing updates for the two state groups.

또한, 전방향 시각 센서만을 가지고 칼라 점유 매핑이 가능하며, 이를 통해 다중의 정보를 제공할 수 있다. 또한, 상기 칼라 점유 매핑에 의한 칼라화 점유 지도 생성 및 전방향 시각 센서 한 개만을 이용하여 위치 추정이 가능하다.In addition, color occupancy mapping is possible using only the omnidirectional visual sensor, thereby providing multiple information. In addition, it is possible to generate a colorized occupancy map by the color occupancy mapping and to estimate a location using only one omnidirectional visual sensor.

Claims

(a) obtaining an omnidirectional panoramic image of the exploration environment at the current position of the moving object, and using the obtained omnidirectional panoramic image, detecting the position of each feature point present in the exploration environment as an azimuth from the moving object; step;

(b) updating the posterior probability distribution of the current moving object position based on the detected azimuth angle and the posterior probability distribution of the feature point position previously updated; And

(c) updating the posterior probability distribution of the feature point position based on the detected azimuth angle and the posterior probability distribution of the updated moving object position,

And repeating steps (a) to (c), wherein the method for omnidirectional visual sensor-based position estimation and mapping is performed in the particle filter framework.

According to claim 1, wherein step (b),

A weight is calculated for each of the moving particles based on the number of feature point particles located on an extension line from the particles of each moving object to the detected azimuth, and based on the calculated weights of the moving particles. A method for omnidirectional visual sensor based position estimation and mapping in a particle filter framework characterized by updating a current posterior probability distribution of a moving object.

The method of claim 1, wherein step (c) comprises:

Considering an extension line in each of the detected azimuth directions from each of the moving particles according to the posterior probability distribution of the updated moving object,

For each particle of the feature point, a weight is calculated for each feature point particle based on the number of extension lines passing through the feature point particle, and the posterior probability distribution of each feature point is based on the calculated weight of the feature point particle. A method for omnidirectional visual sensor based position estimation and mapping in a particle filter framework, characterized in that for updating.

The method of claim 1,

(d) An omnidirectional path panoramic image obtained by stacking omnidirectional panoramic images of each moving time point obtained by repeating steps (a) to (c), and a moving object route that combines the positions of the moving objects at each moving time point. Performing free space carving by determining visibility of each feature point at each position of the moving object route based on the generated feature point map;

(e) obtaining a surface point of the unsculpted area after passing through the free space fragment, and satisfying a visibility condition with respect to the obtained surface point using the omnidirectional path panoramic image, Determining a location; And

(f) extracting an omnidirectional panoramic image obtained at the position of the determined moving object from the omnidirectional panoramic panorama image, extracting a color of the obtained surface point direction from the extracted omnidirectional panoramic image, And assigning the extracted color to a surface point to generate a colorized occupancy map.

The method of claim 4, wherein

(g) the moving object moving the exploration environment and acquiring an omnidirectional horizontal panoramic image at each moving position;

(h) generating an omnidirectional horizontal panoramic image at each mobile particle position from the generated colorized occupancy map, and comparing the generated omnidirectional horizontal panoramic image with the acquired omnidirectional horizontal panoramic image, Updating the posterior probability distribution for the position of the moving body,

And repeating steps (g) to (h). 3. The method for omnidirectional visual sensor based position estimation and mapping in a particle filter framework.