KR101202108B1 - Localizing method for mobile robot - Google Patents
Localizing method for mobile robot Download PDFInfo
- Publication number
- KR101202108B1 KR101202108B1 KR1020100052108A KR20100052108A KR101202108B1 KR 101202108 B1 KR101202108 B1 KR 101202108B1 KR 1020100052108 A KR1020100052108 A KR 1020100052108A KR 20100052108 A KR20100052108 A KR 20100052108A KR 101202108 B1 KR101202108 B1 KR 101202108B1
- Authority
- KR
- South Korea
- Prior art keywords
- matching error
- error
- mobile robot
- scan
- calculated
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 90
- 230000033001 locomotion Effects 0.000 claims description 40
- 238000009826 distribution Methods 0.000 claims description 20
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 238000005266 casting Methods 0.000 claims description 4
- 229920006395 saturated elastomer Polymers 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 3
- 230000003252 repetitive effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 11
- 230000004807 localization Effects 0.000 description 6
- 238000013461 design Methods 0.000 description 5
- 238000004088 simulation Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 238000001514 detection method Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 4
- 239000000203 mixture Substances 0.000 description 3
- 238000010606 normalization Methods 0.000 description 3
- 241000282412 Homo Species 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 241001061260 Emmelichthys struhsakeri Species 0.000 description 1
- 238000012952 Resampling Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003203 everyday effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
- B25J19/021—Optical sensing devices
- B25J19/022—Optical sensing devices using lasers
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
- Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10S901/00—Robots
- Y10S901/01—Mobile robot
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Optics & Photonics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
본 발명은 이동 로봇의 위치 추정 방법에 관한 것이다. 본 발명에 따른 이동 로봇의 위치 추정 방법은 (a) 레인지 센서의 스캔에 따라 스캔 데이터가 획득되는 단계와; (b) 상기 스캔 데이터에 기초한 스캔 범위 영역과, 기 등록된 환경 지도로부터 예측되는 예측 데이터에 기초한 예측 범위 영역 간의 편차에 기초하여 매칭 에러를 산출하는 단계와; (c) 상기 (b) 단계에서 산출된 상기 매칭 에러가 에러 문턱치를 초과하는지 여부에 따라 위치 추정 실패 여부를 판단하는 단계와; (d) 상기 (c) 단계에서 위치 추정 실패로 판단된 경우, 상기 환경 지도 상의 적어도 일 영역으로부터 추출된 복수의 위치 샘플 각각에 대해 매칭 에러를 산출하는 단계와; (e) 상기 (d) 단계에서 산출된 상기 매칭 에러를 기 설정된 확률 밀도 함수에 적용하여 상기 이동 로봇의 위치를 추정하는 단계를 포함하는 것을 특징으로 한다. 이에 따라, 매칭 에러를 스캔 데이터와 예측 데이터에 기초한 면적 단위로 산출하여, 맵핑되지 않은 장애물 등에 의해 야기되는 센서 데이터의 부분적인 손상에 강인하고, 정확한 위치 추정 실패의 판단이 가능하게 된다.The present invention relates to a method for estimating the position of a mobile robot. According to an aspect of the present invention, there is provided a method of estimating a position of a mobile robot, the method comprising: (a) obtaining scan data according to a scan of a range sensor; (b) calculating a matching error based on a deviation between a scan range region based on the scan data and a prediction range region based on prediction data predicted from a pre-registered environment map; (c) determining whether the position estimation has failed according to whether the matching error calculated in the step (b) exceeds an error threshold; (d) calculating a matching error for each of a plurality of location samples extracted from at least one area on the environment map when it is determined that location estimation fails in step (c); (e) estimating the position of the mobile robot by applying the matching error calculated in step (d) to a preset probability density function. Accordingly, the matching error is calculated in the area unit based on the scan data and the predictive data, so that it is robust to partial damage of the sensor data caused by an unmapped obstacle or the like, and the accurate position estimation failure can be determined.
Description
본 발명은 이동 로봇의 위치 추정 방법에 관한 것으로서, 보다 상세하게는 센서 데이터의 부분적인 손상에 강인하고, 정확한 위치 추정 실패의 판단이 가능하면서도 이동 로봇의 위치 추정의 정확성 및 추정 속도가 향상된 이동 로봇의 위치 추정 방법에 관한 것이다.
The present invention relates to a method for estimating a position of a mobile robot, and more particularly, is a mobile robot that is robust to partial damage of sensor data and that can accurately determine a failure of an accurate position estimation while improving accuracy and estimation speed of the position estimation of the mobile robot. It relates to a method for estimating the position of.
이동 로봇은 실용 방안의 지속적인 연구로 인하여 산업용으로서 뿐만 아니라 청소 로봇, 경비 로봇, 안내 로봇과 같은 일상 생활용으로서도 사용되며 실생활에서도 차지하는 비중이 점점 커지고 있다.Mobile robots are used not only for industrial purposes but also for everyday life such as cleaning robots, security robots, and guide robots due to continuous research on practical methods, and the proportion of mobile robots is increasing in real life.
이와 같은 이동 로봇이 자율 주행을 하기 위해서는 이동 로봇의 위치를 파악하는 위치 추정이 필수적으로 요구된다. 이동 로봇의 위치 추정은 환경에 대한 정보와 센서로 측정한 정보를 이용하여 이동 로봇의 위치를 추정하는 기술로써, 이동 로봇의 주행을 위한 핵심 기술이다.In order for such a mobile robot to autonomously travel, position estimation for identifying the position of the mobile robot is essential. Position estimation of the mobile robot is a technology for estimating the location of the mobile robot using information about the environment and information measured by a sensor, and is a core technology for driving the mobile robot.
이동 로봇의 정확한 위치 추정은 매칭 에러(Matching error)와 확률 업데이트 룰(Observation likelihood model)을 포함하는 관측 우도 모델(Observation likelihood model)의 설계, 정확한 위치 추정 실패의 감지, 그리고 위치 추정을 실패한 경우 로봇 위치로의 정확한 복귀 등과 같은 다양한 변수가 영향을 미치게 된다.Accurate position estimation of mobile robots includes the design of Observation likelihood models, including matching errors and probability likelihood models, the detection of failures for accurate position estimation, and robots in the event of a failed position estimation. Various variables affect, such as the exact return to position.
이러한 이동 로봇의 위치 추정 기술과 관련하여, F. Dellaert, D. Fox, W.Burgard and S. Thrun는 논문 "Monte Carlo Localization for Mobile Robots(IEEE International Conference on Robotics & Automation (ICRA'9), Detroit, USA, May. 1999, pp. 1322-1328)"에서 Monte Carlo Localization(이하, "MCL") 기법을 제안하였다. MCL 기법은 전역 위치 추정(Global localization)과 지역 위치 추정(Local localization) 모두를 다룰 수 있으며, 센서 노이즈에 대한 강인성으로 인해 폭넓게 사용되고 있다.Regarding the location estimation technology of such mobile robots, F. Dellaert, D. Fox, W.Burgard and S. Thrun described the paper "Monte Carlo Localization for Mobile Robots (IEEE International Conference on Robotics & Automation (ICRA'9), Detroit). , USA, May. 1999, pp. 1322-1328), proposed the Monte Carlo Localization (hereinafter referred to as "MCL") technique. The MCL technique can handle both global localization and local localization and is widely used due to its robustness to sensor noise.
또한, 관측 우도 모델(Observation likelihood model)은 이동 로봇의 자체 위치 추정 문제의 해결을 위해 폭넓게 연구되어 왔다. 일반적으로 관측 우도 모델(Observation likelihood model)의 설계는 매칭 에러의 설계와 이를 이용한 확률 업데이트 룰(Probability update rule)의 설계를 포함하게 된다.In addition, the Observation likelihood model has been extensively studied to solve the problem of self-positioning of a mobile robot. In general, the design of an observation likelihood model includes the design of a matching error and the design of a probability update rule using the same.
Thrun 등은 논문 "Probabilistic Robotics(The MIT Press, 2005, pp.153-169.)"에서 Beam model을 제안하였다. 그런데, Beam model은 개별적인 센서 측정치의 곱에 의해 계산되므로, 센서 데이터의 부분적인 오류에 민감하게 되는바, 주어진 환경 지도에서 매핑(Mapping)되지 않은 장애물의 제거를 위한 추가적인 필터링 기법이 요구된다.Thrun et al. Proposed a beam model in the paper "Probabilistic Robotics (The MIT Press, 2005, pp. 153-169.)". However, since the beam model is calculated by multiplying individual sensor measurements, the beam model is sensitive to partial errors in the sensor data. Therefore, an additional filtering technique for removing an unmapped obstacle in a given environment map is required.
한편, 이동 로봇의 위치 추정과 관련된 중요한 문제 중 하나는 위치 추정의 실패를 정확히 감지하는 것이다. Sidek 등은 논문 "Integrating Actuator Fault and Wheel Slippage Detections within FDI Framework(Proc. 5th WSEAS International Conference on Circuits, Systems, Electronics, Control & Signal Processing, Dallas, Texas, 2006, pp. 31-36.)"에서 신경망을 이용한 휠 미끄러짐의 감지를 위한 FDI framework를 제안하였다. 그런데, Sidek 등의 논문에 제시된 기법은 추가적인 가속계를 이용하고 있어, 시스템이 복잡해지는 단점이 있다.On the other hand, one of the important problems related to the position estimation of the mobile robot is to accurately detect the failure of the position estimation. Sidek et al. In the article "Integrating Actuator Fault and Wheel Slippage Detections within FDI Framework (Proc. 5th WSEAS International Conference on Circuits, Systems, Electronics, Control & Signal Processing, Dallas, Texas, 2006, pp. 31-36.)" A FDI framework for the detection of wheel slip using However, the technique presented in Sidek et al. Uses an additional accelerometer, which makes the system complicated.
또한, Sundvall 등은 논문 "Fault detection for mobile robots using redundant positioning systems(Proc. IEEE International Conference on Robotics & Automation (ICRA'6), Orlando, Florida, May 2006, pp. 3781-3786.)"에서 레이저 스캐너를 이용하여 매핑되지 않은 장애물에 의해 발생하는 충돌을 감지하는 방법을 제안하고 있다. 이외에도 파티클 필터를 이용하여 6개의 휠이 설치된 Rover의 결함을 감지하기 위한 방법을 제안되었다.Sundvall et al. Also describe a laser scanner in the article "Fault detection for mobile robots using redundant positioning systems (Proc. IEEE International Conference on Robotics & Automation (ICRA'6), Orlando, Florida, May 2006, pp. 3781-3786.)". We propose a method of detecting a collision caused by an unmapped obstacle using. In addition, a method for detecting defects in a 6-wheel installed rover using a particle filter has been proposed.
위치 추정을 실패한 경우 이동 로봇의 실제 위치로의 정확한 복귀를 위해, Thrun 등은 논문 "Monte Carlo Localization with Mixture Proposal Distribution(Proc. AAAI National Conference on Artificial Intelligence (AAAI 2000), Austin, Texas, 2000, pp.839-865.)에서 Mixture proposal distribution을 갖는 MCL 기법을 제안하고 있다.In order to accurately return the mobile robot to its actual position in the event of a failed position estimation, Thrun et al. Described the paper "Monte Carlo Localization with Mixture Proposal Distribution (Proc. AAAI National Conference on Artificial Intelligence (AAAI 2000), Austin, Texas, 2000, pp. .839-865.) Propose an MCL technique with a Mixture proposal distribution.
Mixture proposal distribution을 갖는 MCL 기법은 센서 측정치에 기초하여 분배된 전역 위치 추정 생태로 부분적인 샘플(partial samples)을 이용하고 있다. 그러나, 이 기법은 휠 미끄러짐에 의한 위치 추정 실패에 적용하기에는 그 속도가 느린 문제점이 있다.
The MCL technique with a mix proposal distribution uses partial samples with distributed global position estimation ecology based on sensor measurements. However, this technique has a problem that it is slow to apply to the position estimation failure due to the wheel slip.
이에, 본 발명은 상기와 같은 기존의 연구들이 갖는 문제점을 해결하기 위해 안출된 것으로서, 인간이 공존하는 실제 환경, 예를 들어 박물관이나 오피스 빌딩과 같이 이동 로봇의 환경 지도에 맵핑(Mapping)되지 않은 장애물이 공존하는 실제 환경에서 적용이 가능한 이동 로봇의 위치 추정 방법을 제공하는데 그 목적이 있다.Accordingly, the present invention has been made to solve the problems of the existing studies, and is not mapped to an environment map of a mobile robot such as a real environment where human coexists, for example, a museum or an office building. An object of the present invention is to provide a method for estimating the position of a mobile robot that can be applied in a real environment where obstacles coexist.
또한, 맵핑되지 않은 장애물 등에 의해 야기되는 센서 데이터의 부분적인 손상에 강인하고, 정확한 위치 추정 실패의 판단이 가능하면서도 추정을 실패할 때 이동 로봇의 실제 위치로 복귀하는데 있어 이동 로봇의 위치 추정의 정확성 및 추정 속도가 향상된 이동 로봇의 위치 추정 방법을 제공하는데 또 다른 목적이 있다.In addition, it is robust against partial damage of sensor data caused by unmapped obstacles and the like, and it is possible to accurately determine the failure of the estimation of the position, but the accuracy of the estimation of the position of the mobile robot in returning to the actual position of the mobile robot when the estimation fails. Another object is to provide a method for estimating a position of a mobile robot having an improved estimated speed.
이를 위해 인간과 같은 맵핑되지 않은 장애물에 강인한 관측 우도 모델(Observation likelihood model)을 설계하고, 관측 우도 모델(Observation likelihood model) 상의 매칭 에러를 통한 위치 추정 상태를 추적하며, 돌발적인 휠 미끄러짐 등에 의해 야기되는 위치 추정 실패에서 이동 로봇의 정확하고 빠른 위치 추정이 가능한 이동 로봇의 위치 추정 방법을 제공하는데 그 목적이 있다.
To this end, we design an Observation likelihood model that is robust against unmapped obstacles such as humans, track the position estimation state through matching errors on the Observation likelihood model, and cause it by sudden wheel slippage. It is an object of the present invention to provide a method for estimating the position of a mobile robot, which enables accurate and rapid position estimation in a failed position estimation method.
상기 목적은 본 발명에 따라, 이동 로봇의 위치 추정 방법에 있어서, (a) 레인지 센서의 스캔에 따라 스캔 데이터가 획득되는 단계와; (b) 상기 스캔 데이터에 기초한 스캔 범위 영역과, 기 등록된 환경 지도로부터 예측되는 예측 데이터에 기초한 예측 범위 영역 간의 편차에 기초하여 매칭 에러를 산출하는 단계와; (c) 상기 (b) 단계에서 산출된 상기 매칭 에러가 에러 문턱치를 초과하는지 여부에 따라 위치 추정 실패 여부를 판단하는 단계와; (d) 상기 (c) 단계에서 위치 추정 실패로 판단된 경우, 상기 환경 지도 상의 적어도 일 영역으로부터 추출된 복수의 위치 샘플 각각에 대해 매칭 에러를 산출하는 단계와; (e) 상기 (d) 단계에서 산출된 상기 매칭 에러를 기 설정된 확률 밀도 함수에 적용하여 상기 이동 로봇의 위치를 추정하는 단계를 포함하는 것을 특징으로 하는 이동 로봇의 위치 추정 방법에 의해서 달성된다.According to an aspect of the present invention, there is provided a method for estimating a position of a mobile robot, the method comprising: (a) obtaining scan data according to a scan of a range sensor; (b) calculating a matching error based on a deviation between a scan range region based on the scan data and a prediction range region based on prediction data predicted from a pre-registered environment map; (c) determining whether the position estimation has failed according to whether the matching error calculated in the step (b) exceeds an error threshold; (d) calculating a matching error for each of a plurality of location samples extracted from at least one area on the environment map when it is determined that location estimation fails in step (c); (e) estimating the position of the mobile robot by applying the matching error calculated in step (d) to a preset probability density function.
여기서, 상기 (d) 단계에서 상기 복수의 위치 샘플은 기 등록된 모션 모델에 기초한 최대 모션 경계 영역 내에서 추출될 수 있다.Here, in step (d), the plurality of position samples may be extracted within a maximum motion boundary region based on a previously registered motion model.
한편, 상기 목적은 본 발명의 다른 실시 형태에 따라, 이동 로봇의 위치 추정 방법에 있어서, (a) 레인지 센서의 스캔에 따라 스캔 데이터가 획득되는 단계와; (b) 상기 스캔 데이터와, 기 설정된 환경 지도에 기초하여 예측되는 예측 데이터에 기초하여 매칭 에러를 산출하는 단계와; (c) 상기 (b) 단계에서 산출된 상기 매칭 에러가 에러 문턱치를 초과하는지 여부에 따라 위치 추정 실패 여부를 판단하는 단계와; (d) 상기 (c) 단계에서 위치 추정 실패로 판단된 경우, 기 등록된 모션 모델에 기초한 최대 모션 경계 영역 내에서 추출되는 복수의 위치 샘플 각각에 대해 매칭 에러를 산출하는 단계와; (e) 상기 (d) 단계에서 산출된 상기 매칭 에러를 기 설정된 확률 밀도 함수에 적용하여 상기 이동 로봇의 위치를 추정하는 단계를 포함하는 것을 특징으로 하는 이동 로봇의 위치 추정 방법에 의해서도 달성될 수 있다.On the other hand, the above object is, according to another embodiment of the present invention, a position estimation method of a mobile robot, comprising the steps of: (a) obtaining scan data according to the scan of the range sensor; (b) calculating a matching error based on the scan data and prediction data predicted based on a preset environment map; (c) determining whether the position estimation has failed according to whether the matching error calculated in the step (b) exceeds an error threshold; (d) calculating a matching error for each of a plurality of position samples extracted in a maximum motion boundary region based on a previously registered motion model when it is determined that the position estimation fails in step (c); (e) estimating the position of the mobile robot by applying the matching error calculated in step (d) to a preset probability density function. have.
여기서, 상기 (b) 단계는 상기 스캔 데이터에 기초하여 스캔 범위 영역을 산출하는 단계와; 상기 예측 데이터에 기초하여 예측 범위 영역을 산출하는 단계와; 상기 스캔 범위 영역과 상기 예측 범위 영역 간의 편차에 기초하여 상기 매칭 에러를 산출하는 단계를 포함할 수 있다.Here, the step (b) may include calculating a scan range region based on the scan data; Calculating a prediction range region based on the prediction data; Calculating the matching error based on the deviation between the scan range region and the prediction range region.
여기서, 상기 매칭 에러는 수학식Here, the matching error is expressed by equation
(여기서, Em(x)는 상기 매칭 에러이고, 는 시간 t에서의 j번째 상기 스캔 데이터이고, 는 시간 t에서의 j번째 상기 예측 데이터이고, 는 시간 t에서의 j번째 상기 스캔 범위 영역이고, 는 시간 t에서의 j번째 상기 예측 범위 영역이고, Δθ는 상기 레인지 센서의 각 해상도(Angular resolution)이다)에 의해 산출될 수 있다.Where Em (x) is the matching error, Is the j th scan data at time t, Is the j th prediction data at time t, Is the j th scan range region at time t, Is the j-th prediction region at time t, and Δθ may be calculated by the angular resolution of the range sensor.
그리고, 상기 (b) 단계에서 상기 예측 데이터는 레이-캐스팅 알고리즘(Ray-casting algorithm)을 통해 상기 환경 지도로부터 계산될 수 있다.In the step (b), the prediction data may be calculated from the environment map through a ray-casting algorithm.
또한, 상기 에러 문턱치는 상기 레인지 센서의 이전 스캔 동작들에 대한 상기 (b) 단계의 수행을 통해 산출된 매칭 에러의 통계값에 기초하여 업데이트될 수 있다.In addition, the error threshold may be updated based on a statistical value of a matching error calculated by performing step (b) on previous scan operations of the range sensor.
그리고, 상기 에러 문턱치에는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치가 오차 범위로 반영될 수 있다.In addition, an error reference value determined based on a grid resolution of the environment map may be reflected in the error threshold as an error range.
여기서, 상기 에러 문턱치는 수학식 (여기서, h는 상기 에러 문턱치이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 상기 매칭 에러의 평균값이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 상기 매칭 에러의 표준편차이고, bias는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치이다)에 의해 산출될 수 있다.Here, the error threshold value is expressed in equation Where h is the error threshold, Is an average value of the matching error from the (ik) th scan to the i th scan, Is the standard deviation of the matching error from the (ik) th scan to the i th scan, and the bias is an error reference value determined based on the grid resolution of the environment map.
그리고, 상기 (b) 단계에서 산출된 매칭 에러는 미디언 필터(Median filter)에 의해 필터링되어 상기 (c) 단계에 반영될 수 있다.The matching error calculated in step (b) may be filtered by a median filter and reflected in step (c).
그리고, 상기 모션 모델은 가우시안 확률 분포를 갖는 모션 모델이 적용되며; 상기 최대 모션 경계 영역은 상기 (c) 단계의 수행을 통한 직전 위치 추정 성공 위치를 중심으로 상기 모션 모델에 기초하여 산출되는 최대 이동 거리를 반경으로 하는 원 형태로 설정될 수 있다.The motion model is applied with a motion model having a Gaussian probability distribution; The maximum motion boundary region may be set in the form of a circle having a radius of the maximum movement distance calculated based on the motion model based on the last position estimation success position through the step (c).
또한, 상기 최대 이동 거리는 수학식 (여기서, Smax는 상기 최대 이동 거리이고, σ는 상기 모션 모델의 표준 편차이고, Δsi는 상기 이동 로봇의 휠 엔코더로부터 측정된 이동 거리이다)에 의해 산출될 수 있다.In addition, the maximum moving distance is represented by equation Where S max is the maximum travel distance, sigma is the standard deviation of the motion model, and Δs i is the travel distance measured from the wheel encoder of the mobile robot.
그리고, 상기 (e) 단계는 (e1) 상기 (d) 단계에서 산출된 상기 매칭 에러를 상기 확률 밀도 함수에 적용하여 상기 각 위치 샘플에 대한 추정 확률을 산출하는 단계와; (e2) 상기 산출된 추정 확률에 기초하여 후보 샘플을 추출하는 단계와; (e3) 상기 추출된 후보 샘플에 대한 매칭 에러를 산출하는 단계와; (e4) 상기 (e3) 단계에서 산출된 상기 매칭 에러를 상기 확률 밀도 함수에 적용하여 상기 각 후보 샘플에 대한 추정 확률을 산출하는 단계와; (e4) 상기 (e2) 단계, 상기 (e3) 단계 및 상기 (e4) 단계의 반복적인 수행을 통해 수렴되는 수렴 위치를 상기 이동 로봇의 위치로 추정하는 단계를 포함할 수 있다.Step (e) includes: (e1) calculating the estimated probability for each position sample by applying the matching error calculated in step (d) to the probability density function; (e2) extracting candidate samples based on the calculated estimated probability; (e3) calculating a matching error for the extracted candidate sample; (e4) calculating an estimated probability for each candidate sample by applying the matching error calculated in step (e3) to the probability density function; (e4) may include estimating a convergence position converged through repetitive execution of steps (e2), (e3) and (e4) as the position of the mobile robot.
그리고, 상기 확률 밀도 함수는 포화 가우시안 함수(Saturated Gaussian function)가 적용되며, 수학식In addition, the probability density function is applied with a saturated Gaussian function,
(여기서, p(Em(x))는 상기 위치 샘플 또는 상기 후보 샘플의 추정 확률이고, η는 노르말라이저(Normalizer)이고, σt는 시간 t에서 상기 위치 샘플 또는 상기 후보 샘플에 대한 상기 매칭 에러의 표준편차이고, μt는 시간 t에서 상기 위치 샘플 또는 상기 후보 샘플에 대한 상기 매칭 에러의 평균값이고, bias는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치이다)로 표현될 수 있다.
Where p (E m (x)) is the estimated probability of the location sample or the candidate sample, η is a normalizer, and σ t is the match for the location sample or the candidate sample at time t and a standard deviation of the error, μ t is the mean value of the matching error for the position samples or the candidate sample at time t, bias may be expressed as an error threshold value is determined based on a grid resolution of the environmental map) .
상기와 같은 구성을 통해, 매칭 에러를 스캔 데이터와 예측 데이터에 기초한 면적 단위로 산출하여, 맵핑되지 않은 장애물 등에 의해 야기되는 센서 데이터의 부분적인 손상에 강인하고, 정확한 위치 추정 실패의 판단이 가능한 이동 로봇의 위치 추정 방법이 제공된다.Through the above configuration, the matching error is calculated in the area unit based on the scan data and the predictive data, so that it is robust against partial damage of the sensor data caused by unmapped obstacles and the like, and the accurate positioning estimation movement is possible. A method for estimating the position of a robot is provided.
또한, 위치 추정 실패의 상황에서 환경 지도 전체 영역으로부터 위치 샘플을 추출하지 않고 매칭 에러의 통계값에 기초한 최대 모션 경계 영역 내에서만 위치 샘플을 추출함으로써, 위치 추정 실패 상황으로부터 이동 로봇의 실제 위치로 복귀하는데 있어 이동 로봇의 위치 추정의 정확성 및 추정 속도를 현저히 향상시킬 수 있는 이동 로봇의 위치 추정 방법이 제공된다.Also, in the event of location estimation failure, the location sample is extracted only within the maximum motion boundary region based on the statistical value of the matching error without extracting the location sample from the entire area of the environment map, thereby returning to the actual position of the mobile robot from the location estimation failure situation. To provide a method for estimating the position of a mobile robot, which can significantly improve the accuracy and the estimation speed of the position estimation of the mobile robot.
이를 통해, 인간이 공존하는 실제 환경, 예를 들어 박물관이나 오피스 빌딩과 같이 이동 로봇의 환경 지도에 맵핑(Mapping)되지 않은 장애물이 공존하는 실제 환경에서 적용이 가능하게 된다.
Through this, it is possible to apply in a real environment where humans coexist, for example, a real environment where obstacles that are not mapped to an environment map of a mobile robot coexist, such as a museum or an office building.
도 1은 본 발명에 따른 이동 로봇의 위치 추정 방법을 설명하기 위한 제어 흐름도이고,
도 2는 본 발명에 따른 이동 로봇의 위치 추정 방법에 따라 산출되는 매칭 에러를 개념적으로 설명하기 위한 도면이고,
도 3은 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러 분포를 시뮬레이션한 예를 도시한 도면이고,
도 4는 본 발명에 따른 이동 로봇의 위치 추정 방법에서 이동 로봇의 주시 방향(Heading direction)의 오차가 존재할 때의 매칭 에러를 나타낸 도면이고,
도 5는 이동 로봇에 MCL이 적용된 생태에서, MCL이 11,000 스캔 과정을 수행하는 동안 실험적으로 측정된 매칭 에러의 히스토리를 나타낸 도면이고,
도 6은 본 발명에 따른 이동 로봇의 위치 추정 방법의 최대 모션 경계 영역을 개념적으로 설명하기 위한 도면이고,
도 7은 Beam model과 본 발명에 따른 이동 로봇의 위치 추정 방법에 따른 확률 분포를 각각 도시한 도면이고,
도 8 및 도 9는 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러를 이용한 위치 추정 결과를 도시한 도면이고,
도 10은 미디언 필터가 적용되지 않은 상태와 적용된 상태에서 각각 측정된 매칭 에러와 에러 문턱치를 나타낸 그래프이고,
도 11 및 도 12는 본 발명에 따른 이동 로봇의 위치 추정 방법에 따라 이동 로봇의 실제 위치로의 복귀 과정을 설명하기 위한 도면이다.1 is a control flowchart for explaining a position estimation method of a mobile robot according to the present invention;
2 is a view for conceptually explaining a matching error calculated according to a position estimation method of a mobile robot according to the present invention;
3 is a diagram illustrating an example of simulation of a matching error distribution of a position estimation method of a mobile robot according to the present invention;
4 is a diagram illustrating a matching error when an error in a heading direction of a mobile robot exists in a method for estimating a position of a mobile robot according to the present invention;
FIG. 5 is a diagram illustrating a history of matching errors experimentally measured while MCL performs a 11,000 scan process in an ecology to which MCL is applied to a mobile robot.
6 is a view for conceptually explaining the maximum motion boundary region of the position estimation method of the mobile robot according to the present invention;
7 is a diagram illustrating probability distributions according to a beam model and a position estimation method of a mobile robot according to the present invention,
8 and 9 are diagrams showing a position estimation result using a matching error of the position estimation method of the mobile robot according to the present invention,
10 is a graph illustrating matching errors and error thresholds measured in a state where a median filter is not applied and an applied state, respectively.
11 and 12 are views for explaining a process of returning to the actual position of the mobile robot according to the position estimation method of the mobile robot according to the present invention.
이하에서는 첨부된 도면을 참조하여 본 발명을 상세히 설명한다.Hereinafter, with reference to the accompanying drawings will be described in detail the present invention.
도 1은 본 발명에 따른 이동 로봇의 위치 추정 방법을 설명하기 위한 제어 흐름도이다.1 is a control flowchart for explaining a position estimation method of a mobile robot according to the present invention.
도 1을 참조하여 설명하면, 이동 로봇에 설치된 레인지 센서의 스캔에 따라 스캔 데이터가 획득된다(S10). 여기서, 본 발명에 따른 레인지 센서는 레이저 레인지 센서 형태로 마련되며, 각 해상도(Angular resolution)가 1ㅀ로, 180ㅀ의 범위를 1ㅀ단위로 스캔하는 것을 예로 한다.Referring to FIG. 1, scan data is acquired according to a scan of a range sensor installed in a mobile robot (S10). Here, the range sensor according to the present invention is provided in the form of a laser range sensor, an example in which each resolution (Angular resolution) is 1 kHz, scanning a range of 180 kHz in 1 kHz units.
레인지 센서에 의해 스캔 데이터가 획득되면, 획득된 스캔 데이터를 이용하여 매칭 에러를 산출한다(S11). 여기서, 본 발명에서는 스캔 데이터와, 기 설정된 환경 지도에 기초하여 예측되는 예측 데이터에 기초하여 매칭 에러가 산출되는 것을 예로 한다.When scan data is acquired by the range sensor, a matching error is calculated using the acquired scan data (S11). Here, in the present invention, a matching error is calculated based on the scan data and the prediction data predicted based on the preset environment map.
도 2를 참조하여 보다 구체적으로 설명하면, 스캔 데이터에 기초하여 산출되는 스캔 범위 영역과, 예측 데이터에 기초하여 산출되는 예측 범위 영역 간의 편차에 기초하여 매칭 에러가 산출된다.More specifically, referring to FIG. 2, a matching error is calculated based on a deviation between a scan range region calculated based on scan data and a prediction range region calculated based on prediction data.
즉, 본 발명에 따른 이동 로봇의 위치 추정 방법에 적용되는 관측 우도 모델(Observation likelihood model)의 매칭 에러는 스캔 데이터와 예측 데이터 자체의 차이가 아닌, 스캔 데이터, 즉 스캔된 거리 데이터에 의해 산출되는 스캔 범위 영역과 예측 데이터에 의해 산출되는 예측 범위 영역 간의 편차를 이용하게 된다. 여기서, 도 2를 참조하여 설명하면, 본 발명에 따른 매칭 에러는 [수학식 1]에 의해 산출되는 것을 예로 한다.That is, the matching error of the observation likelihood model applied to the position estimation method of the mobile robot according to the present invention is calculated by the scan data, that is, the scanned distance data, not the difference between the scan data and the prediction data itself. The deviation between the scan range region and the prediction range region calculated by the prediction data is used. 2, the matching error according to the present invention is calculated by Equation 1 as an example.
[수학식 1][Equation 1]
여기서, Em(x)는 매칭 에러이고, 는 시간 t에서의 j번째 스캔 데이터이고, 는 시간 t에서의 j번째 예측 데이터이다. 그리고, 는 시간 t에서의 j번째 상기 스캔 범위 영역이고, 는 시간 t에서의 j번째 상기 예측 범위 영역이고, Δθ는 레인지 센서의 각 해상도(Angular resolution)이다. 그리고, 본 발명에 따른 이동 로봇의 위치 추정 방법에서의 예측 데이터는 레이-캐스팅 알고리즘(Ray-casting algorithm)을 통해 환경 지도로부터 계산되는 것을 예로 한다.Where Em (x) is a matching error, Is the jth scan data at time t, Is the j th prediction data at time t. And, Is the j th scan range region at time t, Is the j-th prediction region at time t, and Δθ is the angular resolution of the range sensor. In addition, the prediction data in the position estimation method of the mobile robot according to the present invention is an example of being calculated from the environment map through a ray-casting algorithm.
[수학식 1]의 식 (4)가 매칭 에러를 정의한다. 여기서, [수학식 4]의 분모는 정규화(Normalization)을 수행한다. 여기서, 정규화는 매칭 에러가 임의의 확률로 표현되는 숫자가 아닌 % 오차로 표현되는 것을 의미하며, 이를 통해 사용자가 매칭 에러를 직관적인 의미로 인식 가능하게 된다.Equation (4) in [Equation 1] defines a matching error. Here, the denominator of [Equation 4] performs normalization. Here, normalization means that the matching error is expressed as a% error rather than a number represented by a random probability, thereby allowing the user to recognize the matching error in an intuitive meaning.
또한, 정규화는 스캔 범위 영역과 예측 범위 영역의 편차, 즉 면적의 편차를 이용하여 매칭 에러를 산출함으로서, 센서 데이터의 부분적인 손상, 예를 들어 인간과 같이 환경 지도에 맵핑되지 않은 장애물에 의해 발생하는 센서 데이터의 부분적인 손상에도 강인함을 의미한다. 따라서, 종래의 매칭 에러가 센서 데이터와 예측 데이터에 대해 개별적으로 산출됨에 따라, 손상된 센서 데이터를 제거하기 위한 추가적인 필터링 기술을 적용해여야 하는 문제점을 해소할 수 있다.In addition, normalization is caused by partial damage of sensor data, such as obstacles that are not mapped to an environment map, such as a human, by calculating a matching error using the deviation between the scan range region and the predicted range region, that is, the area variation. This means that it is robust against partial damage of sensor data. Therefore, as the conventional matching error is calculated separately for the sensor data and the prediction data, it is possible to solve the problem of applying an additional filtering technique for removing the damaged sensor data.
도 3은 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러 분포를 시뮬레이션한 예를 도시하고 있다. 도 3에서는 실제 이동 로봇의 포즈가 환경 지도의 (101,101)에 위치할 때의 매칭 에러의 분포를 시뮬레이션한 예을 도시하고 있다. 여기서, 도 3에서는 환경 지도가 정사각형이고, 그리드 맵(Grid map)을 사용하는 것을 예로 하고 있다. 그리고, 그리드 맵의 정사각형 경계는 검은 점으로 표시되어 있다.3 illustrates an example of simulation of a matching error distribution of the position estimation method of the mobile robot according to the present invention. FIG. 3 shows an example of simulation of distribution of matching errors when the pose of the actual mobile robot is positioned at (101, 101) of the environment map. Here, in FIG. 3, the environment map is square and an example of using a grid map is used. The square borders of the grid map are marked with black dots.
도 3의 (a)는 장애물이 존재하지 않은 상태에서의 시뮬레이션을 도시한 것이고, 도 3의 (b)는 검은 블록으로 표시된 4개의 장애물이 존재하는 상태에서의 시뮬레이션을 도시하고 있다. 도 3(a)에 도시된 바와 같이, 실제 이동 로봇의 포즈, 즉 (101,101) 위치에서 매칭 에러가 최소가 되고, 매칭 에러의 경계면이 이동 로봇의 포즈 에러에 따라 서서히 증가함을 확인할 수 있다.FIG. 3A illustrates a simulation in the absence of an obstacle, and FIG. 3B illustrates a simulation in the presence of four obstacles represented by black blocks. As shown in FIG. 3 (a), it can be seen that the matching error is minimized at the pose of the actual mobile robot, that is, at positions (101, 101), and the boundary of the matching error gradually increases with the pose error of the mobile robot.
도 3의 (b)에 도시된 바와 같이, 4개의 장애물이 존재하더라도 매칭 에러의 경계면의 전체적인 형상이, 장애물에 의해 센서 데이터의 손상이 발생하더라도 많이 변하지 않음을 확인할 수 있다. 즉, 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러가 센서 데이터의 부분적인 손상에도 강인함을 확인할 수 있다.As shown in (b) of FIG. 3, even when four obstacles exist, the overall shape of the interface of the matching error does not change much even if the sensor data is damaged by the obstacle. That is, it can be seen that the matching error of the position estimation method of the mobile robot according to the present invention is robust against partial damage of the sensor data.
도 4는 방향 에러(Orientation error), 즉 이동 로봇의 주시 방향(Heading direction)의 오차가 존재할 때의 매칭 에러를 나타낸 도면이다. 여기서, 이동 로봇의 실제 각도는 0ㅀ로 설정된 상태이다.FIG. 4 is a diagram illustrating an orientation error, that is, a matching error when an error in a heading direction of a mobile robot exists. Here, the actual angle of the mobile robot is set to 0 ㅀ.
도 4에서 실선으로 나타낸 그래프가 도 3의 (a)에서와 같이 장애물이 존재하지 않은 상태에서의 매칭 에러이고, 점선으로 나타낸 그래프가 도 3의 (b)에서와 같이 장애물이 존재하는 상태에서의 매칭 에러이다. 도 4를 통해 본 발명에 따른 매칭 에러가 이동 로봇의 실제 각도에서 최소가 되는 것을 확인할 수 있으며, 이를 통해 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러가 이동 로봇의 위치나 주시 방향과 같은 모든 케이스에서 이상적인 수렴 특성을 나타내는 것을 확인할 수 있다.In FIG. 4, the graph indicated by the solid line is a matching error in the absence of an obstacle as shown in FIG. 3A, and the graph indicated by the dotted line is in the state where an obstacle exists as in FIG. 3B. Matching error. 4, it can be seen that the matching error according to the present invention is minimized from the actual angle of the mobile robot. Through this, the matching error of the position estimation method of the mobile robot according to the present invention is equal to the position or gaze direction of the mobile robot. It can be seen that in all cases, they show ideal convergence characteristics.
다시, 도 1을 참조하여 설명하면, 상기와 같은 방법을 통해 이동 로봇의 주행 중에 레인지 센서로부터의 스캔 데이터를 이용하여 매칭 에러가 산출되면, 산출된 매칭 에러가 에러 문턱치를 초과하는지 여부를 판단한다(S13). 여기서, 매칭 에러가 에러 문턱치를 초과하는 것으로 판단되는 경우에는 위치 추정 실패로 인식하게 되고, 매칭 에러가 에러 문턱치를 초과하지 않는 것으로 판단되는 경우에는 위치 추정 성공으로 인식하게 된다.Referring back to FIG. 1, if a matching error is calculated using the scan data from the range sensor while the mobile robot is running through the above method, it is determined whether the calculated matching error exceeds the error threshold. (S13). Here, if it is determined that the matching error exceeds the error threshold, it is recognized as a location estimation failure, and if it is determined that the matching error does not exceed the error threshold, it is recognized as the location estimation success.
여기서, 본 발명에 따른 이동 로봇의 위치 추정 방법에서는 매칭 에러가 에러 문턱치를 초과하는지 여부를 판단하기 전에 S11 단계에서 산출된 매칭 에러에 대해 미디언 필터(Median filter)가 적용되는 것을 예로 한다.Here, in the method for estimating the position of the mobile robot according to the present invention, a median filter is applied to the matching error calculated in step S11 before determining whether the matching error exceeds the error threshold.
매칭 에러는 짧은 시간 간격 동안에 에러 문턱치보다 크게 나타날 수 있다. 이에, 본 발명에서는 짧은 순간의 이상치(short-time outlier value)의 제거를 위해 미디언 필터를 사용한다. 미디언 필터는 데이터가 n surrounding point의 미디언 값으로 대체하는 것에 의해 이상치를 제거하게 되며, 본 발명에서는 n값으로 15를 설정하는 것을 예로 한다.The matching error may appear greater than the error threshold for a short time interval. Accordingly, the present invention uses a median filter to remove a short-time outlier value. The median filter removes outliers by replacing data with median values of n surrounding points. In the present invention, 15 is set to n as an example.
한편, 본 발명에 따른 이동 로봇의 위치 추정 방법의 에러 문턱치는 레인지 센서의 이전 스캔 동작들을 통해 상술한 바와 같이 산출된 매칭 에러의 통계값에 기초하여 업데이트된다. 즉, 에러 문턱치는 고정된 값이 아닌 레인지 센서의 이전 스캔 동작 과정에서 산출된 매칭 에러를 이용하여 지속적으로 업데이트된다.Meanwhile, the error threshold of the position estimation method of the mobile robot according to the present invention is updated based on the statistical value of the matching error calculated as described above through the previous scan operations of the range sensor. In other words, the error threshold is continuously updated using a matching error calculated during a previous scan operation of the range sensor instead of a fixed value.
도 5는 이동 로봇에 MCL이 적용된 생태에서, MCL이 11,000 스캔 과정을 수행하는 동안 실험적으로 측정된 매칭 에러의 히스토리이다. 도 5에 도시된 매칭 에러는 이동 로봇이 전형적인 오피스 빌딩을 주행하는 환경에 대해 측정된 것으로, 다양한 에러 원인에 의해 매칭 에러가 0이 아님을 확인할 있다. 즉, 매칭 에러는 환경 지도의 사이즈, 매핑되지 않은 장애물이나 센서 정밀도 등에 의해 발생할 수 있다. 따라서, 본 발명에서는 이와 같이 매칭 에러에 영향을 미치는 요소가 반영될 수 있도록 이전 스캔 동작 과정에서 산출된 매칭 에러를 이용하여 에러 문턱치를 업데이트하는 것을 예로 한다.5 is a history of matching errors experimentally measured while the MCL performs a 11,000 scan procedure in an ecology with MCL applied to a mobile robot. The matching error shown in FIG. 5 is measured for an environment in which a mobile robot travels in a typical office building, and it can be confirmed that the matching error is not zero due to various error sources. That is, the matching error may occur due to the size of the environment map, unmapped obstacles, sensor accuracy, or the like. Accordingly, in the present invention, an error threshold value is updated using a matching error calculated in a previous scan operation so that the factors affecting the matching error may be reflected.
여기서, 본 발명에 따른 이동 로봇의 위치 추정 방법에서는 에러 문턱치가 매칭 에러의 평균값과 표준편차를 이용하는 것을 예로 하며, 이는 [수학식 2]와 같이 정의될 수 있다.Here, in the position estimation method of the mobile robot according to the present invention, an error threshold value uses an average value of a matching error and a standard deviation as an example, which may be defined as shown in [Equation 2].
[수학식 2]&Quot; (2) "
여기서, h는 에러 문턱치이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 매칭 에러의 평균값이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 매칭 에러의 표준편차이고, bias는 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치이다.Where h is the error threshold, Is the average of the matching errors from the (ik) th scan to the i th scan, Is the standard deviation of the matching error from the (ik) th scan to the i th scan, and the bias is the error criterion determined based on the grid resolution of the environment map.
그리고, 본 발명에 따른 이동 로봇의 위치 추정 방법의 에러 문턱치의 산출에 적용되는 매칭 에러는 이전 스캔 과정에서 위치 추정 성공으로 판단된 매칭 에러를 사용하게 되며, 이에 따라, 본 발명에 따른 에러 문턱치는 위치 추정 성공으로 판단된 매칭 에러의 상위 한계로 정의할 수 있다.In addition, the matching error applied to the calculation of the error threshold of the position estimation method of the mobile robot according to the present invention uses the matching error determined as the position estimation success in the previous scanning process, and thus, the error threshold according to the present invention It can be defined as the upper limit of the matching error determined as the position estimation success.
여기서, [수학식 2]는 가우시안 확률과 테스트를 이용한 Hypothesis testing에 기초한 슬라이딩 윈도우 기법(Sliding window method)을 적용한 것으로, Hypothesis testing을 간소화시켜 나타내고 있다. 즉, 본 발명에서는 기존의 Hypothesis testing에 기초한 슬라이딩 윈도우 기법(Sliding window method)에서 함수가 갖는 계산의 복잡성을 제거하기 위해 에러 문턱치의 계산에 [수학식 8]과 같이, 슬라이딩 윈도우 기법(Sliding window method)을 간소화시켰다. 여기서, 슬라이딩 윈도우 사이즈 k는 여러 실험으로부터 경험적으로 결정될 수 있으며, 본 발명에서는 슬라이딩 윈도우 사이즈 k를 100초를 기준으로 하여 500으로 설정하는 것을 예로 한다.Where
또한, [수학식 2]에서의 bias는 가우시안 확률의 상위 경계(Upper bound)를 나타내는데, 상술한 바와 같이, 환경 지도의 그리드 해상도에 기초하여 결정된다. 예를 들어, 환경 지도의 그리드 해상도가 10cm인 경우 bias 값으로 3%로 결정될 수 있다.In addition, the bias in [Equation 2] represents the upper bound of the Gaussian probability. As described above, the bias is determined based on the grid resolution of the environment map. For example, if the grid resolution of the environment map is 10cm, it may be determined as a bias value of 3%.
여기서, bias는 매칭 에러의 평균값 및 표준편차가 극단적으로 0인 경우에도 에러 문턱치가 bias의 크기로 존재하는 것을 의미하게 되며, 매칭 에러가 bias 만큼 존재하더라도, 이는 완경지도의 그리드 해상도에 따른 오차 범위 내에 속하기 때문에 위치 추정 실패나 위치 추정 성공의 판단에 영향을 미치지 않기 때문이다. 이와 같이, 환경 지도의 그리드 해상도가 bias를 통해 에러 문턱치에 반영됨으로써, 환경 지도의 그리드 해상도에 의해 발생 가능한 매칭 에러가 위치 추정 성공이나 실패를 결정하는데 미치는 영향을 제거할 수 있게 된다.Here, the bias means that even if the average value and the standard deviation of the matching error is extremely zero, the error threshold exists as the size of the bias. Even if the matching error exists as the bias, this means that the error range according to the grid resolution of the slow map. This is because it does not affect the determination of the position estimation failure or the position estimation success. As such, since the grid resolution of the environment map is reflected in the error threshold through bias, it is possible to eliminate the influence of the matching error, which may be caused by the grid resolution of the environment map, in determining the location success or failure.
다시, 도 1을 참조하여 설명하면, 상기와 같이 업데이트되는 에러 문턱치에 기초하여, S13 단계에서 이동 로봇의 현재 위치 추정이 위치 추정 실패인지 위치 추정 성공인지 여부를 판단하게 된다.Referring back to FIG. 1, it is determined whether the current position estimation of the mobile robot is position estimation failure or position estimation success in step S13 based on the updated error threshold.
여기서, 위치 추정 성공으로 판단되는 경우, 즉 현재 스텝에서의 매칭 에러가 에러 문턱치 이하인 경우, 현재 스캔 단계에서 산출된 매칭 에러와, [수학식 2]에서와 같이, 슬라이딩 윈도우 사이즈인 k번째 이전까지의 매칭 에러를 이용하여 평균값과 표준편차를 산출하여(S14), 에러 문턱치를 업데이트한다(S15).Here, when it is determined that the position estimation is successful, that is, when the matching error in the current step is less than or equal to the error threshold, the matching error calculated in the current scanning step and up to the kth before the sliding window size as shown in [Equation 2] The average value and the standard deviation are calculated using the matching error of (S14), and the error threshold value is updated (S15).
한편, S12 단계에서 매칭 에러가 에러 문턱치를 초과하여 위치 추정 실패로 판단되는 경우, 환경 지도 상의 적어도 일 영역으로부터 복수의 위치 샘플을 추출한다(S16). 여기서, 위치 샘플은 환경 지도 상에서 이동 로봇이 위치하는 것으로 추정되는 위치들에 대한 샘플에 해당하며, 본 발명에서는 기 등록된 모션 모델에 기초한 최대 모션 경계 영역 내에서 위치 샘플이 추출되는 것을 예로 한다.On the other hand, when it is determined that the matching error exceeds the error threshold in step S12 and the location estimation failure, a plurality of location samples are extracted from at least one region on the environment map (S16). Here, the position sample corresponds to a sample of positions estimated to be located on the environment map. In the present invention, the position sample is extracted within the maximum motion boundary region based on a previously registered motion model.
도 6을 참조하여 보다 구체적으로 설명하면, 이동 로봇의 위치 추정이 실패로 판단된 후, 도 6에 도시된 바와 같이, 원 형태의 최대 모션 경계 영역(Maximum Motion Boundary)이 설정된다. 이동 로봇이 시간 t0에서 경로 1(reference path)을 추종하여 주행을 시작하는 것으로 가정하고, 시간 t0에서는 위치 추정 성공 상태인 것으로 가정한다. 그리고, 이동 로봇이 주행하는 동안, 시간 t1에서 이동 로봇의 좌측 휠이 미끄러져 이동 로봇이 경로 2(real path)를 따라 이동한 것으로 가정한다.More specifically, referring to FIG. 6, after it is determined that the position estimation of the mobile robot has failed, as shown in FIG. 6, a maximum motion boundary of a circular shape is set. It assumed that the mobile robot is the time t by following the path 1 (reference path) from 0 it is assumed to start running, a time t 0, the position estimation success. In addition, it is assumed that while the mobile robot travels, the left wheel of the mobile robot slides at time t 1 and the mobile robot moves along a real path.
이 때, 이동 로봇은 시간 t3에서 위치 추정 실패로 인식한다. 실제로 경로 1(reference path)에서 벗어나는 시간 t1과 이를 감지한 시간 t3 사이의 차이는 본 발명에 따른 이동 로봇의 위치 추정 방법의 진행 사이클 타임과 레인지 센서의 센싱 사이클 타임 간의 편차에 의해서 발생할 수 있다.At this time, the mobile robot recognizes the position estimation failure at time t 3 . Actually, the difference between the time t 1 deviating from the reference path and the detected time t 3 may be caused by the deviation between the progress cycle time of the position estimation method of the mobile robot and the sensing cycle time of the range sensor according to the present invention. have.
본 발명에 따른 최대 모션 경계 영역(Maximum Motion Boundary)은 위치 추정 실패로 판단된 시간 t3의 직전의 위치 추정 성공 위치를 중심으로 모션 모델에 기초하여 산출되는 최대 이동 거리(Smax)를 반경으로 하는 원 형태로 설정된다. 여기서, 최대 모션 경계 영역(Maximum Motion Boundary)은 이동 로봇이 이동 가능한 모든 방향에서 이동 로봇이 시간 t0에서 t3 동안 이동 가능한 거리를 가정하여 계산된다.The maximum motion boundary area according to the present invention is a radius of the maximum moving distance S max calculated based on the motion model based on the position estimation success position immediately before the time t 3 determined as the position estimation failure. It is set in the form of a circle. Here, the maximum motion boundary is calculated by assuming a distance at which the mobile robot can move for a time t 0 to t 3 in all directions in which the mobile robot can move.
본 발명에 따른 모션 모델은 가우시안 확률 분포를 갖는 모션 모델이 적용되는 것을 예로 하며, 최대 모션 경계 영역(Maximum Motion Boundary)의 산출을 위한 최대 이동 거리(Smax)는 [수학식 3]을 통해 산출될 수 있다.As an example of the motion model according to the present invention, a motion model having a Gaussian probability distribution is applied, and the maximum moving distance S max for calculating the maximum motion boundary is calculated through Equation 3. Can be.
[수학식 3]&Quot; (3) "
여기서, Smax는 최대 이동 거리이고, σ는 모션 모델, 즉 가우시안 확률 분포를 갖는 모션 모델의 표준 편차이고, Δsi는 이동 로봇에 설치된 휠 엔코더로부터 측정된 이동 거리이다.Where S max is the maximum travel distance, sigma is the standard deviation of the motion model, ie a motion model with Gaussian probability distribution, and Δs i is the travel distance measured from the wheel encoder installed in the mobile robot.
이와 같이, 이동 로봇이 위치 추정 실패로 판단된 상태, 즉 이동 로봇의 현재 위치를 알지 못하는 상태에서, 이동 로봇의 실제 위치를 추정하기 위한 위치 샘플을 환경 지도의 전체 영역이 아닌 최대 모션 경계 영역(Maximum Motion Boundary) 내에서 추출함으로써, 전역 위치 추정(Global localization) 기법을 이용한 위치 추정보다 위치 추정에 필요한 계산량을 현저히 감소시킬 수 있게 된다.As described above, in a state where the mobile robot is determined to have failed position estimation, that is, the current position of the mobile robot is not known, the position sample for estimating the actual position of the mobile robot is not the entire region of the environment map but the maximum motion boundary region ( By extracting within the Maximum Motion Boundary, it is possible to significantly reduce the amount of computation required for the position estimation rather than the position estimation using the global localization technique.
상기와 같이 설정된 최대 모션 경계 영역(Maximum Motion Boundary) 내에서 복수의 위치 샘플이 추출되면(S16), 각 위치 샘플에 이동 로봇이 위치하는 것으로 가정하여 매칭 에러를 산출한다(S17). 여기서, 매칭 에러는 상술한 바와 같이, 본 발명에 따른 이동 로봇의 위치 추정 방법의 관측 오도 모델(Observation likelihood model)의 매칭 에러의 산출 방법에 따라 산출되며, [수학식 1]에서와 같이 산출되는 것을 예로 한다.When a plurality of position samples are extracted in the maximum motion boundary set as described above (S16), a matching error is calculated on the assumption that the mobile robot is located at each position sample (S17). Here, as described above, the matching error is calculated according to the calculation method of the matching error of the observation likelihood model of the position estimation method of the mobile robot according to the present invention, and is calculated as shown in [Equation 1] Take this as an example.
그런 다음, 산출된 매칭 에러를 기 설정된 확률 밀도 함수(Probability density function)에 적용하여 이동 로봇의 실제 위치를 추정하게 된다. 여기서, 본 발명에 따른 이동 로봇의 위치 추정 방법에서는 상술한 바와 같이 추출된 위치 샘플들을 본 발명에 따른 위치 추정 방법의 관측 우도 모델(Observation likelihood model)의 확률 밀도 함수(Probability density function), 즉 확률 업데이트 룰(Probability update rule)에 적용하여 이동 로봇의 실제 위치를 추정하게 된다.Then, the calculated matching error is applied to a preset probability density function to estimate the actual position of the mobile robot. Here, in the position estimation method of the mobile robot according to the present invention, the position samples extracted as described above are included in the probability density function, that is, the probability of the observation likelihood model of the position estimation method according to the present invention. The actual position of the mobile robot is estimated by applying the update update rule.
도 1을 참조하여 보다 구체적으로 설명하면, 각 위치 샘플에 대해 산출된 매칭 에러를 확률 밀도 함수(Probability density function)에 적용하여 각 위치 샘플에 대한 추정 확률을 산출한다(S18). 그리고, 산출된 추정 확률에 기초하여 위치 샘플 중 후보 샘플을 추출한다(S19).In more detail with reference to FIG. 1, an estimated probability for each location sample is calculated by applying a matching error calculated for each location sample to a probability density function (S18). Then, candidate samples of the position samples are extracted based on the calculated estimation probability (S19).
여기서, 본 발명에 따른 이동 로봇의 위치 추정 방법에서는 확률 밀도 함수(Probability density function)로 포화 가우시안 함수(Saturated Gaussian function)가 적용되는 것을 예로 하며, [수학식 4]와 같이 표현될 수 있다.Here, in the position estimation method of the mobile robot according to the present invention, a saturated Gaussian function is applied as a probability density function, and may be expressed as
[수학식 4]&Quot; (4) "
여기서, p(Em(x))는 위치 샘플의 추정 확률이고, η는 노르말라이저(Normalizer)이다. 그리고, σt는 시간 t에서 위치 샘플에 대한 매칭 에러의 표준편차이고, μt는 시간 t에서 위치 샘플에 대한 매칭 에러의 평균값이다. 그리고, bias는 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치로 [수학식 2]의 bias와 동일하게 설정된다. 이와 같이, 위치 샘플의 통계적 데이터, 즉 각 위치 샘플에 대한 매칭 에러의 평균값과 표준편차를 이용함으로써, 수렴 속도(Convergence speed)의 조절이 가능하게 된다.Where p (Em (x)) is the estimated probability of the position sample, and η is the normalizer. Σ t is the standard deviation of the matching error for the position sample at time t, and μ t is the mean value of the matching error for the position sample at time t. The bias is an error reference value determined based on the grid resolution of the environment map, and is set equal to the bias shown in [Equation 2]. In this way, the convergence speed can be adjusted by using the statistical data of the position samples, that is, the average value and the standard deviation of the matching error for each position sample.
[수학식 4]의 식 (1)은 확률의 포화 영역(Saturation region of probability)을 나타낸다. 여기서, bias, 즉 에러 기준치는 매칭 에러가 에러 기준치보다 작으면 확률이 포화되는 것을 의미하며, 본 발명에 따른 이동 로봇의 위치 추정 방법의 관측 우도 모델(Observation likelihood model)의 수렴 한계(convergence limit)를 의미하게 된다. 즉, 매칭 에러가 에러 기준치보다 작은 경우에는 [수학식 4]의 식 (2)에 나타낸 가우시한 확률 함수의 최대값과 동일한 추정 확률을 갖도록 한다.Equation (1) of [Equation 4] represents a saturation region of probability. Here, the bias, that is, the error reference value means that the probability is saturated when the matching error is smaller than the error reference value, and the convergence limit of the observation likelihood model of the position estimation method of the mobile robot according to the present invention. Will mean. In other words, when the matching error is smaller than the error reference value, the matching error is equal to the maximum value of the Gaussian probability function shown in Equation (2).
그리고, [수학식 4]의 식 (3)은 매칭 에러가 보다 큰 경우를 나타낸 것으로, 이는 수렴 영역이 전체 확률 분포 영역의 99.7%를 차지하는 것을 의미한다. 그리고, [수학식 4]의 식 (2)는 가우시안 확률 함수를 나타낸다.Equation (3) of
도 7의 (a)는 Beam model(Thrun 등은 논문 "Probabilistic Robotics", The MIT Press, 2005, pp.153-169.)의 확률 분포를 도시한 도면이고, 도 7의 (b)는 본 발명에 따른 이동 로봇의 위치 추정 방법에 따른 확률 분포를 도시한 도면이다. 도 7의 (a)와 (b)는 동일한 환경 조건 하에서 시뮬레이션된 그래프를 도시하고 있다.FIG. 7 (a) is a diagram illustrating a probability distribution of a beam model (Thrun et al., The paper "Probabilistic Robotics", The MIT Press, 2005, pp. 153-169.), And FIG. 7 (b) is the present invention. The probability distribution according to the position estimation method of the mobile robot according to FIG. 7 (a) and 7 (b) show graphs simulated under the same environmental conditions.
일반적으로 연속 분포(Continuous distribution)가 불연속 분포(Discontinuous distribution)보다 강인한 것으로 알려져 있으며, 도 7의 (a)에서는 확률 분포가 실제 위치 주변에서 지역적으로 불연속적이라는 것을 확인할 수 있고, 위치 에러가 20cm보다 크다는 것을 확인할 수 있다. 이는 Beam model이 센서 노이즈를 처리하기 위해 많은 수의 위치 샘플을 필요로 한다는 것을 의미하며, 본 발명에 따른 이동 로봇의 위치 추정 방법에 따른 확률 분포가 도 7의 (b)에 도시된 바와 같이, 실제 위치 주변에서 스무드(smooth)하여 보다 효과적임을 확인할 수 있다.In general, the continuous distribution is known to be stronger than the discontinuous distribution, and in FIG. 7A, it is confirmed that the probability distribution is locally discontinuous around the actual position, and the position error is greater than 20 cm. You can see that it is large. This means that the beam model needs a large number of position samples to process the sensor noise, and the probability distribution according to the position estimation method of the mobile robot according to the present invention is shown in FIG. It can be confirmed that it is more effective by smoothing around the actual position.
다시, 도 1을 참조하여 설명하면, 상기와 같은 과정을 통해 각 위치 샘플에 대한 추정 확률이 산출되면, 추정 확률에 기초하여 후보 샘플을 추출하는(S19) 리샘플링(Re-sampling) 과정을 수행하게 된다. 그리고, 추출된 후보 샘플들에 대한 매칭 에러를 산출하고(S20), 산출된 매칭 에러를 상술한 바와 같이, 확률 밀도 함수에 적용하여 추정 확률을 산출한다(S21).Referring back to FIG. 1, when the estimated probability for each position sample is calculated through the above process, a resampling process is performed to extract candidate samples based on the estimated probability (S19). do. Then, a matching error for the extracted candidate samples is calculated (S20), and the estimated matching error is applied to a probability density function to calculate an estimated probability (S21).
여기서, S19 단계 내지 S20 단계를 반복적으로 수행하는 과정에서, 확률 분포가 하나의 수렴 위치로 수렴되는지 여부를 판단하여(S22), 수렴 위치로 수렵하는 경우 해당 수렴 위치를 이동 로봇의 현재 위치로 판단하게 된다(S21).Here, in the process of repeatedly performing steps S19 to S20, it is determined whether the probability distribution converges to one convergence position (S22), and when the hunting is converged, the corresponding convergence position is determined as the current position of the mobile robot. It is made (S21).
이하에서는 본 발명에 따른 이동 로봇의 위치 추정 방법을 적용한 이동 로봇의 주행 결과에 대해 설명한다. 이동 로봇에는 각 해상도 1ㅀ로 180ㅀ를 스캔하는 레이저 레인지 센서가 설치되며, 레이저 레인지 센서의 최대 스캔 범위는 9m로 설정되었다.Hereinafter, a driving result of the mobile robot to which the position estimation method of the mobile robot according to the present invention is applied will be described. The mobile robot is equipped with a laser range sensor that scans 180 ㅀ at 1 ㅀ of each resolution, and the maximum scan range of the laser range sensor is set to 9m.
도 8 및 도 9는 본 발명에 따른 이동 로봇의 위치 추정 방법의 매칭 에러를 이용한 위치 추정 결과를 도시한 도면이다. 도 8 및 도 9의 좌측에는 매칭 에러의 등고선이 도시되어 있고, 우측에는 확률 분포가 도시되어 있다. 도 8의 (a)는 위치 샘플이 랜덤하게 분포된 상태의 첫 번째 샘플링 결과를 도시한 도면이다. 그리고, 상술한 바와 같이 확률 업데이트 룰에 따라 업데이트된 후, 도 8의 (b)에 도시된 바와 같이 매칭 에러가 60%보다 작은 영역으로 위치 샘플이 수렴되었다.8 and 9 are diagrams showing a position estimation result using a matching error of the position estimation method of the mobile robot according to the present invention. The contours of the matching errors are shown on the left side of Figs. 8 and 9, and the probability distribution is shown on the right side. FIG. 8A is a diagram illustrating a first sampling result in a state in which position samples are randomly distributed. After updating according to the probability update rule as described above, as shown in FIG. 8B, the location sample converges to a region where the matching error is less than 60%.
그리고, 3번의 업데이트 후에, 도 9의 (a)에 도시된 바와 같이, 위치 샘플이 10% 영역으로 수렴하는 것을 확인할 수 있다. 그리고, 업데이트의 반복을 통해 위치 샘플이, 도 9의 (b)에 도시된 바와 같이, 실제 로봇 포즈로 수렴하는 것을 확인할 수 있다. 여기서, 도 8 및 도 9의 σ 값은 위치 샘플들에 대해 산출된 매칭 에러의 표준 편차이다.After 3 updates, as shown in FIG. 9A, it can be seen that the location samples converge to the 10% region. The repetition of the update may confirm that the position sample converges to the actual robot pose, as shown in FIG. 9B. 8 and 9 are the standard deviation of the matching error calculated for the position samples.
한편, 도 10은 이동 로봇의 200초 동안 실험 환경을 이동할 때 측정된 매칭 에러와 에러 문턱치를 나타낸 그래프이다. 도 10의 (a)는 매칭 에러에 미디언 필터가 적용되지 않은 상태이고, 도 10의 (b)는 본 발명에 따른 이동 로봇의 위치 추정 방법에 따라 미디언 필터가 적용된 상태이다.On the other hand, Figure 10 is a graph showing the matching error and the error threshold measured when moving the experimental environment for 200 seconds of the mobile robot. 10A illustrates a state in which a median filter is not applied to a matching error, and FIG. 10B illustrates a state in which a median filter is applied according to a method for estimating a position of a mobile robot according to the present invention.
도 10의 (a)에 도시된 바와 같이, 에러 문턱치 h를 초과하는 매칭 에러가 붉은색 원으로 표시되어 있으며, 위치 추정 실패가 6회 발생하였다. 반면, 본 발명에 따른 이동 로봇의 위치 추정 방법에서는 통상적인 주행 상태, 즉 매핑되지 않는 장애물이 존재하지 않는 상태에서의 주행에서는 위치 추정 실패로의 전환 없이 주행되는 것을 확인할 수 있다.As shown in (a) of FIG. 10, a matching error exceeding the error threshold h is indicated by a red circle, and a position estimation failure occurs six times. On the other hand, in the method for estimating the position of the mobile robot according to the present invention, it can be confirmed that the vehicle is driven without switching to the position estimation failure in the normal driving state, that is, in the state where no unmapped obstacle exists.
한편, 도 11은 이동 로봇이 붉은 점으로 표시된 위치에 위치한 상태에서 환경 지도와 레인지 센서에 의해 스캔된 스캔 데이터를 오버랩하여 도시한 도면이다. 도 11의 (a) 및 (b)는 이동 로봇에서 휠 미끄러짐이 발생하여 위치 추정 실패가 발생하는 경우의 환경 지도와 레인지 센서에 의해 스캔된 스캔 데이터를 오버랩하여 도시한 도면이다. 그리고, 도 12는 상기와 같은 상황에서의 매칭 에러와 에러 문턱치를 나타낸 그래프이다. 휠 미끄러짐에 의해 870초 부분에서 이동 로봇의 위치 추정이 실패되었음을 나타내고 있다.FIG. 11 is a diagram illustrating overlapping scan data scanned by an environment map and a range sensor in a state where a mobile robot is located at a position indicated by a red dot. 11 (a) and 11 (b) are diagrams showing overlapping scan data scanned by a range sensor and an environment map when a wheel slip occurs in a mobile robot and a position estimation failure occurs. 12 is a graph showing a matching error and an error threshold in the above situation. It is indicated that the position estimation of the mobile robot failed at 870 seconds due to the wheel slip.
이 때, 본 발명에 따른 이동 로봇의 위치 추정 방법은 870s 이후에 이동 로봇의 실제 위치를 추정하게 되며, 상술한 바와 같이, 최대 모션 경계 영역 내에서 위치 샘플을 추출하여 이동 로봇의 실제 위치를 추정하게 된다. 도 11의 (c)에 도시된 바와 같이, 최대 모션 경계 영역은 좌표 (2, 17)에서 미끄러짐 이후에 도달 가능한 영역에 걸쳐 퍼지게 된다. 여기서, 위치 추정은, 도 12에 도시된 바와 같이, 대략 20s 후에 위치 추정 실패로부터 복귀(도 11의 (d) 참조)되는 것을 확인할 수 있다.In this case, the position estimation method of the mobile robot according to the present invention estimates the actual position of the mobile robot after 870 s. As described above, the position sample is extracted within the maximum motion boundary region to estimate the actual position of the mobile robot. Done. As shown in Fig. 11C, the maximum motion boundary region is spread over the reachable region after slipping at coordinates (2, 17). Here, it can be seen that the position estimation is returned from the position estimation failure after approximately 20 s (see FIG. 11D) as shown in FIG. 12.
여기서, 전체 환경 지도 상의 영역이 1539m2라 가정하는 경우, 계산 비용은 위치 샘플의 개수에 비례하게 된다. 따라서, 전역 위치 추정 기법을 적용하는 경우의 위치 샘플의 사이즈는 대략 5,000 ~ 10,000개가 된다. 반면, 본 발명에 따른 이동 로봇의 위치 추정 방법을 적용하는 경우, 휠 미끄러짐에 의한 위치 추정 실패 상황에서의 최대 모션 경계 영역이 대략 2.5m2으로 설정되어 위치 샘플의 사이즈가 대략 300개로 감소되었으며, 이는 본 발명이 계산 비용 측면에서도 전역 위치 추정 기법에 비해 매우 효과적임을 알 수 있다.Here, if it is assumed that the area on the whole environment map is 1539m 2 , the calculation cost is proportional to the number of location samples. Therefore, the size of the position samples when the global position estimation technique is applied is approximately 5,000 to 10,000. On the other hand, in the case of applying the position estimation method of the mobile robot according to the present invention, the maximum motion boundary region in the case of the position estimation failure due to the wheel slip is set to approximately 2.5m 2 , thereby reducing the size of the position sample to approximately 300, It can be seen that the present invention is very effective compared to the global position estimation technique in terms of computational cost.
비록 본 발명의 몇몇 실시예들이 도시되고 설명되었지만, 본 발명이 속하는 기술분야의 통상의 지식을 가진 당업자라면 본 발명의 원칙이나 정신에서 벗어나지 않으면서 본 실시예를 변형할 수 있음을 알 수 있을 것이다. 별명의 범위는 첨부된 청구항과 그 균등물에 의해 정해질 것이다.Although several embodiments of the present invention have been shown and described, those skilled in the art will appreciate that various modifications may be made without departing from the principles and spirit of the invention . The scope of the alias will be defined by the appended claims and their equivalents.
Claims (14)
(a) 레인지 센서의 스캔에 따라 스캔 데이터가 획득되는 단계와;
(b) 상기 스캔 데이터에 기초한 스캔 범위 영역과, 기 등록된 환경 지도로부터 예측되는 예측 데이터에 기초한 예측 범위 영역 간의 편차에 기초하여 매칭 에러를 산출하는 단계와;
(c) 상기 (b) 단계에서 산출된 상기 매칭 에러가 에러 문턱치를 초과하는지 여부에 따라 위치 추정 실패 여부를 판단하는 단계와;
(d) 상기 (c) 단계에서 위치 추정 실패로 판단된 경우, 상기 환경 지도 상의 적어도 일 영역으로부터 추출된 복수의 위치 샘플 각각에 대해 매칭 에러를 산출하는 단계와;
(e) 상기 (d) 단계에서 산출된 상기 매칭 에러를 기 설정된 확률 밀도 함수에 적용하여 상기 이동 로봇의 위치를 추정하는 단계를 포함하며,
상기 매칭 에러는 수학식
(여기서, Em(x)는 상기 매칭 에러이고, 는 시간 t에서의 j번째 상기 스캔 데이터이고, 는 시간 t에서의 j번째 상기 예측 데이터이고, 는 시간 t에서의 j번째 상기 스캔 범위 영역이고, 는 시간 t에서의 j번째 상기 예측 범위 영역이고, Δθ는 상기 레인지 센서의 각 해상도(Angular resolution)이다)에 의해 산출되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.In the position estimation method of a mobile robot,
(a) obtaining scan data according to a scan of the range sensor;
(b) calculating a matching error based on a deviation between a scan range region based on the scan data and a prediction range region based on prediction data predicted from a pre-registered environment map;
(c) determining whether the position estimation has failed according to whether the matching error calculated in the step (b) exceeds an error threshold;
(d) calculating a matching error for each of a plurality of location samples extracted from at least one area on the environment map when it is determined that location estimation fails in step (c);
(e) estimating the position of the mobile robot by applying the matching error calculated in step (d) to a preset probability density function,
The matching error is
Where Em (x) is the matching error, Is the j th scan data at time t, Is the j th prediction data at time t, Is the j th scan range region at time t, Is the j-th prediction region at time t, and Δθ is calculated by the angular resolution of the range sensor.
상기 (d) 단계에서 상기 복수의 위치 샘플은 기 등록된 모션 모델에 기초한 최대 모션 경계 영역 내에서 추출되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method of claim 1,
In step (d), the plurality of position samples are extracted within the maximum motion boundary region based on a previously registered motion model.
(a) 레인지 센서의 스캔에 따라 스캔 데이터가 획득되는 단계와;
(b) 상기 스캔 데이터와, 기 설정된 환경 지도에 기초하여 예측되는 예측 데이터에 기초하여 매칭 에러를 산출하는 단계와;
(c) 상기 (b) 단계에서 산출된 상기 매칭 에러가 에러 문턱치를 초과하는지 여부에 따라 위치 추정 실패 여부를 판단하는 단계와;
(d) 상기 (c) 단계에서 위치 추정 실패로 판단된 경우, 기 등록된 모션 모델에 기초한 최대 모션 경계 영역 내에서 추출되는 복수의 위치 샘플 각각에 대해 매칭 에러를 산출하는 단계와;
(e) 상기 (d) 단계에서 산출된 상기 매칭 에러를 기 설정된 확률 밀도 함수에 적용하여 상기 이동 로봇의 위치를 추정하는 단계를 포함하고,
상기 (b) 단계는,
상기 스캔 데이터에 기초하여 스캔 범위 영역을 산출하는 단계와;
상기 예측 데이터에 기초하여 예측 범위 영역을 산출하는 단계와;
상기 스캔 범위 영역과 상기 예측 범위 영역 간의 편차에 기초하여 상기 매칭 에러를 산출하는 단계를 포함하며,
상기 매칭 에러는 수학식
(여기서, Em(x)는 상기 매칭 에러이고, 는 시간 t에서의 j번째 상기 스캔 데이터이고, 는 시간 t에서의 j번째 상기 예측 데이터이고, 는 시간 t에서의 j번째 상기 스캔 범위 영역이고, 는 시간 t에서의 j번째 상기 예측 범위 영역이고, Δθ는 상기 레인지 센서의 각 해상도(Angular resolution)이다)에 의해 산출되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.In the position estimation method of a mobile robot,
(a) obtaining scan data according to a scan of the range sensor;
(b) calculating a matching error based on the scan data and prediction data predicted based on a preset environment map;
(c) determining whether the position estimation has failed according to whether the matching error calculated in the step (b) exceeds an error threshold;
(d) calculating a matching error for each of a plurality of position samples extracted in a maximum motion boundary region based on a previously registered motion model when it is determined that the position estimation fails in step (c);
(e) estimating the position of the mobile robot by applying the matching error calculated in step (d) to a preset probability density function;
The step (b)
Calculating a scan range region based on the scan data;
Calculating a prediction range region based on the prediction data;
Calculating the matching error based on a deviation between the scan range region and the prediction range region,
The matching error is
Where Em (x) is the matching error, Is the j th scan data at time t, Is the j th prediction data at time t, Is the j th scan range region at time t, Is the j-th prediction region at time t, and Δθ is calculated by the angular resolution of the range sensor.
상기 (b) 단계에서 상기 예측 데이터는 레이-캐스팅 알고리즘(Ray-casting algorithm)을 통해 상기 환경 지도로부터 계산되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method according to claim 2 or 3,
In the step (b), the prediction data is calculated from the environment map through a ray-casting algorithm (Ray-casting algorithm), the location estimation method of the mobile robot.
상기 에러 문턱치는 상기 레인지 센서의 이전 스캔 동작들에 대한 상기 (b) 단계의 수행을 통해 산출된 매칭 에러의 통계값에 기초하여 업데이트되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method according to claim 2 or 3,
And the error threshold is updated based on a statistical value of a matching error calculated by performing step (b) for previous scan operations of the range sensor.
상기 에러 문턱치에는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치가 오차 범위로 반영되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method of claim 7, wherein
And an error reference value determined based on a grid resolution of the environment map as an error range in the error threshold.
상기 에러 문턱치는 수학식
(여기서, h는 상기 에러 문턱치이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 상기 매칭 에러의 평균값이고, 는 (i-k) 번째 스캔에서 i번째 스캔까지의 상기 매칭 에러의 표준편차이고, bias는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치이다)에 의해 산출되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.9. The method of claim 8,
The error threshold is expressed as
Where h is the error threshold, Is an average value of the matching error from the (ik) th scan to the i th scan, (Ik) is a standard deviation of the matching error from the i th scan to the i th scan, and the bias is an error reference value determined based on the grid resolution of the environment map). Way.
상기 (b) 단계에서 산출된 매칭 에러는 미디언 필터(Median filter)에 의해 필터링되어 상기 (c) 단계에 반영되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method according to claim 2 or 3,
The matching error calculated in step (b) is filtered by a median filter and reflected in step (c).
상기 모션 모델은 가우시안 확률 분포를 갖는 모션 모델이 적용되며;
상기 최대 모션 경계 영역은 상기 (c) 단계의 수행을 통한 직전 위치 추정 성공 위치를 중심으로 상기 모션 모델에 기초하여 산출되는 최대 이동 거리를 반경으로 하는 원 형태로 설정되는 것을 특징으로 이동 로봇의 위치 추정 방법.The method according to claim 2 or 3,
The motion model is applied with a motion model having a Gaussian probability distribution;
The maximum motion boundary region is set in a circle having a radius of a maximum movement distance calculated based on the motion model based on the last position estimation success position through the step (c). Estimation method.
상기 최대 이동 거리는 수학식
(여기서, Smax는 상기 최대 이동 거리이고, σ는 상기 모션 모델의 표준 편차이고, Δsi는 상기 이동 로봇의 휠 엔코더로부터 측정된 이동 거리이다)에 의해 산출되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method of claim 11,
The maximum moving distance is expressed by equation
(Where S max is the maximum travel distance, sigma is the standard deviation of the motion model, and Δs i is the travel distance measured from the wheel encoder of the mobile robot). Estimation method.
상기 (e) 단계는,
(e1) 상기 (d) 단계에서 산출된 상기 매칭 에러를 상기 확률 밀도 함수에 적용하여 상기 각 위치 샘플에 대한 추정 확률을 산출하는 단계와;
(e2) 상기 산출된 추정 확률에 기초하여 후보 샘플을 추출하는 단계와;
(e3) 상기 추출된 후보 샘플에 대한 매칭 에러를 산출하는 단계와;
(e4) 상기 (e3) 단계에서 산출된 상기 매칭 에러를 상기 확률 밀도 함수에 적용하여 상기 각 후보 샘플에 대한 추정 확률을 산출하는 단계와;
(e4) 상기 (e2) 단계, 상기 (e3) 단계 및 상기 (e4) 단계의 반복적인 수행을 통해 수렴되는 수렴 위치를 상기 이동 로봇의 위치로 추정하는 단계를 포함하는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method according to claim 2 or 3,
In step (e),
(e1) calculating the estimated probability for each location sample by applying the matching error calculated in step (d) to the probability density function;
(e2) extracting candidate samples based on the calculated estimated probability;
(e3) calculating a matching error for the extracted candidate sample;
(e4) calculating an estimated probability for each candidate sample by applying the matching error calculated in step (e3) to the probability density function;
(e4) estimating a convergence position converged through repetitive execution of steps (e2), (e3) and (e4) as the position of the mobile robot. Location estimation method.
상기 확률 밀도 함수는 포화 가우시안 함수(Saturated Gaussian function)가 적용되며, 수학식
(여기서, p(Em(x))는 상기 위치 샘플 또는 상기 후보 샘플의 추정 확률이고, η는 노르말라이저(Normalizer)이고, σt는 시간 t에서 상기 위치 샘플 또는 상기 후보 샘플에 대한 상기 매칭 에러의 표준편차이고, μt는 시간 t에서 상기 위치 샘플 또는 상기 후보 샘플에 대한 상기 매칭 에러의 평균값이고, bias는 상기 환경 지도의 그리드 해상도에 기초하여 결정되는 에러 기준치이다)로 표현되는 것을 특징으로 하는 이동 로봇의 위치 추정 방법.The method of claim 13,
The probability density function is applied with a saturated Gaussian function,
Where p (E m (x)) is the estimated probability of the location sample or the candidate sample, η is a normalizer, and σ t is the match for the location sample or the candidate sample at time t characterized in that, expressed as the standard deviation of the error, μ t is the mean value of the matching error for the position samples or the candidate sample at time t, bias is an error reference value, which is determined on the basis of the grid, the resolution of the environmental map) Position estimation method of the mobile robot.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
KR1020100052108A KR101202108B1 (en) | 2010-06-03 | 2010-06-03 | Localizing method for mobile robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
KR1020100052108A KR101202108B1 (en) | 2010-06-03 | 2010-06-03 | Localizing method for mobile robot |
Publications (2)
Publication Number | Publication Date |
---|---|
KR20110132659A KR20110132659A (en) | 2011-12-09 |
KR101202108B1 true KR101202108B1 (en) | 2012-11-15 |
Family
ID=45500540
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
KR1020100052108A KR101202108B1 (en) | 2010-06-03 | 2010-06-03 | Localizing method for mobile robot |
Country Status (1)
Country | Link |
---|---|
KR (1) | KR101202108B1 (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20150004568A (en) * | 2013-07-03 | 2015-01-13 | 삼성전자주식회사 | Method for recognizing position of autonomous mobile robot |
US9126338B2 (en) | 2012-08-16 | 2015-09-08 | Hanwha Techwin Co., Ltd. | Robot system and method for driving the same |
US11579298B2 (en) | 2017-09-20 | 2023-02-14 | Yujin Robot Co., Ltd. | Hybrid sensor and compact Lidar sensor |
US11874399B2 (en) | 2018-05-16 | 2024-01-16 | Yujin Robot Co., Ltd. | 3D scanning LIDAR sensor |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101341204B1 (en) * | 2012-02-29 | 2013-12-12 | 부산대학교 산학협력단 | Device and method for estimating location of mobile robot using raiser scanner and structure |
KR101508860B1 (en) * | 2013-12-31 | 2015-04-07 | 고려대학교 산학협력단 | Method for evaluating distance type of measured distance measured by laser range finder and method for localization of mobile robot using the same |
EP3144765B1 (en) | 2015-09-18 | 2020-01-08 | Samsung Electronics Co., Ltd. | Apparatus for localizing cleaning robot, cleaning robot, and controlling method of cleaning robot |
KR20180070062A (en) | 2016-12-16 | 2018-06-26 | 삼성전자주식회사 | A movable object and a method for controlling the same |
KR101888295B1 (en) * | 2017-01-24 | 2018-08-14 | 고려대학교 산학협력단 | Method for estimating reliability of distance type witch is estimated corresponding to measurement distance of laser range finder and localization of mobile robot using the same |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2009205226A (en) | 2008-02-26 | 2009-09-10 | Toyota Motor Corp | Autonomous moving robot, method of estimating self position, method and apparatus for creating environmental map, and data structure of environmental map |
-
2010
- 2010-06-03 KR KR1020100052108A patent/KR101202108B1/en active IP Right Grant
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2009205226A (en) | 2008-02-26 | 2009-09-10 | Toyota Motor Corp | Autonomous moving robot, method of estimating self position, method and apparatus for creating environmental map, and data structure of environmental map |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9126338B2 (en) | 2012-08-16 | 2015-09-08 | Hanwha Techwin Co., Ltd. | Robot system and method for driving the same |
KR20150004568A (en) * | 2013-07-03 | 2015-01-13 | 삼성전자주식회사 | Method for recognizing position of autonomous mobile robot |
KR102096398B1 (en) * | 2013-07-03 | 2020-04-03 | 삼성전자주식회사 | Method for recognizing position of autonomous mobile robot |
US11579298B2 (en) | 2017-09-20 | 2023-02-14 | Yujin Robot Co., Ltd. | Hybrid sensor and compact Lidar sensor |
US11874399B2 (en) | 2018-05-16 | 2024-01-16 | Yujin Robot Co., Ltd. | 3D scanning LIDAR sensor |
Also Published As
Publication number | Publication date |
---|---|
KR20110132659A (en) | 2011-12-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR101202108B1 (en) | Localizing method for mobile robot | |
KR101619076B1 (en) | Method of detecting and tracking moving object for mobile platform | |
Wijk et al. | Triangulation-based fusion of sonar data with application in robot pose tracking | |
CN109186610B (en) | Robust BSLAM method for AUV terrain matching navigation | |
O'Callaghan et al. | Learning navigational maps by observing human motion patterns | |
JP6240595B2 (en) | Self-position estimation apparatus and mobile body equipped with self-position estimation apparatus | |
JP5276931B2 (en) | Method for recovering from moving object and position estimation error state of moving object | |
US20210213619A1 (en) | Robot and control method therefor | |
Yang et al. | Feasibility grids for localization and mapping in crowded urban scenes | |
CN110375736B (en) | Path planning method, system and device of intelligent device and readable storage medium | |
Berrio et al. | Identifying robust landmarks in feature-based maps | |
Abrate et al. | Experimental EKF-based SLAM for Mini-rovers with IR Sensors Only. | |
Zhang et al. | Robotic mapping assisted by local magnetic field anomalies | |
Rabiee et al. | Competence-aware path planning via introspective perception | |
Himstedt et al. | Semantic monte-carlo localization in changing environments using rgb-d cameras | |
Li et al. | Real time obstacle detection in a water tank environment and its experimental study | |
Blanco et al. | Localization by voronoi diagrams correlation | |
Abd Rahman et al. | Tracking uncertain moving objects using dynamic track management in multiple hypothesis tracking | |
Gao et al. | Shared autonomy for assisted mobile robot teleoperation by recognizing operator intention as contextual task | |
Marzorati et al. | Particle-based sensor modeling for 3d-vision slam | |
Tian et al. | Kidnapping detection and recognition in previous unknown environment | |
Yu et al. | Detection and tracking of moving object with a mobile robot using laser scanner | |
ElKaissi et al. | SEDLRF: A new door detection system for topological maps | |
Thallas et al. | Particle filter—Scan matching SLAM recovery under kinematic model failures | |
Visser et al. | Robust weighted scan matching with quadtrees |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A201 | Request for examination | ||
E902 | Notification of reason for refusal | ||
E701 | Decision to grant or registration of patent right | ||
GRNT | Written decision to grant | ||
FPAY | Annual fee payment |
Payment date: 20151030 Year of fee payment: 4 |
|
FPAY | Annual fee payment |
Payment date: 20160928 Year of fee payment: 5 |
|
FPAY | Annual fee payment |
Payment date: 20181025 Year of fee payment: 7 |