VEHICLE 4D MILLIMETER-WAVE RADAR INERTIAL ODOMETRY METHOD AND COMPUTER-READABLE MEDIUM

Abstract

Provided is a vehicle 4D millimeter-wave radar inertial odometry method and a computer-readable medium. The method includes: eliminating significant radar spatial noise through relaxation filtering; performing single-frame velocity measurement based on a selecting weight iteration-based robust estimation method, and eliminating dynamic noise; performing IMU observation integration by mechanical arrangement to obtain a predicted vehicle state; constructing velocity constraints based on single-frame velocity measurement and a predicted velocity to initially update the vehicle state; constructing a local map based on the previous point cloud frame, searching n-nearest neighbor points of each radar point in the local map at next moment, calculating a weighted distance from distribution to multi-distribution as matching constraints from point cloud to local map, and performing further updating based on iterative filtering to obtain a precise vehicle state; and updating the vehicle state at all moments by 4Dradar closed-loop detection and GICP to reduce the positioning drift.

Description

TECHNICAL FIELD

The present disclosure belongs to the technical field of vehicle state estimation, and particularly relates to a vehicle 4D millimeter-wave radar inertial odometry method and a computer-readable medium.

BACKGROUND

In recent years, autonomous driving and autonomous robots have developed rapidly. Positioning and navigation technology based on perceptual sensor data has always been a hot research direction in this field. At present, most self-driving cars use LiDAR and optical cameras as main sensors. These sensors can maintain good performance in good weather conditions, but have difficulties in working in bad weather conditions such as rain, snow and fog. The millimeter-wave radar has a longer wavelength, a larger FOV, has strong resistance to small particles such as dust, fog, rain and snow, can work better in the case that the visual environment deteriorates, and has a strong penetration ability to some materials. At the same time, the millimeter-wave radar can provide Doppler velocity observation, and can estimate the robot's ego-velocity through single frame data.

Compared with 3D millimeter-wave radar, the 4D imaging millimeter-wave radar greatly improves resolution in the vertical direction, increases the detection and analysis of target height, and can realize the information perception in four dimensions of distance, azimuth, height and velocity, and can better detect the outline of obstacles, surrounding environmental information, as well as better path planning and passable space detection functions. However, the 4D millimeter-wave radar has low spatial resolution and large sparsity of point clouds, and is affected by noise factors such as multipath effects and harmonics, thus it is difficult to achieve stable, continuous, high-precision data correlation. Therefore, high-precision three-dimensional spatial localization and mapping by millimeter-wave radar remains a challenging task.

SUMMARY

Aiming at the shortcomings of methods in the prior art, the present disclosure provides a vehicle 4D millimeter-wave radar inertial odometry method and a computer-readable medium.

The technical solution of the method of the present disclosure is a vehicle 4D millimeter-wave radar inertial odometry method, which specifically includes the following steps:

• • Step 1: acquiring a sequence of 4D radar observation data at a plurality of radar sampling moments according to a certain frequency, and performing significant multipath scattering and speckle noise point removal screening on a three-dimensional point cloud at each radar sampling moment through a spatial distance and a relaxation filtering method based on the local point cloud statistical characteristics to obtain a three-dimensional point cloud after relaxation filtering at each moment; and estimating a vehicle velocity in the radar local coordinate system at each moment by the three-dimensional point cloud after relaxation filtering at each moment and the Doppler velocity corresponding to each point in the point cloud through a selecting weight iteration-based minimum 2-norm method, and eliminating dynamic interference points still existing in the point cloud after relaxation filtering to obtain radar-preprocessed data at each moment;
  • Step 2: performing time integration on inertial navigation data of IMU observation subsequences synchronously acquired between adjacent radar sampling moments through a mechanical arrangement algorithm to obtain a predicted state at a next radar sampling moment;
  • Step 3: constructing a velocity error equation by the vehicle velocity in the radar local coordinate system estimated through the selecting weight iteration-based minimum 2-norm method in step 1 and a predicted vehicle velocity in the global coordinate system at the next radar sampling moment in step 2, obtaining a residual initial value at the next radar sampling moment and a Jacobian matrix at the next radar sampling moment through a linearized error equation, and preliminarily updating a vehicle state by using Kalman filtering to obtain a velocity-updated vehicle state at the next radar sampling moment;
  • Step 4: constructing a local map by three-dimensional point clouds of the radar-preprocessed data in the past multiple radar sampling moments through an ikdtree tree strategy, and obtaining n-nearest neighbor points, in the local map, of each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next radar sampling moment through K-Nearest Neighbor (KNN) search in the local map; weighting the average Euclidean distance from each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next sampling moment to the corresponding n-nearest neighbor points by the local point cloud spatial distribution covariance method to obtain the average weighted distance from distribution to multi-distribution, forming spatial point matching constraints from point cloud to local map, so as to construct a matching error equation and a linearized matching error equation, obtaining an initial value of a matching residual at the next radar sampling moment and a matching residual Jacobian matrix at the next radar sampling moment through the linearized matching error equation, constructing a maximum a posteriori estimation problem by considering matching constraints corresponding to all 4D radar-preprocessed data, and performing further updating based on iterative Kalman filtering to obtain a vehicle state with high precision;
  • Step 5: describing and retrieving a frame, that is most similar to a point cloud frame at the current moment, among historical radar point cloud frames by the ScanContext feature; judging whether the similarity is greater than an established threshold, if the similarity is less than the threshold, discarding the closed loop, otherwise accepting the closed loop, calculating a three-dimensional projection transformation relationship between two point cloud frames using a GICP algorithm, and updating the vehicle state at all moments before by this spatial transformation relationship as closed-loop constraints.

Preferably, 4D radar observation data at each radar sampling moment in step 1 consists of a three-dimensional point cloud and the Doppler velocity of each three-dimensional observation point in the three-dimensional point cloud;

the 4D radar observation data at each radar sampling moment in step 1 is defined as:

data_i={{ρ_i,j,v_i,j^d},j=1 . . . N}

i∈[1,N_t]

ρ_i,j=(x_i,j,y_i,j,z_i,j)

• • wherein, data_i represents the 4D radar observation data at the ith radar sampling moment, ρ_i,j represents the three-dimensional coordinates of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, x_i,j represents the X-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, y_i,j represents the Y-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, z_i,j represents the Z-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, v_i,j^d represents the Doppler velocity observation of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, N represents the number of three-dimensional observation points in the three-dimensional point cloud of the 4D radar observation data at each radar sampling moment, and N_t represents the number of radar sampling moments;

The selecting weight iteration-based minimum 2-norm method in step 1 is specifically as follows:

• • a robust estimation problem at each radar sampling moment is constructed, which is specifically as follows:

$\underset{{ }^R{v}_{i_m}}{\min }{\sum }_{j=1}^N{\lambda }_{i,j}v_{i,j}^{{ }_{}d}-\frac{{\rho }_{i,j}^{{ }_{}T}}{{\rho }_{i,j}}·{ }^R{v}_{i_m}$

• • wherein λ_i,j is the weight of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, v_i,j^d represents the Doppler velocity observation of the jth observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, ρ_i,j is the three-dimensional coordinates of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment,

${ }^R{v}_{i_m}$

is the vehicle velocity in the radar local coordinate system at the ith radar sampling moment, and N represents the number of three-dimensional observation points in the three-dimensional point cloud of 4D radar observation data at each radar sampling moment;

• • in the first iteration, λ_i,j=1, and in subsequent iterations,

${\lambda }_{i,j}=1/\left(v_{i,j}^{{ }_{}d}-\frac{{\rho }_{i,j}^{{ }_{}T}}{{\rho }_{i,j}}·{ }^R{v}_{i_m}+ϵ\right),$

wherein ∈ represents the iteration coefficient;

• • summation is performed through the selecting weight iteration-based minimum 2-norm method to obtain the vehicle velocity

${ }^R{v}_{i_m}$

in the radar local coordinate system at the ith radar sampling moment and a covariance matrix at the ith radar sampling moment, and an inverse value of the covariance matrix at the ith radar sampling moment is taken as an observed covariance R_ivel of the vehicle velocity in the radar local coordinate system at the ith radar sampling moment;

• • eliminating the dynamic interference points still existing in the point cloud after relaxation filtering to obtain radar-preprocessed data at each moment in step 1 specifically includes:
  • setting a weight threshold λ_th;
  • if λ_i,j<λ_th, taking three-dimensional observation points corresponding to ρ_i,j as dynamic noise points, otherwise taking the three-dimensional observation points as static inline points;
  • obtaining a set of static inline observation points {ρ_i,j, v_i,j^d}, that is, the radar-preprocessed data after the dynamic noise points are eliminated;

Preferably, the IMU observation subsequence synchronously acquired between adjacent radar sampling moments in step 2 is defined as:

$S_i=\left({\mu }_i^1,{\mu }_i^2,...{\mu }_i^k...,{\mu }_i^{N_{i_I}}\right),{\mu }_i^k=\left(a_i^k,{\omega }_i^k\right)$ $i\in \left[1,N_t\right]$

wherein S_i represents the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, μ_i^k represents the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, a_i^k represents the acceleration in the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, ω_i^k is the angular velocity in the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, N_iI represents the number of observations in the IMU subsequence, and N_t represents the number of radar sampling moments;

• • the predicted state at the next radar sampling moment in step 2 is defined as:

${\hat{x}}_{i+1}=\left[{ }^G{\hat{p}}_{I_{i+1}},{ }^G{\hat{v}}_{I_{i+1}},{ }^G{\hat{R}}_{I_{i+1}}, {\hat{b}}_{a_{i+1}}, {\hat{b}}_{{\omega }_{i+1}},{ }^I{\hat{R}}_{R_{i+1}},{ }^I{\hat{l}}_{R_{i+1}},{ }^G{\hat{g}}_{i+1}\right]$

• • wherein {circumflex over (x)}_i+1 represents the predicted state at the (i+1)th radar sampling moment,

${ }^G{\hat{p}}_{I_{i+1}}$

represents the position predicted value at the (i+1)th radar sampling moment,

${ }^G{\hat{v}}_{I_{i+1}}$

represents the velocity predicted value at the (i+1)th radar sampling moment, ^G{circumflex over (R)}_Ii+1 represents the attitude predicted value at the (i+1)th radar sampling moment, {circumflex over (b)}_ai+1 represents the acceleration bias predicted value at the (i+1)th radar sampling moment, {circumflex over (b)}_ωi+1 represents the angular velocity bias predicted value at the (i+1)th radar sampling moment, ^I{circumflex over (R)}_Ri+1 represents the predicted value of the extrinsic parameter attitude between IMU and radar at the (i+1)th radar sampling moment, ^I{circumflex over (l)}_Ri+1 represents the predicted value of the extrinsic parameter distance between IMU and radar at the (i+1)th radar sampling moment, and ^Gĝ_i+1 represents the gravity vector predicted value in the global coordinate system at the (i+1)th radar sampling moment;

• • preferably, the velocity error equation in step 3 is defined as:

$r_{i+1_{\mathrm{vel}}}={ }^R{v}_{i+1_m}-{ }^R{v}_{i+1}$ ${ }^R{v}_{i+1}=\left[{ }^I{R}_{R_{i+1}}^{{ }_{}T}\left({ }^G{R}_{I_{i+1}}^{{ }_{}T}{ }^G{v}_{I_{i+1}}+{\left({\omega }_{i+1}-b_{{\omega }_{i+1}}\right)}_{\times }·{ }^I{l}_{R_{i+1}}\right)\right]$

• • wherein r_i+1vel is the velocity residual at the (i+1)th radar sampling moment,

${ }^R{v}_{i+1_m}$

is the vehicle velocity in the radar local coordinate system at the (i+1)th radar sampling moment, ^Rv_i+1 is the velocity true value in the radar local coordinate system at the (i+1)th radar sampling moment, ^GR_Ii+1 represents the attitude at the (i+1)th radar sampling moment, ^IR_Ri+1 represents the rotational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, ^Gv_Ii+1 represents the velocity at the (i+1)th radar sampling moment, ω_i+1 represents the angular velocity measured value at the (i+1)th radar sampling moment, b_ωi+1 represents the angular velocity bias at the (i+1)th radar sampling moment,

${ }^I{l}_{R_{i+1}}$

represents the translational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, (⋅)_x represents the vector cross product, and (⋅)^T represents transposition;

• • the linearized error equation in step 3 is defined as:

$r_{i+1_{\mathrm{vel}}}={\hat{r}}_{i+1_{\mathrm{vel}}}+H_{i+1_{\mathrm{vel}}}{\tilde{x}}_{i+1}$ ${\hat{r}}_{i+1_{\mathrm{vel}}}={ }^R{\hat{v}}_{i+1}-{ }^R{v}_{i+1_m}$

• • wherein r_i+1vel represents the velocity residual at the (i+1)th radar sampling moment, {circumflex over (r)}_i+1vel represents the velocity residual initial value at the (i+1)th radar sampling moment, ^R{circumflex over (v)}_i+1 is the IMU predicted value of the velocity in the radar local coordinate system at the (i+1)th radar sampling moment, H_i+1vel is the Jacobian matrix at the (i+1)th radar sampling moment, and {tilde over (x)}_i+1 is the error state at the (i+1)th radar sampling moment;
  • preferably, the matching error equation in step 4 is defined as

$r_{i+1_{\mathrm{pro}}}=G_{i+1}\left({{\sum }_{\tau =1}^n}^G{b}_{\tau }/n-{ }^G{T}_{R_{i+1}}·{\rho }_{i+1}\right)$ ${ }^G{T}_{R_{i+1}}=\left[\begin{array}{cc}{ }^G{R}_{I_{i+1}}& { }^G{l}_{I_{i+1}}\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{cc}{ }^I{R}_{R_{i+1}}& { }^I{l}_{R_{i+1}}\\ 0_{1\times 3}& 1\end{array}\right]$ $G_{i+1}={\left({\sum }_{\tau =1}^n{C}_{\tau }^B/n+{ }^G{T}_{R_{i+1}}·C^A·{ }^G{T_{R_{i+1}}}^T\right)}^{-\frac{1}{2}}$

• • wherein r_i+1pro represents the point cloud matching residual, ρ_i+1 is the coordinates of the radar observation point in the radar coordinate system at the (i+1)th radar sampling moment, ^Gb_τ is the coordinates of n-nearest neighbor points in the ρ_i+1 local map in the global coordinate system, and is a transformational matrix from the global coordinate system to the radar coordinate system, G_i+1 is the distance weighted matrix from distribution to multi-distribution at the (i+1)th radar sampling moment, ^GT_Ri+1 is the transformation matrix from radar local coordinate system to global coordinate system, ^GR_Ii+1 represents the attitude at the (i+1)th radar sampling moment, ^Gl_Ii+1 represents the position at the (i+1)th radar sampling moment, ^IR_Ri+1 represents the rotational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, ^Il_Ri+1 is the rotational extrinsic parameter 1 between IMU and radar at the (i+1)th radar sampling moment, 0_1×3 represents a 0 vector with one row and three columns, C^A is the spatial distribution covariance corresponding to the current point ρ_i+1, and CP is the spatial distribution covariance corresponding to the nearest neighbor point ^Gb_τ.

The linearized matching error equation in step 4 is defined as:

$r_{i+1_{\mathrm{pro}}}={\hat{r}}_{i+1_{\mathrm{pro}}}+H_{i+1_{\mathrm{pro}}}{\tilde{x}}_{i+1}$

• • wherein {circumflex over (r)}_i+1pro is the initial value of the matching residual at the (i+1)th radar sampling moment, H_i+1pro is the matching Jacobian matrix at the (i+1)th radar sampling moment, and {tilde over (x)}_i+1 is the error state at the (i+1)th radar sampling moment;
  • the maximum a posteriori estimation problem of the matching constraints in step 4 is defined as:

$\underset{{\tilde{x}}^{\kappa }}{\min }\left({\left({\hat{x}}_{i+1}^{\kappa }{\stackrel{\_}{x}}_{i+1_{vel}}\right)+J_{i+1}^{\kappa }{\tilde{x}}_{i+1}^{\kappa }}_{{\overline{P}}_{i+1_{\mathrm{vel}}}^{-1}}^2+{\sum }_{\zeta =1}^{\Pi }{{{ }^{\zeta }{T}_{i+1_{\mathrm{pro}}}^{\kappa }+{ }^{\zeta }{H}_{i+1_{\mathrm{pro}}}^{\kappa }{\tilde{x}}_{i+1}^{\kappa }}^2}_{{ }^{\zeta }{R}_{i+1_{\mathrm{pro}}}^{-1}}\right)$

• • wherein x_i+1vel is the state at the (i+1)th radar sampling moment after the velocity is updated, P_i+1vel is the covariance matrix at the (i+1)th radar sampling moment after the velocity observation is updated, R_i+1pro⁻¹ is the observation noise of the ζth point in the processed data at the (i+1)th radar sampling moment, Π is the number of effective observations in the radar point cloud frame at the (i+1)th radar sampling moment, {circumflex over (x)}_i+1^κ is the state updated value obtained by the κth iterative Kalman filtering at the (i+1)th radar sampling moment, {tilde over (x)}_i+1^κ is the error state corresponding to the κth iterative Kalman filtering at the (i+1)th radar sampling moment, J_i+1^κx_i+1vel to {tilde over (x)}^κ linearized Jacobian matrix, ^ζ{tilde over (r)}_i+1pro^κ represents the initial value of the matching residual corresponding to the ζth radar observation point in the Kth iteration at the (i+1)th radar sampling moment, ^ζH_i+1pro^κ represents the matching Jacobian matrix corresponding to the ζth radar observation point in the κth iteration at the (i+1)th radar sampling moment, is the state quantity subtraction operation, in which the rotational state quantity conforms to the right multiplication inverse disturbance, and the others conform to the vector subtraction operation, and ∥.∥ represents the Euclidean distance.

The iterative Kalman updated system state in step 4 is defined as follows:

${\hat{x}}_{i+1}^{\kappa +1}={\hat{x}}_{i+1}^{\kappa }\left(-K_{i+i_{\mathrm{pro}}}^{\kappa }{r}_{i+i_{\mathrm{pro}}}+\left(I-K_{i+i_{\mathrm{pro}}}^{\kappa }{H}_{i+1_{\mathrm{pro}}}^{\kappa }\right){\left(J^{\kappa }\right)}^{-1}\right)\left({\hat{x}}_{i+1}^{\kappa }{\overline{x}}_{i+1_{\mathrm{vel}}}\right)$ $P_{i+1}^{\kappa +1}=\left(I-K_{i+i_{\mathrm{pro}}}^{\kappa }{H}_{i+1_{\mathrm{pro}}}^{\kappa }\right)P_{i+1}^{\kappa }$

• • wherein: {tilde over (x)}_i+1^κ+1 is the state obtained by the Kth iterative Kalman filtering at the (i+1)th radar sampling moment, K_i+1pro^κ is the matching Kalman gain of the Kth iteration filtering at the (i+1)th radar sampling moment, r_i+1pro represents the point cloud matching residual, I is identity matrix, P_i+1 is the covariance matrix updated by matching at the (i+1)th radar sampling moment, P_i+1 is the covariance matrix updated by the κth iterative filtering, ρ_i+1^κ is the covariance matrix updated by the κ+1th iterative filtering, H_i+1pro^κ is the matching Jacobian matrixin the κth iteration at the (i+1)th radar sampling moment, is a state quantity addition operation, in which the rotational state quantity conforms to the right multiplication disturbance and the others conform to the vector subtraction operation, and is a state quantity subtraction operation.

The present disclosure also provides a computer-readable medium, which stores a computer program executed by an electronic device, and when running on the electronic device, the computer program causes the electronic device to execute the steps of the vehicle 4D millimeter-wave radar inertial odometry method.

The present disclosure has the advantages of innovatively providing a vehicle 3D pose robust estimation method base on 4D radar, weakening the influence of various radar noise on the positioning results, and effectively improving the matching precision and robustness of sparse radar point clouds.

BRIEF DESCRIPTION OF FIGURES

FIG. 1 is a flowchart of a method provided by embodiments of the present disclosure;

FIG. 2 is a comparison diagram of pose recovery effects of three constraint combination modes in the embodiments of the present disclosure; and

FIG. 3 is a comparison diagram of pose recovery results in the embodiments of the present disclosure.

DETAILED DESCRIPTION

The technical solution in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, but not all the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by a person having ordinary skill in the art without creative labor belong to the scope of protection of the present disclosure.

During specific implementation, the method provided in the technical solution of the present disclosure may be automatically run by those skilled in the art using computer software technology, and system devices for realizing the method, such as computer-readable storage media for storing the corresponding computer program in the technical solution of the present disclosure and computer equipment for running the corresponding computer program, should also be within the protection scope of the present disclosure.

The technical solution of the embodiments of the present disclosure, which is a vehicle 4D millimeter-wave radar inertial odometry method, will be introduced with reference to FIGS. 1-3. FIG. 1 is a flowchart of the method of the embodiments of the present disclosure, and the specific steps are as follows:

• • Step 1: acquiring a sequence of 4D radar observation data at a plurality of radar sampling moments according to a certain frequency, and performing significant multipath scattering and speckle noise point removal screening on a three-dimensional point cloud at each radar sampling moment through a spatial distance and a relaxation filtering method based on the local point cloud statistical characteristics to obtain a three-dimensional point cloud after relaxation filtering at each moment; and estimating a vehicle velocity in the radar local coordinate system at each moment by the three-dimensional point cloud after relaxation filtering at each moment and the Doppler velocity corresponding to each point in the point cloud through a selecting weight iteration-based minimum 2-norm method, and eliminating dynamic interference points still existing in the point cloud after relaxation filtering to obtain radar-preprocessed data at each moment;
  • the 4D radar observation data at each radar sampling moment in step 1 consists of a three-dimensional point cloud and the Doppler velocity of each three-dimensional observation point in the three-dimensional point cloud;
  • the 4D radar observation data at each radar sampling moment in step 1 is defined as:

${\mathrm{data}}_i=\left\{\left\{{\rho }_{i,j},v_{i,d}^d\right\},j=1...N\right\}$ $i\in \left[1,N_t\right]$ ${\rho }_{i,j}=\left(x_{i,j},y_{i,j},z_{i,j}\right)$

• • wherein, data_i represents the 4D radar observation data at the ith radar sampling moment, ρ_i,j represents the three-dimensional coordinates of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, x_i,j represents the X-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, y_i,j represents the Y-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, z_i,j represents the Z-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, v_i,j^d represents the Doppler velocity observation of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, N represents the number of three-dimensional observation points in the three-dimensional point cloud of the 4D radar observation data at each radar sampling moment, and N_t represents the number of radar sampling moments;
  • the selecting weight iteration-based minimum 2-norm method in step 1 is specifically as follows:
  • a robust estimation problem at each radar sampling moment is constructed, which is specifically as follows:

$\underset{R_{v_{i_m}}}{\min }{\sum }_{j=1}^N{\lambda }_{i,j}v_{i,j}^d-\frac{{\rho }_{i,j}^T}{{\rho }_{i,j}}·{ }^R{v}_{i_m}$

wherein λ_i,j is the weight of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, v_i,j^d represents the Doppler velocity observation of the jth observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, ρ_i,j is the three-dimensional coordinates of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment,

${ }^R{v}_{i_m}$

is the vehicle velocity in the radar local coordinate system at the ith radar sampling moment, and N represents the number of three-dimensional observation points in the three-dimensional point cloud of 4D radar observation data at each radar sampling moment;

• • in the first iteration, λ_i,j=1, and in subsequent iterations,

${\lambda }_{i,j}=1/\left(v_{i,j}^d-\frac{{\rho }_{i,j}^T}{{\rho }_{i,j}}·{ }^R{v}_{i_m}+\in \right),$

wherein ∈ represents the iteration coefficient;

• • summation is performed through the selecting weight iteration-based minimum 2-norm method to obtain the vehicle velocity

${ }^R{v}_{i_m}$

in the radar local coordinate system at the ith radar sampling moment and a covariance matrix at the ith radar sampling moment, and an inverse value of the covariance matrix at the ith radar sampling moment is taken as an observed covariance R_ivel of the vehicle velocity in the radar local coordinate system at the ith radar sampling moment;

• • eliminating dynamic interference points still existing in the point cloud after relaxation filtering to obtain radar-preprocessed data at each moment in step 1 specifically includes:
  • setting a weight threshold λ_th;
  • if λ_i,j<λ_th, taking three-dimensional observation points corresponding to ρ_i,j as dynamic noise points, otherwise taking the three-dimensional observation points as static inline points;
  • obtaining a set of static inline observation points {ρ_i,j, v_i,j^d}, that is, the radar-preprocessed data after the dynamic noise points are eliminated;
  • Step 2: performing time integration on inertial navigation data of IMU observation subsequences synchronously acquired between adjacent radar sampling moments through a mechanical arrangement algorithm to obtain a predicted state at the next radar sampling moment;
  • the IMU observation subsequence synchronously acquired between adjacent radar sampling moments in step 2 is defined as:

$S_i=\left({\mu }_i^1,{\mu }_i^2,...{\mu }_i^k...,{\mu }_i^{N_{i_I}}\right),{\mu }_i^k=\left(a_i^k,{\omega }_i^k\right)$ $i\in \left[1,N_t\right]$

wherein S_i represents the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, μ_i^k represents the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, a_i^k represents the acceleration in the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, ω_i^k is the angular velocity in the kth IMU observation data in the IMU observation subsequence between the ith radar sampling moment and the (i+1)th radar sampling moment, N_iI represents the number of observations in the IMU subsequence, and N_t represents the number of radar sampling moments;

• • the predicted state at the next radar sampling moment in step 2 is defined as:

${\hat{x}}_{i+1}=\left[{ }^G{\hat{p}}_{I_{i+1}},{ }^G{\hat{v}}_{I_{i+1}},{ }^G{\hat{R}}_{I_{i+1}}, {\hat{b}}_{a_{i+1}}, {\hat{b}}_{{\omega }_{i+1}},{ }^I{\hat{R}}_{R_{i+1}},{ }^I{\hat{l}}_{R_{i+1}},{ }^G{\hat{g}}_{{i+1}_{}}\right]$

• • wherein {circumflex over (x)}_i+1 represents the predicted state at the (i+1)th radar sampling moment, ^G{circumflex over (p)}_Ii+1 represents the position predicted value at the (i+1)th radar sampling moment, ^G{circumflex over (v)}_Ii+1 represents the velocity predicted value at the (i+1)th radar sampling moment, ^G{circumflex over (R)}_Ii+1 represents the attitude predicted value at the (i+1)th radar sampling moment, {circumflex over (b)}_ai+1 represents the acceleration bias predicted value at the (i+1)th radar sampling moment, {circumflex over (b)}_ωi+1 represents the angular velocity bias predicted value at the (i+1)th radar sampling moment, ^I{circumflex over (R)}_Ri+1 represents the predicted value of the extrinsic parameter attitude between IMU and radar at the (i+1)th radar sampling moment, ^I{circumflex over (l)}_Ri+1 represents the predicted value of the extrinsic parameter distance between IMU and radar at the (i+1)th radar sampling moment, and ^Gĝ_i+1 represents the gravity vector predicted value in the global coordinate system at the (i+1)th radar sampling moment;
  • Step 3: constructing a velocity error equation by the vehicle velocity in the radar local coordinate system estimated through the selecting weight iteration-based minimum 2-norm method in step 1 and the predicted vehicle velocity in the global coordinate system at the next radar sampling moment in step 2, obtaining a residual initial value at the next radar sampling moment and a Jacobian matrix at the next radar sampling moment by a linearized error equation, and preliminarily updating a vehicle state by using Kalman filtering to obtain a velocity-updated vehicle state at the next radar sampling moment;
  • the velocity error equation in step 3 is defined as:

$r_{i+1_{\mathrm{vel}}}={ }^R{v}_{i+1_m}-{ }^R{v}_{i+1}$ ${ }^R{v}_{i+1}=\left[{ }^I{R_{R_{i+1}}}^T\left({ }^G{R_{I_{i+1}}}^T{ }^G{v}_{I_{i+1}}+{\left({\omega }_{i+1}-b_{{\omega }_{i+1}}\right)}_{\times }·{ }^I{l}_{R_{i+1}}\right)\right]$

wherein r_i+1vel is the velocity residual at the (i+1)th radar sampling moment,

${ }^R{v}_{i+1_m}$

is the vehicle velocity in the radar local coordinate system at the (i+1)th radar sampling moment, ^Rv_i+1 is the velocity true value in the radar local coordinate system at the (i+1)th radar sampling moment, ^GR_Ii+1 represents the attitude at the (i+1)th radar sampling moment, ^IR_Ri+1 represents the rotational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, ^Gv_Ii+1 represents the velocity at the (i+1)th radar sampling moment, ω_i+1 represents the angular velocity measured value at the (i+1)th radar sampling moment, b_ωi+1 represents the angular velocity bias at the (i+1)th radar sampling moment, ^Il_Ri+1 represents the translational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, (⋅)_x represents the vector cross product, and (⋅)^T represents transposition;

• • the linearized error equation in step 3 is defined as:

$r_{i+1_{\mathrm{vel}}}={\hat{r}}_{i+1_{\mathrm{vel}}}+H_{i+1_{\mathrm{vel}}}{\tilde{x}}_{i+1}$ ${\hat{r}}_{i+1_{\mathrm{vel}}}={ }^R{\hat{v}}_{i+1}+{ }^R{v}_{i+1_m}$

• • wherein r_i+1vel represents the velocity residual at the (i+1)th radar sampling moment, {circumflex over (r)}_i+1vel represents the velocity residual initial value at the (i+1)th radar sampling moment, ^R{circumflex over (v)}_i+1 is the IMU predicted value of the velocity in the radar local coordinate system at the (i+1)th radar sampling moment, H_i+1vel is the Jacobian matrix at the (i+1)th radar sampling moment, and {tilde over (x)}_i+1 is the error state at the (i+1)th radar sampling moment;
  • Step 4: constructing a local map by three-dimensional point clouds of the radar-preprocessed data in the past multiple radar sampling moments through an ikdtree tree strategy, and obtaining n n-nearest neighbor points, in the local map, of each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next radar sampling moment through K-Nearest Neighbor (KNN) search in the local map; weighting the average Euclidean distance from each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next sampling moment to the corresponding n-nearest neighbor points by the local point cloud spatial distribution covariance method to obtain the average weighted distance from distribution to multi-distribution, forming spatial point matching constraints from point cloud to local map, so as to construct a matching error equation and a linearized matching error equation, obtaining an initial value of a matching residual at the next radar sampling moment and a matching residual Jacobian matrix at the next radar sampling moment through the linearized matching error equation, constructing a maximum a posteriori estimation problem by considering matching constraints corresponding to all 4D radar-preprocessed data, and performing further updating based on iterative Kalman filtering to obtain a vehicle state with high precision;
  • As shown in FIG. 2, compared with only using the velocity constraints (vel) in step 3 and only using the matching constraints (match) in step 4, the iRIO method with both velocity constraints and matching constraints can significantly reduce the positioning error and improve the robustness of the algorithm.
  • the matching error equation in step 4 is defined as

$r_{i+1_{\mathrm{pro}}}=G_{i+1}\left({{\sum }_{\tau =1}^n}^G{b}_{\tau }/n-{ }^G{T}_{R_{i+1}}·{\rho }_{i+1}\right)$ ${ }^G{T}_{R_{i+1}}=\left[\begin{array}{cc}{ }^G{R}_{I_{i+1}}& { }^G{l}_{I_{i+1}}\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{cc}{ }^I{R}_{R_{i+1}}& { }^I{l}_{R_{i+1}}\\ 0_{1\times 3}& 1\end{array}\right]$ $G_{i+1}={\left({\sum }_{\tau =1}^n{C}_{\tau }^B/n+{ }^G{T}_{R_{i+1}}·C^A·{ }^G{T_{R_{i+1}}}^T\right)}^{-\frac{1}{2}}$

• • wherein r_i+1pro represents the point cloud matching residual, ρ_i+1 is the coordinates of the radar observation point in the radar coordinate system at the (i+1)th radar sampling moment, ^Gb_τ is the coordinates of n-nearest neighbor points in the ρ_i+1 local map in the global coordinate system, n=5 is the number of nearest neighbor points, G_i+1 is the distance weighted matrix from distribution to multi-distribution at the (i+1)th radar sampling moment, ^GT_Ri+1 is the transformation matrix from radar local coordinate system to global coordinate system, ^GR_Ii+1 represents the attitude at the (i+1)th radar sampling moment, ^Gl_Ii+1 represents the position at the (i+1)th radar sampling moment, ^IR_Ri+1 represents the rotational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, ^Il_Ri+1 is the rotational extrinsic parameter between IMU and radar at the (i+1)th radar sampling moment, 0_1×3 represents a 0 vector with one row and three columns, C^A is the spatial distribution covariance corresponding to the current point ρ_i+1, and C_τ^B is the spatial distribution covariance corresponding to the nearest neighbor point ^Gb_τ.

The linearized matching error equation in step 4 is defined as:

$r_{i+1_{\mathrm{pro}}}={\hat{r}}_{i+1_{\mathrm{pro}}}+H_{i+1_{\mathrm{pro}}}{\tilde{x}}_{i+1}$

• • wherein {circumflex over (r)}_i+1pro is the initial value of the matching residual at the (i+1)th radar sampling moment, H_i+1pro is the matching Jacobian matrix at the (i+1)th radar sampling moment, and {tilde over (x)}_i+1 is the error state at the (i+1)th radar sampling moment;
  • the maximum a posteriori estimation problem of the matching constraints in step 4 is defined as:

$\underset{{\tilde{x}}^{\kappa }}{\min }\left({\left({\hat{x}}_{i+1}^{\kappa }{\stackrel{\_}{x}}_{i+1_{\mathrm{vel}}}\right)+J_{i+1}^{\kappa }{\tilde{x}}_{i+1}^{\kappa }}_{{\stackrel{\_}{P}}_{i+1_{\mathrm{vel}}}^{-1}}^2+{\sum }_{\zeta =1}^{\Pi }{{ }^{\zeta }{\hat{r}}_{i+1_{\mathrm{pro}}}^{\kappa }+{ }^{\zeta }{H}_{i+1_{\mathrm{pro}}}^{\kappa }{x}_{i+1_{\mathrm{pro}}}^{\kappa }}_{{\zeta }_{R_{i+1_{\mathrm{pro}}}^{-1}}}^2\right)$

wherein x_i+1vel is the state at the (i+1)th radar sampling moment after the velocity is updated, P_i+1vel is the covariance matrix at the (i+1)th radar sampling moment after the velocity observation is updated, ζR_i+1pro⁻¹ is the observation noise of the ζth point in the processed data at the (i+1)th radar sampling moment, Π is the number of effective observations in the radar point cloud frame at the (i+1)th radar sampling moment, {circumflex over (x)}_i+1^κ is the state updated value obtained by the kth iterative Kalman filtering at the (i+1)th radar sampling moment, {tilde over (x)}_i+1^κ is the error state corresponding to the kth iterative Kalman filtering at the (i+1)th radar sampling moment, J_i+1^κ is a {circumflex over (x)}_i+1^κx_i+1vel to {tilde over (x)}^κ linearized Jacobian matrix,

${\zeta }^{}{\hat{r}}_{i+1_{\mathrm{pro}}}^{\kappa }$

represents the initial value of the matching residual corresponding to the ζth radar observation point in the kth iteration at the (i+1)th radar sampling moment, ^ζH_i+1pro^κ represents the matching Jacobian matrix corresponding to the ζth radar observation point in the kth iteration at the (i+1)th radar sampling moment, is the state quantity subtraction operation, in which the rotational state quantity conforms to the right multiplication inverse disturbance, and the others conform to the vector subtraction operation, and ∥⋅∥ represents the Euclidean distance.

The iterative Kalman updated system state in step 4 is defined as follows:

${\hat{x}}_{i+1}^{\kappa +1}={\hat{x}}_{i+1}^{\kappa }\left(-K_{i+1_{\mathrm{pro}}}^{\kappa }{r}_{i+1_{\mathrm{pro}}}+\left(I-K_{i+1_{\mathrm{pro}}}^{\kappa }{H}_{i+1_{\mathrm{pro}}}^{\kappa }\right){\left(J^{\kappa }\right)}^{-1}\right)\left({\hat{x}}_{i+1}^{\kappa }{\stackrel{\_}{x}}_{i+1_{\mathrm{vel}}}\right)$ $P_{i+1}^{\kappa +1}=\left(I-K_{i+1_{\mathrm{pro}}}^{\kappa }{H}_{i+1_{\mathrm{pro}}}^{\kappa }\right)P_{i+1}^{\kappa }$

• • wherein: {circumflex over (x)}_i+1^κ+1 is the state obtained by the kth iterative Kalman filtering at the (i+1)th radar sampling moment, K_i+1pro^κ is the matching Kalman gain of the κth iteration filtering at the (i+1)th radar sampling moment, r_i+1pro^κ represents the point cloud matching residual, I is identity matrix, P_i+1 is the covariance matrix updated by matching at the (i+1)th radar sampling moment, P_i+1^κ is the covariance matrix updated by the κth iterative filtering, P_i+1^κ+1 is the covariance matrix updated by the κ+1th iterative filtering, H_i+1pro^κ is the matching Jacobian matrix in the κth iteration at the (i+1)th radar sampling moment, is a state quantity addition operation, in which the rotational state quantity conforms to the right multiplication disturbance and the others conform to the vector subtraction operation, and is a state quantity subtraction operation.
  • Step 5: describing and retrieving a frame, that is most similar to a point cloud frame at the current moment, among historical radar point cloud frames by the ScanContext feature; judging whether the similarity is greater than an established threshold, if the similarity is less than the threshold, discarding the closed loop, otherwise accepting the closed loop, calculating a three-dimensional projection transformation relationship between two point cloudframes using a GICP algorithm, and updating the vehicle state at all moments before by taking this spatial transformation relationship as a closed-loop constraints.

As shown in FIG. 3, the pose recovery precision of an iRIOM method with closed-loop constraint-based updating is obviously superior to an iRIO without closed-loop constraints and an existing EKFRIO method.

The specific embodiments of the present disclosure further provide a computer-readable medium.

The computer-readable medium is a server workstation;

• • the server workstation stores a computer program executed by an electronic device, and when running on the electronic device, the computer program causes the electronic device to execute the steps of the vehicle 4D millimeter-wave radar inertial odometry method according to the embodiments of the present disclosure.

It should be understood that the parts not elaborated in detail in this description belong to the prior art.

It should be understood that the above detailed description of the preferred embodiments should not be regarded as a limitation on the patent protection scope of the present disclosure. Under the inspiration of the present disclosure, a person having ordinary skill in the art can make substitutions or modifications to the present disclosure without departing from the scope protected by the claims of the present disclosure, all of which fall within the protection scope of the present disclosure, which should be subject to the appended claims.

Claims

1. A vehicle 4D millimeter-wave radar inertial odometry method, comprising the following steps: Step 1: performing significant multipath scattering and speckle noise point removal screening through a spatial distance and a relaxation filtering method based on local point cloud statistical characteristics to obtain a three-dimensional point cloud after relaxation filtering at each moment, and estimating a vehicle velocity in a radar local coordinate system at each moment through a selecting weight iteration-based minimum 2-norm method, and obtaining radar-preprocessed data at each moment;Step 2: performing time integration on inertial navigation data of an IMU observation subsequence synchronously acquired between adjacent radar sampling moments through a mechanical arrangement algorithm to obtain a predicted state at a next radar sampling moment;Step 3: constructing a velocity error equation by the vehicle velocity in the radar local coordinate system estimated through the selecting weight iteration-based minimum 2-norm method and a predicted vehicle velocity in a global coordinate system at a next radar sampling moment, obtaining a residual initial value at the next radar sampling moment and a Jacobian matrix at the next radar sampling moment by a linearized error equation, and preliminarily updating a vehicle state by using Kalman filtering to obtain a velocity-updated vehicle state at the next radar sampling moment;Step 4: acquiring n-nearest neighbor points, in a local map, of each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next radar sampling moment, constructing a spatial point matching constraint from point cloud to local map, a matching error equation and a linearized matching error equation, obtaining a matching residual initial value at the next radar sampling moment and a matching residual Jacobian matrix at the next radar sampling moment through the linearized matching error equation, constructing a maximum a posteriori estimation problem by considering matching constraints corresponding to all 4D radar-preprocessed data, and performing further updating based on iterative Kalman filtering to obtain a vehicle state with high precision; andStep 5: describing and retrieving a frame, that is most similar to a point cloud frame at a current moment, among historical radar point cloud frames by a ScanContext feature; judging whether a similarity is greater than an established threshold, under the condition that the similarity is less than the threshold, discarding a closed loop, otherwise accepting the closed loop, calculating a three-dimensional projection transformation relationship between two point cloud frames using a GICP algorithm, and updating the vehicle state at all moments before by taking this spatial transformation relationship as a closed-loop constraint.

2. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 1, wherein: performing significant multipath scattering and speckle noise point removal screening through a spatial distance and a relaxation filtering method based on local point cloud statistical characteristics to obtain a three-dimensional point cloud after relaxation filtering at each moment in step 1 specifically comprises:acquiring a sequence of 4D radar observation data at a plurality of radar sampling moments at a certain frequency, and performing significant multipath scattering and speckle noise point removal screening on the three-dimensional point cloud at each radar sampling moment through a spatial distance and a relaxation filtering method based on local point cloud statistical characteristics to obtain a three-dimensional point cloud after relaxation filtering at each moment;wherein the 4D radar observation data at each radar sampling moment consists of a three-dimensional point cloud and a Doppler velocity of each three-dimensional observation point in the three-dimensional point cloud;the 4D radar observation data at each radar sampling moment is defined as: datai={{ρi,j,vi,jd},j=1 . . . N}i∈[1,Nt]ρi,j=(xi,j,yi,j,zi,j)wherein, datai represents the 4D radar observation data at the ith radar sampling moment, ρi,j represents the three-dimensional coordinates of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, xi,j represents the X-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, yi,j represents the Y-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, zi,j represents the Z-axis of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, vi,jd represents the Doppler velocity observation of the jth three-dimensional observation point in the three-dimensional point cloud of the 4D radar observation data at the ith radar sampling moment, N represents the number of three-dimensional observation points in the three-dimensional point cloud of the 4D radar observation data at each radar sampling moment, and Nt represents the number of radar sampling moments.

3. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 2, wherein: estimating a vehicle velocity in the radar local coordinate system at each moment through a selecting weight iteration-based minimum 2-norm method, and obtaining radar-preprocessed data at each moment in step 1 specifically comprises:estimating a vehicle velocity in the radar local coordinate system at each moment by the three-dimensional point cloud after relaxation filtering at each moment and the Doppler velocity corresponding to each point in the point cloud through the selecting weight iteration-based minimum 2-norm method, and eliminating dynamic interference points still existing in the point cloud after relaxation filtering to obtain radar-preprocessed data at each moment.

4. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 3, wherein: The selecting weight iteration-based minimum 2-norm method is specifically as follows:a robust estimation problem at each radar sampling moment is constructed, which is specifically as follows:

5. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 4, wherein: the IMU observation subsequence synchronously acquired between adjacent radar sampling moments in step 2 is defined as:

6. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 5, wherein: the velocity error equation in step 3 is defined as:

7. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 6, wherein: acquiring n-nearest neighbor points, in a local map, of each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next radar sampling moment in step 4 specifically comprises:constructing a local map by three-dimensional point clouds of the radar-preprocessed data in the past multiple radar sampling moments through an ikdtree tree strategy, and obtaining n-nearest neighbor points, in the local map, of each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next radar sampling moment through KNN search in the local map;constructing a spatial point matching constraint from point cloud to local map, a matching error equation and a linearized matching error equation in step 4 specifically comprises:weighting an average Euclidean distance from each three-dimensional point in the three-dimensional point cloud of the radar-preprocessed data at the next sampling moment to the corresponding n-nearest neighbor points through a local point cloud spatial distribution covariance method to obtain an average weighted distance from distribution to multi-distribution, and forming a spatial point matching constraint from point cloud to local map, so as to construct a matching error equation and a linearized matching error equation.

8. The vehicle 4D millimeter-wave radar inertial odometry method according to claim 7, wherein: the matching error equation is defined as

9. A computer-readable medium, storing a computer program executed by an electronic device, wherein when running on the electronic device, the computer program causes the electronic device to execute the steps of the method according to claim 1.