A method and system for environmental modeling based on RTK and laser radar

Abstract

The present invention relates to the field of vehicle positioning technology, and is an environment modeling method and system based on RTK and laser radar. The specific method includes: calculating and obtaining a point cloud density correction factor, and dynamically adjusting the scanning density of the laser radar according to the point cloud density correction factor; calculating the position and posture increment of each frame of point cloud data; obtaining the comprehensive position and posture after fusion positioning; checking the similarity between the current frame and the previous key frame through the ICP algorithm, and generating loop detection constraints if they are successfully matched; performing optimization calculations to generate the optimal position and optimal map, and constructing an environment model based on the optimal position and optimal map, and automatically positioning vehicles within the range according to the environment model. The present invention solves the problem that in the prior art, the vehicle positioning method is implemented by laser radar alone, and the position of the current environment in the geographic coordinate system cannot be obtained, so that there is a problem of difficulty in initialization and re-positioning in subsequent autonomous driving applications.

Description

Technical Field

[0001] The invention relates to the technical field of vehicle positioning, and is an environment modeling method and system based on RTK and laser radar. Background Art

[0002] The current common vehicle positioning method uses only lidar. This method cannot obtain the position of the current environment in the geographic coordinate system, that is, the point cloud map produced cannot contain longitude and latitude information, which leads to many problems in subsequent autonomous driving applications: such as the initialization repositioning problem, that is, it is difficult to obtain the position of the autonomous driving vehicle in the map after the initial power-on; the point cloud map cannot be aligned with satellite images or aerial photos, making it difficult to mark road information; at the same time, when the autonomous driving vehicle is driving on a road with a small turning radius, the collection density of its point cloud data will also lead to a decrease in the accuracy of the environmental model.

[0003] In the existing disclosed invention technology, for example, a Chinese patent with publication number CN118549939A discloses a method for global positioning of a vehicle based on a laser radar, which constructs a reference point cloud map, collects environmental information using a laser radar along the optimal walking route of the vehicle in the environment, and constructs the reference point cloud map using a normal distribution transformation algorithm, and records the optimal walking route of the vehicle and the route points contained in the optimal walking route in the reference point cloud map, wherein the route points include the position and posture of the vehicle; the point cloud collected by the laser radar on the vehicle is used as the source point cloud; the route points are used as initial pose candidate points, and a normal distribution transformation algorithm is used to perform registration calculations based on the source point cloud and the reference point cloud map to determine the initial pose required for the vehicle positioning calculation; and the vehicle is globally positioned based on the initial pose required for the vehicle positioning calculation.

[0004] The above patent has high requirements on the quality of the reference point cloud map. If the reference map contains noise, errors or the point cloud information is inaccurate or incomplete due to environmental factors, inaccurate positioning will result. Summary of the invention

[0005] The technical problem to be solved by the present invention is that in the prior art, the vehicle positioning method is implemented solely by using laser radar, which is unable to obtain the position of the current environment in the geographic coordinate system, resulting in the problem of difficulty in initialization and re-positioning in subsequent autonomous driving applications. An environment modeling method and system based on RTK and laser radar are proposed.

[0006] In order to achieve the above object, the technical solution of an environment modeling method based on RTK and laser radar of the present invention comprises the following steps:

[0007] S1: Obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor;

[0008] S2: dynamically capture the point cloud data of the surrounding environment through the lidar and calculate the position increment of each frame of point cloud data;

[0009] S3: Synchronously collect two types of RTK data, fuse the two types of RTK data with the pose increment of each frame of point cloud data, and obtain the comprehensive pose after fusion positioning;

[0010] S4: Check the similarity between the current frame and the previous key frame through the ICP algorithm. If the match is successful, a loop detection constraint is generated;

[0011] S5: perform optimization calculations to generate optimal posture and optimal map, and construct an environment model based on the optimal posture and optimal map;

[0012] S6: Automatically locate vehicles within the range according to the environmental model.

[0013] Specifically, S1 includes the following specific steps:

[0014] S11: extracting real-time driving environment data, the real-time driving environment data including: determining curvature data and road flatness data of the current road through a vehicle-mounted sensor;

[0015] S12: fixing the first frame of point cloud data when the autonomous driving vehicle starts to drive as the baseline point cloud data, and importing the real-time driving environment data and the baseline point cloud data into the point cloud density correction strategy to calculate and obtain the point cloud density correction factor, wherein the point cloud density correction factor includes: a road turning radius correction factor and a road flatness correction factor;

[0016] The road turning radius correction factor The calculation strategy is:

[0017] ;

[0018] in, They respectively represent the tire steering angle of the front wheels of the autonomous vehicle and the lateral tilt angle of the autonomous vehicle relative to the road when the point cloud data is collected at time t;

[0019] Indicates the lateral tilt angle of the autonomous vehicle relative to the road when it starts driving;

[0020] It represents the curvature radius of the road on which the autonomous vehicle is traveling when the point cloud data is collected at time t; Indicates the radius of curvature of the road on which the autonomous vehicle is traveling when it starts driving.

[0021] The road roughness correction factor The calculation strategy is:

[0022] ;

[0023] in, is a subscript, indicating that when the point cloud data is collected at time t, the first A pothole, The total number of potholes on the road surface where the vehicle is traveling in real time;

[0024] It means that when the point cloud data is collected at time t, the first The relative height of the depression or protrusion in a pothole;

[0025] It represents the average relative height of all potholes in the road plane at the time t when the point cloud data is collected;

[0026] is a subscript, indicating that when collecting baseline point cloud data, the first A pothole, is the total number of potholes at the baseline road level;

[0027] Indicates the first The relative height of the depression or protrusion in a pothole;

[0028] It represents the average relative height of all potholes in the road plane when the baseline point cloud data is collected;

[0029] S13: Dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor. The dynamic adjustment strategy is specifically as follows: ,in, is the point cloud collection density at the point cloud data collection time point t; is the point cloud acquisition density in the baseline point cloud data; They respectively represent the road turning radius correction factor and the road flatness correction factor.

[0030] Specifically, S2 includes the following specific steps:

[0031] S21: fix the point cloud in the baseline point cloud dataset as the global map and add all points to the global map;

[0032] S22: Acquire the position information of the laser odometer and the speed data set of the autonomous driving vehicle. Includes: Location data and posture data ; The speed data set of the autonomous driving vehicle includes: linear speed data , angular velocity data and acceleration data ;

[0033] S23: Calculate the relative position and relative posture of the laser radar when collecting each point, where the position compensation strategy is:

[0034] ;

[0035] in, is the relative position after compensation; Represents the timestamp of each point in the point cloud data collected by the LiDAR;

[0036] The posture compensation strategy is:

[0037] ;

[0038] in, is the relative posture after compensation.

[0039] S2 also includes the following specific steps:

[0040] S24: Divide the point cloud according to the line bundles, and calculate the curvature of the local points in the point cloud data. The specific calculation formula of the curvature is: ,in, is the curvature of the ith local point, are the normal vector of the i-th local point and the mean of the normal vectors of all local points in the neighborhood of the i-th local point; are the position coordinates of the ith local point in the global map and the mean position coordinates of all local points in the neighborhood of the ith local point; is the local smoothing factor;

[0041] S25: Preset a curvature threshold, filter out local points in the point cloud data whose curvature is greater than or equal to the curvature threshold to form a point feature set, and filter out local points in the point cloud data whose curvature is less than the curvature threshold to form a surface feature set;

[0042] S26: Calculate the residual between the point features and surface features of the current frame and the baseline point cloud data features in the global map, where the calculation strategy for the residual between the local point and the feature line is:

[0043] r po int − line ( i ) = ‖ q i − projection ( q i , line map ( t )) ‖ × cd [ line map ( t )] CD jx ;

[0044] Indicate point Feature lines in global maps The projection point on ; cd [ line map ( t )] Characteristic line Length, is the mean length of feature lines in the baseline point cloud data;

[0045] The calculation strategy of the residual between the local point and the feature surface is:

[0046] r po int − plane ( i ) = ‖ q i − projection ( q i , plane map ( t )) ‖ × mj [ plane map ( t )] MJ jx ;

[0047] Indicate point Feature faces in the global map The projection point on ;

[0048] mj [ plane map ( t )] The characteristic surface The area of is the mean area of ​​the feature surface in the baseline point cloud data.

[0049] Specifically, S2 also includes the following specific steps:

[0050] S27: Traverse the current frame Point cloud data is used to minimize the residual bi-norm using the LM (Levenberg-Marquardt) method, thereby calculating the optimal position and posture of the current frame in the global map. The optimization formula is: <m> min < / m> ∑ i Di t [ r po int − line ( i ) 2 + r po int − plane ( i ) 2 ] ;

[0051] S28: Output the translation vector and rotation matrix of the current frame relative to the previous frame.

[0052] Specifically, S3 includes the following specific steps:

[0053] S31: Align the RTK positioning system with the laser radar coordinate system through the initial alignment step;

[0054] S32: Use the error state Kalman filter framework to fuse the positioning information from RTK and LiDAR;

[0055] S33: Output the fused key frame positioning result at a frequency of 1 Hz.

[0056] Specifically, in S4, the generation strategy of the loop detection constraint is:

[0057] S41: Save the point features and surface features of each frame and add them to the key frame;

[0058] S42: Traverse all key frames that are more than 30 seconds apart and less than 7 meters from the current key frame, and evaluate and calculate the similarity feature value of the building in each key frame. The similarity feature value is calculated as follows: ;in, Keyframe The difference in feature F between them; D ( F J 1 , F J 2 ) =− ln[ ∑ x = 1 X H J 1 ( x ) × H J 2 ( x ) ] ; Keyframes The color histogram of the building in , wherein the color histogram consists of X bins; x is the index of the bin, and X is the total number of bins in the histogram; Represents a key frame The overlapping parts in the color histogram of the buildings;

[0059] S43: Preset the first similarity registration threshold. When the similarity eigenvalue is greater than or equal to the first similarity registration threshold, filter the previous frame as the first preliminary screening key frame. When the similarity eigenvalue is less than the first similarity registration threshold, both key frames are the first preliminary screening key frames. Traverse all key frames and filter to obtain the first preliminary screening key frame set.

[0060] Specifically, in S4, the generation strategy of the loop detection constraint further includes:

[0061] S44: Using the ICP registration algorithm, the first preliminary screening key frame set is further screened to extract key frames from the first preliminary screening key frame set. The rotation matrix and the corresponding displacement vector , calculate the relative pose eigenvalue , the calculation strategy of the relative posture feature value is: E ( Z 1 , Z 2 ) = [ JZ 2 T ⋅ JZ 1 , wy 1 − JZ 2 T ⋅ wy 1 ] ;

[0062] S45: According to S41-S44, output loop detection constraints, wherein the loop detection constraints are specifically:

[0063] ;

[0064] Among them, Loop is the loop detection constraint function; They are the constraint proportional coefficients of the similarity eigenvalue and the relative pose eigenvalue respectively.

[0065] Specifically, S5 includes the following steps:

[0066] S51: Subscribe to the comprehensive pose and loop detection constraints after fusion positioning;

[0067] S52: extracting the comprehensive pose of each frame after fusion positioning;

[0068] S53: The comprehensive posture and loop detection constraints after the fusion positioning are added to the optimization problem, and the optimization calculation is performed to generate the optimal posture and the optimal map.

[0069] In addition, the present invention provides an environment modeling system based on RTK and laser radar, which includes the following modules:

[0070] Point cloud density correction module, pose increment calculation module, fusion positioning module, loop detection module, environment model construction module and vehicle positioning module;

[0071] The point cloud density correction module is used to obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate and obtain the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor;

[0072] The pose increment calculation module dynamically captures point cloud data of the surrounding environment through a laser radar, and calculates the pose increment of each frame of point cloud data;

[0073] The fusion positioning module is used to synchronously collect two types of RTK data, fuse the two types of RTK data with the position and posture increment of each frame of point cloud data, and obtain the comprehensive position and posture after fusion positioning;

[0074] The loop detection module checks the similarity between the current frame and the previous key frame through the ICP algorithm, and generates loop detection constraints if they are successfully matched;

[0075] The environment model construction module is used to perform optimization calculations, generate optimal postures and optimal maps, and construct an environment model according to the optimal postures and optimal maps;

[0076] The vehicle positioning module automatically positions vehicles within a range according to the environmental model.

[0077] A storage medium stores instructions, and when a computer reads the instructions, the computer executes the environmental modeling method based on RTK and laser radar.

[0078] An electronic device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the above-mentioned environment modeling method based on RTK and laser radar is implemented.

[0079] Compared with the prior art, the technical effects of the present invention are as follows:

[0080] 1. The present invention can better adapt to different road conditions, such as the curvature and flatness of the road, by dynamically adjusting the scanning density of the laser radar, thereby improving the quality and density of the point cloud data, thereby improving the accuracy of vehicle positioning, and solving the problem of sparse or over-dense point clouds of the laser radar under different road conditions, so that the point cloud data can maintain high quality under various environmental conditions.

[0081] 2. The present invention fuses the two RTK data with the laser radar's position increments to provide more accurate and stable positioning information. The RTK system provides high-precision positioning data, while the laser radar provides detailed geometric information of the environment. The combination of the two can significantly improve the accuracy of overall positioning, solving the shortcomings of a single sensor in terms of positioning accuracy and stability, especially in complex or dynamic environments.

[0082] 3. The present invention can identify and correct the accumulated errors in the map through loop detection technology, and enhance the global consistency of the map. This is especially important for vehicle positioning in long-term operation, which can significantly improve the accuracy of the environmental model, solve the problem of continuous accumulation of map errors in long-term operation, and improve the reliability and stability of the environmental model.

[0083] 4. The present invention simultaneously considers the registration of relative pose and building similarity in loop detection, and can perform multiple verifications on loop candidate frames. By comparing features such as the color histogram of the building, the authenticity of the loop candidate frames can be further confirmed, the probability of mismatching can be reduced, and the robustness of loop detection can be improved. In addition, in a complex urban environment, relative pose information may be affected by noise and dynamic factors, while the characteristics of the building are relatively stable. Therefore, the introduction of building similarity can help the system better improve the accuracy of vehicle positioning in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative labor. Among them:

[0085] Figure 1A schematic diagram of a flow chart of an environment modeling method based on RTK and laser radar of the present invention;

[0086] Figure 2 A schematic diagram of a process for dynamically adjusting the scanning density of a laser radar;

[0087] Figure 3 It is a structural schematic diagram of an environment modeling system based on RTK and laser radar of the present invention;

[0088] Figure 4 This is an example diagram of a scenario where a point cloud density correction factor needs to be calculated to dynamically adjust the scanning density of the laser radar;

[0089] Figure 5 This is an example diagram of an application scenario for generating a loop detection constraint strategy by evaluating the similarity feature values ​​of buildings. DETAILED DESCRIPTION

[0090] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the accompanying drawings.

[0091] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0092] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The term "in one embodiment" that appears in different places in this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive with other embodiments.

[0093] Embodiment 1:

[0094] like Figure 1 and Figure 2 As shown, an environment modeling method based on RTK and laser radar in an embodiment of the present invention is as follows: Figure 1 As shown, the specific steps are as follows:

[0095] S1: Obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor;

[0096] like Figure 2 As shown, S1 includes the following specific steps:

[0097] S11: extracting real-time driving environment data, the real-time driving environment data including: determining curvature data and road flatness data of the current road through a vehicle-mounted sensor;

[0098] S12: Fix the first frame of point cloud data when the autonomous driving vehicle starts to drive as the baseline point cloud data, and import the real-time driving environment data and the baseline point cloud data into the point cloud density correction strategy to calculate and obtain the point cloud density correction factor. For example, Figure 4 As shown, in this embodiment, an example diagram of a scene in which a point cloud density correction factor needs to be calculated to dynamically adjust the scanning density of the laser radar is provided. Figure 4 In the driving environment where the self-driving car is in a sharp bend and there are cracks and potholes on the road, it is necessary to dynamically adjust the scanning density of the lidar to better adapt Figure 4 Road conditions in.

[0099] In this embodiment, the point cloud density correction factor is calculated according to the curvature of the road and the road flatness, and the point cloud density correction factor includes: a road turning radius correction factor and a road flatness correction factor;

[0100] The road turning radius correction factor The calculation strategy is:

[0101] ;

[0102] in, They respectively represent the tire steering angle of the front wheels of the autonomous vehicle and the lateral tilt angle of the autonomous vehicle relative to the road when the point cloud data is collected at time t;

[0103] Indicates the lateral tilt angle of the autonomous vehicle relative to the road when it starts driving;

[0104] It represents the curvature radius of the road on which the autonomous vehicle is traveling when the point cloud data is collected at time t; Indicates the radius of curvature of the road on which the autonomous vehicle is traveling when it starts driving.

[0105] The road roughness correction factor The calculation strategy is:

[0106] ;

[0107] in, is a subscript, indicating that when the point cloud data is collected at time t, the first A pothole, The total number of potholes on the road surface where the vehicle is traveling in real time;

[0108] It means that when the point cloud data is collected at time t, the first The relative height of the depression or protrusion in a pothole;

[0109] It represents the average relative height of all potholes in the road plane at the time t when the point cloud data is collected;

[0110] is a subscript, indicating that when collecting baseline point cloud data, the first A pothole, is the total number of potholes at the baseline road level;

[0111] Indicates the first The relative height of the depression or protrusion in a pothole;

[0112] It represents the average relative height of all potholes in the road plane when the baseline point cloud data is collected;

[0113] S13: Dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor. The dynamic adjustment strategy is specifically as follows: ,in, is the point cloud collection density at the point cloud data collection time point t; is the point cloud acquisition density in the baseline point cloud data; They respectively represent the road turning radius correction factor and the road flatness correction factor.

[0114] S2: dynamically capture the point cloud data of the surrounding environment through the lidar and calculate the position increment of each frame of point cloud data;

[0115] S2 includes the following specific steps:

[0116] S21: fix the point cloud in the baseline point cloud dataset as the global map and add all points to the global map;

[0117] S22: Acquire the position information of the laser odometer and the speed data set of the autonomous driving vehicle. Includes: Location data and posture data ; The speed data set of the autonomous driving vehicle includes: linear speed data , angular velocity data and acceleration data ;

[0118] S23: Calculate the relative position and relative posture of the laser radar when collecting each point, where the position compensation strategy is:

[0119] ;

[0120] in, is the relative position after compensation; Represents the timestamp of each point in the point cloud data collected by the LiDAR;

[0121] The posture compensation strategy is:

[0122] ;

[0123] in, is the relative posture after compensation.

[0124] S2 also includes the following specific steps:

[0125] S24: Divide the point cloud according to the line bundles, and calculate the curvature of the local points in the point cloud data. The specific calculation formula of the curvature is: ,in, is the curvature of the ith local point, are the normal vector of the i-th local point and the mean of the normal vectors of all local points in the neighborhood of the i-th local point; are the position coordinates of the ith local point in the global map and the mean position coordinates of all local points in the neighborhood of the ith local point; is the local smoothing factor;

[0126] S25: Preset a curvature threshold, filter out local points in the point cloud data whose curvature is greater than or equal to the curvature threshold to form a point feature set, and filter out local points in the point cloud data whose curvature is less than the curvature threshold to form a surface feature set;

[0127] S26: Calculate the residual between the point features and surface features of the current frame and the baseline point cloud data features in the global map, where the calculation strategy for the residual between the local point and the feature line is:

[0128] r po int − line ( i ) = ‖ q i − projection ( q i , line map ( t )) ‖ × cd [ line map ( t )] CD jx ;

[0129] Indicate point Feature lines in global maps The projection point on ; cd [ line map ( t )] Characteristic line Length, is the mean length of feature lines in the baseline point cloud data;

[0130] The calculation strategy of the residual between the local point and the feature surface is:

[0131] r po int − plane ( i ) = ‖ q i − projection ( q i , plane map ( t )) ‖ × mj [ plane map ( t )] MJ jx ;

[0132] Indicate point Feature faces in the global map The projection point on ;

[0133] mj [ plane map ( t )] The characteristic surface The area of is the mean area of ​​the feature surface in the baseline point cloud data.

[0134] S2 also includes the following specific steps:

[0135] S27: Traverse the current frame Point cloud data is used to minimize the residual bi-norm using the LM method, thereby calculating the optimal position and posture of the current frame in the global map. The optimization formula is: <m> min < / m> ∑ i Di t [ r po int − line ( i ) 2 + r po int − plane ( i ) 2 ] ;

[0136] S28: Output the translation vector and rotation matrix of the current frame relative to the previous frame.

[0137] S3: Synchronously collect two types of RTK data, fuse the two types of RTK data with the pose increment of each frame of point cloud data, and obtain the comprehensive pose after fusion positioning;

[0138] S3 includes the following specific steps:

[0139] S31: Align the RTK positioning system with the laser radar coordinate system through the initial alignment step;

[0140] S32: Use the error state Kalman filter framework to fuse the positioning information from RTK and LiDAR;

[0141] S33: Output the fused key frame positioning result at a frequency of 1 Hz.

[0142] S4: Check the similarity between the current frame and the previous key frame through the ICP algorithm. If the match is successful, a loop detection constraint is generated;

[0143] In S4, the generation strategy of the loop detection constraint is specifically:

[0144] S41: Save the point features and surface features of each frame and add them to the key frame;

[0145] S42: Traverse all key frames that are more than 30 seconds apart and less than 7 meters from the current key frame, and evaluate and calculate the similarity feature value of the building in each key frame. For example, Figure 5 As shown, in this embodiment, an example diagram of an application scenario for generating loop detection constraints by real-time evaluation of similarity feature values ​​of buildings in all key frames in a real-time driving environment of a car is provided. Figure 5 When there are multiple similar curved road conditions, the authenticity of the loop candidate frames can be further confirmed by comparing features such as the building's appearance color histogram or geometric angles, which can reduce the probability of false matching and improve the robustness of loop detection.

[0146] Preferably, in this embodiment, the similarity feature value is calculated as follows: ;in, Keyframe The difference in feature F between them; D ( F J 1 , F J 2 ) =− ln[ ∑ x = 1 X H J 1 ( x ) × H J 2 ( x ) ] ; Keyframes The color histogram of the building in , wherein the color histogram consists of X bins; x is the index of the bin, and X is the total number of bins in the histogram; Represents a key frame The overlapping parts in the color histogram of the buildings;

[0147] S43: Preset the first similarity registration threshold. When the similarity eigenvalue is greater than or equal to the first similarity registration threshold, filter the previous frame as the first preliminary screening key frame. When the similarity eigenvalue is less than the first similarity registration threshold, both key frames are the first preliminary screening key frames. Traverse all key frames and filter to obtain the first preliminary screening key frame set.

[0148] In S4, the generation strategy of the loop detection constraint further includes:

[0149] S44: Using the ICP registration algorithm, the first preliminary screening key frame set is further screened to extract key frames from the first preliminary screening key frame set. The rotation matrix and the corresponding displacement vector , calculate the relative pose eigenvalue , the calculation strategy of the relative posture feature value is: E ( Z 1 , Z 2 ) = [ JZ 2 T ⋅ JZ 1 , wy 1 − JZ 2 T ⋅ wy 1 ] ;

[0150] S45: According to S41-S44, output loop detection constraints, wherein the loop detection constraints are specifically:

[0151] ;

[0152] Among them, Loop is the loop detection constraint function; They are the constraint proportional coefficients of the similarity eigenvalue and the relative pose eigenvalue respectively.

[0153] S5: Perform optimization calculations to generate an optimal pose and an optimal map, and construct an environment model based on the optimal pose and the optimal map.

[0154] S5 includes the following steps:

[0155] S51: Subscribe to the comprehensive pose and loop detection constraints after fusion positioning;

[0156] S52: extracting the comprehensive pose of each frame after fusion positioning;

[0157] S53: adding the comprehensive posture and loop detection constraints after the fusion positioning to the optimization problem, performing optimization calculation, and generating the optimal posture and optimal map;

[0158] Exemplarily, in this embodiment, the environment model is constructed by minimum pose and minimum loop detection constraint function values.

[0159] S6: Automatically locate vehicles within the range according to the environmental model.

[0160] Embodiment 2:

[0161] like Figure 3 As shown, an environment modeling system based on RTK and laser radar according to an embodiment of the present invention is as follows: Figure 3 As shown, it includes the following modules:

[0162] Point cloud density correction module, pose increment calculation module, fusion positioning module, loop detection module, environment model construction module and vehicle positioning module;

[0163] The point cloud density correction module is used to obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate and obtain the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor;

[0164] The pose increment calculation module dynamically captures point cloud data of the surrounding environment through a laser radar, and calculates the pose increment of each frame of point cloud data;

[0165] The fusion positioning module is used to synchronously collect two types of RTK data, fuse the two types of RTK data with the position and posture increment of each frame of point cloud data, and obtain the comprehensive position and posture after fusion positioning;

[0166] The loop detection module checks the similarity between the current frame and the previous key frame through the ICP algorithm, and generates loop detection constraints if they are successfully matched;

[0167] The environment model construction module is used to perform optimization calculations, generate optimal postures and optimal maps, and construct an environment model according to the optimal postures and optimal maps;

[0168] The vehicle positioning module automatically positions vehicles within a range according to the environmental model.

[0169] Embodiment three:

[0170] This embodiment provides an electronic device, including: a processor and a memory, wherein the memory stores a computer program that can be called by the processor;

[0171] The processor executes the above-mentioned environment modeling method based on RTK and laser radar by calling the computer program stored in the memory.

[0172] The electronic device may have relatively large differences due to different configurations or performances, and may include one or more processors (Central Processing Units, CPU) and one or more memories, wherein the memory stores at least one computer program, and the computer program is loaded and executed by the processor to implement an RTK and LiDAR-based environment modeling method provided in the above method embodiment. The electronic device may also include other components for implementing the functions of the device, for example, the electronic device may also have components such as a wired or wireless network interface and an input and output interface to input and output data. This embodiment will not be described in detail here.

[0173] Embodiment 4:

[0174] This embodiment provides a computer-readable storage medium having a rewritable computer program stored thereon;

[0175] When the computer program runs on a computer device, the computer device executes the above-mentioned environment modeling method based on RTK and laser radar.

[0176] For example, the computer readable storage medium can be a read-only memory (ROM), a random access memory (RAM), a compact disc (CD-ROM), a magnetic tape, a floppy disk, an optical data storage device, etc.

[0177] It should be understood that in the various embodiments of the present application, the size of the serial numbers of the above-mentioned processes 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.

[0178] It should be understood that determining B based on A does not mean determining B only based on A. B can also be determined based on A and / or other information.

[0179] The above embodiments can be implemented in whole or in part by software, hardware, firmware or any other combination. When implemented by software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When a computer instruction or computer program is loaded or executed on a computer, a process or function according to an embodiment of the present invention is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. Computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, computer instructions can be transmitted from one website site, computer, server or data center to another website site, computer, server or data center through a wired network or / and a wireless network. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state hard disk.

[0180] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in the present invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0181] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0182] In the several embodiments provided by the present invention, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of units is only one, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0183] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0184] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0185] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0186] In summary, compared with the prior art, the technical effects of the present invention are as follows:

[0187] 1. The present invention can better adapt to different road conditions, such as the curvature and flatness of the road, by dynamically adjusting the scanning density of the laser radar, thereby improving the quality and density of the point cloud data, thereby improving the accuracy of vehicle positioning, and solving the problem of sparse or over-dense point clouds of the laser radar under different road conditions, so that the point cloud data can maintain high quality under various environmental conditions.

[0188] 2. The present invention fuses the two RTK data with the laser radar's position increments to provide more accurate and stable positioning information. The RTK system provides high-precision positioning data, while the laser radar provides detailed geometric information of the environment. The combination of the two can significantly improve the accuracy of overall positioning, solving the shortcomings of a single sensor in terms of positioning accuracy and stability, especially in complex or dynamic environments.

[0189] 3. The present invention can identify and correct the accumulated errors in the map through loop detection technology, and enhance the global consistency of the map. This is especially important for vehicle positioning in long-term operation, which can significantly improve the accuracy of the environmental model, solve the problem of continuous accumulation of map errors in long-term operation, and improve the reliability and stability of the environmental model.

[0190] 4. The present invention simultaneously considers the registration of relative pose and building similarity in loop detection, and can perform multiple verifications on loop candidate frames. By comparing features such as the color histogram of the building, the authenticity of the loop candidate frames can be further confirmed, the probability of mismatching can be reduced, and the robustness of loop detection can be improved. In addition, in a complex urban environment, relative pose information may be affected by noise and dynamic factors, while the characteristics of the building are relatively stable. Therefore, the introduction of building similarity can help the system better improve the accuracy of vehicle positioning in complex environments.

[0191] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It should be understood by those skilled in the art that the present invention is not limited to the above embodiments. The above embodiments and descriptions are only for explaining the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. An environment modeling method based on RTK and laser radar, characterized in that: The method comprises the following specific steps: S1: Obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor; S2: dynamically capture the point cloud data of the surrounding environment through the lidar and calculate the position increment of each frame of point cloud data; S3: Synchronously collect two types of RTK data, fuse the two types of RTK data with the pose increment of each frame of point cloud data, and obtain the comprehensive pose after fusion positioning; S4: Check the similarity between the current frame and the previous key frame through the ICP algorithm. If the match is successful, a loop detection constraint is generated; S5: perform optimization calculations to generate optimal posture and optimal map, and construct an environment model based on the optimal posture and optimal map; S6: Automatically positioning vehicles within the range according to the environmental model; S1 includes the following specific steps: S11: extracting real-time driving environment data, the real-time driving environment data including: determining curvature data and road flatness data of the current road through a vehicle-mounted sensor; S12: fixing the first frame of point cloud data when the autonomous driving vehicle starts to drive as the baseline point cloud data, and importing the real-time driving environment data and the baseline point cloud data into the point cloud density correction strategy to calculate and obtain the point cloud density correction factor, wherein the point cloud density correction factor includes: a road turning radius correction factor and a road flatness correction factor; The road turning radius correction factor The calculation strategy is: ； in, They respectively represent the tire steering angle of the front wheels of the autonomous vehicle and the lateral tilt angle of the autonomous vehicle relative to the road when the point cloud data is collected at time t; Indicates the lateral tilt angle of the autonomous vehicle relative to the road when it starts driving; It represents the curvature radius of the road on which the autonomous vehicle is traveling when the point cloud data is collected at time t; Indicates the radius of curvature of the road on which the autonomous vehicle is traveling when it starts driving. The road roughness correction factor The calculation strategy is: ； in, is a subscript, indicating that when the point cloud data is collected at time t, the first A pothole, The total number of potholes on the road surface where the vehicle is traveling in real time; It means that when the point cloud data is collected at time t, the first The relative height of the depression or protrusion in a pothole; It represents the average relative height of all potholes in the road plane at the time t when the point cloud data is collected; is a subscript, indicating that when collecting baseline point cloud data, the first A pothole, is the total number of potholes at the baseline road level; Indicates the first The relative height of the depression or protrusion in a pothole; It represents the average relative height of all potholes in the road plane when the baseline point cloud data is collected; S13: Dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor. The dynamic adjustment strategy is specifically as follows: ,in, is the point cloud collection density at the point cloud data collection time point t; is the point cloud acquisition density in the baseline point cloud data; They respectively represent the road turning radius correction factor and the road flatness correction factor.

2. The method for environmental modeling based on RTK and laser radar according to claim 1, characterized in that: S2 includes the following specific steps: S21: fix the point cloud in the baseline point cloud dataset as the global map and add all points to the global map; S22: Acquire the position information of the laser odometer and the speed data set of the autonomous driving vehicle. Includes: Location data and posture data ; The speed data set of the autonomous driving vehicle includes: linear speed data , angular velocity data and acceleration data ; S23: Calculate the relative position and relative posture of the laser radar when collecting each point, where the position compensation strategy is: ； in, is the relative position after compensation; Represents the timestamp of each point in the point cloud data collected by the LiDAR; The posture compensation strategy is: ； in, is the relative posture after compensation.

3. The method for environmental modeling based on RTK and laser radar according to claim 2, characterized in that: S2 also includes the following specific steps: S24: dividing the point cloud according to line bundles, and calculating the local point curvature in the point cloud data; S25: Preset a curvature threshold, filter out local points in the point cloud data whose curvature is greater than or equal to the curvature threshold to form a point feature set, and filter out local points in the point cloud data whose curvature is less than the curvature threshold to form a surface feature set; S26: Calculate the residual between the point features and surface features of the current frame and the baseline point cloud data features in the global map.

4. The method for environmental modeling based on RTK and laser radar according to claim 3 is characterized in that: S2 also includes the following specific steps: S27: Traverse the current frame Point cloud data, using the LM method to minimize the residual bi-norm, thereby calculating the optimal position and posture of the current frame in the global map; S28: Output the translation vector and rotation matrix of the current frame relative to the previous frame.

5. The method for environmental modeling based on RTK and laser radar according to claim 4, characterized in that: S3 includes the following specific steps: S31: Align the RTK positioning system with the laser radar coordinate system through the initial alignment step; S32: Use the error state Kalman filter framework to fuse the positioning information from RTK and LiDAR; S33: Output the fused key frame positioning result at a frequency of 1 Hz.

6. The method for environmental modeling based on RTK and laser radar according to claim 5, characterized in that: In S4, the generation strategy of the loop detection constraint is specifically: S41: Save the point features and surface features of each frame and add them to the key frame; S42: traverse all key frames that are more than 30 seconds apart and within 7 meters from the current key frame, and evaluate and calculate the similarity feature value of the building in each key frame; S43: Preset the first similarity registration threshold. When the similarity eigenvalue is greater than or equal to the first similarity registration threshold, filter the previous frame as the first preliminary screening key frame. When the similarity eigenvalue is less than the first similarity registration threshold, both key frames are the first preliminary screening key frames. Traverse all key frames and filter to obtain the first preliminary screening key frame set.

7. The method for environmental modeling based on RTK and laser radar according to claim 6, characterized in that: In S4, the generation strategy of the loop detection constraint further includes: S44: Using the ICP registration algorithm, the first preliminary screening key frame set is further screened to extract key frames from the first preliminary screening key frame set. The rotation matrix and the corresponding displacement vector , calculate the relative pose eigenvalue ; S45: Output loop detection constraints according to S41-S44.

8. The method for environmental modeling based on RTK and laser radar according to claim 7, characterized in that: S5 includes the following steps: S51: Subscribe to the comprehensive pose and loop detection constraints after fusion positioning; S52: extracting the comprehensive pose of each frame after fusion positioning; S53: The comprehensive posture and loop detection constraints after the fusion positioning are added to the optimization problem, and the optimization calculation is performed to generate the optimal posture and the optimal map.

9. An environment modeling system based on RTK and laser radar, which is used to implement an environment modeling method based on RTK and laser radar as described in any one of claims 1 to 8, characterized in that: The system includes the following modules: Point cloud density correction module, pose increment calculation module, fusion positioning module, loop detection module, environment model construction module and vehicle positioning module; The point cloud density correction module is used to obtain the real-time driving environment data of the driverless car, import the real-time driving environment data into the point cloud density correction strategy, calculate and obtain the point cloud density correction factor, and dynamically adjust the scanning density of the laser radar according to the point cloud density correction factor; The pose increment calculation module dynamically captures point cloud data of the surrounding environment through a laser radar, and calculates the pose increment of each frame of point cloud data; The fusion positioning module is used to synchronously collect two types of RTK data, fuse the two types of RTK data with the position and posture increment of each frame of point cloud data, and obtain the comprehensive position and posture after fusion positioning; The loop detection module checks the similarity between the current frame and the previous key frame through the ICP algorithm, and generates loop detection constraints if they are successfully matched; The environment model construction module is used to perform optimization calculations, generate optimal postures and optimal maps, and construct an environment model according to the optimal postures and optimal maps; The vehicle positioning module automatically positions vehicles within a range according to the environmental model.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, an environment modeling method based on RTK and laser radar as described in any one of claims 1 to 8 is implemented.

11. An electronic device, characterized in that: include: A memory for storing instructions; A processor is used to execute the instructions so that the device performs operations to implement an environment modeling method based on RTK and lidar as described in any one of claims 1-8.

Images