Road three-dimensional reconstruction method based on main-auxiliary laser radar and integrated navigation

Abstract

The invention relates to a road three-dimensional reconstruction method based on a main-auxiliary laser radar and integrated navigation, and the method comprises the following steps: S1, obtaining filtered point cloud data, namely point cloud in an ECEF coordinate system; s2, performing advancing direction registration on the mobile frame of the filtered point cloud data to obtain the point cloud data after the advancing direction registration; s3, extracting a curb point cloud and a road surface point cloud from the point cloud data after registration of the advancing direction; s4, carrying out refined pavement registration on the pavement point cloud to obtain a pavement point cloud after pavement registration; and S5, carrying out accurate curb registration on the curb point cloud to obtain the curb point cloud after curb registration, and combining the road surface point cloud after road surface registration to obtain reconstructed point cloud data. Compared with the prior art, the method has the advantages of improving the accuracy and technical robustness of road three-dimensional reconstruction and the like.

Description

Technical Field

[0001] The present invention relates to the field of laser radar technology, and in particular to a road three-dimensional reconstruction method based on primary and secondary laser radars and combined navigation. Background Art

[0002] LiDAR (Light Detection And Ranging) is an advanced 3D real-scene replication technology. The 3D point cloud it generates has richer spatial geometric information than image videos when representing different complex objects. Mobile Laser Scanning (MLS) system is a device that fixes LiDAR and other sensors on a mobile carrier (usually a vehicle). The 3D point cloud data collected by it has been widely used in civil and traffic engineering fields in recent years, such as 3D reconstruction, autonomous driving and safety systems. Road 3D reconstruction aims to restore the real road scene to the greatest extent, and provide high-quality 3D point cloud data for applications such as urban 3D modeling, road deformation disease inspection, road linear geometry parameter calculation, road asset inventory and transportation infrastructure digitization.

[0003] The MLS system is usually composed of LiDAR and Integrated Navigation System (INS). However, most of the existing commercial MLS systems are produced and developed abroad, and are very expensive (usually more than 250,000 US dollars), bulky and sometimes difficult to operate, which limits the number of users worldwide and hinders the rapid popularization and application of the MLS system. The manufacture of the MLS system is a trade secret, and none of the manufacturers have disclosed the technical details. In view of this, the present invention aims to independently develop a low-cost and lightweight MLS system device (less than 5,000 US dollars) based on primary and auxiliary LiDAR and INS, and propose an accurate and efficient road three-dimensional reconstruction method based on the collected MLS point cloud data. The present invention provides a low-cost solution for small and medium-sized enterprises, research institutions, etc. to replace the expensive commercial MLS system.

[0004] In most commercial MLS systems, only one mechanical rotating laser radar is usually used for data collection, but this type of laser radar is usually expensive, and because the mechanical rotating laser radar rotates 360 degrees, only a small part of the collected point cloud data is the road, and the rest are buildings, vehicles and pedestrians, etc., which is not conducive to the task of road three-dimensional reconstruction. In this regard, the present invention innovatively proposes a data collection mode of collaboration between the main and auxiliary laser radars, and adopts a more cost-effective semi-solid laser radar (which uses non-repetitive scanning technology. As the integration time increases, the point cloud field coverage rate will continue to increase, and more details in the field of view will be detected). The main laser radar focuses on the collection of road surface point cloud data, while the auxiliary laser radar focuses on capturing the point cloud data of the curb and part of the roadside. This mode of collaborative data collection between the main and auxiliary laser radars can not only increase the road point cloud data, but also ensure the coverage of the road curb and part of the roadside point cloud data, thereby providing richer and more effective detail information for road three-dimensional reconstruction, especially near the edge of the road, which is often a key area in traffic safety control.

[0005] In the MLS system, LiDAR devices are usually installed on mobile vehicles to continuously collect 3D point cloud data. However, vehicle movement will cause the position and posture of LiDAR and INS to change, and there will be position and angle deviations between adjacent point cloud frames. Therefore, point cloud registration technology is needed to correct these deviations to ensure the spatial continuity and accuracy of point cloud data. Point cloud registration is the most important step in the 3D road reconstruction process. Its main purpose is to fuse point cloud data collected from multiple different perspectives or different timestamps into a unified coordinate system. The registration result will directly determine the accuracy and reliability of road 3D reconstruction. Therefore, effective point cloud registration technology is the key to achieving high-quality road 3D reconstruction. However, current point cloud reconstruction methods often fail to provide high accuracy and high robustness when dealing with complex or dynamically changing road environments, especially in scenes with a large number of similar point clouds. They are easily disturbed by environmental interference such as vibration, require good initial postures and multiple iterative operations, and have a long calculation time, making it difficult to meet the needs of real-time or near-real-time road information extraction. Summary of the invention

[0006] The purpose of the present invention is to improve the accuracy and technical robustness of road three-dimensional reconstruction, while reducing the errors caused by different point cloud densities and environmental interference, and to provide a road three-dimensional reconstruction method based on primary and auxiliary laser radars and combined navigation.

[0007] The purpose of the present invention can be achieved by the following technical solutions:

[0008] A method for three-dimensional reconstruction of a road based on primary and secondary laser radars and integrated navigation, the method comprising the following steps:

[0009] Step S1: Based on the main and auxiliary laser radar LiDAR and the integrated navigation INS, the initial point cloud data acquired by the vehicle is fused to obtain the point cloud in the ECEF coordinate system, and then filtered to obtain filtered point cloud data, wherein the initial point cloud data includes point cloud data of the road surface and both sides of the road;

[0010] Step S2: performing travel direction registration on the moving frames of the filtered point cloud data to obtain the point cloud data after the travel direction registration;

[0011] Step S3: extracting the curb point cloud and the road surface point cloud from the point cloud data after the travel direction is aligned;

[0012] Step S4: performing refinement and road registration on the road surface point cloud to obtain a road surface point cloud after road surface registration;

[0013] Step S5: Perform accurate curb registration on the curb point cloud to obtain the curb point cloud after curb registration, and combine it with the road surface point cloud after road surface registration to obtain reconstructed point cloud data.

[0014] Furthermore, step S1 is specifically as follows:

[0015] Step S1.1: Convert the initial point cloud data in the main and auxiliary LiDAR coordinate systems to the inertial measurement unit IMU coordinate system through rigid body transformation;

[0016] Step S1.2: transform the point cloud in the IMU coordinate system to the ENU coordinate system;

[0017] Step S1.3: transform the point cloud in the Northeastern ENU coordinate system into the Earth-centered Earth-fixed rectangular ECEF coordinate system to obtain the point cloud in the ECEF coordinate system;

[0018] Step S1.4: Select the position and attitude data output by the integrated navigation INS at time t1 and t2, calculate the origin of the main and auxiliary LiDAR and the origin of the inertial measurement unit IMU at time t1 and t2 respectively, and use the coordinates of the three origins in the ECEF coordinate system as reference points r1, r2, and r3 respectively to define the segmentation plane κ i , where i = 1, 2, 3, 4, the point cloud in the ECEF coordinate system is compared with the segmentation plane, and the point cloud that intersects with the segmentation plane or is inside the segmentation plane is retained as the filtered point cloud data.

[0019] Furthermore, each point of the point cloud in the ECEF coordinate system is:

[0020] p ECEF =M EE M EI (M IL p LiDAR +p IL)+p EE

[0021] Among them, p ECEF is a point in the ECEF coordinate system, M EE is the rotation matrix from the ENU coordinate system to the ECEF coordinate system, p EE is the first translation vector, M EI is the rotation matrix from the IMU coordinate system to the ENU coordinate system, M IL is the rotation matrix from the LiDAR coordinate system to the IMU coordinate system, p IL is the second translation vector.

[0022] Furthermore, step S2 is specifically as follows:

[0023] Step S2.1: extract the tree trunk point cloud from the filtered point cloud data and project it onto a plane parallel to the road surface, then cluster the projected points using the DBSACN algorithm to obtain multiple projected point clusters in the fixed frame and the moving frame in the point cloud;

[0024] Step S2.2: Let V = {v1,v2,…,v n} is a fixed point, U={u1,u2,…,u n} is a moving point, the fixed point forms a fixed frame, and the moving point forms a moving frame. Let t * For the optimal translation vector from U to V, define the error function e(t) in the least squares sense and derive the optimal translation vector t from the centroid of the moving point to the centroid of the fixed point * , the optimal translation vector t * Specifically:

[0025]

[0026] Step S2.3: Set is the centroid of the projected point cluster of the fixed frame, is the centroid of the projected point cluster of the moving frame, and P is found in the projected point cluster. c and Q c The matching projection point cluster pairs between ;

[0027] Step S2.4: The moving frame of the point cloud is registered according to the optimal translation vector and the number of matching projection point cluster pairs to obtain the registered moving frame of the point cloud and the point cloud data after the registered moving direction. The registration process is as follows:

[0028]

[0029] Among them, (v r ,v t ) and (u h ,uk ) is a set of matching point pairs, s opt represents the optimal translation vector, κ represents the number of matching cluster pairs, 2κ represents the number of matching point pairs, and p moving Represents an arbitrary point in the moving frame.

[0030] Furthermore, the matching projection point cluster pairs satisfy:

[0031]

[0032] Where I is the index of the cluster matched by any moving point u, ρ is the Euclidean distance threshold for a given execution, and d ij is the point p in the fixed frame j and point q in the moving frame i The Euclidean distance between j For d ij is the distance vector set of the elements.

[0033] Furthermore, step S3 is specifically as follows:

[0034] Step S3.1: According to the initial plane w, a dual-plane window w1-w2 is obtained, and the dual-plane window w1-w2 is slid to both sides at a given step length on the road point cloud of the point cloud data after the travel direction is aligned;

[0035] Step S3.2: During the window sliding process, intercept the point cloud located in the current two-plane windows w1 and w2, denoted as A;

[0036] Step S3.3: performing plane fitting on the intercepted point cloud, if the angle between the fitting plane and the initial plane w is greater than a given threshold, then determining the point in A that meets the conditions as the curb point cloud;

[0037] Step S3.4: After all curb point clouds are found, the point cloud between the curb point clouds on both sides is the road surface point cloud.

[0038] Further, step S4 is specifically as follows:

[0039] Step S4.1: Let u F and u M are the fitting planes of the road point cloud of the fixed frame and the moving frame, respectively, denoted by R V is the Rodriguez rotation matrix, p0 represents a point on the rotation axis, and the rotation matrix R is calculated. p ;

[0040] Step S4.2: Let p1 be plane u M A point on M to u F The first homogeneous translation transformation matrix makes the two fitting planes u M to uF overlap with each other, the first homogeneous translation transformation matrix is:

[0041]

[0042] Step S4.3: Based on the KNN algorithm, the overlapping point clouds between the moving frame and the fixed frame are extracted, and then the ICP algorithm is used to refine the overlapping point clouds for road registration to obtain the point cloud after road registration [p moving :]←M ICP T1R p [p moving :], where p moving : represents any point in the road point cloud in the moving frame. [u:] represents the homogeneous coordinate form of u.

[0043] Furthermore, the rotation matrix R p for:

[0044]

[0045] Among them, R V is the Rodriguez rotation matrix, and p0 represents a point on the rotation axis.

[0046] Furthermore, step S5 is specifically as follows:

[0047] Step S5.1: firstly, plane fitting is performed on the curb point cloud to determine the geometric position of the curb, then the curb point cloud is slid along its normal direction, the road surface point cloud adjacent to the curb is intercepted, the adjacent road surface point cloud is plane fitted again, and the intersection line of the curb and the road surface is determined based on the intersection line of the fitted plane;

[0048] Step S5.2: Assume that the direction vectors of the intersection line between the curb and the road surface in the fixed frame and the moving frame are l F and l M , p F and p M l F and l M For any point on M To F The rotation angle β, the rotation axis vector k and a point p0 on the rotation axis are substituted into the rotation matrix R. p The formula is used to obtain the homogeneous rotation matrix R from the moving frame to the fixed frame p1 ;

[0049] Step S5.3: Solve so that the two fitting lines l F and l M The second homogeneous translation matrix T of the overlapped roadside is used to obtain the roadside point cloud after roadside registration [p moving :]←M ICPTR p1 [p moving :], where p moving : represents any point in the curb point cloud of the moving frame.

[0050] Furthermore, the M To F The rotation angle β, the rotation axis vector k and a point p0 on the rotation axis are:

[0051]

[0052] Among them, p1 represents the average value of the curb point cloud;

[0053] The second homogeneous translation matrix T is:

[0054]

[0055] Among them, p0 represents a point on the rotation axis, that is, point p1 to l M Projection.

[0056] Compared with the prior art, the present invention has the following beneficial effects:

[0057] (1) The present invention adopts a new key point matching method based on projected point clusters, which can effectively handle complex road scenes with multiple similar shapes or points, and significantly reduce displacement deviations and mismatches along the driving direction (Y axis). In addition, combined with a dual-plane sliding window algorithm, the present invention not only accurately extracts point clouds of the road surface and curb, but also optimizes the data processing process under variable road conditions, greatly improving the precision and overall quality of three-dimensional reconstruction. The present invention introduces a plane-to-plane registration method and an overlapping point cloud extraction technology based on the KNN algorithm, realizing the calibration of the road surface point cloud from coarse to fine, effectively correcting the errors in the vertical direction (Z axis) of the road surface point cloud and the X axis direction of the curb point cloud, and improving the accuracy and reliability of the registration results; at the same time, a registration method based on intersection lines is proposed for accurate calibration of the curb point cloud, effectively correcting the errors in the horizontal direction (X axis), and meeting the needs of real-time or near-real-time road information extraction. By emphasizing precise control along the three directions of X, Y and Z, the errors caused by different point cloud densities and environmental interference are effectively reduced, and the entire road three-dimensional reconstruction process is optimized.

[0058] (2) The present invention proposes a collaborative acquisition method of the main laser radar and the auxiliary laser radar, and fuses the laser radar (LiDAR) and the integrated navigation (INS) data. The fused point cloud data completely restores the three-dimensional road scene. Through this innovative main and auxiliary radar data acquisition and fusion method, the data coverage and quality are significantly enhanced, and the details and accuracy of the three-dimensional reconstruction are improved. Especially in complex road environments or variable road edge conditions, the system can effectively capture complete road information, enhance the applicability and reliability of the system, and realize accurate three-dimensional reconstruction of road pavement and curbs. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 The conversion principle of the point cloud data from LiDAR to ECEF coordinate system of the present invention;

[0060] Figure 2 It is a structural schematic diagram of a laser radar and a combined navigation data acquisition device of the present invention;

[0061] Figure 3 The principle of the dual-plane sliding window algorithm for extracting curbstone and road surface point clouds of the present invention;

[0062] Figure 4 Schematic diagram of the process of the three-dimensional road reconstruction method of the present invention DETAILED DESCRIPTION

[0063] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0064] The main purpose of the present invention is to provide a multi-directional registration method for three-dimensional road reconstruction using primary and secondary laser radar (LiDAR) and integrated navigation system (INS) data. First, the displacement deviation along the travel direction (Y-axis) is corrected by calculating the optimal translation direction; then, a dual-plane sliding window algorithm is proposed for the most critical road surface and curb information in road characterization, and the road surface and curb point clouds are extracted respectively; finally, the Z-axis direction of the road surface point cloud and the X-axis direction of the curb point cloud are respectively registered to further reduce the error of three-dimensional road reconstruction, which has the characteristics of high reconstruction accuracy and good technical robustness.

[0065] The present invention comprises the following steps:

[0066] Step S1: Based on the main and auxiliary laser radar LiDAR and the integrated navigation INS, the initial point cloud data acquired by the vehicle is fused to obtain the point cloud in the ECEF coordinate system, and then filtered to obtain filtered point cloud data, wherein the initial point cloud data includes point cloud data of the road surface and both sides of the road;

[0067] Step S2: performing travel direction registration on the moving frames of the filtered point cloud data to obtain the point cloud data after the travel direction registration;

[0068] Step S3: extracting the curb point cloud and the road surface point cloud from the point cloud data after the travel direction is aligned;

[0069] Step S4: performing refinement and road registration on the road surface point cloud to obtain a road surface point cloud after road surface registration;

[0070] Step S5: Perform accurate curb registration on the curb point cloud to obtain the curb point cloud after curb registration, and combine it with the road surface point cloud after road surface registration to obtain reconstructed point cloud data.

[0071] The details of the present invention are shown in the flowchart Figure 4 The various steps of the present invention are specifically as follows:

[0072] Step S1 is specifically implemented as follows:

[0073] Step S1.1: Figure 1 The figure shows the schematic diagram of the relevant coordinate system principle for LiDAR to ECEF conversion. The integrated navigation system (INS) includes IMU and GNSS, which are used to obtain the precise position and attitude data of the vehicle. Since the geodetic position of the origin of the LiDAR coordinate system changes as the vehicle moves, the point clouds of all frames are eventually stacked in the same area. To solve this problem, the inertial navigation system (IMU) data in the integrated navigation (INS) is used to unify the coordinate system of the LiDAR point cloud data to restore the complete shape and outline of the scanned object. First, the conversion from the LiDAR coordinate system to the inertial measurement unit (IMU) coordinate system is realized through rigid body transformation, which specifically includes rotation and translation operations, namely:

[0074] p IMU =M IL ·p LiDAR +p IL

[0075] Among them, p LiDAR is a point in the LiDAR coordinate system, p IMU is a point in the IMU coordinate system, M IL is the rotation matrix from the LiDAR coordinate system to the IMU coordinate system, p IL is the translation vector.

[0076] Step S1.2: Using the output of the IMU, the point cloud data is further converted from the IMU coordinate system to the Northeastern Universe (ENU) coordinate system, specifically by performing a rotation operation using the attitude angle measured by the IMU, namely:

[0077] p ENU =MEI ·p IMU

[0078] Among them, p ENU is a point in the ENU coordinate system, M EI is the rotation matrix from the IMU coordinate system to the ENU coordinate system.

[0079] Step S1.3: First, the ENU coordinate system is converted to the longitude, latitude and altitude (LLA) coordinate system, because the ENU coordinate system is a local coordinate system relative to a point on the surface, while the ECEF is a global coordinate system; then, based on the longitude, latitude and altitude of the observation location, the ENU coordinate system is converted to the Earth-centered Earth-fixed Cartesian (ECEF) coordinate system, which specifically includes rotation and translation operations, namely:

[0080] p ECEF =M EE ·p ENU +p EE

[0081] Among them, p ECEF is a point in the ECEF coordinate system, M EE is the rotation matrix from the ENU coordinate system to the ECEF coordinate system, p EE is the translation vector.

[0082] Finally, the formula for converting point cloud data from the initial LiDAR coordinate system to the ECEF coordinate system can be summarized as:

[0083] p ECEF =M EE M EI (M IL p LiDAR +p IL )+p EE

[0084] Step S1.4: Figure 2 As shown in the figure, since the main radar and the auxiliary radar are used to collect point cloud data on the road surface and both sides of the road respectively, there are a large number of irrelevant point clouds. Therefore, the position and attitude data output by the integrated navigation (INS) at specific times t1 and t2 (t2>t1) are selected, and the origin of the auxiliary LiDAR and the origin of the inertial navigation system (IMU) at times t1 and t2 are calculated respectively, and the coordinates of the three origins in the ECEF coordinate system are used as reference points r1, r2, and r3, respectively, to define the segmentation plane κ i (i=1, 2, 3, 4); then all point clouds in the ECEF coordinate system are compared with the segmentation planes, and the point clouds that intersect with or are inside these planes are retained.

[0085] Step S2 is specifically implemented as the following steps:

[0086] Step S2.1: In order to reduce the possible slight displacement deviation between two adjacent frames along the travel direction, the travel displacement direction is calibrated by combining the tree point clouds on both sides of the road. The tree trunk point cloud is extracted and projected onto a plane parallel to the road surface, and then the DBSACN (Density-Based Spatial Clustering of Applications with Noise) algorithm is used to cluster the projected points, and further search for matching point pairs based on the obtained projected point clusters.

[0087] Step S2.2: Let V = {v1, v2, …, v n} is a fixed point, U={u1,u2,…,u n} is the moving point, t * To find the optimal translation vector from U to V, define the error function e(t) in the sense of least squares and derive the optimal translation vector t from the centroid of the moving point to the centroid of the fixed point * , the specific formula is as follows:

[0088]

[0089] Step S2.3: Set is the centroid of the projected point cluster of the fixed frame, is the centroid of the projected point cluster of the moving frame, where and Corresponding to and P can be found according to the following formula c and Q c The matching cluster pairs between:

[0090]

[0091] Where I is the index of the cluster that u matches, ρ is the Euclidean distance threshold for a given execution, and d ij is the point p in the fixed frame j and point q in the moving frame i The Euclidean distance between j For d ij is the distance vector set of the elements.

[0092] Step S2.4: Finally, the moving frame can be further registered based on the derived optimal translation vector and the extracted matching projection point pair, that is:

[0093]

[0094] Among them, (v r ,v t ) and (uh ,u k ) is a set of matching point pairs, s opt represents the optimal translation vector, κ represents the number of matching cluster pairs, 2κ represents the number of matching point pairs, and p moving Represents an arbitrary point in the moving frame.

[0095] Step S3 is specifically implemented as the following steps:

[0096] Step S3.1: Figure 3 The figure shows the principle diagram of the dual-plane sliding window algorithm for curbstone and road surface point cloud extraction. First, based on the initial plane w, the dual-plane window w1-w2 can be obtained, and then the window is slid to both sides on the road point cloud with a given step length. Among them, l represents the window length and s represents the window sliding step length.

[0097] Step S3.2: During the window sliding process, intercept the point cloud located in the current dual-plane windows w1 and w2, denoted as A.

[0098] Step S3.3: Perform plane fitting on the intercepted point cloud. If the angle between the fitting plane and the initial plane w is greater than a given threshold, the point in A that meets the conditions is determined to be a curb point cloud.

[0099] Step S3.4: Finally, the point cloud between the left and right curbs is the road surface point cloud.

[0100] Step S4 is specifically implemented as follows:

[0101] Step S4.1: Let u F and u M are the fitting planes of the road point cloud of the fixed frame and the moving frame, respectively, denoted by R V is the Rodriguez rotation matrix, p0 represents a point on the rotation axis, and the rotation matrix R can be obtained according to the following formula p :

[0102]

[0103] Step S4.2: To reduce the number of fitted planes u that are parallel to each other after rotation F and u M The distance between them, let p1 be the plane u M A point on the M to u F The homogeneous translation transformation matrix of is used to make the two fitting planes coincide with each other:

[0104]

[0105] Step S4.3: Based on the KNN algorithm, the overlapping point clouds between the moving frame and the fixed frame are extracted, and then the ICP algorithm is used to further refine the road surface registration. The final road surface point cloud registration formula of the moving frame can be summarized as: [p moving :]←M ICP TR p [p moving :], where M ICP Represents the transformation matrix obtained by applying the ICP algorithm.

[0106] Step S5 is specifically implemented as the following steps:

[0107] Step S5.1: First, plane fitting is performed on the curb point cloud to determine the geometric position of the curb. Then, the fitting plane is slid along its normal direction to intercept the road surface point cloud adjacent to the curb. The road surface point cloud intercepted from the curb is re-plane fitted to improve the accuracy of intersection line extraction.

[0108] Step S5.2: Assume that the direction vectors of the intersection line between the curb and the road surface in the fixed frame and the moving frame are l F and l M , p F and p M l F and l M For any point on the M To F The rotation angle β, the rotation axis vector k and a point p0 on the rotation axis (i.e., point p1 to l M Projection):

[0109]

[0110] Where p1 represents the average value of the curb point cloud. Substituting the relevant rotation parameters into the formula of step S4.1, the homogeneous rotation matrix R from the moving frame to the fixed frame can be obtained. p The specific substitution is: calculate the Rodriguez rotation matrix Rv according to the rotation angle β and the rotation axis vector k, and then bring Rv and a point p0 on the rotation axis into the formula of the rotation matrix Rp in the 8th point.

[0111] Step S5.3: After the rotation transformation, the moving frame needs to be translated so that the two fitting lines coincide. The homogeneous translation matrix can be solved by the following formula:

[0112]

[0113] Finally, the roadside point cloud registration formula of the moving frame can be summarized as: moving :]←M ICP TR p [p moving :].

[0114] Existing 3D road reconstruction technologies mainly use classic point cloud registration methods, such as ICP and NDT. Although these methods often fail to provide high accuracy and robustness when dealing with complex or dynamically changing road environments, especially in scenes with a large number of similar point clouds. These traditional methods are susceptible to environmental interference such as vibration, require a good initial posture and multiple iterative operations, and have a long calculation time, making it difficult to meet the needs of real-time or near-real-time road information extraction. In response to these problems, the present invention proposes a multi-directional registration method, which combines primary and auxiliary LiDAR and INS data, and significantly improves the accuracy and technical robustness of road 3D reconstruction through innovative key point matching algorithms and dual-plane sliding window algorithms. In addition, the present invention places special emphasis on precise control along the three directions of X, Y and Z during the registration process, effectively reducing the errors caused by different point cloud densities and environmental interference, and optimizing the entire road 3D reconstruction process.

[0115] The present invention proposes a collaborative acquisition method of the main laser radar and the auxiliary laser radar, and fuses the laser radar (LiDAR) and the integrated navigation (INS) data. The fused point cloud data completely restores the three-dimensional road scene. Through this innovative main and auxiliary radar data acquisition and fusion method, the data coverage and quality are significantly enhanced, and the details and accuracy of the three-dimensional reconstruction are improved. Especially in complex road environments or variable road edge conditions, the system can effectively capture complete road information, enhance the applicability and reliability of the system, and realize accurate three-dimensional reconstruction of road pavement and curbs.

[0116] The present invention adopts a new key point matching method based on projected point clusters, which can effectively handle complex road scenes with multiple similar shapes or points, significantly reducing displacement deviations and mismatches along the driving direction (Y axis). In addition, combined with a dual-plane sliding window algorithm, it not only accurately extracts point clouds of the road surface and curb, but also optimizes the data processing process under variable road conditions, greatly improving the precision and overall quality of 3D reconstruction.

[0117] In order to improve the registration accuracy of road surface and curb point clouds, the present invention introduces a plane-to-plane registration method and an overlapping point cloud extraction technology based on the KNN algorithm, realizes the coarse-to-fine calibration of road surface point clouds, effectively corrects the errors in the vertical direction (Z axis), and improves the accuracy and reliability of the registration results; at the same time, a registration method based on intersection lines is proposed for accurate calibration of curb point clouds, effectively corrects the errors in the horizontal direction (X axis), optimizes the data processing and application of the curb area, and enhances the practicality and adaptability of the technology.

[0118] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A method for three-dimensional road reconstruction based on primary and secondary laser radars and combined navigation, characterized in that: The method comprises the following steps: Step S1: Based on the main and auxiliary laser radar LiDAR and the integrated navigation INS, the initial point cloud data acquired by the vehicle is fused to obtain the point cloud in the ECEF coordinate system, and then filtered to obtain filtered point cloud data, wherein the initial point cloud data includes point cloud data of the road surface and both sides of the road; Step S2: performing travel direction registration on the moving frames of the filtered point cloud data to obtain the point cloud data after the travel direction registration; Step S3: extracting the curb point cloud and the road surface point cloud from the point cloud data after the travel direction is aligned; Step S4: performing refinement and road registration on the road surface point cloud to obtain a road surface point cloud after road surface registration; Step S5: Perform accurate curb registration on the curb point cloud to obtain the curb point cloud after curb registration, and combine it with the road surface point cloud after road surface registration to obtain reconstructed point cloud data.

2. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 1, characterized in that: Step S1 is specifically as follows: Step S1.1: Convert the initial point cloud data in the main and auxiliary LiDAR coordinate systems to the inertial measurement unit IMU coordinate system through rigid body transformation; Step S1.2: transform the point cloud in the IMU coordinate system to the ENU coordinate system; Step S1.3: transform the point cloud in the Northeastern ENU coordinate system into the Earth-centered Earth-fixed rectangular ECEF coordinate system to obtain the point cloud in the ECEF coordinate system; Step S1.4: Select the position and attitude data output by the integrated navigation INS at time t1 and t2, calculate the origin of the main and auxiliary LiDAR and the origin of the inertial measurement unit IMU at time t1 and t2 respectively, and use the coordinates of the three origins in the ECEF coordinate system as reference points r1, r2, and r3 respectively to define the segmentation plane κ i , where i = 1, 2, 3, 4, the point cloud in the ECEF coordinate system is compared with the segmentation plane, and the point cloud that intersects with the segmentation plane or is inside the segmentation plane is retained as the filtered point cloud data.

3. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 2, characterized in that: Each point of the point cloud in the ECEF coordinate system is: p ECEF =M EE M EI (M IL p LiDAR +p IL )+p EE Among them, p ECEF is a point in the ECEF coordinate system, M EE is the rotation matrix from the ENU coordinate system to the ECEF coordinate system, p EE is the first translation vector, M EI is the rotation matrix from the IMU coordinate system to the ENU coordinate system, M IL is the rotation matrix from the LiDAR coordinate system to the IMU coordinate system, p IL is the second translation vector.

4. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 1, characterized in that: Step S2 is specifically as follows: Step S2.1: extract the tree trunk point cloud from the filtered point cloud data and project it onto a plane parallel to the road surface, then cluster the projected points using the DBSACN algorithm to obtain multiple projected point clusters in the fixed frame and the moving frame in the point cloud; Step S2.2: Let V = {v1, v2, ..., v n } is a fixed point, U={u1,u2,...,u n } is a moving point, the fixed point forms a fixed frame, and the moving point forms a moving frame. Let t * For the optimal translation vector from U to V, define the error function e(t) in the least squares sense and derive the optimal translation vector t from the centroid of the moving point to the centroid of the fixed point * , the optimal translation vector t * Specifically: Step S2.3: Set is the centroid of the projected point cluster of the fixed frame, is the centroid of the projected point cluster of the moving frame, and P is found in the projected point cluster. c and Q c The matching projection point cluster pairs between ; Step S2.4: The moving frame of the point cloud is registered according to the optimal translation vector and the number of matching projection point cluster pairs to obtain the registered moving frame of the point cloud and the point cloud data after the registered moving direction. The registration process is as follows: Among them, (v r , v t ) and (u h ,u k ) is a set of matching point pairs, s opt represents the optimal translation vector, κ represents the number of matching cluster pairs, 2κ represents the number of matching point pairs, and p moving Represents an arbitrary point in the moving frame.

5. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 4, characterized in that: The matching projection point cluster pairs satisfy: Where I is the index of the cluster matched by any moving point u, ρ is the Euclidean distance threshold for a given execution, and d ij is the point p in the fixed frame j and point q in the moving frame i The Euclidean distance between j For d ij is the distance vector set of the elements.

6. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 1, characterized in that: Step S3 is specifically as follows: Step S3.1: According to the initial plane w, a dual-plane window w1-w2 is obtained, and the dual-plane window w1-w2 is slid to both sides at a given step length on the road point cloud of the point cloud data after the travel direction is aligned; Step S3.2: During the window sliding process, intercept the point cloud located in the current two-plane windows w1 and w2, denoted as A; Step S3.3: performing plane fitting on the intercepted point cloud, if the angle between the fitting plane and the initial plane w is greater than a given threshold, then determining the point in A that meets the conditions as the curb point cloud; Step S3.4: After all curb point clouds are found, the point cloud between the curb point clouds on both sides is the road surface point cloud.

7. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 1, characterized in that: Step S4 is specifically as follows: Step S4.1: Let u F and u M are the fitting planes of the road point cloud of the fixed frame and the moving frame, respectively, denoted by R V is the Rodriguez rotation matrix, p0 represents a point on the rotation axis, and the rotation matrix R is calculated. p ; Step S4.2: Let p1 be plane u M A point on M to u F The first homogeneous translation transformation matrix makes the two fitting planes u M to u F overlap with each other, the first homogeneous translation transformation matrix is: Step S4.3: Based on the KNN algorithm, the overlapping point clouds between the moving frame and the fixed frame are extracted, and then the ICP algorithm is used to refine the overlapping point clouds for road registration to obtain the point cloud after road registration [p moving :]←M ICP T1R p [p moving :], where p moving : represents any point in the road point cloud in the moving frame. [u:] represents the homogeneous coordinate form of u.

8. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 7, characterized in that: The rotation matrix R p for: Among them, R V is the Rodriguez rotation matrix, and p0 represents a point on the rotation axis.

9. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 1, characterized in that: Step S5 is specifically as follows: Step S5.1: firstly, plane fitting is performed on the curb point cloud to determine the geometric position of the curb, then the curb point cloud is slid along its normal direction, the road surface point cloud adjacent to the curb is intercepted, the adjacent road surface point cloud is plane fitted again, and the intersection line of the curb and the road surface is determined based on the intersection line of the fitted plane; Step S5.2: Assume that the direction vectors of the intersection line between the curb and the road surface in the fixed frame and the moving frame are l F and l M , p F and p M l F and l M For any point on M To F The rotation angle β, the rotation axis vector k and a point p0 on the rotation axis are substituted into the rotation matrix R. p The formula is used to obtain the homogeneous rotation matrix R from the moving frame to the fixed frame p1 ; Step S5.3: Solve so that the two fitting lines l F and l M The second homogeneous translation matrix T of the overlapped roadside is used to obtain the roadside point cloud after roadside registration [p moving :]←M ICP TR p1 [p moving :], where p moving : represents any point in the curb point cloud of the moving frame.

10. The method for three-dimensional road reconstruction based on primary and secondary laser radar and integrated navigation according to claim 9, characterized in that: The l M To F The rotation angle β, the rotation axis vector k and a point p0 on the rotation axis are: Among them, p1 represents the average value of the curb point cloud; The second homogeneous translation matrix T is: Among them, p0 represents a point on the rotation axis, that is, point p1 to l M Projection.

Images