A vehicle-mounted radar target heading angle filtering tracking method and system

Abstract

The application provides a vehicle-mounted radar target heading angle filtering tracking method and system, the method comprises the following steps: S1: converting the radial velocity of the clustered point cloud into absolute radial velocity relative to the earth; S2: when a new track is created, an independent heading angle extended Kalman filtering tracking module is initialized synchronously; S3: based on the heading angle estimation value output by the module, the projection direction is set, and the data association between the point cloud and the track and the track state updating are completed through projection; S4: based on the updated track state, the position change amount is extracted, and combined with the absolute radial velocity of the associated point cloud, the observation quantity is formed and input into the heading angle module for extended Kalman filtering updating, and the updated heading angle estimation value is output and fed back to the step S3 of the next frame. Through the construction of the cooperative closed loop of the heading angle estimation and the data association, the stability of the heading angle estimation and the overall tracking robustness are significantly improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle-mounted radar, and particularly relates to a vehicle-mounted radar target heading angle filtering tracking method and system. BACKGROUND

[0002] In a target tracking system of a millimeter wave radar, accurate calculation of a heading angle is a core technical basis for realizing stable perception and decision. The heading angle not only determines the judgment of the target motion direction, but also directly affects the accuracy of point cloud association, trajectory prediction, contour fitting and behavior intention understanding. However, the current mainstream heading angle calculation schemes all have significant defects in engineering application, which limits the tracking performance and makes it difficult to meet the needs of high reliability scenarios, especially automatic driving.

[0003] The current widely used heading angle calculation methods mainly include a direct method based on the included angle of the tracking post-velocity and a least square fitting method based on point cloud density. The direct method extracts the longitudinal velocity VX and the transverse velocity VY in the target geodetic coordinate system output by the tracking filter, and calculates the included angle to obtain the heading angle. This method is simple in logic, but in actual application, it is severely dependent on the accuracy of the front-end tracking filter. Due to the sparse and flickering characteristics of millimeter wave radar point clouds, the position and number of strong reflection points on the target will change dramatically with the slight change of the relative radar viewing angle, which causes the filtered velocity state quantity, especially the transverse velocity VY, to have large noise and fluctuations. Therefore, when the target moves at low speed or performs high maneuvering actions such as sharp turns and U-turns, the heading angle calculated by this method will appear severe jitter, and cannot truly reflect the motion trend of the target, even if post-processing smoothing is performed, a non-negligible lag effect will be introduced.

[0004] The least square fitting method based on point cloud density attempts to bypass the error of the tracking filter, and directly uses the geometric distribution of a single frame of point cloud to estimate the target heading. This method first clusters the point cloud, and then performs linear fitting on the points in the same target cluster, and takes the main direction of the fitting as the heading angle. However, the physical characteristics of the millimeter wave radar determine that its point cloud data is extremely sparse, and usually only a few strong reflection points such as vehicle bumpers and license plates can be captured, and dense point clouds covering the surface of the object cannot be obtained like laser radars. Using these sparse, discrete and unstable distributed points for least square fitting is a typical ill-posed problem, and the solution is extremely sensitive to the position of the points. Any slight change in the distribution of the point cloud, such as the disappearance or appearance of a strong reflection point, can cause the fitted heading angle to jump, making the output extremely unstable.

[0005] The root cause of these technical defects lies in the fundamental mismatch between the method and the data characteristics. The weakness of the direct method lies in the excessive dependence on the front-end data, which transmits the instantaneous flicker noise of the point cloud to the heading angle output; while the least square method is limited by insufficient information, trying to solve a strong modeling problem with insufficient geometric information. The shortcomings of these two methods will be further magnified in complex real road scenes. The inaccuracy of the heading angle will trigger a series of serious chain problems, including the wrong association of the point cloud and the track, which may cause the track to break or produce a ghost target; the misjudgment of the target contour and size, which affects the vehicle classification and collision risk assessment; and the failure of lateral motion estimation, which makes the system unable to correctly identify the lane changing and turning intention of the vehicle, posing a serious safety hazard.

[0006] There are also some attempts to improve in the prior art, such as introducing vehicle shape prior knowledge to assist point cloud clustering and heading angle constraint. Related patent documents disclose a point cloud clustering method based on elliptical wave door, which uses the heading angle and vehicle type classification provided by the track history information to adjust the orientation and size of the clustering wave door. This method recognizes the importance of the heading angle in the clustering link, but its effect is still based on the premise that the existing heading angle is relatively accurate. If the input heading angle itself has severe fluctuations or deviations, then the clustering model based on this will not only fail to correct the error, but may also fall into a vicious cycle of error amplification, making it difficult to fundamentally solve the robustness problem of heading angle estimation. SUMMARY

[0007] The technical problem to be solved by the present application is that in view of the technical problems existing in the prior art, the present application provides a vehicle-mounted radar target heading angle filtering and tracking method and system, which can effectively fuse space-time information and resist point cloud noise, thereby laying a solid foundation for high-reliability target tracking and behavior prediction.

[0008] To solve the above technical problems, the technical solution provided by the present application is:

[0009] A vehicle-mounted radar target heading angle filtering and tracking method, comprising the following steps:

[0010] Step S1: converting the radial velocity of the clustered point cloud combined with the vehicle's own motion state into absolute radial velocity relative to the earth;

[0011] Step S2: when creating a new track, an independent heading angle extended Kalman filter tracking module is created and initialized simultaneously, which is used for tracking and estimating the target heading angle and the target heading angle change rate;

[0012] Step S3: obtaining a heading angle estimation value output by the heading angle extended Kalman filter tracking module, setting a projection direction of the point cloud and the track data association based on the heading angle estimation value, processing the radar point cloud data of the current frame along the projection direction to determine the point cloud matched with each track; filtering and updating the state of each track using the matched point cloud to obtain an updated track state;

[0013] Step S4: extracting a position change amount based on the updated track state, and generating an observation value by combining the absolute radial velocity of the current associated point cloud, inputting the observation value into the heading angle extended Kalman filter tracking module to perform extended Kalman filter updating, and outputting an updated target heading angle estimation value; taking the updated target heading angle estimation value as the input of the next frame to perform step S3.

[0014] As a further improvement of the method of the application: in step S4, the observation value includes the longitudinal position change amount and the transverse position change amount between adjacent two frames extracted based on the updated track state, and the absolute radial velocity of the current associated point cloud; and the observation value does not include the longitudinal velocity and the transverse velocity output after the track state filtering and updating.

[0015] As a further improvement of the method of the application: the longitudinal position change amount and the transverse position change amount are obtained according to the updated track state:

[0016]

[0017]

[0018] The observation vector is:

[0019]

[0020]

[0021] wherein, represents the observation vector at time k, represents the position state estimation value in the longitudinal direction after the filtering and updating of the previous frame, represents the position state estimation value in the transverse direction after the filtering and updating of the previous frame, represents the position state estimation value in the longitudinal direction after the filtering and updating of the current frame, represents the position state estimation value in the transverse direction after the filtering and updating of the current frame, represents the longitudinal position change of the target between the previous frame and the current frame, represents the transverse position change of the target between the previous frame and the current frame, represents the absolute velocity of the target relative to the earth, represents the absolute radial velocity.

[0022] As a further improvement of the method of the present application: the performing extended Kalman filter update comprises:

[0023] According to the state transition matrix Predicting the state quantity of the heading angle extended Kalman filter tracking module to obtain a predicted state quantity and a predicted error covariance;

[0024] According to the predicted state quantity, calculating a measurement Jacobian matrix H, the measurement Jacobian matrix H being used to linearize the nonlinear relationship between the measurement and the state quantity;

[0025] Calculating a Kalman gain K, and updating the predicted state quantity using the measurement to obtain an updated state quantity and an error covariance.

[0026] As a further improvement of the method of the present application: in step S3, the processing of the radar point cloud data of the current frame along the projection direction to determine the point cloud matched with each track comprises:

[0027] According to the current heading angle estimation value output by the heading angle extended Kalman filter tracking module, determining a projection region along the heading direction;

[0028] Mapping the point cloud data of the current frame into the projection region, and calculating the matching cost between each point cloud and the predicted position of the track;

[0029] Based on the matching cost, assigning the best matching point cloud to the track to complete the association.

[0030] As a further improvement of the method of the present application: in step S3, if there is a track that fails to determine the matched point cloud, the heading angle extended Kalman filter tracking module corresponding to the track does not perform the update of step S4, but only predicts according to the internal state transition model, and the predicted heading angle is used for the processing of the next period.

[0031] As a further improvement of the method of the present application: the life cycle of the heading angle extended Kalman filter tracking module is bound with the track to which the heading angle extended Kalman filter tracking module is attached; if the track is revoked due to the failure of association to the point cloud for continuous multiple times, the heading angle extended Kalman filter tracking module corresponding to the track is revoked synchronously.

[0032] As a further improvement of the method of the present application: in step S2, the initializing the independent heading angle extended Kalman filter tracking module comprises initializing the target absolute velocity in the state quantity of the heading angle extended Kalman filter tracking module:

[0033]

[0034] wherein, represents the absolute radial velocity of the associated point cloud relative to the earth, represents the azimuth angle of the associated point cloud, represents the preset initial heading angle.

[0035] As a further improvement of the method of the application: in step S1, the calculation formula of the absolute radial velocity relative to the earth is:

[0036]

[0037] wherein, represents the absolute radial velocity, represents the radial velocity of the point cloud relative to the radar, represents the vehicle speed, and theta represents the azimuth angle of the point cloud detection, and beta represents the angle between the vehicle motion direction and the axis direction of the radar coordinate system.

[0038] The application also provides a vehicle-mounted radar target heading angle filtering and tracking system, comprising a microprocessor and a memory connected to each other, and the microprocessor is programmed or configured to execute the vehicle-mounted radar target heading angle filtering and tracking method.

[0039] Compared with the prior art, the application has the beneficial effects that:

[0040] 1. The application introduces an independent heading angle extended Kalman filtering and tracking module, and uses the filtered position change and absolute radial velocity between two adjacent frames as observation, thereby abandoning the direct dependence on unstable velocity state and getting rid of the requirement for single-frame point cloud density. The extended Kalman filtering performs recursive estimation and smoothing on the heading angle in the time dimension, effectively suppresses the high-frequency noise introduced by point cloud flickering and tracking jitter, and thus can output stable, smooth and accurate heading angle estimation under various motion states of the target, thereby significantly enhancing the robustness and adaptability of the algorithm to complex scenes.

[0041] 2. The application realizes the overall leap of the system-level tracking performance by constructing a synergistic enhancement closed loop of "heading angle optimization association and feedback of the associated results to optimize the heading angle". The method directly uses the updated high-quality heading angle estimation value to guide the point cloud data association of the next frame, greatly improves the accuracy of the association by setting an accurate projection direction. And the accurate association results in turn provide more reliable position change and radial velocity observation input for the heading angle filtering module, and then calculate a better heading angle. This closed loop positive feedback mechanism promotes the heading angle accuracy and the data association reliability, and fundamentally enhances the continuity of multi-target tracking, effectively reduces the trajectory breakage and false target phenomenon, and improves the accuracy of target contour and size estimation. BRIEF DESCRIPTION OF DRAWINGS

[0042] Figure 1 This is a flowchart of the vehicle-mounted radar target heading angle filtering tracking method in this embodiment.

[0043] Figure 2 This is a schematic diagram of the radial velocity conversion relationship of the clustered point traces in this embodiment.

[0044] Figure 3 This is a schematic diagram illustrating the calculation of the target velocity in this embodiment.

[0045] Figure 4 This is a flowchart of the heading angle tracking process in this embodiment.

[0046] Figure 5 This is a diagram of the vehicle-mounted radar trajectory processing system architecture in this embodiment. Detailed Implementation

[0047] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0048] like Figure 1 As shown, this embodiment provides a vehicle-mounted radar target heading angle filtering tracking method, including the following steps:

[0049] Step S1: Combine the radial velocity of the clustered point cloud with the vehicle's own motion state to convert it into an absolute radial velocity relative to the ground.

[0050] In this embodiment, the radial velocity of the point cloud in each cluster is converted into a radial velocity relative to the ground by traversing the clustered point cloud tracking list. The formula for calculating the absolute radial velocity relative to the ground is as follows:

[0051] (1)

[0052] in, Indicates absolute radial velocity. This represents the radial velocity of the point cloud relative to the radar. θ represents the vehicle's own speed, θ is the azimuth angle detected by the point cloud, and β is the angle between the vehicle's own motion direction and the axis of the radar coordinate system.

[0053] like Figure 2 As shown, this embodiment reveals the core geometric relationships upon which the radial velocity conversion of a dot pattern depends. Among them, the vector... The resultant velocity of the target relative to the ground, its direction being determined by the heading angle. the definition, while the dynamics of the heading angle are given by is described. This model explicitly maps the radar observations, the ego-motion state and the true motion parameters of the target: by compensating the ego-motion, the raw radial velocity measured by the radar can be converted into the absolute radial velocity of the target in the earth frame, which is geometrically the projection of the relative velocity of the target onto the radar line-of-sight direction. This relationship provides a direct physical constraint and measurement model basis for the subsequent extended Kalman filter tracker of the heading angle, which is the key state quantity, and is the core basis for the realization of the stable estimation of the high-level target heading information from the low-level point cloud observations. is described. This model explicitly maps the radar observations, the ego-motion state and the true motion parameters of the target: by compensating the ego-motion, the raw radial velocity measured by the radar can be converted into the absolute radial velocity of the target in the earth frame, which is geometrically the projection of the relative velocity of the target onto the radar line-of-sight direction. This relationship provides a direct physical constraint and measurement model basis for the subsequent extended Kalman filter tracker of the heading angle, which is the key state quantity, and is the core basis for the realization of the stable estimation of the high-level target heading information from the low-level point cloud observations.

[0054] Step S2: When a new track is created, an independent heading angle extended Kalman filter tracking module is created and initialized synchronously, which is used to track and estimate the target heading angle and the target heading angle rate.

[0055] In this embodiment, the initialization of the heading angle extended Kalman filter tracking module is synchronized with the creation of the new track. Specifically, the system obtains the initial state (including position and velocity information) of the newly created track and the source point cloud information (including radial distance, absolute radial velocity and azimuth angle) used to establish the track by traversing the tracking list. Based on these information, the module completes the initialization setting of the state quantities, including the target absolute velocity, the target heading angle, the target heading angle rate and the corresponding error covariance matrix.

[0056] Further, the initialization calculation of the target absolute velocity follows a specific geometric relationship and is calculated by the following formula:

[0057] (2)

[0058] wherein represents the absolute radial velocity of the associated point cloud relative to the earth at the time of creating the track, is the azimuth angle of the associated point cloud, is the preset initial heading angle.

[0059] In this embodiment, the physical basis of formula (2) is that the projection of the absolute relative velocity of the target in the radial observation direction is the measured absolute radial velocity, from which the magnitude of the relative velocity can be inferred, thereby providing a reasonable initial motion state estimate for the heading angle extended Kalman filter tracking module. On this basis, the motion state of the target can be further decomposed: the transverse absolute velocity component of the target relative to the earth is , and the longitudinal absolute velocity component is , wherein is the target azimuth angle, ​​This is the target heading angle. The heading angle can be simplified during the initialization phase. The geometric relationships between the above physical quantities are as follows: Figure 3 As shown, the spatial projection and decomposition relationship between the target resultant velocity, its lateral and longitudinal components, heading angle, and radial observation direction is clearly demonstrated, establishing an intuitive mathematical model for the initialization of state variables and subsequent filter updates.

[0060] Step S3: Obtain the heading angle estimate currently output by the heading angle extended Kalman filter tracking module, set the projection direction associated with the point cloud and track data based on the heading angle estimate, process the radar point cloud data of the current frame along the projection direction to determine the point cloud that matches each track, and use the matching point cloud to filter and update the state of each track to obtain the updated track state.

[0061] In this embodiment, a logical method is used to transform the track header into a steady-state track during the initial stage of the track. For established steady-state tracks, the data association process relies on heading angle information for optimization. Specifically, the radar point cloud data of the current frame is processed along the projection direction to determine the point cloud matching each track, including the following steps:

[0062] Step 1: Determine the projection area along the heading based on the current heading angle estimate output by the heading angle extended Kalman filter tracking module;

[0063] Step 2: Map the point cloud data of the current frame to the projection area and calculate the matching cost between each point cloud and the predicted position of the track;

[0064] Step 3: Assign the best matching point cloud to the track based on the matching cost to complete the association.

[0065] Step S4: Extract the position change based on the updated track state, and generate an observation by combining it with the absolute radial velocity of the current associated point cloud. Input the observation to the heading angle extended Kalman filter tracking module to perform extended Kalman filter update, and output the updated target heading angle estimate. Use the updated target heading angle estimate as the input for the next frame to execute step S3.

[0066] In this embodiment, the observations include the longitudinal and lateral position changes between two adjacent frames extracted based on the updated track state, as well as the absolute radial velocity of the currently associated point cloud; and the observations do not include the longitudinal and lateral velocities output after the track state filtering update.

[0067] In this embodiment, at any time k, the filter state value of the previous frame of the trajectory is extracted. , and the filter state of the current frame , and an absolute radial velocity of the point cloud on the current association , the observation vector is calculated as:

[0068] (3)

[0069] (4)

[0070] The longitudinal position change and the lateral position change are obtained according to the updated track state:

[0071] (5)

[0072] (6)

[0073] wherein, represents the observation vector at the kth moment, represents the position state estimation value in the longitudinal direction after the filter update of the previous frame, represents the position state estimation value in the lateral direction after the filter update of the previous frame, represents the position state estimation value in the longitudinal direction after the filter update of the current frame, represents the position state estimation value in the lateral direction after the filter update of the current frame, represents the longitudinal position change of the target between the previous frame and the next frame, represents the lateral position change of the target between the previous frame and the next frame, represents the absolute velocity of the target relative to the earth, represents the absolute radial velocity.

[0074] In the embodiment, the extended Kalman filter update includes:

[0075] According to the state transition matrix , the state quantity of the heading angle extended Kalman filter tracking module is predicted for one step to obtain the predicted state quantity and the prediction error covariance ; wherein is a process noise matrix, wherein is a radar update period, generally 50 ms.

[0076] According to the predicted state quantity, the measurement Jacobian matrix is calculated, and the measurement Jacobian matrix H is used to linearize the nonlinear relationship between the observation quantity and the state quantity;

[0077] The Kalman gain is calculated, and the observation quantity is used to update the predicted state quantity to obtain the updated state quantity and the error covariance, wherein is a measurement error noise matrix.

[0078] Then according to the following and Perform filtering updates on the state variables and their error covariance, where I is the identity matrix, a square matrix whose dimension is the dimension of the tracking state variables.

[0079] like Figure 4 As shown, in the process of data association of target tracks in this embodiment, if a track fails to find a matching point cloud, the heading angle extended Kalman filter tracking module corresponding to the track will not perform the update in step S4, but will only make a prediction based on the internal state transition model and use the predicted heading angle for the next cycle of processing.

[0080] In this embodiment, the lifecycle of the heading angle extended Kalman filter tracking module is bound to the track to which the heading angle extended Kalman filter tracking module is attached; if the track is canceled due to multiple consecutive failures to be associated with the point cloud, the heading angle extended Kalman filter tracking module corresponding to the track is canceled simultaneously.

[0081] It should be further explained that the heading angle extended Kalman filter tracking module in this embodiment is implemented as a reusable filtering algorithm framework or service. When the system determines that heading angle tracking is needed for a newly created moving target track (e.g., determined to be a vehicle based on its speed or point cloud features), it invokes this service and dynamically allocates and initializes a set of independent tracking resources for that specific track, including a proprietary state vector, error covariance matrix, and related context information. This set of resources constitutes a "heading angle tracking instance" uniquely bound to that track. Therefore, the creation and initialization module mentioned in this text essentially refers to creating and initializing such a tracking instance bound to the track. Accordingly, the lifecycle of this tracking instance is completely synchronized with the track it is attached to: it is instantiated when the track is established, and when the track is revoked due to continuous loss of tracking, the resources occupied by the instance are released, and the tracking process terminates. For stationary targets or targets that do not require heading angle tracking, the system may choose not to create such instances or assign them a default fixed heading angle value.

[0082] At the level of heading angle estimation, the embodiment fundamentally overcomes the dependence on unstable velocity data or dense point clouds of traditional schemes by designing a novel observation input and state model. Specifically, the embodiment takes the longitudinal and lateral displacement difference between adjacent frames, together with the absolute radial velocity of the target, as the observation input of the extended Kalman filter, and recursively estimates the target absolute velocity, heading angle and its rate of change as the internal state. Since the displacement difference has better smoothness than the instantaneous velocity, and the radial velocity itself is a direct observation with high signal-to-noise ratio, the filter model can effectively suppress the high-frequency fluctuations introduced by point cloud flickering and measurement noise, thereby realizing continuous, stable and accurate tracking of the target heading angle. This mechanism makes the heading angle estimation process no longer sensitive to the density and distribution of single-frame point clouds, and still maintains reliable performance in challenging scenarios such as low-speed targets, target maneuvers or sparse point clouds at a distance.

[0083] At the level of tracking process, the embodiment builds an optimized closed loop of heading angle estimation and data association to realize self-enhancement of overall performance. The embodiment directly uses the real-time updated high-precision heading angle estimation value to guide the point cloud-track association in the next cycle, significantly improves the accuracy and efficiency of association by projecting along the heading angle direction to greatly compress the search space. The more accurate association results, in turn, provide cleaner and more reliable displacement and radial velocity observations for the heading angle filter. The "precise heading angle Optimized association Quality observation More accurate heading angle" positive feedback mechanism makes the heading angle precision and data association quality form a virtuous cycle, thereby improving the continuity of multi-target tracking as a whole, effectively reducing track breaks and identity jumps, and enhancing the ability to distinguish target outlines and motion intentions.

[0084] In summary, the method provided by the embodiment realizes the stability and anti-interference of heading angle estimation itself by constructing the heading angle as an independent tracking state and establishing its closed-loop coupling with the association link, and further transforms this local advantage into overall improvement of the tracking performance of the whole process through systematic collaborative design, thereby providing a solid and reliable technical foundation for target motion perception in complex driving environments.

[0085] The embodiment also provides a vehicle-mounted radar target heading angle filtering and tracking system, which comprises a microprocessor and a memory connected to each other, and the microprocessor is programmed or configured to execute the vehicle-mounted radar target heading angle filtering and tracking method.

[0086] As Figure 5The complete signal chain containing multi-stage processing modules in this embodiment is shown. The system takes the vehicle parameters and raw point cloud data acquired by the front-end as input, sequentially performs radar calibration, moving and static separation, and speed conversion, etc. preprocessing steps to obtain the calibrated dynamic point cloud containing absolute speed information. The preprocessed point cloud forms candidate target point cloud clusters through the clustering module.

[0087] The core of the system is a cooperative tracking loop composed of a track filter, a heading angle filter, and a data association module. The clustered point cloud is input into the loop, and the data association module performs matching of the point cloud with existing tracks according to the real-time heading angle estimate provided by the heading angle filter. The matching result drives the track filter to update the position and speed state of the target on one hand, and provides observation input for the heading angle filter, enabling it to recursively estimate and optimize the heading angle combined with the position change of the track. The heading angle filter and the track filter form a two-way feedback through data association, forming an enhanced closed loop of heading angle-assisted association and the result of the association feeding back to the heading angle estimation.

[0088] Finally, the track management module maintains the track output by the tracking loop throughout its life cycle, completes the starting, confirmation, fusion, and cancellation logic of the track, and outputs a target list containing accurate position, speed, and heading angle information for use by the upper-layer automatic driving function. The entire system realizes efficient and robust conversion from raw radar data to stable target state.

[0089] The above is only the preferred embodiment of the present application, and does not limit the present application in any form. Although the present application has been disclosed as above with the preferred embodiment, it is not intended to limit the present application. Therefore, any simple modification, equivalent change and modification of the above embodiment without departing from the technical solution of the present application, according to the technical essence of the present application, shall fall within the scope of protection of the technical solution of the present application.

Claims

1. A method for filtering and tracking the heading angle of a target by a vehicular radar, characterized by, The method comprises the following steps: Step S1: combine the radial velocity of the clustered point cloud with the motion state of the vehicle to convert the radial velocity into absolute radial velocity relative to the earth; Step S2: when a new track is created, an independent heading angle extended Kalman filter tracking module is created and initialized synchronously to track the target heading angle and the change rate of the target heading angle; Step S3: obtain the heading angle estimation value output by the heading angle extended Kalman filter tracking module, set the projection direction of the point cloud and track data association based on the heading angle estimation value, process the radar point cloud data of the current frame along the projection direction to determine the point cloud matched with each track, and filter and update the state of each track by using the matched point cloud to obtain the updated track state; Step S4: extract the position change based on the updated track state, generate an observation quantity by combining the absolute radial velocity of the current associated point cloud, input the observation quantity into the heading angle extended Kalman filter tracking module to perform extended Kalman filter updating, and output the updated target heading angle estimation value; and use the updated target heading angle estimation value as the input of the next frame to perform step S3. The observation quantity comprises the longitudinal position change and the lateral position change between adjacent two frames extracted based on the updated track state, and the absolute radial velocity of the current associated point cloud; and the observation quantity does not contain the longitudinal velocity and the lateral velocity output after the track state filter updating.

2. The vehicular radar target heading angle filter tracking method according to claim 1, characterized by, The longitudinal position change and the lateral position change are obtained according to the updated track state. The observation vector is: wherein, represents the observation vector at time k, represents the position state estimation value in the longitudinal direction after the filter update of the previous frame, represents the position state estimation value in the lateral direction after the filter update of the previous frame, represents the position state estimation value in the longitudinal direction after the filter update of the current frame, represents the position state estimation value in the lateral direction after the filter update of the current frame, represents the longitudinal position change of the target between the previous frame and the current frame, represents the lateral position change of the target between the previous frame and the current frame, represents the absolute velocity of the target relative to the earth, represents the absolute radial velocity.

3. The vehicular radar target heading angle filter tracking method according to claim 1, wherein, The extended Kalman filter updating comprises: According to the state transition matrix The state quantity of the heading angle extended Kalman filter tracking module is predicted to obtain a predicted state quantity and a predicted error covariance. calculating a measurement Jacobian matrix H according to the predicted state quantity, the measurement Jacobian matrix H being used to linearize the nonlinear relationship between the observation quantity and the state quantity; calculating a Kalman gain K, and updating the predicted state quantity by using the observation quantity to obtain an updated state quantity and an error covariance.

4. The vehicular radar target heading angle filter tracking method according to claim 1, wherein, In step S3, the processing of the radar point cloud data of the current frame along the projection direction to determine the point cloud matched with each track specifically comprises: determining a projection region along the heading direction according to the current heading angle estimation value output by the heading angle extended Kalman filter tracking module; mapping the point cloud data of the current frame into the projection region, and calculating the matching cost between each point cloud and the predicted position of the track; assigning the best matching point cloud to the track based on the matching cost to complete the association.

5. The vehicular radar target heading angle filter tracking method according to claim 1, wherein, In step S3, if there is a track that fails to determine the matched point cloud, the heading angle extended Kalman filter tracking module corresponding to the track does not perform the updating of step S4, but only performs prediction according to the internal state transition model, and uses the predicted heading angle for the processing of the next period.

6. The vehicular radar target heading angle filter tracking method according to claim 1, wherein The life cycle of the heading angle extended Kalman filter tracking module is bound to the track to which the heading angle extended Kalman filter tracking module is attached; if a track is revoked because it fails to associate with the point cloud for a plurality of times, the heading angle extended Kalman filter tracking module corresponding to the track is revoked synchronously.

7. The vehicular radar target heading angle filter tracking method according to claim 1, wherein, In step S2, the initialization of the independent heading angle extended Kalman filter tracking module comprises initializing a target absolute velocity in a state quantity of the heading angle extended Kalman filter tracking module: wherein, represents the absolute radial velocity of the associated point cloud relative to the ground at the time of creating the track, is the azimuth angle of the associated point cloud, is a preset initial heading angle.

8. The vehicular radar target heading angle filter tracking method according to claim 1, wherein, In step S1, the calculation formula of the absolute radial velocity relative to the earth is: wherein, denotes the absolute radial velocity, denotes the radial velocity of the point cloud relative to the radar, denotes the vehicle's own speed, θ is the azimuth angle of the point cloud detection, and β is the angle between the vehicle's own movement direction and the axial direction of the radar coordinate system.

9. A vehicular radar target heading angle filter tracking system comprising a microprocessor and memory interconnected, characterized by, The microprocessor is programmed or configured to perform the vehicle-mounted radar target heading angle filter tracking method according to any one of claims 1-8.

Images