KR20110132659A - Localizing method for mobile robot - Google Patents

Abstract

PURPOSE: A location estimating method of a mobile robot is provided to improve the accuracy and speed of the location estimating of a mobile robot by extracting a location sample within a maximum motion boundary not the entire section of an environmental map. CONSTITUTION: A location estimating method of a mobile robot comprises the following steps. The scan data is acquired according to the scan of a range sensor(S10). The matching error is generated based on the deviation between the scan range section based on the estimated data and estimating range section based on the estimated data(S11). The failure of location estimating is determined according to the exceeding of the calculated matching error to the error threshold. If the failure of location estimating is determined, the matching error is generated about each location sample extracted from at least one section of an environment map(S16). The matching error is applied to the probability density function and the location of the mobile robot is estimated.

Description

Location estimation method of mobile robot {LOCALIZING METHOD FOR MOBILE ROBOT}

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.

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.

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.

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 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.

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 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.

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.

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.

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.

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.

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.

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.

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.

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

는 시간 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.

In the step (b), the prediction data may be calculated from the environment map through a ray-casting algorithm.

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.

The matching error calculated in step (b) may be filtered by a median filter and reflected in step (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).

(여기서, S_max는 상기 최대 이동 거리이고, σ는 상기 모션 모델의 표준 편차이고, Δs_i는 상기 이동 로봇의 휠 엔코더로부터 측정된 이동 거리이다)에 의해 산출될 수 있다.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.

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.

In addition, 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 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.

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 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 is a control flowchart for explaining a position estimation method of a mobile robot according to the present invention.

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.

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.

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.

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.

[Equation 1]

는 시간 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.

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 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.

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.

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.

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 ㅀ.

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.

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.

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.

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 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.

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].

[Equation 2]

는 (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.

테스트를 이용한 Hypothesis testing에 기초한 슬라이딩 윈도우 기법(Sliding window method)을 적용한 것으로, Hypothesis testing을 간소화시켜 나타내고 있다. 즉, 본 발명에서는 기존의 Hypothesis testing에 기초한 슬라이딩 윈도우 기법(Sliding window method)에서

함수가 갖는 계산의 복잡성을 제거하기 위해 에러 문턱치의 계산에 [수학식 8]과 같이, 슬라이딩 윈도우 기법(Sliding window method)을 간소화시켰다. 여기서, 슬라이딩 윈도우 사이즈 k는 여러 실험으로부터 경험적으로 결정될 수 있으며, 본 발명에서는 슬라이딩 윈도우 사이즈 k를 100초를 기준으로 하여 500으로 설정하는 것을 예로 한다.Where Equation 2 is a Gaussian probability

It applies the sliding window method based on the hypothesis testing using the test, and shows the simplified hypothesis testing. That is, in the present invention, in the sliding window method based on the existing hypothesis testing,

In order to remove the complexity of the calculation, the sliding window method is simplified to calculate the error threshold as shown in [Equation 8]. Here, the sliding window size k may be determined empirically from various experiments. In the present invention, the sliding window size k is set to 500 based on 100 seconds as an example.

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%.

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.

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.

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).

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.

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 ₀ 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 ₁ and the mobile robot moves along a real path.

At this time, the mobile robot recognizes the position estimation failure at time t ₃ . Actually, the difference between the time t ₁ deviating from the reference path and the detected time t ₃ 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.

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 ₃ 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 ₀ to t ₃ in all directions in which the mobile robot can move.

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.

&Quot; (3) "

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.

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.

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.

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.

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).

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 Equation 4 below.

&Quot; (4) "

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.

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).

보다 큰 경우를 나타낸 것으로, 이는 수렴 영역이 전체 확률 분포 영역의 99.7%를 차지하는 것을 의미한다. 그리고, [수학식 4]의 식 (2)는 가우시안 확률 함수를 나타낸다.Equation (3) of Equation 4 shows a matching error.

In this case, the convergence region occupies 99.7% of the total probability distribution region. Equation (2) in [Equation 4] represents a Gaussian probability function.

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.

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.

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).

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).

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 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%.

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.

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.

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.

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.

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.

Here, if it is assumed that the area on the whole environment map is 1539m ² , 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 ² , 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 some embodiments of the invention have been shown and described, it will be apparent to those skilled in the art that modifications may be made to the embodiment without departing from the spirit or spirit of the invention. . The scope of the alias will be defined by the appended claims and their equivalents.

Claims (14)

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 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. 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 method of claim 3,
In 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 the deviation between the scan range region and the prediction range region. The method according to claim 2 or 4,
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. The method of claim 5,
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. The method of claim 5,
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. 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. The method of claim 5,
The matching error calculated in step (b) is filtered by a median filter and reflected in step (c). The method of claim 5,
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. 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. The method of claim 5,
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. 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.

Images