Three-dimensional point cloud high-precision map labeling system and method for closed place

Abstract

The invention relates to a three-dimensional point cloud high-precision map labeling system and method for a closed place. The method comprises the following steps: collecting three-dimensional point cloud data, positioning data and inertial data of the closed place; performing data processing on the acquired three-dimensional point cloud data, the positioning data and the inertial data, and performing fusion processing on the three-dimensional point cloud data, the positioning data and the inertial data by using a fusion algorithm; generating a three-dimensional map of the closed place based on the processed three-dimensional point cloud data; performing pose estimation on the mechanical equipment in the processed three-dimensional point cloud data, and determining position information of the mechanical equipment in combination with the positioning data; and marking the mechanical equipment on the three-dimensional map according to the determined position information and pose estimation, thereby obtaining a basic map. The method has the advantages of being high in precision, real-time, stable and the like, challenges of complex environments and changeable scenes can be effectively dealt with, and reliable technical support is provided for automatic driving operation of closed places.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and in particular to a three-dimensional point cloud high-precision map annotation system and method for closed places. Background Art

[0002] In the existing technology, the mapping of specific environments such as precast beam yards is usually done manually, or by using a map construction and annotation method based on two-dimensional images or lidar data. The latter collects environmental images or point cloud data, uses image processing or point cloud processing algorithms to extract key information such as roads and obstacles, and then constructs and annotates the map.

[0003] However, these methods have obvious limitations. First, two-dimensional images or lidar data are often not comprehensive and accurate enough in describing environmental information and cannot provide sufficient accuracy. Secondly, the precast beam yard has large equipment such as beam hoists, ground pumps, steel bars, and crane arms that often change positions, requiring high-frequency map updates. Existing methods are inefficient when processing large-scale point cloud data and are difficult to meet real-time requirements. In addition, the stability and robustness of existing methods need to be improved when facing complex environments and changing scenarios.

[0004] The shortcomings of the prior art are mainly manifested in the following aspects:

[0005] Insufficient accuracy: Maps constructed with existing technologies are often not accurate enough to meet the needs of autonomous vehicles for high-precision road network information. This may lead to positioning deviations, path planning errors, and other issues during vehicle driving, affecting operational efficiency and safety.

[0006] Poor real-time performance: Existing technologies are inefficient in processing large-scale point cloud data, making it difficult to update map information in real time. This makes it impossible for autonomous vehicles to respond to environmental changes in a timely manner, reducing operational flexibility and response speed.

[0007] Insufficient stability and robustness: In the face of complex environments and changing scenarios, the stability and robustness of existing technologies need to be improved. For example, in the case of lighting changes, occlusion, noise interference, etc., existing methods may not be able to accurately extract key information, resulting in the failure of map construction and annotation. Summary of the invention

[0008] The purpose of the present invention is to overcome the defects of the prior art and provide a three-dimensional point cloud high-precision map annotation system and method for closed places to solve the problems of existing map construction methods such as insufficient accuracy, poor real-time performance, and insufficient stability and robustness.

[0009] The technical solution to achieve the above purpose is:

[0010] The present invention provides a method for annotating a three-dimensional point cloud high-precision map of a closed place, comprising the following steps:

[0011] Collect 3D point cloud data, positioning data and inertial data in closed places;

[0012] Processing the collected three-dimensional point cloud data, positioning data and inertial data, and fusing the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm;

[0013] Generate a three-dimensional map of the enclosed place based on the processed three-dimensional point cloud data;

[0014] Estimating the position and posture of the mechanical equipment in the processed three-dimensional point cloud data, and determining the position information of the mechanical equipment in combination with the positioning data;

[0015] Mechanical equipment is marked on the three-dimensional map according to the determined location information and posture estimation, thus obtaining a basic map.

[0016] A further improvement of the method for marking a three-dimensional point cloud high-precision map of a closed place of the present invention is that it also includes:

[0017] Installing sensors on the autonomous driving vehicle, and using the installed sensors to collect data from the enclosed space during the autonomous driving process of the vehicle;

[0018] Determine the actual position and actual posture of the mechanical equipment based on the collected data;

[0019] The actual position and actual posture of the mechanical equipment are compared and determined to be consistent with the position information and posture estimation of the corresponding mechanical equipment on the basic map. If they are inconsistent, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

[0020] A further improvement of the method for annotating a three-dimensional point cloud high-precision map of a closed place of the present invention is that the step of collecting three-dimensional point cloud data, positioning data and inertial data of the closed place includes:

[0021] A vehicle is provided, and a laser radar, a real-time differential positioning sensor, and an inertial measurement sensor are installed on the vehicle;

[0022] The vehicle is driven in a closed place. During the driving process, the laser radar is used to obtain three-dimensional point cloud data of the closed place, the real-time differential positioning sensor is used to obtain positioning data of the closed place, and the inertial measurement sensor is used to obtain inertial data of the closed place.

[0023] A further improvement of the method for annotating a three-dimensional point cloud high-precision map of a closed place of the present invention is that the step of fusing the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm comprises:

[0024] Vehicle status x k =[p k , v k ,q k ] T , where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k;

[0025] Motion Model k =[f k , w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data;

[0026] The position, velocity and attitude of the vehicle state can be expressed as:

[0027]

[0028] v k =v k-1 +Δt(C ns f k-1 +g)

[0029]

[0030] C ns =C ns (q k-1 )

[0031] Where Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q;

[0032] The collected 3D point cloud data and positioning data can be expressed as:

[0033] y k =h(x k )+vk

[0034] =H k x k +v k =[ 1 0 0] x k +v k

[0035] =p k +v k

[0036]

[0037] h(x k ) refers to the vehicle state x k Functions that convert to measurement results;

[0038] The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get

[0039] The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in,

[0040]

[0041] F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k ;

[0042] The uncertainty calculation and propagation process is expressed as:

[0043] Calculate the Kalman gain:

[0044] The dynamic error is finally expressed as:

[0045] A further improvement of the method for annotating a three-dimensional point cloud high-precision map of a closed place of the present invention is that it also includes annotating the road position, shape, width and attribute information of the closed place on the three-dimensional map.

[0046] The present invention also provides a three-dimensional point cloud high-precision map annotation system for closed places, comprising:

[0047] A collection unit, used to collect three-dimensional point cloud data, positioning data and inertial data in closed places;

[0048] A data processing unit connected to the acquisition unit, the data processing unit is used to process the acquired three-dimensional point cloud data, positioning data and inertial data, and is also used to fuse the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm;

[0049] a map generating unit connected to the data processing unit, the map generating unit being used to generate a three-dimensional map of the enclosed place based on the processed three-dimensional point cloud data;

[0050] The marking unit connected to the map generating unit is used to mark the position and posture of the mechanical equipment on the three-dimensional map, so as to form a basic map.

[0051] A further improvement of the closed-place three-dimensional point cloud high-precision map annotation system of the present invention is that it further includes a map updating unit, and the map updating unit is connected to the annotation unit and the acquisition unit;

[0052] The collection unit is also used to collect data of the closed place in real time during the process of automatic driving of the vehicle;

[0053] The map updating unit is used to obtain the actual position and actual posture of the mechanical equipment from the collected data of the closed place, and then when it is determined that the actual position and actual posture of the mechanical equipment are inconsistent with the position information and posture estimation of the mechanical equipment corresponding to the basic map, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

[0054] A further improvement of the closed-place three-dimensional point cloud high-precision map annotation system of the present invention is that the acquisition unit is communicatively connected with a laser radar, a real-time differential positioning sensor, and an inertial measurement sensor installed on a vehicle.

[0055] When the vehicle is driving in a closed place, the acquisition unit acquires the three-dimensional point cloud data of the closed place through the laser radar, acquires the positioning data of the closed place through the real-time differential positioning sensor, and acquires the inertial data of the closed place through the inertial measurement sensor.

[0056] A further improvement of the closed-place three-dimensional point cloud high-precision map annotation system of the present invention is that the data processing unit uses a fusion algorithm to fuse the three-dimensional point cloud data, positioning data and inertial data, including:

[0057] Vehicle status x k =[p k , vk ,q k ] T , where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k;

[0058] Motion Model k =[f k , w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data;

[0059] The position, velocity and attitude of the vehicle state can be expressed as:

[0060]

[0061] V k =v k-1 +Δt(C ns f k-1 +g)

[0062]

[0063] C ns =C ns (q k-1 )

[0064] Where Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q;

[0065] The collected 3D point cloud data and positioning data can be expressed as:

[0066] y k =h(x k )+v k

[0067] =H k x k +v k =[1 0 0]x k +v k

[0068] =p k +v k

[0069] ,

[0070] h(x k ) refers to the vehicle state x k Functions that convert to measurement results;

[0071] The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get

[0072] The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in,

[0073] F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k ;

[0074] The uncertainty calculation and propagation process is expressed as:

[0075] Calculate the Kalman gain:

[0076] The dynamic error is finally expressed as:

[0077] A further improvement of the three-dimensional point cloud high-precision map annotation system for closed places of the present invention is that the annotation unit is also used to annotate the road position, shape, width and attribute information of the closed place on the three-dimensional map.

[0078] The beneficial effects of the three-dimensional point cloud high-precision map annotation system and method for closed places of the present invention are:

[0079] The present invention collects 3D point cloud data, positioning data and inertial data, fuses positioning data and inertial data based on an improved fusion algorithm to improve the accuracy of 3D point cloud data, uses software interface and software to build and annotate high-precision maps, establishes basic high-precision maps, and then uses data collection, accurate identification and positioning of large equipment during daily autonomous driving operation to map and update to high-precision maps. The system has the advantages of high accuracy, real-time performance, stability, etc., can effectively cope with the challenges of complex environments and changing scenes, and provide reliable technical support for autonomous driving operations in precast beam yards. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 The present invention is a flow chart of a method for labeling a three-dimensional point cloud high-precision map of a closed place.

[0081] Figure 2 This is a schematic diagram of three-dimensional point cloud data of a closed place, a precast beam yard, collected in the system and method for annotating a three-dimensional point cloud high-precision map of a closed place of the present invention.

[0082] Figure 3 This is a schematic diagram of the three-dimensional map and annotation information generated by the three-dimensional point cloud high-precision map annotation system and method for closed places of the present invention. DETAILED DESCRIPTION

[0083] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0084] See also Figure 1 The present invention provides a three-dimensional point cloud high-precision map annotation system and method for closed places, which are used to construct high-precision three-dimensional maps for closed places, and then provide high-precision map annotation information for autonomous driving vehicles traveling in closed places, so as to support the path planning, navigation and decision-making and other operational needs of autonomous driving vehicles. The closed places of the present invention can be prefabricated beam yards, ports, mining areas or large industrial parks, etc. The three-dimensional point cloud high-precision map annotation system and method of the present invention can quickly establish high-precision maps, significantly improve the operating efficiency of autonomous driving vehicles in closed places such as prefabricated beam yards, and reduce safety risks. The three-dimensional point cloud high-precision map annotation system and method for closed places of the present invention are described below in conjunction with the accompanying drawings.

[0085] See also Figure 1 , showing a flow chart of the method for labeling a three-dimensional point cloud high-precision map of a closed place according to the present invention. Figure 1 , the three-dimensional point cloud high-precision map annotation system for closed places of the present invention is explained.

[0086] like Figure 1As shown, a high-precision three-dimensional point cloud map annotation system for closed places of the present invention comprises a collection unit, a data processing unit, a map generation unit and an annotation unit; wherein the collection unit is connected to the data processing unit, the data processing unit is connected to the map generation unit, and the map generation unit is connected to the standard unit. The collection unit is used to collect three-dimensional point cloud data, positioning data and inertial data of the closed place; as shown Figure 2 As shown, the data processing unit is used to process the collected three-dimensional point cloud data, positioning data and inertial data, and the data processing unit is also used to fuse the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm; the map generation unit is used to generate a three-dimensional map of a closed place based on the processed three-dimensional point cloud data, preferably, using the Fast-Lio-Slam algorithm to generate a three-dimensional map of a closed place. The map can accurately reflect the spatial structure and object distribution of the place; the annotation unit is used to annotate the position and posture of the mechanical equipment on the three-dimensional map, thereby forming a basic map.

[0087] Furthermore, when annotating mechanical equipment, the annotation unit first obtains position information and posture information of the mechanical equipment. The position information and posture information of the mechanical equipment can be manually input into the annotation unit, or can be obtained by the annotation unit based on three-dimensional point cloud data and positioning data.

[0088] Preferably, when collecting data in a closed place, the collection unit is also used to collect images of the closed place to form image data, and the corresponding mechanical equipment is detected by fusing the image data and the three-dimensional point cloud data and estimating the posture to obtain posture information. The posture estimation can be completed automatically by the annotation unit, or it can be manually identified and then input to the annotation unit. Specifically, a camera is installed on the vehicle, and when the vehicle is driving, the camera is used to collect images of the place to form image data.

[0089] When the enclosed place is a prefabricated beam site, the mechanical equipment on site includes beam lifting machines, ground pumps, large steel bars, crane arms, etc.

[0090] Furthermore, on the generated three-dimensional map, information such as road location, width, attributes, traffic instructions, large equipment, etc. are marked to generate a basic map.

[0091] In a specific embodiment of the present invention, the annotation system of the present invention further includes a map updating unit, which is connected to the annotation unit and the collection unit;

[0092] The collection unit is also used to collect data from closed places in real time during the process of automatic driving of the vehicle;

[0093] The map update unit is used to obtain the actual position and actual posture of the mechanical equipment from the collected data of the closed place, and then when it is determined that the actual position and actual posture of the mechanical equipment are inconsistent with the position information and posture estimate of the mechanical equipment corresponding to the basic map, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

[0094] Specifically, the basic map formed by the annotation system of the present invention is used in the automatic driving of the vehicle. The automatic driving control system designs the vehicle's driving route according to the basic map, and controls the vehicle to drive in a closed place to complete the corresponding functions. During the automatic driving of the vehicle, data is collected from the closed place in real time. When it is found that the position of the mechanical equipment on site has changed, the mapping relationship and two-dimensional attribute information of the mechanical equipment in the basic map are updated.

[0095] Furthermore, the map update unit also pre-processes the data collected by the collection unit, performs algorithm fusion processing, and then determines whether the position and posture of the object have changed, and updates the corresponding object on the map after the change. The object includes mechanical equipment, roads, traffic signs, etc. Although large equipment in the prefabricated beam yard may move, the roads and traffic signs in the prefabricated beam yard may also change. After these objects change, the map update unit can update their changes in a timely manner.

[0096] In a specific embodiment of the present invention, the acquisition unit is connected to the laser radar, the real-time differential positioning sensor and the inertial measurement sensor installed on the vehicle.

[0097] When the vehicle is driving in a closed place, the acquisition unit obtains the three-dimensional point cloud data of the closed place through the laser radar, obtains the positioning data of the closed place through the real-time differential positioning sensor, and obtains the inertial data of the closed place through the inertial measurement sensor.

[0098] Preferably, the lidar, real-time differential positioning sensor and inertial measurement sensor are mounted on the top of the vehicle.

[0099] In the data collection stage, the vehicle is driven along the road in a closed place. During the driving process, the three-dimensional point cloud data, positioning data and inertial data of the closed place such as the prefabricated beam yard are collected through the laser radar, real-time differential positioning sensor (RTK), and inertial measurement sensor (IMU), including the location and shape information of objects such as roads, buildings, trees, traffic signs, and large mechanical equipment. Furthermore, when controlling the vehicle to drive, the vehicle is driven on every road in the closed place to obtain more comprehensive data of the closed place.

[0100] In a specific embodiment of the present invention, the data processing unit is used to pre-process the collected three-dimensional point cloud data, positioning data (RTK data), and inertial data (IMU data), including time synchronization, filtering, denoising, alignment and other steps to improve the accuracy and consistency of the data.

[0101] The purpose of data preprocessing is to eliminate noise and errors in the data to make the data more accurate and reliable. The RTK and IMU data are processed by a low-pass filter based on the frequency response, and high-frequency noise is filtered out to improve the stability and accuracy of the data. The adjacent frames of the LiDAR point cloud are aligned through ICP, the displacement between adjacent point clouds is calculated, and the point clouds are spliced ​​to eliminate splicing errors.

[0102] Furthermore, after data preprocessing, the data processing unit also uses the ES-EKF fusion algorithm to fuse the above data. The three-dimensional point cloud data (Lidar data) contains three-dimensional point cloud information, and the RTK data contains absolute coordinate positioning information. However, the acquisition frequency of these two sensors is low, and the IMU acquisition frequency is high, which can be used to correct and improve data accuracy.

[0103] Specifically, the data processing unit uses a fusion algorithm to perform fusion processing on the three-dimensional point cloud data, positioning data, and inertial data, including:

[0104] Vehicle status x k =[p k , v k ,q k ]I, where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k;

[0105] Motion Model k =[f k , w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data;

[0106] The position, velocity and attitude of the vehicle state can be expressed as:

[0107]

[0108] v k =v k-1 +Δt(C ns f k-1 +g)

[0109]

[0110] C ns =C ns (q k-1 )

[0111] Where Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q;

[0112] The collected 3D point cloud data and positioning data can be expressed as:

[0113] y k =h(x k )+v k

[0114] =H k x k +v k =[1 0 0]x k +v k

[0115] =p k +v k

[0116]

[0117] h(x k ) refers to the vehicle state x k The function converted to the measurement result is simplified to H; y k It is the collected 3D point cloud data and positioning data, i.e. the measurement result, which is actually the vehicle position;

[0118] The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get The performance of EFK is improved by introducing error states, and the nonlinear nature is better handled by error linearization, thereby improving the robustness and accuracy of the filter.

[0119] The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in,

[0120] F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k ;

[0121] The uncertainty calculation and propagation process is expressed as:

[0122] Calculate the Kalman gain:

[0123] The dynamic error is finally expressed as: Q k Represents the noise variance of the hypothesis or measurement, which is an algorithm parameter.

[0124] The ES-EKF algorithm is used to fuse lidar, IMU and RTK data to achieve high-precision positioning. The algorithm can effectively handle the uncertainty of various sensor data and improve positioning accuracy.

[0125] Then as Figure 3 As shown, based on the processed three-dimensional point cloud data with absolute coordinate information, a three-dimensional map of the precast beam yard is generated. The map can accurately reflect the location information, spatial structure and distribution of the beam yard, providing a basis for subsequent marking and path planning.

[0126] In a specific implementation of the present invention, the marking unit is further used to mark the road position, shape, width and attribute information of the closed place on the dimensional map.

[0127] On the map, the road location, shape, width, attributes and other information are annotated based on RoadRunner. This information is crucial for the navigation of autonomous vehicles. At the same time, environmental information such as buildings, large equipment, and traffic signs are also annotated so that autonomous vehicles can accurately understand the map environment and traffic rules of the beam yard, and combine the point cloud 3D map and 2D attribute information to form a basic high-precision map of the beam yard.

[0128] According to the actual situation of the beam yard construction work, combined with the marked road and environmental information, the best driving path is selected for the autonomous driving vehicle. This step needs to take into account factors such as the traffic rules, road conditions, obstacles, etc. of the beam yard to ensure that the autonomous driving vehicle can complete the task safely and efficiently.

[0129] Finally, in the daily autonomous driving process, it is better than the actual situation of beam yard operations. Large equipment often changes its position according to the actual needs of the operation, and these large equipment need to be treated as buildings. Therefore, after detecting and identifying the corresponding large equipment, the mapping relationship and two-dimensional attribute information of the equipment in the three-dimensional map are updated.

[0130] The 3D point cloud high-precision map annotation system of the present invention generates a high-precision 3D map by collecting and processing 3D point cloud data, image data, RTK and IMU data, and annotates the road and environmental information in the map in detail, providing detailed navigation information for the autonomous driving vehicle. The system not only improves the navigation accuracy and safety of the autonomous driving vehicle in closed places such as prefabricated beam yards, but also improves the automation level and work efficiency of the beam yard.

[0131] The advantages of the present invention are: high-precision maps are generated through three-dimensional point cloud data, which can accurately reflect the spatial structure and object distribution of closed places; detailed annotation information provides rich navigation information for autonomous driving vehicles; the best driving path is selected according to the actual situation of the place, thereby improving work efficiency; and dangerous areas are marked to improve driving safety.

[0132] In summary, the three-dimensional point cloud high-precision map annotation system for closed places such as cement pouring precast beam yards proposed in the present invention has broad application prospects and practical value.

[0133] The present invention helps cement trucks to drive automatically in closed places such as precast beam yards in industrial parks in the following aspects:

[0134] Improve navigation accuracy: By collecting 3D point cloud data of the precast beam yard and generating high-precision maps, this technology can provide cement trucks with detailed and accurate information on road location, shape, width, etc. Compared with traditional 2D maps, 3D high-precision maps can more accurately reflect the actual spatial structure and object distribution of the beam yard, allowing cement trucks to locate themselves more accurately and drive accurately along the predetermined path.

[0135] Enhanced environmental perception: High-precision maps not only mark road information, but also integrate rich environmental information, such as the location and shape of buildings, trees, traffic signs, etc. This enables cement filling trucks to have a more comprehensive understanding of the environmental conditions in the beam yard, identify and avoid obstacles in advance, and avoid potential safety risks.

[0136] Optimized route planning: Based on the information from high-precision maps, this technology can select the best driving route for cement trucks. Taking into account factors such as traffic rules, road conditions, obstacles, etc. in the beam yard, the system can plan a safe and efficient driving route, thereby improving the operating efficiency of cement trucks in the prefabricated beam yard.

[0137] Improved safety: By marking potentially dangerous areas in the beam yard, such as sharp turns, loading and unloading areas, etc., the technology can remind cement trucks to take necessary safety measures. This helps reduce the possibility of accidents and ensures the safe operation of cement trucks in the beam yard.

[0138] Support for automated operations: This technology provides a solid foundation for the automatic driving of cement trucks. Through high-precision maps and detailed annotation information, the automatic driving system can more accurately control the driving of cement trucks, achieve a higher level of automated operations, reduce reliance on manual operations, and further improve work efficiency and operation quality.

[0139] In summary, the present invention can significantly improve the navigation accuracy, environmental perception capability, path planning efficiency and safety of cement filling trucks in precast beam yards in closed parks, and provide strong support for the automated operation of cement filling trucks.

[0140] The present invention also provides a method for annotating a three-dimensional point cloud high-precision map of a closed place, such as Figure 1 As shown, the marking method includes the following steps:

[0141] Collect 3D point cloud data, positioning data and inertial data in closed places;

[0142] Process the collected 3D point cloud data, positioning data and inertial data, and use fusion algorithm to fuse the 3D point cloud data, positioning data and inertial data;

[0143] Generate a three-dimensional map of the enclosed place based on the processed three-dimensional point cloud data;

[0144] Estimate the pose of the mechanical equipment in the processed three-dimensional point cloud data, and determine the location information of the mechanical equipment in combination with the positioning data;

[0145] Mechanical equipment is marked on the three-dimensional map according to the determined location information and posture estimation, thus obtaining a basic map.

[0146] In a specific embodiment of the present invention, the marking method of the present invention further includes:

[0147] Installing sensors on the autonomous driving vehicle, and using the installed sensors to collect data from the enclosed space during the autonomous driving process of the vehicle;

[0148] Determine the actual position and actual posture of the mechanical equipment based on the collected data;

[0149] The actual position and actual posture of the mechanical equipment are compared to determine whether they are consistent with the position information and posture estimation of the corresponding mechanical equipment on the basic map. If they are inconsistent, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

[0150] In a specific embodiment of the present invention, the step of collecting three-dimensional point cloud data, positioning data and inertial data of a closed place includes:

[0151] A vehicle is provided, and a laser radar, a real-time differential positioning sensor, and an inertial measurement sensor are installed on the vehicle;

[0152] Let the vehicle drive in a closed place. During the driving process, use the lidar to obtain the three-dimensional point cloud data of the closed place, use the real-time differential positioning sensor to obtain the positioning data of the closed place, and use the inertial measurement sensor to obtain the inertial data of the closed place.

[0153] In a specific embodiment of the present invention, the step of fusing the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm includes:

[0154] Vehicle status x k =[p k , v k ,q k ] T , where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k;

[0155] Motion Model k =[f k , w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data;

[0156] The position, velocity and attitude of the vehicle state can be expressed as:

[0157]

[0158] V k =v k-1 +Δt(C ns f k-1 +g)

[0159]

[0160] C ns =C ns (q k-1 )

[0161] Where Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q;

[0162] The collected 3D point cloud data and positioning data can be expressed as:

[0163] y k =h(x k )+v k

[0164] =H k x k +v k =[ 1 0 0]x k +v k

[0165] =p k +v k

[0166]

[0167] h(x k ) refers to the vehicle state x k Functions that convert to measurement results;

[0168] The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get

[0169] The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in,

[0170] F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k;

[0171] The uncertainty calculation and propagation process is expressed as:

[0172] Calculate the Kalman gain:

[0173] The dynamic error is finally expressed as:

[0174] In a specific implementation of the present invention, the marking method of the present invention further includes marking the road position, shape, width and attribute information of the closed place on the three-dimensional map.

[0175] The present invention is described in detail above in conjunction with the embodiments of the accompanying drawings. A person skilled in the art can make various variations of the present invention according to the above description. Therefore, certain details in the embodiments should not constitute a limitation of the present invention, and the scope of protection of the present invention shall be defined by the scope of the attached claims.

Claims

1. A method for annotating a three-dimensional point cloud high-precision map of a closed place, characterized in that: The steps include: Collect 3D point cloud data, positioning data and inertial data in closed places; Processing the collected three-dimensional point cloud data, positioning data and inertial data, and fusing the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm; Generate a three-dimensional map of the enclosed place based on the processed three-dimensional point cloud data; Estimating the position and posture of the mechanical equipment in the processed three-dimensional point cloud data, and determining the position information of the mechanical equipment in combination with the positioning data; Mechanical equipment is marked on the three-dimensional map according to the determined location information and posture estimation, thus obtaining a basic map.

2. The method for labeling a three-dimensional point cloud high-precision map of a closed place according to claim 1, characterized in that: Also includes: Installing sensors on the autonomous driving vehicle, and using the installed sensors to collect data from the enclosed space during the autonomous driving process of the vehicle; Determine the actual position and actual posture of the mechanical equipment based on the collected data; The actual position and actual posture of the mechanical equipment are compared and determined to be consistent with the position information and posture estimation of the corresponding mechanical equipment on the basic map. If they are inconsistent, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

3. The method for labeling a three-dimensional point cloud high-precision map of a closed place according to claim 1, characterized in that: The steps for collecting 3D point cloud data, positioning data, and inertial data of a closed space include: A vehicle is provided, and a laser radar, a real-time differential positioning sensor, and an inertial measurement sensor are installed on the vehicle; The vehicle is driven in a closed place. During the driving process, the laser radar is used to obtain three-dimensional point cloud data of the closed place, the real-time differential positioning sensor is used to obtain positioning data of the closed place, and the inertial measurement sensor is used to obtain inertial data of the closed place.

4. The method for labeling a three-dimensional point cloud high-precision map of a closed place according to claim 1 or 3, characterized in that: The steps of fusing the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm include: Vehicle status x k =[p k , v k ,q k ] T , where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k; Motion Model k =[f k , w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data; The position, velocity and attitude of the vehicle state can be expressed as: v k =v k-1 +Δt(C ns f k-1 +g) in, Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q; The collected 3D point cloud data and positioning data can be expressed as: y k =h(x k )+v k =H k x k +v k =[1 0 0]x k +v k =p k +n k ， h(x k ) means that the vehicle state x k Functions that convert to measurement results; The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in, F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k ; The uncertainty calculation and propagation process is expressed as: Calculate the Kalman gain: The dynamic error is finally expressed as:

5. The method for labeling a three-dimensional point cloud high-precision map of a closed place according to claim 1, characterized in that: It also includes marking the road location, shape, width and attribute information of closed places on the three-dimensional map.

6. A three-dimensional point cloud high-precision map annotation system for closed places, characterized in that: include: A collection unit, used to collect three-dimensional point cloud data, positioning data and inertial data in closed places; A data processing unit connected to the acquisition unit, the data processing unit is used to process the acquired three-dimensional point cloud data, positioning data and inertial data, and is also used to fuse the three-dimensional point cloud data, positioning data and inertial data using a fusion algorithm; a map generating unit connected to the data processing unit, the map generating unit being used to generate a three-dimensional map of the enclosed place based on the processed three-dimensional point cloud data; The marking unit connected to the map generating unit is used to mark the position and posture of the mechanical equipment on the three-dimensional map, so as to form a basic map.

7. The three-dimensional point cloud high-precision map annotation system for closed places according to claim 6, characterized in that: It also includes a map updating unit, which is connected to the marking unit and the collection unit; The collection unit is also used to collect data of the closed place in real time during the process of automatic driving of the vehicle; The map updating unit is used to obtain the actual position and actual posture of the mechanical equipment from the collected data of the closed place, and then when it is determined that the actual position and actual posture of the mechanical equipment are inconsistent with the position information and posture estimation of the mechanical equipment corresponding to the basic map, the actual position and actual posture of the mechanical equipment are mapped and replaced with the corresponding mechanical equipment on the basic map to update the map.

8. The three-dimensional point cloud high-precision map annotation system for closed places according to claim 6, characterized in that: The acquisition unit is connected to the laser radar, real-time differential positioning sensor and inertial measurement sensor installed on the vehicle. When the vehicle is driving in a closed place, the acquisition unit obtains three-dimensional point cloud data of the closed place through the laser radar, obtains positioning data of the closed place through the real-time differential positioning sensor, and obtains inertial data of the closed place through the inertial measurement sensor.

9. The three-dimensional point cloud high-precision map annotation system for closed places according to claim 6 or 8, characterized in that: The data processing unit uses a fusion algorithm to fuse the three-dimensional point cloud data, positioning data and inertial data, including: Vehicle status x k =[p k ,v k ,q k ] T , where p k is the position of the vehicle at time k, v k is the speed of the vehicle at time k, q k is the posture of the vehicle at time k; Motion Model k =[f k ,w k ] T , where f k is the proportional acceleration of the vehicle at time k, w k is the angular velocity of the vehicle at time k, f k and w k Obtained through the collected inertial data; The position, velocity and attitude of the vehicle state can be expressed as: in k =in k-1 +Γt(C ns f k-1 +g) in, Δt represents the sampling time, C ns represents the coordinate transformation matrix to transform the measured acceleration from the sensor coordinates to the coordinates of the position p, g is the gravity acceleration vector, The function representing the vehicle's posture q, q w and q v are two parts of the posture q; The collected 3D point cloud data and positioning data can be expressed as: y k =h(x k )+v k =H k x k +v k =[1 0 0]x k +v k =p k +v k ， h(x k ) means that the vehicle state x k Functions that convert to measurement results; The results are divided into measurements using a fusion algorithm And the error value δx, and then correct the error result After linearizing the motion model, we get The dynamic error is expressed as: δx k =F k-1 δx k-1 +L k-1 n k-1 ,in, F k-1 is the vehicle state transfer formula matrix, which is used to calculate the state at time k based on the state at time k-1. k-1 is the noise transfer matrix, which transforms the noise n k Transform to state space x k ; The uncertainty calculation and propagation process is expressed as: Calculate the Kalman gain: The dynamic error is finally expressed as:

10. The three-dimensional point cloud high-precision map annotation system for closed places according to claim 6, characterized in that: The marking unit is also used to mark the road position, shape, width and attribute information of the closed place on the dimensional map.

Images