Course solving method and system based on optimal estimation of motion amount among multiple frames and application of course solving method and system

Abstract

The invention relates to the technical field of millimeter wave radar signal processing, in particular to a course solving method and system through optimal estimation of the amount of motion among multiple frames and application of the course solving method and system. S2, assuming a uniform motion model; s3, constructing a cost function; and S4, solving the optimal displacement. According to the invention, jitter errors caused by dependence on single-point amplitude in a traditional method can be avoided; the estimation robustness is improved by using the point cloud information of the whole frame; constructing a point cloud distance sum cost function to directly reflect the inter-frame matching degree; the method is easy to implement, high in calculation efficiency and suitable for a real-time system.

Description

Technical Field

[0001] This invention relates to the field of millimeter-wave radar signal processing technology, specifically a method, system, and application for optimal estimation of motion between multiple frames to determine heading. Background Technology

[0002] In millimeter-wave radar systems, target heading estimation is one of the key technologies for achieving target tracking and behavior prediction. Traditional methods typically select the point with the highest reflection amplitude in the target point cloud and estimate the target's direction of motion by calculating its displacement change over multiple frames. However, due to factors such as jitter of the target reflection point, angle measurement errors, and environmental noise, single-point estimation methods have poor robustness in complex scenarios and insufficient heading estimation accuracy, making it difficult to meet the requirements of high-precision applications. Summary of the Invention

[0003] To address the aforementioned problems, this invention proposes a method, system, and application for optimal estimation of motion between multiple frames to determine heading. By constructing a cost function, and assuming the target moves at a constant velocity, the optimal displacement between frames is solved. This enables high-precision estimation of the target's course.

[0004] A method for optimal estimation of motion across multiple frames to determine heading is described below: S1. Data Acquisition and Preprocessing: Acquire multiple consecutive frames of millimeter-wave radar point cloud data, preprocess each frame of point cloud, and extract the target point cloud set; S2. Assuming a uniform motion model: Assuming the target moves at a constant velocity in a straight line between adjacent frames, the target point cloud as a whole undergoes translation on the plane, with the inter-frame displacement being... ; S3. Construct the cost function: Construct a method for evaluating different The cost function for combined accuracy uses the sum of the distances between two point clouds as the cost function; S4. Solving for the optimal displacement: Within a predefined search range, find the optimal displacement (dx, dy) that minimizes the cost function; S5. Heading Angle Calculation: Calculate the target heading angle based on the optimal displacement.

[0005] Each point in the point cloud data in step S1 contains distance, angle, and intensity information.

[0006] The preprocessing in step S1 includes noise removal, filtering, and clustering operations.

[0007] In step S3, the cost function is specifically as follows: After translating the point cloud of frame t+1 by (dx, dy), calculate the sum of the nearest neighbor distances between it and the point cloud of frame t: ; in, and These are the target point cloud sets for frame t and frame t+1, respectively.

[0008] In step S4, the optimal displacement is solved using a traversal search or optimization algorithm.

[0009] In step S5, the formula for calculating the heading angle is: .

[0010] A system for optimal motion estimation and heading determination across multiple frames includes: Millimeter-wave radar sensors are used to acquire continuous multi-frame point cloud data; The preprocessing module is used to filter, cluster, and process point cloud data on a cluster basis. The cost function construction module is used to construct the cost function for the sum of distances between two frames of point clouds; The optimal displacement solution module is used to find the optimal displacement that minimizes the cost function within a preset search range. The heading angle calculation module is used to calculate the target heading angle based on the optimal displacement. The multi-frame fusion module is used to perform first-order exponential smoothing on the optimal displacement of multiple consecutive frames to improve the stability of heading estimation.

[0011] The formula for calculating first-order exponential smoothing is as follows: x=(1-α)*x_old+α*x_new (0 < α ≤ 1); Where: x_old is the "smoothing result" of the previous time step; x_new is the observation value at the current time or from another input; The larger α is, the closer it follows new data, meaning more high frequencies are retained; the smaller α is, the smoother the curve, meaning high frequencies are suppressed.

[0012] An application of a method for optimal estimation of motion across multiple frames to determine heading is presented, which is applied to fields such as autonomous driving, intelligent traffic monitoring, and robot navigation.

[0013] The beneficial effects of this invention are: it avoids the jitter error caused by relying on single-point amplitude in traditional methods; it improves the estimation robustness by utilizing whole-frame point cloud information; it constructs a cost function for the sum of point cloud distances, which directly reflects the degree of inter-frame matching; it is simple to implement, has high computational efficiency, and is suitable for real-time systems. Attached Figure Description

[0014] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0015] Figure 1 This is a schematic diagram of the process of the present invention; Figure 2 This is a schematic diagram showing the corresponding position of the radar antenna in the vehicle body coordinate system of the present invention; Figure 3 This is a schematic diagram of angle calculation under the coordinate axes of the present invention; Figure 4 This is a schematic diagram of the computer interface for performance testing according to the present invention; Figure 5 This is a schematic diagram of two heading calculation methods of the present invention. Detailed Implementation

[0016] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below.

[0017] Example 1: like Figures 1 to 3 As shown, a method for optimal motion estimation and heading determination across multiple frames is described, and its specific steps are as follows: S1. Data Acquisition and Preprocessing: The monitoring area is continuously scanned using a millimeter-wave radar sensor to acquire multiple consecutive frames of point cloud data. Each frame of point cloud is preprocessed to extract the target point cloud set. S2. Assuming a uniform motion model: Assuming the target moves at a constant linear velocity between adjacent frames, meaning its speed and direction remain unchanged over short periods, the motion of the target point cloud between frames can be modeled as a global translation, with the translation amount being (dx, dy). By estimating the optimal (dx, dy), the motion vector of the target between adjacent frames can be obtained, and then the heading angle of the target can be calculated. S3. Construct the cost function: Construct a method for evaluating different The cost function for combined accuracy uses the sum of the distances between two point clouds as the cost function; S4. Solving for the optimal displacement: Within a predefined search range, find the optimal displacement (dx, dy) that minimizes the cost function; S5. Heading Angle Calculation: Calculate the target heading angle based on the optimal displacement.

[0018] Each point in the point cloud data in step S1 contains distance, angle, and intensity information.

[0019] The preprocessing in step S1 includes noise removal, filtering, and clustering operations to extract the target point cloud. The purpose of preprocessing is to reduce the impact of environmental clutter and noise on subsequent processing and improve the quality and accuracy of the target point cloud.

[0020] In step S3, the cost function is specifically as follows: After translating the point cloud of frame t+1 by (dx, dy), calculate the sum of the nearest neighbor distances between it and the point cloud of frame t: ; in, and These are the target point cloud sets for frame t and frame t+1, respectively.

[0021] In step S4, the optimal displacement is solved using a traversal search or optimization algorithm.

[0022] In step S5, the formula for calculating the heading angle is: , The heading angle indicates the target's orientation relative to the radar and can be used for target tracking and behavior analysis.

[0023] like Figure 2 and Figure 3 As shown in the figure, this diagram shows the corresponding positions of points measured by the angle radar in the vehicle body coordinate system and the radar antenna. The front is the positive X-axis, the left is the positive Y-axis, 0 degrees is directly in front, -30 degrees is 30 degrees to the right of directly in front, and +30 degrees is 30 degrees to the left of directly in front. The formula for calculating angles on the coordinate axes is: .

[0024] Example 2: A system for optimal motion estimation and heading determination across multiple frames includes: Millimeter-wave radar sensors are used to acquire continuous multi-frame point cloud data; The preprocessing module is used to filter, cluster, and process point cloud data on a cluster basis. The cost function construction module is used to construct the cost function for the sum of distances between two frames of point clouds; The optimal displacement solution module is used to find the optimal displacement that minimizes the cost function within a preset search range. The heading angle calculation module is used to calculate the target heading angle based on the optimal displacement. The multi-frame fusion module is used to perform first-order exponential smoothing on the optimal displacement of multiple consecutive frames to improve the stability of heading estimation.

[0025] The input to the preprocessing module is the point cloud information acquired by the millimeter-wave radar sensor in the current frame, including information such as distance, velocity, angle, and signal-to-noise ratio of the point cloud; the output is the cluster information of the point cloud, including the cluster number and the position and heading of the cluster. The following is the processing content of the preprocessing module: Point cloud removal: Remove points with low energy within a certain distance and points on the car's bumper; Point cloud clustering: grouping points in a point cloud with similar attributes into a cluster based on three dimensions: distance, velocity, and angle; Processing of point clouds on clusters: Filter out clusters with very low height and a number of clusters less than a certain threshold.

[0026] The cost function module takes as input the point cloud information acquired by the millimeter-wave radar sensor in the current frame, including information such as the distance, velocity, angle, and signal-to-noise ratio of the point cloud; and outputs the cost function calculated from the input parameters. Using two frames of point cloud data acquired by radar, the cluster information matched between the two consecutive frames of the target is calculated. After shifting the point cloud of frame t+1 by (dx, dy), the sum of the nearest neighbor distances between it and the point cloud of frame t is calculated. The cost function formula used is: .

[0027] The optimal quantity solution module takes as input the point cloud information acquired by the millimeter-wave radar sensor in the current frame, including information such as the distance, velocity, angle, and signal-to-noise ratio of the point cloud; and outputs the position offset that minimizes the cost function. The point cloud within the matched clusters traversed within a 2-meter radius centered on the target origin makes the cost function... The minimum value is (dx, dy), which is taken as the optimal inter-frame displacement.

[0028] The formula for calculating first-order exponential smoothing is as follows: x=(1-α)*x_old+α*x_new (0 < α ≤ 1); Where: x_old is the "smoothing result" of the previous time step; x_new is the observation value at the current time or from another input; The larger α is, the closer it follows new data, meaning more high frequencies are retained; the smaller α is, the smoother the curve, meaning high frequencies are suppressed.

[0029] It avoids the jitter error caused by relying on single-point amplitude in traditional methods; it utilizes whole-frame point cloud information to improve estimation robustness; it constructs a cost function for the sum of point cloud distances to directly reflect the degree of inter-frame matching; it is simple to implement, computationally efficient, and suitable for real-time systems.

[0030] Example 3: An application of a method for optimal estimation of motion between multiple frames to determine heading. This method is applicable to fields such as autonomous driving, intelligent traffic monitoring, and robot navigation.

[0031] For example, in autonomous driving systems, accurately estimating the heading angles of surrounding vehicles or pedestrians is crucial for path planning and collision warning. The method of this invention can improve the accuracy and reliability of heading estimation, thereby enhancing the safety and intelligence of autonomous driving systems.

[0032] Example 4: Performance Verification The method of this invention was verified through actual road tests and simulation experiments, such as... Figure 4 and Figure 5 As shown, in a scenario where the vehicle is stationary and a vehicle is approaching from behind, the heading is calculated using ① the point with the highest energy in a single frame and ② the heading is calculated using the optimal displacement in this description. The results show that, compared with the traditional method based on single-point amplitude, this invention can more accurately estimate the heading angle of the target when there is jitter at the target reflection point and limited angular accuracy, and has better robustness and adaptability.

[0033] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely prisms of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A method for optimal estimation of motion across multiple frames to determine heading, characterized in that: The specific steps are as follows: S1. Data Acquisition and Preprocessing: Acquire multiple consecutive frames of millimeter-wave radar point cloud data, preprocess each frame of point cloud, and extract the target point cloud set; S2. Assuming a uniform motion model: Assuming the target moves at a constant velocity in a straight line between adjacent frames, the target point cloud as a whole undergoes translation on the plane, with the inter-frame displacement being... ; S3. Construct the cost function: Construct a method for evaluating different The cost function for combined accuracy uses the sum of the distances between two point clouds as the cost function; S4. Solving for the optimal displacement: Within a predefined search range, find the optimal displacement (dx, dy) that minimizes the cost function; S5. Heading Angle Calculation: Calculate the target heading angle based on the optimal displacement.

2. The method for optimal estimation of motion across multiple frames to determine heading according to claim 1, characterized in that: Each point in the point cloud data in step S1 contains distance, angle, and intensity information.

3. The method for optimal estimation of motion across multiple frames to determine heading according to claim 1, characterized in that: The preprocessing in step S1 includes noise removal, filtering, and clustering operations.

4. The method for optimal estimation of motion between multiple frames to determine heading according to claim 1, characterized in that: In step S3, the cost function is specifically as follows: After translating the point cloud of frame t+1 by (dx, dy), calculate the sum of the nearest neighbor distances between it and the point cloud of frame t: ； in, and These are the target point cloud sets for frame t and frame t+1, respectively.

5. The method for optimal estimation of motion across multiple frames to determine heading according to claim 1, characterized in that: In step S4, the optimal displacement is solved using a traversal search or optimization algorithm.

6. The method for optimal estimation of motion across multiple frames to determine heading according to claim 1, characterized in that: In step S5, the formula for calculating the heading angle is: 。 7. A heading system for optimal estimation of motion across multiple frames using any one of claims 1 to 6, characterized in that: include: Millimeter-wave radar sensors are used to acquire continuous multi-frame point cloud data; The preprocessing module is used to filter, cluster, and process point cloud data on a cluster basis. The cost function construction module is used to construct the cost function for the sum of distances between two frames of point clouds; The optimal displacement solution module is used to find the optimal displacement that minimizes the cost function within a preset search range. The heading angle calculation module is used to calculate the target heading angle based on the optimal displacement. The multi-frame fusion module is used to perform first-order exponential smoothing on the optimal displacement of multiple consecutive frames to improve the stability of heading estimation.

8. The system for optimal estimation of motion between multiple frames and determining heading according to claim 7, characterized in that: The formula for calculating first-order exponential smoothing is as follows: x=(1-α)*x_old+α*x_new (0 < α ≤ 1) Where: x_old is the "smoothing result" of the previous time step; x_new is the observation value at the current time or from another input; The larger α is, the closer it follows new data, meaning more high frequencies are retained; the smaller α is, the smoother the curve, meaning high frequencies are suppressed.

9. An application of the method for optimal estimation of motion between multiple frames using any one of claims 1 to 6, characterized in that: This method can be applied to fields such as autonomous driving, intelligent traffic monitoring, and robot navigation.

Images