CN114485683B - Local fragment map matching method and device based on Monte Carlo optimization - Google Patents

Abstract

The present invention relates to a local fragment map matching method and device based on Monte Carlo optimization. The method includes: defining an energy function for evaluating the degree of matching; calculating the areas of local fragment maps A and B and their overlapping areas into Calculate the initial energy value in the energy function; perform random translation or rotation on the local fragment map B, substitute into the energy function to calculate a new energy value, compare the new energy value with the initial energy value, if the new energy value is lower, accept The current translation or rotation operation of the local fragment map B, otherwise, the current translation or rotation operation of the partial fragment map B is rejected. The matching problem between complex geometric shapes on a two-dimensional plane is transformed into an iterative solution problem through the Monte Carlo sampling method. The algorithm is simple to implement and does not depend on specific matching rules.

Description

A local fragment map matching method and device based on Monte Carlo optimization

Technical Field

The present invention relates to the field of autonomous driving technology, and in particular to a local fragment map matching method and device based on Monte Carlo optimization in an automated mapping scenario of an autonomous driving vehicle.

Background Art

The planning capability of autonomous vehicles is based on lane-level high-precision maps. Traditional high-precision map acquisition equipment based on lidar and high-precision positioning sensors is very expensive, and it is difficult to achieve real-time map updates. The solution using visual and inertial navigation sensors can greatly reduce the cost of map production, but the error of single-point GPS devices on vehicles is usually large, and optimization methods in SLAM need to be used to eliminate or smooth the accumulated error. When using optimization-based methods, it is necessary to determine the correlation between data and the relative relationship between the vehicle's postures at different times. In order to obtain this correspondence, we need to match the vehicle fragment map data. Traditional lidar point clouds usually use the iterative closest point (ICP) method to optimize the matching relationship between two point clouds. In scenarios with rich feature points and few perception errors, the correspondence between points can be well estimated, but the semantic geometric data obtained through visual perception usually consists of geometric shapes composed of only a small number of points (feature points are not rich), so the ICP method usually has poor results and does not meet this application scenario.

Summary of the invention

In view of the technical problems existing in the prior art, the present invention provides a local fragment map matching method and device based on Monte Carlo optimization, which adopts enhanced sampling to estimate the matching relationship between two sets of data. When the data quality is good, the method can match the fragment map data between each other more stably. The algorithm is simple to implement and does not rely on specific matching rules.

The technical solution of the present invention to solve the above technical problems is as follows:

In a first aspect, the present invention provides a local fragment map matching method based on Monte Carlo optimization, comprising the following steps:

S1, defining an energy function for evaluating the degree of matching, wherein the energy function takes the areas of the two local fragment maps involved in the matching and their overlapping areas as variables;

S2, for the two local fragment maps A and B to be matched, calculating the areas of the local fragment maps A and B and their overlapping areas, and substituting them into the energy function to calculate the initial energy value;

S3, randomly translate or rotate the local fragment map B, substitute it into the energy function to calculate the new energy value, compare the new energy value with the energy threshold, if the new energy value is less than or equal to the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and output the translated or rotated local fragment map B, otherwise, execute step S4;

S4, compare the new energy value with the initial energy value. If the new energy value is lower, accept the translation or rotation operation of the local fragment map B and jump to step S3. Otherwise, reject the translation or rotation operation of the local fragment map B and execute step S5.

S5, judging whether the number of consecutive rejections reaches a preset number, if so, it is considered that the energy function has converged, and the local fragment map B before the consecutive rejections is output, otherwise jump to step S3.

Furthermore, step S2 also includes determining a lateral direction and a longitudinal direction of the local fragment map B according to the lane line direction, wherein the lateral direction is parallel to the lane line direction.

Furthermore, the random translation or rotation of the local fragment map B includes:

Generate a random number x between [0,1];

If 0≤x≤0.33, the local fragment map B is translated horizontally by a random distance;

If 0.33＜x≤0.66, the local fragment map B is translated vertically by a random distance;

If 0.66＜x≤1, rotate the local fragment map B by a random angle;

The random distance and the random angle are both set in a value range according to the matching accuracy requirement.

Furthermore, step S2 also includes comparing the initial energy value with an energy threshold. If the initial energy value is less than or equal to the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and the translated or rotated local fragment map B is output.

Further, the step S4 includes:

Compare the new energy value with the initial energy value. If the new energy value is higher, the acceptance probability is calculated based on the energy difference using the Mayor algorithm.

Whether to accept the translation or rotation operation of the local fragment map B is determined according to the acceptance probability.

Furthermore, judging whether to accept the translation or rotation operation of the local fragment map B according to the acceptance probability includes:

Generate a random number y between [0,1];

Compare the random number y with the received probability;

If the random number y is less than the acceptance probability, the translation or rotation operation of the local fragment map B is accepted;

Otherwise, the translation or rotation operation of the local fragment map B is rejected.

Furthermore, the energy function is shown in the following formula:

Where A _i and B _j are two overlapping geometric shapes in the local fragment maps A and B, respectively; the Overlap function is used to calculate the overlapping area between two geometric shapes, and the Area function is used to calculate the area of a single geometric shape.

In a second aspect, the present invention provides a local fragment map matching device based on Monte Carlo optimization, comprising:

A function definition module, defining an energy function for evaluating the degree of matching, wherein the energy function takes the areas of two local fragment maps involved in the matching and their overlapping areas as variables;

An energy value calculation module, used to calculate the areas of local fragment maps A and B and their overlapping areas, and substitute them into the energy function to calculate the initial energy value; used to calculate the energy values of B and A after translation or rotation;

A local fragment map posture adjustment module is used to randomly translate or rotate the local fragment map B;

A comparison and judgment module is used to compare the new energy value with the initial energy value, to compare the new energy value with the energy threshold, to judge whether to accept or reject the translation or rotation operation of the local fragment map B according to the comparison result, and to judge whether the number of consecutive rejections reaches a preset number.

In a third aspect, the present invention provides an electronic device, comprising:

Memory for storing computer software programs;

The processor is used to read and execute the computer software program, thereby implementing the local fragment map matching method based on Monte Carlo optimization described in the first aspect of the present invention.

In a fourth aspect, the present invention provides a non-transitory computer-readable storage medium, wherein the storage medium stores a computer software program for implementing a local fragment map matching method based on Monte Carlo optimization as described in the first aspect of the present invention.

The beneficial effects of the present invention are as follows: the present invention transforms the matching problem between complex geometric shapes on a two-dimensional plane into an iterative solution problem through the Monte Carlo sampling method, which can avoid encoding complex rules in the algorithm. By adjusting the virtual temperature factor, we can control whether to adopt a conservative matching strategy or an aggressive matching strategy. When the two local geometric maps are not far apart, the algorithm can naturally search for the best matching result.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic flow chart of a local fragment map matching method based on Monte Carlo optimization provided by an embodiment of the present invention;

FIG2 is a schematic diagram of the structure of a local fragment map matching device based on Monte Carlo optimization provided by an embodiment of the present invention;

FIG3 is a schematic diagram of an embodiment of an electronic device provided by an embodiment of the present invention;

FIG4 is a schematic diagram of an embodiment of a computer-readable storage medium provided by an embodiment of the present invention;

FIG5 is a schematic diagram of local fragment maps A and B provided by an embodiment of the present invention.

DETAILED DESCRIPTION

The principles and features of the present invention are described below in conjunction with the accompanying drawings. The examples given are only used to explain the present invention and are not used to limit the scope of the present invention.

The two sets of local geometric fragment data A and B collected by existing vehicles are shown in Figure 5. They contain traffic elements such as lane line contours, ground arrows, zebra crossings, stop lines and aerial signs observed by the vehicle. These traffic elements are composed of polygons. Due to observation errors, the observed geometric shapes may be different from the actual ones, and there may be certain differences between multiple observations. When measuring vehicles, single-point GPS with low accuracy is used. Their accuracy is usually 2 to 10 meters, which is not enough to achieve lane-level matching. Moreover, in locations with poor GPS signals, its accuracy will be greatly degraded, so there is a certain position difference between multiple observations of the same location by the vehicle. However, the relative positions between multiple geometric features observed by the vehicle at the same time are relatively accurate, that is, relative accuracy is met. Now it is necessary to estimate the correspondence between A and B and the affine transformation relationship between them. Since we have determined the coarse-grained matching relationship between the fragment data, that is, we know which fragment data are in the same section, so we only need to solve this problem in two-dimensional space. The problem is simplified to finding the transformation relationship between two rigid bodies with similar shapes and composed of multiple geometric shapes. This problem seems simple, but in fact there are many uncertainties, and it is difficult to get the optimal solution through a deterministic algorithm.

FIG1 is a flow chart of a local fragment map matching method based on Monte Carlo optimization provided by an embodiment of the present invention. As shown in FIG1 , the method includes the following steps:

S1, define an energy function for evaluating the quality of matching, wherein the energy function takes the areas of the two local fragment maps involved in the matching and their overlapping areas as variables; the energy function reflects the degree of mismatch, the lower the better.

The energy function Objective is as follows:

Where A _i and B _j are two overlapping geometric shapes in the local fragment maps A and B, respectively; the Overlap function is used to calculate the overlapping area between two geometric shapes, and the Area function is used to calculate the area of a single geometric shape.

If the two geometric shapes completely overlap, the value of the objective function is 1, and if they do not overlap at all, it is 0. The matching between two fragmented geometric data is to minimize the energy function under the premise of satisfying non-mutual exclusion of the overlapping area between all geometric elements.

S2, for the two local fragment maps A and B to be matched, calculating the areas of the local fragment maps A and B and their overlapping areas, and substituting them into the energy function to calculate the initial energy value;

After the initial energy value is calculated here, it is necessary to compare the initial energy value with the energy threshold. If the initial energy value ≤ the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and the translated or rotated local fragment map B is output without the need for subsequent steps.

S3, on a two-dimensional plane, there are two possible ways to move a local fragment map: translation and rotation. Since lane lines are almost everywhere in road geometry data, they have obvious local straight line characteristics, so translation can be divided into translation along the lane line and translation perpendicular to the lane line. The former is called longitudinal translation and the latter is called lateral translation. Counterclockwise rotation is positive and clockwise rotation is negative.

In this embodiment, A is kept fixed, and the local fragment map B is randomly translated or rotated. The area of the translated or rotated local fragment map B and the fixed local fragment map A and their overlapping area are substituted into the energy function to calculate a new energy value. The new energy value is compared with the energy threshold. If the new energy value is less than or equal to the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and the translated or rotated local fragment map B is output. Otherwise, step S4 is executed.

S4, compare the new energy value with the initial energy value. If the new energy value is lower, accept the translation or rotation operation of the local fragment map B and jump to step S3. Otherwise, reject the translation or rotation operation of the local fragment map B and execute step S5.

S5, judging whether the number of consecutive rejections reaches a preset number, if so, it is considered that the energy function has converged, and the local fragment map B before the consecutive rejections is output, otherwise jump to step S3.

Preferably, in some embodiments, the random translation or rotation of the local fragment map B in step S3 includes:

Generate a random number x between [0,1];

If 0≤x≤0.33, the local fragment map B is translated horizontally by a random distance;

If 0.33＜x≤0.66, the local fragment map B is translated vertically by a random distance;

If 0.66＜x≤1, rotate the local fragment map B by a random angle;

The random distance and random angle are both set in a value range according to the matching accuracy requirement. For example, if the matching accuracy requirement is 0.05 meters, the random distance of translation should be between 0.01 and 0.1 meters.

Preferably, in some embodiments, after comparing the new energy value with the initial energy value in step S4, if the new energy value is lower, the acceptance probability is calculated using the Mayor algorithm based on the energy difference, and then a determination is made based on the acceptance probability whether to accept the translation or rotation operation of the local fragment map B.

The specific method of calculating the acceptance probability using the mayor algorithm is as shown in the following formula:

_Pacc = αe ^{- βTΔE}

Among them, α and β are two constants, ΔE is the energy difference after the state update, and T is the virtual temperature of the system. The higher the temperature, the more active the system is, and the more likely it is to reach a high energy state through state update, and vice versa, the more difficult it is to cross the energy barrier. By adjusting this factor, we can control the acceptance rate of the update. Generally speaking, we need to make this acceptance probability reach 50%, otherwise the system may fall into a local energy minimum state.

Generate a random number y between [0,1];

Compare the random number y with the received probability;

If the random number y is less than the acceptance probability, the translation or rotation operation of the local fragment map B is accepted;

Otherwise, the translation or rotation operation of the local fragment map B is rejected.

The present invention transforms the matching problem between complex geometric shapes on a two-dimensional plane into an iterative solution problem through the Monte Carlo sampling method, which can avoid encoding complex rules in the algorithm. By adjusting the virtual temperature factor, we can control whether to adopt a conservative matching strategy or an aggressive matching strategy. When the two local geometric maps are not far apart, the algorithm can naturally search for the best matching result.

FIG2 is a schematic diagram of the structure of a local fragment map matching device based on Monte Carlo optimization provided by an embodiment of the present invention. As shown in FIG2 , the device includes:

A function definition module, defining an energy function for evaluating the degree of matching, wherein the energy function takes the areas of two local fragment maps involved in the matching and their overlapping areas as variables;

An energy value calculation module, used to calculate the areas of local fragment maps A and B and their overlapping areas, and substitute them into the energy function to calculate the initial energy value; used to calculate the energy values of B and A after translation or rotation;

A local fragment map posture adjustment module is used to randomly translate or rotate the local fragment map B;

A comparison and judgment module is used to compare the new energy value with the initial energy value, to compare the new energy value with the energy threshold, to judge whether to accept or reject the translation or rotation operation of the local fragment map B according to the comparison result, and to judge whether the number of consecutive rejections reaches a preset number.

Please refer to FIG3, which is a schematic diagram of an embodiment of an electronic device provided by an embodiment of the present invention. As shown in FIG3, an embodiment of the present invention provides an electronic device 500, including a memory 510, a processor 520, and a computer program 511 stored in the memory 520 and executable on the processor 520. When the processor 520 executes the computer program 511, the following steps are implemented:

S1, defining an energy function for evaluating the degree of matching, wherein the energy function takes the areas of the two local fragment maps involved in the matching and their overlapping areas as variables;

S2, for the two local fragment maps A and B to be matched, calculating the areas of the local fragment maps A and B and their overlapping areas, and substituting them into the energy function to calculate the initial energy value;

S3, randomly translate or rotate the local fragment map B, substitute it into the energy function to calculate the new energy value, compare the new energy value with the energy threshold, if the new energy value is less than or equal to the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and output the translated or rotated local fragment map B, otherwise, execute step S4;

S4, compare the new energy value with the initial energy value. If the new energy value is lower, accept the translation or rotation operation of the local fragment map B and jump to step S3. Otherwise, reject the translation or rotation operation of the local fragment map B and execute step S5.

S5, judging whether the number of consecutive rejections reaches a preset number, if so, it is considered that the energy function has converged, and the local fragment map B before the consecutive rejections is output, otherwise jump to step S3.

Please refer to Figure 4, which is a schematic diagram of an embodiment of a computer-readable storage medium provided by an embodiment of the present invention. As shown in Figure 4, this embodiment provides a computer-readable storage medium 600, on which a computer program 611 is stored. When the computer program 611 is executed by a processor, the following steps are implemented:

S1, defining an energy function for evaluating the degree of matching, wherein the energy function takes the areas of the two local fragment maps involved in the matching and their overlapping areas as variables;

S2, for the two local fragment maps A and B to be matched, calculating the areas of the local fragment maps A and B and their overlapping areas, and substituting them into the energy function to calculate the initial energy value;

S3, randomly translate or rotate the local fragment map B, substitute it into the energy function to calculate the new energy value, compare the new energy value with the energy threshold, if the new energy value is less than or equal to the energy threshold, it is considered that the translated or rotated local fragment map B matches the local fragment map A, and output the translated or rotated local fragment map B, otherwise, execute step S4;

S4, compare the new energy value with the initial energy value. If the new energy value is lower, accept the translation or rotation operation of the local fragment map B and jump to step S3. Otherwise, reject the translation or rotation operation of the local fragment map B and execute step S5.

S5, judging whether the number of consecutive rejections reaches a preset number, if so, it is considered that the energy function has converged, and the local fragment map B before the consecutive rejections is output, otherwise jump to step S3.

It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

The present invention is described with reference to the flowchart and/or block diagram of the method, device (system), and computer program product according to the embodiment of the present invention. It should be understood that each process and/or box in the flowchart and/or block diagram, as well as the combination of the process and/or box in the flowchart and/or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for implementing the functions specified in one process or multiple processes in the flowchart and/or one box or multiple boxes in the block diagram.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device that implements the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims (10)

1. The local fragment map matching method based on Monte Carlo optimization is characterized by comprising the following steps of:

s1, defining an energy function Objective for evaluating the matching quality, wherein the energy function takes the area of two partial fragment maps participating in matching and the overlapping area of the two partial fragment maps as variables;

s2, calculating the areas of the partial fragment maps A and B and the overlapping areas of the partial fragment maps A and B aiming at the two partial fragment maps A and B to be matched, substituting the areas into the energy function, and calculating an initial energy value;

s3, carrying out random translation or rotation on the partial fragment map B, substituting an energy function to calculate a new energy value, comparing the new energy value with an energy threshold value, if the new energy value is less than or equal to the energy threshold value, considering that the translated or rotated partial fragment map B is matched with the partial fragment map A, outputting the translated or rotated partial fragment map B, otherwise, executing the step S4;

s4, comparing the new energy value with the initial energy value, if the new energy value is lower, receiving the translation or rotation operation of the local fragment map B, and jumping to the step S3, otherwise, refusing the translation or rotation operation of the local fragment map B and executing the step S5;

and S5, judging whether the continuous rejection times reach the preset times, if so, considering that the energy function is converged, outputting the partial fragment map B before continuous rejection, otherwise, jumping to the step S3.

2. The method according to claim 1, wherein step S2 further comprises determining a lateral direction and a longitudinal direction of the partial fragment map B according to the lane line direction, the lateral direction being parallel to the lane line direction.

3. The method according to claim 1 or 2, wherein said randomly translating or rotating the partial fragment map B comprises:

generating a random number x between [0,1 ];

if x is more than or equal to 0 and less than or equal to 0.33, transversely shifting the partial fragment map B by a random distance;

if x is more than 0.33 and less than or equal to 0.66, longitudinally translating the partial fragment map B by a random distance;

if x is more than 0.66 and less than or equal to 1, rotating the local fragment map B by a random angle;

and setting a value range according to the matching precision requirement by the random distance and the random angle.

4. The method according to claim 1, wherein step S2 further comprises comparing the initial energy value with the energy threshold, and if the initial energy value is less than or equal to the energy threshold, considering the translated or rotated partial fragment map B to match the partial fragment map a, and outputting the translated or rotated partial fragment map B.

5. The method according to claim 1, wherein the step S4 comprises:

comparing the new energy value with the initial energy value, and calculating the acceptance probability by using the city length algorithm based on the energy difference value if the new energy value is higher;

and judging whether the local fragment map B is subjected to the translation or rotation operation at the time according to the acceptance probability.

6. The method of claim 5, wherein said determining whether to accept the partial fragment map B for the translation or rotation based on the acceptance probability comprises:

generating a random number y between [0,1 ];

comparing the size between the random number y and the receiving probability;

if the random number y is smaller than the receiving probability, the local fragment map B is received to perform the translation or rotation operation for the time;

otherwise, the local fragment map B is rejected for this translation or rotation operation.

7. The method of claim 1, wherein the energy function is represented by the formula:

wherein A is _i And B _j Two overlapping geometric figures in the partial fragment maps A and B respectively; the overlay function is used to calculate the Overlap Area between two geometries and the Area function is used to calculate the Area of a single geometry.

8. A monte carlo optimization-based local fragment map matching apparatus, comprising:

the function definition module is used for defining an energy function for evaluating the matching quality degree, and the energy function takes the area of two partial fragment maps participating in matching and the overlapping area of the two partial fragment maps as variables;

the energy value calculation module is used for calculating the areas of the partial fragment maps A and B and substituting the overlapping areas of the partial fragment maps A and B into the energy function to calculate an initial energy value; the energy values of B and A after translation or rotation are calculated;

the local fragment map posture adjustment module is used for randomly translating or rotating the local fragment map B;

the comparison judging module is used for comparing the new energy value with the initial energy value, comparing the new energy value with the energy threshold value, judging whether to accept or reject the translation or rotation operation of the local fragment map B according to the comparison result, and judging whether the continuous rejection times reach the preset times or not.

9. An electronic device, comprising:

a memory for storing a computer software program;

a processor for reading and executing the computer software program to further implement a partial fragment map matching method based on monte carlo optimization as defined in any one of claims 1 to 7.

10. A non-transitory computer readable storage medium having stored therein a computer software program for implementing a monte carlo optimization based local fragment map matching method of any one of claims 1-7.

Images