Multi-sensor fusion positioning method and system in dynamic degradation environment of construction tunnel

Abstract

The invention discloses a multi-sensor fusion positioning method and system in a construction tunnel dynamic degradation environment, and the method comprises the steps: carrying out the calculation based on UWB distance measurement data, obtaining the position information of each UWB base station and the current mileage of each segment, and carrying out the fusion of the current mileage of each segment, tunnel design data, the point cloud of each construction rack, and the point cloud of an excavation segment, according to the method, the dynamic updating of the construction tunnel point cloud map is realized, different point cloud construction strategies are adopted for each segment, the real-time three-dimensional point cloud map can be quickly and accurately constructed, and the real-time requirements of scene matching and positioning such as automatic driving and unmanned inspection on the global point cloud map are well met. Moreover, in the multi-sensor fusion positioning stage, each UWB base station is utilized to provide high-frequency continuous distance measurement data for the to-be-positioned vehicle as an anchor chain, and constraint can be provided in the degradation direction, so that degradation does not occur in the point cloud registration link, and high-precision real-time positioning in the dynamic degradation environment of the construction tunnel can be realized.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor fusion positioning, and in particular, to a multi-sensor fusion positioning method and system, an electronic device, and a computer-readable storage medium in a dynamically degenerated environment of a construction tunnel. Background Art

[0002] When performing multi-sensor fusion positioning in a tunnel under construction, problems such as dynamic environmental changes and scene degradation will be encountered. Among them, dynamic environmental changes refer to that as the tunnel excavation progresses, the built section, the structural construction section, and the excavation section advance forward, and the point cloud map of the environment and the coordinates of the UWB base stations arranged in the environment need to be updated. The degenerated environment refers to an environment in which, due to the lack of environmental features, the point clouds and images collected by lidar, vision cameras, etc. have a high degree of consistency, such as long straight tunnels, underground pipe galleries, and underground roadways. In such a dynamically degenerated environment, the point cloud map and the UWB base station position in the scene are changing, and due to environmental degradation, the registration based on lidar point clouds and images will have a large error or complete failure in the axial direction of the tunnel. Therefore, it is difficult to perform dynamic positioning in such a dynamically degenerated environment. However, with the development of technologies such as robots and sensors, the demand for unmanned driving and automated inspection in such a dynamically degenerated environment is becoming increasingly strong. Therefore, it is of great significance to carry out multi-sensor fusion positioning in a dynamically highly degenerated environment.

[0003] Currently, most of the existing ground positioning technologies rely on GPS to provide a global reference for fusion positioning, but it is not feasible in a closed space due to satellite signal occlusion; deploying markers such as reflective stickers, reflective columns, two-dimensional codes, RFID, etc. in a degenerated environment to transform the original degenerated environment into a non-degenerated environment is an idea to solve such problems, but there are defects such as a large number of markers to be deployed, easy to be soiled, and frequent updates required as the tunnel construction progresses; using the method of mapless dynamic SLAM (Simultaneous Localization and Mapping) for positioning has problems such as cumulative error and matching degradation; and the existing multi-sensor fusion positioning methods mostly use a weakly coupled method when using lidar and UWB ranging data. First, it uses the laser scan point cloud to register with the point cloud map, and then fuses the registration result with the UWB ranging data. A large error will be introduced in the direction of environmental degradation in the laser registration link, which will have an adverse impact on the subsequent fusion link, reducing the positioning accuracy or even causing positioning drift. Summary of the Invention

[0004] The present invention provides a multi-sensor fusion positioning method and system, an electronic device, and a computer-readable storage medium for a construction tunnel in a dynamic degradation environment, which can realize dynamic update of the point cloud map of the construction tunnel and can also provide constraints in the degradation direction, so that the point cloud registration link will not degenerate, thereby enabling high-precision real-time positioning in the dynamic degradation environment of the construction tunnel.

[0005] According to one aspect of the present invention, there is provided a multi-sensor fusion positioning method for a construction tunnel in a dynamic degradation environment, including the following:

[0006] Based on the UWB ranging data, calculate the three-dimensional space coordinates of each UWB base station and the current mileage of each segment of the construction tunnel. Among them, each segment of the construction tunnel includes a completed segment, a structured construction segment, and an excavation segment. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed segment, and mobile UWB base stations are installed on the construction benches of the structured construction segment and the excavation segment;

[0007] Obtain the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation segment, and generate a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each segment of the construction tunnel;

[0008] Obtain the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned. Use the three-dimensional space coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be positioned in the degradation direction, and in combination with the real-time three-dimensional point cloud map of the construction tunnel, fuse and position the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned in the dynamic degradation environment to obtain the positioning result of the vehicle to be positioned.

[0009] Further, the process of fusing and positioning the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned in the dynamic degradation environment includes the following:

[0010] Integrate based on the IMU data to obtain the attitude and perform complementary filtering processing to obtain the corrected value of the IMU gyroscope;

[0011] Use the corrected value of the IMU gyroscope to perform distortion removal processing on the laser point cloud data to obtain the distortion-removed laser point cloud data;

[0012] Perform rasterization processing on the real-time three-dimensional point cloud map of the construction tunnel and calculate the probability distribution within each grid;

[0013] Calculate the likelihood probability based on the distortion-removed laser point cloud data and the probability distribution within each grid, and construct a point cloud likelihood error;

[0014] Construct a ranging error based on the three-dimensional space coordinates of each UWB base station and the current ranging value of the vehicle to be positioned;

[0015] Fusion registration is performed based on the point cloud likelihood error and the ranging error, and the optimal registration result is obtained by solving.

[0016] Taking the IMU integration result as the prediction and the optimal registration result as the observation, the solution is carried out according to the standard error state Kalman filter, and the final positioning result is output.

[0017] Further, the ranging error is calculated based on the following formula:

[0018]

[0019] where ψ2 represents the ranging error, C i represents the three-dimensional spatial coordinates of the i-th mobile UWB base station, D i represents the current ranging value between the positioning tag on the vehicle to be positioned and the i-th mobile UWB base station, T(p) represents the current pose of the vehicle to be positioned, and p represents the pose parameter.

[0020] Further, the fusion registration of the point cloud likelihood error and the ranging error is carried out based on the following formula:

[0021] J = J1 + μ × J2

[0022] H = H1 + μ × H2

[0023] where J and H respectively represent the fused Jacobian matrix and Hessian matrix, J1 and H1 respectively represent the Jacobian matrix and Hessian matrix of the point cloud likelihood error, J2 and H2 respectively represent the Jacobian matrix and Hessian matrix of the ranging error, and μ represents the fusion ratio of the ranging error.

[0024] Further, the Jacobian matrix and Hessian matrix of the ranging error are calculated based on the following formula:

[0025]

[0026] where C i represents the three-dimensional spatial coordinates of the i-th mobile UWB base station, D i represents the current ranging value between the positioning tag on the vehicle to be positioned and the i-th mobile UWB base station, T(p) represents the current pose of the vehicle to be positioned, and p represents the pose parameter.

[0027] Further, when the tunnel is under construction at a large mileage and the reference tag is behind the bench, or under construction at a small mileage and the reference tag is in front of the bench, the current mileage calculation formula of each mobile UWB base station is:

[0028]

[0029] When the tunnel is under construction at the small mileage and the reference tag is behind the gantry, or when the UWB base station is in front of the gantry during the large mileage construction, the current mileage calculation formula for each mobile UWB base station is as follows:

[0030]

[0031] Among them, L i represents the current mileage of the i-th mobile UWB base station, L1 represents the mileage of the reference tag, and w and h respectively represent the lateral deviation and height deviation of the reference tag relative to each mobile UWB base station in the tunnel.

[0032] Furthermore, the process of obtaining the tunnel design data, the point cloud data of each construction gantry, and the point cloud data of the excavation section, and generating the real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel includes the following contents:

[0033] Obtain the theoretical secondary lining contour point cloud data of the tunnel design data and the current mileage of the mobile UWB base station on the secondary lining trolley, and calculate the completed section point cloud in the tunnel coordinate system;

[0034] Obtain the point cloud data of each construction gantry in the structured construction section and the current mileage of the mobile UWB base station on each construction gantry in the structured construction section, and calculate the point cloud of the structural construction section in the tunnel coordinate system;

[0035] Obtain the point cloud data of the excavation section and the current mileage of the mobile UWB base station on the excavation section construction gantry, and calculate the point cloud of the excavation section in the tunnel coordinate system;

[0036] Stitch the completed section point cloud, the structural construction section point cloud, and the excavation section point cloud to obtain the real-time three-dimensional point cloud map of the construction tunnel.

[0037] In addition, the present invention also provides a multi-sensor fusion positioning system in a dynamically degraded environment of a construction tunnel, including:

[0038] A UWB ranging data processing module for calculating the three-dimensional space coordinates of each UWB base station and the current mileage of each section of the construction tunnel based on the UWB ranging data. Among them, each section of the construction tunnel includes a completed section, a structured construction section, and an excavation section. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed section, and mobile UWB base stations are installed on the construction gantries of the structured construction section and the excavation section;

[0039] A dynamic point cloud map generation module for obtaining the tunnel design data, the point cloud data of each construction gantry, and the point cloud data of the excavation section, and generating the real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel;

[0040] The multi-sensor fusion positioning module is used to obtain the lidar point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned. It uses the three-dimensional spatial coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be positioned in the degradation direction, and combines with the real-time three-dimensional point cloud map of the construction tunnel to fuse and position the lidar point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned in a dynamic degradation environment, so as to obtain the positioning result of the vehicle to be positioned.

[0041] In addition, the present invention also provides an electronic device, including a processor and a memory. A computer program is stored in the memory, and the processor is used to execute the steps of the method described above by calling the computer program stored in the memory.

[0042] In addition, the present invention also provides a computer-readable storage medium for storing a computer program for multi-sensor fusion positioning in a dynamic degradation environment of a construction tunnel. When the computer program runs on a computer, it executes the steps of the method described above.

[0043] The present invention has the following beneficial effects:

[0044] The multi-sensor fusion positioning method in the dynamic degradation environment of the construction tunnel of the present invention uses UWB ranging data analysis and calculation to obtain the position information of each UWB base station in the construction tunnel and the current mileage of each section of the construction tunnel, providing a data basis for subsequent multi-sensor fusion positioning. It also fuses the current mileage of each section of the construction tunnel with the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section, realizing the dynamic update of the point cloud map of the construction tunnel. Moreover, the construction tunnel is divided into a completed section, a structured construction section, and an excavation section, and different point cloud construction strategies are adopted for each section, which can quickly and accurately construct the real-time three-dimensional point cloud map of the construction tunnel, and can well meet the real-time requirements of the global point cloud map for scene matching positioning such as autonomous driving and unmanned inspection. Furthermore, in the multi-sensor fusion positioning stage, using each UWB base station to provide high-frequency and continuous ranging data for the vehicle to be positioned as an anchor chain can provide constraints in the degradation direction, so that the point cloud registration link will not degenerate, and thus high-precision real-time positioning in the dynamic degradation environment of the construction tunnel can be realized.

[0045] In addition, the multi-sensor fusion positioning system in the dynamic degradation environment of the construction tunnel of the present invention also has the above advantages.

[0046] In addition to the purposes, features, and advantages described above, the present invention has other purposes, features, and advantages. The following will refer to the drawings to further describe the present invention in detail. Brief Description of the Drawings

[0047] The accompanying drawings, which form a part of this application, are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the accompanying drawings:

[0048] Figure 1 is a schematic flow chart of a multi-sensor fusion positioning method in a dynamic degradation environment of a construction tunnel according to a preferred embodiment of this application;

[0049] Figure 2 is a schematic diagram of installing UWB base stations in a construction tunnel according to a preferred embodiment of this application;

[0050] Figure 3 is Figure 1 a sub-flow schematic diagram of step S2 in

[0051] Figure 4 is Figure 1 a sub-flow schematic diagram of step S3 in

[0052] Figure 5 is a schematic diagram of the module structure of a multi-sensor fusion positioning system in a dynamic degradation environment of a construction tunnel according to another embodiment of this application. Specific Embodiments

[0053] It should be noted that, without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The following will refer to the accompanying drawings and combine the embodiments to detail this application.

[0054] Referring to Figure 1 , a preferred embodiment of this application provides a multi-sensor fusion positioning method in a dynamic degradation environment of a construction tunnel, including the following:

[0055] Step S1: Calculate the three-dimensional space coordinates of each UWB base station and the current mileage of each segment of the construction tunnel based on the UWB ranging data. Among them, each segment of the construction tunnel includes a completed segment, a structured construction segment, and an excavation segment. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed segment, and mobile UWB base stations are installed on the construction platforms of the structured construction segment and the excavation segment;

[0056] Step S2: Obtain the tunnel design data, the point cloud data of each construction platform, and the point cloud data of the excavation segment, and generate a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each segment of the construction tunnel;

[0057] Step S3: Obtain the lidar point cloud data, UWB ranging data, and IMU data of the vehicle to be located. Use the three-dimensional spatial coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be located in the degradation direction. Combine with the real-time three-dimensional point cloud map of the construction tunnel, and perform fusion positioning on the lidar point cloud data, UWB ranging data, and IMU data of the vehicle to be located in the dynamic degradation environment to obtain the positioning result of the vehicle to be located.

[0058] It can be understood that for the multi-sensor fusion positioning method in the dynamic degradation environment of the construction tunnel in this embodiment, the position information of each UWB base station in the construction tunnel and the current mileage of each section of the construction tunnel are obtained by analyzing and calculating the UWB ranging data, providing a data basis for subsequent multi-sensor fusion positioning. The current mileage of each section of the construction tunnel is also fused with the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section to realize the dynamic update of the point cloud map of the construction tunnel. Moreover, the construction tunnel is divided into a completed section, a structured construction section, and an excavation section, and different point cloud construction strategies are adopted for each section, which can quickly and accurately construct the real-time three-dimensional point cloud map of the construction tunnel, and can well meet the real-time requirements of the global point cloud map for scenario matching positioning such as autonomous driving and unmanned inspection. Furthermore, in the multi-sensor fusion positioning stage, the high-frequency continuous ranging data provided by each UWB base station for the vehicle to be located is used as an anchor chain, which can provide constraints in the degradation direction, so that the point cloud registration link will not degenerate, and thus high-precision real-time positioning in the dynamic degradation environment of the construction tunnel can be achieved.

[0059] Among them, in the step S1, as Figure 2 shown, the present invention divides the construction tunnel into a completed section, a structured construction section, and an excavation section. The completed section refers to the section where secondary lining or segment laying has been carried out. The structured construction section refers to the section where construction such as lining is being carried out. The excavation section refers to the section where excavation construction is being carried out. The present invention deploys UWB base stations and positioning tags in the construction tunnel. Specifically, fixed UWB base stations and reference tags are installed on the tunnel wall of the completed section. Mobile UWB base stations are installed on the construction benches in the structured construction section and the excavation section. For example, a mobile UWB base station is set on the excavation bench in the excavation section, and a mobile UWB base station is installed on the lining bench in the structured construction section, etc. The initial global coordinates of each UWB base station are obtained through equipment such as total stations. In addition, according to the ranging values between each UWB base station and the reference tag, the mileage of the reference tag can be calculated based on the triangular conversion relationship. The specific calculation process belongs to the prior art and will not be elaborated here. Of course, the mileage of the installation position can also be directly obtained after installing the reference tag.

[0060] It can be understood that during the tunnel construction process, by obtaining the ranging data between each mobile UWB base station and the reference tag, the coordinates of each mobile UWB base station can be calculated. Among them, when the tunnel is under construction at the large mileage and the reference tag is behind the bench, or under construction at the small mileage and the reference tag is in front of the bench, the current mileage calculation formula for each mobile UWB base station is:

[0061]

[0062] When the tunnel is under construction at the small mileage and the reference tag is behind the bench, or the UWB base station at the large mileage is in front of the bench, the current mileage calculation formula for each mobile UWB base station is:

[0063]

[0064] Among them, L i represents the current mileage of the i-th mobile UWB base station, L1 represents the mileage of the reference tag, and w and h respectively represent the lateral deviation and height deviation of the reference tag relative to each mobile UWB base station in the tunnel. After calculating the current mileage of each mobile UWB base station on the construction bench, the current mileage of each mobile UWB base station is the current mileage of the corresponding construction bench. For special benches, the current mileage of the secondary lining bench is the mileage of the completed section, and the current mileage of the excavation bench is the mileage of the excavation section. Moreover, according to the tunnel design data, the three-dimensional space coordinates of each mobile UWB base station can be obtained after knowing the mileage. In addition, in railway construction, extending from the starting point in a specific direction, the farther away from the starting point, the larger the mileage value, which is called the large mileage; in railway construction, extending from the starting point in a specific direction, the farther away from the starting point, the smaller the mileage value, which is called the small mileage.

[0065] It can be understood that in the present invention, by installing mobile UWB base stations on the construction benches in the structured construction section and the excavation section, and installing reference tags on the tunnel wall in the completed section, the current mileage of each mobile UWB base station can be calculated through the ranging values between each mobile UWB base station and the reference tag, and thus the current mileage of each section of the construction tunnel can be obtained, so as to obtain the position information of the dynamically changing objects in the construction tunnel, providing a data basis for the subsequent dynamic update of the tunnel point cloud and multi-sensor fusion positioning.

[0066] It can be understood that as Figure 3 shown, in the step S2, the process of obtaining the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section, and generating the real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel includes the following content:

[0067] Step S21: Obtain the theoretical secondary lining contour point cloud data of the tunnel design data and the current mileage of the mobile UWB base station on the secondary lining trolley, and calculate the completed section point cloud in the tunnel coordinate system;

[0068] Step S22: Obtain the point cloud data of each construction scaffold in the structured construction section and the current mileage of the mobile UWB base station on each construction scaffold in the structured construction section, and calculate the structural construction section point cloud in the tunnel coordinate system;

[0069] Step S23: Obtain the excavation section point cloud data and the current mileage of the mobile UWB base station on the excavation section construction scaffold, and calculate the excavation section point cloud in the tunnel coordinate system;

[0070] Step S24: Stitch the completed section point cloud, the structural construction section point cloud, and the excavation section point cloud to obtain the real-time three-dimensional point cloud map of the construction tunnel.

[0071] Specifically, first obtain the theoretical secondary lining contour point cloud data of the tunnel design data and discretize it into point cloud S1. After calculating the current mileage of the mobile UWB base station on the secondary lining trolley based on step S1, according to Discretize the point cloud S1 into different rings, and calculate the transformation matrix of each ring to the tunnel coordinate system T i1 , and then stitch the point clouds of each ring to obtain the completed section point cloud C1 in the tunnel coordinate system as: C1 = ∑T i1 S1.

[0072] It can be understood that when generating the completed section point cloud in the present invention, after obtaining the current mileage of the completed section based on the UWB ranging data, the point cloud map is generated using the tunnel design data. To update it, only the mileage of the completed section needs to be updated and the foregoing steps are repeated to achieve point cloud update. The point cloud generation and update are highly efficient, with less computational effort and good dynamic performance.

[0073] Then, use the station-setting laser scanning device to perform multi-angle scanning and segmentation on each construction scaffold in the structured construction section, stitch the point clouds to obtain the individual point cloud data of each construction scaffold in the structured construction section, and then use the transformation matrix T i2 from the station to the tunnel coordinate system during station setting, i to transform the point cloud data of each construction scaffold to the tunnel coordinate system. The transformation formula is: p′ i2 -1 (x, y, z) = T i2 -1 p i (x, y, z), where p i (x, y, z) represents the point cloud data before transformation, and p′ i (x, y, z) represents the point cloud data after transformation. Then, according to the current mileage of each construction scaffold and the transformation matrix T i2 from the station to the tunnel coordinate system during station setting, calculate the transformation matrix T from the point cloud coordinate system to the tunnel coordinate system at the current mileagej2 Thus, the structured construction section point cloud C2 in the tunnel coordinates can be calculated as: C2 = ∑T j2 p′ i .

[0074] It can be understood that when generating the structured construction section point cloud in the present invention, the UWB ranging data is used to obtain the current mileage of each construction gantry in the tunnel structured construction section and the individual point clouds of each construction gantry to be pre-scanned, realizing the point cloud construction of dynamically stitching the construction gantry point clouds in the global point cloud, and avoiding large point cloud deviations in the structured construction section due to the existence of a large number of construction machinery when generating the structured construction section point cloud.

[0075] Next, the excavation section point cloud data S2 is scanned by the scanning device installed on the excavation gantry. After obtaining the current mileage of the mobile UWB base station on the excavation gantry in the excavation section, the transformation matrix T3 from the excavation section point cloud to the tunnel coordinate system can be calculated. Thus, the excavation section point cloud C3 in the tunnel coordinate system can be calculated as: C3 = T3S2.

[0076] It can be understood that when generating the excavation section point cloud in the present invention, due to the relatively frequent spatial changes in the excavation section, a fixed scanning device is used to scan it. Specifically: first, the fixed scanning device is installed on the excavation gantry, then it is calibrated using a total station, and then combined with the current mileage of the mobile UWB base station on the excavation gantry, the transformation matrix T3 from the scanned point cloud to the tunnel coordinate system can be calculated. Finally, the excavation section point cloud S2 obtained by the real-time scanning of the fixed scanning device is passed through the transformation matrix T3 to obtain the current excavation section point cloud C3 in the tunnel coordinate system, realizing the point cloud construction of dynamically stitching the excavation section point cloud in the global point cloud, avoiding large point cloud deviations caused by a large number of construction machinery and excavation errors in the excavation section when generating the excavation section point cloud. To update it, only by repeating the foregoing steps in combination with the current mileage of the excavation section can the point cloud be updated, and the point cloud generation and update are highly efficient, with less computational effort and good dynamic performance.

[0077] Finally, the completed section point cloud C1, the structured construction section point cloud C2, and the excavation section point cloud C3 are stitched together to obtain the real-time three-dimensional point cloud map C of the construction tunnel: C = C1 + C2 + C3. Subsequently, when it is detected that the UWB ranging value changes, or the excavation section point cloud changes significantly, repeating the foregoing steps can realize the dynamic update of the tunnel point cloud map.

[0078] It can be understood that the present invention divides the construction tunnel into a completed section, a structured construction section, and an excavation section, and adopts different point cloud construction strategies for each section, which can quickly and accurately construct the real-time three-dimensional point cloud map of the construction tunnel, and can well meet the real-time requirements of the global point cloud map for scene matching and positioning in scenarios such as autonomous driving and unmanned inspection.

[0079] It can be understood that after obtaining the three-dimensional spatial coordinates of each mobile UWB base station according to step S1 and obtaining the real-time three-dimensional point cloud map according to step S2, the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located are obtained. Then, the three-dimensional spatial coordinates of each UWB base station can be used to provide constraints for the UWB ranging data of the vehicle to be located in the degradation direction, and combined with the real-time three-dimensional point cloud map of the construction tunnel, the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located are fused and located in the dynamic degradation environment to obtain the positioning result of the vehicle to be located. Among them, a positioning tag is installed on the vehicle to be located, and each UWB base station can measure the ranging value to the positioning tag in real time. Among them, as Figure 4 shown, the process of fusing and positioning the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located in the dynamic degradation environment includes the following contents:

[0080] Step S31: Integrate based on the IMU data to obtain the attitude and perform complementary filtering processing to obtain the correction value of the IMU gyroscope;

[0081] Step S32: Use the correction value of the IMU gyroscope to perform distortion removal processing on the laser point cloud data to obtain the distortion-removed laser point cloud data;

[0082] Step S33: Perform rasterization processing on the real-time three-dimensional point cloud map of the construction tunnel and calculate the probability distribution within each grid;

[0083] Step S34: Calculate the likelihood probability based on the distortion-removed laser point cloud data and the probability distribution within each grid, and construct the point cloud likelihood error;

[0084] Step S35: Construct a ranging error based on the three-dimensional spatial coordinates of each UWB base station and the current ranging value of the vehicle to be located;

[0085] Step S36: Perform fusion registration based on the point cloud likelihood error and the ranging error, and solve to obtain the optimal registration result;

[0086] Step S37: Use the IMU integration result as the prediction and the optimal registration result as the observation, and solve according to the standard error state Kalman filter to output the final positioning result.

[0087] Specifically, first integrate based on the IMU data to obtain the attitude and perform complementary filtering processing to obtain the correction value of the IMU gyroscope. The specific process is as follows:

[0088] First, average the output values of the IMU gyroscope during a period when the carrier is stationary as the gyro zero bias estimate, and directly subtract it from the measurement data to remove the constant error of the angular velocity in the IMU measurement;

[0089] During the stationary period, use the acceleration measurement values to obtain the initial attitude by the following formula:

[0090]

[0091] yaw = 0

[0092] where a x , a y and a z represent the output values of the IMU accelerometer, roll, pitch, and yaw represent the roll angle value, pitch angle value, and heading angle value of the carrier attitude respectively. After converting them into the quaternion attitude representation, they are used as the initial values of the subsequent attitude;

[0093] After the stationary period ends, use the following formula to obtain the real-time attitude:

[0094]

[0095] where represents the quaternion attitude representation at the previous moment, represents the updated quaternion attitude representation at the current moment, represents the three-axis angular velocity output value of the IMU gyroscope, and T represents the update time interval;

[0096] Then, use the acceleration measurement value and the updated quaternion attitude representation at the current moment to construct an error:

[0097]

[0098] where e represents the error, represents the quaternion attitude representation at the current moment;

[0099] Then, use the PI controller to calculate the correction amount of the gyro measurement value: γ = K p e + K i ∫e, where K p and K i represent the proportional factor and the integral factor respectively, and γ represents the correction amount;

[0100] Finally, use the correction amount to correct the gyro measurement value and then perform the attitude update in the next cycle. The correction formula is: ω' = ω + γ, where ω' represents the corrected value of the IMU gyroscope.

[0101] Then, use the corrected value ω' of the IMU gyroscope to perform distortion removal processing on the lidar point cloud data to obtain the distortion-removed lidar point cloud data. The specific process is as follows: For each scan point of the lidar of the vehicle to be located and the IMU gyroscope corrected value ω' (ω' x , ω' y , ω'z ) Integrate to obtain the changed pose ΔR of each scan point relative to the frame start i , so that the position of each scan point after distortion correction can be calculated at the current vehicle pose: p i ′(x, y, z) = ΔR i p i (x, y, z), p i (x, y, z) represents the original coordinates of the scan point, p i ′(x, y, z) represents the coordinates of the scan point after distortion correction.

[0102] Next, rasterize the real-time three-dimensional point cloud map of the construction tunnel and calculate the probability distribution within each grid. The specific process is as follows: Divide the real-time three-dimensional point cloud map of the construction tunnel into grids according to the set grid size, and calculate the normal distribution parameters of the three-dimensional point cloud map within each grid. Assume p i represents the xyz coordinates of the i-th scan point within the grid, then: Among them, μ0 represents the mean of the coordinates of all scan points within the grid, σ represents the variance of the coordinates of all scan points within the grid, m represents the number of scan points within the grid, then the normal probability distribution within the grid is:

[0103] Then, substitute each scan point in the distortion-corrected lidar point cloud data into the above normal probability distribution function p(x) for likelihood probability calculation, and perform cumulative multiplication to obtain the likelihood probability: Among them, ψ1 represents the likelihood probability (i.e., the point cloud likelihood error), Π represents the consecutive multiplication operation, y i represents the coordinates of the i-th scan point, p represents the pose parameter, T(p, y i ) represents the current pose, and n represents the number of points in the scan point cloud.

[0104] Next, calculate the ranging error based on the following formula:

[0105]

[0106] Among them, ψ2 represents the ranging error, C i represents the three-dimensional space coordinates of the i-th mobile UWB base station, D i represents the current ranging value from the positioning tag on the vehicle to be located to the i-th mobile UWB base station, T(p) represents the current pose of the vehicle to be located, and p represents the pose parameter.

[0107] Then, based on the point cloud likelihood error and the ranging error, fusion registration is performed, and the optimal registration result is obtained by solving. Among them, in the registration stage, for the likelihood error, according to the standard NDT (Normal Distribution Transform) matching process, the Jacobian matrix J1 and the Hessian matrix H1 of the point cloud likelihood error can be calculated. For the ranging error, the Jacobian matrix and the Hessian matrix of the ranging error are calculated based on the following formula:

[0108]

[0109]

[0110] Among them, J2 and H2 respectively represent the Jacobian matrix and the Hessian matrix of the ranging error, and C i represents the three-dimensional space coordinates of the i-th mobile UWB base station, and D i represents the current ranging value from the positioning tag on the vehicle to be located to the i-th mobile UWB base station. T(p) represents the current pose of the vehicle to be located, and p represents the pose parameter. Then, based on the following formula, fusion registration of the point cloud likelihood error and the ranging error is performed:

[0111] J = J1 + μ × J2

[0112] H = H1 + μ × H2

[0113] Among them, J and H respectively represent the fused Jacobian matrix and the Hessian matrix, and μ represents the fusion ratio of the ranging error. The Newton method is used for optimization and solution, which can be expressed as: HΔp = -J, p = p + Δp, so that the optimal registration result can be obtained by solving. Among them, the specific Newton method optimization and solution process belongs to the prior art and will not be elaborated here.

[0114] Finally, using the IMU integration result as the prediction and the optimal registration result as the observation, the solution is carried out according to the standard error state Kalman filter, and the final positioning result is output. Among them, the specific standard error state Kalman filter solution process belongs to the prior art and will not be elaborated here.

[0115] It can be understood that the present invention incorporates UWB ranging data into the point cloud registration link. First, the UWB ranging information and the lidar scan point cloud information are fused into a whole. The result after fusion registration is then subjected to error Kalman filtering with the IMU data to output positioning information. Since a ranging error term is added to the error function of the registration, strong constraints are imposed on the degradation direction using the UWB ranging data, so that the error in the degradation direction will not appear in the final fusion link, which can greatly improve the positioning accuracy and robustness in the degraded environment.

[0116] In addition, asFigure 5 As shown in the figure, another embodiment of the present invention further provides a multi-sensor fusion positioning system in a dynamically degraded environment of a construction tunnel, preferably adopting the multi-sensor fusion positioning method in the dynamically degraded environment of a construction tunnel as described above, including:

[0117] A UWB ranging data processing module, configured to calculate the three-dimensional spatial coordinates of each UWB base station and the current mileage of each section of the construction tunnel based on the UWB ranging data. Among them, each section of the construction tunnel includes a completed section, a structured construction section, and an excavation section. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed section, and mobile UWB base stations are installed on the construction benches of the structured construction section and the excavation section;

[0118] A dynamic point cloud map generation module, configured to obtain tunnel design data, point cloud data of each construction bench, and point cloud data of the excavation section, and generate a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel;

[0119] A multi-sensor fusion positioning module, configured to obtain the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned, use the three-dimensional spatial coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be positioned in the degradation direction, and combine the real-time three-dimensional point cloud map of the construction tunnel to perform fusion positioning on the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned in the dynamically degraded environment, so as to obtain the positioning result of the vehicle to be positioned.

[0120] It can be understood that the multi-sensor fusion positioning system in the dynamically degraded environment of the construction tunnel in this embodiment uses UWB ranging data analysis to calculate the position information of each UWB base station in the construction tunnel and the current mileage of each section of the construction tunnel, providing a data basis for subsequent multi-sensor fusion positioning. It also fuses the current mileage of each section of the construction tunnel with the tunnel design data, point cloud data of each construction bench, and point cloud data of the excavation section to realize the dynamic update of the point cloud map of the construction tunnel. Moreover, the construction tunnel is divided into a completed section, a structured construction section, and an excavation section, and different point cloud construction strategies are adopted for each section, which can quickly and accurately construct a real-time three-dimensional point cloud map of the construction tunnel, and can well meet the real-time requirements of the global point cloud map for scenario matching positioning such as autonomous driving and unmanned inspection. Furthermore, in the multi-sensor fusion positioning stage, using each UWB base station to provide high-frequency continuous ranging data for the vehicle to be positioned as an anchor chain can provide constraints in the degradation direction, so that the point cloud registration link will not degenerate, and thus high-precision real-time positioning in the dynamically degraded environment of the construction tunnel can be realized.

[0121] In addition, another embodiment of the present invention further provides an electronic device, including a processor and a memory. A computer program is stored in the memory, and the processor is configured to execute the steps of the method described above by invoking the computer program stored in the memory.

[0122] In addition, another embodiment of the present invention further provides a computer-readable storage medium for storing a computer program for multi-sensor fusion positioning in a dynamically degraded environment of a construction tunnel. The computer program, when running on a computer, executes the steps of the method described above.

[0123] The forms of common computer-readable storage media generally include: floppy disks, flexible disks, hard disks, magnetic tapes, any other magnetic media, CD-ROMs, any other optical media, punch cards, paper tapes, any other physical media with a pattern of holes, random access memories (RAMs), programmable read-only memories (PROMs), erasable programmable read-only memories (EPROMs), flash erasable programmable read-only memories (FLASH-EPROMs), any other memory chips or cartridges, or any other media readable by a computer. Instructions can further be transmitted or received by a transmission medium. The term transmission medium can include any tangible or intangible medium that can be used to store, encode, or carry instructions for execution by a machine, and includes digital or analog communication signals or an intangible medium that facilitates the communication of the above instructions. The transmission medium includes coaxial cables, copper wires, and optical fibers, which include the wires of a bus for transmitting a computer data signal.

[0124] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) containing computer-usable program code. The solutions in the embodiments of the present application can be implemented in various computer languages, for example, object-oriented programming languages such as Java and interpreted scripting languages such as JavaScript.

[0125] This application is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in one or more of the flows Figure 1 one or more flows and / or blocks Figure 1 or means for implementing the functions specified in one or more of the blocks.

[0126] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means that implement the functions specified in one or more of the flows Figure 1 one or more flows and / or blocks Figure 1 or means for implementing the functions specified in one or more of the blocks.

[0127] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate computer-implemented processing Process, so as to be in a computer or Instructions executed on other programmable devices provide for implementing in a process Figure 1 pieces one or more flows and / or blocks Figure 1 or steps for implementing the functions specified in one or more of the blocks.

[0128] Although the preferred embodiments of the present application have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concepts. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications falling within the scope of the present application.

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

[0130] The above description is only for the preferred embodiments of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A multi-sensor fusion positioning method under the dynamic degradation environment of a construction tunnel, characterized in that It includes the following: Calculating the three-dimensional spatial coordinates of each UWB base station and the current mileage of each section of the construction tunnel based on the UWB ranging data, where each section of the construction tunnel includes a completed section, a structured construction section, and an excavation section. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed section, and mobile UWB base stations are installed on the construction benches of the structured construction section and the excavation section; Obtaining the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section, and generating a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel; Obtaining the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located, using the three-dimensional spatial coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be located in the degradation direction, and combining with the real-time three-dimensional point cloud map of the construction tunnel to perform fusion positioning on the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located in a dynamic degradation environment to obtain the positioning result of the vehicle to be located.

2. The multi-sensor fusion positioning method under the dynamic degradation environment of a construction tunnel according to claim 1, wherein The process of performing fusion positioning on the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be located in a dynamic degradation environment includes the following: Integrating based on the IMU data to obtain the attitude and performing complementary filtering processing to obtain the corrected value of the IMU gyroscope; Using the corrected value of the IMU gyroscope to perform distortion removal processing on the laser point cloud data to obtain the distortion-removed laser point cloud data; Performing rasterization processing on the real-time three-dimensional point cloud map of the construction tunnel and calculating the probability distribution within each grid; Calculating the likelihood probability based on the distortion-removed laser point cloud data and the probability distribution within each grid, and constructing a point cloud likelihood error; Constructing a ranging error based on the three-dimensional spatial coordinates of each UWB base station and the current ranging value of the vehicle to be located; Performing fusion registration based on the point cloud likelihood error and the ranging error, and solving to obtain the optimal registration result; Taking the IMU integration result as the prediction and the optimal registration result as the observation, and solving according to the standard error state Kalman filter to output the final positioning result.

3. The multi-sensor fusion positioning method under the dynamic degradation environment of a construction tunnel according to claim 2, wherein, Calculating the ranging error based on the following formula: where ψ2 represents the ranging error, and C i represents the three-dimensional spatial coordinates of the i-th mobile UWB base station, and D i represents the current ranging value from the positioning tag on the vehicle to be located to the i-th mobile UWB base station, T(p) represents the current pose of the vehicle to be located, and p represents the pose parameter.

4. The multi-sensor fusion positioning method in the dynamically degraded environment of a construction tunnel according to claim 2, characterized in that, Performing fusion registration of the point cloud likelihood error and the ranging error based on the following formula: J = J1 + μ × J2 H = H1 + μ × H2 Where J and H respectively represent the fused Jacobian matrix and Hessian matrix, J1 and H1 respectively represent the Jacobian matrix and Hessian matrix of the point cloud likelihood error, J2 and H2 respectively represent the Jacobian matrix and Hessian matrix of the ranging error, and μ represents the fusion ratio of the ranging error.

5. The multi-sensor fusion positioning method in the dynamically degraded environment of a construction tunnel according to claim 4, wherein Calculating the Jacobian matrix and Hessian matrix of the ranging error based on the following formula: Among them, C i represents the three-dimensional space coordinates of the i-th mobile UWB base station, D i represents the current ranging value between the positioning tag on the vehicle to be located and the i-th mobile UWB base station, T(p) represents the current pose of the vehicle to be located, and p represents the pose parameter.

6. The multi-sensor fusion positioning method in the dynamically degenerated environment of a construction tunnel according to claim 1, wherein When the tunnel is under construction at a large mileage and the reference tag is behind the bench, or under construction at a small mileage and the reference tag is in front of the bench, the formula for calculating the current mileage of each mobile UWB base station is: When the tunnel is under construction at a small mileage and the reference tag is behind the bench, or when the UWB base station is in front of the bench during large mileage construction, the formula for calculating the current mileage of each mobile UWB base station is: Among them, L i represents the current mileage of the i-th mobile UWB base station, L1 represents the mileage of the reference tag, and w and h respectively represent the lateral deviation and height deviation of the reference tag relative to each mobile UWB base station in the tunnel.

7. The multi-sensor fusion positioning method in the dynamically degraded environment of a construction tunnel according to claim 6, wherein The process of obtaining the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section, and generating a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel includes the following: Obtain the theoretical point cloud data of the secondary lining contour of the tunnel design data and the current mileage of the mobile UWB base station on the secondary lining trolley, and calculate the point cloud of the completed section in the tunnel coordinate system; Obtain the point cloud data of each construction bench in the structured construction section and the current mileage of the mobile UWB base station on each construction bench in the structured construction section, and calculate the point cloud of the structural construction section in the tunnel coordinate system; Obtain the point cloud data of the excavation section and the current mileage of the mobile UWB base station on the construction bench in the excavation section, and calculate the point cloud of the excavation section in the tunnel coordinate system; Stitch the point cloud of the completed section, the point cloud of the structural construction section, and the point cloud of the excavation section to obtain a real-time three-dimensional point cloud map of the construction tunnel.

8. A multi-sensor fusion positioning system under the dynamic degradation environment of a construction tunnel, characterized in that, Including: The UWB ranging data processing module is used to calculate the three-dimensional space coordinates of each UWB base station and the current mileage of each section of the construction tunnel based on the UWB ranging data. Among them, each section of the construction tunnel includes a completed section, a structured construction section, and an excavation section. Fixed UWB base stations and reference tags are installed on the tunnel walls of the completed section, and mobile UWB base stations are installed on the construction benches in the structured construction section and the excavation section; The dynamic point cloud map generation module is used to obtain the tunnel design data, the point cloud data of each construction bench, and the point cloud data of the excavation section, and generate a real-time three-dimensional point cloud map of the construction tunnel in combination with the current mileage of each section of the construction tunnel; The multi-sensor fusion positioning module is used to obtain the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned, use the three-dimensional space coordinates of each UWB base station to provide constraints for the UWB ranging data of the vehicle to be positioned in the degradation direction, and combine the real-time three-dimensional point cloud map of the construction tunnel to perform fusion positioning on the laser point cloud data, UWB ranging data, and IMU data of the vehicle to be positioned in a dynamic degradation environment to obtain the positioning result of the vehicle to be positioned.

9. An electronic device, characterized in that, Including a processor and a memory, a computer program is stored in the memory, and the processor is used to execute the steps of the method according to any one of claims 1 to 7 by calling the computer program stored in the memory.

10. A computer-readable storage medium for storing a computer program for multi-sensor fusion positioning in a dynamically deteriorating environment of a construction tunnel, characterized in that, When the computer program runs on a computer, it executes the steps of the method according to any one of claims 1 to 7.

Images