KR101596323B1 - System and method for self-localizing and a medium having computer readable program for executing the method - Google Patents
System and method for self-localizing and a medium having computer readable program for executing the method Download PDFInfo
- Publication number
- KR101596323B1 KR101596323B1 KR1020140027335A KR20140027335A KR101596323B1 KR 101596323 B1 KR101596323 B1 KR 101596323B1 KR 1020140027335 A KR1020140027335 A KR 1020140027335A KR 20140027335 A KR20140027335 A KR 20140027335A KR 101596323 B1 KR101596323 B1 KR 101596323B1
- Authority
- KR
- South Korea
- Prior art keywords
- particle filter
- filter
- estimation
- particle
- resetting
- Prior art date
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/40—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4808—Evaluating distance, position or velocity data
Abstract
위치 추정 시스템, 방법, 및 상기 방법을 실행시키기 위한 컴퓨터 판독 가능한 프로그램을 기록한 매체가 개시된다. 위치 추정 시스템은, 위치 추정부, 추정 성능 평가부, 및 파티클 필터 재설정부를 포함한다. 위치 추정부는 파티클 필터를 이용하여 이동체의 위치를 추정하고, 추정 성능 평가부는 위치 추정 결과를 미리 설정된 기준에 따라 판단하여 파티클 필터의 추정 성능을 평가하며, 파티클 필터 재설정부는 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR(Finite Impulse Response) 필터를 이용하여 파티클 필터를 재설정(reset)한다. 이와 같은 구성에 의하면, 파티클 필터의 추정 성능을 평가하고 평과 결과에 따라 파티클 필터를 재설정함으로써, 위치 추정을 위해 사용되는 파티클 필터에서 'sample impoverishment'로 인해 위치 추정 실패가 발생할 경우에도 파티클 필터의 기능을 정상으로 회복시켜 정확한 위치 추정을 다시 수행할 수 있게 된다.A location estimation system, method, and medium recording a computer readable program for executing the method are disclosed. The position estimation system includes a position estimation unit, an estimation performance evaluation unit, and a particle filter reset unit. The position estimating unit estimates the position of the moving object by using the particle filter, and the estimation performance evaluating unit evaluates the estimation performance of the particle filter by determining the position estimation result according to a preset reference. The particle filter resetting unit The particle filter is reset using a finite impulse response (FIR) filter. According to such a configuration, even if a position estimation failure occurs due to 'sample impoverishment' in a particle filter used for position estimation by evaluating the estimation performance of the particle filter and resetting the particle filter according to the evaluation result, And the accurate position estimation can be performed again.
Description
본 발명은 자율 이동 로봇, 무인 자동차 등의 자율 주행 시스템에서 사용되는 자기 위치 추정(self-localization) 기술에 관한 것으로서, 더욱 상세하게는 파티클 필터(particle filter)를 이용하여 자기 위치 추정을 수행하는 시스템, 및 방법에 관한 것이다.BACKGROUND OF THE
사람의 조작 없이 스스로 목표지점을 찾아가는 자율 주행 시스템은 1) 먼저, 주어진 지도(map)에서 자기 위치를 파악하고, 2) 목표지점까지의 효율적인 경로를 생성한 뒤, 3) 그 경로를 따라 장애물 등을 회피하며 목표점까지 주행한다. The autonomous navigation system, which locates the target point by itself without human manipulation, can be classified as follows: 1) First, it grasps its position in a given map, 2) creates an efficient path to the target point, and 3) And travels to the target point.
이러한 자율 주행 시스템에서 필요로 하는 요소 기술은 위치 추정(self-localization 또는 localization), 지도 작성(map building), 경로 계획(path planning), 장애물 회피(obstacle avoidance) 등이다. 이 중에서 가장 중요하고도 기본이 되는 기술은 자기 위치 추정인데, 그 이유는 로봇이나 차량이 지도상의 자기 위치를 알아내는 것이 자율 주행의 첫 번째 단계이기 때문이다. The elemental technologies required for such an autonomous navigation system are self-localization or localization, map building, path planning, obstacle avoidance, and the like. The most important and fundamental technique is self-localization because it is the first step of autonomous navigation for a robot or vehicle to find its position on the map.
자기 위치 추정에 필요한 지도정보는 시스템에 사전 입력되어있는 경우도 있고 주행 중에 직접 지도를 작성하기도 한다. 전자는 순수한 의미의 위치 추정이며 후자는 위치 추정과 지도 작성을 동시에 수행하는 기술로서 SLAM(Simultaneous Localization And Mapping)이라고 불린다.The map information necessary for the self-localization may be pre-input in the system or may be directly generated during driving. The former is a position estimation in a pure sense, and the latter is called a simultaneous localization and mapping (SLAM) as a technique for performing position estimation and mapping at the same time.
위치 추정 기술은 자율 이동 로봇, 무인 자동차 등에서 주로 사용되고 있다. 대표적인 사례는 청소 로봇, 군사용 경계 로봇, 무인 자동차 등이 있다. 이뿐만 아니라 위치 추정 기술은 사람이나 사물의 위치를 추적하는데도 활용된다. Position estimation techniques are mainly used in autonomous mobile robots and unmanned vehicles. Typical examples are cleaning robots, military bounding robots, and unmanned vehicles. In addition to this, location estimation techniques are also used to track the location of people or objects.
예를 들어, 건설 현장에서 작업자와 건축 자재, 설비에 RFID 태그를 부착하여 그 위치를 실시간으로 모니터링하는 시스템에도 위치 추정 기술이 사용된다. 또한, 레이더(radar)를 이용하여 물체의 위치를 추적하는 데에도 위치 추정 기술이 사용된다. For example, location estimation technology is used in a system that monitors the location of an RFID tag attached to an operator, building materials, or equipment at a construction site in real time. In addition, a position estimation technique is used to track the position of an object using a radar.
'위치 추정'이라는 용어는 어떤 대상의 위치를 측정(measurement)을 통해 알아낼 수 없는 경우, 또는 위치를 측정할 수는 있지만 센서에서 발생하는 잡음(noise) 때문에 측정값을 신뢰하기 어려운 경우에 수학적인 도구를 이용하여 보다 정확하고 신뢰할 수 있는 위치정보를 생성해주기 때문에 붙여진 이름이다. 즉, 센서의 측정값을 이용하여 더 정확한 값을 추정(estimation)해주는 것인데, 여기에 사용되는 수학적 도구가 바로 필터(filter)이다. The term 'position estimate' is used to refer to the position of a subject when it is not possible to determine the position of a subject through a measurement, or if it is difficult to trust the measurement because of the noise generated by the sensor, This is the name given because it uses tools to generate more accurate and reliable location information. That is, a more accurate value is estimated by using the measured value of the sensor. The mathematical tool used here is a filter.
위치 추정 시스템을 하드웨어와 소프트웨어로 구분한다면, 하드웨어 부분의 핵심은 정확도 높은 센서를 사용하는 것이 될 것이다. 위치 추정 시스템 중에서 소프트웨어 부분의 핵심은 필터라는 수학적 도구(또는 알고리즘)이다. 필터는 센서로부터 얻은 측정값을 처리하여 위치 정보를 추정해준다. If the location estimation system is divided into hardware and software, the core of the hardware part will be to use sensors with high accuracy. At the heart of the software part of the position estimation system is a mathematical tool (or algorithm) called a filter. The filter processes the measured values from the sensor and estimates the position information.
최근 가장 많이 사용되며 뛰어난 성능을 보여주는 대표적인 필터는 파티클 필터이다. 그런데 파티클 필터는 ‘sample impoverishment’라는 문제점을 갖고 있다. 이 문제점은 특정한 조건 하에서 샘플들의 다양성이 악화되어 필터의 추정 오차가 발산하는 현상이다. 이 문제가 발생하면 파티클 필터는 실제 위치와 큰 차이가 나는 엉뚱한 위치 값을 생성해주게 되므로 결국 위치 추정에 실패하게 된다. One of the most popular filters used today is the particle filter. Particle filters, however, have the problem of 'sample impoverishment'. This problem is a phenomenon in which the diversity of samples under certain conditions deteriorates and the estimation error of the filter diverges. When this problem occurs, the particle filter generates an incorrect position value which is significantly different from the actual position, and thus, the position estimation fails.
‘sample impoverishment’는 파티클 필터가 갖고 있는 근본적인 문제점으로서 1) 시스템 잡음(process noise )이 작은 경우, 2) 측정 잡음(measurement noise)이 작은 경우, 3) 필터링에 사용한 샘플의 개수가 부족한 경우, 4) 시스템에 강한 외란이 발생한 경우 등 여러 가지 조건 하에서 일어날 수 있다. The 'sample impoverishment' is a fundamental problem with particle filters: 1) the process noise is small, 2) the measurement noise is small, and 3) the number of samples used for filtering is insufficient. ) It may happen under various conditions such as strong disturbance in system.
이 문제점을 해결하기 위해 개선된 파티클 필터들이 제안되었는데, 대표적으로 regularized particle filter, auxiliary particle filter, MCMC(Markov-Chain Monte-Carlo) move step, Mixture MCL(Monte-Carlo Localization) 등이 있다. 그러나 이러한 방법들은 'sample impoverishment' 같은 문제가 발생할 가능성을 줄여줄 뿐, 완전하게 예방하는 것은 아니다. In order to solve this problem, improved particle filters have been proposed, such as regularized particle filter, auxiliary particle filter, MCMC (Markov-Chain Monte-Carlo) move step and Mixture MCL (Monte-Carlo Localization). However, these methods reduce the likelihood of problems such as 'sample impoverishment' and do not prevent them completely.
또한, 앞에서 열거한 개선된 파티클 필터들은 어디까지나 예방적인 조치일 뿐 'sample impoverishment'가 이미 발생한 상황에서 파티클 필터의 기능을 정상적으로 회복시켜 주지는 못한다. In addition, the improved particle filters listed above are only preventive measures and do not normally restore the function of the particle filter in case of 'sample impoverishment' already occurred.
이에 따라, 위치 추정 시스템에 사용된 파티클 필터에서 'sample impoverishment'와 이로 인한 파티클 필터의 추정 실패가 일어나는 경우, 이동로봇이나 무인자동차는 자신의 위치를 제대로 파악하지 못해 목표지점이 아닌 엉뚱한 곳으로 이동하게 되는 문제가 발생한다. Accordingly, when 'sample impoverishment' in the particle filter used in the position estimation system and the estimation failure of the particle filter due to the 'sample impoverishment' occur, the mobile robot or the unmanned vehicle can not grasp the position of the self, A problem occurs.
본 발명은 상술한 종래의 문제점을 해결하기 위해 안출된 것으로서, 위치 추정을 위해 사용되는 파티클 필터에서 'sample impoverishment'로 인해 위치 추정 실패가 발생할 경우에도 파티클 필터의 기능을 정상으로 회복시켜 정확한 위치 추정을 수행할 수 있도록 해주는 위치 추정 시스템, 및 방법을 제공하는 것을 목적으로 한다.SUMMARY OF THE INVENTION The present invention has been conceived to solve the above-mentioned problems of the prior art, and it is an object of the present invention to provide a position estimation method and a position estimation method capable of accurately recovering the function of a particle filter even when a position estimation failure occurs due to 'sample impoverishment' And to provide a method and a method for estimating a position of a vehicle.
상기 목적을 달성하기 위해 본 발명에 따른 위치 추정 시스템은, 위치 추정부, 추정 성능 평가부, 및 파티클 필터 재설정부를 포함한다. 위치 추정부는 파티클 필터를 이용하여 이동체의 위치를 추정하고, 추정 성능 평가부는 위치 추정 결과를 미리 설정된 기준에 따라 판단하여 파티클 필터의 추정 성능을 평가하며, 파티클 필터 재설정부는 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR(Finite Impulse Response) 필터를 이용하여 파티클 필터를 재설정(reset)한다.In order to achieve the above object, a position estimation system according to the present invention includes a position estimation unit, an estimation performance evaluation unit, and a particle filter reset unit. The position estimating unit estimates the position of the moving object by using the particle filter, and the estimation performance evaluating unit evaluates the estimation performance of the particle filter by determining the position estimation result according to a preset reference. The particle filter resetting unit The particle filter is reset using a finite impulse response (FIR) filter.
이와 같은 구성에 의하면, 파티클 필터의 추정 성능을 평가하고 평과 결과에 따라 파티클 필터를 재설정함으로써, 위치 추정을 위해 사용되는 파티클 필터에서 'sample impoverishment'로 인해 위치 추정 실패가 발생할 경우에도 파티클 필터의 기능을 정상으로 회복시켜 정확한 위치 추정을 다시 수행할 수 있게 된다.According to such a configuration, even if a position estimation failure occurs due to 'sample impoverishment' in the particle filter used for position estimation by evaluating the estimation performance of the particle filter and resetting the particle filter according to the evaluation result, And the accurate position estimation can be performed again.
이때, 파티클 필터 재설정부는 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR 필터를 구동하는 FIR 필터 구동부, 및 FIR 필터로부터 파티클 필터 재설정에 필요한 정보를 획득하는 재설정 정보 획득부를 포함할 수 있다. 이와 같은 구성에 의하면, 통상적인 위치 추정은 파티클 필터를 이용하여 수행하고 FIR 필터는 파티클 필터의 이상시에만 파티클 필터의 재설정을 위해서 사용되므로, 전체 위치 추정 과정을 보다 효율적으로 수행할 수 있게 된다.In this case, the particle filter resetting unit may include an FIR filter driving unit for driving the FIR filter when the estimation performance of the particle filter is deteriorated, and a reset information obtaining unit for obtaining information necessary for resetting the particle filter from the FIR filter. According to such a configuration, since the conventional position estimation is performed using a particle filter, and the FIR filter is used for resetting the particle filter only when the particle filter is abnormal, the entire position estimation process can be performed more efficiently.
이때, 파티클 필터의 재설정에 필요한 정보는 FIR 필터를 이용해 추정된 이동체의 위치 추정값, 및 위치 추정 오차 공분산값을 포함할 수 있다. 이와 같은 구성에 의하면, 파티클 필터가 보다 빠른 속도로 정상 동작을 수행할 수 있게 된다.At this time, information necessary for resetting the particle filter may include a position estimation value of the moving object estimated using the FIR filter, and a position estimation error covariance value. According to such a configuration, the particle filter can perform normal operation at a higher speed.
또한, 추정 성능 평가부는 파티클 필터가 샘플링한 샘플들 중 센서 측정값으로부터 미리 설정된 범위 내에 포함되는 샘플의 개수가 미리 설정된 개수보다 적을 경우 파티클 필터의 추정 성능이 저하되었다고 판단할 수 있으며, 특히, 마할라노비스 거리를 이용하여 샘플들이 미리 설정된 범위 내에 포함되는 지를 판단할 수 있다.The estimation performance evaluating unit may determine that the estimation performance of the particle filter is deteriorated when the number of samples included in the predetermined range from the sensor measurement values among the samples sampled by the particle filter is smaller than a predetermined number, It is possible to judge whether the samples are included in the predetermined range by using the La nobis distance.
아울러, 상기 시스템을 방법의 형태로 구현한 발명과 그 방법을 실행하기 위한 컴퓨터 판독 가능한 프로그램이 기록된 매체가 함께 개시된다.In addition, there is disclosed an invention in which the system is implemented in the form of a method and a medium in which a computer-readable program for executing the method is recorded.
본 발명에 의하면, 파티클 필터의 추정 성능을 평가하고 평과 결과에 따라 파티클 필터를 재설정함으로써, 위치 추정을 위해 사용되는 파티클 필터에서 'sample impoverishment'로 인해 위치 추정 실패가 발생할 경우에도 파티클 필터의 기능을 정상으로 회복시켜 정확한 위치 추정을 다시 수행할 수 있게 된다.According to the present invention, by evaluating the estimation performance of the particle filter and resetting the particle filter according to the evaluation result, even if the position estimation failure occurs due to 'sample impoverishment' in the particle filter used for position estimation, It is possible to recover to the normal state and perform accurate position estimation again.
또한, 통상적인 위치 추정은 파티클 필터를 이용하여 수행하고 FIR 필터는 파티클 필터의 이상시에만 파티클 필터의 재설정을 위해서 사용되므로, 전체 위치 추정 과정을 보다 효율적으로 수행할 수 있게 된다.In addition, since the conventional position estimation is performed using a particle filter and the FIR filter is used for resetting the particle filter only when the particle filter is abnormal, the entire position estimation process can be performed more efficiently.
또한, 파티클 필터가 보다 빠른 속도로 정상 동작을 수행할 수 있게 된다.In addition, the particle filter can perform normal operation at a higher speed.
도 1은 본 발명의 일 실시예에 따른 위치 추정 시스템의 개략적인 블록도.
도 2는 주파수 영역의 필터와 시간 영역의 필터(추정기)의 차이점을 도시한 개략적인 도면.
도 3은 IIR 필터와 FIR 필터의 차이점을 개략적으로 도시한 도면이다.
도 4는 본 발명의 일 실시예에 따른 위치 추정 방법을 수행하기 위한 개략적인 흐름도.
도 5는 파티클 필터의 추정 실패 또는 추정 성능 저하의 판단 과정을 도시한 개략적인 도면.
도 6 내지 도 8은 도 1의 위치 추정 시스템의 실제 위치 추정 결과를 도시한 도면. 1 is a schematic block diagram of a position estimation system in accordance with an embodiment of the present invention;
2 is a schematic diagram showing the difference between a filter in the frequency domain and a filter in the time domain (estimator);
3 is a view schematically showing the difference between the IIR filter and the FIR filter.
4 is a schematic flow chart for performing a position estimation method according to an embodiment of the present invention;
FIG. 5 is a schematic view showing a process of determining an estimation failure or estimation performance degradation of a particle filter; FIG.
Figs. 6 to 8 are diagrams showing actual position estimation results of the position estimation system of Fig. 1; Fig.
이하, 첨부된 도면을 참조하여 본 발명의 바람직한 실시예를 설명한다.Hereinafter, preferred embodiments of the present invention will be described with reference to the accompanying drawings.
도 1은 본 발명의 일 실시예에 따른 위치 추정 시스템의 개략적인 블록도이다. 1 is a schematic block diagram of a position estimation system in accordance with an embodiment of the present invention.
도 1에서, 위치 추정 시스템(100)은 위치 추정부(110), 추정 성능 평가부(120), 및 파티클 필터 재설정부(130)를 포함하며, 파티클 필터 재설정부(130)는 내부에 다시 FIR 필터 구동부(132), 및 재설정 정보 획득부(134)를 포함한다. 1, the
이때, 위치 추정 시스템(100)의 각 구성 요소들은 하드웨어로만 구현할 수도 있겠지만, 하드웨어 상에서 동작하는 소프트웨어로 구현할 수도 있다.At this time, each component of the
위치 추정부(110)는 파티클 필터를 이용하여 이동체의 위치를 추정한다. 이때, 이동체란 위치 추정의 대상이 되는 객체로서, 입체 형태로 구현된 이동체인 것이 일반적이겠으나, 디스플레이 화면에서 이동하는 평면 형태의 이동체와 같이 다른 형태의 이동체로도 구현할 수 있을 것이다.The
필터는 원래 불필요한 것을 걸러내는 도구를 지칭한다. 신호처리 분야에서 사용되는 필터는 원하는 주파수 대역을 제외한 나머지의 불필요한 신호를 제거해주는 역할을 한다. A filter is a tool that originally filters out unwanted stuff. The filter used in the signal processing field removes other unnecessary signals except the desired frequency band.
위치 추정에서 사용하는 통계적 필터는 물체의 위치 정보를 불확실하게 만드는 요소인 측정 잡음(measurement noise), 프로세스 잡음(process noise) 등의 영향을 제거해 준다. The statistical filters used in position estimation remove the effects of measurement noise, process noise, etc., which make the position information of the object uncertain.
통계적 필터는 신호처리 분야에서 사용되는 주파수 영역의 필터와 대비하여 시간 영역 필터라고도 불리며, 시스템의 상태 값을 추정해주므로 추정기(estimator)라고도 불린다. A statistical filter is also called a time domain filter in comparison with a frequency domain filter used in the signal processing field and is also called an estimator because it estimates the state value of the system.
도 2는 주파수 영역의 필터와 시간 영역의 필터(추정기)의 차이점을 도시한 개략적인 도면이다. 이러한 시간 영역의 필터(이하 필터)를 사용하기 위해서는, 추정하고자 하는 변수를 상태 변수로 지정하여 시스템을 상태 공간 모델(state-space model)로 표현해야 한다. 2 is a schematic diagram showing the difference between a filter in the frequency domain and a filter in the time domain (estimator). In order to use a filter (hereinafter, referred to as a filter) in the time domain, the system should be represented as a state-space model by designating a variable to be estimated as a state variable.
가장 기본적인 선형 시불변 시스템(linear time-invariant system)의 상태 공간 모델은 다음과 같이 표현된다.
The state-space model of the most basic linear time-invariant system is expressed as:
여기에서 변수 x, u, y, w, v 는 각각 시스템의 상태(state), 입력(input), 출력(output), 프로세스 잡음(process noise), 측정 잡음(measurement noise)을 의미한다. 또한, A, B, C, D 는 앞의 변수들 간의 상관관계를 표현하는 행렬들이다. 필터(추정기)는 알 수 있는 정보인 시스템의 입력 u, 출력 y를 이용하여, 알 수 없는 정보인 상태 변수 x를 추정해주는 역할을 한다. Here, variables x, u, y, w, and v mean the state, input, output, process noise, and measurement noise of the system, respectively. A, B, C, and D are matrices representing the correlation between the preceding variables. The filter (estimator) estimates the state variable x, which is unknown information, using the input u and output y of the system, which are known information.
가장 대표적인 필터(추정기)로는 칼만 필터(Kalman filter)가 있다. 칼만 필터는 1960년대에 루돌프 칼만(Rudolf E. Kalman)이 개발한 필터로, 백색 가우시안 잡음(white Gaussian noise)을 갖는 선형 시스템의 상태 변수를 추정하는데 사용된다. 칼만 필터는 선형 시스템에만 적용 가능하기 때문에 적용 분야가 협소하다는 단점이 있다. The most representative filter (estimator) is Kalman filter. The Kalman filter is a filter developed by Rudolf E. Kalman in the 1960s and is used to estimate the state variables of a linear system with white Gaussian noise. The Kalman filter has a disadvantage in that the application field is narrow because it is applicable only to a linear system.
이를 해결하기 위해 비선형시스템에 적용 가능한 칼만 필터가 개발되었는데, 그것이 바로 확장 칼만 필터(extended Kalman filter, EKF)이다. 그러나 확장 칼만 필터 역시 칼만 필터에 기반을 두고 있으므로 시스템이 반드시 백색 가우시안 잡음을 가져야 한다는 제약 조건이 있다. 이 문제를 해결하기 위해 등장한 필터가 바로 파티클 필터(particle filter)이다. To solve this problem, a Kalman filter which can be applied to a nonlinear system has been developed, which is an extended Kalman filter (EKF). However, since the extended Kalman filter is also based on the Kalman filter, there is a constraint that the system must have white Gaussian noise. To solve this problem, a particle filter is an emerging filter.
칼만 필터는 상태 변수를 가우시안 분포를 갖는 확률 변수(random variable)로 표현하는데 반해, 파티클 필터는 상태 변수를 샘플들의 집합으로 표현한다. 즉, 칼만 필터에서는 상태 변수를 평균(mean)과 공분산(covariance)을 갖는 정규분포함수로 표현하는데 반해, 파티클 필터에서는 상태변수가 가질 수 있는 구체적인 값들의 집합으로 표현하는 것이다. A Kalman filter represents a state variable as a random variable with a Gaussian distribution, while a particle filter represents a state variable as a set of samples. In the Kalman filter, the state variable is represented by a normal distribution function having mean and covariance, while the particle filter is expressed by a set of specific values that the state variable can have.
파티클 필터는 비선형(nonlinear)/비가우시안 잡음(non-Gaussian noise) 시스템에 적용이 가능하며, 시스템을 선형화하는 과정을 거치지 않기 때문에 비선형성이 큰 시스템에서 확장 칼만 필터에 비해 월등한 성능을 보여준다. 파티클 필터는 1990년대에 개발되었으며 현재 가장 많이 사용되는 대표적인 비선형 필터이다. Particle filters can be applied to nonlinear / non-Gaussian noise systems and are superior to extended Kalman filters in systems with large nonlinearities because the system is not linearized. Particle filters were developed in the 1990s and are currently the most popular nonlinear filters.
이와 같이, 파티클 필터를 이용하여 물체의 위치를 추정하는 기술 자체는 이미 존재하는 기술이므로 이에 대한 더 이상의 추가 설명은 생략한다.As described above, since the technique itself for estimating the position of an object using the particle filter is already existing, further explanation for it will be omitted.
추정 성능 평가부(120)는 위치 추정 결과를 미리 설정된 기준에 따라 판단하여 파티클 필터의 추정 성능을 평가한다.The estimation
이를 위해, 추정 성능 평가부(120)는 파티클 필터가 샘플링한 샘플들 중 센서 측정값으로부터 미리 설정된 범위 내에 포함되는 샘플의 개수가 미리 설정된 개수보다 적을 경우 파티클 필터의 추정 성능이 저하되었다고 판단하며, 특히, 마할라노비스 거리를 이용하여 샘플들이 미리 설정된 범위 내에 포함되는 지를 판단할 수 있다.For this, the estimation
파티클 필터 재설정부(130)는 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR(Finite Impulse Response) 필터를 이용하여 파티클 필터를 재설정(reset)한다.The particle filter reset
칼만 필터나 파티클 필터는 상태 추정을 위해 초기 시점부터 현재 시점까지의 모든 입출력 정보를 사용한다. 이러한 필터에서 시스템의 입출력 값과 상태추정 값 사이의 관계를 식으로 표현해보면 도 3의 좌측과 같다. The Kalman filter or the particle filter uses all input and output information from the initial point to the current point in order to estimate the state. The relationship between the input and output values of the system and the state estimation values in such a filter is expressed by the equation as shown in the left side of FIG.
도 3은 IIR 필터와 FIR 필터의 차이점을 개략적으로 도시한 도면이다. 3 is a view schematically showing the difference between the IIR filter and the FIR filter.
도 3에서 IIR 필터(무한 구간 필터)는 k시점의 상태를 추정하기 위해 초기시점 k0부터 k-1 시점까지의 모든 정보를 사용하지만, FIR 필터(유한 구간 필터)는 제한된 구간의 정보만을 사용하는 것을 확인할 수 있다.In FIG. 3, the IIR filter (infinite interval filter) uses all the information from the initial point k 0 to k-1 to estimate the state at time k, but the FIR filter (finite interval filter) .
이 구조는 신호처리에서의 IIR(Infinite Impulse Response) 구조와 같기 때문에 칼만 필터, 파티클 필터는 IIR 필터에 속한다고 볼 수 있다. 이러한 IIR 필터는 초기 시점부터 현재시점까지의 모든 정보들이 누적되는 특징이 있는데, 이는 상황에 따라 단점으로 작용하기도 한다. Since this structure is the same as IIR (Infinite Impulse Response) structure in signal processing, Kalman filter and particle filter belong to IIR filter. Such an IIR filter is characterized in that all information from the initial point to the current point is accumulated, which is a disadvantage depending on the situation.
즉, 시스템에 모델 오차, 계산상의 오차 등 필터에 나쁜 영향을 주는 요인들이 발생할 경우 이것들로 인한 영향이 누적되어 필터가 발산(추정한 상태 값과 실제 상태 값이 점점 멀어짐)할 수 있다. In other words, if there are factors that adversely affect the filter such as model error and calculation error in the system, the influence due to them may accumulate and the filter may diverge (the estimated state value and the actual state value gradually become distant).
FIR 필터는 칼만 필터와 같은 IIR 필터의 단점인 발산 문제를 해결하기 위해 제시된 것으로, 상태 추정을 위해 유한한 구간의 입출력 정보만을 사용한다. 따라서 반올림 오차, 모델링 오차, 수치 해석적 오차 등에 강인한 특성을 갖고 있으며 유한입출력 안정성(BIBO stability)이 보장되는 장점을 갖고 있다.The FIR filter is proposed to solve the problem of divergence, which is a disadvantage of the IIR filter such as Kalman filter. In order to estimate the state, only the input / output information of the finite interval is used. Therefore, it has strong characteristics such as rounding error, modeling error, and numerical error, and it has a merit of guaranteeing finite input / output stability (BIBO stability).
본 발명의 구성에 의하면, 파티클 필터의 추정 성능을 평가하고 평과 결과에 따라 파티클 필터를 재설정함으로써, 위치 추정을 위해 사용되는 파티클 필터에서 'sample impoverishment'로 인해 위치 추정 실패가 발생할 경우에도 파티클 필터의 기능을 정상으로 회복시켜 정확한 위치 추정을 다시 수행할 수 있게 된다.According to the configuration of the present invention, the estimation performance of the particle filter is evaluated, and the particle filter is reset according to the evaluation result, so that even when the position estimation failure occurs due to 'sample impoverishment' in the particle filter used for position estimation, The function can be restored to the normal state and accurate position estimation can be performed again.
FIR 필터 구동부(132)는 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR 필터를 구동하며, 재설정 정보 획득부(134)는 FIR 필터로부터 파티클 필터 재설정에 필요한 정보를 획득한다. 이와 같은 구성에 의하면, 통상적인 위치 추정은 파티클 필터를 이용하여 수행하고 FIR 필터는 파티클 필터의 이상시에만 파티클 필터의 재설정을 위해서 사용되므로, 전체 위치 추정 과정을 보다 효율적으로 수행할 수 있게 된다.The FIR
이때, 파티클 필터의 재설정에 필요한 정보는 FIR 필터를 이용해 추정된 이동체의 위치 추정값, 및 위치 추정 오차 공분산값을 포함할 수 있다. 이와 같은 구성에 의하면, 파티클 필터가 보다 빠른 속도로 정상 동작을 수행할 수 있게 된다.At this time, information necessary for resetting the particle filter may include a position estimation value of the moving object estimated using the FIR filter, and a position estimation error covariance value. According to such a configuration, the particle filter can perform normal operation at a higher speed.
이하, 본 발명을 보다 구체적인 예와 함께 더욱 상세하게 설명한다.Hereinafter, the present invention will be described in more detail with reference to specific examples.
도 4는 본 발명의 일 실시예에 따른 위치 추정 방법을 수행하기 위한 개략적인 흐름도이다.4 is a schematic flowchart for performing a position estimation method according to an embodiment of the present invention.
도 4에서, 본 발명의 핵심은 1) 파티클 필터의 위치 추정 실패를 판단하는 과정과, 2) FIR 필터를 이용하여 파티클 필터를 리셋 및 재시동하는 과정인 것을 확인할 수 있다. In FIG. 4, it can be seen that the core of the present invention is a process of 1) determining failure of position estimation of a particle filter, and 2) a process of resetting and restarting a particle filter using an FIR filter.
위치 추정 시스템의 모델은 다음과 같이 AWGN(Additive White Gaussian Noise)을 갖는 비선형 모델로 가정한다.
The model of the position estimation system is assumed to be a nonlinear model with AWGN (Additive White Gaussian Noise) as follows.
여기서 
위의 식에서 볼 수 있듯이 k+1 시점에서의 위치 sk +1은 이전 시점인 k에서의 위치 sk와 함수 
이러한 시스템 모델에 대해 파티클 필터를 통한 위치 추정을 수행하게 되면 매 시점마다 Ntotal개의 위치 정보(상태 변수 값)를 얻게 된다. 여기서, Ntotal은 파티클 필터에서 사용한 샘플(sample)의 개수이다. When the position estimation is performed through the particle filter for this system model, N total position information (state variable value) is obtained at each time point. Where N total is the number of samples used in the particle filter.
샘플은 파티클(particle)이라고도 불리는데, 파티클 필터라는 이름은 여기에서 유래되었다. 만약, 100개의 파티클을 사용하는 파티클 필터를 구동하였다면 매시점마다 100개의 샘플들이 생성되며 100개 샘플들의 평균을 구해 최종 추정값을 얻는다. The sample is also called a particle. The name of the particle filter comes from this. If you run a particle filter that uses 100 particles, 100 samples are generated at each point in time, and the average of 100 samples is obtained to get the final estimate.
(1) (One) 파티클particle 필터의 추정 실패 또는 추정 성능 저하를 판단하는 방법 How to determine the estimation failure or estimated performance degradation of a filter
파티클 필터의 위치 추정 실패를 판단하기 위해 센서로부터 얻은 시스템 출력의 측정값 zk를 이용한다. 파티클 필터의 위치 추정값 
We use the measured value z k of the system output from the sensor to determine the failure to estimate the position of the particle filter. Location Estimation of Particle Filter
이때 측정잡음 vk 는 0으로 가정한다. 그리고 나서 추정된 출력 
비교 방법은 측정값 zk의 오차 범위 (또는 불확실성 범위) 내에 추정된 출력 
도 5는 파티클 필터의 추정 실패 또는 추정 성능 저하의 판단 과정을 도시한 개략적인 도면이다.5 is a schematic view showing a process of determining an estimation failure or estimation performance degradation of a particle filter.
파티클 필터가 총 Ntotal개의 샘플을 사용한다면 매시점마다 Ntotal개의 
그리고 그 개수를 Neffective라고 하자. 유효한 파티클의 개수가 전체 파티클 개수의 일정 비율 이하가 되면, 파티클 필터의 추정 성능이 저하되었거나 발산하고 있는 상태라고 판단을 한다. 이때 
(2) (2) FIRFIR 필터를 이용한 위치 추정 Location estimation using filters
위의 과정 (1)을 통해 파티클 필터가 추정에 실패하거나 추정 성능이 저하되었다고 판단되면, 그 시점에서 FIR 필터를 동작시켜 위치 추정값을 얻는다. FIR 필터는 상태 변수에 대한 초기 정보 없이 임의의 시점에서 상태 추정값을 얻을 수 있는 장점이 있다. If it is determined through the above process (1) that the particle filter fails to estimate or the estimation performance deteriorates, the FIR filter is operated at that point to obtain the position estimation value. The FIR filter has an advantage of obtaining the state estimation value at an arbitrary point of time without initial information on the state variable.
또한, FIR 필터는 IIR 필터인 파티클 필터의 추정 성능을 저하시키거나 발산하게 만드는 원인에 대해 강인한 특정을 갖고 있기 때문에, 파티클 필터의 성능이 나쁠 때 대체 수단으로 사용할 수 있다. k시점에서 파티클 필터의 성능 저하를 확인했다면, k-Nh부터 k-1까지의 측정값을 사용하여 FIR 필터의 위치 추정값을 얻는다. FIR filters can also be used as an alternative to poor particle filter performance because they have robust specifics as to why they degrade or diverge the estimation performance of IIR filters. If the degradation of the particle filter is confirmed at time k, the position estimate of the FIR filter is obtained using the measured values from kN h to k-1.
여기서, Nh는 FIR 필터에서 사용하는 측정값의 개수이다. 위치 추정값과 함께 추정 오차의 공분산 값도 계산해두는데 이값은 파티클 필터를 리셋하는데 사용된다. Here, N h is the number of measured values used in the FIR filter. The covariance value of the estimation error along with the position estimate is also calculated, which is used to reset the particle filter.
일반적인 FIR 필터는 선형 필터이며, 비선형인 위치 추정 시스템에 적용하기 위해서는 비선형 FIR 필터가 필요하다. 본 발명에서 개발한 비선형 FIR 필터의 수식과 알고리즘은 다음과 같다. A typical FIR filter is a linear filter, and a non-linear FIR filter is required for application to a nonlinear localization system. The formulas and algorithms of the nonlinear FIR filter developed in the present invention are as follows.
먼저, 다음과 같은 비선형 시변 이산시간 상태공간 모델을 고려한다.
First, consider the following nonlinear time-varying discrete-time state space model.
여기서 fk(·)와 hk(·)는 임의의 비선형 함수이며, 
식(1)의 상태방정식을 추정 상태값 
The state equation of equation (1)
여기서 Fk와 
Where F k and
이번에는 식(1)의 측정 방정식을 추정 상태값 
This time, the measurement equation of equation (1)
여기서 Hk와 
Where H k and
이제 선형화된 시스템(식(2)와 식(8))에 시변 일반형 FIR 필터를 적용하면 된다. 여기에 사용할 필터로 본 발명에서는 시변(time-varying) 일반형(general) MVF(minimum variance FIR) 필터를 개발하였다. 본 발명에서 개발한 비선형 FIR 필터의 수식과 적용 방법은 다음과 같다.
We now apply the time-varying general-purpose FIR filter to the linearized system (Eqs. (2) and (8)). In the present invention, a time-varying general minimum variance FIR (MVF) filter has been developed. The nonlinear FIR filter developed in the present invention is expressed as follows.
<알고리즘 1: 확장형 FIR 필터 >≪ Algorithm 1: Extended FIR filter >
시간 k=1,2,… 에 대해, 다음 과정을 반복해서 수행한다. Time k = 1,2, ... , The following process is repeatedly performed.
1. 만약, k≤N(여기서 N은 FIR 필터의 horizon size 이다.)이면, 확장칼만필터 같은 비선형 필터를 이용하여 상태 추정값을 얻는다. 1. If k ≤ N (where N is the horizon size of the FIR filter), use a nonlinear filter such as an extended Kalman filter to obtain the state estimate.
2. 상태 추정값을 이용하여 식 (3), (6), (4), (8)에 나와 있는 
3. k 〉N이면, 다음의 time-varying general MVF 필터를 이용하여 상태 추정값 
3. If k> N, use the following time-varying general MVF filter to calculate the state estimate
여기서, 
here,
위의 식과 행렬들에서 m=k-N과 n=k-1은 각각 FIR 필터 구간의 초기 시점과 최종 시점을 의미한다. In the above equations and matrices, m = k-N and n = k-1 represent the initial and final points of the FIR filter period, respectively.
확장 FIR 필터의 추정 오차 공분산 행렬 Pk는 다음의 식으로 구할 수 있다.
The estimated error covariance matrix P k of the extended FIR filter can be obtained by the following equation.
(3) (3) 파티클particle 필터 리셋 및 재시동 ( Reset and restart filters resetreset & & rebootreboot ))
FIR 필터를 통해 위치 추정값과 추정오차 공분산이 구해지면 이값을 이용하여 파티클 필터를 초기화한 뒤, 다시 파티클 필터링을 시작한다. 파티클 필터를 초기화한다는 것은 초기 파티클을 어떤 분포를 따라서 생성해주는 것을 말한다. When the position estimate and the estimation error covariance are obtained through the FIR filter, the particle filter is initialized using this value, and particle filtering is started again. Initializing a particle filter means creating an initial particle along a certain distribution.
앞에서 FIR 필터를 통해 구한 위치 추정값을 평균(mean)으로, 추정 오차 공분산을 공분산(covariance)로 사용하는 가우시안 확률 분포를 따라 초기 파티클을 생성해주면 된다. The initial particles are generated according to the Gaussian probability distribution using the estimated position values obtained through the FIR filter as the mean and the covariance of the estimated error covariance as the covariance.
도 6 내지 도 8은 도 1의 위치 추정 시스템의 실제 위치 추정 결과를 도시한 도면이다. Figs. 6 to 8 are diagrams showing actual position estimation results of the position estimation system of Fig. 1. Fig.
도 6 내지 도 8은 각각 파티클 개수가 부족한 경우, 측정 잡음이 너무 작은 경우, 강한 외란이 발생한 경우(kidnapped robot problem)에서의 위치 추정 결과를 도시하고 있다.FIGS. 6 to 8 show position estimation results in the case where the number of particles is insufficient, the measurement noise is too small, and the strong disturbance occurs (kidnapped robot problem).
도 6 내지 도 8에서, 파티클 필터를 이용한 위치 추정에 문제가 발생하는 경우 위치 추정 시스템이 파티클 필터를 재설정하여 실제의 위치를 정확히 추정하는 것을 확인할 수 있다.
6 to 8, it can be confirmed that when a problem occurs in the position estimation using the particle filter, the position estimation system accurately resets the particle filter by resetting the particle filter.
본 발명은 자율 이동 로봇, 무인 자동차 등의 자율 주행 시스템에서 사용되는 기술인 자기 위치 추정(self-localization) 기술에 관한 것으로서, 보다 상세하게는 파티클 필터(particle filter)와 FIR 필터(FIR filter)가 혼합된 하이브리드 필터(hybrid filter)를 사용하여, 기존의 파티클 필터 기반 위치 추정 방법들의 문제점을 해결하며 추정 정확도를 향상시키는 새로운 위치 추정 방법에 관한 것이다.Field of the Invention [0002] The present invention relates to a self-localization technique, which is a technique used in an autonomous mobile robot such as an autonomous mobile robot or an unmanned automobile. More specifically, the present invention relates to a technique of mixing a particle filter and an FIR filter The present invention relates to a new position estimation method for solving the problems of existing particle filter based position estimation methods and improving estimation accuracy by using a hybrid filter.
본 발명은 파티클 필터에 'sample impoverishment' 등의 요인으로 인해 추정 실패(particle filter failure) 혹은 발산(divergence) 현상 등 파티클 필터가 비정상적으로 동작을 보이는 경우, 이를 알고리즘에 의해 자동으로 감지하여, FIR 필터를 사용하여 파티클 필터를 리셋(reset)하고 재시동(rebooting)하여, 파티클 필터의 기능을 정상적으로 복구시켜 준다.The present invention detects an abnormal particle filter operation such as a particle filter failure or a divergence due to a factor such as 'sample impoverishment' in a particle filter, automatically detects the particle filter by an algorithm, , The particle filter is reset and rebooted to restore the function of the particle filter normally.
IIR 필터는 외란이나 오차가 없는 경우에 정확도가 높고 계산량도 적은 장점이 있다. 그러나 외란, 오차 등이 발생한 경우 필터가 부정확해지거나 발산하는 단점이 있다. FIR 필터는 외란이나 오차가 없는 경우에는 IIR 필터에 비해 추정 정확도가 떨어지며 계산량이 상대적으로 많은 단점이 있다. 그러나 외란, 오차에 강인한 특성을 갖고 있다. 본 발명은 IIR 필터인 파티클 필터를 메인 필터로, FIR 필터를 보조 필터로 사용하는 하이브리드 필터를 구현하여 IIR 필터의 문제점을 FIR 필터로 극복해준다.The IIR filter has the advantages of high accuracy and small amount of calculation in the absence of disturbance or error. However, there is a disadvantage that the filter becomes inaccurate or diverges when disturbance or error occurs. The FIR filter has a disadvantage in that the estimation accuracy is lower than that of the IIR filter when there is no disturbance or error, and the calculation amount is relatively large. However, disturbance and error are robust. The present invention overcomes the problem of the IIR filter with the FIR filter by implementing the hybrid filter using the particle filter as the main filter and the FIR filter as the auxiliary filter.
본 발명은 FIR 필터를 이용하여 파티클 필터를 보조하는 것으로서 파티클 필터의 장점을 모두 유지한 채, 파티클 필터의 약점만 FIR 필터로 보완해주는 것이다. 따라서 기존의 파티클 필터 및 이를 이용한 위치 추정 방법들에 모두 적용할 수 있어 범용성이 높다.
The present invention assists the particle filter using the FIR filter, and only the weak point of the particle filter is compensated by the FIR filter while maintaining all the advantages of the particle filter. Therefore, it can be applied to existing particle filters and position estimation methods using the same.
본 발명이 비록 일부 바람직한 실시예에 의해 설명되었지만, 본 발명의 범위는 이에 의해 제한되어서는 아니 되고, 특허청구범위에 의해 뒷받침되는 상기 실시예의 변형이나 개량에도 미쳐야 할 것이다.
Although the present invention has been described in terms of some preferred embodiments, the scope of the present invention should not be limited thereby but should be modified and improved in accordance with the above-described embodiments.
100: 위치 추정 시스템
110: 위치 추정부
120: 추정 성능 평가부
130: 파티클 필터 재설정부
132: FIR 필터 구동부
134: 재설정 정보 획득부100: Position estimation system
110:
120: estimation performance evaluation unit
130: Particle Filter Resetting Government
132: FIR filter driver
134: Reset information acquisition unit
Claims (11)
상기 위치 추정 결과를 미리 설정된 기준에 따라 판단하여 상기 파티클 필터의 추정 성능을 평가하는 추정 성능 평가부; 및
상기 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR(Finite Impulse Response) 필터를 이용하여 상기 파티클 필터를 재설정(reset)하는 파티클 필터 재설정부를 포함하며,
상기 파티클 필터 재설정부는 상기 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR 필터를 구동하는 FIR 필터 구동부; 및
상기 FIR 필터로부터 상기 파티클 필터 재설정에 필요한 정보를 획득하는 재설정 정보 획득부를 포함하는 것을 특징으로 하는 위치 추정 시스템.A position estimating unit estimating a position of the moving object by using a particle filter that performs sampling and resampling of the particle;
An estimation performance evaluation unit for evaluating the estimation performance of the particle filter by determining the position estimation result according to a preset reference; And
And a particle filter resetting unit for resetting the particle filter by using a finite impulse response (FIR) filter when it is determined that the estimation performance of the particle filter is deteriorated,
Wherein the particle filter resetting unit comprises: an FIR filter driver for driving the FIR filter when it is determined that the estimation performance of the particle filter is degraded; And
And a reset information obtaining unit for obtaining information necessary for resetting the particle filter from the FIR filter.
상기 파티클 필터 재설정에 필요한 정보는 상기 FIR 필터를 이용해 추정된 상기 이동체의 위치 추정값, 및 위치 추정 오차 공분산값을 포함하는 것을 특징으로 하는 위치 추정 시스템. The method according to claim 1,
Wherein the information required for resetting the particle filter includes a position estimation value of the moving object estimated using the FIR filter, and a position estimation error covariance value.
상기 추정 성능 평가부는 상기 파티클 필터가 샘플링한 샘플들 중 센서 측정값으로부터 미리 설정된 범위 내에 포함되는 샘플의 개수가 미리 설정된 개수보다 적을 경우 상기 파티클 필터의 추정 성능이 저하되었다고 판단하는 것을 특징으로 하는 위치 추정 시스템.The method of claim 3,
Wherein the estimation performance evaluating unit determines that the estimation performance of the particle filter is deteriorated when the number of samples included in the predetermined range from the sensor measurement value among the samples sampled by the particle filter is smaller than a predetermined number. Estimation system.
상기 추정 성능 평가부는 마할라노비스 거리를 이용하여 상기 샘플들이 상기 미리 설정된 범위 내에 포함되는 지를 판단하는 것을 특징으로 하는 위치 추정 시스템.5. The method of claim 4,
Wherein the estimation performance evaluating unit uses the Mahalanobis distance to determine whether the samples are included in the predetermined range.
위치 추정부가 파티클의 샘플링과 리샘플링을 수행하는 파티클 필터를 이용하여 이동체의 위치를 추정하는 위치 추정 단계;
추정 성능 평가부가 상기 위치 추정 결과를 미리 설정된 기준에 따라 판단하여 상기 파티클 필터의 추정 성능을 평가하는 추정 성능 평가 단계; 및
파티클 필터 재설정부가 상기 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR(Finite Impulse Response) 필터를 이용하여 상기 파티클 필터를 재설정(reset)하는 파티클 필터 재설정 단계를 포함하며,
상기 파티클 필터 재설정 단계는 상기 파티클 필터의 추정 성능이 저하되었다고 판단되는 경우 FIR 필터를 구동하는 FIR 필터 구동 단계; 및
상기 FIR 필터로부터 상기 파티클 필터 재설정에 필요한 정보를 획득하는 재설정 정보 획득부 단계를 포함하는 것을 특징으로 하는 위치 추정 방법.Included in the position estimation system,
A position estimating step of estimating a position of the moving object by using a particle filter that performs sampling and resampling of the particle of the position estimating part;
An estimation performance evaluation step of estimating an estimation performance of the particle filter by estimating the position estimation result according to a preset reference; And
And a particle filter resetting step of resetting the particle filter by using a finite impulse response (FIR) filter when the particle filter resetting unit determines that the estimation performance of the particle filter is deteriorated,
The FIR filter driving step of driving the FIR filter when it is determined that the estimation performance of the particle filter is degraded; And
And a reset information obtaining step of obtaining information necessary for resetting the particle filter from the FIR filter.
상기 파티클 필터 재설정에 필요한 정보는 상기 FIR 필터를 이용해 추정된 상기 이동체의 위치 추정값, 및 위치 추정 오차 공분산값을 포함하는 것을 특징으로 하는 위치 추정 방법. The method according to claim 6,
Wherein the information necessary for resetting the particle filter includes a position estimation value of the moving object estimated using the FIR filter, and a position estimation error covariance value.
상기 추정 성능 평가 단계는 상기 파티클 필터가 샘플링한 샘플들 중 센서 측정값으로부터 미리 설정된 범위 내에 포함되는 샘플의 개수가 미리 설정된 개수보다 적을 경우 상기 파티클 필터의 추정 성능이 저하되었다고 판단하는 것을 특징으로 하는 위치 추정 방법.9. The method of claim 8,
Wherein the estimated performance evaluation step determines that the estimation performance of the particle filter is deteriorated when the number of samples included in the predetermined range from the sensor measurement value among the samples sampled by the particle filter is smaller than a predetermined number Position estimation method.
상기 추정 성능 평가 단계는 마할라노비스 거리를 이용하여 상기 샘플들이 상기 미리 설정된 범위 내에 포함되는 지를 판단하는 것을 특징으로 하는 위치 추정 방법.10. The method of claim 9,
Wherein the estimating performance evaluating step uses the Mahalanobis distance to determine whether the samples are included within the predetermined range.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020140027335A KR101596323B1 (en) | 2014-03-07 | 2014-03-07 | System and method for self-localizing and a medium having computer readable program for executing the method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| KR1020140027335A KR101596323B1 (en) | 2014-03-07 | 2014-03-07 | System and method for self-localizing and a medium having computer readable program for executing the method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| KR20150105106A KR20150105106A (en) | 2015-09-16 |
| KR101596323B1 true KR101596323B1 (en) | 2016-03-07 |
Family
ID=54244599
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| KR1020140027335A KR101596323B1 (en) | 2014-03-07 | 2014-03-07 | System and method for self-localizing and a medium having computer readable program for executing the method |
Country Status (1)
| Country | Link |
|---|---|
| KR (1) | KR101596323B1 (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101908534B1 (en) * | 2016-06-23 | 2018-10-16 | 조선대학교산학협력단 | Apparatus and method for determining position and attitude of a vehicle |
| KR102357967B1 (en) * | 2020-01-16 | 2022-02-03 | 서울대학교산학협력단 | Apparatus and method for providing microscopic dust information |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101231378B1 (en) | 2011-08-12 | 2013-02-15 | 숭실대학교산학협력단 | Apparatus and recording media for tracking user location |
| KR101268564B1 (en) * | 2012-01-26 | 2013-05-28 | 숭실대학교산학협력단 | Apparatus, method and recoding media for tracking location of mobile device based on particle filter |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3246822B2 (en) * | 1994-02-03 | 2002-01-15 | 株式会社日立製作所 | Mobile tracking device |
| KR20120048958A (en) * | 2010-11-08 | 2012-05-16 | 재단법인대구경북과학기술원 | Method for tracking object and for estimating |
-
2014
- 2014-03-07 KR KR1020140027335A patent/KR101596323B1/en active IP Right Grant
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101231378B1 (en) | 2011-08-12 | 2013-02-15 | 숭실대학교산학협력단 | Apparatus and recording media for tracking user location |
| KR101268564B1 (en) * | 2012-01-26 | 2013-05-28 | 숭실대학교산학협력단 | Apparatus, method and recoding media for tracking location of mobile device based on particle filter |
Also Published As
| Publication number | Publication date |
|---|---|
| KR20150105106A (en) | 2015-09-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11465643B2 (en) | Obstacle recognition device and obstacle recognition method | |
| Agamennoni et al. | An outlier-robust Kalman filter | |
| KR101829560B1 (en) | Method, system and computer device for predicting a capacity based on kalman filter | |
| CN110687555B (en) | Navigation satellite atomic clock weak frequency hopping on-orbit autonomous rapid detection method | |
| KR102168288B1 (en) | System and method for tracking multiple object using multi-LiDAR | |
| US8949027B2 (en) | Multiple truth reference system and method | |
| Bassiri et al. | Particle filter and finite impulse response filter fusion and hector SLAM to improve the performance of robot positioning | |
| Wüthrich et al. | Robust Gaussian filtering using a pseudo measurement | |
| KR101390776B1 (en) | Localization device, method and robot using fuzzy extended kalman filter algorithm | |
| Attari et al. | An SVSF-based generalized robust strategy for target tracking in clutter | |
| KR101596323B1 (en) | System and method for self-localizing and a medium having computer readable program for executing the method | |
| KR101878617B1 (en) | Method and system for processing traictory data | |
| Sircoulomb et al. | State estimation under nonlinear state inequality constraints. A tracking application | |
| US20220289220A1 (en) | Method for Determining an Object's Position Using Different Items of Sensor Information | |
| Fatemi et al. | A study of MAP estimation techniques for nonlinear filtering | |
| RU2296348C2 (en) | Tracking measuring device with a maneuver detector and an adaptive forecast correction | |
| Rameshbabu et al. | Target tracking system using kalman filter | |
| Houts et al. | A robust framework for failure detection and recovery for terrain-relative navigation | |
| CN112560689A (en) | Parking space detection method and device, electronic equipment and storage medium | |
| US20180372885A1 (en) | Method for Measuring the Variance in a Measurement Signal, Method for Data Fusion, Computer Program, Machine-Readable Storage Medium, and Device | |
| Attari et al. | Target tracking formulation of the SVSF as a probabilistic data association algorithm | |
| CN112344933B (en) | Information generation method and device and storage medium | |
| Ren et al. | Adaptive sensor fusion of camera, GNSS and IMU for autonomous driving navigation | |
| Zhang et al. | Cumulative error estimation from noisy relative measurements | |
| Worrall et al. | GPS/GNSS consistency in a multi-path environment and during signal outages |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A201 | Request for examination | ||
| E902 | Notification of reason for refusal | ||
| AMND | Amendment | ||
| E601 | Decision to refuse application | ||
| AMND | Amendment | ||
| X701 | Decision to grant (after re-examination) | ||
| GRNT | Written decision to grant | ||
| FPAY | Annual fee payment |
Payment date: 20190211 Year of fee payment: 4 |
|
| FPAY | Annual fee payment |
Payment date: 20200128 Year of fee payment: 5 |