Distributed detection device and 4D millimeter wave point cloud registration method based on three radars

Abstract

The invention discloses a distributed detection device and a 4D millimeter wave point cloud registration method based on three radars. According to the device, a distributed detection module comprises left, middle and right 4D millimeter wave radars; the processor comprises an offline calibration and data synchronization module, an original point cloud preprocessing module, a coordinate system unification module, a point cloud coarse registration module and a point cloud precise registration and fusion output module; the distributed detection module, the processor and the communication module are connected in sequence; the method comprises the steps that off-line external parameter calibration is carried out in a combined mode, and accurate pose external parameters of the left radar and the right radar are obtained based on multi-pose point cloud data; executing statistical filtering processing; uniformly mapping the de-noised point clouds of the left radar and the right radar to a coordinate system of the middle reference radar to form a point cloud set of a three-path common reference system; determining an initial pose matrix through coarse registration; and completing accurate alignment and fusion based on the high-precision pose matrix, and outputting dense 4D millimeter wave radar point clouds. According to the invention, dense and high-quality 4D millimeter wave point cloud data can be obtained.

Description

Technical Field

[0001] This invention belongs to the field of millimeter-wave radar sensing and point cloud processing technology, specifically relating to a distributed detection device and a 4D millimeter-wave point cloud registration method based on three radars. Background Technology

[0002] Existing single 4D millimeter-wave radars are limited by antenna aperture, number of transmission channels, and ranging angular resolution, resulting in sparse output point clouds that are susceptible to environmental factors such as metal reflections, making it difficult to meet the requirements for robust target detection and identification. Multi-radar cooperative technology can significantly improve spatial sampling density and spectral coverage, but it suffers from technical problems such as synchronization difficulties, severe extrinsic parameter drift, mismatch caused by dynamic targets, and multi-source noise aliasing, leading to unstable fusion results in practical applications and failing to meet high-precision detection requirements. Therefore, there is an urgent need to provide a distributed detection device and a 4D millimeter-wave point cloud registration method based on three radars through collaborative design of structure and algorithms, in order to reliably obtain dense and high-quality 4D millimeter-wave point clouds under engineering conditions. Summary of the Invention

[0003] To address the problems existing in the prior art, this invention provides a 4D millimeter-wave point cloud registration method based on three radars. This device has a simple structure, low manufacturing cost, reliable performance, and good practicality. It can obtain dense and high-quality 4D millimeter-wave point cloud data, which helps to significantly improve the accuracy of target contour, distance, velocity, and angle information. The method is simple to implement and has low implementation cost, which can significantly improve point cloud coverage and effective echo ratio, and improve the accuracy and stability of target detection, segmentation, and tracking. It can solve the problems of sparse point clouds and insufficient coverage of single radars in the prior art, as well as the low reliability of detection under complex conditions such as dust, water mist, and strong reflection.

[0004] To achieve the above objectives, the present invention provides a distributed detection device, comprising a housing, a distributed detection module, a processor, and a communication module; The distributed detection module includes a left-side 4D millimeter-wave radar, a middle-side 4D millimeter-wave radar, and a right-side 4D millimeter-wave radar, which are installed side by side in the housing at equal intervals along the horizontal direction. The processor includes an offline calibration and data synchronization module, a raw point cloud preprocessing module, a coordinate system module, a point cloud coarse registration module, and a point cloud precise registration and fusion output module. The offline calibration and data synchronization module is connected to the distributed detection module, the checkerboard target, and the laser tracker, respectively, and is used to complete the orientation extrinsic parameter calibration and data synchronization of the three radars. The original point cloud preprocessing module is connected to the offline calibration and data synchronization module, and is used to filter out environmental random noise in the original point cloud and output a denoised clean point cloud. The coordinate system module is connected to the original point cloud preprocessing module and is used to map the point clouds of the left and right radars to the coordinate system of the middle base station radar, so as to construct a common reference system point cloud set for the three radars. The point cloud coarse registration module is connected to the coordinate system module and is used to quickly align the positions of the left and right radar point clouds with the reference radar point cloud based on the radar assembly geometry and external parameter information, and output the initial pose matrix. The point cloud precise registration and fusion output module is connected to the point cloud coarse registration module. It is used to achieve high-precision pose optimization based on the initial pose obtained by coarse registration, and complete the precise alignment and fusion of the three point clouds, and finally output a dense 4D millimeter-wave radar point cloud. The communication module is connected to the processor and is used to establish a communication connection between the processor and an external terminal.

[0005] In this invention, three 4D millimeter-wave radars (left, center, and right) are horizontally and equally spaced side-by-side within a housing. This effectively protects the 4D millimeter-wave radars from damage and overcomes the limitations of a single radar's detection field of view, expanding the horizontal detection range and reducing blind spots, making it particularly suitable for scenarios requiring wide field-of-view monitoring. Furthermore, the independent acquisition of point clouds from the three radars significantly increases the density of the detected point cloud, effectively compensating for the sparse point cloud defects of a single radar in long-range, complex environments, and improving the accuracy of target identification and localization. The offline calibration and data synchronization modules facilitate automated pose extrinsic parameter calibration and data synchronization using a checkerboard target and laser tracker, eliminating installation and time synchronization errors between multiple radars at the source, laying a precise foundation for subsequent point cloud processing. The raw point cloud preprocessing module automatically filters out random environmental noise from the point cloud data, preventing noise interference with subsequent registration accuracy. By configuring the coordinate system module, three radar point clouds can be automatically and efficiently mapped to the same reference coordinate system, solving the heterogeneity problem of multiple radar coordinate systems. The point cloud coarse registration module facilitates automated and rapid reduction of pose deviations based on geometric relationships and extrinsic parameters, lowering the computational complexity of precise registration. The point cloud precise registration and fusion output module facilitates automated high-precision alignment of point clouds, ultimately outputting a dense 4D point cloud. The communication module ensures real-time information exchange between the external terminal and the distributed detection device, supports the output of fused dense point cloud data, and can flexibly connect to external systems such as autonomous driving domain controllers and industrial monitoring platforms, adapting to the application needs of different scenarios. Integrating the distributed detection module, processor, and communication module into a single housing results in a compact structure, convenient installation, and applicability to integrated applications in various scenarios such as vehicle-mounted, road testing, and industrial inspection.

[0006] This device is compact, low in manufacturing cost, reliable in performance, highly practical, and highly integrated and intelligent. It can automatically complete perception enhancement and adapt to different risk conditions. It can obtain dense and high-quality 4D millimeter-wave point cloud data, which helps to significantly improve the accuracy of target contour, distance, speed and angle information, and can provide stable and reliable data support for upper-level target detection, segmentation and tracking.

[0007] The present invention also provides a 4D millimeter-wave point cloud registration method based on three radars, comprising the following steps; Step 1: Offline extrinsic parameter calibration and data synchronization; After the distributed detection device is installed, offline extrinsic parameter calibration is carried out using a combination of checkerboard target and laser tracker; Multiple sets of point cloud data of the target in different attitudes in the three radars are collected, and the accurate pose extrinsic parameters of the left and right radars relative to the middle reference radar are obtained based on the multi-attitude point cloud data; The distributed detection device includes a left-path 4D millimeter-wave radar, a middle-path 4D millimeter-wave radar and a right-path 4D millimeter-wave radar arranged at equal intervals along the horizontal direction; Step 2: Point cloud preprocessing; Statistical filtering is performed independently on the raw point clouds of the three radars. Outliers are removed based on the neighborhood average distance and the global mean standard deviation to obtain clean point clouds after denoising of the three radars. Step 3: Reference Coordinate System 1; Based on the extrinsic parameter matrix, homogeneous coordinate transformation, coordinate transformation and integration of common reference point clouds, the denoised point clouds of the left and right radars are uniformly mapped to the coordinate system of the middle reference radar, forming a point cloud set with a three-way common reference system; Step 4: Coarse registration; The coordinate system of the intermediate reference radar is denoted as the M system. By voxel downsampling of the three common coordinate point clouds under the M system, extracting the geometric features of the overlapping area, screening effective matching point pairs, iteratively estimating the rigid body increment, and combining the external parameter matrix, the random sampling consensus algorithm is used to complete the coarse registration with point cloud feature matching, and finally the initial pose matrix is ​​determined. At the same time, the geometric features of the overlapping area are used to further eliminate the errors caused by assembly and external parameters. Step 5: Fine registration and dense point cloud output; Based on the initial pose output from coarse registration, an improved iterative nearest point algorithm is used to perform high-precision pose optimization of the left and right point clouds relative to the reference point cloud in the M-frame. By constructing a cost function with the goal of minimizing the normal residual, a small increment of Lie algebra is introduced to transform the pose optimization problem into a problem of solving a system of linear equations. The LM algorithm is used to iteratively update until convergence. Finally, based on the converged high-precision pose matrix, the three-way point clouds are accurately aligned and fused to output a dense 4D millimeter-wave radar point cloud.

[0008] As a preferred option, the process of obtaining the clean point cloud after three-way denoising in step two is as follows: S21: Define a single-path original point cloud set; let a certain path's original point cloud be... , ,in, For a single 3D point in a point cloud, This represents the total number of points in the current path point cloud. S22: Select neighboring points; for each point Select its The nearest neighbor points constitute the neighborhood set. ; S23: Calculate the average distance within the neighborhood; for each point... The average distance between the point and all points in the neighborhood set is calculated according to formula (1). ; (1); S24: Calculate the global mean and standard deviation; based on the average neighborhood distance of all points, calculate the global mean according to formulas (2) and (3) respectively. and global standard deviation ; (2); (3); S25: Remove outliers; Set coefficients If a certain point If the average distance of the neighborhood satisfies formula (4), then the point is determined to be an outlier and removed; the remaining points that satisfy the conditions are retained to obtain the denoised clean point cloud. (4).

[0009] As a preferred option, the process of forming the point cloud set of the three-way common reference frame in step one is as follows: S31: Define the extrinsic parameter matrix; obtain the extrinsic parameter matrix from the left radar coordinate system to the intermediate reference radar coordinate system according to formula (5). According to formula (6), the external parameter matrix from the right radar coordinate system to the intermediate reference radar coordinate system is obtained. ; (5); In the formula, It is a 3×3 left rotation matrix. It is a 3×1 left translation vector; (6); In the formula, It is a 3×3 right rotation matrix. It is a 3×1 right translation vector; S32: Homogeneous coordinate transformation; expanding 3D points in the left and right radar clouds into 4D homogeneous coordinates. , , 3D point coordinates; S33: Perform coordinate transformation; perform coordinate transformation on the denoised point clouds of the left and right radars according to formulas (6) and (7) respectively, to obtain point clouds mapped to the M system. and ; (7); (8); S34: Integrated common point cloud; intermediate reference radar point cloud Without transformation, the final set of the three common points in the M system is as follows: , , .

[0010] As a preferred option, the process of obtaining the initial pose matrix in step four is as follows: S41: Point cloud downsampling; set voxel edge length Three-way coherent point clouds under the M system , , The voxel downsampling method was used to obtain the downsampled point cloud. , , ; S42: Geometric feature extraction; sampling point cloud from a benchmark radar. Above, the point cloud normal vector is estimated based on k neighborhood points of each point. Complete the geometric feature extraction of the overlapping region of the three point clouds; S43: Candidate matching relationship filtering; Based on the extracted feature descriptors, candidate correspondences between left-base station and right-base station cloud are established through the nearest neighbor method; At the same time, a multi-dimensional consistency threshold is applied to filter out valid matching point pairs and eliminate incorrect matches; S44: Rigid body increment estimation; using multiple sets of non-collinear effective matching point pairs as the minimum sample, the random sampling consensus algorithm is used to iteratively estimate the rigid body increments of the left and right point clouds relative to the reference point cloud, so that the cost function... Minimize, and finally obtain the small-scale rigid body context of the left and right point clouds relative to the reference point cloud. , ; S45: Determine the initial pose; combining the extrinsic parameter matrix and rigid body increment, obtain the initial pose matrices of the left and right radar point clouds relative to the reference radar point cloud according to formulas (9) and (10), respectively. , ; (9); (10).

[0011] As a preferred option, the process of outputting a dense 4D millimeter-wave radar point cloud in step five is as follows: S51: Construct the objective function; Under the M system, solve for the pose increments of the left and right point clouds relative to the reference point cloud respectively, and construct the cost function with the minimum normal residual as the objective according to formula (11). ; (11); In the formula, For the points in the left / right path midpoint cloud, For reference point cloud Zhongyu The corresponding nearest neighbor, for unit normal, Weights for matching point pairs; S52: Iterative optimization solution; using Lie algebra small increments The pose optimization problem is transformed into a problem of solving a system of linear equations. The normal equations are constructed according to formula (12), and the LM algorithm is used for iterative updates until the iteration converges. (12); S53: Point cloud fusion and output; Based on the converged high-precision pose matrix, the left and right radar point clouds are precisely aligned and fused with the reference radar point cloud, and finally dense 4D millimeter radar point cloud data is output.

[0012] As a preferred option, in step S22 of step two, a domain set is constructed. The process is as follows: For each point Implemented using kd-tree Nearest neighbor search is used, and a distance threshold is applied during the search process to filter out neighbors that meet the distance requirements. Find the nearest neighbor points and construct the domain set. .

[0013] As a preferred option, in step S34 of step three, the already processed three-way point cloud... , , The point clouds of the left and right radars are transformed to the coordinate system M of the middle reference radar.

[0014] As a preferred embodiment, in step S42 of step four, principal component analysis is used to process the neighborhood point set based on k neighborhood points of each point to estimate the point cloud normal vector. .

[0015] As a preferred option, in step S51 of step five, during the construction of the objective function, Huber robust loss is applied to the residuals to suppress the interference of outliers; according to formula (13), the weights of the matching point pairs are determined by combining physical and geometric information. And the weight normalization process makes In step five, S52, the final pose is obtained based on the iteration results. , Based on this, the left and right point clouds are aligned according to formulas (14) and (15) respectively, and the aligned point clouds are obtained respectively. , ; (13); (14); (15)

[0016] In the formula, , , These are three different weighting coefficients; It is the first Signal-to-noise ratio of each matched point pair; For the first Spatial distance between matching point pairs; This is a preset distance threshold; For the first The normal angle between a pair of matching points.

[0017] This invention provides a 4D millimeter-wave point cloud registration method based on three radars. Firstly, in the calibration stage, a combination of checkerboard targets and laser trackers is employed. The checkerboard targets provide clear feature benchmarks, while the laser trackers ensure measurement accuracy. This combination effectively reduces extrinsic parameter calculation errors, ensuring the accuracy of the left and right radar pose extrinsic parameters relative to the central reference radar, laying a solid foundation for subsequent coordinate unification. Simultaneously, by collecting multiple sets of target point cloud data under different attitudes, rather than single attitude data, a wider range of spatial angles can be covered, reducing the impact of attitude deviations on the calibration, improving the robustness of extrinsic parameter calculation, and avoiding calibration deviations caused by missing single attitude data. The three 4D millimeter-wave radars are installed side-by-side, with the central radar serving as the reference for point cloud fusion. This layout and coordination ensures effective overlap of the detection ranges of the three radars, providing sufficient overlap area for subsequent point cloud registration and fusion, while also facilitating the establishment of geometric relationships for extrinsic parameter calibration. Secondly, a statistical filtering algorithm is employed, based on outlier removal logic using the neighborhood average distance and global mean standard deviation. This accurately identifies and removes random noise points caused by environmental factors, improving data quality while preserving valid point cloud information. Statistical filtering is performed independently on the original point clouds of the three radars without cross-processing, fully considering the potential differences in noise characteristics between different radars and avoiding mutual interference between them. This ensures that the denoising effect of each point cloud is unaffected by others. Next, a complete process of defining extrinsic parameters, homogeneous coordinate transformation, coordinate transformation execution, and common-system point cloud integration achieves coordinate unification. The steps are tightly connected and logically rigorous. In particular, the application of homogeneous coordinate transformation unifies the rotation and translation transformations of 3D points into matrix multiplication operations, simplifying the coordinate transformation calculation process and improving transformation efficiency. Simultaneously, using the intermediate radar coordinate system as a unified reference balances the coordinate transformation distances of the left and right radars, reducing the cumulative error caused by unilateral transformations. Furthermore, the stronger stability of the intermediate reference provides a unified and reliable spatial reference for subsequent registration and fusion, avoiding the confusion caused by multiple references. Subsequently, voxel downsampling was performed on the coherent point cloud under the M-system before registration. This reduced the number of point clouds without losing core geometric features, significantly decreasing the computational load for subsequent feature extraction and matching, and greatly improving the efficiency of coarse registration, thus meeting the real-time requirements of engineering applications. Valid matching point pairs were established by extracting geometric features of overlapping regions and using multi-dimensional consistency threshold filtering. Incorrect matches could be effectively eliminated through multi-dimensional thresholds, ensuring the validity of the matching point pairs and providing reliable data support for subsequent rigid body increment estimation.Furthermore, employing a random sampling consensus algorithm combined with extrinsic parameter matrix estimation of rigid body increments effectively eliminates assembly errors and residual errors from extrinsic parameter calibration, preventing these errors from propagating to the fine registration stage. The final determined initial pose matrix can quickly align the left and right point clouds with the reference point cloud to the convergence range, providing good initial conditions for fine registration and effectively preventing fine registration from getting trapped in local optima. Finally, a cost function aimed at minimizing the normal residual is constructed, which can more accurately reflect the geometric fit of the point cloud surface through the normal residual. Compared with the traditional distance residual, it can significantly improve the accuracy of pose optimization and ensure the fit of point cloud alignment. On this basis, the introduction of Lie algebraic small increments transforms the pose optimization problem into a system of linear equations, reducing the complexity of nonlinear optimization. At the same time, iterative updates using the LM algorithm effectively combine the advantages of gradient descent and Gauss-Newton methods, ensuring both the convergence speed and stability of the iteration, avoiding iterative divergence, and ensuring a high-precision pose matrix. Based on the converged high-precision pose matrix, the three-way point cloud is accurately aligned and fused, realizing a complete closed loop from initial pose to accurate alignment and then to fusion output. The final output is a dense, high-quality 4D millimeter-wave radar point cloud that can integrate the detection information of the three radars, significantly improving the accuracy and robustness of the detection task, making up for the detection blind spot of a single radar, and improving the overall detection range and data density.

[0018] This method is simple to implement and has low implementation costs. It can significantly improve point cloud coverage and effective echo ratio, and improve the accuracy and stability of target detection, segmentation and tracking. It can solve the problems of sparse point cloud coverage and low detection reliability of single radar in existing technologies under complex working conditions such as dust, water mist and strong reflection. It can be used in scenarios with high perception requirements in low visibility and strong interference environments such as mines, transportation, and robotics. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the device portion of the present invention; Figure 2 This is a schematic block diagram of the device portion of the present invention; Figure 3 This is a flowchart of the method section of this invention.

[0020] In the diagram: 1. Left-side 4D millimeter-wave radar, 2. Middle-side 4D millimeter-wave radar, 3. Right-side 4D millimeter-wave radar, 4. Housing. Detailed Implementation

[0021] The invention will now be further described with reference to the accompanying drawings.

[0022] like Figure 1 and Figure 2As shown, the present invention provides a distributed detection device, including a housing 4, a distributed detection module, a processor, and a communication module; The distributed detection module includes a left-path 4D millimeter-wave radar 1, a middle-path 4D millimeter-wave radar 2, and a right-path 4D millimeter-wave radar 3. The left-path 4D millimeter-wave radar 1, the middle-path 4D millimeter-wave radar 2, and the right-path 4D millimeter-wave radar 3 are installed side by side in the housing 4 at equal intervals along the horizontal direction. The processor includes an offline calibration and data synchronization module, a raw point cloud preprocessing module, a coordinate system module, a point cloud coarse registration module, and a point cloud precise registration and fusion output module. The offline calibration and data synchronization module is connected to the distributed detection module, the checkerboard target, and the laser tracker, respectively, and is used to complete the orientation extrinsic parameter calibration and data synchronization of the three radars. The original point cloud preprocessing module is connected to the offline calibration and data synchronization module, and is used to filter out environmental random noise in the original point cloud and output a denoised clean point cloud. The coordinate system module is connected to the original point cloud preprocessing module and is used to map the point clouds of the left and right radars to the coordinate system of the middle base station radar, so as to construct a common reference system point cloud set for the three radars. The point cloud coarse registration module is connected to the coordinate system module and is used to quickly align the positions of the left and right radar point clouds with the reference radar point cloud based on the radar assembly geometry and external parameter information, and output the initial pose matrix. The point cloud precise registration and fusion output module is connected to the point cloud coarse registration module. It is used to achieve high-precision pose optimization based on the initial pose obtained by coarse registration, and complete the precise alignment and fusion of the three point clouds, and finally output a dense 4D millimeter-wave radar point cloud. The communication module is connected to the processor and is used to establish a communication connection between the processor and an external terminal. Preferably, the processor adopts a heterogeneous computing architecture, capable of parallel processing of data acquisition and registration algorithm calculations from three radars, and its data interface supports high-speed serial communication with the radar. As a preferred embodiment, the housing 4 is made of metal, and the front end of the housing 4 is provided with anti-static and flame-retardant ABS and PC boards to effectively reduce external power consumption interference, while effectively protecting the internal components from dust corrosion.

[0023] In this invention, three 4D millimeter-wave radars (left, center, and right) are horizontally and equally spaced side-by-side within a housing. This effectively protects the 4D millimeter-wave radars from damage and overcomes the limitations of a single radar's detection field of view, expanding the horizontal detection range and reducing blind spots, making it particularly suitable for scenarios requiring wide field-of-view monitoring. Furthermore, the independent acquisition of point clouds from the three radars significantly increases the density of the detected point cloud, effectively compensating for the sparse point cloud defects of a single radar in long-range, complex environments, and improving the accuracy of target identification and localization. The offline calibration and data synchronization modules facilitate automated pose extrinsic parameter calibration and data synchronization using a checkerboard target and laser tracker, eliminating installation and time synchronization errors between multiple radars at the source, laying a precise foundation for subsequent point cloud processing. The raw point cloud preprocessing module automatically filters out random environmental noise from the point cloud data, preventing noise interference with subsequent registration accuracy. By configuring the coordinate system module, three radar point clouds can be automatically and efficiently mapped to the same reference coordinate system, solving the heterogeneity problem of multiple radar coordinate systems. The point cloud coarse registration module facilitates automated and rapid reduction of pose deviations based on geometric relationships and extrinsic parameters, lowering the computational complexity of precise registration. The point cloud precise registration and fusion output module facilitates automated high-precision alignment of point clouds, ultimately outputting a dense 4D point cloud. The communication module ensures real-time information exchange between the external terminal and the distributed detection device, supports the output of fused dense point cloud data, and can flexibly connect to external systems such as autonomous driving domain controllers and industrial monitoring platforms, adapting to the application needs of different scenarios. Integrating the distributed detection module, processor, and communication module into a single housing results in a compact structure, convenient installation, and applicability to integrated applications in various scenarios such as vehicle-mounted, road testing, and industrial inspection.

[0024] This device is compact, low in manufacturing cost, reliable in performance, highly practical, and highly integrated and intelligent. It can automatically complete perception enhancement and adapt to different risk conditions. It can obtain dense and high-quality 4D millimeter-wave point cloud data, which helps to significantly improve the accuracy of target contour, distance, speed and angle information, and can provide stable and reliable data support for upper-level target detection, segmentation and tracking.

[0025] like Figure 3 As shown, the present invention also provides a 4D millimeter-wave point cloud registration method based on three radars, including the following steps; Step 1: Offline extrinsic parameter calibration and data synchronization; After the distributed detection device is installed, offline extrinsic parameter calibration is carried out using a combination of checkerboard target and laser tracker; Multiple sets of point cloud data of the target in different attitudes in the three radars are collected, and the accurate pose extrinsic parameters of the left and right radars relative to the middle reference radar are obtained based on the multi-attitude point cloud data, so as to lay the foundation for subsequent coordinate unification; The distributed detection device includes a left-path 4D millimeter-wave radar 1, a middle-path 4D millimeter-wave radar 2 and a right-path 4D millimeter-wave radar 3 arranged side by side at equal intervals along the horizontal direction; Step 2: Point cloud preprocessing; Statistical filtering is performed independently on the raw point clouds of the three radars without cross-processing to avoid mutual interference from different radar noise characteristics. Outliers are removed based on the neighborhood average distance and the global mean standard deviation to obtain clean point clouds after denoising. Thus, by filtering out random noise points caused by environmental factors, the data quality of the raw point cloud of a single channel can be improved, and noise can be avoided from interfering with the subsequent fusion accuracy. Step 3: Reference Coordinate System 1; Based on the extrinsic parameter matrix, homogeneous coordinate transformation, performing coordinate transformation and integrating common reference point clouds, the denoised point clouds of the left and right radars are uniformly mapped to the coordinate system of the middle reference radar to form a point cloud set with three common reference systems, so as to provide a unified spatial reference for subsequent registration and fusion. Step 4: Coarse Registration; The coordinate system of the intermediate reference radar is denoted as the M-system. By voxel downsampling of the three common-system point clouds under the M-system, extracting the geometric features of the overlapping area, screening effective matching point pairs, iteratively estimating the rigid body increment, and combining the extrinsic parameter matrix, the coarse registration is completed by using the random sampling consensus algorithm combined with point cloud feature matching. Finally, the initial pose matrix is ​​determined. At the same time, the geometric features of the overlapping area are used to further eliminate the errors caused by assembly and extrinsic parameters. In this way, assembly errors and residual errors of extrinsic parameter calibration can be effectively eliminated, providing an initial pose within the convergence range for fine registration.

[0026] Step 5: Fine registration and dense point cloud output; Based on the initial pose output from coarse registration, the improved Iterative Closest Point (ICP) algorithm is used to perform high-precision pose optimization of the left and right point clouds relative to the reference point cloud in the M-frame. By constructing a cost function with the goal of minimizing the normal residual, the pose optimization problem is transformed into a problem of solving a system of linear equations by introducing Lie algebraic small increments. The LM algorithm is used to iteratively update until convergence. Finally, based on the converged high-precision pose matrix, the three-way point clouds are accurately aligned and fused to output a dense 4D millimeter-wave radar point cloud.

[0027] As a preferred option, the process of obtaining the clean point cloud after three-way denoising in step two is as follows: S21: Define a single-path original point cloud set; let a certain path's original point cloud be... , ,in, For a single 3D point in a point cloud, This represents the total number of points in the current path point cloud. S22: Select neighboring points; for each point Select its The nearest neighbor points constitute the neighborhood set. ; S23: Calculate the average distance within the neighborhood; for each point... The average distance between the point and all points in the neighborhood set is calculated according to formula (1). ; (1); S24: Calculate the global mean and standard deviation; based on the average neighborhood distance of all points, calculate the global mean according to formulas (2) and (3) respectively. and global standard deviation ; (2); (3); S25: Remove outliers; Set coefficients If a certain point If the average distance of the neighborhood satisfies formula (4), then the point is determined to be an outlier and removed; the remaining points that satisfy the conditions are retained to obtain the denoised clean point cloud. (4).

[0028] As a preferred option, the process of forming the point cloud set of the three-way common reference frame in step one is as follows: S31: Define the extrinsic parameter matrix; obtain the extrinsic parameter matrix from the left radar coordinate system to the intermediate reference radar coordinate system according to formula (5). According to formula (6), the external parameter matrix from the right radar coordinate system to the intermediate reference radar coordinate system is obtained. ; (5); In the formula, It is a 3×3 left rotation matrix. It is a 3×1 left translation vector; (6); In the formula, It is a 3×3 right rotation matrix. It is a 3×1 right translation vector; S32: Homogeneous coordinate transformation; expanding 3D points in the left and right radar clouds into 4D homogeneous coordinates. , , 3D point coordinates; S33: Perform coordinate transformation; perform coordinate transformation on the denoised point clouds of the left and right radars according to formulas (6) and (7) respectively, to obtain point clouds mapped to the M system. and ; (7); (8); S34: Integrated common point cloud; intermediate reference radar point cloud Without transformation, the final set of the three common points in the M system is as follows: , , .

[0029] As a preferred option, the process of obtaining the initial pose matrix in step four is as follows: S41: Point cloud downsampling; set voxel edge length Three-way coherent point clouds under the M system , , The voxel downsampling method was used to obtain the downsampled point cloud. , , This reduces the computational load for subsequent feature extraction and matching. S42: Geometric feature extraction; sampling point cloud from a benchmark radar. Above, the point cloud normal vector is estimated based on k (15) neighborhood points for each point. Complete the geometric feature extraction of the overlapping region of the three point clouds; S43: Candidate matching relationship screening; Based on the extracted feature descriptors, candidate correspondences between left-base station and right-base station cloud are established through the nearest neighbor method; At the same time, multi-dimensional consistency thresholds (including distance threshold, normal angle threshold, and velocity consistency threshold) are applied to screen out valid matching point pairs and eliminate incorrect matches. S44: Rigid body increment estimation; using multiple sets of non-collinear effective matching point pairs as the minimum sample, the random sampling consensus algorithm is used to iteratively estimate the rigid body increments of the left and right point clouds relative to the reference point cloud, so that the cost function... Minimize, and finally obtain the small-scale rigid body context of the left and right point clouds relative to the reference point cloud. , ; S45: Determine the initial pose; combining the extrinsic parameter matrix and rigid body increment, obtain the initial pose matrices of the left and right radar point clouds relative to the reference radar point cloud according to formulas (9) and (10), respectively. , ; (9); (10).

[0030] As a preferred option, the process of outputting a dense 4D millimeter-wave radar point cloud in step five is as follows: S51: Construct the objective function; Under the M system, solve for the pose increments of the left and right point clouds relative to the reference point cloud respectively, and construct the cost function with the minimum normal residual as the objective according to formula (11). ; (11); In the formula, For the points in the left / right path midpoint cloud, For reference point cloud Zhongyu The corresponding nearest neighbor, for unit normal, Weights for matching point pairs; S52: Iterative optimization solution; using Lie algebra small increments The pose optimization problem is transformed into a problem of solving a system of linear equations. The normal equations are constructed according to formula (12), and the LM algorithm is used for iterative updates until the iteration converges. (12); S53: Point cloud fusion and output; Based on the converged high-precision pose matrix, the left and right radar point clouds are precisely aligned and fused with the reference radar point cloud, and finally dense 4D millimeter radar point cloud data is output.

[0031] As a preferred option, in step S22 of step two, a domain set is constructed. The process is as follows: For each point Implemented using kd-tree Nearest neighbor search is used, and a distance threshold is applied during the search process to filter out neighbors that meet the distance requirements. Find the nearest neighbor points and construct the domain set. .

[0032] As a preferred option, in step S34 of step three, the already processed three-way point cloud... , , The point clouds of the left and right radars are transformed to the coordinate system M of the middle reference radar.

[0033] As a preferred option, in step S42 of step four, principal component analysis is used to process the neighborhood point set based on k (15) neighborhood points for each point to estimate the point cloud normal vector. .

[0034] As a preferred option, in step S51 of step five, during the construction of the objective function, Huber robust loss is applied to the residuals to suppress the interference of outliers; according to formula (13), the weights of the matching point pairs are determined by combining physical and geometric information. And the weight normalization process makes In step five, S52, the final pose is obtained based on the iteration results. , Based on this, the left and right point clouds are aligned according to formulas (14) and (15) respectively, and the aligned point clouds are obtained respectively. , ; (13); (14); (15)

[0035] In the formula, , , These are three different weighting coefficients; It is the first Signal-to-noise ratio of each matched point pair; For the first Spatial distance between matching point pairs; This is a preset distance threshold; For the first The normal angle between a pair of matching points.

[0036] This invention provides a 4D millimeter-wave point cloud registration method based on three radars. Firstly, in the calibration stage, a combination of checkerboard targets and laser trackers is employed. The checkerboard targets provide clear feature benchmarks, while the laser trackers ensure measurement accuracy. This combination effectively reduces extrinsic parameter calculation errors, ensuring the accuracy of the left and right radar pose extrinsic parameters relative to the central reference radar, laying a solid foundation for subsequent coordinate unification. Simultaneously, by collecting multiple sets of target point cloud data under different attitudes, rather than single attitude data, a wider range of spatial angles can be covered, reducing the impact of attitude deviations on the calibration, improving the robustness of extrinsic parameter calculation, and avoiding calibration deviations caused by missing single attitude data. The three 4D millimeter-wave radars are installed side-by-side, with the central radar serving as the reference for point cloud fusion. This layout and coordination ensures effective overlap of the detection ranges of the three radars, providing sufficient overlap area for subsequent point cloud registration and fusion, while also facilitating the establishment of geometric relationships for extrinsic parameter calibration. Secondly, a statistical filtering algorithm is employed, based on outlier removal logic using the neighborhood average distance and global mean standard deviation. This accurately identifies and removes random noise points caused by environmental factors, improving data quality while preserving valid point cloud information. Statistical filtering is performed independently on the original point clouds of the three radars without cross-processing, fully considering the potential differences in noise characteristics between different radars and avoiding mutual interference between them. This ensures that the denoising effect of each point cloud is unaffected by others. Next, a complete process of defining extrinsic parameters, homogeneous coordinate transformation, coordinate transformation execution, and common-system point cloud integration achieves coordinate unification. The steps are tightly connected and logically rigorous. In particular, the application of homogeneous coordinate transformation unifies the rotation and translation transformations of 3D points into matrix multiplication operations, simplifying the coordinate transformation calculation process and improving transformation efficiency. Simultaneously, using the intermediate radar coordinate system as a unified reference balances the coordinate transformation distances of the left and right radars, reducing the cumulative error caused by unilateral transformations. Furthermore, the stronger stability of the intermediate reference provides a unified and reliable spatial reference for subsequent registration and fusion, avoiding the confusion caused by multiple references. Subsequently, voxel downsampling was performed on the coherent point cloud under the M-system before registration. This reduced the number of point clouds without losing core geometric features, significantly decreasing the computational load for subsequent feature extraction and matching, and greatly improving the efficiency of coarse registration, thus meeting the real-time requirements of engineering applications. Valid matching point pairs were established by extracting geometric features of overlapping regions and using multi-dimensional consistency threshold filtering. Incorrect matches could be effectively eliminated through multi-dimensional thresholds, ensuring the validity of the matching point pairs and providing reliable data support for subsequent rigid body increment estimation.Furthermore, employing a random sampling consensus algorithm combined with extrinsic parameter matrix estimation of rigid body increments effectively eliminates assembly errors and residual errors from extrinsic parameter calibration, preventing these errors from propagating to the fine registration stage. The final determined initial pose matrix can quickly align the left and right point clouds with the reference point cloud to the convergence range, providing good initial conditions for fine registration and effectively preventing fine registration from getting trapped in local optima. Finally, a cost function aimed at minimizing the normal residual is constructed, which can more accurately reflect the geometric fit of the point cloud surface through the normal residual. Compared with the traditional distance residual, it can significantly improve the accuracy of pose optimization and ensure the fit of point cloud alignment. On this basis, the introduction of Lie algebraic small increments transforms the pose optimization problem into a system of linear equations, reducing the complexity of nonlinear optimization. At the same time, iterative updates using the LM algorithm effectively combine the advantages of gradient descent and Gauss-Newton methods, ensuring both the convergence speed and stability of the iteration, avoiding iterative divergence, and ensuring a high-precision pose matrix. Based on the converged high-precision pose matrix, the three-way point cloud is accurately aligned and fused, realizing a complete closed loop from initial pose to accurate alignment and then to fusion output. The final output is a dense, high-quality 4D millimeter-wave radar point cloud that can integrate the detection information of the three radars, significantly improving the accuracy and robustness of the detection task, making up for the detection blind spot of a single radar, and improving the overall detection range and data density.

[0037] This method is simple to implement and has low implementation costs. It can significantly improve point cloud coverage and effective echo ratio, and improve the accuracy and stability of target detection, segmentation and tracking. It can solve the problems of sparse point cloud coverage and low detection reliability of single radar in existing technologies under complex working conditions such as dust, water mist and strong reflection. It can be used in scenarios with high perception requirements in low visibility and strong interference environments such as mines, transportation, and robotics.

Claims

1. A distributed detection device, characterized in that, Includes a housing (4), a distributed detection module, a processor, and a communication module; The distributed detection module includes a left-side 4D millimeter-wave radar (1), a middle-side 4D millimeter-wave radar (2), and a right-side 4D millimeter-wave radar (3). The left-side 4D millimeter-wave radar (1), the middle-side 4D millimeter-wave radar (2), and the right-side 4D millimeter-wave radar (3) are installed side by side in the housing (4) at equal intervals along the horizontal direction. The processor includes an offline calibration and data synchronization module, a raw point cloud preprocessing module, a coordinate system module, a point cloud coarse registration module, and a point cloud precise registration and fusion output module. The offline calibration and data synchronization module is connected to the distributed detection module, the checkerboard target, and the laser tracker, respectively, and is used to complete the orientation extrinsic parameter calibration and data synchronization of the three radars. The original point cloud preprocessing module is connected to the offline calibration and data synchronization module, and is used to filter out environmental random noise in the original point cloud and output a denoised clean point cloud. The coordinate system module is connected to the original point cloud preprocessing module and is used to map the point clouds of the left and right radars to the coordinate system of the middle base station radar, so as to construct a common reference system point cloud set for the three radars. The point cloud coarse registration module is connected to the coordinate system module and is used to quickly align the positions of the left and right radar point clouds with the reference radar point cloud based on the radar assembly geometry and external parameter information, and output the initial pose matrix. The point cloud precise registration and fusion output module is connected to the point cloud coarse registration module. It is used to achieve high-precision pose optimization based on the initial pose obtained by coarse registration, and complete the precise alignment and fusion of the three point clouds, and finally output a dense 4D millimeter-wave radar point cloud. The communication module is connected to the processor and is used to establish a communication connection between the processor and an external terminal.

2. A 4D millimeter-wave point cloud registration method based on three radars, employing a distributed detection device as described in claim 1, characterized in that, Includes the following steps; Step 1: Offline extrinsic parameter calibration and data synchronization; After the distributed detection device is installed, offline extrinsic parameter calibration is carried out using a combination of checkerboard target and laser tracker; Multiple sets of point cloud data of the target in three radars under different attitudes are collected, and the precise pose extrinsic parameters of the left radar and right radar relative to the middle reference radar are obtained based on the multi-attitude point cloud data; The distributed detection device includes a left-path 4D millimeter-wave radar (1), a middle-path 4D millimeter-wave radar (2) and a right-path 4D millimeter-wave radar (3) arranged side by side at equal intervals along the horizontal direction. Step 2: Point cloud preprocessing; Statistical filtering is performed independently on the raw point clouds of the three radars. Outliers are removed based on the neighborhood average distance and the global mean standard deviation to obtain clean point clouds after denoising of the three radars. Step 3: Reference Coordinate System 1; Based on the extrinsic parameter matrix, homogeneous coordinate transformation, coordinate transformation and integration of common reference point clouds, the denoised point clouds of the left and right radars are uniformly mapped to the coordinate system of the middle reference radar, forming a point cloud set with a three-way common reference system; Step 4: Coarse registration; The coordinate system of the intermediate reference radar is denoted as the M system. By voxel downsampling of the three common coordinate point clouds under the M system, extracting the geometric features of the overlapping area, screening effective matching point pairs, iteratively estimating the rigid body increment, and combining the external parameter matrix, the random sampling consensus algorithm is used to complete the coarse registration with point cloud feature matching, and finally the initial pose matrix is ​​determined. At the same time, the geometric features of the overlapping area are used to further eliminate the errors caused by assembly and external parameters. Step 5: Fine registration and dense point cloud output; Based on the initial pose output from coarse registration, an improved iterative nearest point algorithm is used to perform high-precision pose optimization of the left and right point clouds relative to the reference point cloud in the M-frame. By constructing a cost function with the goal of minimizing the normal residual, a small increment of Lie algebra is introduced to transform the pose optimization problem into a problem of solving a system of linear equations. The LM algorithm is used to iteratively update until convergence. Finally, based on the converged high-precision pose matrix, the three point clouds are accurately aligned and fused to output a dense 4D millimeter-wave radar point cloud.

3. The 4D millimeter-wave point cloud registration method based on three radars according to claim 2, characterized in that, In step two, the process of obtaining the clean point cloud after three-way denoising is as follows: S21: Define a single-path original point cloud set; let a certain path's original point cloud be... , ,in, For a single 3D point in a point cloud, This represents the total number of points in the current path point cloud. S22: Select neighboring points; for each point Select its The nearest neighbor points constitute the neighborhood set. ; S23: Calculate the average distance within the neighborhood; for each point... The average distance between the point and all points in the neighborhood set is calculated according to formula (1). ; （1）； S24: Calculate the global mean and standard deviation; based on the average neighborhood distance of all points, calculate the global mean according to formulas (2) and (3) respectively. and global standard deviation ; （2）； （3）； S25: Remove outliers; Set coefficients If a certain point If the average distance of the neighborhood satisfies formula (4), then the point is determined to be an outlier and removed; the remaining points that satisfy the conditions are retained to obtain the denoised clean point cloud. （4）。 4. The 4D millimeter-wave point cloud registration method based on three radars according to claim 3, characterized in that, In step one, the process of forming the point cloud set of the three common reference frames is as follows: S31: Define the extrinsic parameter matrix; obtain the extrinsic parameter matrix from the left radar coordinate system to the intermediate reference radar coordinate system according to formula (5). According to formula (6), the external parameter matrix from the right radar coordinate system to the intermediate reference radar coordinate system is obtained. ; （5）； In the formula, It is a 3×3 left rotation matrix. It is a 3×1 left translation vector; （6）； In the formula, It is a 3×3 right rotation matrix. It is a 3×1 right translation vector; S32: Homogeneous coordinate transformation; expanding 3D points in the left and right radar clouds into 4D homogeneous coordinates. , , 3D point coordinates; S33: Perform coordinate transformation; perform coordinate transformation on the denoised point clouds of the left and right radars according to formulas (6) and (7) respectively, to obtain point clouds mapped to the M system. and ; （7）； （8）； S34: Integrated common point cloud; intermediate reference radar point cloud Without transformation, the final set of the three common points in the M system is as follows: , , .

5. The 4D millimeter-wave point cloud registration method based on three radars according to claim 4, characterized in that, In step four, the process of obtaining the initial pose matrix is ​​as follows: S41: Point cloud downsampling; set voxel edge length Three-way coherent point clouds under the M system , , The voxel downsampling method was used to obtain the downsampled point cloud. , , ; S42: Geometric feature extraction; sampling point cloud from a benchmark radar. Above, the point cloud normal vector is estimated based on k neighborhood points of each point. Complete the geometric feature extraction of the overlapping region of the three point clouds; S43: Candidate matching relationship filtering; Based on the extracted feature descriptors, candidate correspondences between left-base station and right-base station cloud are established through the nearest neighbor method; At the same time, a multi-dimensional consistency threshold is applied to filter out valid matching point pairs and eliminate incorrect matches; S44: Rigid body increment estimation; using multiple sets of non-collinear effective matching point pairs as the minimum sample, the random sampling consensus algorithm is used to iteratively estimate the rigid body increments of the left and right point clouds relative to the reference point cloud, so that the cost function... Minimize, and finally obtain the small-scale rigid body context of the left and right point clouds relative to the reference point cloud. , ; S45: Determine the initial pose; combining the extrinsic parameter matrix and rigid body increment, obtain the initial pose matrices of the left and right radar point clouds relative to the reference radar point cloud according to formulas (9) and (10), respectively. , ; （9）； （10）。 6. The 4D millimeter-wave point cloud registration method based on three radars according to claim 5, characterized in that, In step five, the process of outputting a dense 4D millimeter-wave radar point cloud is as follows: S51: Construct the objective function; Under the M system, solve for the pose increments of the left and right point clouds relative to the reference point cloud respectively, and construct the cost function with the minimum normal residual as the objective according to formula (11). ; （11）； In the formula, For the points in the left / right path midpoint cloud, Reference point cloud Zhongyu The corresponding nearest neighbor, for unit normal, Weights for matching point pairs; S52: Iterative optimization solution; Using Lie algebra small increments The pose optimization problem is transformed into a problem of solving a system of linear equations. The normal equations are constructed according to formula (12), and the LM algorithm is used for iterative updates until the iteration converges. （12）； S53: Point cloud fusion and output; Based on the converged high-precision pose matrix, the left and right radar point clouds are precisely aligned and fused with the reference radar point cloud, and finally dense 4D millimeter radar point cloud data is output.

7. The 4D millimeter-wave point cloud registration method based on three radars according to claim 6, characterized in that, In step S22 of step two, the domain set is constructed. The process is as follows: For each point Implemented using kd-tree Nearest neighbor search is used, and a distance threshold is applied during the search process to filter out neighbors that meet the distance requirements. Find the nearest neighbor points and construct the domain set. .

8. The 4D millimeter-wave point cloud registration method based on three radars according to claim 7, characterized in that, In step S34 of step three, the already processed three-way point cloud... , , The point clouds of the left and right radars are transformed to the coordinate system M of the middle reference radar.

9. The 4D millimeter-wave point cloud registration method based on three radars according to claim 8, characterized in that, In step S42 of step four, principal component analysis is used to process the neighborhood point set based on k neighborhood points of each point to estimate the point cloud normal vector. .

10. A 4D millimeter-wave point cloud registration method based on three radars according to claim 9, characterized in that, In step S51 of step 5, during the construction of the objective function, Huber robust loss is applied to the residuals to suppress the interference of outliers; according to formula (13), the weights of the matching point pairs are determined by combining physical and geometric information. And the weight normalization process makes In step five, S52, the final pose is obtained based on the iteration results. , Based on this, the left and right point clouds are aligned according to formulas (14) and (15) respectively, and the aligned point clouds are obtained respectively. , ; （13）； （14）； （15 ）； In the formula, , , These are three different weighting coefficients; It is the first Signal-to-noise ratio of each matched point pair; For the first Spatial distance between matching point pairs; This is a preset distance threshold; For the first The normal angle between a pair of matching points.

Images