Millimeter wave radar based fusion of multiple sensors for autonomous positioning of unmanned vehicles

Abstract

The application relates to an unmanned vehicle autonomous positioning method based on fusion of various sensors of a millimeter wave radar, and relates to an unmanned vehicle autonomous positioning method. The application aims to solve the problem that an existing indoor autonomous positioning scheme is difficult to normally work in a dark scene, a smoky scene and other harsh scenes. The process is as follows: one, the vehicle is loaded with a millimeter wave radar and an IMU; two, the initial position coordinates of the vehicle are set; the initial attitude of the vehicle is calculated; three, the position and attitude of the vehicle are constantly updated; four, the radar speed is estimated according to radar point cloud data; five, the position and attitude of the vehicle and the speed of the radar are fused until the vehicle path planning is completed; six, features of each radar frame are generated according to radar original echo data; seven, matching constraints are obtained based on the features of each radar frame; eight, pose graph optimization is carried out based on the planned vehicle path and the matching constraints, and the vehicle pose is obtained. The application belongs to the fields of indoor positioning and wireless communication.

Description

Technical Field

[0001] This invention relates to an autonomous positioning method for unmanned vehicles, belonging to the fields of indoor positioning and wireless communication. Background Technology

[0002] With the development of robotics technology, various unmanned vehicles, drones, and robots are gradually becoming more autonomous and intelligent. The demand for indoor positioning is increasing. Accurate self-positioning is a key technology for robots to achieve autonomous movement and operation. Positioning and navigation, especially in the absence of or with weak GNSS, remains a challenge. Because a single sensor cannot achieve the desired results, sensor fusion has been a popular research area. Currently, most platforms use LiDAR and cameras as their primary sensors. These sensors perform well in good weather and ideal environmental conditions. However, visual sensor methods like cameras typically work in darkness, direct sunlight, fog, or smoke. While LiDAR is unaffected by light, it is susceptible to interference from rain, fog, and small particles such as dust and smoke.

[0003] Recently, millimeter-wave radar sensors have attracted in-depth research. Millimeter-wave radar typically has longer wavelengths, a wider detection range, can operate in environments with diminished visual capabilities, and is virtually unaffected by small particles such as fog, rain, and smoke. When autonomous vehicles are operating in harsh environments such as fires, earthquakes, or construction sites with heavy smoke, it offers significant advantages over other sensors. Furthermore, it can provide Doppler velocity observations of target points, allowing for the determination of ego velocity within a single radar scan. Summary of the Invention

[0004] The purpose of this invention is to solve the problem that existing indoor autonomous positioning solutions are difficult to work properly in harsh environments such as dim lighting and smoke, and to propose an autonomous positioning method for unmanned vehicles based on millimeter-wave radar and the fusion of multiple sensors.

[0005] The specific process of the autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors is as follows:

[0006] Step 1: The vehicle is equipped with millimeter-wave radar and IMU;

[0007] The radar point cloud data and the raw echo data of the radar are acquired through millimeter radar.

[0008] The radar point cloud data includes the position and Doppler velocity of each point;

[0009] Accelerometer acceleration data and gyroscope angular velocity data are acquired through the IMU;

[0010] The IMU is an inertial measurement unit;

[0011] Step 2: Set the initial position coordinates of the car to (0, 0);

[0012] The IMU data of the stationary car during time period T are averaged, and the initial attitude of the car is calculated based on the averaged IMU data. The initial attitude of the car is the initial Euler angle of the car.

[0013] Step 3: Based on the initial position coordinates and initial attitude of the car set in Step 2, continuously update the position and attitude of the car;

[0014] Step 4: Estimate the radar velocity based on the radar point cloud data obtained in Step 1;

[0015] Step 5: Use the error Kalman filter algorithm to fuse the position and attitude of the car obtained in Step 3 with the speed of the radar obtained in Step 4 until the car path planning is completed;

[0016] Step 6: Generate the features of each radar frame based on the raw radar echo data obtained in Step 1;

[0017] Step 7: Based on the features of each radar frame generated in Step 6, match the features of different radar frames according to the similarity of the radar frames to obtain matching constraints;

[0018] Step 8: Based on the planned car path obtained in Step 5 and the matching constraints obtained in Step 7, perform pose graph optimization to obtain the car pose.

[0019] The beneficial effects of this invention are:

[0020] This invention verifies the reliability of the proposed solution in different environments through experiments in multiple scenarios. In our experiments, the starting and ending poses of the walking trajectory were equal; therefore, it can be assessed that the final position error of the trajectory is less than 1% of the travel trajectory.

[0021] The innovative points of this invention are:

[0022] More effective features are obtained by processing the raw radar echo data to constrain the pose estimation.

[0023] Pose graph optimization is used to optimize the front-end navigation filter and closure constraints, thereby reducing the cumulative error of the front-end filter output and obtaining a reliable trajectory over a long period of time.

[0024] The details of this invention are described in two parts: A. Problems to be solved and B. Solutions.

[0025] A. The problem that this invention aims to solve is as follows:

[0026] 1) How to eliminate the cumulative error of the position obtained by IMU integration.

[0027] 2) Due to the multipath effect and the noise of the radar itself, the radar observation data will have many ghost points and error points. How can we obtain a basically accurate radar estimate under these error interferences?

[0028] 3) How to obtain radar features that can be used for loop closure detection from the original radar echo sampling data. Attached Figure Description

[0029] Figure 1 Flowchart of the overall implementation process: Detailed Implementation

[0030] Specific Implementation Method 1: The specific process of this implementation method for autonomous vehicle localization based on millimeter-wave radar and the fusion of multiple sensors is as follows:

[0031] Step 1: The vehicle is equipped with a millimeter-wave radar (10 samples per second) and an IMU (200 samples per second);

[0032] The radar point cloud data and the raw echo data of the radar are acquired through millimeter radar.

[0033] The radar point cloud data includes the position and Doppler velocity of each point;

[0034] Accelerometer acceleration data and gyroscope angular velocity data are acquired through the IMU;

[0035] The IMU (Inertial Measurement Unit) is an inertial measurement unit.

[0036] Step 2: Set the initial position coordinates of the car to (0, 0);

[0037] The IMU data of the stationary car during the time period T (10 seconds) are averaged, and the initial attitude of the car is calculated based on the averaged IMU data. The initial attitude of the car is the initial Euler angle of the car.

[0038] Step 3: Based on the initial position coordinates and initial attitude of the car set in Step 2, continuously update the position and attitude of the car (attitude: car rotation matrix R);

[0039] Step 4: Estimate the radar velocity based on the radar point cloud data obtained in Step 1;

[0040] Step 5: Use the error Kalman filter algorithm to fuse the position and attitude of the car obtained in Step 3 with the speed of the radar obtained in Step 4 until the car path planning is completed;

[0041] Step 6: Generate the features of each radar frame based on the raw radar echo data obtained in Step 1;

[0042] Step 7: Based on the features of each radar frame generated in Step 6, match the features of different radar frames according to the similarity of the radar frames to obtain matching constraints;

[0043] Step 8: Based on the planned car path obtained in Step 5 and the matching constraints obtained in Step 7, perform pose graph optimization to correct the cumulative error of the car and obtain a reliable pose of the car over a long period of time.

[0044] Specific Implementation Method Two: This implementation method differs from Specific Implementation Method One in that the initial position coordinates of the trolley are set to (0, 0) in step two.

[0045] The IMU data of the stationary car during the time period T (10 seconds) are averaged, and the initial attitude of the car is calculated based on the averaged IMU data. The initial attitude of the car is the initial Euler angle of the car.

[0046] The specific process is as follows:

[0047] The initial Euler angles of the vehicle include the initial roll angle, the initial pitch angle, and the initial yaw angle.

[0048] The initial yaw angle of the vehicle is set to zero;

[0049] The expressions for the initial roll angle and the initial pitch angle of the trolley are:

[0050]

[0051]

[0052] Where roll0 is the initial roll angle of the vehicle, and pitch0 is the initial pitch angle of the vehicle. The acceleration value along the x-axis of the accelerometer. This represents the acceleration value along the y-axis of the accelerometer. is the acceleration value along the z-axis of the accelerometer, and g is the acceleration due to gravity.

[0053] The other steps and parameters are the same as in Specific Implementation Method 1.

[0054] Specific Implementation Method 3: This implementation method differs from Specific Implementation Method 1 or 2 in that: in step 3, the position and attitude of the car are continuously updated based on the initial position coordinates and initial attitude of the car set in step 2 (attitude: car rotation matrix R);

[0055] The specific process is as follows:

[0056] Step two yields the initial position coordinates and initial attitude of the vehicle. Then, the current attitude of the vehicle needs to be continuously estimated and updated based on the IMU data. This is the motion part of the error Kalman filter (ESKF).

[0057] The variables obtained by IMU recursion are called nominal state variables, and the state variables in ESKF are called error state variables.

[0058] When IMU measurement data arrives, it is integrated and then stored in the nominal state variable. Since this approach doesn't account for noise, the result will naturally drift rapidly. Therefore, we want to treat the error component as an error state variable and store it in the ESKF. The ESKF internally considers the effects of various noises and zero bias, and provides a Gaussian distribution description of the error state. Simultaneously, the ESKF itself, as a Kalman filter, also has prediction and correction processes, with the correction process relying on sensor observations outside the IMU. After correction, the ESKF can provide a posterior error Gaussian distribution. We can then store this error in the nominal state variable and set the ESKF to zero, thus completing one cycle.

[0059] Construct the kinematic equations of the car;

[0060] The kinematic equations of the vehicle include the discrete-time kinematic equations of the nominal state variables and the discrete-time kinematic equations of the error state. These two equations illustrate the process of updating the vehicle's attitude based on IMU measurement data.

[0061] The discrete-time kinematic equations for the nominal state variables are written as follows:

[0062]

[0063]

[0064]

[0065] b g (t+Δt)=b g (t)

[0066] b a (t+Δt)=b a (t)

[0067] g(t+Δt)=g(t)

[0068] Where (t+Δt) represents the state at time t+Δt, t represents the state at time t, Δt represents the IMU data sampling interval, P(t) represents the position of the vehicle at time t, v(t) represents the velocity of the vehicle at time t, and R(t) represents the rotation matrix of the vehicle at time t (R(t) is obtained based on Euler angles, let Euler angles be [α,β,γ], then the corresponding...

[0069]

[0070] This represents the accelerometer reading at time t. Let represent the gyroscope measurement at time t, g(t) represent the gravitational acceleration at time t, Exp() represent the exponential function with base e, and b g (t) represents the gyroscope zero bias at time t (a rough estimate), b a (t) represents the accelerometer zero bias at time t (rough estimate);

[0071] The discrete-time kinematic equations for the error state are written as follows:

[0072] δP(t+Δt)=δP(t)+δv(t)Δt

[0073]

[0074]

[0075] δb g (t+Δt)=δb g (t)+η g

[0076] δb a (t+Δt)=δb a (t)+η a

[0077] δg(t+Δt)=δg(t)

[0078] Where, η v For velocity noise, η θ For angular velocity noise, η g For gyroscope noise, η a The accelerometer noise is represented by θ(t), which represents the angular velocity of the trolley at time t. ^ is the antisymmetry symbol.

[0079] Other steps and parameters are the same as in specific implementation method one or two.

[0080] Specific Implementation Method Four: This implementation method differs from Specific Implementation Methods One to Three in that: in step four, the radar velocity is estimated based on the radar point cloud data obtained in step one; the specific process is as follows:

[0081] Step 41: Determine whether the target point detected by the radar is a noise point. If it is a noise point, discard it; if it is not a noise point, record it to obtain a set of target points that are not noise points. The specific process is as follows:

[0082] Step 411: Randomly select 3 target points from the target points detected by the radar, and calculate the radar velocity v based on the 3 target points. r The expression is:

[0083] The first target point detected by the radar is located at position X1, and the direction of the first target point is obtained as follows:

[0084] The second target point detected by the radar is located at position X2, and the direction of the second target point is obtained as follows:

[0085] The third target point detected by the radar is located at position X3, and the direction of the third target point is obtained as follows:

[0086] Based on the direction r1 of the first target point, the direction r2 of the second target point, the direction r3 of the third target point, and the radial Doppler velocity v of the first target point. d1 The radial Doppler velocity v at the second target point d2 The radial Doppler velocity v at the third target point d3 Calculate radar velocity v r The expression is:

[0087]

[0088] Among them, v d1 Let v be the radial Doppler velocity of the first target point. d2 Let v be the radial Doppler velocity of the second target point. d3 The radial Doppler velocity of the third target point; |||| is the symbol for calculating the vector magnitude; · is the symbol for vector dot product;

[0089] Step 412: The radar velocity v obtained in Step 411... r Substitute Result A was obtained;

[0090] Step 413: If the magnitude of result A is less than the threshold, then the three target points are considered not to be noise points. Record the three target points and then execute step 414.

[0091] If the magnitude of result A is greater than or equal to the threshold, proceed directly to step 414;

[0092] Step 414: Repeat step 411 (randomly select 3 target points from the target points detected by the radar, and calculate the radar velocity v based on the 3 target points). r Repeat step four three times (B is 40) to obtain the set of target points that are not noise points;

[0093] Step 42: Calculate the radar's final velocity v based on the set of target points that are not noise points obtained in Step 41. r The specific process is as follows:

[0094] Based on the radial Doppler velocity v of the target point measured by radar d Calculate the radar velocity v based on the direction r of the target point. r The expression is:

[0095] v d =r·v r

[0096] Among them, v r V represents the radar speed (the radar is mounted on the vehicle), · represents the vector dot product symbol, and v d Radial Doppler velocity;

[0097] Due to radar speed v r It is a three-dimensional quantity, so the radar velocity v cannot be calculated from a single target point. r At least three target points are needed to determine the radar velocity v. r ;

[0098] Based on the radial Doppler velocities of the n target points (not noise points) obtained in step four-one and the directions of the n target points, the final radar velocity v is calculated. r The expression is:

[0099]

[0100] Among them, v dn Let r be the radial Doppler velocity of the nth target point. n The direction of the nth target point is given, where n is the number of target points detected by the radar, and 6 ≤ n ≤ 70.

[0101] The other steps and parameters are the same as those in one of the specific implementation methods one to three.

[0102] Specific Implementation Method Five: This implementation method differs from Specific Implementation Methods One through Four in that: in step five, an error Kalman filter algorithm is used to fuse the position and attitude of the vehicle obtained in step three with the speed of the radar obtained in step four, until the vehicle path planning is completed; the specific process is as follows:

[0103] Step 51: Predict the error state variables and covariance matrix of the vehicle based on the Error Kalman Filter (ESKF); the specific process is as follows:

[0104]

[0105] Among them, S pred Let S be the predicted covariance matrix, and S be the covariance matrix of the vehicle's state (an initial matrix). Let F be the noise covariance matrix of the vehicle's state; F is the error state equation of the vehicle written in matrix form; T represents the transpose.

[0106] δx=[δP,δv,δθ,δb g ,δb a ,δg] T

[0107]

[0108]

[0109] Where δx is the error state variable of the vehicle;

[0110] diag is the notation for a diagonal matrix, Cov() is the notation for a covariance matrix, and 03 is a 3-dimensional zero matrix;

[0111] η v For velocity noise, η θ For angular velocity noise, η g For gyroscope noise, η a Accelerometer noise;

[0112] δP represents the error position coordinates of the trolley, δv represents the error velocity of the trolley, δθ represents the error angular velocity of the trolley, and δb represents the error position coordinates of the trolley. g For the gyroscope's error to be zero bias, δb a δg represents the zero bias of the accelerometer error, and δg is the error value of the gravitational acceleration.

[0113] ^ represents antisymmetry, and I represents the identity matrix;

[0114] Step 52: Update the error state variables and covariance matrix of the vehicle obtained in Step 51; the specific process is as follows:

[0115] Because the radar's observation equation is:

[0116]

[0117] Among them, v r Let u be the radar velocity, denoted by u, and U be the noise covariance matrix of the radar observation equation. Let R be a normal distribution with covariance matrix U. rb Let t be the rotation matrix from the vehicle coordinate system to the radar coordinate system. br This represents the displacement from the vehicle coordinate system to the radar coordinate system; R is the angular velocity detected by the gyroscope, R is the rotation matrix of the car (obtained based on Euler angles), and v is the speed of the car.

[0118] set up

[0119] Therefore, the update process is as follows:

[0120] K = S pred HT (HS pred H T +U) -1

[0121] δx=K(V r -h(x))

[0122] S fin =(I-KH)S pred

[0123] Where K is the Kalman gain, S pred S is the predicted covariance matrix. fin H is the final corrected state covariance matrix of the vehicle, H is the Jacobian matrix of the radar's observation equation compared to the vehicle's error state variable δx, h(x) is the radar's observation equation; δx is the corrected error state variable of the vehicle.

[0124] The radar's observation equation is compared to the Jacobian matrix of the vehicle's error state variable δx.

[0125] in It involves linearizing the radar's observation equations.

[0126] Using the Baker-Campbell-Hausdorff formula (BCH formula), we can approximate the result.

[0127]

[0128] Where I3 is a 3-dimensional identity matrix. This is a right-multiplied Jacobian matrix;

[0129] Step 53: Merge the error state variable δx of the vehicle from Step 52 with the nominal state variable of the vehicle, and then reset the error variable of ESKF; the specific process is as follows:

[0130] Step 531: Combine the error state variable δx of the vehicle from Step 52 with the nominal state variable of the vehicle, using the following formula:

[0131] P′=P+δP

[0132] v′=v+δv

[0133] R′=RExp(δθ)

[0134] b′ g =b g +δb g

[0135] b′ a =ba +δb a

[0136] g′=g+δg

[0137] Among them, P', v', R', b' g b' a g' represents the final fusion output, P, v, R, b g b a Let g be the nominal state variables of the car, and δP, δv, δb be the variables of the car. g , δθ, δb a δg represents the error state variable of the trolley;

[0138] Step 532: Reset the ESKF error variable. The process is as follows:

[0139] The mean portion is set as follows:

[0140] δx=0

[0141] The covariance component is

[0142] S reset =JS fin J T

[0143] Where S fin Let J be the final corrected state covariance matrix of the vehicle, and J be the Jacobian matrix, where J = diag(I3, I3, I...). θ ,I3,I3,I3);

[0144] I θ Let be the Jacobian matrix of the angular velocity. ^ represents the antisymmetry symbol, and θ represents the angular velocity of the trolley;

[0145] I3 is the identity matrix; S reset The covariance matrix of the prediction is reset;

[0146] Step 533: Take the data fused from Step 531 and bring it into Step 3 to continuously update the position and attitude of the car. When there is new radar data, execute Step 4 and Step 5 based on Step 532.

[0147] Step 534: Repeat step 533 until the car path planning is complete.

[0148] The other steps and parameters are the same as those in specific implementation methods one through four.

[0149] Specific Implementation Method Six: This implementation method differs from Specific Implementation Methods One to Five in that: in step six, features for each radar frame are generated based on the raw radar echo data obtained in step one; the specific process is as follows:

[0150] The radar uses a frame as the smallest processing unit, and the frequency-modulated signal transmitted by the radar is called chirp;

[0151] Each frame contains 3 antennas, each antenna transmits 32 chirps, and each chirp is received by 4 receiving antennas. The receiving antennas collect data from each received chirp, and the data collected is complex data.

[0152] Therefore, each frame of radar data consists of 32*3*4*256 complex numbers; * represents a multiplication sign;

[0153] Generating the features for each radar frame involves the following steps:

[0154] a) The radar data of each frame is grouped according to the antenna and divided into a three-dimensional array A with dimensions (32, 12, 256);

[0155] The three dimensions of the three-dimensional array A are respectively called the doppler dimension, the aoa dimension, and the range dimension;

[0156] b) Different distances and orientations of target points detected by radar will result in different frequencies and phases of the echoes, so this information can be extracted by performing a discrete Fourier transform.

[0157] First, perform a 256-point Fourier transform (FFT) on the range dimension data of the three-dimensional array A to obtain the three-dimensional array A after one Fourier transform. The dimensions of the three-dimensional array A after one Fourier transform are (32, 12, 256).

[0158] Perform a 32-point Fourier transform on the doppler dimension data of the three-dimensional array A after the first Fourier transform to obtain the three-dimensional array A after the second Fourier transform. The dimensions of the three-dimensional array A after the second Fourier transform are (32, 12, 256).

[0159] Finally, the data of the aoa dimension of the three-dimensional array A after the second Fourier transform are added together to obtain a two-dimensional array B, with dimensions (32, 256).

[0160] c) Perform CFAR detection on the two-dimensional array B to obtain the coordinates of the doppler and range dimensions with higher peak values;

[0161] Based on the coordinates of the coordinate points in the doppler dimension and range dimension, extract the first 8 numbers of the corresponding aoa dimension in the three-dimensional array A after the second Fourier transform, perform a 64-point Fourier transform, extract the point with the highest peak in the aoa dimension as the abscissa of the feature, and the corresponding range dimension coordinate as the ordinate, and record it.

[0162] Then, take the last 4 numbers in the aoa dimension of the three-dimensional array A after the second Fourier transform (there are 12 numbers in the aoa dimension of the three-dimensional array A after the second FFT) and perform a 64-point Fourier transform. Take the point with the highest peak as the x-coordinate of the feature and the corresponding range dimension coordinate as the y-coordinate. Add 64 to the x-coordinate and record it.

[0163] Construct a two-dimensional array, setting the coordinates recorded above to 1 and the rest to 0; thus, a 128*265 two-dimensional array consisting of 1s and 0s is obtained, representing the features of each radar frame.

[0164] The other steps and parameters are the same as those in specific implementation methods one through five.

[0165] Specific Implementation Method Seven: This implementation method differs from Specific Implementation Methods One through Six in that: in step seven, based on the features of each radar frame generated in step six, features of different radar frames are matched according to the similarity of the radar frames to obtain matching constraints; the specific process is as follows:

[0166] Step 71: Take the features of the first radar frame and match them with the features of all other radar frames. Take the feature with the highest similarity as the feature pair of the first radar frame. Add the feature arrays of the two radar frames together. The more times the sum of the sums ...

[0167] The features of the second radar frame are matched with the features of all other radar frames, and the feature with the highest similarity is taken as the feature pair of the second radar frame.

[0168] The process continues until the feature pairs corresponding to the features of each radar frame are obtained and recorded.

[0169] Step 72: Set a threshold. Calculate the difference between the quaternions of the cars corresponding to the two features of the recorded feature pairs. Compare the difference with the threshold. Remove feature pairs with differences greater than the threshold and retain those with differences less than or equal to the threshold to obtain the final matched feature pairs.

[0170] The other steps and parameters are the same as those in one of the specific implementation methods one to six.

[0171] Specific Implementation Method Eight: This implementation method differs from any of Specific Implementation Methods One through Seven in that: in step eight, based on the planned vehicle path obtained in step five and the matching constraints obtained in step seven, pose graph optimization is performed to correct the cumulative error of the vehicle and obtain a reliable pose of the vehicle over a long period of time; the specific process is as follows:

[0172] Graph optimization is a way of representing optimization problems as graphs. Here, the graph refers to a graph in the sense of graph theory. A graph consists of vertices and edges connecting these vertices. Vertices represent optimization variables, and edges represent error terms. For any nonlinear least squares problem of the above form, a corresponding graph can be constructed.

[0173] Step 81: Define the pose of the car as nodes in the pose graph, named T1, T2, ..., T n Let the edges represent the motion estimates between pose nodes; the overall objective function of the pose graph is:

[0174]

[0175] in, Let ε be the information matrix (here, the identity matrix), and let ε be the set of edges.

[0176] e ij For the error term; e ij The expression is:

[0177]

[0178] Among them, T i Let i be a node in the pose graph, and T be a node in the pose graph. j Let j be a node in the pose graph; T ij For the movement between nodes i and j;

[0179] T ij The expression is:

[0180]

[0181] Step 82: Solve the objective function using the Levenberg-Marquardt method to obtain the optimized poses of the vehicle, T1, T2, ..., T. n .

[0182] The other steps and parameters are the same as those in specific implementation methods one through seven.

[0183] The beneficial effects of the present invention are verified using the following embodiments:

[0184] Example 1:

[0185] For radar, we use the Texas Instruments IWR1843 millimeter-wave radar; for raw radar echo sampling, we use the Texas Instruments DCA1000EVM; and for the IMU sensor, we use the Yuansheng Innovation YIS300A.

[0186] First, IMU zero-bias calibration and the calibration of IMU and radar extrinsic parameters from the IMU coordinate system to the radar coordinate system must be performed. Then, IMU data, radar point cloud data, and raw data are acquired simultaneously, and the vehicle is started. The IMU data acquisition frequency is 200 Hz, and the radar data acquisition frequency is 10 Hz.

[0187] First, the vehicle's state is initialized using IMU data, and this state is continuously updated. When radar data arrives, the vehicle's speed is estimated using radar point cloud data, and this speed is fused and corrected using the ESKF algorithm. Then, radar features are calculated based on radar echo data to perform loop closure detection. When a loop closure is detected, the pose graph optimization module is triggered to optimize the past trajectory, thereby obtaining the final result.

[0188] This invention may have other embodiments. Without departing from the spirit and essence of this invention, those skilled in the art can make various corresponding changes and modifications according to this invention, but these corresponding changes and modifications should all fall within the protection scope of the appended claims.

Claims

1. An autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors, characterized in that: The specific process of the method is as follows: Step 1: The vehicle is equipped with millimeter-wave radar and IMU; The radar point cloud data and the raw echo data of the radar are acquired through millimeter radar. The radar point cloud data includes the position and Doppler velocity of each point; Accelerometer acceleration data and gyroscope angular velocity data are acquired through the IMU; The IMU is an inertial measurement unit; Step 2: Set the initial position coordinates of the car as follows ; The IMU data of the stationary car during time period T are averaged, and the initial attitude of the car is calculated based on the averaged IMU data. The initial attitude of the car is the initial Euler angle of the car. Step 3: Based on the initial position coordinates and initial attitude of the car set in Step 2, continuously update the position and attitude of the car; Step 4: Estimate the radar velocity based on the radar point cloud data obtained in Step 1; Step 5: Use the error Kalman filter algorithm to fuse the position and attitude of the car obtained in Step 3 with the speed of the radar obtained in Step 4 until the car path planning is completed; Step Six: Generate features for each radar frame based on the raw radar echo data obtained in Step One; the specific process is as follows: The radar uses a frame as the smallest processing unit, and the frequency-modulated signal transmitted by the radar is called chirp; Each frame contains 3 antennas, each antenna transmits 32 chirps, and each chirp is received by 4 receiving antennas. The receiving antennas collect data from each received chirp, and the data collected is complex data. Therefore, each frame of radar data consists of 32*3*4*256 complex numbers; * represents a multiplication sign; Generating the features for each radar frame involves the following steps: a) The radar data of each frame is grouped according to the antenna and divided into a three-dimensional array A with dimensions (32, 12, 256); The three dimensions of the three-dimensional array A are respectively called the doppler dimension, the aoa dimension, and the range dimension; b) First, perform a 256-point Fourier transform on the range dimension data of the three-dimensional array A to obtain the three-dimensional array A after one Fourier transform. The dimensions of the three-dimensional array A after one Fourier transform are (32, 12, 256). Perform a 32-point Fourier transform on the doppler dimension data of the three-dimensional array A after the first Fourier transform to obtain the three-dimensional array A after the second Fourier transform. The dimensions of the three-dimensional array A after the second Fourier transform are (32, 12, 256). Finally, the data of the three-dimensional array A after the second Fourier transform are added together to obtain a two-dimensional array B, with dimensions (32, 256). c) Perform CFAR detection on the two-dimensional array B to obtain the coordinates of the doppler and range dimensions; Based on the coordinates of the coordinate points in the doppler dimension and range dimension, extract the first 8 numbers of the corresponding aoa dimension in the three-dimensional array A after the second Fourier transform, perform a 64-point Fourier transform, extract the point with the highest peak in the aoa dimension as the abscissa of the feature, and the corresponding range dimension coordinate as the ordinate, and record it. Then, take the last four numbers of the aoa dimension in the three-dimensional array A after the second Fourier transform and perform a 64-point Fourier transform. Take the point with the highest peak as the abscissa of the feature and the corresponding range dimension coordinate as the ordinate. Add 64 to the abscissa and record it. Construct a two-dimensional array, setting the coordinates recorded above to 1 and the rest to 0; thus obtaining a 128*265 two-dimensional array consisting of 1s and 0s representing the features of each radar frame; Step 7: Based on the features of each radar frame generated in Step 6, match the features of different radar frames according to the similarity of the radar frames to obtain matching constraints; the specific process is as follows: Step 71: Take the features of the first radar frame and match them with the features of all other radar frames. Take the feature with the highest similarity as the feature pair of the first radar frame. The features of the second radar frame are matched with the features of all other radar frames, and the feature with the highest similarity is taken as the feature pair of the second radar frame. The process continues until the feature pairs corresponding to the features of each radar frame are obtained and recorded. Step 72: Set a threshold, calculate the difference between the quaternions of the cars corresponding to the two features of the recorded feature pairs, compare the difference with the threshold, and remove the feature pairs corresponding to the difference that is greater than the threshold. The feature pairs corresponding to the differences less than or equal to the threshold are retained to obtain the final matched feature pairs; Step 8: Based on the planned car path obtained in Step 5 and the matching constraints obtained in Step 7, perform pose graph optimization to obtain the car pose.

2. The autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors according to claim 1, characterized in that: In step two, the initial position coordinates of the trolley are set as follows: ; The IMU data of the stationary car during time period T are averaged, and the initial attitude of the car is calculated based on the averaged IMU data. The initial attitude of the car is the initial Euler angle of the car. The specific process is as follows: The initial Euler angles of the vehicle include the initial roll angle, the initial pitch angle, and the initial yaw angle. The initial yaw angle of the vehicle is set to zero; The expressions for the initial roll angle and the initial pitch angle of the trolley are: in, It is the initial roll angle of the car. It is the initial pitch angle of the car. The acceleration value along the x-axis of the accelerometer. The acceleration value along the y-axis of the accelerometer. The acceleration value along the z-axis of the accelerometer. This is the acceleration due to gravity.

3. The autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors according to claim 2, characterized in that: In step three, the position and attitude of the car are continuously updated based on the initial position coordinates and initial attitude of the car set in step two. The specific process is as follows: Construct the kinematic equations of the car; The kinematic equations of the vehicle include the discrete-time kinematic equations of the nominal state variables and the discrete-time kinematic equations of the error state. The discrete-time kinematic equations for the nominal state variables are written as follows: in, express The state at time t, where t represents the state at time t. Indicates the IMU data sampling interval. This represents the position of the car at time t. This represents the speed of the car at time t. Let represent the rotation matrix of the car at time t. This represents the accelerometer reading at time t. This represents the measurement value of the gyroscope at time t. This represents the value of gravitational acceleration at time t. This represents an exponential function with base e. This indicates that the gyroscope has zero bias at time t. This indicates the zero bias of the accelerometer at time t; The discrete-time kinematic equations for the error state are written as follows: in, For speed noise, This is angular velocity noise. This is gyroscope noise. Indicates accelerometer noise. This represents the angular velocity of the car at time t. It is an antisymmetric symbol; Here are the coordinates of the error position of the car. The error speed of the car, Let ω be the error angular velocity of the trolley. To achieve zero bias in the gyroscope's error, To achieve zero bias in the accelerometer error, This represents the error value for gravitational acceleration.

4. The autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors according to claim 3, characterized in that: In step four, the radar velocity is estimated based on the radar point cloud data obtained in step one; the specific process is as follows: Step 41: Determine whether the target point detected by the radar is a noise point. If it is a noise point, discard it; if it is not a noise point, record it to obtain a set of target points that are not noise points. The specific process is as follows: Step 411: Randomly select 3 target points from the target points detected by the radar, and calculate the radar velocity based on the 3 target points. The expression is: The first target point detected by the radar is located at position The direction of the first target point is obtained as ; The second target point detected by the radar is located at [location]. The direction of the second target point is obtained as follows ; The third target point detected by the radar is located at [location]. The direction of the third target point is obtained as follows ; Based on the direction of the first target point The direction of the second target point The direction of the third target point Radial Doppler velocity of the first target point Radial Doppler velocity of the second target point The radial Doppler velocity of the third target point Calculate radar speed The expression is: in, The radial Doppler velocity of the first target point, The radial Doppler velocity of the second target point. The radial Doppler velocity of the third target point; To determine the sign of the vector magnitude; The dot product symbol is used for vectors. Step 412: Calculate the radar velocity obtained in Step 411. Substitute The result is A. Step 413: If the magnitude of result A is less than the threshold, then the three target points are considered not to be noise points. Record the three target points and then execute step 414. If the magnitude of result A is greater than or equal to the threshold, proceed directly to step 414; Step 414: Repeat steps 411 to 413B times to obtain the set of target points that are not noise points; Step 42: Calculate the radar's final velocity based on the set of target points that are not noise points obtained in Step 41. The specific process is as follows: Based on the radial Doppler velocities of n target points (not noise points) obtained in step 41 and the directions of the n target points, the final velocity of the radar is calculated. The expression is: in, Let n be the radial Doppler velocity of the nth target point. The direction of the nth target point is given, where n is the number of target points detected by the radar.

5. The autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors according to claim 4, characterized in that: In step five, an error Kalman filter algorithm is used to fuse the position and attitude of the vehicle obtained in step three with the speed of the radar obtained in step four, until the vehicle path planning is completed; the specific process is as follows: Step 51: Predict the error state variables and covariance matrix of the vehicle based on the Error Kalman Filter (ESKF); the specific process is as follows: in, The predicted covariance matrix, Let be the covariance matrix of the vehicle's state. Let be the covariance matrix of the noise in the vehicle's state; The error state equation of the car is written in matrix form; T represents the transpose. in, For the error state variables of the vehicle; The symbol for a diagonal matrix is... The symbol for the covariance matrix is ​​. It is a 3D zero matrix; For speed noise, This is angular velocity noise. This is gyroscope noise. Accelerometer noise; Here are the coordinates of the error position of the car. The error speed of the car, Let ω be the error angular velocity of the trolley. To achieve zero bias in the gyroscope's error, To achieve zero bias in the accelerometer error, This represents the error value for gravitational acceleration. As an antisymmetric symbol, It is the identity matrix; Step 52: Update the error state variables and covariance matrix of the vehicle obtained in Step 51; the specific process is as follows: Because the radar's observation equation is: in, For radar speed, For noise, Let be the noise covariance matrix of the radar observation equation. The covariance matrix is The normal distribution Let be the rotation matrix from the vehicle coordinate system to the radar coordinate system. This represents the displacement from the vehicle coordinate system to the radar coordinate system; The angular velocity detected by the gyroscope. Let be the rotation matrix of the car. The speed of the car; set up Therefore, the update process is as follows: Where K is the Kalman gain. The predicted covariance matrix, This is the final corrected state covariance matrix of the vehicle. The radar's observation equation compared to the car's error state variable Jacobian matrix, The radar's observation equation; The corrected error state variables for the vehicle; Step 53: Calculate the error state variables of the vehicle from Step 52. The nominal state variables of the vehicle are merged, and then the error variables of ESKF are reset; the specific process is as follows: Step 531: Calculate the error state variables of the vehicle from Step 52. The formula for fusing the nominal state variables of the vehicle is as follows: in, , , , , The final output of the fusion process. , , , , Let be the nominal state variable of the car. , , , , For the error state variables of the vehicle; Step 532: Reset the ESKF error variable. The process is as follows: The mean portion is set as follows: The covariance component is in This is the final corrected state covariance matrix of the vehicle. For Jacobian matrices, ; Let be the Jacobian matrix of the angular velocity. As an antisymmetric symbol, The angular velocity of the trolley; It is the identity matrix; The covariance matrix of the prediction is reset; Step 533: Take the data fused from Step 531 and bring it into Step 3 to continuously update the position and attitude of the car. When there is new radar data, execute Step 4 and Step 5 based on Step 532. Step 534: Repeat step 533 until the car path planning is complete.

6. The autonomous localization method for unmanned vehicles based on millimeter-wave radar and fusion of multiple sensors according to claim 5, characterized in that: In step eight, based on the planned vehicle path obtained in step five and the matching constraints obtained in step seven, pose graph optimization is performed to obtain the vehicle pose; the specific process is as follows: Step 81: Define the pose of the car as a node in the pose graph, with... Let the edges represent the motion estimates between pose nodes; the overall objective function of the pose graph is: in, For information matrix, Let be the set of edges; This is the error term; The expression is: in Let i be a node in the pose graph. Let j be a node in the pose graph; For the movement between nodes i and j; The expression is: Step 82: Solve the objective function using the Levenberg-Marquardt method to obtain the optimized pose of the car. .

Images