A method for continuously tracking the motion trajectory of a vehicle

Abstract

The present invention discloses a method for continuously tracking a vehicle motion trajectory, which mainly includes four stages: perception of target motion information, construction of short trajectory segments, association of short trajectory segments, and reconstruction of interrupted target trajectories. First, a millimeter-wave radar installed above the detection road is used to perceive the position and speed information of traffic targets within the detection range. Reliable short trajectory segments are formed by connecting adjacent-frame targets according to the radar detection results. Then, a fuzzy correlation function is constructed based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, and the short trajectories are connected into long trajectories. Finally, the gaps in the new and old trajectories that satisfy the association relationship are reconstructed based on the cubic Hermite interpolation method to achieve continuous tracking of the vehicle motion trajectory. The present invention can not only improve the accuracy of target number estimation, but also solve problems such as trajectory breakage and multiple trajectories for a single target caused by incorrect association in traditional algorithms.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent transportation, and particularly relates to a method for continuously tracking the motion trajectory of a vehicle. Background Art

[0002] As a sensor that can work all-weather, the millimeter-wave radar has strong anti-interference ability and high detection accuracy, can well adapt to various different scenarios, and is not affected by external factors such as weather and light. The millimeter-wave radar was initially used in military applications such as low-angle tracking, precision guidance, measurement, and imaging. Thanks to the development of intelligent transportation systems, the millimeter-wave radar has been widely used in the field of intelligent transportation in recent years. By installing the millimeter-wave radar at intersections or roadside, full-coverage detection and tracking of traffic targets can be achieved by adjusting the installation position, and vehicle driving trajectory data in urban roads can be obtained. These trajectory data contain a large amount of traffic information, providing basic data support for subsequent traffic parameter extraction, traffic anomaly event monitoring, traffic state estimation, etc.

[0003] Currently, the technologies for collecting vehicle trajectories mainly include vision-based sensing technologies, lidar-based sensing technologies, and millimeter-wave radar-based sensing technologies. The vision-based sensing and collection technology was first applied to trajectory tracking. A video multi-object tracking method based on multi-dimensional feature fusion (201910964726) obtains the moving direction and predicted position through the analysis of the target's historical trajectory, and combines the content feature similarity between targets for matching and connection. However, since the camera is easily affected by external factors, it will cause image blurring and result in poor trajectory tracking effect. With the emergence of autonomous driving, radar technology has gradually become an advanced technology direction in the field of vehicle trajectory tracking. The lidar-based sensing and collection technology mainly detects moving targets to obtain three-dimensional point cloud data containing motion information, and realizes three-dimensional high-precision identification and detection of objects. A dynamic multi-object tracking method based on lidar (202110467582) establishes different motion models for objects with different motion characteristics respectively. However, the lidar will be more severely attenuated under atmospheric conditions such as thick smoke and fog or bad weather, and the propagation distance is limited. As a sensor that can work all-weather, the millimeter-wave radar has strong anti-interference ability and high detection accuracy, has weak attenuation under the influence of the atmospheric environment, strong penetrability to smoke, can realize long-distance sensing and detection, solves the problem of lidar attenuation, and has significant advantages in the field of vehicle trajectory tracking. Summary of the Invention

[0004] The object of the present invention is to address the difficulties and deficiencies in multi-object tracking in actual traffic scenarios, and a multi-stage object tracking method for short vehicle motion trajectory segments based on millimeter-wave radar point cloud is proposed. First, adjacent frame objects are connected into reliable short trajectory segments according to the radar parsing results, then a fuzzy correlation function is constructed based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, and the short trajectories are connected into long trajectories. Finally, the trajectory pairs that meet the association relationship are connected based on the cubic Hermite interpolation method to achieve continuous tracking of traffic objects. The present invention can solve problems such as trajectory breakage caused by incorrect association and multiple trajectories for a single object, and achieve continuous tracking of the motion trajectories of traffic objects.

[0005] The present invention is realized by at least one of the following technical solutions.

[0006] A method for continuous tracking of vehicle motion trajectories includes the following steps:

[0007] S1. Obtain the original point cloud data of traffic objects within the detection range using a millimeter-wave radar to obtain radar detection data, and each point represents a detected traffic object;

[0008] S2. Extract the position and speed characteristics of traffic objects based on the radar detection data, generate and terminate the traffic object trajectories by associating the measurement data of two consecutive frames, connect adjacent frame objects into short trajectory segments, and correct the association results to complete the construction of initial small trajectory segments;

[0009] S3. Construct a fuzzy correlation function based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, calculate the similarity between trajectories to obtain a cost matrix, and solve the short trajectory segment association problem as an assignment problem;

[0010] S4. For the associated old and new trajectory pairs with discontinuities, connect the interrupted trajectories that meet the association relationship, estimate the data corresponding to the discontinuity moments according to the state values corresponding to the known moments of the trajectories, and fill the gaps between the trajectory segments to complete the continuous tracking of the target trajectories.

[0011] Further, according to the method for continuous tracking of vehicle motion trajectories based on millimeter-wave radar point cloud described in claim 1, the point cloud data information detected by the millimeter-wave radar in step S1 includes the lateral distance, longitudinal distance of the traffic object relative to the radar installation position, as well as the lateral position, longitudinal position, lateral speed, longitudinal speed and timestamp of the point.

[0012] Further, the construction of the short trajectory segments in step S2 includes:

[0013] S201. When associating adjacent frames, select speed and position information for similarity matching calculation. If it exceeds the set threshold θ, the detection responses d i (k - 1) and d j (k) belong to the trajectory sequence of the same target. If they do not belong to the same target, take the detection point of the k-th frame as the starting point of a new trajectory, and continue to pair it with the detection response of the next frame until all the detection responses of all detection sequences are associated;

[0014] S202. Further correct the results of the preliminary association and eliminate the split trajectories generated by the same target at the same time:

[0015] Suppose there are trajectories TR A and TR B . The sampling times of the two trajectories overlap from the i-th frame to the j-th frame. Establish the judgment criterion as shown in Equation (1). If the judgment criterion is satisfied, it is considered that the two trajectories originate from the same target. Then, regard the shorter trajectory as a split trajectory and eliminate it, and the other trajectory corresponds to the original target. And so on, perform multiple eliminations until there are no split trajectories that meet the following conditions within the trajectory sampling time:

[0016]

[0017] In the formula, respectively represent the distances from the k-th frame of the trajectories TR A , TR B to the radar; represents the speeds at the k-th frame of the trajectories TR A , TR B ; ξ R is the distance difference threshold, and ξ v is the speed difference threshold.

[0018] Furthermore, step S3 specifically includes:

[0019] S301. For the motion information of traffic targets, perform association based on the fuzzy strategy of motion prediction. Suppose there are two short trajectories that may be associated, denoted as TR i and TR j . The short trajectory with an earlier time is called the reference trajectory TR i . The starting time of this trajectory is and the ending time is The short trajectory with a later time is the new trajectory TR j . The starting time of this trajectory is and the ending time is Perform forward and backward predictions on the reference trajectory and the new trajectory respectively to and the midpoint time k c . To quantify the midpoint time k of the reference trajectoryc Prediction state and the new trajectory k c The prediction state at time The matching relationship of is calculated using a normal membership function to calculate the correlation degree of two trajectory segments:

[0020]

[0021] In the formula, u m is the m-th fuzzy factor; σ m is the error variance of the m-th fuzzy factor; τ m is the adjustment degree; ε T is the attenuation factor;

[0022]

[0023]

[0024] In the formula, u1 and u2 respectively represent the fuzzy factors of position and velocity; μ1(u1) and μ2(u2) respectively represent the correlation degrees of position and velocity; τ1 and τ2 respectively represent the adjustment degrees of position and velocity; respectively represent the error variances of position and velocity; k c represents and The midpoint time of; respectively represent the estimated values of the longitudinal position and lateral position predicted by the reference trajectory and the new trajectory at k c time; respectively represent the estimated values of the longitudinal velocity and lateral velocity predicted by the reference trajectory and the new trajectory at k c time;

[0025] The reference trajectory TR i and the new trajectory TR j The fuzzy similarity based on position and velocity is expressed as:

[0026] f(TR j |TR i ) = a1μ1(u1) + a2μ2(u2) (5)

[0027] In the formula, a1 and a2 respectively represent the weights corresponding to the position and velocity fuzzy factors;

[0028] S302. Based on formula (5), an association cost matrix is obtained. Based on the small trajectory set T and the fuzzy association function f(·), the Hungarian algorithm is used to select the overall optimal association matching result from multiple association possibilities:

[0029]

[0030] wherein, T = {TR1, TR2,..., TR M} is a set of short trajectories generated by preliminary association; L = {l1, l2,..., l N} is a set of conflict matrices; f(·) is a fuzzy function for associating small trajectories; M is the total number of trajectories in the short trajectory segment set; N is the total number of conflict matrices; TR M represents the Mth short trajectory in the trajectory set; l N represents the association result of the Nth short trajectory.

[0031] Further, in step S3, if the interruption is caused by target occlusion, then perform step S301 to calculate the fuzzy similarity between short trajectories; if the interruption is caused by target stop, then directly calculate the fuzzy similarity according to formula (5) without prediction.

[0032] Further, for the newly and old trajectory pairs that have been associated in step S3, use the cubic Hermite interpolation method to connect and reconstruct the interrupted trajectories that have been successfully associated, estimate the state value at the discontinuous moment based on the known state value of the target, and seek the connection trajectory closest to the true target trajectory to make the target trajectory complete and continuous, thus completing the reconstruction of the target trajectory.

[0033] Further, in step S1, when obtaining the radar detection data, it is necessary to screen the radar targets within a specific recognition range and eliminate the invalid targets:

[0034]

[0035] wherein, p x and p y are respectively the target position attribute components; Y dist is the set lateral range; X max is the maximum longitudinal range that the radar can detect, i.e., the maximum effective distance; X min is the minimum longitudinal range that the radar can detect, i.e., the minimum effective distance.

[0036] Further, in step S2, the association of adjacent frames specifically includes three cases:

[0037] Case 1: When the number of vehicles m in the kth frame is less than the number of vehicles n in the (k - 1)th frame, for the target point tracks that are not paired in the (k - 1)th frame, it is considered that the target may have left the detection area or had an interruption in the (k - 1)th frame, and store it in the short trajectory segment set;

[0038] Case 2: When the number of vehicles m in the kth frame is equal to the number of vehicles n in the (k - 1)th frame, at this time, the traffic targets in the two frames are most likely to be successfully associated with each other, but there may also be a situation where the target in the (k - 1)th frame disappears and a new target appears in the kth frame;

[0039] Case 3: When the number of vehicles m in the k-th frame is greater than the number of vehicles n in the (k - 1)-th frame, it is possible that new targets appear in the k-th frame. For the target point tracks that are not successfully paired in the k-th frame, they are used as the starting points of new tracks and participate in the pairing of the next frame.

[0040] Furthermore, in step S2, the millimeter-wave radar measures the speed and position of the target more accurately, and the samples can form continuous features of adjacent frames. Therefore, the speed and position features can be selected for correlation matching.

[0041] Furthermore, in step S3, the determination of the track segments to be correlated includes:

[0042] Two short tracks to be correlated cannot overlap in the time when they appear respectively; the same moving track cannot belong to multiple targets at the same time. The spatio-temporal constraint condition is the premise for judging whether two short tracks can be correlated. According to the spatio-temporal constraint condition, the conflict matrix L = [l(i, j)] M×M .

[0043]

[0044] In the formula, represents the time of the last frame of the track TR i , represents the time of the starting frame of the track TR j ; represents that there is a time overlap between two small tracks, l(TR j , TR i ) = 0 indicates that the two small tracks cannot be correlated, that is, the two small tracks do not belong to the same target, and l(TR j , TR i ) = 1 indicates that the two small tracks to be correlated originate from the same target.

[0045] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0046] First, the present invention uses a millimeter-wave radar installed on the detection road, makes full use of the returned point cloud data, eliminates the split tracks caused by the large reflection area of some traffic targets, and greatly reduces the number of redundant tracks. It realizes the unification of the traffic target ID tags before and after the interruption, thereby obtaining the continuous tracks of traffic targets within the radar monitoring range. It well makes up for the deficiencies of traditional tracking algorithms in tracking targets in the actual scene and realizes the continuous tracking of the movement tracks of traffic targets.

[0047] Second, the traffic target trajectory continuous tracking method proposed by the present invention does not rely on the video image information required by traditional methods. It can obtain the continuous motion trajectory of traffic targets timely, accurately, and effectively only relying on the point cloud data obtained by millimeter-wave radar. Even under the influence of low light intensity, bad weather, etc., it can have good performance, with low cost and universal applicability. Description of the Drawings

[0048] Figure 1 It is a flowchart of the vehicle motion trajectory continuous tracking method for the millimeter-wave radar point cloud in the embodiment;

[0049] Figure 2 It is a schematic diagram of the relationship between the preliminary association threshold and the number of trajectory segments in the embodiment;

[0050] Figure 3 It is a tracking result diagram of Scenario A in the embodiment;

[0051] Figure 4 It is a tracking result diagram of Scenario B in the embodiment;

[0052] Figure 5 It is a tracking result diagram of Scenario C in the embodiment. Detailed Embodiment

[0053] The present invention will be further described in detail below in conjunction with the embodiments and the drawings. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0054] The present invention relates to a method for continuously tracking the vehicle motion trajectory of millimeter-wave radar point cloud. This method makes full use of the data returned by the millimeter-wave radar. By identifying and detecting moving targets, the state information such as their positions and speeds in each frame is obtained. Then, based on the motion characteristics, the target trajectories are associated, and the target trajectory segments are corrected and connected to complete the reconstruction of the moving target trajectories, realizing the continuous tracking of traffic target trajectories.

[0055] Embodiment 1

[0056] As Figure 1 shown, a method for continuously tracking the vehicle motion trajectory includes the following steps:

[0057] S1. Use the millimeter-wave radar to obtain the original point cloud data of traffic targets within the detection range to obtain radar detection data, and each point represents the detected traffic target; the point cloud data information includes the lateral distance, longitudinal distance of the traffic target relative to the radar installation position, as well as the lateral position, longitudinal position, lateral speed, longitudinal speed, and timestamp of the point.

[0058] In step S1, to obtain the radar detection data, it is necessary to screen the radar targets within a specific recognition range and eliminate the invalid targets.

[0059]

[0060] where p x and p y are the target position attribute components respectively; Y dist is the set lateral range; X max is the maximum longitudinal range that the radar can detect, that is, the maximum effective distance; X min is the minimum longitudinal range that the radar can detect, that is, the minimum effective distance.

[0061] S2. Extract the traffic target position and speed features based on the radar detection data, generate and terminate the traffic target trajectory by correlating the measurement data of two consecutive frames, connect the adjacent frame targets into short trajectory segments, and correct the correlation result to complete the construction of the initial small trajectory segments, specifically as follows:

[0062] S201. When correlating adjacent frames, select the speed and position information to calculate the similarity matching. If the detection responses d i (k - 1) and d j (k) belong to the trajectory sequence of the same target (the dots of adjacent frames belong to the same target trajectory sequence), then their connection probability should be high enough, That is exceeding the set threshold θ; if it exceeds the set threshold, it can be considered that the two dots of adjacent frames can be correlated, and it is determined that the dots of adjacent two frames belong to the same target; if they do not belong to the same target, the detection dot of the k-th frame is used as the starting point of the new trajectory, and continue to pair with the detection response of the next frame until all the detection responses of the detection sequences are correlated;

[0063] S202. Further correct the result of the preliminary correlation to eliminate the split trajectories generated by the same target at the same time:

[0064] Suppose there are trajectories TR A and TR B , the sampling times of the two trajectories overlap from the i-th frame to the j-th frame. Establish the judgment criterion as shown in Equation (1). If the judgment criterion holds, it is considered that the two trajectories originate from the same target, then the shorter trajectory is regarded as the split trajectory and eliminated, and the other trajectory corresponds to the original target (referring to the target that generates the split trajectory. The purpose of this step is to eliminate the split trajectory), and so on, perform multiple eliminations until there are no split trajectories that meet the following conditions within the trajectory sampling time:

[0065]

[0066] In the formula, respectively represent the distances from the radar to the k-th frames of the trajectories TR A and TR B ; represents the velocity at the k-th frame of the trajectories TR A and TR B ; ξ R is the distance difference threshold, and ξ v is the velocity difference threshold.

[0067] The association of adjacent frames specifically includes three cases:

[0068] Case 1: When the number of vehicles m in the k-th frame is less than the number of vehicles n in the (k - 1)-th frame, for the target point traces that are not paired in the (k - 1)-th frame, it is considered that the target may have left the detection area or had an interruption in the (k - 1)-th frame, and they are stored in the short trajectory segment set;

[0069] Case 2: When the number of vehicles m in the k-th frame is equal to the number of vehicles n in the (k - 1)-th frame, at this time, it is most likely that the traffic targets in the two frames are all successfully associated with each other, but there may also be a situation where the target in the (k - 1)-th frame disappears and a new target appears in the k-th frame;

[0070] Case 3: When the number of vehicles m in the k-th frame is greater than the number of vehicles n in the (k - 1)-th frame, at this time, a new target may appear in the k-th frame. For the target point traces that are not successfully paired in the k-th frame, they are used as the starting points of new trajectories and participate in the pairing of the next frame.

[0071] Millimeter-wave radar measures the velocity and position of targets more accurately, and the samples can form continuous features of adjacent frames. Therefore, velocity and position features can be selected for association matching.

[0072] S3. Construct a fuzzy correlation function based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, calculate the similarity between the trajectories to obtain the cost matrix, and regard the short trajectory segment association problem as an assignment problem for solution; specifically including:

[0073] S301. For the motion information of traffic targets, perform association based on the fuzzy strategy of motion prediction. Assume that two short trajectories that may be associated are TR i and TR j . The short trajectory with an earlier time is called the reference trajectory TR i , and the starting time of this trajectory is and the ending time is . The short trajectory with a later time is the new trajectory TR j , and the starting time of this trajectory is and the ending time is Perform forward and backward predictions on the reference trajectory and the new trajectory respectively to and The midpoint time k c , which is the midpoint time k of the quantization reference trajectory c Predicted state and the new trajectory k c The predicted state at time For the matching relationship, the normal membership function is used to calculate the correlation degree of two trajectory segments:

[0074]

[0075] In the formula, u m is the m-th fuzzy factor; σ m is the error variance of the m-th fuzzy factor; τ m is the adjustment degree; ε T is the attenuation factor;

[0076]

[0077]

[0078] In the formula, u1 and u2 respectively represent the fuzzy factors of position and velocity; μ1(u1) and μ2(u2) respectively represent the correlation degrees of position and velocity; τ1 and τ2 respectively represent the adjustment degrees of position and velocity; respectively represent the error variances of position and velocity; k c represents and The midpoint time of; respectively represent the estimated values of the longitudinal position and the lateral position of the reference trajectory and the new trajectory predicted to time k c ; respectively represent the estimated values of the longitudinal velocity and the lateral velocity of the reference trajectory and the new trajectory predicted to time k c .

[0079] The reference trajectory TR i and the new trajectory TR j The fuzzy similarity based on position and velocity is expressed as:

[0080] f(TR j |TR i ) = a1μ1(u1) + a2μ2(u2) (5)

[0081] In the formula, a1 and a2 respectively represent the weights corresponding to the position and velocity fuzzy factors.

[0082] S302. Based on formula (5), the association cost matrix can be obtained. Based on the small trajectory set T and the fuzzy association function f(·), the Hungarian algorithm is used to select the overall optimal association matching result among multiple association possibilities:

[0083]

[0084] wherein, T = {TR1, TR2,..., TR M} is a set of short trajectories generated by preliminary association; L = {l1, l2,..., l N} is a set of conflict matrices; f(·) is a fuzzy function for associating small trajectories; M is the total number of trajectories in the set of short trajectory segments;

[0085] For the newly and old trajectory pairs that have been associated, the cubic Hermite interpolation method is used to connect and reconstruct the interrupted trajectories that have been successfully associated, estimate the state values at the discontinuous moments based on the known state values of the target, and seek the connecting trajectory that is closest to the true target trajectory, so as to make the target trajectory complete and continuous and complete the reconstruction of the target trajectory.

[0086] The determination of the trajectory segments to be associated includes:

[0087] Two short trajectories to be associated cannot overlap in the time when they appear respectively; the same moving trajectory cannot belong to multiple targets at the same time. The spatio-temporal constraint condition is the prerequisite for judging whether two short trajectories can be associated. According to the spatio-temporal constraint condition, the conflict matrix L = [l)i,j)] M×M .

[0088]

[0089] wherein, represents the time of the last frame of trajectory TR i ; represents the time of the starting frame of trajectory TR j ; represents that there is a time overlap between two small trajectories, and l(TR j , TR i ) = 0 indicates that the two small trajectories cannot be associated, that is, the two small trajectories do not belong to the same target, and l(TR j , TR i ) = 1 indicates that the two small trajectories to be associated come from the same target.

[0090] For the association of short trajectories, the reasons for the interruption of the traffic target trajectory also need to be considered. If the interruption is caused by target occlusion, then the fuzzy similarity between short trajectories is calculated in step S301; if the interruption is caused by the target stopping, then the fuzzy similarity is directly calculated by formula (5) without prediction.

[0091] S4. For the associated new and old trajectory pairs with discontinuities, connect the interrupted trajectories that meet the association relationship, estimate the data corresponding to the discontinuity moments based on the state values corresponding to the trajectories at known moments, fill the gaps between the trajectory segments, and complete the continuous tracking of the target trajectory.

[0092] The test scenario of the vehicle motion trajectory continuous tracking method based on millimeter-wave radar point cloud in this embodiment specifically includes the following steps:

[0093] First step. The test scenario of this embodiment is a road at an intersection in a certain city. The millimeter-wave radar is installed in the center of the crossbar on the side of an intersection road. Its longitudinal sensing range is 200 m, and the lateral range exceeds the overall width of the lane. The test schematic diagram is as Figure 1 shown. Three groups of experimental scenarios are set according to the different degrees of vehicle density. Scenarios A, B, and C are the scenarios at different time periods on the intersection road. Scenario A tracks both oncoming and outgoing vehicles, and scenarios B and C only track vehicles in one direction. Each scenario includes cars, buses, and electric vehicles with different quantities and proportions.

[0094] Table 1 Description of different test scenarios

[0095]

[0096] In the actual scenario, there is often an angle between the projection of the radar irradiation direction and the lane direction, and the radar may also deviate from the center of the lane. Assume that the angle between the traffic target P and the longitudinal axis of the radar rectangular coordinate system is β, and the angle between the lane coordinate system and the radar coordinate system is α. Since the subsequent processing of radar data is carried out in the lane rectangular coordinate system, after coordinate transformation of the position and velocity information (p, v) measured by the actually installed radar, the obtained (p′, v′) information is:

[0097]

[0098] In the formula, p x , p y are respectively the position components in the observed values of the target; v x , v y are respectively their corresponding velocity components; the angle α between the lane coordinate system and the radar coordinate system is 4.5 degrees; p′ x , p′ y are respectively the target position components after coordinate transformation; v′ x , v′ y are respectively the velocity components after coordinate transformation.

[0099] Second step. Construct initial small trajectory segments based on radar detection data. When performing the association of adjacent frames, select the speed and position information to calculate the similarity matching. If the detection responses d of two adjacent framesi (k - 1) and d j If (k) belongs to the same target trajectory sequence, then their connection probability should be high enough, that is, exceeding the set threshold θ; if the above conditions are not met, it is used as the starting point of a new trajectory and continues to be paired with the detection response of the next frame until all the detection responses of the detection sequences are associated.

[0100] During the process of trajectory association, it may be due to multiple detections of a single target resulting in split trajectories formed at the same time (for example, it may be because the detected target is large in size (such as a bus or a truck), and there may be more than one measurement point returned by the detector), thus causing trajectory interruption. Therefore, it is necessary to correct the association result after the initial trajectory association and eliminate the split trajectories generated by the same target at the same time. Suppose there are trajectories TR A and TR B , and the sampling times of the two trajectories overlap from the i-th frame to the j-th frame. The following judgment criterion is established. If the judgment criterion holds, the shorter trajectory is regarded as a split trajectory and eliminated, and the other trajectory corresponds to a target. By analogy, multiple eliminations are performed until there are no split trajectories that meet the conditions within the trajectory sampling time.

[0101]

[0102] In the formula, respectively represent the distances from the k-th frame of trajectories TR A and TR B to the radar; represents the speed at the k-th frame of trajectories TR A and TR B ; ξ R is the distance difference threshold, and ξ v is the speed difference threshold.

[0103] Step 3: Determine the trajectory segments to be associated based on spatio-temporal constraint information. Specifically, the time constraint is a prerequisite for the association of two small trajectories, that is, there should be no overlapping regions within the time when the two small trajectories to be associated exist, and the same moving trajectory cannot belong to multiple targets at the same time. According to the time constraint condition, the conflict matrix L = [l(i, j)] M×M .

[0104]

[0105] In the formula, represents the time of the last frame of trajectory TR i , represents the time of the starting frame of trajectory TR j ; represents that there is an overlap in the existence time of the two small trajectories, l(TR j,TR i ) = 0 indicates that the two small trajectories cannot be associated, that is, the two small trajectories do not belong to the same target, l(TR j ,TR i ) = 1 indicates that the two small trajectories to be associated may originate from the same target.

[0106] For the motion information of traffic targets, association is performed based on a fuzzy strategy of motion prediction. Assume that two small trajectories that may be associated are set as TR i and TR j . The small trajectory with an earlier time is called the reference trajectory TR j . The starting time of this trajectory is The ending time is The small trajectory with a later time is the new trajectory. The starting time of this trajectory is The ending time is The reference trajectory and the new trajectory are respectively predicted forward and backward to and The midpoint time k c . To quantify the matching relationship between the predicted state c at time k of the reference trajectory and the predicted state at time k of the new trajectory, a normal membership function is used to calculate the correlation degree of the two trajectory segments: c at time k of the new trajectory, a normal membership function is used to calculate the correlation degree of the two trajectory segments: In the formula, u

[0107]

[0108] In the formula, u m is the m-th fuzzy factor; σ m is the error variance of the m-th fuzzy factor; τ m is the adjustment degree; ε T is the attenuation factor.

[0109]

[0110]

[0111] In the formula, u1 and u2 respectively represent the fuzzy factors of position and velocity; μ1(u1) and μ2(u2) respectively represent the correlation degrees of position and velocity; τ1 and τ2 respectively represent the adjustment degrees of position and velocity; respectively represent the error variances of position and velocity; k c represents and The midpoint time; respectively represent the estimated values of the longitudinal position and the lateral position of the reference trajectory and the new trajectory predicted to time k c ; respectively represent the estimated values of the longitudinal position and the lateral position of the reference trajectory and the new trajectory predicted to time kc Estimated values of the longitudinal and lateral speeds at a moment.

[0112] Reference trajectory TR i and new trajectory TR j The fuzzy similarity based on position and speed is expressed as:

[0113] f(TR j |TR i ) = a1μ1(u1) + a2μ2(u2) (7)

[0114] In the formula, a1 and a2 respectively represent the weights corresponding to the position and speed fuzzy factors.

[0115] After obtaining the association cost matrix, the trajectory segment association problem can be regarded as an assignment problem for solution. That is, based on the small trajectory set T and the fuzzy association function f(·), the Hungarian algorithm is used to select the overall optimal association matching result among multiple association possibilities. It can be described by formula (8).

[0116]

[0117] In the formula, T = {TR1, TR2,..., TR M} is the set of small trajectories generated by preliminary association, L = {l1, l2,..., l N} is the set of conflict matrices, f(·) is the fuzzy function used to associate small trajectories; M is the total number of trajectories in the short trajectory segment set; N is the total number of conflict matrices. Step 4: For the newly and old trajectory pairs that have been associated, the cubic Hermite interpolation method is used to connect and reconstruct the interrupted trajectories that are successfully associated, and based on the known state values of the target, the state values at the discontinuous moments are estimated, and the connection trajectory closest to the true target trajectory is sought to make the target trajectory complete and continuous, and the reconstruction of the target trajectory is completed.

[0118] The trajectory tracking results obtained from the three scenarios are as Figure 3 , Figure 4 , Figure 5 shown. Figure 3 For a, it is the lateral tracking result graph, Figure 3 for b, it is the longitudinal tracking result graph, Figure 4 for a, it is the lateral tracking result graph, Figure 4 for b, it is the longitudinal tracking result graph, Figure 5 for a, it is the lateral tracking result graph, Figure 5Graph b is the longitudinal tracking result graph, where graph a in each graph represents the tracking trajectory in the y-axis direction, and graph b represents the tracking trajectory in the x direction. Each line in the graph represents a trajectory of a vehicle, and the trajectories in the three graphs can all correspond one by one to the target vehicle. The tracking result is consistent with the actual situation. The target tracking algorithm based on small trajectory association proposed by the present invention effectively removes multiple measurement values reflected by the same target, greatly reduces the number of redundant trajectories, can correctly identify and delete multiple trajectories from the same target, realizes the unity of the target label before and after the trajectory interruption, and the accuracy of the continuous tracking result of the traffic target trajectory can reach more than 90%. The accuracy of the estimated number of targets in the scenario is significantly improved.

[0119] The results of the present invention can achieve continuous tracking of vehicles, obtain accurate and reliable high-frequency vehicle movement trajectories, enrich the vehicle trajectory data set in the traffic environment, and provide data support for subsequent traffic parameter extraction, traffic flow characteristic analysis, traffic event detection, traffic state estimation, etc.

[0120] Embodiment 2

[0121] A method for continuous tracking of vehicle movement trajectories, characterized by including the following steps:

[0122] The first step is the preprocessing of the original point cloud data. The original point cloud data of traffic targets within the detection range is obtained by using a millimeter-wave radar installed above the detection road. The collected data is parsed according to the specified protocol to obtain traffic target distance, speed and other motion parameter information.

[0123] The second step is to extract traffic target position, speed and other features based on the radar detection data, simply generate and terminate the traffic target trajectory by associating the measurement data of two consecutive frames, and connect the adjacent frame targets into reliable small trajectory segments.

[0124] The third step is that the association of short trajectories in adjacent frames can be regarded as an optimal linear assignment problem for solving a one-to-one mapping relationship.

[0125] Specifically, let (i, j) be the target association hypothesis pair, where i represents the i-th target detected in the (k - 1)-th frame, and j represents the j-th target in the k-th frame. Based on the association event, the relationship between the existing targets and the detected targets is exhausted in two adjacent frames to establish an n×m similarity matrix. When performing the association of adjacent frames of the target trajectory, for a determined target, the changes in speed and position in different frames within a short time are not significant. In addition, the millimeter-wave radar measures the speed and position of the target more accurately, and the samples can form continuous features of adjacent frames. Therefore, the speed and position features can be selected for association matching, and a similarity function as shown in Equation (8) is defined to calculate the similarity between points in adjacent frames:

[0126] S{di (k - 1)|d j (k)} = S pos (d i |d j )·S v (d i |d j ) (8)

[0127] Wherein, S pos (d j |d i ) reflects the similarity degree between the position coordinates of the i-th target in the (k - 1)-th frame and the position coordinates of the j-th target in the k-th frame; S v (d i |d j ) reflects the similarity degree between the velocity of the i-th target in the (k - 1)-th frame and the velocity of the j-th target in the k-th frame;

[0128]

[0129]

[0130] S{d i (k - 1)|d j (k)} ≥ θ (11)

[0131] Wherein, s x and s y are respectively the position variances of all current trajectory segments in the x and y components; s vx and s vy are respectively the velocity variances of all current trajectory segments in the x and y components; θ is the threshold for preliminary association.

[0132] When constructing short trajectories through preliminary association between adjacent frames, the magnitude of the preliminary association threshold θ can determine the length and quantity of the generated trajectory segments. When the threshold is too large, as long as the data in the next frame slightly deviates from the data in the previous frame, it will be considered an unreliable short trajectory, thus generating more new trajectory segments; when the threshold is too small, the conditions for adjacent frame association become loose, resulting in low discrimination, and some terminated short trajectories can get the opportunity to continue growing, which may lead to false association situations. Figure 2 Indicates the influence of the values of the three scenario association thresholds on the quantity of the generated trajectory segments. It can be seen that: obvious inflection points all appear at θ = 0.9 for the curves; when θ < 0.9, the change in the quantity of the generated trajectory segments fluctuates little; when θ > 0.9, the quantity of the generated trajectory segments increases sharply. Therefore, in this paper, the preliminary association threshold is set to 0.9, and the preliminary association results are shown in Table 2.

[0133] The technical solution of the embodiment of the present invention obtains the point cloud data of traffic targets within the detection range by using a millimeter-wave radar installed above the detection road, determines the threshold for preliminary association of short trajectories according to the radar detection data, plays a key role in the length and quantity of the subsequent generated trajectory segments, and improves the accuracy of continuous tracking of lane trajectories.

[0134] Embodiment 3 In order to test the applicability of the method disclosed in the present invention in different time periods, an experiment was carried out by changing the daytime scene to a nighttime scene on the basis of Embodiment 1:

[0135] S1. Perception of traffic target motion information. The original point cloud data of traffic targets within the detection range is obtained by using a millimeter-wave radar installed above the detection road. Each point represents a detected traffic target, and its data fields at least include the lateral distance, longitudinal distance of the radar installation position, as well as the lateral position, longitudinal position, lateral speed, longitudinal speed and timestamp of the point;

[0136] S2. Construction of small trajectory segments. Based on the radar detection data, features such as the position and speed of traffic targets are extracted. The traffic target trajectories are simply generated and terminated by associating the measurement data of two consecutive frames, and the adjacent frame targets are connected into reliable small trajectory segments, and the association results are corrected to complete the construction of the initial small trajectory segments;

[0137] S3. Association of small trajectory segments. A fuzzy correlation function is constructed based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, the similarity between trajectories is calculated to obtain a cost matrix, and the problem of associating small trajectory segments is regarded as an assignment problem to be solved;

[0138] S4. Reconstruction of interrupted target trajectories. For the associated old and new trajectory pairs with interruptions, the cubic Hermite interpolation method is used to connect the interrupted trajectories that meet the association relationship, and according to the state values corresponding to the known moments of the trajectories, the data corresponding to the interrupted moments is estimated to fill the gaps between the trajectory segments and complete the reconstruction of the target trajectories.

[0139] The experiment mainly verifies the feasibility of the proposed algorithm from two aspects: the correct tracking accuracy rate and the trajectory interruption rate, and analyzes the accuracy of the trajectory extraction algorithm. The experimental results show that: the results of the present invention can also have good performance under the influence of nighttime scenes, low light intensity, bad weather, etc., with low required costs and universal applicability.

[0140] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principle of the present invention shall be equivalent replacement methods and are all included in the protection scope of the present invention.

Claims

1. A method for continuously tracking the motion trajectory of a vehicle, characterized in that, It includes the following steps: S1. Use a millimeter-wave radar to obtain the original point cloud data of traffic targets within the detection range to obtain radar detection data, where each point represents a detected traffic target; S2. Extract the position and speed features of traffic targets based on the radar detection data. Generate and terminate the traffic target trajectory by correlating the measurement data of two consecutive frames, connect adjacent frame targets into short trajectory segments, and correct the correlation result to complete the construction of the initial small trajectory segments; The construction of short trajectory segments includes: S201. When associating adjacent frames, select speed and position information for similarity matching calculation. If it exceeds the set threshold θ, the detection responses d i (k - 1) and d j (k) belong to the trajectory sequence of the same target. If they do not belong to the same target, use the detection point of the k-th frame as the starting point of the new trajectory, and continue to pair with the detection responses of the next frame until all the detection responses of the detection sequences are associated; S202. Further correct the preliminary correlation result and eliminate the split trajectories generated by the same target at the same moment; Assume there is a trajectory TR A and TR B , the sampling times of the two trajectories overlap from the i-th frame to the j-th frame. The judgment criterion as shown in Equation (1) is established. If the judgment criterion holds, it is considered that the two trajectories originate from the same target. Then, the shorter trajectory is regarded as a split trajectory and removed, and the other trajectory corresponds to the original target. And so on, multiple removals are performed until there are no split trajectories that meet the following conditions within the trajectory sampling time: In the formula, respectively represent the distances of the k-th frame on the trajectories TR A , TR B from the radar; represents the speeds of the k-th frame on the trajectories TR A , TR B ; ξ R is the distance difference threshold, and ξ v is the speed difference threshold; S3. Construct a fuzzy correlation function based on spatio-temporal characteristics and motion characteristics to describe the matching relationship between the reference trajectory and the new trajectory, calculate the similarity between trajectories to obtain a cost matrix, and regard the short trajectory segment association problem as an assignment problem for solution; S4. For the associated new and old trajectory pairs with interruptions, connect the interrupted trajectories that meet the association relationship, estimate the data corresponding to the interrupted moment according to the state values corresponding to the known moments of the trajectories, fill the gaps between the trajectory segments, and complete the continuous tracking of the target trajectory.

2. The vehicle motion trajectory continuous tracking method according to claim 1, wherein, The point cloud data information detected by the millimeter-wave radar in step S1 includes the lateral distance, longitudinal distance of the traffic target relative to the radar installation position, as well as the lateral position, longitudinal position, lateral speed, longitudinal speed and timestamp of the point.

3. A method for continuously tracking a vehicle motion trajectory according to claim 1, characterized in that, Step S3 specifically includes: S301. Correlate the motion information of traffic targets based on a fuzzy strategy of motion prediction. Assume two short trajectories TR i and TR j that may be correlated. The short trajectory with an earlier time is called the reference trajectory TR i . The starting time of this trajectory is and the ending time is . The short trajectory with a later time is the new trajectory TR j . The starting time of this trajectory is and the ending time is . Forward and backward predict the reference trajectory and the new trajectory respectively to the and midpoint time k c . To quantify the matching relationship between the predicted state c at the midpoint time k of the reference trajectory and the predicted state at time k of the new trajectory c , use a normal membership function to calculate the correlation degree of the two trajectory segments:​ where u m is the m-th fuzzy factor; σ m is the error variance of the m-th fuzzy factor; τ m is the adjustment degree; ε T is the attenuation factor; In the formula, u1 and u2 respectively represent the fuzzy factors of position and velocity; μ1(u1) and μ2(u2) respectively represent the correlation degrees of position and velocity; τ1 and τ2 respectively represent the adjustment degrees of position and velocity; respectively represent the error variances of position and velocity; k c represents and the midpoint time; respectively represent the estimated values of the longitudinal position and the lateral position of the reference trajectory and the new trajectory predicted at time k c ; respectively represent the estimated values of the longitudinal velocity and the lateral velocity of the reference trajectory and the new trajectory predicted at time k c ; Reference trajectory TR i and the new trajectory TR j The fuzzy similarity based on position and velocity is expressed as: f(TR j |TR i ) = a1μ1(u1) + a2μ2(u2) (5) In the formula, a1 and a2 respectively represent the weights corresponding to the position and speed fuzzy factors; S302. Obtain the association cost matrix based on formula (5). Based on the small trajectory set T and the fuzzy association function f(·), use the Hungarian algorithm to select the overall optimal association matching result among multiple association possibilities; where, T = {TR1, TR2,..., TR M} is the set of short trajectories generated by preliminary association; L = {l1, l2,..., l N} is the set of conflict matrices; f(·) is the fuzzy function for associating small trajectories; M is the total number of trajectories in the set of short trajectory segments; N is the total number of conflict matrices; TR M represents the M-th short trajectory in the trajectory set; l N represents the association result of the N-th short trajectory.

4. A method for continuously tracking a vehicle motion trajectory according to claim 3, characterized in that In step S3, if the interruption is caused by target occlusion, then perform step S301 to calculate the fuzzy similarity between short trajectories; if the interruption is caused by the target stopping, then directly calculate the fuzzy similarity by formula (5) without prediction.

5. A vehicle motion trajectory continuous tracking method according to claim 3, characterized in that, For the associated new and old trajectory pairs in step S3, use the cubic Hermite interpolation method to connect and reconstruct the interrupted trajectories that are successfully associated, estimate the state value at the interrupted moment based on the known state value of the target, and seek the connection trajectory closest to the real target trajectory to make the target trajectory complete and continuous, and complete the reconstruction of the target trajectory.

6. A method for continuously tracking the vehicle motion trajectory according to claim 1, characterized in that, In step S1, the acquisition of radar detection data requires screening radar targets within a specific recognition range and eliminating invalid targets; where p x and p y are the target position attribute components respectively; Y dist is the set horizontal range; X max is the maximum longitudinal range that the radar can detect, i.e., the maximum effective distance; X min is the minimum longitudinal range that the radar can detect, i.e., the minimum effective distance.

7. A method for continuously tracking a vehicle's motion trajectory according to claim 1, characterized in that In step S2, the association between adjacent frames specifically includes three cases: Case 1: When the number of vehicles m in the k-th frame is less than the number of vehicles n in the (k - 1)-th frame, for the target point tracks that are not paired in the (k - 1)-th frame, it is considered that the target may have left the detection area or had an interruption in the (k - 1)-th frame, and store it in the short trajectory segment set; Case 2: When the number of vehicles m in the k-th frame is equal to the number of vehicles n in the (k - 1)-th frame, at this time, the traffic targets in the two frames are most likely to be successfully associated with each other, but there may also be a situation where the target in the (k - 1)-th frame disappears and a new target is added in the k-th frame; Case 3: When the number of vehicles m in the k-th frame is greater than the number of vehicles n in the (k - 1)-th frame, it is possible that new targets appear in the k-th frame. For the target point tracks that are not successfully paired in the k-th frame, they are used as the starting points of new tracks and participate in the pairing in the next frame.

8. A method for continuously tracking a vehicle motion trajectory according to claim 1, characterized in that, In step S2, the millimeter-wave radar measures the speed and position of the target accurately, and the samples can form continuous features of adjacent frames. Therefore, the speed and position features can be selected for correlation matching.

9. A method for continuously tracking a vehicle motion trajectory according to claim 1, characterized in that, In step S3, the determination of the trajectory segments to be correlated includes: The two short trajectories to be associated cannot overlap in the time of their respective appearances; the same motion trajectory cannot belong to multiple targets at the same time. The spatio-temporal constraint condition is the premise for judging whether two short trajectories can be associated. According to the spatio-temporal constraint condition, the conflict matrix L = [l(i, j)] M×M ; In the formula, represents the trajectory TR i the time of the last frame, represents the trajectory TR j the time of the starting frame; represents that there is a time overlap between two small trajectories, l(TR j , TR i ) = 0 indicates that the two small trajectories cannot be associated, that is, the two small trajectories do not belong to the same target, l(TR j , TR i ) = 1 indicates that the two small trajectories to be associated originate from the same target.

Images