Vehicle 4D millimeter-wave radar inertial odometry method and computer-readable medium

A vehicle 4D millimeter-wave radar inertial odometry method, comprising 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 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 an inertial measurement unit (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 precision;
• and
• Step 5:
• describing and retrieving a frame, from among historical radar point cloud frames, that is most similar to a point cloud frame at a current moment 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 Generalized Iterative Closest Point (GICP) algorithm, and updating the vehicle state at all moments before by taking the projection transformation relationship as a closed-loop constraint.