KR101072876B1 - Method and apparatus for estimating position in a mobile robot - Google Patents

Abstract

A method and apparatus for estimating its position in a mobile robot is disclosed. The method for estimating its own position in a mobile robot according to the present invention comprises the steps of: acquiring an image using a camera provided in the robot; Extracting feature points from the image using a SIFT algorithm; Photographing the extracted feature points and the space where the robot exists and comparing feature points on a 3D map constructed with feature points extracted using a SIFT algorithm to detect at least three feature point pairs matching each other; And estimating the position of the robot using the coordinates of the three-dimensional map of each of the detected pairs of feature points and the coordinates of the acquired image. According to the present invention, the position of the robot can be effectively estimated at a low cost by using an image obtained from a camera which is a low-cost vision sensor.

Robot, Position Estimation, SIFT Algorithm, Feature Point

Description

Method and apparatus for estimating position in a mobile robot

The present invention relates to a method and apparatus for estimating its own position in a mobile robot, and more particularly, to a method and apparatus for estimating a position using an image acquired through a camera.

Early robots were used as tools for mass production on industrial sites, performing simple and repetitive tasks in fixed locations. However, with the development of robot technology, robots have more various forms and have higher intelligence and ability to do various tasks.

In order for a robot to carry out more diverse tasks automatically, it must be capable of driving itself. In order to solve the autonomous driving problem of the robot, first, it is necessary to be able to determine where the robot itself is. Especially, even though the robot can remember its initial position and find out the current position from the movement information up to now and the previous position information, if the robot is moved by external force, the movement information and position saved while moving to the previous position You face the problem of no information available.

In order to solve this problem, a range sensor such as a laser sensor or an ultrasonic sensor has been proposed, but it is expensive and expensive, and has a limitation in that more detailed information about the surrounding environment cannot be obtained.

SUMMARY OF THE INVENTION The present invention provides a method and apparatus for estimating a position of a robot in a mobile robot, which can estimate the position of a robot at low cost using an image obtained from a camera, which is a low-cost vision sensor. There is.

In order to solve the technical problem, a method for estimating its position in a mobile robot according to the present invention, (a) obtaining an image using a camera provided in the robot; (b) extracting feature points from the image using a SIFT algorithm; (c) capturing the extracted feature points and the space in which the robot exists in advance and comparing feature points on a 3D map constructed with feature points extracted using a SIFT algorithm to detect at least three feature point pairs that match each other; step; And (d) estimating the position of the robot using the coordinates in the three-dimensional map of each of the detected pairs of feature points, the coordinates in the obtained image, and the focal length of the camera.

Here, in the step (b), a pair of feature points that match or are similar to a descriptor may be detected as a pair of feature points that match each other.

Also, in the step (c), at least three pairs of feature points matched with each other may be detected by comparing the extracted feature points with feature points in a feature point cluster that is an area where feature points are concentrated on the 3D map.

The method may further include repeating steps (a) to (c) after rotating the robot by an angle in place when the pairs of feature points are not detected in step (c). It may further include.

Also, in the step (d), the direction of the robot may be estimated using the estimated position of the robot and the coordinates of the feature points on the 3D map.

In the step (d), the distance on the xy plane between the robot and the feature point on the 3D map is obtained for each of the detected feature point pairs, and the robot position is obtained by using the obtained distance and the coordinates of the 3D map. You can calculate the coordinates of.

In order to solve the above technical problem, the apparatus for estimating its own position in the mobile robot according to the present invention, the feature point extraction unit for extracting the feature points by using the SIFT algorithm from the image obtained using the camera provided in the robot; Feature point pair detection unit for detecting at least three pairs of feature points matched with each other by comparing the extracted feature points and the space in which the robot exists and comparing the feature points on the three-dimensional map constructed by the feature points extracted using a SIFT algorithm. ; And a position estimator for estimating the position of the robot using coordinates in the three-dimensional map of each of the detected feature point pairs, coordinates in the obtained image, and a focal length of the camera.

In order to solve the above another technical problem, there is provided a computer-readable recording medium having recorded thereon a program for executing a method for estimating its position in a mobile robot according to the present invention.

According to the present invention described above, it is possible to effectively estimate the position of the mobile robot at low cost by using an image obtained from a camera which is a low-cost vision sensor.

Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings. In the following description and the accompanying drawings, substantially the same components are denoted by the same reference numerals, and redundant description will be omitted. In addition, in the following description of the present invention, if it is determined that a detailed description of a related known function or configuration may unnecessarily obscure the subject matter of the present invention, the detailed description thereof will be omitted.

1 is a block diagram of an apparatus for estimating its position in a mobile robot according to an embodiment of the present invention. The position estimating apparatus according to the present embodiment is provided in a robot which can move itself, and includes a camera 110, a 3D map storage 120, a feature point extractor 130, a feature point pair detector 140, and a position estimator ( 150, the controller 160.

The camera 110 basically acquires an image of a scene facing the robot in a moving direction, and the direction of the image to be acquired by the camera 110 changes according to the rotation of the robot. However, the direction of the scene to acquire an image may be changed by rotating the camera horizontally about the axis of the robot without rotating the robot.

The 3D map storage unit 120 previously photographs a space in which the robot exists and stores a 3D map constructed of feature points extracted using a scale invariant feature transform (SIFT) algorithm. The SIFT algorithm is a feature point extraction algorithm devised by D.Lowe. The feature points extracted by this algorithm are not affected by the scale, rotation, distortion, and 3D perspective change of the image. This method finds and applies a descriptor to the point of interest by finding invariant feature points in scale space. Since there are descriptors that describe the feature points, maps constructed from feature points extracted using the SIFT algorithm can be used to locate the feature points.

The 3D map can be constructed by extracting the feature points of the wall at regular intervals while the robot is driving in a space such as a corridor, a lobby, or a large room.In the case of a small room, the robot rotates in place or It can be constructed by extracting feature points by acquiring an image at a predetermined angle while rotating the camera provided in the robot horizontally. The 3D absolute coordinates of the feature points are generated using the acquired image and the position information of the robot. Hereinafter, this absolute coordinate will be expressed as (x, y, z).

2 shows an example of a three-dimensional map constructed in accordance with one embodiment of the present invention. The points marked in red represent the feature points, and the line marked in green is the movement path of the robot that traveled while extracting the feature points.

The robot described in the present embodiment may directly extract the feature points to build the 3D map, and first, the other robots extract the feature points to construct the 3D map, and the 3D map storage 120 Can also be stored in In addition, in the present embodiment, the three-dimensional map is described as being stored in the three-dimensional map storage unit 120 provided in the robot, but stored in a separate server, etc. provided separately received from there by using a wireless communication means, if necessary It may be implemented in a manner.

The feature point extractor 130 extracts the feature points from the image acquired using the camera 110 using a SIFT algorithm. Since the current robot assumes a situation where it is not known, the feature point extractor 130 obtains coordinates in the acquired image of the extracted feature points. When coordinates in this image are expressed as (u, v), these coordinates (u, v) generally have a coordinate system whose origin is the upper left corner of the image. However, in the present embodiment, it is assumed that the origin is moved to the center of the image for convenience, and the coordinates of the feature point will be displayed. This conversion can be expressed as the following equation.

Here, u 'and v' mean a coordinate system whose origin is the upper left corner of the image. Therefore, u and v are coordinate systems whose origin is the center of the image. u makes the positive direction right, and v makes the positive direction upward.

The feature point pair detector 140 compares the feature points extracted by the feature point extractor 130 with the feature points on the 3D map stored in the 3D map storage 120 and detects at least three pairs of feature points that match each other. Whether the feature points extracted by the feature point extractor 130 and the feature points on the 3D map match each other may be determined by the sameness of the descriptors. However, it is a principle that a descriptor detects an exact pair of feature points, but a slight error may occur in the descriptor value due to an error in processing, so that the descriptor can detect similar feature points within the allowable range. In other words, feature points extracted by the feature point extractor 130 are compared with each of the feature points on the 3D map to detect feature point pairs that exactly match the descriptors or that the difference between the descriptors is within a predetermined threshold. If more than two pairs of feature points are detected, the three pairs of feature points that are exactly identical or most similar are taken.

However, since feature points extracted from objects present in the room are used without using an artificial recognition mark, similar feature points may occur in separate similar objects. For example, in a building corridor, fire extinguishers are placed at certain distances, which are often the same type. Thus, more than one of the same descriptor can be extracted from them. In this case, confusion arises when the feature points extracted from the feature point extractor 130 are compared with the feature points on the 3D map. This is because two or more feature points on the 3D map matched with one feature point of the acquired image. In other words, the robot is looking at a fire extinguisher and is not sure which fire extinguisher.

In order to solve this problem, the feature point pair detector 140 compares the feature points extracted by the feature point extractor 130 with feature points in a feature cluster, which is a region where feature points are concentrated on a 3D map, to which a descriptor is identical or similar. Can be detected. That is, the descriptor is searched for each feature cluster, which is a region where feature points are aggregated, on the 3D map, instead of searching for matching or similar feature points from the feature points extracted by the feature point extractor 130.

Formation of the feature point cluster is, for example, by setting a window of a certain size, moving the window on a three-dimensional map while the number of feature points belonging to the inside of the window corresponding to the feature point cluster becomes a certain number of points. Can be defined However, the method of forming the feature point cluster may have various modifications. Meanwhile, the formation of the feature point cluster may be performed together with the construction of the 3D map to store information about the cluster in the 3D map storage 120 in advance, and the feature point pair detector 140 may detect the feature point pairs. For example, after reading the 3D map from the 3D map storage unit 120 and analyzing the 3D map, the feature point cluster may be generated and the feature point pairs may be detected.

FIG. 3 is a view of the 3D map shown in FIG. 2 in a direction viewed from above, i.e., projected onto the xy plane, and FIG. 4 is formed at a point of view viewed from the top in the map shown in FIG. It is an enlarged view of a feature point cluster. Referring to FIG. 4, it can be seen that feature points are concentrated in two parts, so that these two regions are clusters of feature points, respectively.

The feature point pair detector 140 compares the feature points extracted by the feature point extractor 130 with the feature points in the feature point cluster formed as described above and detects feature pairs having identical or similar descriptors. By doing so, it is possible to find a feature point on the 3D map that is correctly matched with the feature point extracted from the feature point extractor 130, and improve the accuracy of location recognition.

Referring back to FIG. 1, when three feature point pairs are detected from the feature point pair detector 140, the position estimator 150 may determine the coordinates of each of the feature point pairs and the image obtained from the camera 110. Estimate the position of the robot using the coordinates of. A more detailed process of estimating the position of the robot will be described later. If the three feature point pairs are not detected by the feature point pair detector 140, a signal informing of this is transmitted to the controller 160. The controller 160 is a module that controls the movement and rotation of the robot. When the controller 160 receives the signal, the controller 160 rotates the robot at a certain angle so that the controller 160 faces the other direction. Then, the above-described operations, that is, the camera 110 acquires an image, the feature point extractor 130 extracts feature points from the image, and the feature point pair detector 140 detects matching feature point pairs again. do. This process is repeated until three feature point pairs are detected, and when the three feature point pairs are detected, the position estimator 150 estimates the position of the robot based on the information about the feature point pairs.

The process of estimating the position of the robot with feature point pairs will now be described in detail. This process starts with knowing the absolute coordinates (x, y, z) on the three-dimensional map corresponding to the current three singularity pairs and the coordinates (u, v) in the image coordinate system as described above.

5 is a reference diagram for explaining a process of estimating the position of a robot. Referring to FIG. 5, when the vectors connecting the singular points of the absolute coordinate system and the singular points of the image coordinate system corresponding thereto are drawn, three vectors corresponding to each singular point pair come out, and the intersection of these vectors becomes the position of the robot (camera). . For convenience, in the present embodiment, assuming that the position of the robot does not change in the vertical direction in the space, it is assumed that the z coordinate value of the robot position is 0.

For a singular point whose image coordinate value is (u, v) and the absolute coordinate value on the 3D map is (x _feature , y _feature , z _feature ), the distance on the xy plane between the robot and the feature point on the 3D map is d _xy . If the focal length of the camera 110 is f, the following proportional expression is established by the similarity principle of the triangle.

The above equation is established because the z coordinate value of the robot position is assumed to be zero.

The following equation is established from the above equation (2).

Meanwhile, the absolute coordinates of the three singular points are (x _feature1 , y _feature1 , z _feature1 ), (x _feature2 , y _feature2 , z _feature2 ), and (x _feature3 , y _feature3 , z _feature3 ), respectively. (x _{robot, robot} y) La, and when the distance on the xy plane between each of the singular point on the robot and a three-dimensional map _xy1 d, d _xy2, d _xy3 la each expression following equation is established.

If the third equation is subtracted from the first equation of Equation 4 and the third equation is subtracted from the second equation, the following equation is established.

If Equation 5 is arranged into a determinant, it is as follows.

In Equation 6, values other than the x and y coordinate values of the robot can be known. That is, the absolute coordinate of a feature point is already given value, d _xy1, d _xy2, d _xy3 can be obtained from the equation (3). Therefore, the coordinates of the robot position (x _robot , y _robot ) can be obtained.

Not only the position of the robot but also the direction the camera is facing, i.e., the robot bearing, needs to be obtained. 6 is a view showing a relationship between the direction that the robot looks in the xy plane and the direction toward the singularity. Referring to FIG. 6, θ ₁ denotes an angle between a straight line connecting a robot and a feature point on a 3D map and an x axis, which is a reference axis, and θ ₂ denotes an angle between the straight line and a direction viewed by the robot (camera). it means. The larger image coordinate u means that the singularity moves in the clockwise direction, which is opposite to the counterclockwise direction of the angle measurement. The direction that the robot is facing is θ ₁ and θ ₂ , and θ ₁ and θ ₂ can be obtained using the following equation.

Here, f is the focal length of the camera 110.

As described above, the position of the robot and the direction in which the robot is directed can be obtained from the coordinate values of the singular points in the image acquired by the camera 110 and the absolute coordinate values on the three-dimensional map corresponding thereto.

7 is a flowchart illustrating a method of estimating its own position in a robot according to an embodiment of the present invention. The position estimation method according to the present embodiment consists of steps processed in the position estimation apparatus described above. Therefore, even if omitted below, the above description of the position estimation apparatus is also applied to the position estimation method according to the present embodiment.

An image is acquired using a camera provided in the robot (step 610), and feature points are extracted from the image by using a SIFT algorithm (step 620).

In operation 630, at least three pairs of feature points matched with each other are detected by comparing the extracted feature points with the feature points on the previously constructed 3D map. At this time, if at least three pairs of feature points are not detected (step 640), the robot is rotated by a predetermined angle (step 650), and then steps 610 to 640 are repeated.

In operation 630, it may be determined whether the descriptors match or are similar to each other between the photographed image and the feature points on the 3D map. In comparing the feature points extracted from the captured image with the feature points on the 3D map, it is preferable to check whether at least three feature point pairs are detected by comparing the feature points in the feature point cluster defined on the 3D map.

When three pairs of feature points are detected, the current position of the robot is estimated using the coordinates in the 3D map of each of the feature point pairs and the coordinates in the image acquired in step 610 (step 660).

Meanwhile, the above-described embodiments of the present invention can be written as a program that can be executed in a computer, and can be implemented in a general-purpose digital computer that operates the program using a computer-readable recording medium. The computer-readable recording medium may be a magnetic storage medium (for example, a ROM, a floppy disk, a hard disk, etc.), an optical reading medium (for example, a CD-ROM, DVD, etc.) and a carrier wave (for example, the Internet). Storage medium).

So far I looked at the center of the preferred embodiment for the present invention. Those skilled in the art will appreciate that the present invention can be implemented in a modified form without departing from the essential features of the present invention. Therefore, the disclosed embodiments should be considered in an illustrative rather than a restrictive sense. The scope of the present invention is shown in the appended claims rather than the foregoing description, and all differences within the scope will be construed as being included in the present invention.

1 is a block diagram of an apparatus for estimating its position in a mobile robot according to an embodiment of the present invention.

2 shows an example of a three-dimensional map constructed in accordance with one embodiment of the present invention.

FIG. 3 illustrates a map in which the three-dimensional map illustrated in FIG. 2 is projected in a direction viewed from above, that is, on an xy plane.

FIG. 4 is an enlarged view of a feature point cluster formed at a point of view of an upper direction in the map illustrated in FIG. 3.

5 is a reference diagram for explaining a process of estimating the position of a robot.

6 is a view showing a relationship between the direction that the robot looks in the xy plane and the direction toward the singularity.

7 is a flowchart of a method for estimating its position in a mobile robot according to an embodiment of the present invention.

Claims (13)

In the method for estimating its position in a mobile robot, (a) acquiring an image using a camera provided in the robot; (b) extracting feature points from the image using a SIFT algorithm; (c) capturing the extracted feature points and the space in which the robot exists in advance and comparing feature points on a 3D map constructed with feature points extracted using a SIFT algorithm to detect at least three feature point pairs that match each other; step; And (d) estimating the position of the robot using the coordinates in the three-dimensional map of each of the detected pairs of feature points, the coordinates in the obtained image, and the focal length of the camera; In the step (c), the extracted feature points and the feature points in the feature point cluster, which are areas where feature points are concentrated on the 3D map, are compared to detect at least three feature point pairs matching each other. . The method of claim 1, In the step (b), a feature point pair having a descriptor that matches or is similar to a descriptor is detected as a feature point pair that matches each other. delete The method of claim 1, The method may further include repeating steps (a) to (c) after rotating the robot at an angle in the case where the pair of feature points are not detected in the step (c). Estimation method. The method of claim 1, In the step (d), using the estimated position of the robot and the coordinates of the feature points on the three-dimensional map to estimate the direction of the robot, characterized in that the estimation. The method of claim 1, In the step (d), the distance on the xy plane between the robot and the feature point on the 3D map is obtained for each of the detected feature point pairs, and the coordinates of the robot location are obtained by using the obtained distance and the coordinates on the 3D map. Position estimation method, characterized in that for calculating. A computer-readable recording medium having recorded thereon a program for executing a method for estimating its position in a mobile robot according to any one of claims 1, 2, and 4-6. In the device for estimating its position in a mobile robot, A feature point extraction unit for extracting feature points from an image acquired using a camera provided in the robot using a SIFT algorithm; Feature point pair detection unit for detecting at least three pairs of feature points matched with each other by comparing the extracted feature points and the space in which the robot exists and comparing the feature points on the three-dimensional map constructed by the feature points extracted using a SIFT algorithm. ; And And a position estimator for estimating the position of the robot using coordinates in the three-dimensional map of each of the detected feature point pairs, coordinates in the obtained image, and a focal length of the camera. And the feature point pair detector detects at least three pairs of feature points that match each other by comparing the extracted feature points and feature points in a feature point cluster that is a region where feature points are concentrated on the 3D map. The method of claim 8, And the feature point pair detector detects feature point pairs that match or are similar to each other as feature point pairs that match each other. delete The method of claim 8, The controller may further include a controller configured to rotate the robot at a predetermined angle when the feature pair pair is not detected by the feature point pair detector. The feature point extractor and the feature point pair detector are repeatedly rotated by the robot. And extracting the feature point pairs. The method of claim 8, And the position estimating unit estimates a direction that the robot faces using the estimated position of the robot and coordinates of the feature points on the 3D map. The method of claim 8, The position estimating unit obtains the distance on the xy plane between the robot and the feature point on the 3D map for each of the detected pairs of feature points, and calculates the coordinates of the robot position using the obtained distance and the coordinates on the 3D map. Position estimating apparatus, characterized in that.

Images