A 3D reconstruction method based on IMU and millimeter-wave radar

Abstract

The present invention discloses a three-dimensional reconstruction method based on IMU and millimeter wave radar, including: carrying IMU and radar on the same mobile device, collecting motion parameters of the mobile device during movement around the target to be reconstructed and point cloud data of the target; filtering out dynamic point clouds based on point cloud speed; converting all point cloud data to the same local coordinate system for denoising according to the collected motion parameters and point cloud data; obtaining the position and posture of each frame of the radar by matching the denoised point cloud data of adjacent frames; using the position and posture of each frame of the radar as observation data, using the motion parameters collected by IMU as control input, and using the Kalman filter algorithm to optimize and adjust the position and posture of each frame; constructing a three-dimensional map of the target according to the optimized and adjusted position and posture of each frame and the denoised point cloud data of each frame. The present invention uses IMU to assist 4D millimeter wave radar SLAM to improve the accuracy of three-dimensional reconstruction of the target.

Description

Technical Field

[0001] The invention belongs to the technical field of measurement and positioning, and specifically relates to a three-dimensional reconstruction method based on IMU and millimeter-wave radar. Background Art

[0002] In recent years, with the development of computer technology and sensor technology, target 3D reconstruction has become a research hotspot and the application market demand has increased significantly. SLAM technology has become one of the key technologies for target 3D reconstruction. However, the diversification and complexity of application scenarios have posed greater challenges to high-precision and high-performance SLAM technology.

[0003] Currently mature SLAM technologies include visual SLAM technology and laser radar SLAM technology, but since optical cameras and laser radars do not have the ability to work all day and all weather, such technologies are no longer suitable for scenes such as dense fog, heavy snow, and thick smoke. Moreover, the methods of filtering dynamic targets by laser SLAM and visual SLAM are mainly divided into three categories: filtering algorithms based on point cloud segmentation, filtering algorithms based on visibility, and filtering algorithms based on grids. These algorithms are highly complex and difficult to operate.

[0004] There is also technology that uses millimeter-wave radar for SLAM, but most of the existing millimeter-wave radar SLAM algorithms are modified and optimized based on the lidar SLAM algorithm. Due to the obvious difference in density and accuracy between the lidar point cloud and the millimeter-wave radar point cloud, the algorithm performance is poor.

[0005] With the update and iteration of millimeter wave radar, the traditional 3D millimeter wave radar has been upgraded to 4D millimeter wave radar, which not only has the ability to detect the three-dimensional spatial information of the target, but also has an increase in the number of antennas compared to the traditional 3D millimeter wave radar, which improves the point cloud accuracy and point cloud density. Although the point cloud accuracy and point cloud density of millimeter wave radar have been significantly improved, they still cannot reach the accuracy and density of laser radar point clouds. Therefore, the use of a single millimeter wave radar SLAM is not enough to cope with the challenges of complex and changing environments. Summary of the invention

[0006] The present invention provides a three-dimensional reconstruction method based on IMU and millimeter-wave radar, which utilizes IMU to assist 4D millimeter-wave radar SLAM to improve the three-dimensional reconstruction accuracy of the target.

[0007] In order to achieve the above technical objectives, the present invention adopts the following technical solutions:

[0008] A three-dimensional reconstruction method based on IMU and millimeter-wave radar, comprising:

[0009] Step 1: Mount the IMU and 4D millimeter-wave radar on the same mobile device, and keep the IMU and 4D millimeter-wave radar relatively still; move the mobile device around the target to be three-dimensionally reconstructed, collect the motion parameters of the mobile device at each moment through the IMU, and collect continuous multi-frame point cloud data of the target through the 4D millimeter-wave radar;

[0010] Step 2, extracting speed information from the point cloud data of the target, and filtering out dynamic point clouds in the point cloud data based on the speed information;

[0011] Step 3, obtain the point cloud data collected by the radar at the jth frame and the motion parameters collected by the IMU at the frame time, and transform the point cloud data of the jth frame to the radar local coordinate system of the j-1th frame by constructing the local rotation matrix and local translation matrix of the IMU at the jth frame; repeat step 3 until the point cloud data of all frames are transformed to the radar local coordinate system of the same frame; wherein, ;

[0012] Step 4, denoising all point cloud data in the same frame radar local coordinate system;

[0013] Step 5, by matching the denoised point cloud data of adjacent frames, the position and posture of the 4D millimeter-wave radar in each frame is obtained;

[0014] Step 6: Using the posture of the 4D millimeter-wave radar at each frame as observation data and the motion parameters collected by the IMU as control input, the Kalman filter algorithm is used to optimize and adjust the posture at each frame;

[0015] Step 7: construct a three-dimensional map of the target based on the poses of each frame obtained in step 6 and the point cloud data of each frame after denoising in step 4.

[0016] Furthermore, in step 2, if the speed of a point in the point cloud exceeds the speed resolution of the 4D millimeter-wave radar, the point is determined to be a dynamic point cloud and filtered out.

[0017] Furthermore, step 3 converts the point cloud data of the jth frame to the radar local coordinate system of the j-1th frame, specifically:

[0018] Step 3.1: Based on the motion parameters collected by the IMU at the jth frame of the radar, construct the local rotation matrix of the IMU at the jth frame and the local translation matrix :

[0019]

[0020]

[0021] In the formula, are the rotation angles of the radar around the X, Y, and Z axes in its local coordinate system at the jth frame, They are the displacements of the radar along the X, Y, and Z axes in its local coordinate system at the jth frame, respectively, obtained from the three-axis angular velocity and three-axis acceleration collected by the IMU:

[0022]

[0023]

[0024] in, is the collection interval, are the rotation angles around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the displacements of the radar along the X, Y, and Z axes in its local coordinate system at the j-1th frame, are the angular velocities around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the velocities along the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the accelerations along the X, Y, and Z axes in its local coordinate system at the j-1th frame, respectively;

[0025] Step 3.2, based on the constructed local rotation matrix and the local translation matrix , convert the point cloud data collected by the radar at the jth frame to the radar local coordinate system at the j-1th frame:

[0026]

[0027] In the formula, is the three-dimensional point cloud data collected by the radar at the jth frame, Collect point cloud data for the jth frame of the radar The three-dimensional coordinates in the local coordinate system of the radar at the j-1th frame.

[0028] Furthermore, all point cloud data in the same frame radar local coordinate system are denoised by improving the DBSCAN algorithm, including:

[0029] Step 4.1, obtain the angular accuracy of the 4D millimeter wave radar and all point cloud data in the local coordinate system of the same frame radar, and calculate the point Coordinates considering angular accuracy in a Cartesian coordinate system ;in, Points The three-axis position in the Cartesian coordinate system, Points Three-axis accuracy error in Cartesian coordinate system;

[0030] Step 4.2, for each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point ε-neighborhood of The corresponding ellipsoid is constructed as:

[0031]

[0032] In the formula, For point The corresponding ellipsoid The coordinates of the points on ;

[0033] In step 4.3, based on the ε-neighborhood of each point, the DBSCAN algorithm is used to determine and filter out the noise points in the point cloud data.

[0034] Furthermore, the calculation of each point in the point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system , specifically:

[0035] (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates :

[0036]

[0037] In the formula, Points Range, elevation and horizontal angle in the radar local coordinate system;

[0038] (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system:

[0039]

[0040] In the formula, The elevation angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar;

[0041] (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system :

[0042]

[0043] In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

[0044] Furthermore, in step 5, the iterative nearest point method is used to match the denoised point cloud data of adjacent frames. Specifically, the minimization of the distance between the same target point clouds of adjacent frames is used as the optimization goal, and the optimization objective function is established as follows:

[0045]

[0046] In the formula, They are the rotation matrix and translation matrix of the 4D millimeter-wave radar at the jth frame, are the three-axis position coordinates of point i in the j-1th frame and the jth frame respectively, and N is the number of target points in the j-1th frame and the jth frame.

[0047] Furthermore, step 6 uses the Kalman filter algorithm to optimize and adjust the posture at each frame, specifically including:

[0048] Step 6.1, obtain the motion parameters collected by IMU at the j-1th frame , the position and posture of 4D millimeter wave radar at the jth frame And the Kalman filter algorithm is used to optimize and adjust the optimal posture at the j-1th frame ;

[0049] Step 6.2, based on the obtained motion parameters of the j-1th frame And the optimal pose after optimization adjustment , predict the pose at the jth frame :

[0050]

[0051] In the formula, , are the posture state transfer matrix and control matrix at the jth frame respectively; Given by initialization;

[0052] Step 6.3, predict the pose state covariance matrix at the jth frame:

[0053]

[0054] In the formula, represents the predicted pose state covariance matrix, represents the pose state covariance matrix at the j-1th frame, is the process noise covariance matrix;

[0055] Step 6.4, calculate the Kalman gain:

[0056]

[0057] In the formula, represents the Kalman gain at the jth frame, represents the observation matrix at the jth frame, is the observation noise covariance matrix;

[0058] Step 6.5, take the pose at the jth frame As observation data, the predicted pose Perform optimization and adjustment to obtain the optimal posture of the jth frame after optimization and adjustment :

[0059]

[0060] Step 6.6, if the jth frame is the last frame, the optimal posture adjustment for all frames has been completed; otherwise, update the posture state covariance matrix at the jth frame , and return to step 6.1 to continue optimizing the optimal posture of the next frame; among which, the posture state covariance matrix at the jth frame is updated , expressed as:

[0061]

[0062] In the formula, Represents the identity matrix with all 1s on the diagonal and all 0s elsewhere.

[0063] Furthermore, in the process of IMU collecting motion parameters and 4D millimeter-wave radar collecting point cloud data, the IMU and 4D millimeter-wave radar are synchronized based on GPS and the industrial computer on the mobile device, that is:

[0064] When the GPS signal exists, the GPS signal is used to synchronize the timing of the IMU and 4D millimeter wave radar;

[0065] When the GPS signal does not exist, the timestamp of the industrial computer is used to synchronize the time of the IMU and 4D millimeter-wave radar.

[0066] Furthermore, when the mobile device moves around the target to be three-dimensionally reconstructed, if it is determined based on the GPS signal that the mobile device is in the same position in a certain frame as in another previous frame, then: the posture in the certain frame is modified to the posture in the other frame.

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

[0068] 1. Compared with single-frame millimeter-wave radar point cloud clustering, the present invention uses IMU-assisted multi-frame millimeter-wave radar point cloud matching and improves the DBSCAN algorithm to more accurately remove outliers and clutter point clouds and improve the point cloud quality.

[0069] 2. Although DBSCAN can effectively improve the quality of millimeter-wave radar point cloud, the accuracy of millimeter-wave radar point cloud is low, and the effect of three-dimensional reconstruction relying solely on millimeter-wave radar is average. The present invention uses Kalman filtering to jointly optimize IMU and millimeter-wave radar odometer to obtain high-precision posture and improve the accuracy of three-dimensional reconstruction.

[0070] 3. The combined timing of GPS and industrial computer in the present invention ensures the continuity of system time. The loop detection is completed by GPS, which can effectively reduce the amount of calculation.

[0071] 4. The present invention solves the problem that the working ability of optical cameras and lidar is limited in extreme weather conditions such as clouds and fog, resulting in large three-dimensional reconstruction errors. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of the three-dimensional reconstruction method based on IMU and millimeter-wave radar described in an embodiment of the present application. DETAILED DESCRIPTION

[0073] The following is a detailed description of an embodiment of the present invention. This embodiment is based on the technical solution of the present invention, and provides a detailed implementation method and a specific operation process to further explain the technical solution of the present invention.

[0074] This embodiment provides a 3D reconstruction method based on IMU and millimeter wave radar. Figure 1 As shown, including:

[0075] Step 1: Install the IMU and 4D millimeter-wave radar on the same mobile device, and keep them relatively still; move the mobile device around the target to be three-dimensionally reconstructed (such as vehicles, buildings, environments, etc.), collect the motion parameters of the mobile device at each moment through the IMU, and collect continuous multi-frame point cloud data of the target through the 4D millimeter-wave radar.

[0076] There is a significant difference between the frame rate of the IMU and the frame rate of the millimeter-wave radar point cloud. The frame rate of the IMU is mostly between 100Hz and 300Hz, while the actual frame rate of the millimeter-wave radar point cloud is usually around 20Hz. It is impossible to ensure that the millimeter-wave radar point cloud is completely aligned with the IMU timestamp, resulting in increased system errors. Therefore, it is necessary to align the IMU timestamp with the millimeter-wave radar point cloud timestamp. At present, the commonly used timestamp alignment method uses GPS for unified timing, but in closed spaces such as tunnels, underground roads, and underground garages, the GPS signal is unstable or disappears, and the unified timing work cannot be completed. In view of the timing problem, the present invention proposes an IMU and millimeter-wave radar time synchronization method based on GPS combined with industrial computer timing, that is, in the process of IMU collecting motion parameters and 4D millimeter-wave radar collecting point cloud data, the industrial computer on the mobile device is combined with GPS to synchronize the IMU and the 4D millimeter-wave radar: first, the industrial computer, the IMU and the millimeter-wave radar are unified on the GPS time reference to provide unified clock information; then, when the GPS signal exists, the GPS signal can be used to synchronize the IMU and the 4D millimeter-wave radar; when the GPS signal does not exist, the timestamp of the industrial computer is obtained under the Linux system, and the timestamp of the industrial computer is used to synchronize the IMU and the 4D millimeter-wave radar; and when the GPS signal is restored, the GPS signal time is used again.

[0077] In a more preferred embodiment, the IMU data will be affected by different noises during the acquisition process, and the accuracy will be reduced. Therefore, filtering is required to reduce the influence of noise. A sliding average filtering method can be used, which is expressed as:

[0078]

[0079] in, Input IMU data, M is the size of the sliding window. The motion trajectory of the mobile device is obtained by performing inertial navigation solution.

[0080] Step 2: extract speed information from the point cloud data of the target, and filter out dynamic point clouds in the point cloud data based on the speed information.

[0081] Dynamic targets in the scene affect the accuracy of point cloud matching, reduce the accuracy of pose estimation, and cause ghosting in the SLAM map. Therefore, filtering out dynamic targets is crucial to improving SLAM accuracy. At present, the methods for filtering out dynamic targets in laser SLAM and visual SLAM are mainly divided into three categories: filtering algorithms based on point cloud segmentation, filtering algorithms based on visibility, and filtering algorithms based on grids. These algorithms are highly complex and difficult to operate.

[0082] The embodiment of the present invention utilizes the characteristic that the millimeter wave radar can directly measure the speed of the target, and identifies and filters the dynamic point cloud by judging the motion attribute of the target, thereby improving the SLAM accuracy of the millimeter wave radar. The specific processing method is: if the speed of a point in the point cloud exceeds the speed resolution of the 4D millimeter wave radar, the point is determined to be a dynamic point cloud and filtered out.

[0083] Step 3, obtain the point cloud data collected by the radar at the jth frame and the motion parameters collected by the IMU at the frame time, and transform the point cloud data of the jth frame into the radar local coordinate system of the j-1th frame by constructing the local rotation matrix and local translation matrix of the IMU at the jth frame; wherein, .

[0084] The point cloud data of the jth frame is converted to the radar local coordinate system of the j-1th frame, specifically:

[0085] Step 3.1: Based on the motion parameters collected by the IMU at the jth frame of the radar, construct the local rotation matrix of the IMU at the jth frame and the local translation matrix :

[0086]

[0087]

[0088] In the formula, They are the rotation angles of the IMU around the X, Y, and Z axes in the local coordinate system when the IMU collects the jth frame of the radar. They are the displacements of the X, Y, and Z axes in the local coordinate system when the IMU collects the jth frame of the radar, and are obtained from the three-axis angular velocity and three-axis acceleration collected by the IMU:

[0089]

[0090]

[0091] in, is the collection interval, are the rotation angles around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the displacements of the radar along the X, Y, and Z axes in its local coordinate system at the j-1th frame, are the angular velocities around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the velocities along the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the accelerations along the X, Y, and Z axes in its local coordinate system at the j-1th frame, respectively;

[0092] Step 3.2, based on the constructed local rotation matrix and the local translation matrix , convert the point cloud data collected by the radar at the jth frame to the radar local coordinate system at the j-1th frame:

[0093]

[0094] In the formula, is the three-dimensional point cloud data collected by the radar at the jth frame, Collect point cloud data for the jth frame of the radar The three-dimensional coordinates in the local coordinate system of the radar at the j-1th frame.

[0095] Repeat step 3 above until the point cloud data of all frames are converted to the radar local coordinate system of the same frame.

[0096] Step 4: De-noise all point cloud data in the same frame radar local coordinate system.

[0097] The embodiment of the present invention performs denoising processing on all point cloud data in the local coordinate system of the same frame radar by improving the DBSCAN algorithm, which specifically includes:

[0098] Step 4.1, obtain the angular accuracy of the 4D millimeter wave radar and all point cloud data in the local coordinate system of the same frame radar, and calculate the point Coordinates considering angular accuracy in a Cartesian coordinate system ;

[0099] Each point in the calculation point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system , specifically:

[0100] (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates :

[0101]

[0102] In the formula, Points The three-axis position in the Cartesian coordinate system, Points Range, elevation and horizontal angle in the radar local coordinate system;

[0103] (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system:

[0104]

[0105] In the formula, The elevation angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar; Points Three-axis accuracy error in Cartesian coordinate system;

[0106] (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system :

[0107]

[0108] In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

[0109] Step 4.2, for each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point ε-neighborhood of The corresponding ellipsoid is constructed as:

[0110]

[0111] In the formula, For point The corresponding ellipsoid The coordinates of the points on ;

[0112] Step 4.3, based on the ε-neighborhood of each point, use the DBSCAN algorithm to determine and filter out the noise points in the point cloud data. Specifically include:

[0113] (1) All points in the frame are set as sample point sets, and the minimum number of included points MinPts is set. In this embodiment, the minimum number of included points MinPts is set to 3.

[0114] (2) Statistical sample points The number of sample points in its ε-neighborhood, if the number is greater than or equal to MinPts, then the sample point As the core sample point;

[0115] (3) For each non-core sample point in the frame, determine whether it is in the ε-neighborhood of any core sample point. If so, the non-core sample point is a boundary sample; if not, the non-core sample point is a noise point.

[0116] (4) Discard the noise points from the point cloud data of the frame.

[0117] Step 5: By matching the denoised point cloud data of adjacent frames, the position and posture of the 4D millimeter-wave radar in each frame are obtained.

[0118] This step uses the iterative nearest point method to match the denoised point cloud data of adjacent frames. Specifically, the minimization of the distance between the same target point clouds of adjacent frames is used as the optimization goal, and the optimization objective function is established as follows:

[0119]

[0120] In the formula, They are the rotation matrix and translation matrix of the 4D millimeter-wave radar at the jth frame, are the three-axis position coordinates of point i in the j-1th frame and the jth frame respectively, and N is the number of target points in the j-1th frame and the jth frame.

[0121] Step 6: Use the position and posture of the 4D millimeter-wave radar at each frame obtained in step 5 (including the rotation matrix and translation matrix ) as observation data, the motion parameters collected by IMU as control input, and the Kalman filter algorithm are used to optimize the posture of each frame. Specifically include:

[0122] Step 6.1, obtain the motion parameters collected by IMU at the j-1th frame , the position and posture of 4D millimeter wave radar at the jth frame And the Kalman filter algorithm is used to optimize and adjust the optimal posture at the j-1th frame ;

[0123] Step 6.2, based on the obtained motion parameters of the j-1th frame And the optimal pose after optimization adjustment , predict the pose at the jth frame :

[0124]

[0125] In the formula, , are the posture state transfer matrix and control matrix at the jth frame respectively;

[0126] Step 6.3, predict the pose state covariance matrix at the jth frame:

[0127]

[0128] In the formula, represents the predicted pose state covariance matrix, represents the pose state covariance matrix at the j-1th frame, is the process noise covariance matrix;

[0129] Step 6.4, calculate the Kalman gain:

[0130]

[0131] In the formula, represents the Kalman gain at the jth frame, represents the observation matrix at the jth frame, is the observation noise covariance matrix;

[0132] Step 6.5, take the pose at the jth frame As observation data, the predicted pose Perform optimization and adjustment to obtain the optimal posture of the jth frame after optimization and adjustment :

[0133]

[0134] Step 6.6, if the jth frame is the last frame, the optimal posture adjustment for all frames has been completed; otherwise, update the posture state covariance matrix at the jth frame , and return to step 6.1 to continue optimizing the pose of the next frame. Among them, the pose state covariance matrix at the jth frame is updated , expressed as:

[0135]

[0136] In the formula, Represents the identity matrix with all 1s on the diagonal and all 0s elsewhere.

[0137] The IMU provides short-term smooth and high-frequency motion estimation, and the millimeter-wave radar provides relatively accurate position and attitude measurement. Therefore, this embodiment uses the Kalman filter algorithm to effectively fuse the information of the two, making the position and attitude estimation more accurate and robust.

[0138] Step 7: construct a three-dimensional map of the target based on the poses of each frame obtained in step 6 and the point cloud data of each frame after denoising in step 4.

[0139] When facing large-scale 3D reconstruction scenes, due to the large cumulative errors of IMU and millimeter-wave radar odometers, in a better embodiment, loop detection can be used to calibrate the cumulative errors, that is, the mobile device equipped with sensors returns to the starting position or arrives at a place it has been to before, and determines whether the current posture of the mobile device is the same as the starting position or the posture of the place it has been to. If they are different, the current posture needs to be changed to the previous posture to complete the cumulative error correction.

[0140] Specifically, since GPS can obtain absolute position information, when the mobile device moves around the target to be 3D reconstructed, if the mobile device is judged to be in the same position in a certain frame as in another frame according to the GPS signal, then the position in the certain frame is modified to the position in the other frame. In this way, loop detection can be achieved using only GPS position, solving the problem of high complexity and large amount of calculation in traditional loop detection methods.

[0141] The above embodiments are preferred embodiments of the present application. Ordinary technicians in this field can also make various changes or improvements on this basis. Without departing from the overall concept of the present application, these changes or improvements should fall within the scope of protection required by the present application.

Claims

1. A three-dimensional reconstruction method based on IMU and millimeter-wave radar, characterized in that: include: Step 1: Mount the IMU and 4D millimeter-wave radar on the same mobile device, and keep the IMU and 4D millimeter-wave radar relatively still; move the mobile device around the target to be three-dimensionally reconstructed, collect the motion parameters of the mobile device at each moment through the IMU, and collect continuous multi-frame point cloud data of the target through the 4D millimeter-wave radar; Step 2, extracting speed information from the point cloud data of the target, and filtering out dynamic point clouds in the point cloud data based on the speed information; Step 3, obtain the point cloud data collected by the radar at the jth frame and the motion parameters collected by the IMU at the frame time, and transform the point cloud data of the jth frame to the radar local coordinate system of the j-1th frame by constructing the local rotation matrix and local translation matrix of the IMU at the jth frame; repeat step 3 until the point cloud data of all frames are transformed to the radar local coordinate system of the same frame; wherein, ; Step 4, denoising all point cloud data in the same frame radar local coordinate system; By improving the DBSCAN algorithm, all point cloud data in the local coordinate system of the same frame radar are denoised, including: Step 4.1, obtain the angular accuracy of the 4D millimeter wave radar and all point cloud data in the local coordinate system of the same frame radar, and calculate the point Coordinates considering angular accuracy in a Cartesian coordinate system ;in, Points The three-axis position in the Cartesian coordinate system, Points Three-axis accuracy error in Cartesian coordinate system; Step 4.2, for each point in the point cloud data , based on its coordinates Construct the corresponding ellipsoid and use the ellipsoid as the point ε-neighborhood of The corresponding ellipsoid is constructed as: ； In the formula, For point The corresponding ellipsoid The coordinates of the points on ; Step 4.3, based on the ε-neighborhood of each point, the DBSCAN algorithm is used to determine and filter out the noise points in the point cloud data; Step 5, by matching the denoised point cloud data of adjacent frames, the position and posture of the 4D millimeter-wave radar in each frame is obtained; Step 6: Using the posture of the 4D millimeter-wave radar at each frame as observation data and the motion parameters collected by the IMU as control input, the Kalman filter algorithm is used to optimize and adjust the posture at each frame; Step 7: construct a three-dimensional map of the target based on the poses of each frame obtained in step 6 and the point cloud data of each frame after denoising in step 4.

2. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: In step 2, if the speed of a point in the point cloud exceeds the speed resolution of the 4D millimeter-wave radar, the point is determined to be a dynamic point cloud and filtered out.

3. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: Step 3 converts the point cloud data of the jth frame to the radar local coordinate system of the j-1th frame, specifically: Step 3.1: Based on the motion parameters collected by the IMU at the jth frame of the radar, construct the local rotation matrix of the IMU at the jth frame and the local translation matrix : ； ； In the formula, are the rotation angles of the radar around the X, Y, and Z axes in its local coordinate system at the jth frame, They are the displacements of the radar along the X, Y, and Z axes in its local coordinate system at the jth frame, respectively, obtained from the three-axis angular velocity and three-axis acceleration collected by the IMU: ； ； in, is the collection interval, are the rotation angles around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the displacements of the radar along the X, Y, and Z axes in its local coordinate system at the j-1th frame, are the angular velocities around the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the velocities along the X, Y, and Z axes in the local coordinate system at the j-1th frame, are the accelerations along the X, Y, and Z axes in its local coordinate system at the j-1th frame, respectively; Step 3.2, based on the constructed local rotation matrix and the local translation matrix , convert the point cloud data collected by the radar at the jth frame to the radar local coordinate system at the j-1th frame: ； In the formula, is the three-dimensional point cloud data collected by the radar at the jth frame, Collect point cloud data for the jth frame of the radar The three-dimensional coordinates in the local coordinate system of the radar at the j-1th frame.

4. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: Each point in the calculation point cloud data Coordinates considering angular accuracy in a Cartesian coordinate system , specifically: (1) Point Coordinates in the radar local coordinate system , converted to Cartesian coordinates : ； In the formula, Points Range, elevation and horizontal angle in the radar local coordinate system; (2) Calculate the point according to the angular accuracy of the 4D millimeter wave radar Precision error in Cartesian coordinate system: ； In the formula, The elevation angle accuracy and horizontal angle accuracy of the 4D millimeter wave radar; (3) Comprehensive points Coordinates in Cartesian coordinate system and precision error, we get the point Coordinates considering angular accuracy in a Cartesian coordinate system : ； In the formula, For point Three-axis position considering angular accuracy in Cartesian coordinates.

5. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: Step 5 uses the iterative nearest point method to match the denoised point cloud data of adjacent frames. Specifically, the minimization of the distance between the same target point clouds of adjacent frames is used as the optimization goal, and the optimization objective function is established as follows: ； In the formula, They are the rotation matrix and translation matrix of the 4D millimeter-wave radar at the jth frame, are the three-axis position coordinates of point i in the j-1th frame and the jth frame respectively, and N is the number of target points in the j-1th frame and the jth frame.

6. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: Step 6 uses the Kalman filter algorithm to optimize and adjust the posture of each frame, including: Step 6.1, obtain the motion parameters collected by IMU at the j-1th frame , the position and posture of 4D millimeter wave radar at the jth frame And the Kalman filter algorithm is used to optimize and adjust the optimal posture at the j-1th frame ; Step 6.2, based on the obtained motion parameters of the j-1th frame And the optimal pose after optimization adjustment , predict the pose at the jth frame : ； In the formula, , are the posture state transfer matrix and control matrix at the jth frame respectively; Given by initialization; Step 6.3, predict the pose state covariance matrix at the jth frame: ； In the formula, represents the predicted pose state covariance matrix, represents the pose state covariance matrix at the j-1th frame, is the process noise covariance matrix; Step 6.4, calculate the Kalman gain: ； In the formula, represents the Kalman gain at the jth frame, represents the observation matrix at the jth frame, is the observation noise covariance matrix; Step 6.5, take the pose at the jth frame As observation data, the predicted pose Perform optimization and adjustment to obtain the optimal posture of the jth frame after optimization and adjustment : ； Step 6.6, if the jth frame is the last frame, the optimal posture adjustment for all frames has been completed; otherwise, update the posture state covariance matrix at the jth frame , and return to step 6.1 to continue optimizing the optimal posture of the next frame; among which, the posture state covariance matrix at the jth frame is updated , expressed as: ； In the formula, Represents the identity matrix with all 1s on the diagonal and all 0s elsewhere.

7. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: In the process of IMU collecting motion parameters and 4D millimeter wave radar collecting point cloud data, the industrial computer on the mobile device is combined with GPS to synchronize the time of IMU and 4D millimeter wave radar, that is: When the GPS signal exists, the GPS signal is used to synchronize the timing of the IMU and 4D millimeter wave radar; When the GPS signal does not exist, the timestamp of the industrial computer is used to synchronize the time of the IMU and 4D millimeter-wave radar.

8. The three-dimensional reconstruction method based on IMU and millimeter-wave radar according to claim 1, characterized in that: When a mobile device moves around a target to be three-dimensionally reconstructed, if it is determined based on GPS signals that the mobile device is at the same position in a certain frame as in another previous frame, then: the posture in the certain frame is modified to the posture in the other frame.

Images