KR100757937B1 - Simultaneous localization and map building method for robot - Google Patents

Abstract

A method for localizing a robot and making a map is provided efficiently to track the position of the robot and make the map at the same time by reducing calculation related to tracking the position of the robot and making the map with a CEKF, and enhancing discrimination ability of the feature point of a stereo image and a mark in a 3D(Dimensional) restored image even if the robot is rotated. The stereo image for a front side of the driving robot is captured(S100). The feature point is extracted from the captured stereo image(S101). A position of the extracted feature point and the mark on space is extracted by restoring the stereo image into a 3D image(S103). The position of the feature point and the robot is updated by periodically calculating the position of the extracted feature point with the CEKF(S109). The map is made by periodically updating the mark with the CEKF(S116). The position of the mark is updated by using an additional matrix calculated when the position of the feature point and the robot is updated.

Description

{Simultaneous localization and map building method for robot}

1 is a conceptual diagram of a general SLAM.

2 is a conceptual diagram of a general CEKF SLAM.

3 is a control flowchart of a method for tracking and mapping a robot according to the present invention.

FIG. 4 is a diagram for describing stereo image photographing by the traveling robot of FIG. 3.

FIG. 5 is a diagram illustrating a 3D reconstructed image of the stereo image of FIG. 3.

FIG. 6 is a diagram for describing a position X _B of a mark in space of the 3D reconstructed image of FIG. 4.

FIG. 7 shows the markers X _B on the space of FIG. 5.

FIG. 8 is a diagram for describing a feature point x _A and a mark X _B in space in the stereo image of FIG. 3.

* Description of the symbols for the main functions of the drawings *

10: obstacle 20: driving robot

21: stereo camera

The present invention relates to a method of localization and map building of a robot, and more particularly, to a method of simultaneously performing location tracking and mapping of a robot (SAM).

For navigation of robots moving in an obstacle environment, it is essential to track their location and map the environment.

This is because the created map can be used to perform tasks such as planning the route of the robot, manipulating objects, or communicating with people.

In order to navigate in an unknown environment, a robot must map while tracking its location. However, it is difficult to calculate since the location and map are made using the noisy infrared sensor data.

Position tracking refers to identifying the absolute position of the robot in the working environment using various sensor information and natural landmarks. Since the error occurs due to various causes (wheel and ground slip, change of wheel diameter, etc.) while the robot is running, an error occurs at the position of the robot as the driving progresses, and correction is necessary accordingly.

Mapping is modeling the environment by observing natural or artificial markers based on sensor data, through which the robot sets its own path. In order to model a complex environment, location tracking must be guaranteed to create a reliable map. Therefore, there is a need for a method that can simultaneously perform the problem of mapping and location tracking in a short time.

The present invention is to solve the above-described problems, an object of the present invention is to apply a compression extended Kalman filter (CEKF) to the stereo image to track the location of the robot can be performed more efficiently at the same time to map the robot location tracking And to provide a mapping method.

In order to achieve the above object, a method of tracking and mapping a robot according to an embodiment of the present invention includes capturing a stereo image in front of a traveling robot, extracting feature points from the captured stereo image, and three-dimensionally capturing the captured stereo image. Restoring and extracting the position of the extracted feature point and the position of the marker on the space, and calculating the position of the extracted feature point every week using an extended Kalman filter to update the position of the feature point and the position of the robot; And updating the extracted mark on the extracted space using the Extended Kalman Filter at regular intervals to create a map.

      The position of the mark on the space is updated by using the additional matrix calculated at the time of updating the position of the feature point and the position of the robot.

      The mark on the space is an object of a certain height or more in the three-dimensional reconstructed image, the position value is characterized in that the cross-section at that height is the two-dimensional coordinate value of the elliptical fitting center point.

      The position value of the feature point may be a 3D coordinate value of the feature point of the 3D reconstructed image.

Hereinafter, preferred embodiments of the present invention will be described in detail with reference to the drawings.

1 is a conceptual diagram of a general SLAM. As shown in FIG. 1, the position value Z _{K -1} of the mark 1 measured by the traveling robot 2 at the X _{K -1} position has a measurement error. In addition, when the driving robot 2 travels to the X _K position by the driving input U _K at X _{K - 1} , the measured value Z _K of the peripheral marker also has an error at this position. These errors gradually accumulate and eventually cannot accurately estimate the position of the traveling robot 2 and the position of the marker 1. In order to prevent this, the proposed SLAM (Simultaneous Localization And Map-building; SLAM) is a technique for simultaneously correcting the position of the traveling robot 2 and the marker 1 using a probabilistic method when there is no prior knowledge.

The SLAM consists of estimating the position of the traveling robot 2 using the traveling input of the traveling robot 2 and correcting the positions of the traveling robot 2 and the marker 1 using the sensor measurement values. Since the speed input of the traveling robot 2 and the measured value of the sensor have an error, the position is estimated using a probabilistic approach. The most used method is EKF. However, in the case of EKF, when the size of the state vector increases, the computational amount increases drastically, and real-time calculation is impossible.

To compensate for this disadvantage, the Compressed Extended Kalman Filter (CEKF) has been proposed. The CEKF separates the state vector (X) into two groups, and differentially applies the Extended Kalman Filter (EKF) to the critical group (X _A ) and the less important group (X _B ) to improve the accuracy of the EKF. It is a way to reduce the amount of computation while maintaining.

The CEKF SLAM is a way to reduce the computation by separating the two groups (X _A and X _B ) from the markers when the robot moves for a long time in a certain area.

CEKF separates the state vector (X) into two groups, updates the X _A and the error covariance matrix (P _AA ) (error of X _A ) by applying the EKF every week to the important group (X _A ), and adds the additional matrix ( Calculate Auxiliary Matrix. And the low priority group (X _B) and the error covariance matrix of X _B (P _AB, P _BB) (error of X _B) for the state a predetermined period calculated by the addition matrix for each, and the whole application the EKF vector (X ), The error covariance matrix (P) is updated.

As shown in FIG. 2, the CEKF SLAM regards the mark existing in the area as an important group (X _A ) when the traveling robot 2 works for a long time in a certain area, and applies the EKF every cycle, The mark of the other regions is regarded as a group of low importance (X _B ), and the EKF is applied to update the overall mark when the traveling robot 2 is out of a certain region.

As mentioned above, EKF SLAM has a disadvantage in that real-time calculation is impossible when the size of the state vector increases. In order to make up for this shortcoming, the proposed CEKF SLAM categorizes X _A as a marker that the surrounding markers are repeatedly measured when working for a long time in a certain area. Classify the marker as X _B. The periodic update is performed for X _A of the classified state vectors, and the update amount is reduced when X _B is out of the range, thereby reducing the amount of computation. Therefore, the CEKF SLAM has no disadvantage in reducing the computation time and complicates the calculation when the traveling robot does not work for a long time in a certain area.

Since the proposed CEKF SLAM uses a laser distance sensor, the surrounding markers are recognized in two dimensions. Therefore, the CEKF SLAM can be applied only to robots moving in a plane. This has the disadvantage of having to work for a long time.

In addition, the conventionally proposed image-based SLAM is difficult to distinguish feature points, so when the driving robot rotates, it is difficult to observe the feature points used in the SLAM again.

Therefore, in the present invention, it is difficult to distinguish the feature points when using only the image and the disadvantage of having to work for a long time in a certain area of the existing CEKF SLAM, and to solve the disadvantages of difficult to observe the feature points used in the SLAM.

CEKF SLAM based on stereo vision according to the present invention is a real-time SLAM algorithm that supplements the disadvantages of existing CEKF SLAM and image-based SLAM, and can be applied to outdoor environment exploration robots, office service robots, and art gallery tour assistant robots.

      The present invention includes the following components.

First, the method of applying a CEKF SLAM using a stereo image, and then capturing a stereo image by the three-dimensional topography to recognize and distinguish between more than a certain height object to the landmark X _B in space, separated by a feature point x _A stereo image Apply CEKF SLAM.

Second, the feature point of the stereo image itself is obtained and the position value of x _A is defined from the three-dimensional position value of the point.

Third, three-dimensional reconstruction from a stereo image is performed to classify objects having a predetermined height or more by the marker X _B in space, and the position value of X _B is defined as its center point (x, y) by elliptical fitting a cross section at the height.

Fourth, the three-dimensional coordinates of the feature points obtained from the stereo images and defined by _A x, _A x tracks.

Fifth, the overall map is created using only the marker X _{B in} space.

Referring to FIG. 3, the operation of the present invention will be described. First, in operation S100, two images, that is, stereo images are received from a stereo camera and captured. In this case, as shown in FIG. 4, the traveling robot 20 equipped with two stereo cameras 21 installed on both front sides of the vehicle captures and captures a stereo image on the front road.

The feature point x _A is extracted from the stereo image captured in step S101. In step S102 after the feature point x _A extraction, the captured stereo image is three-dimensionally reconstructed.

In this case, as shown in FIG. 5, the captured stereo image is 3D reconstructed by terrain recognition modeling.

Then, in step S103, the position of the extracted feature point x _A and the position X _B of the mark in space are extracted from the 3D reconstructed image. 6 to 8, the position X _B of the marker in the extracted space is an elliptical fitting of a cross section at a certain height to an object 10 having a predetermined height or more in the 3D reconstructed image. It is defined as a two-dimensional coordinate value (x, y). 7 shows an image of the markers X _{B in} the space described above. The position of the feature point (x _A ) is defined as a three-dimensional coordinate value as a coordinate value for the feature position such as an edge of the mark (X _B ) in space.

In step S104, it is determined whether the number of feature points x _A exceeds two. If, when the determination result the number of feature points (x _A) in the step S104 up to two, in step S105 and step S106, the position of the marker in the position and space of the robot by using the EKF from the movement command of the robot (X _B) Is predicted, the position of the predicted robot and the position X _B of the mark in space are updated using X _B extracted in step S103, and then returned to step S100.

On the other hand, if the number of feature points x _A in the determination result in step S104 exceeds two, in step S107, the number of n times, that is, the number of times of EKF application is set to 0, and in steps S108 and S109, the EKF is used. the predicted to x _a and the error covariance matrix P _AA including the location of the position and the feature point x _a of the robot from the movement command of the robot, and the predicted x _a and the error covariance matrix P _AA, extracted in step S103, a feature point x _a Update using the position of. In operation S110, an additional matrix is calculated using the updated X _A and the error covariance matrix P _AA .

In step S111, it is determined whether the number of n exceeds 20 times or the number of feature points x _A is less than three. If, as a result of the determination in step S111, the number of times n is 20 or less, or the number of feature points x _A is three or more, in step S112, a stereo image is captured, and in step S113, the number of n sets n + 1 circuits. In step S114, the feature point x _A is tracked by the Kanade-Lucas tracking method in the captured image, and then steps S108 and below are performed.

On the other hand, if the number of times n is greater than 20 times or the number of feature points x _A is less than 3 times as a result of the determination in step S111, the position (X) of the mark in space using the additional matrix calculated in step S118 in step S115. _B ), the error covariance matrices P _AB and P _BB are updated. Then, in step S116, a map is created using the position X _B of the mark on the space updated in step S115, and then the process returns to step S100.

That is, after extracting the three-dimensional coordinate values of the feature point x _A using the three-dimensional reconstruction result of the input stereo image, if the number of x _A is two or less, the EKF is applied to the position and space of the robot from the movement command of the robot. The position X _{B of} the marker is updated using the position of the marker in the predicted and extracted space. A feature point (x _A), if the number is three or more, X _A, including the position of the point and the characteristic point x _A of the robot by applying the EKF the feature point x _A of the stereo image is obtained every period from the movement command of the robot and an error covariance matrix of the P _AA is updated by using the positions of the predicted and extracted feature points x _A , and an additional matrix is calculated. The stereo image is recaptured and the position of the feature point x _A is tracked by the Kanade-Lucas tracking method in the recaptured stereo image. If the number of x _A is 2 or less, or the number of EKFs is 20 or more, update the position (X _B ) and covariance matrix (P _AB , P _BB ) of the marker in space by using an additional matrix, Create a map using the location of the marker (X _B ).

Therefore, since the stereo vision-based CEKF SLAM according to the present invention can measure the three-dimensional coordinates of the feature point of the stereo vision, it is possible to obtain not only the traveling robot moving on the plane but also the position and the map of the robot running on the curved surface. have.

In addition, the calculation time that is most problematic in performing SLAM can be solved by applying CEKF. In addition, the markers used in CEKF can be divided into two types of markers in the stereo image and two markers in the space, and can be calculated in real time. Robust to error. In addition, the limitation of the work area generated when the CEKF is applied can be solved by using the image repeatedly measured as the robot moves.

As described in detail above, the present invention reduces the calculation amount related to the location tracking and mapping of the robot using CEKF, and distinguishes the feature points even if the robot rotates using the feature points of the stereo image and the markers in the 3D reconstructed image. By improving the ability, the robot can track the location and map the map more efficiently.

Claims (4)

Capturing a stereo image in front of the traveling robot, Extracting feature points from the captured stereo image; 3D reconstructing the captured stereo image to extract the location of the extracted feature point and the location of the marker in space; Updating the position of the feature point and the position of the robot by calculating the position of the feature point every cycle using an extended Kalman filter; A method of estimating and mapping a robot, comprising: generating a map by updating the extracted mark on a space by using an extended Kalman filter at regular intervals. The method of claim 1, wherein the position of the mark on the space is updated by using the additional matrix calculated when the position of the feature point and the position of the robot are updated. The robot according to claim 1, wherein the mark on the space targets an object of a predetermined height or more in the 3D reconstructed image, and the position value is a two-dimensional coordinate value of an elliptic-fitted center point at a cross section at the height. Location estimation and mapping. The method of claim 1, wherein the position value of the feature point is a 3D coordinate value of the feature point of the 3D reconstructed image.

Images