A method, medium, and system for constructing a two-dimensional grid map based on a laser radar

Abstract

Embodiments of the present application disclose a kind of two-dimensional grid map construction method, medium and system based on laser radar.The two-dimensional grid map construction method based on laser radar includes: obtaining the first two-dimensional point cloud data of target region scanned by laser radar on target autonomous body at first moment;According to the first two-dimensional point cloud data, obtain the first pose information of the target autonomous body;Predict the second two-dimensional point cloud data obtained by the laser radar when the target autonomous body is in the first pose information;According to the first two-dimensional point cloud data and the second two-dimensional point cloud data, the first pose information is optimized to obtain the second pose information;According to the second pose information, the two-dimensional grid map of the target region is constructed.The embodiments of the present application realize the construction of high-precision two-dimensional grid map with high efficiency and low occupation.

Description

[0001] This application is a divisional application of the invention patent application filed on March 27, 2020, with application number 202010228792.3 entitled "A Method and System for Constructing Two-Dimensional Grid Maps Based on LiDAR". Technical Field

[0002] The embodiments of the present invention relate to positioning and mapping technologies, and more particularly to a method, medium, and system for constructing two-dimensional grid maps based on lidar. Background Technology

[0003] As humanity enters the information industry revolution era, artificial intelligence technology is developing rapidly, and intelligent robot technology is innovating at an unprecedented speed, gradually penetrating various industries.

[0004] Localization and mapping technology is an indispensable core module in fields such as intelligent robots and autonomous driving. This technology can tell the robot the location of its body and guide it to move and avoid obstacles.

[0005] Currently, commonly used localization and mapping techniques in engineering include Gmapping and Cartographer, which utilize particle filtering and graph optimization techniques respectively to achieve 2D localization and mapping. Gmapping employs adaptive Monte Carlo localization, using a large number of particles to represent the robot's possible poses. Each particle represents a possible assumption of the robot's pose in real space, and during movement, it is necessary to update the state of all particles and maintain the pose of each particle as well as the map. Although this algorithm can achieve good localization and mapping, it is very time-consuming and consumes a lot of computer resources. Summary of the Invention

[0006] This invention provides a method, medium, and system for constructing two-dimensional grid maps based on lidar, so as to achieve efficient and low-occupancy construction of high-precision two-dimensional grid maps.

[0007] To achieve this objective, embodiments of the present invention provide a method for constructing a two-dimensional grid map based on lidar, the method comprising:

[0008] The first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment is acquired.

[0009] The first pose information of the target autonomous entity is obtained based on the first two-dimensional point cloud data;

[0010] Predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information;

[0011] The first pose information is optimized based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information;

[0012] A two-dimensional grid map of the target area is constructed based on the second pose information.

[0013] Furthermore, the acquisition of the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment includes:

[0014] When the lidar on the target body is a single-line lidar, the first two-dimensional point cloud data of the target area at the first moment is directly obtained through the lidar.

[0015] The lidar on the target body is a multi-line lidar. The first laser in the multi-line lidar is identified at a preset emission angle, and the point cloud data acquired by the first laser is projected onto a horizontal plane according to the preset emission angle to obtain the first two-dimensional point cloud data of the target area at the first moment.

[0016] Furthermore, obtaining the first pose information of the target autonomous entity based on the first two-dimensional point cloud data includes:

[0017] The third two-dimensional point cloud data of the target area obtained by the lidar at the second time point is the time point before the first time point.

[0018] Obtain the third pose information of the target from the subject at the second time point;

[0019] The first two-dimensional point cloud data and the third two-dimensional point cloud data are matched according to the NDT algorithm to obtain the first relative pose relationship between the first time and the second time.

[0020] The first pose information of the target autonomous body is determined based on the first relative pose relationship and the third pose information.

[0021] Furthermore, the step of matching the first two-dimensional point cloud data and the third two-dimensional point cloud data according to the NDT algorithm to obtain the first relative pose relationship at the first time point and the second time point includes:

[0022] Obtain the target's moving speed from the subject at the second moment;

[0023] Predict the fourth pose information of the target from the first moment based on the moving speed and the third pose information;

[0024] Based on the fourth pose information, the NDT algorithm is used to match the first two-dimensional point cloud data and the third two-dimensional point cloud data to obtain the first relative pose relationship between the first time and the second time.

[0025] Furthermore, the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information includes:

[0026] Obtain the performance parameters of the lidar and the pre-established two-dimensional environment map;

[0027] Based on the performance parameters and the two-dimensional environment map, the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information is predicted.

[0028] Furthermore, the step of optimizing the first pose information based on the second two-dimensional point cloud data to obtain the second pose information includes:

[0029] Confirm the mapping relationship between the first pose information and the second two-dimensional point cloud data;

[0030] The first pose information is adjusted according to the mapping relationship to obtain the second pose information when the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data is minimized.

[0031] Furthermore, confirming the mapping relationship between the first pose information and the second two-dimensional point cloud data includes:

[0032] Define a mapping function f i (x), where f i The value of (x) represents the second two-dimensional point cloud data theoretically obtained by the lidar when the pose information is x, where x represents the pose information and i represents the number of times the target's pose information is determined.

[0033] Furthermore, adjusting the first pose information according to the mapping relationship to obtain the second pose information when the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data is minimized includes:

[0034] Define an error function e i (x), where e i (x)=f i (x)-z i e i The value of (x) represents the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data, z i This represents the first two-dimensional point cloud data;

[0035] Define an objective function F(x), where The objective function F(x) is given by the error function e i The covariance matrix is ​​obtained by squaring (x) and then taking its covariance matrix.

[0036] Adjust x to obtain x when the objective function F(x) is minimized. min and the x min This serves as the second pose information.

[0037] Furthermore, the adjustment of x to obtain x when the objective function F(x) is minimized. min include:

[0038] Use the first pose information as the initial value x0;

[0039] Starting from the initial value x0, the objective function F(x) is iterated multiple times until F(x) is obtained. k+1 The value of ) reaches a minimum, where x k+1 =x k +△x k k represents the number of iterations, when Δx k When the value is less than the first threshold, stop the iteration and set x at this point. k As x min .

[0040] In one aspect, embodiments of the present invention also provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described method.

[0041] On one hand, embodiments of the present invention also provide a two-dimensional grid map construction system based on lidar, the two-dimensional grid map construction system based on lidar includes:

[0042] The data acquisition module is used to acquire the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment.

[0043] The pose acquisition module is used to obtain the first pose information of the target autonomous body based on the first two-dimensional point cloud data.

[0044] The data prediction module is used to predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information.

[0045] The pose optimization module is used to optimize the first pose information based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information.

[0046] A map building module is used to construct a two-dimensional raster map of the target area based on the second pose information.

[0047] On the other hand, embodiments of the present invention also provide a two-dimensional grid map construction device based on lidar, the device comprising: an OR processor; and a storage device for storing an OR program, which, when executed by the OR processor, causes the OR processor to implement the method provided in any embodiment of the present invention.

[0048] In another aspect, embodiments of the present invention also provide a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the method provided in any embodiment of the present invention.

[0049] This invention provides an embodiment of acquiring first two-dimensional point cloud data of a target area scanned by a lidar on a target body at a first moment; obtaining first pose information of the target body based on the first two-dimensional point cloud data; predicting second two-dimensional point cloud data obtained by the lidar when the target body is in the first pose information; optimizing the first pose information based on the second two-dimensional point cloud data to obtain second pose information; and constructing a two-dimensional grid map of the target area based on the second pose information, thus solving the problem of insufficient accuracy in existing two-dimensional grid map construction. Attached Figure Description

[0050] Figure 1 This is a flowchart illustrating a two-dimensional grid map construction method based on lidar provided in Embodiment 1 of the present invention;

[0051] Figure 2 This is a flowchart illustrating a two-dimensional grid map construction method based on lidar provided in Embodiment 2 of the present invention;

[0052] Figure 3 yes Figure 2 A schematic diagram of a specific process for step S230 in the illustrated embodiment;

[0053] Figure 4 yes Figure 2 A detailed flowchart of step S260 in the illustrated embodiment is shown.

[0054] Figure 5 This is a schematic diagram of a two-dimensional grid map construction system based on lidar provided in Embodiment 3 of the present invention;

[0055] Figure 6 This is a schematic diagram of a two-dimensional grid map construction device based on lidar provided in Embodiment 4 of the present invention. Detailed Implementation

[0056] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and not for limiting the invention. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention and not the entire structure.

[0057] Before discussing the exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe the steps as sequential processes, many of these steps can be performed in parallel, concurrently, or simultaneously. Furthermore, the order of the steps can be rearranged. A process can be terminated when its operation is complete, but it may also have additional steps not included in the figures. A process can correspond to a method, function, procedure, subroutine, subroutine, etc.

[0058] Furthermore, the terms "first," "second," etc., may be used herein to describe various directions, actions, steps, or elements, but these directions, actions, steps, or elements are not limited by these terms. These terms are only used to distinguish a first direction, action, step, or element from another direction, action, step, or element. For example, without departing from the scope of this application, first two-dimensional point cloud data may be referred to as second two-dimensional point cloud data, and similarly, second two-dimensional point cloud data may be referred to as first two-dimensional point cloud data. Both first two-dimensional point cloud data and second two-dimensional point cloud data are two-dimensional point cloud data, but they are not the same two-dimensional point cloud data. The terms "first," "second," etc., should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, features defined with "first" or "second" may explicitly or implicitly include one or more of the stated features. In the description of embodiments of the present invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0059] Example 1

[0060] like Figure 1 As shown, Embodiment 1 of the present invention provides a method for constructing a two-dimensional grid map based on lidar, the method comprising:

[0061] S110. Acquire the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment.

[0062] In this embodiment, the target self-entity can be a robot, a self-moving vehicle, or other self-moving entities. For example, the target self-entity is a robotic vacuum cleaner, and the target area is the room area that needs cleaning. The construction of the two-dimensional grid map can be real-time or offline (non-real-time). In this embodiment, the two-dimensional grid map is a real-time mapping process; the first moment refers to the current real-time moment, and the first two-dimensional point cloud data is the point cloud data acquired in real time. In other embodiments, the two-dimensional grid map can also be an offline mapping process. In this case, the first moment refers to any moment during the robotic vacuum cleaner's movement when pose determination is required, and the first two-dimensional point cloud data is the point cloud data obtained by the LiDAR scan corresponding to that first moment. The LiDAR can be installed on the side or top of the self-entity, as long as it can scan the target area. The LiDAR can be a multi-line LiDAR or a single-line LiDAR, and can be a mechanical LiDAR, a hybrid solid-state LiDAR, or a solid-state LiDAR.

[0063] S120. Obtain the first pose information of the target autonomous body based on the first two-dimensional point cloud data.

[0064] After obtaining the first two-dimensional point cloud data, the first pose information of the target autonomous body at the first moment can be determined according to commonly used registration algorithms or deep learning algorithms in this field. For example, the first pose information of the target autonomous body can be obtained by matching the first two-dimensional point cloud data with the two-dimensional point cloud data obtained at the previous moment using the two-dimensional NDT registration module based on the NDT registration algorithm. Specifically, the first pose information at the first moment is determined by matching the first two-dimensional point cloud data with the two-dimensional point cloud data obtained at the previous moment using the two-dimensional NDT registration module based on the NDT registration algorithm.

[0065] S130. Predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information.

[0066] Typically, the performance parameters of a lidar mounted on a target are fixed and known, and the environment in which the target is located can also be scanned in advance by the lidar to pre-construct a corresponding two-dimensional grid map. Therefore, theoretically, it is possible to predict the theoretical two-dimensional point cloud data that the lidar can scan when the target is in any pose, that is, it is possible to predict the theoretical second two-dimensional point cloud data that the lidar can scan when the target is in the first pose.

[0067] S140. Optimize the first pose information based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information.

[0068] In this embodiment, the NDT registration algorithm achieves efficient and real-time point cloud data registration in three-dimensional space with good registration accuracy. However, the two-dimensional space lacks one dimension, allowing the algorithm to further improve matching efficiency without sacrificing accuracy. This provides sufficient time and resources for optimization. In this embodiment, after obtaining the first pose information, it is further optimized to utilize the optimized pose information with higher accuracy for mapping.

[0069] The second two-dimensional point cloud data is the theoretical two-dimensional point cloud data that the lidar can acquire when it is in the first pose information at the first moment, while the first two-dimensional point cloud data is the two-dimensional point cloud data that the lidar actually acquires at the first moment. Therefore, the first pose information can be optimized according to the deviation between the actual and theoretical data to obtain a second pose information that is more accurate than the first pose information.

[0070] Specifically, a preset algorithm is used to optimize the first pose information by combining the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information. The preset algorithm can be an algorithm based on nonlinear optimization technology. Specifically, it first predicts the second two-dimensional point cloud data obtained by the lidar when the target body is in the first pose information, then obtains the error between the second two-dimensional point cloud data and the first two-dimensional point cloud data, and takes the first pose information with the smallest error as the second pose information.

[0071] S150. Construct a two-dimensional grid map of the target area based on the second pose information.

[0072] A two-dimensional grid map of the target area is constructed based on the second pose information, achieving efficient and low-occupancy construction of a high-precision two-dimensional grid map. The two-dimensional grid map is built using the two-dimensional point cloud data obtained from the current LiDAR scan.

[0073] This invention provides an embodiment of acquiring first two-dimensional point cloud data of a target area scanned by a lidar on a target body at a first moment; obtaining first pose information of the target body based on the first two-dimensional point cloud data; predicting second two-dimensional point cloud data obtained by the lidar when the target body is in the first pose information; optimizing the first pose information based on the second two-dimensional point cloud data to obtain second pose information; and constructing a two-dimensional grid map of the target area based on the second pose information. This solves the problems of excessive computer resource consumption and insufficient accuracy in existing methods for constructing two-dimensional grid maps, achieving the effect of constructing high-precision two-dimensional grid maps efficiently and with low resource consumption.

[0074] Example 2

[0075] like Figures 2-4As shown, Embodiment 2 of the present invention provides a method for constructing a two-dimensional grid map based on lidar. Embodiment 2 is a further explanation based on Embodiment 1 of the present invention, as follows: Figure 2 As shown, the method includes:

[0076] S210. When the lidar on the target body is a single-line lidar, the first two-dimensional point cloud data of the target area at the first moment is directly obtained through the lidar.

[0077] S220. The lidar on the target body is a multi-line lidar. The first laser with a preset emission angle in the multi-line lidar is identified, and the point cloud data acquired by the first laser is projected onto the horizontal plane according to the preset emission angle to obtain the first two-dimensional point cloud data of the target area at the first moment.

[0078] In this embodiment, if the lidar on the target body is a single-line lidar, the first two-dimensional point cloud data of the target area at the first moment can be directly obtained through the lidar. If the lidar on the target body is a multi-line lidar, it is necessary to identify the first laser with a preset emission angle in the multi-line lidar, and project the point cloud data obtained by the first laser onto the horizontal plane according to the preset emission angle to obtain the first two-dimensional point cloud data of the target area at the first moment. The preset emission angle is determined by the parameters of the multi-line lidar itself. This enables both single-line and multi-line lidar to be used to construct two-dimensional grid maps.

[0079] In this embodiment, steps S210 and S220 are in an OR relationship, meaning that one of the steps needs to be selected to be executed depending on the type of lidar. After executing either step S210 or step S220, step S230 is executed.

[0080] S230. Obtain the first pose information of the target autonomous body based on the first two-dimensional point cloud data.

[0081] S240. Obtain the performance parameters of the lidar and the pre-established two-dimensional environment map.

[0082] The performance parameters of a lidar can include information such as the emission frequency, the emission angle of the laser beam, and the scanning angle. The pre-built two-dimensional environment map refers to the map that is pre-constructed by scanning the entire environment with lidar before executing the steps in this method. This pre-constructed two-dimensional environment map is a static map. The two-dimensional environment map can be stored directly after its creation for subsequent operations. It can be understood that the two-dimensional environment map will be updated when static objects in the environment change.

[0083] S250. Based on the performance parameters and the two-dimensional environment map, predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information.

[0084] In this embodiment, the performance parameters of the lidar and a pre-established two-dimensional environment map are first obtained. Then, based on these performance parameters and the two-dimensional environment map, the second two-dimensional point cloud data that the lidar may obtain when the target is in the first pose information can be predicted. That is, based on the lidar's performance parameters, the scanning area of ​​the lidar when it is in the first pose information can be known, and based on the pre-established two-dimensional environment map, the environmental distribution corresponding to the scanning area can be known. Thus, the distribution of point cloud data formed by the reflection of the laser beam by the corresponding environmental objects can be determined, that is, the theoretical second two-dimensional point cloud data can be obtained.

[0085] S260. Optimize the first pose information based on the second two-dimensional point cloud data to obtain the second pose information.

[0086] S270. Construct a two-dimensional grid map of the target area based on the second pose information.

[0087] Furthermore, such as Figure 3 As shown, in the two-dimensional grid map construction method based on lidar provided in Embodiment 2 of the present invention, step S230 may specifically include:

[0088] S231. Obtain the third two-dimensional point cloud data of the target area scanned by the lidar at the second time point, where the second time point is the time point preceding the first time point.

[0089] The second moment is the moment before the first moment. Therefore, the third two-dimensional point cloud data of the target area obtained by the lidar at the second moment can be directly read from the point cloud data already acquired by the lidar. The two-dimensional point cloud data with the timestamp of the second moment is used as the third two-dimensional point cloud data.

[0090] S232. Obtain the third pose information of the target from the subject at the second moment.

[0091] The third pose information at the second time step can be determined based on the third 2D point cloud data at the second time step. Usually, this third pose information has already been obtained and determined during the mapping process at the second time step, so it can be read directly at this time.

[0092] S233. Obtain the moving speed of the target from the second moment of the subject.

[0093] The movement velocity of the subject at the second moment can be determined based on the third two-dimensional point cloud data obtained at the second moment, or by combining the point cloud data from the second moment and at least one previous frame. The movement velocity is a vector, including both direction and magnitude.

[0094] S234. Based on the moving speed and the third pose information, predict the fourth pose information of the target from the first moment of the subject.

[0095] Given the target autonomous body's movement speed, its pose information at future moments can be roughly predicted based on that speed. Therefore, based on the movement speed at the second moment and the pose information at the third moment, the pose information of the target autonomous body at the first moment after the second moment can be roughly predicted, serving as the fourth pose information.

[0096] S235. Based on the fourth pose information, the first two-dimensional point cloud data and the third two-dimensional point cloud data are matched using the NDT algorithm to obtain the first relative pose relationship between the first time and the second time.

[0097] Specifically, the fourth pose information is used as the initial value for NDT algorithm matching to register the first two-dimensional point cloud data and the third two-dimensional point cloud data, which can greatly reduce the amount of data in the matching process and improve the matching efficiency.

[0098] S236. Determine the first pose information of the target autonomous body based on the first relative pose relationship and the third pose information.

[0099] In this embodiment, a two-dimensional NDT registration module based on the NDT registration algorithm is needed to register the first two-dimensional point cloud data acquired by the target autonomous body at the first time and the third two-dimensional point cloud data acquired at the second time. For example, the third two-dimensional point cloud data of the target area at the second time is acquired by the lidar, where the second time is the time preceding the first time. Then, the NDT registration algorithm is used to match the first two-dimensional point cloud data and the third two-dimensional point cloud data to obtain the first relative pose relationship between the first and second times. The pose information of the target autonomous body at the second time has already been determined at the second time, so the third pose information of the target autonomous body at the second time can be directly obtained. Finally, the first pose information of the target autonomous body is determined based on the first relative pose relationship and the third pose information. Wherein, if the first time is the time when the target autonomous body initially moves, i.e., there is no previous time, it is directly used as the map data for the two-dimensional grid map.

[0100] Furthermore, such as Figure 4 As shown, in the two-dimensional grid map construction method based on lidar provided in Embodiment 2 of the present invention, step S260 may specifically include:

[0101] S261. Define a mapping function f. i (x), where f i The value of (x) represents the second two-dimensional point cloud data theoretically obtained by the lidar when the pose information is x, where x represents the pose information and i represents the number of times the pose information of the target is determined.

[0102] S262. Define an error function e i (x), where e i (x)=f i (x)-z i e i The value of (x) represents the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data, z i This represents the first two-dimensional point cloud data.

[0103] S263. Define an objective function F(x), where The objective function F(x) is given by the error function e i The covariance matrix is ​​obtained by squaring (x).

[0104] In this embodiment, when using nonlinear optimization techniques to combine the first two-dimensional point cloud data and the first pose information to obtain the second pose information, it is first necessary to define i as the number of times the target's pose information is determined. Then, x is defined as the pose information, where x is a state vector. The pose information includes the position and orientation information of the target's self-body along the x and y axes in the Cartesian coordinate system. Since the ultimate goal is to establish a two-dimensional grid map, the position information, roll angle information, and pitch angle information along the z axis in the Cartesian coordinate system are not considered. i Let f be the first two-dimensional point cloud data actually obtained by scanning when the target subject is in the first pose information. i Let f(x) be a nonlinear mapping to represent the third two-dimensional point cloud data, where the third two-dimensional point cloud data is the point cloud data predicted from the target region when the target subject is in state x. i (x) represents the point cloud data predicted from the target region based on the first pose information of the target after any action of the subject. Therefore, the error between the first two-dimensional point cloud data and the third two-dimensional point cloud data is defined as e. i (x), where e i (x)=f i (x)-z i .

[0105] Furthermore, it is generally assumed that the error follows a Gaussian distribution, therefore the error e is defined as follows: i The square of (x) is Take E again iThe covariance matrix of (x) can be used to obtain the nonlinear least squares objective function. By finding the x value corresponding to the minimum value of the objective function F(x), the optimal first pose information can be determined.

[0106] S264. Use the first pose information as the initial value x0.

[0107] S265. Starting from the initial value x0, iterate the objective function F(x) multiple times until F(x) is reached. k+1 The value of ) reaches a minimum, where x k+1 =x k +△x k k represents the number of iterations, when Δx k When the value is less than the first threshold, stop the iteration and set x at this point. k As x min and the x min This serves as the second pose information.

[0108] In this embodiment, in order to find the x value corresponding to the minimum value of the objective function F(x), the first pose information is first used as the initial value x0. Since the first pose information is the pose information initially obtained by the registration algorithm based on the first two-dimensional point cloud data obtained by the lidar scanning, using it as the initial value can reduce the number of iterations and accelerate the efficiency of the iteration process.

[0109] Specifically, firstly, e i (x) is expanded using a first-order Taylor series to obtain:

[0110] e i (x + Δx) = e i (x)+J i (x)△x

[0111] Where J is the Jacobian matrix, which represents the derivative with respect to x. Therefore, the objective function F(x) can be transformed into:

[0112]

[0113] Expanding and simplifying the right side of the formula, we get:

[0114]

[0115] Since we are solving for the increment Δx, we express the quantities that are independent of Δx using coefficients to obtain the following:

[0116]

[0117] Now, find the derivative of F(x+Δx) with respect to Δx and set the result equal to 0:

[0118]

[0119] Further simplification yields:

[0120] △x * =-H -1 b

[0121] Based on the above, starting from the initial value x0, the objective function F(x) is iterated k times until F(x) is obtained. k +△x k ), that is, F(x) k+1 ) reaches a minimum value, where x k+1 =x k +△x k In the k iterations of the objective function F(x), if Δx k If the value is less than the first threshold, stop the iteration and set x to the current value. k As x min and x min As the second pose information, a two-dimensional grid map of the target area is constructed based on the second pose information, thereby obtaining a map with extremely high accuracy.

[0122] Example 3

[0123] like Figure 5 As shown, Embodiment 3 of the present invention provides a two-dimensional grid map construction system 100 based on LiDAR. The two-dimensional grid map construction system 100 provided in Embodiment 3 of the present invention can execute the two-dimensional grid map construction method based on LiDAR provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method. The two-dimensional grid map construction system 100 based on LiDAR includes a data acquisition module 200, a pose acquisition module 300, a data prediction module 400, a pose optimization module 500, and a map construction module 600.

[0124] Specifically, the data acquisition module 200 is used to acquire the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment; the pose acquisition module 300 is used to obtain the first pose information of the target body based on the first two-dimensional point cloud data; the data prediction module 400 is used to predict the second two-dimensional point cloud data obtained by the lidar when the target body is in the first pose information; the pose optimization module 500 is used to optimize the first pose information based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information; and the map construction module 600 is used to construct a two-dimensional grid map of the target area based on the second pose information.

[0125] In this embodiment, the data acquisition module 200 is specifically used to directly acquire the first two-dimensional point cloud data of the target area at the first moment when the lidar on the target body is a single-line lidar; when the lidar on the target body is a multi-line lidar, it identifies the first laser with a preset emission angle in the multi-line lidar and projects the point cloud data acquired by the first laser onto the horizontal plane according to the preset emission angle to obtain the first two-dimensional point cloud data of the target area at the first moment.

[0126] The pose acquisition module 300 is specifically used to acquire the third two-dimensional point cloud data of the target area scanned by the lidar at a second time moment, where the second time moment is the time moment preceding the first time moment; acquire the third pose information of the target autonomous body at the second time moment; match the first two-dimensional point cloud data and the third two-dimensional point cloud data according to the NDT algorithm to obtain the first relative pose relationship between the first time moment and the second time moment; and determine the first pose information of the target autonomous body according to the first relative pose relationship and the third pose information. The pose acquisition module 300 is also specifically used to acquire the moving speed of the target autonomous body at the second time moment; predict the fourth pose information of the target autonomous body at the first time moment according to the moving speed and the third pose information; and match the first two-dimensional point cloud data and the third two-dimensional point cloud data according to the fourth pose information using the NDT algorithm to obtain the first relative pose relationship between the first time moment and the second time moment.

[0127] The data prediction module 400 is specifically used to acquire the performance parameters of the lidar and a pre-established two-dimensional environment map; and to predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information based on the performance parameters and the two-dimensional environment map.

[0128] The pose optimization module 500 is specifically used to confirm the mapping relationship between the first pose information and the second two-dimensional point cloud data; and to adjust the first pose information according to the mapping relationship to obtain the second pose information when the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data is minimized. The pose optimization module 500 is also specifically used to define a mapping function f. i (x), where f i The value of (x) represents the second two-dimensional point cloud data theoretically obtained by the lidar when the pose information is x, where x represents the pose information and i represents the number of times the pose information of the target subject is determined. The pose optimization module 500 is also specifically used to define an error function e. i (x), where e i (x)=f i (x)-z i e iThe value of (x) represents the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data, z i Let x represent the first two-dimensional point cloud data; define an objective function F(x), where The objective function F(x) is given by the error function e i The covariance matrix of the squared objective function F(x) is obtained; x is then adjusted to obtain x that minimizes the value of the objective function F(x). min and the x min As the second pose information, the pose optimization module 500 is further configured to use the first pose information as an initial value x0; and to iterate the objective function F(x) multiple times starting from the initial value x0 until F(x) is obtained. k+1 The value of ) reaches a minimum, where x k+1 =x k +△x k k represents the number of iterations, when Δx k When the value is less than the first threshold, stop the iteration and set x at this point. k As x min .

[0129] Example 4

[0130] Figure 6 This is a schematic diagram of a computer device for constructing a two-dimensional grid map based on lidar, provided in Embodiment 4 of the present invention. Figure 6 A block diagram of an exemplary computer device 12 suitable for implementing embodiments of the present invention is shown. Figure 6 The computer device 12 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of the present invention.

[0131] like Figure 6 As shown, the computer device 12 is represented in the form of a general-purpose computing device. The components of the computer device 12 may include, but are not limited to: a processor or processing unit 16, a system memory 28, and a bus 18 connecting different system components (including the system memory 28 and the processing unit 16).

[0132] Bus 18 represents one or more of several bus architectures, including a memory bus or memory controller, a peripheral bus, a graphics acceleration port, a processor, or a local bus using any of the various bus architectures. For example, these architectures include, but are not limited to, the Industry Standard Architecture (ISA) bus, the Micro Channel Architecture (MAC) bus, the Enhanced ISA bus, the Video Electronics Standards Association (VESA) local bus, and the Peripheral Component Interconnect (PCI) bus.

[0133] Computer device 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by computer device 12, including volatile and non-volatile media, removable and non-removable media.

[0134] System memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and / or cache memory 32. Computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, storage system 34 may be used to read and write non-removable, non-volatile magnetic media (…). Figure 6 Not shown; usually referred to as a "hard drive"). Although Figure 6 Not shown, a disk drive for reading and writing to a removable non-volatile disk (e.g., a "floppy disk") and an optical disk drive for reading and writing to a removable non-volatile optical disk (e.g., a CD-ROM, DVD-ROM, or other optical media) may be provided. In these cases, each drive may be connected to bus 18 via a single or multiple data media interface. Memory 28 may include at least one program product having a set (e.g., at least one) of program modules configured to perform the functions of the embodiments of the present invention.

[0135] A program / utility 40 having a set (at least one) of program modules 42 may be stored, for example, in memory 28. Such program modules 42 include—but are not limited to—an operating system, an application program, other program modules, and program data. Each or some combination of these examples may include an implementation of a network environment. Program modules 42 typically perform the functions and / or methods described in the embodiments of the present invention.

[0136] Computer device 12 can also communicate with one or more external devices 14 (e.g., keyboard, pointing device, display 24, etc.), and with one or more devices that enable a user to interact with computer device 12, and / or with any device that enables computer device 12 to communicate with one or more other computing devices (e.g., network card, modem, etc.). This communication can be performed via input / output (I / O) interface 22. Furthermore, computer device 12 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public networks, such as the Internet) via network adapter 20. As shown, network adapter 20 communicates with other modules of computer device 12 via bus 18. It should be understood that, although not shown in the figures, other hardware and / or software modules can be used in conjunction with computer device 12, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0137] Processing unit 16 executes various functional applications and data processing by running programs stored in system memory 28, such as implementing the methods provided in the embodiments of the present invention:

[0138] The first two-dimensional point cloud data of the target area at the first moment is obtained by the lidar on the target itself.

[0139] The first pose information of the target autonomous entity is obtained based on the first two-dimensional point cloud data;

[0140] Predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information;

[0141] The first pose information is optimized based on the second two-dimensional point cloud data to obtain the second pose information;

[0142] A two-dimensional grid map of the target area is constructed based on the second pose information.

[0143] Example 5

[0144] Embodiment 5 of the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the methods provided in all embodiments of the present application:

[0145] The first two-dimensional point cloud data of the target area at the first moment is obtained by the lidar on the target itself.

[0146] The first pose information of the target autonomous entity is obtained based on the first two-dimensional point cloud data;

[0147] Predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information;

[0148] The first pose information is optimized based on the second two-dimensional point cloud data to obtain the second pose information;

[0149] A two-dimensional grid map of the target area is constructed based on the second pose information.

[0150] The computer storage medium of this invention can be any combination of one or more computer-readable media. A computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0151] Computer-readable signal media may include data signals propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. Computer-readable signal media may also be any computer-readable medium other than computer-readable storage media, capable of sending, propagating, or transmitting programs for use by or in connection with an instruction execution system, apparatus, or device.

[0152] Program code contained on a computer-readable medium may be transmitted using any suitable medium, including—but not limited to—wireless, wire, optical fiber, RF, etc., or any suitable combination thereof.

[0153] Computer program code for performing the operations of this invention can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as "C" or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0154] Note that the above description is merely a preferred embodiment of the present invention and the technical principles employed. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and various obvious changes, readjustments, and substitutions can be made without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in detail through the above embodiments, the present invention is not limited to the above embodiments, and may include many other equivalent embodiments without departing from the concept of the present invention, the scope of which is determined by the scope of the appended claims.

Claims

1. A method for constructing a two-dimensional grid map based on lidar, characterized in that, include: Acquire the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment; The first pose information of the target autonomous entity is obtained based on the first two-dimensional point cloud data; Predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information; The first pose information is optimized based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information; A two-dimensional grid map of the target area is constructed based on the second pose information; The step of optimizing the first pose information based on the second two-dimensional point cloud data to obtain the second pose information includes: Define a mapping relationship between the first pose information and the second two-dimensional point cloud data, with pose information as the independent variable; The phrase "defining a mapping relationship between the first pose information and the second two-dimensional point cloud data, with pose information as the independent variable" includes: Define a mapping function f i (x), where f i The value of (x) represents the second two-dimensional point cloud data theoretically obtained by the lidar when the pose information is x, where x represents the pose information and i represents the number of times the pose information of the target is determined. Based on the mapping relationship, construct an objective function for the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data; The phrase "constructing an objective function for the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data based on the mapping relationship" includes: Define an error function e i (x), where e i (x)=f i (x)-z i e i The value of (x) represents the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data, z i This represents the first two-dimensional point cloud data; Define an objective function F(x), where The objective function F(x) is derived from the error function e i The covariance matrix is ​​obtained by squaring (x) and then taking its covariance matrix. Define the pose information corresponding to the minimum value of the objective function as the second pose information; The definition of "defining the pose information corresponding to the minimum value of the objective function as the second pose information" includes: Use the first pose information as the initial value x0; Starting from the initial value x0, the objective function F(x) is iterated multiple times until F(x) is obtained. k+1 The value of ) reaches a minimum, where x k+1 =x k + x k k represents the number of iterations, when x k When the value is less than the first threshold, stop the iteration and set x at this point. k As x min The x min This is the second pose information.

2. The method according to claim 1, characterized in that, The acquisition of the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment includes: When the lidar on the target body is a single-line lidar, the first two-dimensional point cloud data of the target area at the first moment is directly obtained through the lidar. The lidar on the target body is a multi-line lidar. The first laser in the multi-line lidar is identified at a preset emission angle, and the point cloud data acquired by the first laser is projected onto a horizontal plane according to the preset emission angle to obtain the first two-dimensional point cloud data of the target area at the first moment.

3. The method according to claim 1 or 2, wherein obtaining the first pose information of the target autonomous body based on the first two-dimensional point cloud data includes: Acquire the third two-dimensional point cloud data of the target area obtained by the lidar at the second time point, where the second time point is the time point before the first time point; Obtain the third pose information of the target from the subject at the second time point; The first two-dimensional point cloud data and the third two-dimensional point cloud data are matched according to the NDT algorithm to obtain the first relative pose relationship between the first time and the second time. The first pose information of the target autonomous body is determined based on the first relative pose relationship and the third pose information.

4. The method according to claim 3, characterized in that, The step of matching the first two-dimensional point cloud data and the third two-dimensional point cloud data according to the NDT algorithm to obtain the first relative pose relationship between the first time and the second time includes: Obtain the target's moving speed from the subject at the second moment; Predict the fourth pose information of the target from the first moment based on the moving speed and the third pose information; Based on the fourth pose information, the NDT algorithm is used to match the first two-dimensional point cloud data and the third two-dimensional point cloud data to obtain the first relative pose relationship between the first time and the second time.

5. The method according to claim 1, 2 or 4, characterized in that, The second two-dimensional point cloud data obtained by the lidar when predicting the target's position based on the first pose information includes: Obtain the performance parameters of the lidar and the pre-established two-dimensional environment map; Based on the performance parameters and the two-dimensional environment map, the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information is predicted.

6. A computer-readable storage medium, characterized in that, The device contains a computer program that, when executed by a processor, implements the steps of the method described in any one of claims 1 to 5.

7. A two-dimensional grid map construction system based on lidar, characterized in that, include: The data acquisition module is used to acquire the first two-dimensional point cloud data of the target area obtained by the lidar on the target body at the first moment. The pose acquisition module is used to obtain the first pose information of the target autonomous body based on the first two-dimensional point cloud data. The data prediction module is used to predict the second two-dimensional point cloud data obtained by the lidar when the target is in the first pose information. The pose optimization module is used to optimize the first pose information based on the first two-dimensional point cloud data and the second two-dimensional point cloud data to obtain the second pose information. The step of optimizing the first pose information based on the second two-dimensional point cloud data to obtain the second pose information includes: defining a mapping relationship between the first pose information and the second two-dimensional point cloud data, with pose information as the independent variable; the step of "defining a mapping relationship between the first pose information and the second two-dimensional point cloud data, with pose information as the independent variable" includes: defining a mapping function f i (x), where f i The value of (x) represents the second two-dimensional point cloud data theoretically obtained by the lidar when the pose information is x, where x represents the pose information and i represents the number of times the pose information of the target subject is determined; according to the mapping relationship, an objective function for the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data is constructed; the "constructing an objective function for the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data according to the mapping relationship" includes: defining an error function e i (x), where e i (x)=f i (x)-z i e i The value of (x) represents the error between the first two-dimensional point cloud data and the second two-dimensional point cloud data, z i Let x represent the first two-dimensional point cloud data; define an objective function F(x), where The objective function F(x) is derived from the error function e i (x) is squared and its covariance matrix is ​​obtained; the pose information corresponding to the minimum value of the objective function is defined as the second pose information; the "defining the pose information corresponding to the minimum value of the objective function as the second pose information" includes: taking the first pose information as the initial value x0; starting from the initial value x0, performing multiple iterations on the objective function F(x) until F(x) k+1 The value of ) reaches a minimum, where x k+1 =x k + x k k represents the number of iterations, when x k When the value is less than the first threshold, stop the iteration and set x at this point. k As x min The x min This is the second pose information; A map building module is used to construct a two-dimensional raster map of the target area based on the second pose information.

Images