A pose parameter calibration method for a millimeter wave radar-imu

Abstract

A millimeter wave radar-IMU pose parameter calibration method, comprising: collecting multiple sets of IMU and radar point cloud data on a grid plate, and accumulating multiple frames of point clouds in front and back; for point clouds with unchanged IMU attitude or position, point cloud registration is performed relative to the origin, the relative pose of the radar is calculated, the corresponding IMU data is obtained, and the relative pose of the IMU is calculated; based on the degenerated hand-eye calibration algorithm, a least squares loss function is constructed for the two types of relative poses in turn, and the initial values of the rotation and translation parameters are optimized; the point clouds with changing IMU poses are projected to multiple centers, the center point clouds are characterized by four-dimensional normal distribution, a re-projection error loss function of points and distribution characteristics is constructed, and the final parameter values are optimized. It solves the problem of missing IMU odometry or difficulty in fully rotating the body during calibration, and also proposes a millimeter wave radar point cloud registration method, which improves the adaptability of the millimeter wave radar-IMU system in sensor parameter calibration.

Description

Technical Field

[0001] This invention relates to the field of sensor extrinsic parameter calibration, and more specifically to a method for calibrating the pose extrinsic parameters of a millimeter-wave radar-IMU. Background Technology

[0002] In current Simultaneous Localization and Mapping (SLAM) problems, it is often necessary to fuse data from multiple sensors to more accurately describe the vehicle's own state and its surrounding environment. The raw sensor data is based on its own coordinate system. To fuse multi-sensor data, it is necessary to know the transformation relationships between the sensor coordinate systems, i.e., pose extrinsic parameter calibration. Millimeter-wave radar can generally sense the 3D position and radar cross-section (RCS) information of environmental obstacles, while IMU can generally obtain the vehicle's three-axis angular velocity and three-axis acceleration information in real time and calculate the vehicle's spatial attitude. Fusion of the IMU can provide good initial values ​​for radar self-motion estimation and can reduce radar point cloud pose distortion caused by vehicle motion. If the millimeter-wave radar-IMU pose extrinsic parameters are not calibrated, the different coordinate systems between the data will lead to computational confusion, thus reducing the overall accuracy of the SLAM system.

[0003] Most existing radar extrinsic parameter calibration algorithms require the radar's relative pose, thus necessitating radar pose estimation algorithms. However, there are currently no open-source or mature millimeter-wave radar pose estimation algorithms based on point cloud registration. Furthermore, there are currently no millimeter-wave radar-IMU pose extrinsic parameter calibration algorithms; most existing methods are lidar-IMU calibration methods. For calibration without an IMU odometry, current techniques require full rotation around the IMU at a single location to calculate extrinsic parameters, which is inconvenient for systems where multi-degree-of-freedom rotation is difficult. For calibration with an IMU odometry, pre-integration of the IMU or calibration outdoors with GPS signal is required, which can lead to accuracy degradation due to accumulated errors or sacrifice the advantages of calibration in strong indoor environments.

[0004] The following technologies have been publicly disclosed recently:

[0005] Patent No.: CN 202010053386.8, Patent Title: A Method and System for LiDAR-IMU Extrinsic Parameter Calibration. The extrinsic parameter calibration method includes: pre-integrating the IMU to obtain the predicted pose estimate and data association residuals; projecting multiple lidar point clouds onto the world coordinate system based on the pre-integration results and calculating the reprojection error; calculating the reprojection error from the laser point to the calibration target map using data association; and iteratively optimizing the lidar-IMU extrinsic parameters using a nonlinear least squares method. This method reduces point cloud motion distortion by utilizing IMU pre-integration and jointly optimizes the calibration extrinsic parameters, radar relative pose, IMU bias, and time drift, thereby improving the accuracy of extrinsic parameter calibration.

[0006] However, it still requires integration in essence. For low-frequency radar or low-frequency IMU, the increased cumulative error and the large number of parameters that need to be optimized will lead to a decrease in optimization accuracy.

[0007] This application proposes a millimeter-wave radar-IMU pose extrinsic parameter calibration method. Multiple sets of radar and IMU data are acquired on a mesh board, and the radar point cloud is accumulated over multiple frames, increasing point cloud density and stability. Initial extrinsic parameter values ​​are obtained using a degenerate hand-eye calibration algorithm, and the final extrinsic parameters are obtained by optimizing the multi-center reprojection error. Through joint optimization of multiple sets of data at different stages, the accuracy of extrinsic parameter calibration is increased, the influence of manual placement errors is reduced, and the relative pose of the IMU is easily obtained, avoiding the cumulative error caused by integration.

[0008] Patent No.: CN 202010230937.3, Patent Title: A Radar-IMU Calibration Method Based on Hand-Eye Calibration. This method uses feature points extracted from point cloud curvature features to match two adjacent radar scans, obtaining the relative pose between the two radar scans; it aligns IMU data with radar data in the time dimension to calculate the IMU relative pose within adjacent timestamps; based on the calculated radar and IMU relative poses, it uses the hand-eye calibration method to calculate the radar-IMU relative pose; it uses the calculated radar-IMU relative pose to remove motion distortion from the radar point cloud data, reducing point cloud matching errors, and further completing the relative pose calibration. This method applies the traditional hand-eye calibration problem of camera-IMU to radar-IMU, while utilizing IMU pose to reduce point cloud distortion, further improving the accuracy of extrinsic parameter calibration.

[0009] However, the relative pose of the IMU is obtained from the inertial navigation module, not calculated based on the pure IMU, so calibration cannot be performed indoors or in environments without GPS signals. Furthermore, because the point cloud from millimeter-wave radar is sparse in outdoor environments, the point cloud registration accuracy of this method will decrease, thus reducing the accuracy of the hand-eye calibration algorithm.

[0010] This application proposes a millimeter-wave radar-IMU pose extrinsic parameter calibration method. This method allows for the acquisition of multiple sets of radar and IMU data using an indoor grid panel, and the accumulation of radar point clouds across multiple frames, increasing point cloud density and stability. The point cloud registration process employs adaptive voxel NDT registration, improving registration accuracy and success rate. The extrinsic parameter optimization process utilizes joint optimization of multiple sets of data at different stages, increasing accuracy while reducing the impact of manual placement errors.

[0011] Patent No.: CN 202210498192.8, Patent Title: A Method and Apparatus for Extrinsic Parameter Calibration of LiDAR to IMU. The extrinsic parameter calibration method includes: extracting features from N consecutive frames of point clouds from a LiDAR; calculating the inter-frame relative pose based on the feature points; transforming N-1 frames of point clouds to the coordinate system of the first frame point cloud according to the inter-frame relative pose; transforming the N frames of point clouds in the first frame radar coordinate system to the first coordinate system using a preset IMU-to-first coordinate system transformation relationship and the radar-IMU relative pose to be optimized; calculating the distance error between the feature points of the N-1 frames of point clouds and the feature points of the first frame point cloud in the first coordinate system, optimizing to obtain the radar-IMU relative pose, and completing the calibration of the pose extrinsic parameters. This method does not directly use IMU data, reducing the accumulated error caused by IMU integration, and simultaneously uses optimization methods to reduce feature distance errors, improving the accuracy of extrinsic parameter calibration.

[0012] However, transforming all point clouds to the coordinate system of the first frame point cloud relies on the radar's relative pose, which places high demands on the accuracy of radar relative pose estimation. Furthermore, the radar relative pose error will be carried over into subsequent extrinsic parameter optimization, increasing the uncertainty of the optimization results.

[0013] This application proposes a millimeter-wave radar-IMU pose extrinsic parameter calibration method. Multiple sets of radar and IMU data can be acquired indoors using a grid panel. The relative pose of the IMU can be directly calculated by reading sensor values ​​and its position on the grid panel. Based on the relative pose of the IMU and the radar-IMU extrinsic parameters to be optimized, all point clouds are transformed to the same coordinate system, and the final extrinsic parameters are obtained through optimization. This method eliminates the need for radar relative pose to transform point clouds to the same coordinate system, reducing the impact of radar point cloud registration errors on extrinsic parameter optimization.

[0014] To address the challenges of sparse and noisy point clouds in millimeter-wave radar, making registration difficult, the difficulty in obtaining the relative pose of the IMU when the IMU odometry is missing, and the difficulty in multi-degree-of-freedom rotation of the carrier, this application proposes a millimeter-wave radar-IMU pose extrinsic parameter calibration method. This method acquires radar and IMU data on a mesh board, making it easier to obtain the relative pose of the IMU. Simultaneously, it accumulates multiple frames of the millimeter-wave radar point cloud, improving point cloud density and stability. The point cloud registration utilizes adaptive voxel NDT registration, improving registration accuracy and success rate. Based on multiple sets of specific data, a degenerate hand-eye calibration algorithm is used to optimize and obtain initial values ​​for extrinsic parameter rotation and displacement. Then, the multi-center reprojection error is optimized to obtain the final extrinsic parameter values, increasing the success rate of extrinsic parameter calibration and reducing the impact of manual placement errors. Summary of the Invention

[0015] To address the aforementioned technical problems, this invention proposes a method for extrinsic parameter calibration of millimeter-wave radar-IMU. This method allows for the acquisition of sensor data using a grid board placed indoors, and improves registration success rate and accuracy through multi-stage extrinsic parameter optimization. It also solves the problems of missing IMU odometry or difficulty in fully rotating the IMU during calibration. Furthermore, a millimeter-wave radar point cloud registration method is proposed, enhancing the adaptability of the millimeter-wave radar-IMU system to sensor extrinsic parameter calibration problems.

[0016] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0017] This invention provides a method for calibrating the pose extrinsic parameters of a millimeter-wave radar-IMU, the specific steps of which are as follows:

[0018] Step 1: Collect multiple sets of IMU and radar point cloud data on the grid board, and accumulate the point cloud data from multiple frames before and after.

[0019] Step 2: For point clouds with unchanged IMU attitude or position, perform point cloud registration relative to the origin, calculate the radar relative pose, and simultaneously acquire the corresponding IMU data to calculate the IMU relative pose.

[0020] Step 3: Based on the degenerate hand-eye calibration algorithm, the least squares loss function is constructed sequentially for the two types of relative poses to optimize and obtain the initial values ​​of the external parameters of rotation and translation;

[0021] Step 4: Project the point cloud of IMU pose change to multiple centers. The center point cloud is characterized by a four-dimensional normal distribution. Construct a reprojection error loss function for the points and distribution features, and optimize it to obtain the final extrinsic parameter values.

[0022] As a further improvement to the present invention, step 1 is specifically as follows:

[0023] All intersection points of straight lines on the grid plate are taken as optional target locations. The grid size is uniform and known. The IMU on the aircraft is placed horizontally and the coordinate system is marked. The millimeter-wave radar is placed horizontally. Select an optional target location as the origin of the world coordinate system. By visual observation, manually align the IMU coordinate system with the two perpendicular lines at the origin. The state of the millimeter-wave radar and IMU in this state is called the origin state, and the data is called the origin data.

[0024] Recording the origin data involves keeping the IMU in its origin position, holding it still for a period of time, and recording the time points during this period. In addition to the origin data, three sets of data are recorded in different ways. The first set involves keeping the IMU attitude constant, placing the IMU in the remaining selectable positions around the IMU coordinate system, holding it still for a period of time, and recording the time points during this period. The second set involves keeping the IMU position constant, rotating the IMU multiple times around the z-axis perpendicular to the ground in the IMU coordinate system from the origin, holding it still for a period of time each time, and recording the time points during this period. The third set of data involves changing the IMU attitude, placing the IMU in the remaining selectable positions around the IMU coordinate system, holding it still for a period of time, and recording the time points during this period.

[0025] After each settling time, a data window is opened, and the data in the window is all settling data. The millimeter-wave radar point cloud data of each frame in the window is accumulated, and the IMU attitude data in the window is averaged. The accumulated point cloud data and the average IMU attitude are used as the acquired data and written to a file for saving according to the original group.

[0026] As a further improvement to the present invention, step 2 is specifically as follows:

[0027] The radar relative pose is obtained through point cloud registration: the target point cloud is divided into three-dimensional radial grids, and the mean value of the point cloud features within each grid is calculated as the grid feature v.

[0028]

[0029] in Let x be the mean of the point cloud features, n be the number of points in the raster, and x be the mean of the point cloud features. i y i and z i For the position information of the point cloud in the coordinate system, rcs i The radar cross-section value of the point cloud is obtained directly from the data output by the millimeter-wave radar;

[0030] Construct a kd-tree based on the location information of all raster features. Randomly select an unclassified raster G, create a new category set C, and merge G into C. Create a neighbor queue Q, and use the kd-tree to find all unclassified neighbor rasters within a certain range and insert them into Q. Pop all elements in Q and compare them with raster G to determine if they meet the merging criteria. If they do, merge them into C. The merging criteria are as follows:

[0031]

[0032] Where v is the raster feature in Q, v g Let G be the raster feature, Σ be the covariance matrix of all raster features, and d be the Mahalanobis distance. When the distance is less than a set threshold λ, the features are merged.

[0033] Repeatedly select unclassified grid cells G and perform subsequent steps until all grid cells are classified.

[0034] Based on the mapping relationship between the raster and the point cloud it contains, the point cloud is divided into category set P according to category set C. Assuming that the feature x = [x, y, z, rcs] of each category in P conforms to a four-dimensional normal distribution, the mean and covariance matrix of each category's feature are calculated and stored in the distribution set D. To eliminate singular covariance matrices, shrinkage covariance estimation is performed on the obtained covariance matrix of each category.

[0035]

[0036]

[0037]

[0038] Where Tr is a function of the trace of the matrix. Let p be the covariance matrix of the original point cloud, p be the feature dimension of the point cloud, and I be the identity matrix. Let be the shrinkage target matrix, i.e., the identity matrix scaled proportionally; Σ is the point cloud covariance matrix distributed according to a Gaussian pattern, with values ​​of and . The same applies, where n is the number of point clouds, and the calculation yields... for and Weight values ​​between; based on weights The estimated non-singular covariance matrix is ​​calculated.

[0039] Radar relative pose includes rotation R radar With translation t radar Merge into a homogeneous transformation T radar :

[0040]

[0041] Press T on the current point cloud radar Projecting the data onto the target point cloud coordinate system, calculating the distance from each projected point to all distributions in D, and iteratively optimizing T using the nonlinear least squares method. radar Minimize these distances:

[0042]

[0043] Where n is the current number of point clouds, m is the number of distributions in D, and x i For the current point cloud features, μ j and ∑ j Let be the mean and covariance matrices of the distribution in D, respectively;

[0044] IMU relative pose includes rotation R imu With translation t imu R imu t is calculated directly from the IMU output data. imu It can also be directly calculated from the location of the grid plate where the IMU is located.

[0045] As a further improvement to the present invention, step 3 is specifically as follows:

[0046] The traditional hand-eye calibration equation is:

[0047] AX = XB

[0048] Under radar-IMU calibration conditions, the equation becomes:

[0049]

[0050] in For pose transformation between IMUs, For the pose transformation of the radar to the IMU, For pose transformation between radars, the rotation and displacement in the pose transformation are written out separately, and the original expression is expanded into a system of equations:

[0051]

[0052] If the rotational component remains unchanged during relative motion, then the hand-eye calibration equation will degenerate into:

[0053]

[0054] Similarly, if the translational component remains unchanged during relative motion, then the translational component of the hand-eye calibration equation will degenerate into:

[0055]

[0056] Based on the degenerate hand-eye calibration equation and the relative poses of the two types of IMUs and radar, a nonlinear least squares loss function is constructed sequentially and iteratively optimized. and Minimize the function:

[0057]

[0058]

[0059] Where k is the number of relative poses, Let be the relative displacement between the q-th IMU coordinate system and the origin IMU coordinate system. Let be the relative displacement between the q-th radar coordinate system and the origin radar coordinate system. The relative rotation between the q-th IMU coordinate system and the origin IMU coordinate system is calculated; the initial values ​​of the extrinsic rotation parameters are obtained through optimization. Substitute into another formula to obtain the initial values ​​of the translational extrinsic parameters Merge into homogeneous transformation That is, the initial values ​​of the radar-IMU pose extrinsic parameters.

[0060] As a further improvement to the present invention, step 4 is specifically as follows:

[0061] Read the third set of data written, calculate the distribution characteristics of all point clouds, i.e., the mean and covariance matrix of the four-dimensional normal distribution, and perform the calculation process as in step 2; the IMU coordinate system corresponding to all point clouds is the center of each group of projections, and the pose of each IMU coordinate system relative to the origin is obtained directly through the IMU output and the position of the grid board; each group of projections selects an IMU coordinate system as the origin coordinate system, and projects all point clouds except the origin point cloud from the radar coordinate system to the IMU coordinate system according to the radar-IMU attitude extrinsic parameters, and then projects them to the origin IMU coordinate system according to the IMU relative pose. At the same time, the distribution characteristics of the origin point cloud are projected from the radar coordinate system to the origin IMU coordinate system according to the radar-IMU attitude extrinsic parameters. Calculate the reprojection error from the projected point cloud features to the distribution features. The formula for calculating the reprojection error of a certain group of projections is:

[0062]

[0063]

[0064] Where n is the current number of point clouds, m is the current number of distributions, p represents the IMU coordinate system number, q represents the origin IMU coordinate system number, 1≤p, q≤k, and k represents the number of IMU coordinate systems; For the transformation from the p coordinate system to the origin IMU coordinate system, x i p Let μ be the i-th point cloud feature in the point cloud corresponding to the p coordinate system. j q Let ∑ be the mean value of the j-th point cloud distribution feature corresponding to the q coordinate system. j q Let be the covariance matrix of the j-th point cloud distribution feature corresponding to the q coordinate system; res is the residual between the projected point cloud feature and the mean of the distribution feature; error is... q The reprojection error from the point cloud features to the distribution features after the qth group of projections; The redefined point cloud feature vector space transformation operator specifically rotates and translates the position data in the point cloud feature vector, and then concatenates it with the original RCS value to form the transformed point cloud feature vector. The redefined eigencovariance matrix space transformation operator specifically performs eigenvalue decomposition on the covariance matrix:

[0065] ∑=UΛU T

[0066] Perform homogeneous transformation T on each column of eigenvectors in U. The operation is performed to obtain a new U′, and then U′ΛU′ is calculated. T As the transformed feature covariance matrix, eigenvalue decomposition is performed on all feature covariance matrices before nonlinear optimization.

[0067] Accumulate all reprojection errors to construct a nonlinear least squares loss function, and substitute the initial values ​​of the pose extrinsic parameters. As the starting point for optimization, iteratively optimize the pose extrinsic parameters. Minimize reprojection error:

[0068]

[0069] in The final optimized millimeter-wave radar-IMU pose extrinsic parameters.

[0070] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0071] This invention collects millimeter-wave radar and IMU data on a grid board. The relative pose of the IMU can be obtained directly by reading and calculation, avoiding the problems of difficulty in obtaining the relative pose or the carrier not being able to rotate sufficiently when the IMU odometry is missing. At the same time, it accumulates millimeter-wave radar point clouds for multiple frames, improving point cloud density and stability.

[0072] This invention employs adaptive voxel NDT point cloud registration to cluster point clouds into 3D radial grids. It incorporates millimeter-wave radar point cloud RCS features and introduces Mahalanobis distance and kd-tree-based nearest neighbor search, improving the clustering effect and speed of sparse point clouds. Shrinkage estimation of the covariance matrix of the clustered point cloud features removes matrix singularities, overcoming the difficulty of selecting distribution screening thresholds and increasing the success rate of point cloud registration.

[0073] This invention uses a degenerate hand-eye calibration algorithm to sequentially optimize and obtain the initial values ​​of the extrinsic parameters rotation and displacement, avoiding the drift phenomenon of subsequent full-parameter optimization. Starting from the initial values, the multi-center reprojection error is optimized to obtain the final extrinsic parameter values, reducing the influence of manual placement errors and improving calibration accuracy. Attached Figure Description

[0074] Figure 1 Flowchart of millimeter-wave radar-IMU pose extrinsic parameter calibration method;

[0075] Figure 2 This is a schematic diagram of the grid board data acquisition strategy;

[0076] Figure 3 This is a schematic diagram of three-dimensional radial grid division. Detailed Implementation

[0077] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0078] like Figure 1 As shown, a method for calibrating the pose extrinsic parameters of a millimeter-wave radar-IMU includes the following steps:

[0079] (1) As Figure 2 As shown, the IMU on the aircraft is placed horizontally with its coordinate system marked, and the millimeter-wave radar is also placed horizontally. All intersections of straight lines on the grid plate are considered as selectable target locations. The grid size is uniform and known; the grid plate size and selectable locations are not limited to those shown in the figure. A selectable target location is chosen as the origin of the world coordinate system. By visual observation, the IMU coordinate system is manually aligned with the two perpendicular lines at the origin. The state of the millimeter-wave radar and IMU in this state is called the origin state, and the data is called the origin data.

[0080] Recording the origin data involves keeping the IMU in its origin position, holding it still for a period of time, and recording the time points during this period. In addition to the origin data, three sets of data are recorded in different ways. The first set involves keeping the IMU attitude constant, placing the IMU in the remaining selectable positions around the IMU coordinate system, holding it still for a period of time, and recording the time points during this period. The second set involves keeping the IMU position constant, rotating the IMU multiple times around the z-axis perpendicular to the ground in the IMU coordinate system from the origin, holding it still for a period of time each time, and recording the time points during this period. The third set of data involves changing the IMU attitude, placing the IMU in the remaining selectable positions around the IMU coordinate system, holding it still for a period of time, and recording the time points during this period.

[0081] After each settling time, a data window is opened, and the data in the window is all settling data. The millimeter-wave radar point cloud data of each frame in the window is accumulated, and the IMU attitude data in the window is averaged. The accumulated point cloud data and the average IMU attitude are used as the acquired data and written to a file for saving according to the original group.

[0082] (2) For point clouds with unchanged IMU attitude or position, perform point cloud registration relative to the origin: divide the target point cloud into a 3D radial grid map, as shown in the diagram. Figure 3 As shown, the left sub-image represents the horizontal division, and the right sub-image represents the vertical division; each grid is divided based on three parameters. r is the length of the grid generatrix, and θ is the angle between the grid generatrix and the center in the horizontal direction. It is the angle between the grid busbar and the center in the vertical direction.

[0083] Calculate the mean value of the point cloud features within each grid cell as the grid feature v:

[0084]

[0085] in Let x be the mean of the point cloud features, n be the number of points in the raster, and x be the mean of the point cloud features. i ,y i and z i For the position information of the point cloud in the coordinate system, rcs i The radar cross-section value of the point cloud can be obtained directly from the data output by the millimeter-wave radar.

[0086] Construct a kd-tree based on the location information of all raster features. Randomly select an unclassified raster G, create a new category set C, and merge G into C. Create a neighbor queue Q, and use the kd-tree to find all unclassified neighbor rasters within a certain range and insert them into Q. Pop all elements in Q and compare them with raster G to determine if they meet the merging criteria. If they do, merge them into C. The merging criteria are as follows:

[0087]

[0088] Where v is the raster feature in Q, v g Let G be the raster feature, ∑ be the covariance matrix of all raster features, and d be the Mahalanobis distance. When the distance is less than a set threshold λ, they can be merged.

[0089] Repeatedly select unclassified grid cells G and perform subsequent steps until all grid cells are classified.

[0090] Based on the mapping relationship between the raster and the point clouds it contains, the point clouds are divided into category set P according to category set C. Assuming that the feature x = [x, y, z, rcs] of each category in P conforms to a four-dimensional normal distribution, the mean and covariance matrix of each category's feature are calculated and stored in the distribution set D. To eliminate singular covariance matrices, shrinkage covariance estimation is performed on the obtained covariance matrix of each category using existing techniques.

[0091]

[0092]

[0093]

[0094] Where Tr is a function of the trace of the matrix. Let p be the covariance matrix of the original point cloud, p be the feature dimension of the point cloud, and I be the identity matrix. Let be the shrinkage target matrix, i.e., the identity matrix scaled proportionally; Σ is the point cloud covariance matrix distributed according to a Gaussian pattern, with values ​​of and . The same applies, where n is the number of point clouds, and the calculation yields... for and Weight values ​​between; based on weights The estimated non-singular covariance matrix is ​​calculated.

[0095] Radar relative pose includes rotation R radar With translation t radar Merge into a homogeneous transformation T radar :

[0096]

[0097] Press T on the current point cloud radar Projecting the data onto the target point cloud coordinate system, calculating the distance from each projected point to all distributions in D, and iteratively optimizing T using the nonlinear least squares method. radar Minimize these distances:

[0098]

[0099] Where n is the current number of point clouds, m is the number of distributions in D, and x i For the current point cloud features, μ j and ∑ j Let be the mean and covariance matrix of the distribution in D, respectively.

[0100] IMU relative pose includes rotation R imu With translation t imu R imu It can be directly calculated from the IMU output data, t imu It can also be directly calculated from the location of the grid plate where the IMU is located.

[0101] (3) Based on the traditional hand-eye calibration equation:

[0102] AX = XB

[0103] Applying this equation to the conditions of radar-IMU calibration, the equation becomes:

[0104]

[0105] in For pose transformation between IMUs, For the pose transformation of the radar to the IMU, For pose transformation between radars, the rotation and displacement in the pose transformation are written out separately, and the original expression is expanded into a system of equations:

[0106]

[0107] If the rotational component remains unchanged during relative motion, then the hand-eye calibration equation will degenerate into:

[0108]

[0109] Similarly, if the translational component remains unchanged during relative motion, then the translational component of the hand-eye calibration equation will degenerate into:

[0110]

[0111] Based on the degenerate hand-eye calibration equation and the two types of relative poses obtained in (2), a nonlinear least squares loss function is constructed sequentially and iteratively optimized. and Minimize the function:

[0112]

[0113]

[0114] Where k is the number of relative poses, Let be the relative displacement between the q-th IMU coordinate system and the origin IMU coordinate system. Let be the relative displacement between the q-th radar coordinate system and the origin radar coordinate system. The relative rotation between the q-th IMU coordinate system and the origin IMU coordinate system is calculated; the initial values ​​of the extrinsic rotation parameters are obtained through optimization. Substitute into another formula to obtain the initial values ​​of the translational extrinsic parameters Merge into homogeneous transformation That is, the initial values ​​of the radar-IMU pose extrinsic parameters.

[0115] (4) Read the third set of data written in (1), calculate the distribution characteristics of all point clouds, i.e., the mean and covariance matrix of the four-dimensional normal distribution. The calculation process can be performed in (2); project the point cloud with IMU pose change to multiple centers. The IMU coordinate system corresponding to all point clouds is the center of each group of projections. The pose of each IMU coordinate system relative to the origin can be directly obtained through the IMU output and the position of the grid plate; select an IMU coordinate system as the origin coordinate system for each group of projections. First, project all point clouds except the origin point cloud from the radar coordinate system to the IMU coordinate system according to the radar-IMU attitude extrinsic parameters, and then project them to the origin IMU coordinate system according to the IMU relative pose. At the same time, project the distribution characteristics of the origin point cloud from the radar coordinate system to the origin IMU coordinate system according to the radar-IMU attitude extrinsic parameters. Calculate the reprojection error of the point cloud features to the distribution characteristics after projection. The formula for calculating the reprojection error of a certain group of projections is:

[0116]

[0117]

[0118] Where n is the current number of point clouds, m is the current number of distributions, p represents the IMU coordinate system number, q represents the origin IMU coordinate system number, 1≤p, q≤k, and k represents the number of IMU coordinate systems; For the transformation from the p coordinate system to the origin IMU coordinate system, x i p Let μ be the i-th point cloud feature in the point cloud corresponding to the p coordinate system. j q Let ∑ be the mean value of the j-th point cloud distribution feature corresponding to the q coordinate system. j q Let be the covariance matrix of the j-th point cloud distribution feature corresponding to the q coordinate system; res is the residual between the projected point cloud feature and the mean of the distribution feature; error is... q The reprojection error from the point cloud features to the distribution features after the qth group of projections; The redefined point cloud feature vector space transformation operator specifically rotates and translates the position data in the point cloud feature vector, and then concatenates it with the original RCS value to form the transformed point cloud feature vector. The redefined eigencovariance matrix space transformation operator specifically performs eigenvalue decomposition on the covariance matrix:

[0119] ∑=UΛU T

[0120] Perform homogeneous transformation T on each column of eigenvectors in U. The operation yields a new U′, and U′ΛU′T is calculated as the transformed eigencovariance matrix. To reduce computational cost, eigenvalue decomposition can be performed on all eigencovariance matrices before nonlinear optimization.

[0121] Accumulate all reprojection errors to construct a nonlinear least squares loss function, and substitute the initial values ​​of the pose extrinsic parameters. As the starting point for optimization, iteratively optimize the pose extrinsic parameters. Minimize reprojection error:

[0122]

[0123] in The final optimized millimeter-wave radar-IMU pose extrinsic parameters.

[0124] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for calibrating the pose of a millimeter wave radar-IMU, the specific steps being as follows, characterized in that: Step 1: Collect multiple sets of IMU and radar point cloud data on a grid plate, and accumulate multiple frames of point clouds before and after; Step 2: For point clouds with unchanged IMU attitude or position, perform point cloud registration relative to the origin, calculate the relative pose of the radar, and obtain the corresponding IMU data to calculate the relative pose of the IMU; Step 3: Based on the degenerated hand-eye calibration algorithm, construct a least squares loss function for the two types of relative poses in turn, and optimize the initial values of the rotation and translation parameters; Step 4: Project the point cloud with changing IMU pose to multiple centers, and construct a re-projection error loss function for the points and distribution characteristics, and optimize the final extrinsic parameter values.

2. The method of claim 1, wherein: The step 1 is specifically as follows: All straight line intersection points on the grid plate are selected as optional target positions, the grid size is uniform and known, the IMU on the machine body is placed horizontally and the coordinate system is marked, and the millimeter wave radar is placed horizontally; select a certain optional target position as the origin of the world coordinate system, manually align the IMU coordinate system with the two vertical lines at the origin by visual observation, and the state of the millimeter wave radar and the IMU in this state is referred to as the origin state, and the data is referred to as the origin data; The recording of the origin data is to keep the machine body in the origin state, and record the time points after static placement for a period of time; in addition to the origin data, three sets of data are recorded in different ways, the first set of data is to keep the IMU attitude unchanged, and the machine body is placed in the remaining optional positions with the IMU coordinate system as the center, and the time points after static placement for a period of time are recorded; the second set of data is to keep the IMU position unchanged, and the machine body is rotated multiple times around the z-axis perpendicular to the ground in the IMU coordinate system at the origin, and the time points after static placement for a period of time are recorded; the third set of data is to change the attitude of the IMU, and then place the machine body in the remaining optional positions with the IMU coordinate system as the center, and the time points after static placement for a period of time are recorded; After each static placement time, a data window is opened, and the data in the window is static data; accumulate each frame of millimeter wave radar point cloud data in the window, and take the average of the IMU attitude data in the window, and the accumulated point cloud data and the average of the IMU attitude are written into the file as the collected data.

3. The method of claim 1, wherein: The step 2 is specifically as follows: The radar relative pose is obtained by point cloud registration: the target point cloud is divided into three-dimensional radial grids, and the average value of the point cloud features in each grid is calculated as the grid feature : ； wherein is the mean value of the point cloud features, is the number of points in the grid, and is the position information of the point cloud in the coordinate system, is the radar cross section value of the point cloud, which is directly obtained from the data output by the millimeter wave radar;​ A kd-tree is constructed according to the position information of all grid features, a grid G that has not been classified is randomly selected, a new class set C is created, and G is merged into C; a neighbor queue Q is created, and all neighbor grids within the range that have not been classified are found through the kd-tree and inserted into Q; all elements in Q are popped out in turn and compared with grid G to determine whether the merging condition is met, and if the condition is met, G is merged into C; the merging condition is as follows: ； wherein is the grid feature in Q, is the grid G feature, is the covariance matrix of all grid features, is the Mahalanobis distance, when the distance is less than a set threshold then merge; Repeat the selection of the grid G that has not been classified, and execute the subsequent steps until all grids are classified, and then the process is ended; According to the mapping relationship between the grid and the point cloud contained therein, the point cloud is divided into the category set P according to the category set C. It is assumed that the characteristics of each category of point cloud in P conform to the four-dimensional normal distribution. The mean and covariance matrix of each category of point cloud characteristics are calculated and stored in the distribution set D. In order to eliminate singular covariance matrix, shrinkage covariance estimation is performed on each category of point cloud covariance matrix obtained. ； ； ； wherein is a function of the matrix trace, is the original point cloud covariance matrix, is the point cloud feature dimension, is the identity matrix, is the shrinkage target matrix, i.e., the scaled identity matrix; is the point cloud covariance matrix according to the Gaussian distribution, taking values and are the same, is the number of point clouds, calculated as is and is the weight value between and ; according to the weight , the estimated non-singular covariance matrix is calculated as ; The radar relative pose comprises a rotation and a translation , combined into a homogeneous transformation : ； Press the current point cloud Project the points onto the target point cloud coordinate system, calculate the distance from each projected point to all distributions in D, and iteratively optimize using the nonlinear least squares method. Minimize these distances: ； wherein is the current point cloud number, is the number of distributions in D, is the current point cloud feature, and are the mean and covariance matrix of the distribution in D, respectively; The relative pose of the IMU contains a rotation and a translation , The rotation is directly calculated from the output data of the IMU, The translation can also be directly calculated from the position of the IMU on the grid plate.

4. The method of claim 1, wherein: The step 3 is specifically as follows: The traditional hand-eye calibration equation is: ； Under the condition of radar-IMU calibration, the equation becomes: ； wherein is the pose transformation between IMUs, is the pose transformation from radar to IMU, is the pose transformation between radars, writing the rotation and displacement separately in the pose transformation, the original equation is expanded into a system of equations: ； If the rotation part does not change in relative motion, the hand-eye calibration equation will degenerate into: ； Likewise, if the translation part does not change in the relative motion, the translation part of the hand-eye calibration equation will degenerate to: ； According to the degenerated hand-eye calibration equation, the relative poses of the two types of IMUs and the relative pose of the radar, nonlinear least square loss functions are sequentially constructed, and iterative optimization is performed and minimizing the function: ； ； wherein is the number of relative poses, is the relative displacement of the th IMU coordinate system to the origin IMU coordinate system, is the relative displacement of the th radar coordinate system to the origin radar coordinate system, is the relative rotation of the th IMU coordinate system to the origin IMU coordinate system; the rotation initial value of the external parameter is obtained by optimization The descendant is put into another form to obtain the translation external parameter initial value , which is combined into a homogeneous transformation , that is, the radar-IMU pose external parameter initial value.

5. The method of claim 1, wherein: The step 4 is specifically as follows: The third group of data read and written is calculated, and the distribution characteristics of all point clouds, i.e., the mean and covariance matrix of the four-dimensional normal distribution, are calculated. The calculation process is performed according to step 2. The IMU coordinate system corresponding to all point clouds is the center of each projection. The pose of each IMU coordinate system relative to the origin is directly obtained through the IMU output and the grid plate position. An IMU coordinate system is selected as the origin coordinate system for each projection. All point clouds except the origin point cloud are first projected from the radar coordinate system to the IMU coordinate system according to the radar-IMU attitude external parameter, and then projected to the origin IMU coordinate system according to the relative pose of the IMU. At the same time, the distribution characteristics of the origin point cloud are projected from the radar coordinate system to the origin IMU coordinate system according to the radar-IMU attitude external parameter. The re-projection error of the projected point cloud characteristics to the distribution characteristics is calculated. The re-projection error calculation formula of a certain group of projections is: ； ； wherein is the current point cloud number, is the current distribution number, denotes the IMU coordinate system number, denotes the origin IMU coordinate system number, , denotes the number of IMU coordinate systems; is the transformation of the coordinate system to the origin IMU coordinate system, is the first point cloud feature in the point cloud corresponding to the coordinate system, is the first point cloud distribution feature mean corresponding to the coordinate system, is the first point cloud distribution feature covariance matrix corresponding to the coordinate system; is the residual of the projected point cloud feature and the distribution feature mean, is the re-projection error of the first group of projected point cloud features to the distribution features; is the redefined point cloud feature vector space transformation operator, which specifically rotates and translates the position data in the point cloud feature vector and splices it with the original RCS value to become the transformed point cloud feature vector; is the redefined feature covariance matrix space transformation operator, which specifically performs eigenvalue decomposition on the covariance matrix: ； will be described below. each column of eigenvectors with the homogeneous transformation are performed operations, obtaining new , calculating as the transformed feature covariance matrix, performing eigen decomposition on all feature covariance matrices before the non-linear optimization; All the reprojection errors are accumulated to construct a nonlinear least squares loss function, which is substituted into the initial value of the pose parameter The pose parameter is iteratively optimized as the starting position of optimization Minimize the reprojection error: ； wherein is the final optimized millimeter wave radar-IMU pose parameter.

Images