A method, device, equipment and storage medium for generating high-precision maps

Abstract

The present application relates to the field of autonomous driving technology, and discloses a method, device, equipment and storage medium for generating a high-precision map, wherein the method includes obtaining the radar point cloud of the current frame of the vehicle in real time; setting the first frame of radar point cloud as the first key frame, performing point cloud registration on the radar point cloud of the current frame to the first key frame, and obtaining the mapping pose of the current frame; establishing a key frame queue based on the mapping pose, and sequentially setting the next frame of radar point cloud for point cloud registration; adjusting the radar point cloud of each frame according to the key frame of the key frame queue, and constructing a local map based on the adjusted radar point cloud of each frame; establishing an error equation of adjacent local maps based on the pose of the local map and the point cloud registration data; adjusting the pose of the local map until the error value of the adjacent local map in the error equation is minimized; and establishing a global map through the adjusted pose of the local map. The present application has the effect of improving the failure of vehicle positioning and navigation in time-varying scenarios.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a method, device, equipment and storage medium for generating a high-precision map. Background Art

[0002] At present, vehicle positioning technology involves the accurate implementation of environmental perception, path planning and decision-making control functions, and plays a key role in the field of autonomous driving.

[0003] Unmanned vehicles are in scenarios where the environment changes over time, such as warehouses and ports. Since the scene will change, for example, GPS is interfered with in the aisles between container yards, and radar positioning also suffers from motion degradation, the traditional method of using prior maps for positioning and navigation will fail, and vehicle positioning will be a great challenge.

[0004] With respect to the above-mentioned related technologies, the inventors have discovered that in scenarios where the environment changes over time, existing vehicle positioning and navigation methods have the problem of failure. Summary of the invention

[0005] In order to improve the failure of vehicle positioning and navigation in scenarios where the environment changes over time, the present application provides a high-precision map generation method, device, equipment and storage medium.

[0006] On the first aspect, the present application provides a high-precision map generation method, which has the characteristic of improving the failure of vehicle positioning and navigation in scenarios where the environment changes over time.

[0007] This application is achieved through the following technical solutions:

[0008] A method for generating a high-precision map comprises the following steps:

[0009] Obtain the radar point cloud of the vehicle's current frame in real time;

[0010] The first frame of radar point cloud is set as the first key frame, and point cloud registration is performed on the radar point cloud of the current frame to the first key frame to obtain the mapping pose of the current frame;

[0011] A key frame queue is established based on the mapping pose, and the next frame of radar point cloud is set in sequence for point cloud registration;

[0012] Adjusting the radar point cloud of each frame according to the key frames of the key frame queue, and constructing a local map based on the adjusted radar point cloud of each frame;

[0013] Based on the pose and point cloud registration data of the local map, the error equation of the adjacent local map is established;

[0014] Adjusting the position and posture of the local map until the error value of the adjacent local maps in the error equation is minimized;

[0015] The global map is established by adjusting the pose of the local map.

[0016] In a preferred example, the present application may be further configured as follows: after the step of adjusting the radar point cloud of each frame according to the key frame of the key frame queue and constructing a local map based on the adjusted radar point cloud of each frame, the method further includes:

[0017] After obtaining the pose of the last first key frame in the key frame queue, clear the key frame queue;

[0018] Setting the radar point cloud of the current frame as a second key frame, performing point cloud registration from the second key frame to the local map, and obtaining a map pose of the current frame;

[0019] Re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frames for point cloud registration;

[0020] The radar point cloud of each frame is readjusted according to the key frames of the re-established key frame queue, the local map is reconstructed based on the readjusted radar point cloud of each frame, and the current local map is updated.

[0021] In a preferred example, the present application can be further configured as follows: the step of establishing a key frame queue based on the mapping pose and sequentially setting the first key frame for point cloud registration includes:

[0022] Determine whether the mapping pose reaches a preset key frame pose difference threshold;

[0023] If the mapping pose reaches the key frame pose difference threshold, the first key frame is stored in the key frame queue, and the next radar point cloud frame of the current first key frame is set as the current first key frame;

[0024] Continue to perform point cloud registration and threshold judgment from the radar point cloud of the current frame to the current first key frame, and store the current first key frame in the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a first preset value.

[0025] In a preferred example, the present application may be further configured as follows: after the step of establishing a key frame queue based on the mapping pose and sequentially setting the first key frame for point cloud registration, it also includes:

[0026] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset;

[0027] A nonlinear optimization framework is adopted to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases compared to the output value of the previous loss function.

[0028] In a preferred example, the present application can be further configured as follows: the step of re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frame for point cloud registration includes:

[0029] Determine whether the difference between the map pose and the pose of the last first key frame exceeds the key frame pose difference threshold;

[0030] If the difference between the map pose and the pose of the previous key frame exceeds the key frame pose difference threshold, the second key frame is stored in the key frame queue, and the next frame radar point cloud of the current frame is set as the current second key frame;

[0031] Continue to perform point cloud registration and threshold judgment of the current second key frame to the local map, and store the current second key frame to the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a second preset value.

[0032] In a preferred example, the present application can be further configured as follows: after the step of re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frame for point cloud registration, it also includes:

[0033] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset;

[0034] A nonlinear optimization framework is adopted to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases compared to the output value of the previous loss function.

[0035] In a preferred example, the present application can be further configured as follows: the expression of the error equation is:

[0036]

[0037] Where Error represents the error value of the adjacent local map; e imu represents the residual error of the inertial navigation integration between the previous and next local maps, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0038] In a preferred example, the present application can be further configured as follows: the expression of the loss function is:

[0039]

[0040] In the formula, E represents the output value of the loss function, e1 imu represents the residual error of the inertial navigation integral between the previous and next key frames, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0041] In a preferred example, the present application may be further configured as follows: the step of point cloud registration further includes:

[0042] Performing motion distortion removal processing on the radar point cloud to obtain a pose and a pose increment;

[0043] The pose is used as the pose of the key frame, and the pose increment is used as an initial value to perform point cloud registration.

[0044] On the second aspect, the present application provides a high-precision map generation device, which has the characteristic of improving the failure of vehicle positioning and navigation in scenarios where the environment changes over time.

[0045] This application is achieved through the following technical solutions:

[0046] A high-precision map generating device, comprising:

[0047] Fusion module, used to obtain the radar point cloud of the vehicle's current frame in real time;

[0048] The mapping thread startup module is used to set the first frame of radar point cloud as the first key frame, perform point cloud registration from the radar point cloud of the current frame to the first key frame, and obtain the mapping pose of the current frame; establish a key frame queue based on the mapping pose, and set the first key frame in turn for point cloud registration; and adjust the radar point cloud of each frame according to the key frame of the key frame queue, and build a local map based on the adjusted radar point cloud of each frame; the radar point cloud of each frame

[0049] A post-processing optimization module is used to establish an error equation of adjacent local maps based on the position and posture of the local map in combination with the point cloud registration data; and adjust the position and posture of the local map until the error value of the adjacent local map in the error equation is minimized;

[0050] The global map module is used to establish a global map by adjusting the pose of the local map for point cloud registration in the subsequent positioning thread.

[0051] On the third aspect, the present application provides a computer device that has the characteristic of improving the failure of vehicle positioning and navigation in a scenario where the environment changes over time.

[0052] This application is achieved through the following technical solutions:

[0053] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any one of the above-mentioned methods for generating a high-precision map when executing the computer program.

[0054] In a fourth aspect, the present application provides a computer-readable storage medium having the characteristic of improving the failure of vehicle positioning and navigation in a scenario where the environment changes over time.

[0055] This application is achieved through the following technical solutions:

[0056] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of any one of the above-mentioned methods for generating a high-precision map.

[0057] In summary, compared with the prior art, the technical solution provided by this application has the following beneficial effects:

[0058] 1. Acquire the matching posture of the vehicle's inertial navigation data, wheel speed meter data, GPS data and radar data in real time, and fuse them to obtain the radar point cloud of the vehicle's current frame to obtain the vehicle's current positioning status and realize the positioning of the unmanned vehicle; at the same time, use point cloud registration to obtain the mapping posture of the current frame, establish a key frame queue based on the mapping posture, and then adjust the radar point cloud of each frame according to the key frame of the key frame queue to build a local map. At this time, the key frames in the local range are synthesized into a local map, and subsequent positioning optimization operations can be performed at the level of the local map, which reduces the number of computing nodes and improves positioning efficiency; then set the radar point cloud of the current frame as the key frame to perform point cloud registration from the key frame to the local map , get the map pose of the current frame, re-establish the key frame queue based on the map pose, adjust the radar point cloud of each frame according to the key frame of the key frame queue, reconstruct the local map, and update the current local map, so as to realize the positioning and mapping of the unmanned vehicle; finally, establish the error equation of the adjacent local map, adjust the pose of the local map until the error value of the adjacent local map in the error equation is the smallest, and establish the global map through the pose of the local map in the adjusted aisle. The global map eliminates the accumulated error and ensures the global consistency, so as to be used for the precise point cloud registration of the subsequent positioning thread, thereby improving the accuracy of the positioning and navigation of the unmanned vehicle in the time-varying scene, and improving the failure of the vehicle positioning and navigation in the scene where the environment changes with time;

[0059] 2. When the number of key frames in the key frame queue reaches the first preset value or the second preset value, a nonlinear optimization framework is used to adjust the poses of the key frames in the key frame queue for local optimization to improve the accuracy of the poses of the key frames in the key frame queue, which is beneficial to improving the accuracy of subsequent local maps. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A schematic diagram of the main process of a high-precision map generation method provided for an exemplary embodiment of the present application.

[0061] Figure 2 A complete flowchart of a method for generating a high-precision map is provided as another exemplary embodiment of the present application.

[0062] Figure 3 A flowchart of establishing a key frame queue based on mapping pose for a high-precision map generation method provided in another exemplary embodiment of the present application.

[0063] Figure 4 A flowchart of establishing a key frame queue based on the difference between the map pose and the pose of the previous key frame in a high-precision map generation method provided as another exemplary embodiment of the present application.

[0064] Figure 5A schematic diagram of the cumulative error transmission of adjacent local maps established when a vehicle enters a port yard aisle in a high-precision map generation method provided as yet another exemplary embodiment of the present application.

[0065] Figure 6 A structural block diagram of a high-precision map generation device provided as an exemplary embodiment of the present application. DETAILED DESCRIPTION

[0066] This specific embodiment is merely an explanation of the present application and is not a limitation of the present application. After reading this specification, those skilled in the art may make modifications to the present embodiment without any creative contribution as needed, but such modifications are protected by the patent law as long as they are within the scope of the claims of the present application.

[0067] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0068] In addition, the term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article, unless otherwise specified, generally means that the associated objects before and after are in an "or" relationship.

[0069] The embodiments of the present application are further described in detail below in conjunction with the drawings in the specification.

[0070] Reference Figure 1 , an embodiment of the present application provides a method for generating a high-precision map, and the main steps of the method are described as follows.

[0071] Obtain the radar point cloud of the vehicle's current frame in real time;

[0072] The first frame of radar point cloud is set as the first key frame, and point cloud registration is performed on the radar point cloud of the current frame to the first key frame to obtain the mapping pose of the current frame;

[0073] A key frame queue is established based on the mapping pose, and the next frame of radar point cloud is set in sequence for point cloud registration;

[0074] Adjusting the radar point cloud of each frame according to the key frames of the key frame queue, and constructing a local map based on the adjusted radar point cloud of each frame;

[0075] Based on the pose and point cloud registration data of the local map, the error equation of the adjacent local map is established;

[0076] Adjusting the position and posture of the local map until the error value of the adjacent local maps in the error equation is minimized;

[0077] The global map is established by adjusting the pose of the local map.

[0078] Reference Figure 2 , specifically,

[0079] S1: Real-time acquisition and fusion of the matching posture of the vehicle's inertial navigation data, wheel speed meter data, GPS data and radar data to obtain the radar point cloud of the vehicle's current frame;

[0080] S2: Set the first frame of radar point cloud as the first key frame, perform point cloud registration from the radar point cloud of the current frame to the point cloud of the first key frame, and obtain the mapping pose of the current frame;

[0081] S3: establishing a key frame queue based on the mapping pose, and sequentially setting the first key frame, that is, sequentially setting the radar point cloud of the next frame as the first key frame, and performing point cloud registration until the number of key frames in the key frame queue reaches a first preset value;

[0082] S4: adjusting the entire radar point cloud according to the key frames in the key frame queue, the entire radar point cloud being the radar point cloud of each frame, building a local map based on the adjusted entire radar point cloud, and at the same time, clearing the key frame queue after obtaining the pose of the last first key frame in the key frame queue;

[0083] S5: setting the radar point cloud of the current frame as the second key frame, performing point cloud registration from the second key frame to the local map, and obtaining the map pose of the current frame;

[0084] S6: re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frames for point cloud registration until the number of key frames in the key frame queue reaches a second preset value;

[0085] S7: re-adjusting the entire radar point cloud according to the key frames of the re-established key frame queue, the entire radar point cloud being the radar point cloud of each frame, reconstructing the local map based on the re-adjusted entire radar point cloud, and updating the current local map;

[0086] S8: Based on the inertial navigation data, wheel speed meter data and the updated position and posture of the local map, combined with the point cloud registration data, an error equation of the adjacent local map is established;

[0087] S9: adjusting the position and posture of the updated local map until the error value of the adjacent local maps in the error equation is minimized;

[0088] S10: Establish a global map by adjusting the position and posture of the local map for point cloud registration of the subsequent positioning thread.

[0089] Reference Figure 3 , further, S3: establishing a key frame queue based on the mapping pose, and sequentially setting the first key frame for point cloud registration until the number of key frames in the key frame queue reaches a first preset value includes the following steps,

[0090] S31: Determine whether the mapping pose reaches a preset key frame pose difference threshold;

[0091] S32: If the mapping pose reaches the key frame pose difference threshold, the first key frame is stored in the key frame queue, and the next radar point cloud frame of the current first key frame is set as the current first key frame;

[0092] S33: Continue to perform point cloud registration and threshold judgment from the radar point cloud of the current frame to the current first key frame, and store the current first key frame in the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a first preset value.

[0093] When the number of key frames in the key frame queue reaches a first preset value, the key frames in the key frame queue are locally optimized, including the following steps:

[0094] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset;

[0095] A nonlinear optimization framework is used to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function is reduced compared to the output value of the previous loss function.

[0096] Reference Figure 3 , further, S6: re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frame for point cloud registration until the number of key frames in the key frame queue reaches a second preset value, the step includes,

[0097] S61: Determine whether the difference between the map pose and the pose of the last first key frame exceeds the key frame pose difference threshold;

[0098] S62: If the difference between the map pose and the pose of the last first key frame exceeds the key frame pose difference threshold, the second key frame is stored in the key frame queue, and the radar point cloud frame next to the current frame is set as the current second key frame;

[0099] S63: Continue to perform point cloud registration and threshold judgment on the current second key frame to the local map, and store the current second key frame to the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a second preset value.

[0100] When the number of key frames in the key frame queue reaches a second preset value, the key frames in the key frame queue are locally optimized, including the following steps:

[0101] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset;

[0102] A nonlinear optimization framework is used to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function is reduced compared to the output value of the previous loss function.

[0103] Furthermore, the loss function is expressed as,

[0104]

[0105] In the formula, E represents the output value of the loss function, e1 imu represents the residual error of the inertial navigation integral between the previous and next key frames, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0106] Furthermore, the error equation is expressed as,

[0107]

[0108] Where Error represents the error value of the adjacent local map; e imu represents the residual error of the inertial navigation integration between the previous and next local maps, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0109] Further, the step of adjusting the pose of the local map includes iterating an error equation through a gradient descent optimizer.

[0110] Furthermore, the step of point cloud registration also includes:

[0111] Based on the inertial navigation data, the radar point cloud is processed to remove motion distortion and obtain the pose and pose increment;

[0112] The pose is taken as the pose of the key frame, the pose increment is taken as the initial value, and the ICP algorithm is used for point cloud registration.

[0113] The above embodiments are described in detail as follows.

[0114] When a vehicle enters certain scenarios where the GPS signal is poor, this embodiment takes the port yard aisle as an example and uses sensors to obtain the matching posture of the vehicle's inertial navigation data, wheel speed meter data, GPS data, and radar data in real time.

[0115] First, the positioning thread is performed. Based on the inertial navigation data, the radar data is processed to remove motion distortion. Specifically, the acquired inertial navigation data is integrated to obtain the relative motion of the frame radar data, and then the timestamp of each point cloud data is interpolated to obtain a relative motion at a corresponding moment. The relative motion at the corresponding moment is then used to react to the point cloud data to obtain compensated point cloud data to improve the accuracy of the radar data. The compensated point cloud data is the processed pose, and the relative motion at the corresponding moment is the processed pose increment.

[0116] Next, the matching poses of the inertial navigation data, wheel speed meter data, GPS data and radar data are fused. The matching poses of the inertial navigation data, wheel speed meter data, GPS data and radar data are compared to obtain the radar point cloud of the vehicle's current frame, that is, the positioning status of the vehicle at the current moment is obtained by fusion.

[0117] At the same time, the map building thread is carried out. The map building thread mainly runs a radar odometer. The radar odometer is divided into two stages: the startup stage and the state tracking stage.

[0118] In the startup phase, the first frame of radar point cloud is set as the first key frame to obtain the positioning result obtained by the positioning thread, that is, the compensated point cloud data, as the pose of the key frame, and then the pose increment obtained by the positioning thread is used as the initial value, that is, the relative motion at the corresponding moment is used as the initial value to perform point cloud registration of the radar point cloud of the current frame to the current key frame to obtain the mapping pose of the current frame. In this embodiment, the point cloud registration method can use the ICP algorithm.

[0119] Then, it is determined whether the mapping pose reaches the set key frame pose difference threshold. If the mapping pose reaches the set key frame pose difference threshold, the next frame radar point cloud of the current key frame is set as a new key frame, and the first key frame is put into the key frame queue, and so on, until the number of key frames in the key frame queue reaches the first preset value, so as to ensure that the number of key frames in the key frame queue is sufficient, so that the data samples when mapping by frame are sufficient, and the mapping effect is better. In this embodiment, the first preset value can be 7.

[0120] Further, when the number of key frames in the key frame queue reaches a first preset value, the key frames in the key frame queue are locally optimized, including the following steps:

[0121] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset. The expression of loss function is:

[0122]

[0123] In the formula, E represents the output value of the loss function, e1 imi represents the residual error of the inertial navigation integral between the previous and next key frames, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0124] A nonlinear optimization framework is used to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases.

[0125] Among them, the nonlinear optimization framework includes Gauss-Newton optimization method, gradient descent optimization method, ceres library optimization method, graph optimization g2o library optimization method, etc. In this embodiment, the nonlinear optimization framework adopts the gradient descent optimization method, and the gradient is obtained by derivation of each error term of the loss function for the posture of each key frame, so that the current posture moves forward in the negative gradient direction by a preset step length, and the output value of the loss function is calculated, and then compared with the output value of the loss function before adjustment; if the output value of the calculated loss function decreases, the local optimization is completed; otherwise, the step length is reduced, the current posture moves forward in the negative gradient direction by a preset step length, the output value of the loss function is calculated again, and then compared with the output value of the previous loss function, until the output value of the calculated loss function is reduced compared with the output value of the previous loss function.

[0126] Finally, the radar point cloud of each frame is adjusted according to the key frames in the key frame queue, and a local map is constructed based on the adjusted radar point cloud of each frame. At the same time, the key frame queue is cleared after obtaining the pose of the last key frame in the key frame queue.

[0127] Furthermore, in the state tracking stage, the radar point cloud of the current frame is first set as the key frame, and the pose increment obtained by the positioning thread is used as the initial value, that is, the relative motion at the corresponding moment is used as the initial value, and the point cloud of the key frame to the previous local map is aligned, such as using the ICP algorithm for point cloud alignment to obtain the map pose of the current frame.

[0128] Then, based on the difference between the map pose and the pose of the last key frame obtained before clearing the key frame queue, the key frame queue is re-established until the number of key frames in the key frame queue reaches a second preset value. If the difference between the map pose and the pose of the last key frame obtained before clearing the key frame queue exceeds the key frame pose difference threshold, the key frame is added to the key frame queue, and the next frame radar point cloud of the current frame is set as the current key frame, and so on, until the number of key frames in the key frame queue reaches the second preset value, to ensure that the number of key frames in the key frame queue is sufficient, so that the data samples are sufficient when building the map at the local map level, and the mapping effect is better. In this embodiment, the second preset value can be 7.

[0129] Further, when the number of key frames in the key frame queue reaches a second preset value, the key frames in the key frame queue are locally optimized, including the following steps:

[0130] The loss function including radar matching error, IMU integration error and wheel speed meter error is preset. The expression of loss function is:

[0131]

[0132] In the formula, E represents the output value of the loss function, e1 imu represents the residual error of the inertial navigation integral between the previous and next key frames, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0133] A nonlinear optimization framework is used to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases.

[0134] Finally, the radar point cloud of each frame is adjusted according to the key frame of the re-established key frame queue, and the local map is updated based on the re-adjusted radar point cloud of each frame to be used as the map for subsequent point cloud tracking, and the cycle continues, thereby achieving the purpose of updating the current local map.

[0135] By synthesizing the key frames in the local range into a local map, subsequent positioning optimization operations can be performed at the level of the local map.

[0136] Reference Figure 5 , because the GPS data contained in the postures at both ends of the port yard aisle are relatively reliable, reliable postures can be obtained, while the GPS data in the aisle is interfered with and the radar data is degraded in the forward direction of movement. Therefore, only the state of inertial navigation data and wheel speed meter data can be used for recursive positioning, so there are cumulative errors. In addition, the updated local map also has cumulative errors, so post-processing optimization is required to establish a map that eliminates cumulative errors and ensures global consistency, which is used for point cloud matching of subsequent positioning threads.

[0137] The steps of post-processing optimization include:

[0138] Based on the inertial navigation data, wheel speed meter data and the updated local map pose, combined with the point cloud registration data, the error equation of the adjacent local map is established;

[0139] Adjust the pose of the updated local map until the error value of the adjacent local maps in the error equation is minimized.

[0140] Specifically, based on GPS data, the posture of the local map at both ends of the port yard aisle is obtained. By fixing the posture of the two ends of the port yard aisle, that is, the posture of the two ends of the port yard aisle remains unchanged during the optimization process, it is used to optimize and fine-tune the posture in the port yard aisle, thereby improving the utilization rate of high-precision and reliable GPS data.

[0141] Then, the pose of the local maps at both ends of the yard aisle is recursively obtained based on the inertial navigation data, wheel speed meter data and radar matching pose, and subtracted from the pose of the local map obtained based on GPS data to obtain a relative error; at the same time, the relative pose between adjacent local maps obtained by radar matching pose and wheel speed meter data is used to constrain the relative pose of adjacent local maps; in iterative optimization, the relative error is accumulated to obtain the overall error, which is transmitted to the pose of each local map. Therefore, based on the inertial navigation data, wheel speed meter data and the pose of the local map, combined with the point cloud registration data, that is, after performing the point cloud registration of the radar point cloud of the current frame to the current key frame, the radar point cloud of the current frame is obtained, and the expression of the error equation of the adjacent local map is established is as follows:

[0142]

[0143] Where Error represents the error value of the adjacent local map; e imi represents the residual error of the inertial navigation integration between the previous and next local maps, e chas represents the wheel speed meter residual, e reg represents radar matching residual; Cov imu Indicates the inertial navigation integrated covariance; Cov chas represents the covariance of the wheel speed meter; Cov reg Represents the point cloud registration covariance.

[0144] The error equation is then iteratively optimized using a gradient descent optimizer to adjust the position of the local map in the aisle until the overall error no longer decreases or the rate of change of the overall error is less than a preset value, and the overall error of the adjacent local maps is minimized by default.

[0145] Finally, the global map is established by adjusting the position and posture of the local map in the aisle for point cloud registration in the subsequent positioning thread.

[0146] In summary, a high-precision map generation method obtains the matching posture of the vehicle's inertial navigation data, wheel speed meter data, GPS data and radar data in real time; based on the inertial navigation data, the radar data is processed to remove motion distortion to improve the accuracy of the radar data, and the radar point cloud of the vehicle's current frame is obtained by fusion to obtain the vehicle's current positioning status and realize the positioning of the unmanned vehicle.

[0147] At the same time, point cloud registration is used to obtain the mapping pose of the current frame, and a key frame queue is established based on the mapping pose. When the number of key frames in the key frame queue reaches a first preset value, a nonlinear optimization framework is used to adjust the pose of the key frames in the key frame queue for local optimization to improve the accuracy of the pose of the key frames in the key frame queue, which is beneficial to improve the accuracy of subsequent local maps. The radar point cloud of each frame is adjusted according to the key frames in the key frame queue to construct a local map. At this time, the key frames in the local range are synthesized into a local map, and subsequent positioning optimization operations can be continued at the level of the local map, which reduces the number of computing nodes and improves positioning efficiency. The radar point cloud of the current frame is set as the key frame, and the point cloud registration of the key frame to the local map is performed to obtain the map pose of the current frame. The key frame queue is re-established based on the map pose. When the number of key frames in the key frame queue reaches a second preset value, a nonlinear optimization framework is used to adjust the pose of the key frames in the key frame queue for local optimization to improve the accuracy of the pose of the key frames in the key frame queue, which is beneficial to improving the accuracy of subsequent local maps. Then, the radar point cloud of each frame is adjusted according to the key frames in the key frame queue, the local map is reconstructed, and the current local map is updated, thereby realizing the positioning and mapping of the unmanned vehicle. At the same time, positioning and mapping are carried out simultaneously, and the positioning efficiency is higher.

[0148] Finally, the error equation of the adjacent local maps is established. At this time, the high-precision and reliable postures at both ends of the time-varying scene are kept unchanged during the optimization process, so as to be used to optimize and fine-tune the low-precision posture of the time-varying scene, thereby improving the utilization rate of high-precision and reliable GPS data. The posture of the local map is adjusted with the help of the error equation until the error value of the adjacent local map in the error equation is minimized, and a global map is established by adjusting the posture of the local map in the aisle. The global map eliminates the accumulated error and ensures global consistency, which is used for the precise point cloud registration of the subsequent positioning thread, thereby improving the accuracy of the unmanned vehicle positioning and navigation in the time-varying scene, and improving the failure of the vehicle positioning and navigation in the scene where the environment changes with time.

[0149] Therefore, a high-precision map generation method adopts the method of positioning and mapping at the same time, and combines local optimization and post-processing optimization to reduce the impact of cumulative errors, maintain the global consistency of the map, improve the accuracy of unmanned vehicle positioning and navigation in time-varying scenarios, and improve the failure of vehicle positioning and navigation in scenarios where the environment changes over time.

[0150] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0151] Reference Figure 6 The embodiment of the present application also provides a high-precision map generation device, which corresponds one-to-one to a high-precision map generation method in the above embodiment. The high-precision map generation device includes:

[0152] Fusion module, used to obtain the radar point cloud of the vehicle's current frame in real time;

[0153] The mapping thread startup module is used to set the first frame of radar point cloud as the first key frame, perform point cloud registration from the radar point cloud of the current frame to the first key frame, and obtain the mapping pose of the current frame; establish a key frame queue based on the mapping pose, and set the first key frame in turn for point cloud registration; and adjust the radar point cloud of each frame according to the key frame of the key frame queue, and build a local map based on the adjusted radar point cloud of each frame; the radar point cloud of each frame

[0154] A post-processing optimization module is used to establish an error equation of adjacent local maps based on the position and posture of the local map in combination with the point cloud registration data; and adjust the position and posture of the local map until the error value of the adjacent local map in the error equation is minimized;

[0155] The global map module is used to establish a global map by adjusting the pose of the local map for point cloud registration in the subsequent positioning thread.

[0156] A high-precision map generating device also includes:

[0157] The mapping thread state tracking module is used to set the radar point cloud of the current frame as the second key frame, perform point cloud registration from the second key frame to the local map, and obtain the map pose of the current frame; re-establish the key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially set the next frame of radar point cloud for point cloud registration until the number of key frames in the key frame queue reaches a second preset value; readjust the radar point cloud of each frame according to the key frames of the re-established key frame queue, reconstruct the local map based on the readjusted radar point cloud of each frame, and update the current local map;

[0158] The positioning thread module is used to remove motion distortion from the radar point cloud based on the inertial navigation data to obtain the position and pose increment.

[0159] For the specific definition of a high-precision map generation device, please refer to the definition of a high-precision map generation method above, which will not be repeated here. Each module in the above-mentioned high-precision map generation device can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0160] In one embodiment, a computer device is provided, which may be a server. The computer device includes a processor, a memory, a network interface, and a database connected via a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal via a network connection. When the computer program is executed by the processor, any one of the above-mentioned high-precision map generation methods is implemented.

[0161] In one embodiment, a computer-readable storage medium is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the following steps are implemented:

[0162] Obtain the radar point cloud of the vehicle's current frame in real time;

[0163] The first frame of radar point cloud is set as the first key frame, and point cloud registration is performed on the radar point cloud of the current frame to the first key frame to obtain the mapping pose of the current frame;

[0164] A key frame queue is established based on the mapping pose, and the next frame of radar point cloud is set in sequence for point cloud registration;

[0165] Adjusting the radar point cloud of each frame according to the key frames of the key frame queue, and constructing a local map based on the adjusted radar point cloud of each frame;

[0166] Based on the pose and point cloud registration data of the local map, the error equation of the adjacent local map is established;

[0167] Adjusting the position and posture of the local map until the error value of the adjacent local maps in the error equation is minimized;

[0168] The global map is established by adjusting the pose of the local map.

[0169] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0170] Those skilled in the art will clearly understand that for the sake of convenience and brevity of description, only the division of the above-mentioned functional units and modules is used as an example. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the system can be divided into different functional units or modules to complete all or part of the functions described above.

Claims

1. A method for generating a high-precision map, characterized in that: The following steps are included: Obtain the radar point cloud of the vehicle's current frame in real time; The first frame of radar point cloud is set as the first key frame, and point cloud registration is performed on the radar point cloud of the current frame to the first key frame to obtain the mapping pose of the current frame; A key frame queue is established based on the mapping pose, and the next frame of radar point cloud is set in sequence for point cloud registration; Adjusting the radar point cloud of each frame according to the key frames of the key frame queue, and constructing a local map based on the adjusted radar point cloud of each frame; After obtaining the pose of the last first key frame in the key frame queue, clear the key frame queue; Setting the radar point cloud of the current frame as a second key frame, performing point cloud registration from the second key frame to the local map, and obtaining a map pose of the current frame; Re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frames for point cloud registration; Re-adjusting the radar point cloud of each frame according to the key frames of the re-established key frame queue, reconstructing the local map based on the re-adjusted radar point cloud of each frame, and updating the current local map; Based on the pose and point cloud registration data of the local map, the error equation of the adjacent local map is established; Adjusting the position and posture of the local map until the error value of the adjacent local maps in the error equation is minimized; The global map is established by adjusting the pose of the local map.

2. The method for generating a high-precision map according to claim 1, characterized in that: The step of establishing a key frame queue based on the mapping pose and sequentially setting the next frame of radar point cloud for point cloud registration includes: Determine whether the mapping pose reaches a preset key frame pose difference threshold; If the mapping pose reaches the key frame pose difference threshold, the first key frame is stored in the key frame queue, and the next radar point cloud frame of the current first key frame is set as the current first key frame; Continue to perform point cloud registration and threshold judgment from the radar point cloud of the current frame to the current first key frame, and store the current first key frame in the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a first preset value.

3. The method for generating a high-precision map according to claim 1, characterized in that: After the step of establishing a key frame queue based on the mapping pose and sequentially setting the next frame of radar point cloud for point cloud registration, the method further includes: The loss function including radar matching error, IMU integration error and wheel speed meter error is preset; A nonlinear optimization framework is adopted to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases compared to the output value of the previous loss function.

4. The method for generating a high-precision map according to claim 1, characterized in that: The step of re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frame for point cloud registration includes: Determine whether the difference between the map pose and the pose of the last first key frame exceeds the key frame pose difference threshold; If the difference between the map pose and the pose of the previous key frame exceeds the key frame pose difference threshold, the second key frame is stored in the key frame queue, and the next frame radar point cloud of the current frame is set as the current second key frame; Continue to perform point cloud registration and threshold judgment of the current second key frame to the local map, and store the current second key frame to the key frame queue when the threshold condition is met, until the number of key frames in the key frame queue reaches a second preset value.

5. The method for generating a high-precision map according to claim 1, characterized in that: After the step of re-establishing a key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially setting the second key frame for point cloud registration, the method further includes: The loss function including radar matching error, IMU integration error and wheel speed meter error is preset; A nonlinear optimization framework is adopted to adjust the poses of the key frames in the key frame queue for local optimization until the output value of the loss function decreases compared to the output value of the previous loss function.

6. The method for generating a high-precision map according to any one of claims 1 to 5, characterized in that: The error equation is expressed as, In the formula, Represents the error value of the adjacent local map; represents the inertial navigation integral residual between the previous and next local maps, represents the wheel speed meter residual, represents the radar matching residual; represents the inertial navigation integrated covariance; represents the wheel speed meter covariance; Represents the point cloud registration covariance.

7. The method for generating a high-precision map according to claim 3 or 5, characterized in that: The expression of the loss function is: In the formula, represents the output value of the loss function, represents the residual error of the inertial navigation integral between the previous and next key frames, represents the wheel speed meter residual, represents the radar matching residual; represents the inertial navigation integrated covariance; represents the wheel speed meter covariance; Represents the point cloud registration covariance.

8. The method for generating a high-precision map according to claim 1, characterized in that: The step of point cloud registration also includes: Performing motion distortion removal processing on the radar point cloud to obtain a pose and a pose increment; The pose is used as the pose of the key frame, and the pose increment is used as an initial value to perform point cloud registration.

9. A high-precision map generation device, characterized in that: include, Fusion module, used to obtain the radar point cloud of the vehicle's current frame in real time; A mapping thread startup module is used to set the first frame of radar point cloud as the first key frame, perform point cloud registration from the radar point cloud of the current frame to the first key frame, and obtain the mapping pose of the current frame; establish a key frame queue based on the mapping pose, and sequentially set the next frame of radar point cloud for point cloud registration; and adjust the radar point cloud of each frame according to the key frame of the key frame queue, and construct a local map based on the adjusted radar point cloud of each frame; The mapping thread state tracking module is used to set the radar point cloud of the current frame as the second key frame, perform point cloud registration from the second key frame to the local map, and obtain the map pose of the current frame; re-establish the key frame queue based on the difference between the map pose and the pose of the last first key frame, and sequentially set the next frame of radar point cloud for point cloud registration until the number of key frames in the key frame queue reaches a second preset value; readjust the radar point cloud of each frame according to the key frames of the re-established key frame queue, reconstruct the local map based on the readjusted radar point cloud of each frame, and update the current local map; A post-processing optimization module is used to establish an error equation of adjacent local maps based on the position and posture of the local map and the point cloud registration data; adjust the position and posture of the local map until the error value of the adjacent local map in the error equation is minimized; The global map module is used to build a global map by adjusting the pose of the local map.

10. A computer device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the method according to any one of claims 1 to 8.

11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 8 are implemented.

Images