KR102083911B1 - Method for building map including point cloud using LiDAR - Google Patents

Abstract

In a method for generating a map according to an embodiment of the present invention, the map generating apparatus compares a feature of the first point group measured at the first point with a feature of the second point group measured at the second point, and compares the feature from the first point. Estimating a trajectory (Odometry), which is a movement path to the second point, and the map generating device performing position transformation and rotation transformation on the second point group based on the trajectory to have the same coordinate system as the first point group And generating, by the map generating apparatus, a point group map by accumulating the second point group on the first point group by comparing the features of the clustered objects of the first point group with the features of the clustered objects of the second point group. The estimating of the trajectory may include calculating a curvature of all the points belonging to the second point group, and comparing the preset thresholds to the points belonging to the edges and planes. And classifying points into corners among the points belonging to the second point group are projected onto the line of the first point group, and the points belonging to the plane among the points belonging to the second point group to the plane of the first point group. The generating of the point group map may include removing the point group corresponding to the ground from the second point group.

Description

Method for building map including point cloud using LiDAR}

The present invention relates to a method for generating a lidar map including a point group. More specifically, the present invention relates to a method of increasing the accuracy of overlap when generating a map of a plurality of point groups by using a lidar.

LiDAR (Light Detection and Ranging) is a technology that uses light to measure distance and detect objects. It was first developed for meteorological observations in the 1930s, and began to be used in the 1960s when laser technology appeared. At the time, it was mainly applied to the aviation sector and satellites, but was later applied to the global environment, exploration, cars, and robots.

The rider has a similar principle to radar. The radar launches electromagnetic waves outside and checks the distance and direction with the electromagnetic waves that are re-received. The difference is that Lidar uses a pulsed laser. By using lasers with short wavelengths, accuracy and resolution can be increased and stereoscopic identification can be made depending on objects.

Efforts have been made to create three-dimensional maps using such lidar. For example, KR 10-1427364 B1 "Scan system for generating a 3D indoor map using a lidar device" (July 31, 2014) discloses a system for generating a 3D indoor map using a lidar device.

However, the present invention measures the strength of signals received from three or more Wi-Fi transmitters to determine the location of the LiDAR device, thereby reducing the accuracy of the position of the LiDAR device. The accuracy of the location of the LiDAR device will immediately affect the quality of the 3D indoor map.

In addition, KR 10-2013-0123041 A "A method for automatically generating indoor maps using a lidar device" (2013.11.12) discloses a configuration for identifying the position information of a lidar device using a global positioning system (GPS). have. However, GPS has a range of errors ranging from several meters to several tens of meters and it is difficult to use indoors.

Accordingly, there is an increasing need for a map generation method capable of generating a map without distinguishing between indoors and outdoors and increasing the accuracy of the generated map.

The technical problem to be solved by the present invention is to provide a method and apparatus for generating a map using a lidar.

The technical problems of the present invention are not limited to the technical problems mentioned above, and other technical problems not mentioned will be clearly understood by those skilled in the art from the following description.

In the map generation method according to an embodiment of the present invention for solving the above technical problem, the feature of the first point group measured at the first point and the second point group measured at the second point by the map generation device Estimating a locus (Odometry), which is a movement path from the first point to the second point; Positioning and rotating transforming the second point group based on the trajectory so that the map generating device has the same coordinate system as the first point group; And generating, by the map generating apparatus, a point group map by accumulating the second point group on the first point group by comparing the features of the clustered objects of the first point group with the features of the clustered objects of the second point group. It may include.

Preferably, prior to the step of estimating the Odometry, for the second point group, using the IMU measurement to compensate for the distortion that may occur within one measurement period due to the movement of the map generating device It may further include.

Preferably, the step of estimating the trajectory (Odometry), the step of calculating the curvature for all points belonging to the second point group, and classifying the point belonging to the point belonging to the edge and the plane compared to a predetermined threshold value; And a point belonging to an edge among the points belonging to the second point group is projected on the line of the first point group, and a point belonging to a plane among the points belonging to the second point group is projected on the plane of the first point group, Generating equation (9).

[Equation 9]

는 상기 궤적의 회전 행렬이고,

는 상기 궤적의 이동 행렬이다.here,

Is the rotation matrix of the trajectory,

Is the moving matrix of the trajectory.

및

를 유도하는 단계를 더 포함할 수 있다.Preferably, the step of estimating the Odometry, to minimize the d by using an optimization technique

And

It may further include a step of inducing.

Advantageously, estimating said Odometry comprises: estimating a second trajectory which is a moving path from said first point to said second point using an IMU measurement; And merging the trajectory and the second trajectory.

Preferably, generating the point group map may include removing a point group corresponding to the ground from the second point group.

Preferably, the generating of the point group map may include clustering the points corresponding to within a predetermined radius from the second point by performing Euclidean distance based clustering.

Preferably, the step of generating the point group map, RANSAC is applied to the pairs matching the cluster of the first point group and the cluster of the second point group to remove the outliers, the transform supported by the most matching pairs The method may include obtaining a matrix.

Preferably, the calculating of the transformation matrix may include converting the second point group by using the transformation matrix and accumulating the converted second point group in the first point group.

According to another aspect of the present invention, there is provided a map generating apparatus comprising: a lidar sensor; IMU sensor; And comparing features of the first point group with features of the second point group, estimating a trajectory (Odometry) that is a moving path, converting the second point group based on the trajectory to have the same coordinate system as the first point group, and The controller may include a controller configured to compare the characteristics of the clustered objects of the first point group with the characteristics of the clustered objects of the second point group and accumulate the second point group in the first point group to generate a point group map.

Effects according to the present invention are as follows.

Using the map generation method according to the present invention, it is possible to generate a more accurate three-dimensional map. The generated map can be utilized in related fields such as autonomous driving or robot. In addition, 3D models can be easily obtained for various buildings including natural objects without 3D models.

In addition, the 3D map-related industry is one of the fourth industries, and can be used in various industries such as VR, games, architecture, surveying, and robots that require spatial information. In particular, highly accurate three-dimensional point cloud maps can contribute to the creation of new types of industries.

Effects of the present invention are not limited to the above-mentioned effects, and other effects not mentioned will be clearly understood by those skilled in the art from the following description.

1A to 1B are views for explaining the importance of location information when performing measurements using a lidar and overlapping point groups.
2 is a view for explaining a method for generating a map according to an embodiment of the present invention.
FIG. 3 is a diagram for describing distortion that may occur when a lidar moves within a period.
4 is a pseudo code for explaining a trajectory estimation process that can be used in an embodiment of the present invention.
5A is a view for explaining a ground removing process that can be used in one embodiment of the present invention.
5B is a diagram illustrating a clustering process that can be used in an embodiment of the present invention.
6 is a pseudo code for explaining a segment-based mapping process that can be used in an embodiment of the present invention.
7A to 8B are diagrams for comparing the performance of the map generating method according to an embodiment of the present invention with the conventional method.
9A to 9C are diagrams for describing an apparatus for generating a map, according to an exemplary embodiment.
10A to 10C are flowcharts illustrating a method for generating a map according to an embodiment of the present invention.

As the invention allows for various changes and numerous embodiments, particular embodiments will be illustrated in the drawings and described in detail in the written description. However, this is not intended to limit the present invention to specific embodiments, it should be understood to include all modifications, equivalents, and substitutes included in the spirit and scope of the present invention. In describing the drawings, similar reference numerals are used for similar elements.

Terms such as first, second, A, and B may be used to describe various components, but the components 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 the second component, and similarly, the second component may also be referred to as the first component. The term and / or includes a combination of a plurality of related items or any item of a plurality of related items.

When a component is said to be "connected" or "connected" to another component, it may be directly connected to or connected to that other component, but it may be understood that another component may be present in the middle. Should be. On the other hand, when a component is said to be "directly connected" or "directly connected" to another component, it should be understood that there is no other component in between.

The terminology used herein is for the purpose of describing particular example embodiments only and is not intended to be limiting of the present invention. Singular expressions include plural expressions unless the context clearly indicates otherwise. In this application, the terms "comprise" or "have" are intended to indicate that there is a feature, number, step, operation, component, part, or combination thereof described in the specification, and one or more other features. It is to be understood that the present invention does not exclude the possibility of the presence or the addition of numbers, steps, operations, components, components, or a combination 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. Terms such as those defined in the commonly used dictionaries should be construed as having meanings consistent with the meanings in the context of the related art, and shall not be construed in ideal or excessively formal meanings unless expressly defined in this application. Do not.

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

1A to 1B are views for explaining the importance of location information when performing measurements using a lidar and overlapping point groups.

For ease of understanding, it is assumed that a lidar is rotated 360 degrees counterclockwise in a two-dimensional plane to collect information by surrounding the pulse laser. Through this measurement process, the rider can obtain information about nearby objects based on their current position. That is, the reflected pulse laser can be used to determine that there is an object at any distance in any direction from its position.

Referring to FIG. 1A, the measurement is performed at the first point 110 to obtain the first point group 210, and then moved again to obtain the second point group 220 by performing the measurement at the second point 120. It can be seen that the third point group 230 is obtained by performing the measurement at the third point 130.

By accumulating and merging a plurality of point groups measured at each location, a three-dimensional map composed of point groups can be obtained. However, the first point group 210 only has relative coordinates based on the first point 110, and the second point group 220 only has relative coordinates based on the second point 120. Since the point group 230 has only relative coordinates based on the third point 130, in order to merge them, the point group 230 converts the second point group 220 and the third point group 230 into the same coordinate system as the first point group 210. I need to.

To this end, it is necessary to know exactly where the second point 120 and the third point 130 are located in the coordinate system using the first point 110 as the origin. That is, it is necessary to accurately measure the movement path or trajectory of the lidar leading to the first point 110, the second point 120, and the third point 130.

However, in the related art, in order to increase the accuracy due to the problem caused by the error of the trajectory, in order to increase the accuracy, the lidar is installed in a designated place as shown in FIG. 1A, and then the map is generated by measuring and moving in a stationary state for a certain time. . Referring to FIG. 1A, after the measurement is performed for a predetermined time at the first point 110, the measurement is performed by moving to the second point 120 again, and the measurement is performed by moving the third point 130 again. You can see the process.

However, such a mapping method can reduce the error of the trajectory, but has a disadvantage in that it takes a lot of time to prepare the map. In order to solve this problem, it is necessary to measure the movement of the lidar while minimizing the errors caused by the movement by using the position information of the lidar.

Referring to FIG. 1B, a process of simultaneously performing a measurement while moving from the first point 110 to the second point 120 and performing a measurement simultaneously while moving from the second point 120 to the third point 130 again. Can be seen. Although measuring the data while moving in this way can reduce the time to collect the data, as described above, the error that can be caused by the error of the trajectory is amplified more than in the case of FIG.

This error can be minimized by using the position information of the moving lidar. That is, the relative position information of the second point 120 from the first point 110 and the relative position information of the third point 130 from the first point 110 are set as the starting point of the first point 110. Considering this, it is possible to offset the error caused by the movement and increase the accuracy of the map.

In summary, in order to merge a plurality of point groups, the trajectory of Lidar is essential. However, in order to know the trajectory of the lidar, the position information of the lidar is needed, and the error of the position information affects the accuracy of the point group map. In order to minimize this effect, a method of performing a measurement in a stationary state as shown in FIG. 1A, a measurement in a stationary state after moving again, or the like may be used, but this is not preferable in terms of mapping efficiency.

Therefore, the measurement can be performed while moving as shown in FIG. 1B. However, in this case, the measurement is performed while moving, so that an error in the trajectory has a greater effect on the accuracy of the point group map than in the case of FIG. 1A. Therefore, when preparing a map by the method of FIG. 1B, it is important to understand the exact trajectory of the lidar in the point cloud mapping.

As such, the exact trajectory of the lidar, that is, the exact position information of the lidar, is of utmost importance. However, as described above, in the related art, a method of determining a location through triangulation or using GPS using a strength of signals received from three or more Wi-Fi transmitters has been used.

The method using a Wi-Fi transmitter has a disadvantage that it is difficult to distinguish between layers. In addition, the method using GPS also has a large error and is difficult to use indoors. In order to solve this problem, the present invention proposes a method using an Inertial Measurement Unit (IMU).

The IMU can be used to determine the position and posture of the Lidar. For example, the IMU may include a three-axis gyro sensor and a three-way acceleration sensor. Through this, the angular velocity and acceleration of the lidar can be detected and the position and posture of the lidar can be determined based on this. Hereinafter, information including both position and posture is referred to as position information. Posture, in other words, means rotation.

The present invention proposes a map generation method which finally increases the accuracy of the map by increasing the accuracy of the location information using the IMU. Through this, the surrounding environment structure can be generated as a 3D point cloud map in real time. This will be described in more detail with reference to FIG. 2.

2 is a view for explaining a method for generating a map according to an embodiment of the present invention.

Referring to Figure 2 the present invention is largely composed of two steps. One is the Odometry Estimation step and the other is the Segment based mapping step. Through the trajectory estimation step and the segment-based mapping step, the distortion of the point cloud data can be compensated and the accuracy of the map can be improved.

는 k 시점에 측정된 점군을 의미하고,

는 한 주기 내에서 왜곡 보정을 거친 k 시점의 점군을 의미한다.

은 k 시간에 구한 특징(선과 면)을 나타낸다.

은 라이다로부터 추정된 궤적이고,

은 IMU로부터 추정된 궤적이며

는 융합된 궤적이다.Here, looking at terms related to the trajectory estimation step

Means a point group measured at time k,

Denotes a point group at point k after distortion correction within one period.

Denotes the characteristics (line and face) obtained at k time.

Is the trajectory estimated from Lidar,

Is the estimated trajectory from the IMU

Is the converged trajectory.

Referring to FIG. 2, when the trajectory estimation step is performed, distortion correction is performed on a point group within a period of a k-th sample measured by Lidar (Distortion Compensation), and a feature is extracted from a point group in which distortion is corrected (Feature Extraction). This is compared with the feature of the previous frame (Feature Matching), and the trajectory is estimated (Odometry Estimation). After that, the data is fused with information measured using the IMU (Data Fusion) to correct the estimated trajectory and perform point cloud transformation (Point Cloud Transformation).

To examine the distortion compensation step of FIG. 2 in more detail, see FIG. 3. FIG. 3 is a diagram for describing distortion that may occur when a lidar moves within a period.

Referring to FIG. 3, a point group 211 in a stationary state is shown on the left side, and a point group 212 in a moving state is shown on the right side. As shown in FIG. 3, when the measurement is performed while the lidar is stopped, the start point and the end point are the same within one period so that distortion does not occur. However, if you measure the movement of the lidar, the difference between the starting point and the end point occurs within one period of the movement of the lidar.

That is, the trajectory, which is a movement path of the lidar, is not only required to merge (= overlap) a plurality of point groups as shown in FIGS. 1A to 1B, but also a distortion may occur as shown in FIG. 3 within one period. The movement path from the first-first point 111 to the first-second point 112 is required.

In other words, in the case of the left side of FIG. 3, since the start point and the end point use the coordinate system based on the first point 110 which is the same origin, there is no distortion, but in the case of the right side of FIG. 3, the start point Since (start) is a coordinate system using the first-first point 111 as the origin and an end point is the coordinate system using the first-second point 112 as the origin, distortion occurs within one period.

In order to compensate for the distortion caused by the movement of the lidar within this period, the measured point groups are first converted into the reference coordinate system using the value measured by the IMU. Here, the reference coordinate system is a coordinate system whose origin is the position and posture of the point where the lidar starts the measurement. In the example of the right side of FIG. 3, it is a coordinate system which makes the 1st-1st point 111 the origin. To do this, maps are generated using a measuring device that combines Lidar sensors with IMU sensors.

Next, the position, acceleration, and angular velocity of the measuring device are measured to determine the exact position of the point according to the measurement point of each point. In other words, the exact position of the point is obtained for every point between the start point and the end point. Assuming that the measuring device performs the equivalent acceleration motion for the convenience of understanding, after removing the gravity acceleration component from the IMU, and estimating the position and attitude, the first-first point 111 according to the following equations 1 and 2 It is possible to calculate the moving distance and the speed of each axis between the first and second points 112.

[Equation 1]

[Equation 2]

In Equation 1, the acceleration a, the speed v, and the position s are defined in three dimensions, and in Equation 2, the relationship between the speed v and the acceleration a and the relationship between the position s and the speed v are defined. Referring to Equation 2, the speed v may be obtained by adding a value obtained by multiplying the speed change and the acceleration by the speed at the time k-1. In addition, the position s can be obtained by the correlation between the velocity v and the acceleration a.

Since the IMU sensor provides acceleration values in the three-axis direction, it is possible to obtain a velocity by integrating this, and knowing the moving path between the first-first point 111 and the first-second point 112 in FIG. Can be. Through this, the end point of which the first-second point 112 is the origin may be converted into a coordinate system of which the first-first point 111 is the origin.

Here, since the rider rotates at regular intervals to measure the data, in order to correct the accurate position of the measurement data, the measurement point of each point to be measured must be accurately known. When the measuring device moves, the acceleration and angular acceleration in various directions due to the irregular movement affect the uniformity of the lidar measurement cycle.

Suppose that the first measured point in the data measured in one revolution was obtained at t seconds, and the last data obtained at t + 0.1 seconds. That is, since one measurement is made for 0.1 second, ten measurements are possible for one second. In this case, the measuring period of the lidar sensor of the measuring device is 10 Hz.

라 하고, IMU 센서의 측정 시간을

~

라 가정하자. 만약

의 시간 값이

보다 클 경우 측정되는 점군 데이터는 순차적으로 나열될 수 있다.The motion distortion compensation algorithm needs to know the position information of the measurement point of each point measured in one rotation of the lidar. In the previous example, the lidar makes one revolution for 0.1 seconds. At this time, measure the time of each point

And the measurement time of the IMU sensor

To

Suppose if

Time value of

If larger, the point cloud data measured may be listed sequentially.

의 시간 값이

보다 작을 경우에는 다음의 수학식 3을 통해서 각 시점 k에 대한 비율을 구해서 사용할 수 있다. 즉 비율값을 이용하여 수학식 5에 의한 속도

와 위치

에 따라 연속적인 점군의 범위는 재조정되어 나열될 수 있다.But if not, that is

Time value of

If smaller, the ratio for each time point k may be obtained from Equation 3 below. That is, the speed by the equation (5) using the ratio value

And position

Depending on the range of successive point groups can be rearranged and listed.

[Equation 3]

[Equation 4]

[Equation 5]

와 i+1 번째 샘플의 보상된 점군

사이의 궤적을 추정한다. 이를 위해서 각 시점의 특징점을 추출하여 궤적(odometry)을 계산한다.Hereinafter, the trajectory estimation step will be described. The process of estimating the trajectory after correcting the distortion caused by the movement of the point group measured by the measuring device using the measured value of the IMU sensor is as follows. compensated point group of the i th sample

And compensated point groups of the i + 1 th sample

Estimate the trajectory between. To do this, the feature point of each viewpoint is extracted and the odometry is calculated.

에 속하는 모든 점들에 대하여, 곡률 값을 계산한다. 곡률 값은 인접 7개 점간의 상대적인 거리에 기반하여 계산되며 선택된 점 왼쪽의 3개의 점과 오른쪽의 점 3개를 이용하여 계산할 수 있다.More specifically, curvature-based feature points are used. first,

Calculate the curvature value for all points belonging to. The curvature value is calculated based on the relative distance between the seven adjacent points and can be calculated using three points on the left and three points on the right.

, 평면으로 판단된 점은

집합으로 분류한다.The curvature is determined by the average distance between the corresponding point and the adjacent points. When the curvature value is higher than a specific threshold, the curvature is determined as an edge, and when it is lower than that, it is determined as a plane. After that, the points that are judged as corners

, The point determined to be flat

Classify as a set.

와 i-1 번 째 스캔 시점의 측정 장치의 자세

간의 관계를 다음의 수학식 6과 같이 변환 행렬 간의 곱으로 표현할 수 있다.When the points are classified into corners and planes according to the curvature, posture changes of six degrees of freedom (DOF) of the measuring device at the time of measuring two samples are estimated by matching the points classified in the previous sample. The posture change is assumed to be a rigid body transform, and thus the posture of the measuring device at the i th scan point is assumed.

And the posture of the measuring device at the time of i-1th scan

Can be expressed as a product between transformation matrices, as shown in Equation 6 below.

[Equation 6]

에 속하는 점

에 대하여

집합에 속하는 점들 중 가장 가까운 2점을 선택하고 이 두 점을 연결하는 직선을 구성하고, 점

를 그 직선 위에 투영한다.

를 직선 위에 투영된 점이라고 가정하고, i-1번째 측정과 i번째 측정 사이에 발생한 움직임의 회전을

로, 이동을

이라 할 때 다음의 수학식 7의 관계를 만족한다.Set of points judged as corners

Points belonging to

about

Select the two nearest points among the points in the set, construct a straight line connecting these two points, and

Project onto the straight line.

Is a point projected on a straight line, and the rotation of the motion that occurs between the i-1th and ith measurements

Go to

This satisfies the relationship of Equation 7 below.

[Equation 7]

In addition, Equation 7 may be arranged as a non-linear equation as shown in Equation 8 below.

[Equation 8]

에 속하는 모든 점에 대해서 수학식 8과 같은 형태로 정리가 가능하다. 마찬가지로 평면으로 판단된 점들의 집합

에 속하는 점은 3점을 추출하여 평면을 구축하고, 이 평면에 투영되는 점을 이용하여 수학식 8을 유도할 수 있다. 이렇게 유도된 식을 행마다 쌓아 행렬로 표현된 비선형 방정식을 만들 수 있다. 이는 다음의 수학식 9와 같다.Set of points judged as corners

All points belonging to can be arranged in the form as shown in Equation (8). Similarly, a set of points judged to be flat

The points belonging to may be constructed by extracting three points and constructing a plane, and the equation (8) may be derived using the point projected on the plane. The derived equations can be stacked row by row to create nonlinear equations expressed as matrices. This is shown in Equation 9 below.

[Equation 9]

를 최소화하는

,

를 유도할 수 있다. 최적화 기법은 Gradient Descent, Gauss-Newton 등 다양한 방법을 사용할 수 있다. 다만, 본 발명에서는 이해의 편의를 돕기 위해 Levenberg-Marquardt (이하 LM) 기법을 기준으로 설명을 계속하기로 한다.In order to solve Equation 9, an optimization technique can be applied. In other words

To minimize

,

Can be derived. The optimization technique can use various methods such as Gradient Descent and Gauss-Newton. However, in the present invention, the description will be continued based on the Levenberg-Marquardt (LM) technique for convenience of understanding.

와 점군

사이의 측정 장치의 이동량을 계산할 수 있다. 이러한 과정을 모든 시점에 대해서 반복하면 측정 장치가 측정을 시작한 시점부터 종료한 시점 사이의 라이다 장치의 이동 궤적을 추정할 수 있다. 즉 도 1b에서 제1 지점(110), 제2 지점(120) 및 제3 지점(130)으로 이루어지는 궤적을 추정할 수 있다. 이러한 과정은 도 2에서 설명한 특징점 비교를 기반으로 한 궤적 추정 과정이다. 이를 의사 코드로 정리하면 도 4와 같다.This allows the point cloud

And point group

The amount of movement of the measuring device between can be calculated. If this process is repeated for all time points, the movement trajectory of the lidar device can be estimated between the time point at which the measurement device starts the measurement and the time point at which the measurement device ends. That is, in FIG. 1B, a trajectory consisting of the first point 110, the second point 120, and the third point 130 may be estimated. This process is a trajectory estimation process based on the feature point comparison described in FIG. This is summarized in FIG. 4 as pseudo code.

4 is a pseudo code for explaining a trajectory estimation process that can be used in an embodiment of the present invention.

와 i-1 번째 샘플의 보상된 점군

및 i-1 번째 샘플의 측정 장치의 자세

를 이용하여 i 번째 샘플의 측정 장치의 자세

를 구할 수 있다. 이를 위해서

의 모든 점에 대해서 곡률을 기반으로 모서리와 평면으로 점을 분류한다.Referring to FIG. 4, the compensated point group of the i th sample

And compensated point group of the i-1 th sample

And the posture of the measuring device of the i-1th sample

Position of the measuring device of the i th sample using

Can be obtained. for this

Classify the points into corners and planes based on curvature for all points in.

Next, when a specific point is classified as an edge, Equation 8 is generated by selecting two points closest to the specific point in the i-1th sample and projecting the specific point on a line composed of two selected points. When a specific point is classified as a plane, Equation 8 is generated by selecting three points closest to the specific point in the i-1 th sample and projecting the specific point on a plane composed of the selected three points.

와

사이의 측정 장치의 움직임을 회전

와 이동

로 표현할 수 있다. 이처럼 특징점을 기반으로 구한 측정 장치의 움직임을 궤적(Odometry)으로 사용하면 도 1b에서 설명한 것처럼 측정 장치가 이동하면서 측정한 복수개의 점군을 중첩하여 3차원 점군 지도를 생성할 수 있다.Equation 8 derived for all points can be expressed as a matrix, and equation 9 can be obtained.

Wow

Rotate the movement of the measuring device between

Go with

Can be expressed as When the movement of the measuring device obtained based on the feature point is used as an Odometry, a three-dimensional point group map may be generated by overlapping a plurality of measured point groups as the measuring device moves as described in FIG. 1B.

Of course, in order to solve the distortion that may occur due to the movement of the measuring device at a very short time within a period as described above with reference to FIG. 3, the first measurement is performed at the 1-1st point 111 using the value measured by the IMU. It has been described to find the travel path between the -2 points 112.

Similarly, the trajectory of the first point 110, the second point 120, and the third point 130, which are the moving paths of FIG. 1B, may be estimated using the value measured by the IMU. However, the value provided by the IMU is acceleration, and the moving distance is obtained by integrating this, so that the longer the path, the larger the error caused by the integration. Therefore, as shown in FIG. 3, the movement path within one period may be obtained using the measured value of the IMU. However, as shown in FIG. 1B, the moving path in the entire measurement process is obtained using the measured value of the IMU.

Conventionally, in order to solve the problem that an error may occur when the position of the lidar device is detected using GPS, a map may be created by using an IMU in addition to the GPS. However, as described above, if the IMU is used to obtain the acceleration, not the direct position of the measuring device, and the position is obtained by integrating the acceleration, the longer the moving path, the greater the error.

Therefore, in the present invention, the IMU sensor value is used only when calculating a short moving path for correcting distortion occurring within one period as in the case of FIG. 3, and for a long time in the entire measurement process as shown in FIG. 1B. When calculating the moving path of, the moving path is calculated by comparing the feature points of each sample. This can increase the accuracy of the trajectory.

It is also possible to use a trajectory estimated by comparing the feature points and merging the trajectory estimated using the IMU. This is the Data Funsion step of FIG. A weight-based arithmetic operation can be performed to merge the trajectories estimated using the IMU with the trajectories estimated by comparing the feature points.

에 w의 가중치를 곱하고, IMU를 이용해서 추정한 궤적

에 (1-w)의 가중치를 곱해서 이를 더한 값으로 융합된 궤적

를 얻을 수 있다. 물론 이러한 가중치 연산 외에도 다양한 산술 연산을 통해서 궤적

와 궤적

을 융합하여 궤적

를 얻을 수 있다.That is, the trajectory estimated by comparing the feature points

Multiplying by the weight of w and the trajectory estimated using IMU

Multiplied by the weight of (1-w) to add the sum

Can be obtained. Of course, in addition to these weight calculations, trajectories through various arithmetic operations

And trajectory

Trajectory by fusing

Can be obtained.

의 가중치를 높게 설정하고, IMU를 이용해서 추정한 궤적

의 가중치를 낮게 설정할 수 있다.At this time, the weight w may be determined based on the accuracy of the sensor. For example, if the accuracy of the Lidar sensor is higher than that of the IMU sensor, the trajectory estimated by comparing the feature points

The trajectory estimated using the IMU with a high weight of

The weight of can be set low.

The trajectory of the measuring device thus obtained for all measurement points can be expressed by the rotation and movement matrix of the measuring device. As a result, as described above with reference to FIG. 1A, all point groups can be converted into the same coordinate system. That is, the point group at each time point may be converted as shown in Equation 10 by utilizing the relative position and rotation of the measuring device obtained in the trajectory estimation step.

[Equation 10]

Where P is a point group measured by a three-dimensional lidar, R is a rotation matrix of the estimated measuring device, and T is a moving matrix of the estimated measuring device. By estimating the trajectory of the measuring device and using the estimated position information, i.e., the rotation matrix and the moving matrix, the point group obtained at each time point is converted into a standard coordinate system with the origin as the origin of the measuring device. You can create a map.

However, generating a 3D map by accumulating the transformed point groups is inefficient and less accurate. This is due to the error of the IMU sensor, the error of the lidar, etc. In order to increase the accuracy, an additional process is required in the process of accumulating the point group. This is the segment based mapping process of FIG. 2.

는 추정된 궤적을 통해 변환된 점군이고,

는 지면이 제거된 점군의 집합이다.

는 군집화 된 그룹을 나타내며,

는 각 그룹의 특징들의 집합이다.

는 기존 3차원 지도에 저장되어 있던 특징들의 집합을 나타낸다. 마지막으로

는 k번째 샘플까지 누적된 3차원 지도를 의미한다.Here, if you look at the term in relation to the segment-based mapping step,

Is the point group transformed through the estimated trajectory,

Is a set of point groups with ground removed.

Represents a clustered group,

Is a set of features of each group.

Represents a set of features stored in the existing 3D map. Finally

Denotes a three-dimensional map accumulated up to the kth sample.

Referring to the segment-based mapping step with reference to FIG. 2, ground removal is performed on the transformed point group, Euclid segmentation is performed, and the features are compared with the features of the previous 3D map (Feature Matching). ), And accumulate a point group to generate a 3D map (Target 3D Map). In this process, verification is performed on a geometric basis to further increase accuracy (Geometric Verification).

In other words, after clustering and objectifying the point group data, it analyzes how the point group data classified as the same object moves in each measurement sample, accurately predicts the movement of the measuring device, and generates a map using the same. This means that the trajectory estimated in the trajectory estimation step is more precisely estimated. The first step is to remove the ground from the point cloud.

The step of removing ground refers to removing ground data from a point cloud measured every frame. This speeds up the computation and increases the accuracy of the 3D map. For this purpose, the z-axis of the point group measured using the attitude angle value obtained from the IMU sensor combined with the lidar coincides with the earth's gravitational acceleration direction.

Then, using a plane model having a vertical vector on the z-axis part, the coefficients are found so that the number of points and the number of points corresponding to the ground are included the most among the measured point group data. Then, select points that exist within a certain threshold from the plane model, assuming that the selected point groups are land and remove them from the raw data.

Alternatively, the ground may be removed based on the amount of change in angle. Looking more closely at this, first convert the point cloud data into a 2D range image. In this case, the converted range image may be converted into a 2D ange image as shown in Equation 11 below.

[Equation 11]

은 range image에서 (r, c)위치에서의 값이며,

는 angle image를 나타낸다.

는 range image에서 r번째 행에 해당하는 레이저 빔의 수직 각도를 의미하며,

는 range image에서 r-1번째 행에 해당하는 레이저 빔의 수직 각도를 나타낸다.here

Is the value at position (r, c) in the range image,

Indicates an angle image.

Is the vertical angle of the laser beam corresponding to the rth row in the range image.

Denotes the vertical angle of the laser beam corresponding to the r-1th row in the range image.

The generated angle image includes information on the amount of change of angle with surrounding points. That is, starting from the bottom point of the angle image, the breadth-first search (BFS) method, which determines the ground as the angle change with the surrounding pixels, can be extracted and removed.

The ground removed through this process is shown in Figure 5a. 5A is a view for explaining a ground removing process that can be used in one embodiment of the present invention. Referring to FIG. 5A, regions marked with red dots in the center are determined to be ground and removed. Removing the ground and accumulating point groups can benefit from computation and accuracy.

In the preprocessing process, data obtained from the ground is removed and a potential candidate is selected from the data as a reference element for matching. The clustering technique is used here, and the Euclidean distance clustering method can be applied here. In other words, it is called Euclidean segmentation.

This is a method of constructing a measured point in the form of KdTree and then determining that points within a certain threshold distance value around a randomly selected point belong to the same cluster. Since point groups obtained from one object are generally gathered, clustering points based on this method causes objects to be clustered for each object unit.

In this case, clustering is not performed on all points included in the point group, but only the point groups existing at a specific radius R based on the current position of the Lidar may be clustered. This is because, due to the characteristics of LiDAR, the distant objects have a low number of points, which makes them difficult to cluster.

For the point groups within a certain distance from the lidar, the noise is removed and transformed into Voxel Grid coordinate system for efficient data storage. For point clusters converted to Voxel coordinates, Euclidean clustering and region growing segmentation are applied. This allows you to cluster points within a certain distance from the lidar.

Referring to FIG. 5B, the results after performing clustering can be seen. 5B is a diagram illustrating a clustering process that can be used in an embodiment of the present invention. Point groups determined to be the same object are shown in the same color. In this case, the object may be the same as the shape of an object that exists in reality. However, although the object is different from the shape of the object in terms of measurement principle, the objects are grouped and measured.

For example, in the case where the objects separated from each other actually exist in close contact with each other, in the example of FIG. Then, the feature is extracted to know what the clustered object is. Here, the feature may use a variety of things, but may use an eigenvalue-based feature. Eigenvalue-based features are composed of eight, as shown in the following equation (12).

[Equation 12]

Here, e1, e2, and e3 mean eigenvalues normalized in a three-dimensional structure. This allows us to quantify how the data is distributed for each cluster. Since the data measured in series are similar in the overall point cloud distribution, the feature is extracted in this way and compared with the clustered point cloud of the previous sample to show how much the specific object has moved.

For two consecutive Lidar measurements, the above ground removal, object extraction, and feature calculation are performed, and then the matching process is performed for each existing object. This shows how the object clusters extracted from the two samples correspond. The rigid body transformation for the overall lidar movement can then be calculated by calculating how the corresponding points moved within the two samples.

In the previous trajectory estimation step, the trajectory was estimated for all points of the second sample by comparing with the first sample, which was the previous sample. Estimate the trajectory of the measuring device. This can increase the accuracy of the trajectory estimated in the trajectory estimation step.

This is possible only under the assumption that the object represented by the clustered point cloud is actually fixed and not moving. Of course, in reality, there are moving objects in addition to the measuring device, but the surrounding environment, such as buildings, is much more fixed. Therefore, even if the trajectory is estimated based on the clustered point group, the accuracy is high. As will be described later, a pair corresponding to an outlier among the matched pairs is excluded from the trajectory estimation, thereby increasing accuracy.

In order to precisely attach the transformed current point group to the existing 3D map using the estimated trajectory, matching is performed by comparing the features extracted from the current point group with the features stored in the existing 3D map. In this case, in order to perform the comparison effectively, the K-D Tree search method may be used. Matching candidate pairs sorted by the K-D Tree search method are used as inputs to the SVM classifier. The SVM classifier compares each feature and determines that the sum of the differences of each feature preferentially matches the smallest candidates.

Finally, to find out mismatched groups and to reflect them, random sample consensus (RANSAC) is used to remove matching pairs corresponding to outliers. If M matches among the N matching pairs are determined to be outliers, a new six-degree-of-freedom transformation matrix is newly estimated using (N-M) matching pairs. In this process, matched pairs of non-fixed objects are excluded as described above. The current point group is transformed using the estimated transformation matrix, and then added to the existing 3D map. By repeating this every frame, the three-dimensional map can be updated continuously.

6 is a pseudo code for explaining a segment-based mapping process that can be used in an embodiment of the present invention.

The segment-based map mapping process described above is represented by a pseudo code as shown in FIG. 6. Referring to FIG. 6, points having a small amount of change in angle are removed through the angle image as the ground, and Euclidean clustering is performed on the points that are not the ground, and then region growing segmentation is performed. Next, the point groups are accumulated on the existing three-dimensional map by comparing the eigenvalue-based features.

Using the map generation method proposed in the present invention, since the movement amount of the lidar can be precisely obtained in consideration of the change in the position of the points measured at short and medium distances, the accuracy similar to the case of using a high-precision GPS-based map generation method is obtained. It is possible to freely generate a three-dimensional point cloud map indoors and outdoors while securing.

That is, the trajectory can be estimated using the measured value of the IMU, but the error tends to be amplified as the moving distance becomes longer due to the characteristic of acceleration. In the present invention, the measured value of the IMU is one cycle as in the case of FIG. It is used to compensate for short time distortion, and the trajectory of the actual measuring device is estimated by comparing the point groups. As a result, the accuracy of the trajectory can be improved compared to the conventional method of estimating the trajectory using the IMU measurement value or the trajectory using the GPS.

In addition, it stores the position of the reference of the entire map coordinate system of the group of objects and the feature diagram for each object, so that loop closure can be performed using this. This reduces the effects of drift in long distance measurements, ensuring accuracy even in long distance measurements.

That is, in the present invention, in order to expand the three-dimensional map, the point groups in the three-dimensional map are divided into objects through clustering, and thus, when the first starting point using the measuring device and the arrival point ending the measurement are the same, the loop combination is performed. By matching each object recognized from the point group obtained from the starting point and the point group obtained from the starting point, the longer the moving distance can reduce the error that amplifies the longer.

7A to 8B are diagrams for comparing the performance of the map generating method according to an embodiment of the present invention with the conventional method.

Referring to FIGS. 7A and 8A, a three-dimensional map can be seen when the map generation method proposed by the present invention is applied. In contrast, referring to FIGS. 7B and 8B, a 3D map generated based on a conventional GPS may be viewed. In particular, when looking at the point group of the red square region of Figures 8a and 8b it can be seen that the method group proposed in the present invention accumulates the point group in the form of less error.

9A to 9C are diagrams for describing an apparatus for generating a map, according to an exemplary embodiment.

9A, a lidar sensor and an IMU sensor and a circuit for controlling the same are shown. 9B and 9C, an input / output port for transmitting and receiving data is shown. The measurement device configured as described above may be attached to the upper portion of the vehicle and moved, or a three-dimensional map may be generated by measuring a point group while moving through a person or a robot indoors.

10A to 10C are flowcharts illustrating a method for generating a map according to an embodiment of the present invention.

Referring to FIG. 10A, first, a point group is collected by a lidar sensor, and an IMU sensor value at the time of measuring a corresponding point for each point belonging to the point group is collected (S1100). Next, each point belonging to the point group is converted into a reference coordinate system using the sensor value of the IMU (S1200). Here, the reference coordinate system is a coordinate system based on the initial position and posture of the measurement of the lidar device within one period.

In order to convert the point group into the reference coordinate system, the displacement is calculated using the acceleration measured by the IMU sensor, and the distortion is compensated by reflecting the displacement. In this way, even if the lidar moves during the measurement, it is possible to offset the distortion caused by the movement of the lidar within a period. In this case, when the measurement period of the IMU sensor value and the measurement period of the lidar device do not coincide with each other, a ratio may be obtained to compensate for the distortion.

Next, the movement path, that is, the trajectory of the lidar, is estimated based on the feature points of the point group (S1300). In this process, the accuracy of the trajectory can be improved by using the sensor value of the IMU. In more detail, the Lidar estimates the trajectory by comparing the characteristics of the point group of the first sample and the point group of the second sample (S1310). Here, the point group of the first sample is a point group collected by one measurement because the lidar collects information in all directions while rotating once, and may also be referred to as a point group of the first frame.

Since the process of comparing the point group of the first sample and the point group of the second sample is to compare the entire point group included in the sample, for convenience of comparison, first, the point group corresponding to the point group corresponding to the corner and the point group corresponding to the plane Separate by.

The point corresponding to the edge of the second sample is a moving matrix that can project the point corresponding to the edge of the second sample from the point sample of the first sample by projecting on the line created by selecting two points closest to the point group of the first sample. Deduce the equation for the rotation matrix.

Similarly, the point corresponding to the plane of the second sample is a moving matrix in which a point corresponding to the plane of the second sample can be drawn from the point cloud of the first sample by projecting onto a plane made by selecting three points closest to the point cloud of the first sample. And a formula for the rotation matrix.

Deriving a nonlinear equation for all the points belonging to the second sample and using an optimization technique to obtain a movement matrix and a rotation matrix for the points of the first sample to correspond to the points of the second sample. A movement path, i.e., a trajectory, which is a difference between the position of the measuring device and the position of the measuring device at the time of measuring the second sample can be estimated.

Of course, the trajectory can be estimated through the IMU sensor value combined with LiDAR, but since the estimation can be more accurate based on the comparison of the feature points of the point group, the trajectory estimation and the IMU sensor value are used to compensate for this. It is used to increase the accuracy of the point group estimated by (S1320).

Finally, a point group transformation is performed using the rotation matrix R and the movement matrix T obtained in the trajectory estimation process, thereby unifying the point group of the first sample and the point group of the second sample into the same standard coordinate system. Through this, the preparation for accumulating the point group of the first sample and the point group of the second sample is completed (S1330).

Of course, it is possible to generate a three-dimensional map with a certain degree of accuracy by simply accumulating the transformed point group as it is, but in order to improve the accuracy, the object of each sample is identified and a three-dimensional map is created based on the identified object. do. This is a segment based matching step (S1400).

In order to perform segment-based matching, the ground is first removed by preprocessing (S1410). The process of removing the ground is determined by removing the plane that contains the most points perpendicular to the direction of gravity acceleration as the ground, or converting the point group to an angle image, and then removing the ground with a small change in angle compared to the surrounding points. It can be removed by judging.

Next, clustering is performed on the point groups within a predetermined distance in the lidar based on the distance from the other points (S1420). This corresponds to Euclidean segmentation. Next, matching is performed by comparing feature points of the cluster of the first sample and the cluster of the second sample (S1430). When the first sample and the second sample are superimposed so that the matched clusters correspond to each other, the 3D map made of the point group may be expanded (S1450).

In this process, geometric verification may be performed (S1450). The geometry verification process removes outliers by applying RANSAC to matched pairs in matching a cluster of a first sample and a cluster of a second sample, and obtains a transformation matrix supported by the most matching pairs. Thereafter, the point group of the second sample is transformed through the transformation matrix and merged into the point group of the first sample.

Although embodiments of the present invention have been described above with reference to the accompanying drawings, those skilled in the art to which the present invention pertains may implement the present invention in other specific forms without changing the technical spirit or essential features thereof. I can understand that. Therefore, it should be understood that the embodiments described above are exemplary in all respects and not restrictive.

Claims (4)

The map generating apparatus compares the characteristics of the first point group measured at the first point with the characteristics of the second point group measured at the second point, and is a trajectory (Odometry) which is a moving path from the first point to the second point. A trajectory estimating step of estimating;
Positioning and rotating transforming the second point group based on the trajectory so that the map generating device has the same coordinate system as the first point group; And
The map generating apparatus compares a feature of the clustered object of the first point group with a feature of the clustered object of the second point group, and accumulates the second point group on the first point group to generate a point group map. A matching step,
The trajectory estimation step,
Calculating curvature for all points belonging to the second point group, and classifying the curvature into points belonging to an edge and points belonging to a plane by comparing with a preset threshold;
The point belonging to the corner among the points belonging to the second point group is projected on the line of the first point group, and the point belonging to the plane among the points belonging to the second point group is projected on the plane of the first point group, Generating 9; And
[Equation 9]

(here,

Is the rotation matrix of the trajectory,

Is the moving matrix of the trajectory)
Using the optimization technique to minimize the d

And

Inducing; comprising;
The segment-based matching step,
The point group corresponding to the ground in the second point group includes the step of removing,
How to create a Lidar map containing a point cloud.
The map generating apparatus compares the characteristics of the first point group measured at the first point with the characteristics of the second point group measured at the second point, and is a trajectory (Odometry) which is a moving path from the first point to the second point. A trajectory estimating step of estimating;
Positioning and rotating transforming the second point group based on the trajectory so that the map generating device has the same coordinate system as the first point group; And
The map generating apparatus compares a feature of the clustered object of the first point group with a feature of the clustered object of the second point group, and accumulates the second point group on the first point group to generate a point group map. A matching step,
The trajectory estimation step,
Calculating curvature for all points belonging to the second point group, and classifying the curvature into points belonging to an edge and points belonging to a plane by comparing with a preset threshold;
The point belonging to the corner among the points belonging to the second point group is projected on the line of the first point group, and the point belonging to the plane among the points belonging to the second point group is projected on the plane of the first point group, Generating 9; And
[Equation 9]

(here,

Is the rotation matrix of the trajectory,

Is the moving matrix of the trajectory)
Using the optimization technique to minimize the d

And

Inducing; comprising;
The segment-based matching step,
Points within a predetermined radius from the second point includes performing clustering based on Euclidean distance based clustering,
How to create a Lidar map containing a point cloud.
The map generating apparatus compares the characteristics of the first point group measured at the first point with the characteristics of the second point group measured at the second point, and is a trajectory (Odometry) which is a moving path from the first point to the second point. A trajectory estimating step of estimating;
Positioning and rotating transforming the second point group based on the trajectory so that the map generating device has the same coordinate system as the first point group; And
The map generating apparatus compares a feature of the clustered object of the first point group with a feature of the clustered object of the second point group, and accumulates the second point group on the first point group to generate a point group map. A matching step,
The trajectory estimation step,
Calculating curvature for all points belonging to the second point group, and classifying the curvature into points belonging to an edge and points belonging to a plane by comparing with a preset threshold;
The point belonging to the corner among the points belonging to the second point group is projected on the line of the first point group, and the point belonging to the plane among the points belonging to the second point group is projected on the plane of the first point group, Generating 9; And
[Equation 9]

(here,

Is the rotation matrix of the trajectory,

Is the moving matrix of the trajectory)
Using the optimization technique to minimize the d

And

Inducing; comprising;
The segment-based matching step,
Overlapping the cluster of the first point group and the cluster of the second point group to derive a matching pair of point groups, applying RANSAC to the matching pairs, and obtaining a transformation matrix supported by the most matching pairs,
How to create a Lidar map containing a point cloud.
The method of claim 3,
Obtaining the transformation matrix,
Converting the second point group by using the transformation matrix, and accumulating the converted second point group in the first point group,
How to create a Lidar map containing a point cloud.

Images