KR20180016834A - Method and apparatus for estimation of rotation in 3 dimensional space - Google Patents

Abstract

Provided is a method for estimating a rotational movement value used for position estimation in a three-dimensional space. According to an embodiment of the present invention, a method for estimating a rotational movement value used for position estimation in a three-dimensional space estimates a rotation movement value used for position estimation between a first point and a second point when a robot moves to the second point from the first point on a three-dimensional space, and the method comprises the steps of: detecting at least one first point plane and at least one second point plane at each of the first point and the second point on the basis of surrounding depth value information which is a result obtained by measuring a depth value using a distance sensor at each of the two points; sorting at least one plane feature point pair which is to be matched by extracting plane feature points including a distance between a plane and the distance sensor and normal line vector information of the plane from each of the at least one first point plane and the at least one second point plane; and calculating a rotation movement correction value corrected from an uncertain rotation movement value between the two points using an IMU rotation measured value measured selectively by an inertial sensor (IMU) according to the number of the sorted plane feature point pairs.

Description

[0001] The present invention relates to a method and an apparatus for estimating a rotational movement value used for position estimation in a three-dimensional space,

The present invention relates to a method and apparatus for locating a robot in a three-dimensional space, and more particularly, to a method and apparatus for estimating a rotational movement value of a robot moving between two points on a three-dimensional space.

In recent years, demand for robots has expanded to general households, and everyday life has been closely related with each other. In the 3D environment, it is essential that the robot grasp its position in order to perform a given task. Therefore, researches are being made on the technology that robots actively recognize position and create 3D map without prior knowledge.

Since the GPS can not be used in the indoor space, the robot analyzes the raw environment information inputted from the sensor to extract the characteristic points, and then matches the characteristic points on the continuous discrete time, ). ≪ / RTI > Robot positions estimated with high accuracy are applied to 3D mapping technology as well, so that quality maps with small errors can be created.

In order to estimate the position of the robot, there are various feature points that can be extracted and utilized from the image information and the distance information. However, the plane feature points are geometric basic elements abundant in the interior space, Research using this is actively under way.

However, the conventional technology uses a plane feature point in an actual three-dimensional environment to estimate the position of the robot, and the effective feature point (for example, an obstacle due to an object) or a constraint It can not collect enough. In this case, it is impossible to grasp the exact position of the robot continuously.

Although it is possible to estimate the position of a robot through preparatory steps such as artificially attaching artifacts in a real environment or a low-cost system in which some constraints exist, it is possible to damage the appearance of the environment, Time and effort are needed for Therefore, there is a need for a robust algorithm capable of estimating the position of a robot in a three-dimensional space without such a preparation step.

In particular, in order to estimate the position of the robot, both the parallel movement information and the rotation movement information of the robot on the three-dimensional space are required. However, in the case where the available feature points are not sufficient, the method of calculating the parallel movement information between the two points of movement of the robot is already well known, whereas the method of calculating the rotation movement information does not show a clear result, Is emerging.

A related prior art is Korean Patent Publication No. 10-2011-0109177 entitled " Driving Control Method of Autonomous Mobile Robot, Disclosure Date: May 3, 2013].

The present invention provides a method and apparatus for estimating a rotational movement value of a robot moving between two points on a three-dimensional space by selectively using an inertial sensor (IMU).

It is another object of the present invention to provide a method and an apparatus for producing a movement trajectory map of a robot by repeatedly calculating a parallel movement value and a rotation movement value of a robot moving in a three-dimensional space.

In order to achieve the above object, the present invention provides a rotational movement value estimation method used for position estimation in a three-dimensional space, wherein when a robot moves from a first point to a second point on a three-dimensional space, A method for estimating a rotational movement value used for position estimation, the method comprising the steps of: calculating, based on peripheral depth value information obtained as a result of measuring a depth value using a distance sensor at each of the two points, Detecting at least one first point plane and at least one second point plane in each; A plane feature point comprising a distance between the plane and the distance sensor and normal vector information of the plane is extracted from each of the at least one first point plane and the at least one second point plane to form at least one Selecting plane feature point pairs; And calculating a rotation movement correction value for correcting the indefinite rotation movement value between the two points using the IMU rotation measurement value selectively measured by the inertial sensor (IMU) according to the number of the selected at least one plane feature point pair .

Preferably, the step of calculating the rotation movement correction value for correcting the uncertain rotational movement value may include calculating the rotation movement correction value by calculating the rotation movement correction value without calculating the number of pairs of plane feature points if the number of pairs of the plane feature points is two or more, 1, the rotation movement correction value may be calculated based on the uncertain rotational movement value between the two points, the normal vector included in the pair of plane feature points, and the IMU rotation measurement value.

Preferably, the rotation movement correction value may be calculated using Equation (1).

[Equation 1]

는 상기 회전이동보정값이고, r은 상기 제1 지점에 대응되는 시점(時點)이고, l은 상기 제2 지점에 대응되는 시점이고,

는 상기 IMU회전측정값을 상기 로봇이 실제로 회전한 최종회전이동값으로 환산하는 회전 정보이고,

는 쿼터니언 곱이고,

는 상기 r시점부터 상기 l시점까지의 IMU회전측정값이고,

는 상기 r시점부터 상기 l시점까지의 불확정회전이동값의 역회전 정보이다.here,

R is the rotation movement correction value, r is a time point corresponding to the first point, l is a time point corresponding to the second point,

Is rotation information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from the r point to the l point,

Is the inverse rotation information of the uncertain rotational movement value from the r point to the l point of time.

The method may further include calculating a final rotational movement value between the two points, based on at least one of the at least one plane feature point pair and the selectively calculated rotational movement correction value.

Preferably, a plurality of pieces of peripheral depth value information measured at each of a plurality of points on the three-dimensional space and a plurality of robot movement information composed of a final rotational movement value and a parallel movement value between two adjacent points among the plurality of points And generating a map of the three-dimensional space, wherein a translation value between the two points is calculated based on the selected pair of plane feature points and an IMU movement measurement measured by the inertial sensor .

Advantageously, the step of detecting the at least one first point plane and the at least one second point plane comprises using the distance sensor at each of the first point and the second point, 2 surrounding depth value information; Collecting first distance information and second distance information for a plurality of points included in each of the first peripheral depth value information and the second peripheral depth value information; And detecting the at least one first point plane based on the first distance information and detecting the at least one second point plane based on the second distance information.

Advantageously, selecting said at least one planar feature point pair comprises: calculating said planar feature points for each of said at least one first point plane and said at least one second point plane; And at least one plane that is an object of matching between the first point plane and the second point plane based on the normal vector information included in each of the plane feature points and the singular value calculated through singular value decomposition (SVD) And selecting the feature point pair.

Advantageously, the step of selecting at least one plane feature point pair to be matched between the first point plane and the second point plane comprises selecting the at least one first point plane and the at least one second point plane Generating a pseudoinverse of covariance based on a normal vector included in the plane feature points of the covariance; And selecting a pair of planar feature points to be matched based on a ratio between a plurality of singular values calculated by singular value decomposition (SVD) of the pseudoinverse matrix.

In order to achieve the above object, a rotation movement value estimating apparatus used in position estimation in a three-dimensional space provided by the present invention is characterized in that when the robot moves from a first point to a second point on a three-dimensional space, An apparatus for estimating a rotational movement value used for estimating a position between points, the apparatus comprising: means for calculating, based on peripheral depth value information obtained by measuring a depth value using a distance sensor at each of the two points, A detector for detecting at least one first point plane and at least one second point plane at each of the two points; A plane feature point comprising a distance between the plane and the distance sensor and normal vector information of the plane is extracted from each of the at least one first point plane and the at least one second point plane to form at least one A selector for selecting pairs of plane feature points; And an arithmetic unit for calculating a rotation movement correction value for correcting the indefinite rotation movement value between the two points using the IMU rotation measurement value selectively measured by the inertia sensor according to the number of the selected at least one plane feature point pair .

Preferably, the calculating unit does not calculate the rotation movement correction value if the number of the pairs of plane feature points is two or more, and if the number of pairs of plane feature points is one, The rotation motion correction value may be calculated based on the normal vector included in the pair of plane feature points and the IMU rotation measurement value.

Preferably, the rotation movement correction value may be calculated using Equation (1).

[Equation 1]

는 상기 회전이동보정값이고, r은 상기 제1 지점에 대응되는 시점(時點)이고, l은 상기 제2 지점에 대응되는 시점이고,

는 상기 IMU회전측정값을 상기 로봇이 실제로 회전한 최종회전이동값으로 환산하는 회전 정보이고,

는 쿼터니언 곱이고,

는 상기 r시점부터 상기 l시점까지의 IMU회전측정값이고,

는 상기 r시점부터 상기 l시점까지의 불확정회전이동값의 역회전 정보이다.here,

R is the rotation movement correction value, r is a time point corresponding to the first point, l is a time point corresponding to the second point,

Is rotation information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from the r point to the l point,

Is the inverse rotation information of the uncertain rotational movement value from the r point to the l point of time.

Advantageously, said calculating unit may further calculate a final rotational movement value between said two points based on at least one of said at least one planar feature point pair and said selectively calculated rotational motion compensation value.

Preferably, the calculating unit is configured by a final rotational movement value and a parallel movement value between two adjacent points of the plurality of points, based on a plurality of peripheral depth value information measured at each of a plurality of points on the three-dimensional space Further comprising a map generation unit for generating a map of the three-dimensional space on the basis of the plurality of robot movement information when calculating a plurality of robot movement information, wherein the parallel movement value between the two points is determined based on the selected plane A feature point pair, and an IMU movement measurement measured by the inertial sensor.

Preferably, the selector may calculate the plane feature points for each of the at least one first point plane and the at least one second point plane, and calculate the normal vector information and singular value decomposition included in each of the plane feature points The at least one plane feature point pair to be matched between the first point plane and the second point plane may be selected based on the singular value calculated through the first point plane and the second point plane.

Advantageously, the selector is configured to select, when selecting at least one plane feature point pair to be matched between the first point plane and the second point plane, the at least one first point plane and the at least one second A pseudo inverse matrix of covariance is generated based on a normal vector included in plane feature points of each of the point planes, and based on a ratio between a plurality of singular values calculated by singular value decomposition of the pseudo inverse matrix, a pair of plane feature points Can be selected.

In the case where the rotational movement value indicating the degree of rotation of the robot while moving between two points has an infinite solution, the present invention selectively corrects the rotational measurement value measured by the inertial sensor mounted on the robot, It is possible to estimate the solution of the movement.

FIG. 1 is a flowchart for explaining a rotation movement value estimation method used for position estimation in a three-dimensional space according to an embodiment of the present invention.
2 is a flow chart illustrating a method of detecting at least one first point plane and at least one second point plane in accordance with an embodiment of the present invention.
FIG. 3 is a view for explaining a rotation movement correction value according to an embodiment of the present invention.
4 is an algorithm for determining the number of pairs of planar feature points to be matched between the first point plane and the second point plane according to an embodiment of the present invention.
5 is a view for explaining a matching relationship between a first point plane and a second point plane according to the number of pairs of plane feature points according to an embodiment of the present invention.
FIG. 6 is a diagram for explaining a map of a three-dimensional space produced using the movement trajectory of the robot according to an embodiment of the present invention.
7 is a view for explaining a rotation movement value estimating apparatus used for position estimation in a three-dimensional space according to an embodiment of the present invention.
8 is a view for explaining feature points of a plane according to an embodiment of the present invention.

While the invention is susceptible to various modifications and alternative forms, specific embodiments thereof are shown by way of example in the drawings and will herein be described in detail. It should be understood, however, that the invention is not intended to be limited to the particular embodiments, but includes all modifications, equivalents, and alternatives falling within the spirit and scope of the invention. Like reference numerals are used for like elements in describing each drawing.

The terms first, second, A, B, etc. may be used to describe various elements, but the elements should not be limited by the terms. The terms are used only for the purpose of distinguishing one component from another. For example, without departing from the scope of the present invention, the first component may be referred to as a second component, and similarly, the second component may also be referred to as a first component. And / or < / RTI > includes any combination of a plurality of related listed items or any of a plurality of related listed items.

It is to be understood that when an element is referred to as being "connected" or "connected" to another element, it may be directly connected or connected to the other element, . On the other hand, when an element is referred to as being "directly connected" or "directly connected" to another element, it should be understood that there are no other elements in between.

The terminology used in this application is used only to describe a specific embodiment and is not intended to limit the invention. The singular expressions include plural expressions unless the context clearly dictates otherwise. In the present application, the terms "comprises" or "having" and the like are used to specify that there is a feature, a number, a step, an operation, an element, a component or a combination thereof described in the specification, But do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, or combinations thereof.

Unless defined otherwise, all terms used herein, including technical or scientific terms, have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. Terms such as those defined in commonly used dictionaries are to be interpreted as having a meaning consistent with the contextual meaning of the related art and are to be interpreted as either ideal or overly formal in the sense of the present application Do not.

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

FIG. 1 is a flowchart for explaining a rotation movement value estimation method used for position estimation in a three-dimensional space according to an embodiment of the present invention.

The rotational movement value used for position estimation in the three-dimensional space can be used to estimate the relative position between the two points as the robot moves from the first point to the second point on the three-dimensional space. At this time, the relative position is determined not by considering only the parallel movement, but also considering the result of the rotational movement. That is, it can be interpreted that there is a difference in the relative position even when only the rotational movement occurs at the point of the same position between the first point and the second point.

On the other hand, a rotational movement value estimating apparatus for performing a rotational movement estimating method can be mounted on a robot moving in a three-dimensional space. In this specification, it is assumed that a rotational movement estimating apparatus is mounted on a robot. However, it should be understood that the rotational movement value estimating device does not necessarily have to be mounted on the robot, but can be mounted on, for example, a PC, a notebook, a smart phone and a tablet connected through a wired / .

Here, the robot may be an electrical and mechanical device that processes a given task by its own capabilities. More specifically, it may be a device capable of self-moving the three-dimensional space by self-judgment or external control or the like.

In step S110, when the robot moves from the first point to the second point on the three-dimensional space, the rotational movement value estimating apparatus estimates the depth value of each of the two points using the distance sensor At least one first point plane and at least one second point plane at each of the first point and the second point.

The distance sensor can be mounted on a robot as a sensor for obtaining the shape or position of a three-dimensional object or environment. More specifically, the distance sensor can measure a depth value using a time propagation method for measuring a distance from a propagation time from an observation point to an object, a distance measurement method using an optical triangulation, and the like. And Ranging) or a Kinect sensor or the like.

For example, when the robot moves between a first point and a second point adjacent to each other in an unknown three-dimensional space (e.g., inside a building), the rotational movement value estimating apparatus controls the distance sensors at each of the first point and the second point It is possible to generate the surrounding depth value information as a result of measuring the depth value of the surrounding environment. Then, using the surrounding depth value information, at least one first point plane is obtained from the peripheral depth value information corresponding to the first point, and at least one second point plane is calculated from the peripheral depth value information corresponding to the second point Can be detected.

The first point plane represents the plane detected from the depth value information measured by the distance sensor at the first point and the second point plane represents the plane detected from the depth value information measured by the distance sensor at the second point .

On the other hand, a specific method of detecting at least one first point plane and at least one second point plane will be described later in detail with reference to Fig. 2.

In step S120, the rotational movement value estimating device extracts plane feature points, which are composed of the distance between the plane and the distance sensor and the normal vector information of the plane, from each of the at least one first point plane and the at least one second point plane, At least one pair of plane feature points to be matched is selected.

Referring to Fig. 8, the rotational movement value estimating apparatus estimates, from each of the detected at least one first point plane and at least one second point plane, the distance d between the plane and the distance sensor, the normal vector n of the plane, And the plane feature points can be extracted. Then, the rotational movement value estimating device can select at least one pair of plane feature points to be matched with each other from the plane feature points of each of the at least one first point plane and each of the at least one second point plane plane .

At this time, as the angle formed between each normal vector (n) of at least one first point plane is closer to 90 degrees, the rotational movement value estimating apparatus can select more significant pairs of plane feature points. For example, when all three normal vectors (n) of the three first point planes are orthogonal to each other, the rotational movement value estimating apparatus can select the most significant plane feature point pair.

The more the selected pairs of plane feature points are selected, the more accurately the rotational movement value can be calculated, which will be described later with reference to FIG. On the other hand, as for the at least one second point plane, as the angle formed between the normal vector (n) is closer to 90 degrees, the rotational movement value estimating apparatus can select more significant pairs of plane feature points.

Also, for example, the pair of plane feature points to be matched with each other can be determined according to whether the first pair of the first point plane is parallelly or rotationally moved, and then overlaps the at least one second point plane and completely overlaps the at least one second point plane .

That is, referring to FIG. 5, it can be seen that at least one plane A is aligned with at least one plane B after being translated or rotated. In this case, the plane feature points of each of the first point plane and the second point plane to be matched form a pair of plane feature points. Therefore, there are three plane feature point pairs in Fig. 5 (a), two plane feature point pairs exist in (b), and one plane feature point pair exists in (c).

More specifically, the parallel movement and the rotational movement value in which the plane A having three different normal vectors in FIG. 5 (a) are matched with the three planes B are determined to be a single one. On the other hand, in FIG. 5 (b), there is only one rotational movement value in which the plane A having two different normal vectors is matched with the two planes B, but the parallel movement value is not determined as one, Infinite can exist along the intersection of the two planes. On the other hand, in FIG. 5 (c), the rotational movement value and the parallel movement value in which the plane A having one normal vector is matched with one plane B are not determined as one, and there may be infinite. This will be described in detail in the description of Table 1. [

In step S130, the rotational movement value estimating apparatus corrects the indefinite rotational movement value between the two points using the IMU rotational measurement value selectively measured by the inertial sensor (IMU) according to the number of selected at least one plane feature point pair Is calculated.

An inertial sensor has a basic principle of detecting an inertial force acting on an inertial body by an applied acceleration and may be a generic name of a gyro and its application device measuring an angular displacement or a rate of change in a certain direction .

First, the rotational movement value estimating apparatus has a limitation that it is impossible to determine the rotational movement values between the first point plane and the second point plane as one, when there is only one plane feature point pair as shown in Fig. 5 (c). In this case, the rotational movement value has an infinite number of solutions and can be an indefinite rotational movement value that can not be determined as one.

At this time, the rotational movement value estimating apparatus estimates the rotational motion of the robot by using the IMU rotational measurement value measured by the inertial sensor mounted on the robot and the normal vector included in the pair of plane feature points while the robot moves between the first point and the second point, It is possible to calculate the rotation movement correction value for correcting the uncertain rotational movement value between the first point plane and the second point plane.

Secondly, when two or more pairs of plane feature points exist as shown in Fig. 5 (a) or (b), the rotational movement value estimating apparatus can determine the rotational movement values between the first point plane and the second point plane as one, It is not necessary to calculate the rotation movement correction value.

For reference, the rotation movement value estimating device can calculate the final rotation movement value from the IMU rotation measurement value when there is no plane feature point pair.

As a result, the rotation movement value estimating apparatus may selectively calculate the rotation movement correction value only when the pair of plane feature points is 1, and may not calculate the rotation movement correction value when there are two or more pairs of plane feature points.

≪ Relation between the number of pairs of plane feature points and the number of solutions of rotational movement and parallel movement > Matched
Number of Plane Feature Pairs Rotate Translation 3 or more A single year A single year 2 A single year Infinite (one axis missing) One Infinite (one axis missing) Infinite (two axes missing) 0 Infinite (lack of 3 axes) Infinite (lack of 3 axes)

Referring to Table 1, when the number of pairs of planar feature points is three or more, the rotational movement and the parallel movement between the two points are determined as a single solution (see Fig. 5 (a)). In addition, when the number of pairs of plane feature points is two, the rotation movement is determined as a single solution, but the parallel movement has an infinite solution with respect to one axis (refer to FIG. 5 (b)). Is less than or equal to one, the rotational movement and the parallel movement have an infinite solution (see Fig. 5 (c)).

In another embodiment, the rotation movement correction value can be calculated using the following equation (1).

[Equation 1]

는 회전이동보정값이고, r은 제1 지점에 대응되는 시점(time)이고, l은 제2 지점에 대응되는 시점이고,

는 IMU회전측정값을 로봇이 실제로 회전한 최종회전이동값으로 환산하는 회전변환정보이고,

는 쿼터니언 곱이고,

는 r시점부터 l시점까지의 IMU회전측정값이고,

는 r시점부터 l시점까지의 불확정회전이동값의 역회전 정보이다.here,

R is the time corresponding to the first point, l is the point corresponding to the second point,

Is rotation conversion information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from point r to point l ,

Is the inverse rotation information of the uncertain rotational motion value from the r point to the l point of time.

Hereinafter, with reference to FIG. 3, a process for obtaining Equation 1 will be described when there are one pair of plane feature points.

The final rotational motion value at which the robot actually rotates can be calculated from the following equation (2) using the rotational motion correction value and the indeterminate rotational motion value (singular solution).

&Quot; (2) "

은 로봇이 제1 지점에서 제2 지점 간을 이동하는 동안 회전한 최종회전이동값이고,

는 불확정회전이동값을 최종회전이동값으로 보정하기 위한 회전이동보정값이고,

는 r시점부터 l시점까지의 회전이동의 해가 하나로 결정되지 않는 불확정회전이동값이다.here,

Is the final rotational movement value that the robot has rotated while moving between the first point and the second point,

Is a rotational movement correction value for correcting the uncertain rotational movement value to the final rotational movement value,

Is an indeterminate rotational motion value in which the solution of rotational motion from point r to point l is not determined as one.

를 회전이동하여 벡터

와 평행하게 만들어 주는 회전 정보를 의미하며, 관성센서로부터 측정된 정보를 왜곡하지 않도록 가장 짧은 곡선길이(shortest arc length)로 회전이동하는 값이다. At this time, the rotation conversion information is the vector

And the vector

And is a value that rotates to the shortest arc length so as not to distort the information measured from the inertial sensor.

On the other hand, the rotation conversion information can be calculated using Equation (3).

&Quot; (3) "

는 벡터

를 벡터

과 평행하게 되도록 회전시키는 회전변환정보이고,

는 정합의 대상이 되는 제1 지점평면의 법선벡터(

)를 IMU회전측정값으로 회전이동한 벡터이고,

는 정합의 대상이 되는 제2 지점평면의 법선벡터이다.here,

Vector

Vector

Rotation conversion information to be rotated so as to be parallel to the rotation axis,

Is the normal vector of the first point plane to be matched (

) To the IMU rotation measurement value,

Is the normal vector of the second point plane to be matched.

,

및

간에는

및

의 관계가 성립한다. 즉,

는 정합의 대상이 되는 제1 지점평면의 법선벡터이고,

는 정합의 대상이 되는 제1 지점평면의 법선벡터(

)를 IMU회전측정값으로 회전이동한 벡터이다.At this time,

,

And

The liver

And

. In other words,

Is a normal vector of the first point plane to be matched,

Is the normal vector of the first point plane to be matched (

) To the IMU rotation measurement value.

Also, since the rotation conversion information is applied to the IMU rotation measurement value measured by the inertial sensor and the value converted into the actual rotation value of the robot is the final rotation movement value, the final rotation movement value is defined by the following equation (4) .

&Quot; (4) "

은 로봇이 제1 지점에서 제2 지점 간을 이동하는 동안 회전한 최종회전이동값이고,

는 IMU회전측정값을 로봇이 실제로 회전한 최종회전이동값으로 환산하는 회전변환정보이고,

는 r시점부터 l시점까지의 IMU회전측정값이다.here,

Is the final rotational movement value that the robot has rotated while moving between the first point and the second point,

Is rotation conversion information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is the measured IMU rotation from point r to point l .

)을 계산하면, 수학식 1과 동일한 결과를 얻을 수 있다.Here, using the equations (2) and (4), the rotation movement correction value

), The same result as in Equation (1) can be obtained.

In another embodiment, the rotational movement value estimating device may calculate a final rotational movement value between two points based on at least one of at least one of the pair of plane feature points and the selectively calculated rotational movement correction value.

For example, referring to Equation (2), the rotational movement value estimating apparatus estimates the rotational motion of the object by using the calculated rotational motion compensation value and the uncertain rotational motion value calculated from the normal vectors of the first point plane and the second point plane included in the pair of plane feature points , The final rotational movement value can be calculated.

On the other hand, in the case where the rotational movement correction value is not calculated, since the rotational movement between the first point plane and the second point plane will have a single solution, And the normal vector of the second point plane.

In another embodiment, the rotational movement value estimating apparatus estimates a plurality of peripheral depth value information measured at each of a plurality of points on the three-dimensional space and a final rotational movement value and a parallel movement value between two adjacent points among the plurality of points A three-dimensional space map can be generated based on a plurality of robot movement information to be constructed. At this time, the translation value between the two points can be calculated based on the selected pair of plane feature points and the IMU movement measurement value measured by the inertial sensor.

For example, when the robot is moving in an unknown three-dimensional space (e.g., inside of a building) and the surrounding depth information is measured at a plurality of points, the rotational movement value estimating apparatus estimates a plurality of robot movement information of two adjacent points So that the relative positions between the two points can be sequentially determined. Then, after all the relative positions between the two points adjacent to the plurality of points are determined, the rotational movement value estimating apparatus can finally generate the map of the three-dimensional space. At this time, the map of the three-dimensional space can eventually mean the movement trajectory of the robot. On the other hand, a method of calculating the parallel movement value based on the plane feature point pairs and the IMU movement measurement values will be obvious to those of ordinary skill in the art, and a description of the calculation method will be omitted.

6, when the robot moves in (a) inside a two-story building, (b) moves in a single-story building, when the rotational movement value estimating device varies according to the number of pairs of plane feature points It can be seen that the map of the three-dimensional space can be generated from the movement trajectory of the robot with respect to 0-rank, 1-rank, 2-rank, 3-rank.

In another embodiment, the rotational movement value estimating apparatus may select at least one plane feature point pair in the following order.

First, the rotational movement value estimating device calculates plane feature points for each of at least one first point plane and at least one second point plane.

For example, when three first and second point planes are detected, the rotational movement value estimating apparatus estimates a plane feature point having a normal vector and a distance to a robot (distance sensor) for all seven planes Can be calculated.

Secondly, based on the normal vector information included in each of the planar feature points and the singular value calculated through the singular value decomposition (SVD), the rotational movement value estimating apparatus calculates the position of the object to be matched between the first point plane and the second point plane At least one plane feature point pair is selected.

The singular value decomposition (SVD) refers to a method of decomposing a matrix generated by using a normal vector included in each of planar feature points into a specific structure in which a singular value is calculated. The singular value is obtained by singular value decomposition of the matrix Value.

In this case, the specific process of selecting the pair of plane feature points to be mutually mapped using the normal vector information included in each of the plane feature points and the singular value calculated through the singular value decomposition will be described in detail in the following embodiments Will be described later.

In another embodiment, the rotational movement value estimating apparatus may perform singular value decomposition in the following order.

First, the rotational motion estimation apparatus generates a pseudoinverse of covariance based on a normal vector included in the plane feature points of each of the at least one first point plane and the at least one second point plane.

The covariance is a numerical value indicating the relationship between the normal vectors of the two plane feature points to be matched between the first point plane and the second point plane, that is, the uncertainty of the matching, and the pseudoinverse is a matrix generated using covariance Are generated to compute the inverse of the non-square matrix. Here, the covariance can be used for analyzing the layout pattern of the plane feature points of each of the first point plane and the second point plane to select a meaningful pair of plane feature points.

For example, the rotational movement value estimating apparatus can calculate the covariance between the normal vectors of the two plane feature points constituting the pair of plane feature points, and generate the pseudoinverse matrix using the calculated covariance. Second, A pair of plane feature points to be matched is selected based on a ratio between a plurality of singular values calculated by singular value decomposition (SVD) of a pseudo inverse matrix.

For example, referring to FIG. 4, the rotational motion estimation apparatus can perform singular value decomposition on the generated pseudoinverse matrix. Then, the singular values (s1, s2, s3) the ratio (s2 / s1, s3 / s1 ) is previously threshold value of the predetermined specific value ratio based on is greater than (ρ _th), the normal vectors (v2, v3 between the calculated accordingly ) Are matched to select pairs of plane feature points.

Ten thousand and one, s2 / s1 is the threshold value (ρ _th) greater than, s3 / s1 is the threshold value (ρ _th) plane feature point pair having a normal vector which is smaller, the rotational movement estimation unit is present in the v2 and the same line than is valid, , it can be judged that the plane feature point pair having a normal vector which is on the same line as v3 is not valid.

In this case, referring to Table 1, it corresponds to the case where the number of pairs of plane feature points to be matched is two, the solution of the rotation movement exists uniquely, and the solution of the parallel movement has infinite solution.

As described above, in the rotation movement value estimation method used for position estimation in the three-dimensional space according to the embodiment of the present invention, when the rotation movement value measured while the robot moves between two points is an indeterminate rotation movement value, It is possible to estimate a more accurate final rotational movement value by selectively correcting the IMU rotational measurement value measured by the inertial sensor mounted on the vehicle.

2 is a flow chart illustrating a method of detecting at least one first point plane and at least one second point plane in accordance with an embodiment of the present invention.

In step S210, the rotational movement value estimating device measures the first and second perimeter depth value information using a distance sensor at each of the first point and the second point.

For example, the rotational movement value estimating apparatus can measure the depth value at each of the adjacent first and second points. In this case, the depth value measured at the first point may be the first surrounding depth value information, and the depth value measured at the second point may be the second surrounding depth value information.

In step S220, the rotational movement value estimation device collects first distance information and second distance information for a plurality of points included in the first and second neighboring depth value information, respectively.

For example, the rotational movement value estimating apparatus collects first distance information for a plurality of points included in the first surrounding depth value information, and collects second distance information for a plurality of points included in the second surrounding depth value information can do.

In step S230, the rotational movement value estimation device detects at least one first point plane based on the collected first distance information, and detects at least one second point plane based on the collected second distance information.

For example, the rotational movement value estimating apparatus can detect at least one first point plane by classifying points lying on the same plane from the first distance information for a plurality of points. Similarly, at least one second point plane can be detected.

Thus, a method of detecting at least one first point plane and at least one second point plane in accordance with an embodiment of the present invention includes detecting at least one plane from a result of measuring a depth value using a distance sensor There is an effect that can be done.

7 is a view for explaining a rotation movement value estimating apparatus used for position estimation in a three-dimensional space according to an embodiment of the present invention.

7, a rotation movement value estimation apparatus 700 used for position estimation in a three-dimensional space according to an embodiment of the present invention includes a detection unit 710, a selection unit 720, and a calculation unit 730 . In addition, it may further include a map generator (not shown).

At this time, the rotational motion estimation apparatus 700 can be used for position estimation between the two points when the robot moves from the first point to the second point on the three-dimensional space.

The detection unit 710 detects at least one first point plane at each of the first point and the second point and at least one first point plane at each of the first point and the second point based on the depth value information, As shown in FIG.

The selecting unit 720 extracts plane feature points constituted by the distance between the plane and the distance sensor and the normal vector information of the plane from at least one first point plane and at least one second point plane, At least one plane feature point pair is selected.

In another embodiment, the selector 720 may calculate plane feature points for each of the at least one first point plane and the at least one second point plane, and calculate normal vector information and singular value decomposition included in each of the plane feature points The at least one plane feature point pair to be matched between the first point plane and the second point plane can be selected based on the singular value calculated through the first point plane and the second point plane.

In yet another embodiment, the selector 720 may select at least one plane feature point pair that is the subject of registration between the first point plane and the second point plane, the at least one first point plane and the at least one Based on the normal vectors included in the plane feature points of each of the two-point planes, generates a pseudo-inverse matrix of the covariance matrix, and based on the ratio between the plurality of singular values calculated by singular value decomposition of the pseudo- Can be selected.

The calculating unit 730 may calculate a rotation movement correction value for correcting the indefinite rotation movement value between the two points by using the IMU rotation measurement value selectively measured by the inertial sensor according to the number of selected at least one pair of plane feature points .

Finally, the map generation unit (not shown) generates a map of the three-dimensional space based on the plurality of robot movement information. At this time, the calculation unit 730 may be configured to calculate a final rotation movement value and a parallel movement value between the adjacent two points among the plurality of points based on the plurality of peripheral depth value information measured at each of the plurality of points on the three-dimensional space A plurality of robot movement information can be generated. On the other hand, the translation value between the two points can be calculated based on the selected pair of plane feature points and the IMU movement measurement measured by the inertial sensor.

In another embodiment, the calculating unit 730 does not calculate the rotation movement correction value if the number of pairs of planar feature points is two or more, and if the number of pairs of planar feature points is one, The rotation movement correction value can be calculated based on the normal vector included in the pair of plane feature points and the IMU rotation measurement value.

In another embodiment, the rotational motion correction value may be calculated using Equation (5).

&Quot; (5) "

는 회전이동보정값이고, r은 제1 지점에 대응되는 시점이고, l은 제2 지점에 대응되는 시점이고,

는 IMU회전측정값을 로봇이 실제로 회전한 최종회전이동값으로 환산하는 회전 정보이고,

는 쿼터니언 곱이고,

는 r시점부터 l시점까지의 IMU회전측정값이고,

는 r시점부터 l시점까지의 불확정회전이동값의 역회전 정보이다.here,

R is the time corresponding to the first point, l is the time corresponding to the second point,

Is rotation information that converts the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from point r to point l ,

Is the inverse rotation information of the uncertain rotational motion value from the r point to the l point of time.

In yet another embodiment, the calculator 730 may further calculate a final rotational motion value between the two points based on at least one of the at least one planar feature point pair and the selectively computed rotational motion compensation value.

The above-described embodiments of the present invention can be embodied in a general-purpose digital computer that can be embodied as a program that can be executed by a computer and operates the program using a computer-readable recording medium.

The computer readable recording medium includes a magnetic storage medium (e.g., ROM, floppy disk, hard disk, etc.), optical reading medium (e.g., CD ROM, DVD, etc.).

The present invention has been described with reference to the preferred embodiments. It will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims. Therefore, the disclosed embodiments should be considered in an illustrative rather than a restrictive sense. The scope of the present invention is defined by the appended claims rather than by the foregoing description, and all differences within the scope of equivalents thereof should be construed as being included in the present invention.

Claims (15)

A method for estimating a rotational movement value used for position estimation between two points when a robot moves from a first point to a second point on a three-dimensional space,
At least one first point plane at each of the first point and the second point and at least one second point plane at each of the first point and the second point based on the depth value information, Detecting a point plane;
A plane feature point comprising a distance between the plane and the distance sensor and normal vector information of the plane is extracted from each of the at least one first point plane and the at least one second point plane to form at least one Selecting plane feature point pairs; And
Calculating a rotation movement correction value for correcting the indefinite rotation movement value between the two points using the IMU rotation measurement value selectively measured by the inertial sensor (IMU) according to the number of the selected at least one plane feature point pair step
And estimating a rotational movement value used for position estimation in a three-dimensional space. The method according to claim 1,
The step of calculating the rotation movement correction value for correcting the uncertain rotational movement value
When the number of pairs of planar feature points is two or more, the rotation movement correction value is not calculated,
And calculating the rotation movement correction value based on the uncertain rotational movement value between the two points, the normal vector included in the pair of plane feature points, and the IMU rotation measurement value, when the number of the pair of plane feature points is one And estimating a rotational movement value used for position estimation in a three-dimensional space. 3. The method of claim 2,
Wherein the rotation movement correction value is calculated using Equation (1).
[Equation 1]

here,

Is a rotation movement correction value, r is a time point corresponding to the first point, l is a time point corresponding to the second point,

Is rotation information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from the r point to the l point,

Is the inverse rotation information of the uncertain rotational movement value from the r point to the l point of time. The method according to claim 1,
Calculating a final rotational motion value between the two points based on at least one of the at least one planar feature point pair and the selectively calculated rotational motion correction value;
And estimating a rotational movement value for use in position estimation in a three-dimensional space. 5. The method of claim 4,
Based on a plurality of neighboring depth value information measured at each of a plurality of points on the three-dimensional space and a plurality of robot movement information composed of a final rotation movement value and a parallel movement value between two adjacent points among the plurality of points, Steps for generating a map of a three-dimensional space
Further comprising:
The translation value between the two points is
Wherein the motion estimation value is calculated based on the selected pair of plane feature points and the IMU movement measurement value measured by the inertial sensor. The method according to claim 1,
Wherein detecting the at least one first point plane and the at least one second point plane
Measuring first peripheral depth value information and second peripheral depth value information using the distance sensor at each of the first point and the second point;
Collecting first distance information and second distance information for a plurality of points included in each of the first peripheral depth value information and the second peripheral depth value information; And
Detecting the at least one first point plane based on the first distance information and detecting the at least one second point plane based on the second distance information
And estimating a rotational movement value used for position estimation in a three-dimensional space. The method according to claim 1,
The step of selecting the at least one planar feature point pair
Calculating the planar feature points for each of the at least one first point plane and the at least one second point plane; And
Based on the normal vector information included in each of the plane feature points and the singular value calculated through the singular value decomposition (SVD), at least one plane feature point to be an object of matching between the first point plane and the second point plane Selecting a pair;
And estimating a rotational movement value used for position estimation in a three-dimensional space. 8. The method of claim 7,
Wherein the step of selecting at least one plane feature point pair to be matched between the first point plane and the second point plane
Generating a pseudoinverse of a covariance based on a normal vector included in the plane feature points of each of the at least one first point plane and the at least one second point plane; And
Selecting a pair of plane feature points to be matched based on a ratio between a plurality of singular values calculated by performing singular value decomposition (SVD) on the pseudo inverse matrix;
And estimating a rotational movement value used for position estimation in a three-dimensional space. An apparatus for estimating a rotational movement value used for position estimation between two points when a robot moves from a first point to a second point on a three-dimensional space,
At least one first point plane at each of the first point and the second point and at least one second point plane at each of the first point and the second point based on the depth value information, A detector for detecting a point plane;
A plane feature point comprising a distance between the plane and the distance sensor and normal vector information of the plane is extracted from each of the at least one first point plane and the at least one second point plane to form at least one A selector for selecting pairs of plane feature points; And
Calculating a rotation movement correction value for correcting an uncertain rotational movement value between the two points by using the IMU rotation measurement value selectively measured by the inertia sensor according to the number of selected at least one plane feature point pairs;
Dimensional space, and the rotation movement value estimating unit is used to estimate the position in the three-dimensional space. 10. The method of claim 9,
The calculating unit
When the number of pairs of planar feature points is two or more, the rotation movement correction value is not calculated,
And calculating the rotation movement correction value based on the uncertain rotational movement value between the two points, the normal vector included in the pair of plane feature points, and the IMU rotation measurement value, when the number of the pair of plane feature points is one Dimensional space, which is used for estimating a position in a three-dimensional space. 11. The method of claim 10,
Wherein the rotation movement correction value is calculated using Equation (1).
[Equation 1]

here,

Is a rotation movement correction value, r is a time point corresponding to the first point, l is a time point corresponding to the second point,

Is rotation information for converting the IMU rotation measurement value into a final rotation movement value that the robot actually rotates,

Is a quaternion product,

Is an IMU rotation measurement value from the r point to the l point,

Is the inverse rotation information of the uncertain rotational movement value from the r point to the l point of time. The method according to claim 1,
The calculating unit
Wherein the final rotation movement value between the two points is further calculated based on at least one of the at least one plane feature point pair and the selectively calculated rotation movement correction value. The rotational movement value estimating apparatus comprising: 13. The method of claim 12,
Wherein the calculation unit calculates a plurality of neighboring depth values based on a plurality of neighboring depth value information measured at each of a plurality of points on the three-dimensional space, When calculating more information,
A map generating unit for generating a map of the three-dimensional space based on the plurality of robot movement information,
Further comprising:
The translation value between the two points is
Wherein the motion estimation value is calculated on the basis of the selected pair of plane feature points and the IMU movement measurement value measured by the inertial sensor. 10. The method of claim 9,
The selector
Calculating the plane feature points for each of the at least one first point plane and the at least one second point plane,
Selecting at least one pair of plane feature points to be matched between the first point plane and the second point plane based on the normal vector information included in each of the plane feature points and the singular value calculated through singular value decomposition Dimensional space in a three-dimensional space. 14. The method of claim 13,
The selector
When selecting at least one plane feature point pair to be matched between the first point plane and the second point plane,
Generating a pseudoinverse of a covariance based on a normal vector included in the plane feature points of each of the at least one first point plane and the at least one second point plane,
And a pair of plane feature points to be matched is selected based on a ratio between a plurality of singular values calculated by singular value decomposition of the pseudoinverse matrix. .

Images