Joint tracking method based on laser point cloud network detection and clustering targets

Abstract

The application discloses a joint tracking method based on laser point cloud network detection and target clustering, which eliminates the disadvantages of tracking by using single network detection or clustering results by taking the joint tracking of the target and the clustered target as input, increases a track management process in the joint tracking process, prevents track ID switching logic, and sets a shape mutation compensation process before Kalman update, effectively reduces the phenomenon of unstable track tracking or even disassociation caused by the fluctuation of the input network detection target or the clustered target, and improves the stability of the track.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic driving radar tracking, and particularly relates to a joint tracking method based on laser point cloud network detection and clustering targets. BACKGROUND

[0002] In the automatic driving technology, the positioning and tracking of a vehicle are usually realized based on a laser radar, and the existing laser radar tracking method mainly tracks based on single network detection or single clustering result. The single network detection result based on the laser radar is obtained by training a convolutional neural network, the target confidence of the detection is high, and the distance, type and orientation and other state values are relatively stable, but there is a possibility of missing detection for a close-range target of the ego vehicle. The target obtained by single clustering of the point cloud of the laser radar has a high existence probability, but a single object may be clustered into multiple unknown targets, and the orientation and length-width attributes are very unstable. Therefore, the target tracked based on the single clustering result has defects such as uneven position and speed, length jump and track ID switching. SUMMARY

[0003] In view of the above problems, the main purpose of the present application is to design a joint tracking method based on laser point cloud network detection and clustering targets, which combines network detection targets with clustering targets to solve the problems of uneven position and speed, length jump and track ID switching in the prior art.

[0004] In order to achieve the above purpose, the present application adopts the following technical scheme:

[0005] A joint tracking method based on laser point cloud network detection and clustering targets, which is based on two arrays sent by an upstream laser radar perception, outputs a track array of a vehicle, and realizes tracking of the vehicle.

[0006] The two arrays sent by the upstream laser radar perception are a network detection target array and a clustering target array which are independent of each other. The network detection target array stores a plurality of network detection targets, and the clustering target array stores a plurality of clustering targets.

[0007] The network detection target and the clustering target both contain state information of the target. Each track in the track array contains state information of a target and information of a track corresponding to the target.

[0008] As a further description of the present application, the state information of the target includes longitudinal position, transverse position, covariance matrix, length, width, height and heading angle of the target.

[0009] The information of the track includes the track's ID, source, age, lifeSpan, coastSpan, and status.

[0010] Furthermore, the above-mentioned flight track information is explained as follows:

[0011] The track ID is the identifying information of the track, and each track ID is unique;

[0012] The source of a track refers to the origin of the track information. If the source is CNN, it means that the track was created by the network detection target. If the source is CLU, it means that the track was created by the clustering target.

[0013] The age of a flight track refers to the number of frames the track exists in.

[0014] The status of a track refers to its current state, which is divided into life state and coast state. Tracks in the life state are output by this method, while tracks in the coast state only exist in the track list inside this method and are not output externally.

[0015] The lifeSpan of a track is the track count in the life state;

[0016] The coastSpan is the track count in the coast state;

[0017] The newly created track is in the life state and the initial lifeSpan is 1; if the track is successfully associated and updated within a frame, the lifeSpan is incremented by 2, otherwise it is decremented by 1; the maximum lifeSpan is 7.

[0018] When the lifeSpan of the life state track is 0, it enters the coast state and initializes the coastSpan to 100; if the track is successfully associated and updated within a frame, the track enters the life state and initializes the lifeSpan to 1, otherwise the coastSpan is decremented by 1.

[0019] If the coastSpan of a track in the coast state is 0, then the track is deleted from the track list of this method.

[0020] As a further description of the present invention, the specific steps of the method include the following:

[0021] Step 1: Receive the network-detected target array and clustered target array sent by the upstream LiDAR sensor of the vehicle;

[0022] Step 2: Perform motion compensation on all tracks of the vehicle in the previous frame, and convert the state values ​​of the tracks to the new vehicle coordinate system;

[0023] Step 3: Perform Kalman prediction on the track after the transformation in Step 2, keeping the track state and the observed state in the same time domain;

[0024] Step 4: Associate the predicted track with the network detection target; if the association is successful and there is no shape change, use the network detection target to perform a Kalman update on the track state; if the association is successful, there is a shape change and the change compensation is successful, use the compensated network detection target to perform a Kalman update on the track state; if the association is successful, there is a shape change and the change compensation fails, no operation is performed; if the association fails, a track is created.

[0025] Step 5: Associate the updated track with the clustering target; if the association is successful and there is no shape mutation, use the clustering target to perform a Kalman update on the track state; if the association is successful, there is a shape mutation and the mutation compensation is successful, use the compensated clustering target to perform a Kalman update on the track state; if the association is successful, there is a shape mutation and the mutation compensation fails, no operation is performed; if the association fails, a track is created.

[0026] Step 6: Manage the track based on the track status update in Step 5;

[0027] Step 7: Output the vehicle's trajectory.

[0028] As a further description of the present invention, in step 2, before the motion compensation begins, all state values ​​of the trajectory are in the vehicle coordinate system of the previous frame, where the origin of the coordinate system is located at the center of the front bumper of the vehicle, forward is the positive x-axis, and left is the positive y-axis. Therefore, the change in the vehicle's motion state coordinate system is as follows:

[0029] dx = vEgo * dt

[0030] dθ=yawRate*dt

[0031] Where dx is the distance the coordinate system moves forward, dθ is the angle the coordinate system rotates (positive to the left), vEgo is the vehicle speed, yawRate is the vehicle yaw rate (positive to the left), and dt is the execution cycle.

[0032] Therefore, motion compensation for the trajectory is performed as follows:

[0033] x=(x k1 -dx)*cos(dθ)-y k1 *sin(dθ)

[0034] y = (x k1 -dx)*sin(dθ)+y k1 *cos(dθ)

[0035] vx = v xk1 *cos(dθ) - v yk1 *sin(dθ)

[0036] v y = v xk1 *sin(dθ) + v yk1 *cos(dθ)

[0037] wherein x k1 , y k1 , v xk1 , v yk1 are the longitudinal position, lateral position, longitudinal velocity, lateral velocity of the track before motion compensation, and x x , y y , v x , v y are the longitudinal position, lateral position, longitudinal velocity, lateral velocity of the track after motion compensation.

[0038] As a further description of the application, in step 3, before the prediction of the track state, the track state is at the time of the last frame, and the formula of Kalman prediction is:

[0039]

[0040]

[0041] wherein is the track state vector after motion compensation, that is: {x, y, v x , v y};

[0042] is the covariance matrix of the track of the last frame, A is the state transition matrix of the track, and a uniform motion model is adopted, so:

[0043]

[0044] After filtering, the track state of the ego vehicle and the observed state are kept in the same time domain.

[0045] As a further description of the application, in steps 4 and 5, the association of the track with the network detection target and the association of the track with the clustered target are consistent, and specifically include the following:

[0046] The distance matrix of the track array and the network detection target array or the clustered target array is calculated in sequence, and the calculation is as follows:

[0047] dis = dX T *S -1 *dX

[0048] Wherein, dX is the difference between the state vector of the track and the state vector of the network detection target, or the difference between the state vector of the track and the state vector of the distance target; S is the sum of the covariance matrix of the track and the covariance matrix of the network detection target, or the sum of the covariance matrix of the track and the covariance matrix of the clustered target;

[0049] The association threshold is set as disThre, when dis is less than disThre, then the nearest neighbor algorithm is used for 1:1 association of the track and the network detection target, or 1:1 association of the track and the clustered target;

[0050] The network detection target or the clustered target which is not successfully associated is newly tracked, and the network detection target or the clustered target which is successfully associated is updated by Kalman.

[0051] As further description of the application, the Kalman update further comprises shape mutation compensation for the network detection target or the clustered target which has a large difference in length and width from the track before the Kalman update;

[0052] The shape mutation compensation is to calculate the distance between each track corner point and each network detection target corner point, and when the distance is less than a threshold, the track corner point and the network detection target corner point are associated; or to calculate the distance between each track corner point and each clustered target corner point, and when the distance is less than a threshold, the track corner point and the clustered target corner point are associated.

[0053] If the number of associated corner points is less than 2, the shape mutation compensation fails and the Kalman update is not performed; if the number of associated corner points is greater than or equal to 2, the two track corner points and the corresponding two network detection target corner points with the smallest distance between the associated corner points are found, or the two track corner points and the corresponding two clustered target corner points with the smallest distance between the associated corner points are found.

[0054] According to the coordinates of the two network detection target corner points or the two clustered target corner points, a reference edge without mutation is determined; the length mutation value and the width mutation value are calculated according to the length and width attributes of the track and the length and width attributes of the network detection target, and the center point of the new network detection target is obtained for Kalman update, or the length mutation value and the width mutation value are calculated according to the length and width attributes of the track and the length and width attributes of the clustered target, and the center point of the new clustered target is obtained for Kalman update.

[0055] As further description of the application, the Kalman update mode is:

[0056] First, the Kalman gain is calculated:

[0057]

[0058] Then, the track state vector and the covariance matrix are updated:

[0059]

[0060]

[0061] Wherein, Z is the state vector of network detection target or the state vector of clustering target; H is an observation matrix, which converts the state domain to the observation domain:

[0062]

[0063] Wherein, I is a 4-order unit matrix.

[0064] As further description of the application, in step 6, the track management is specifically: each track includes lifeSpan and coastSpan attributes, the lifeSpan attribute decreases to 0, the track is in a coasted state, and the coastSpan of the track is initialized, and if the coastSpan of the coasted track decreases to 0, the track is deleted.

[0065] As further description of the application, in step 7, the output of the track of the ego vehicle further includes processing of overlapping tracks, specifically:

[0066] The overlapping area of the two-dimensional rectangle projected by the track in the horizontal plane is used for judgment, and if the overlapping area exceeds a threshold, the overlapping track is defined;

[0067] If the track of the network detection target overlaps with the track of the clustering target, the track of the clustering target is deleted, if the tracks overlap internally, the track with smaller area is deleted, if the tracks of the clustering target overlap internally, the track with smaller area is deleted, and the track area is the length multiplied by the width of the track;

[0068] The output of the track is that the age of all tracks is added by 1, and the track with age greater than 5 and in the life state is output.

[0069] Compared with the prior art, the technical effect of the application is:

[0070] The application provides a joint tracking method based on laser point cloud network detection and clustering target, which eliminates the drawbacks of using single network detection or clustering results for tracking by using network detection target and clustering target as input for joint tracking, and increases the track management process in the joint tracking process to prevent track ID switching logic, and sets a shape mutation compensation process before Kalman update, effectively reducing the phenomenon of unstable track tracking or even disassociation caused by input network detection target or clustering target fluctuation, and improving the stability of the track. BRIEF DESCRIPTION OF DRAWINGS

[0071] Fig. 1 The overall flowchart of the tracking method of the present application is shown in the figure;

[0072] Fig. 2 The Kalman update in the present application is shown in the figure. DETAILED DESCRIPTION

[0073] The present application will be described in detail below with reference to the accompanying drawings:

[0074] A joint tracking method for network detection and clustering targets based on laser point cloud, as shown in the figure, the method takes two arrays sent by the upstream laser radar perception as input, and outputs a track array of vehicles, realizing tracking of vehicles. Figs. 1-2

[0075] Among them, the two arrays sent by the upstream laser radar perception are network detection target array and clustering target array, which are independent of each other, the network detection target array stores a plurality of network detection targets, and the clustering target array stores a plurality of clustering targets; that is, if there is a real object in the environment, the above two arrays may both exist or may not exist (i.e. not detected), or one array may exist and the other array may not exist.

[0076] The network detection target and the clustering target both contain the state information of the target, and each track in the track array contains the state information of the target and the information of the track corresponding to the target.

[0077] Specifically, in the present embodiment, the state information of the target includes the longitudinal position, the transverse position, the covariance matrix, the length, the width, the height and the heading angle of the target; and the information of the track includes the id, the source, the age, the lifeSpan, the coastSpan and the status of the track.

[0078] In the present embodiment, the information of the track is explained as follows:

[0079] The id of the track is the identification information of the track, and the id of each track is unique;

[0080] The source of the track refers to the source of the track information, and source for cnn indicates that the track is created by the network detection target, and source for clu indicates that the track is created by the clustering target;

[0081] The age of the track refers to the number of frames of the track;

[0082] ​The status of the track indicates the state of the track, which is divided into a life state and a coast state; the track in the life state is the output of the method, and the track in the coast state only exists in the track list inside the method and is not output externally;

[0083] The lifeSpan of the track is the track count in the life state.

[0084] The coastSpan of the track is the track count in the coast state.

[0085] The newly created track is in the life state, and the lifeSpan is initialized to 1; if the track is successfully associated and updated in a frame, the lifeSpan is increased by 2, otherwise it is decreased by 1; the maximum lifeSpan is 7.

[0086] When the lifeSpan of the track in the life state is 0, the track enters the coast state and the coastSpan is initialized to 100; if the track is successfully associated and updated in a frame, the track enters the life state and the lifeSpan is initialized to 1, otherwise the coastSpan is decreased by 1.

[0087] If the coastSpan of the track in the coast state is 0, the track is deleted from the track list of the method.

[0088] The method disclosed in the embodiment includes steps of creating a track, predicting, associating and updating to realize temperature tracking of the target position and speed, and specifically, the method is realized through the following steps:

[0089] Step 1: receiving the network detection target array and the clustering target array sent by the upstream laser radar perception of the vehicle;

[0090] Step 2: performing motion compensation on all tracks of the previous frame of the vehicle, and converting the state value of the track into a new ego coordinate system;

[0091] Step 3: performing Kalman prediction on the track converted in step 2, and keeping the track state and the observation state in the same time domain;

[0092] Step 4: associating the predicted track with the network detection target; if the association is successful and there is no shape mutation, performing Kalman update on the track state using the network detection target; if the association is successful and there is shape mutation and the mutation compensation is successful, performing Kalman update on the track state using the compensated network detection target; if the association is successful and there is shape mutation and the mutation compensation fails, no operation is performed; if the association fails, a track is created;

[0093] Step 5: Associate the updated track with the clustering target; if the association is successful and there is no shape mutation, use the clustering target to perform a Kalman update on the track state; if the association is successful, there is a shape mutation and the mutation compensation is successful, use the compensated clustering target to perform a Kalman update on the track state; if the association is successful, there is a shape mutation and the mutation compensation fails, no operation is performed; if the association fails, a track is created.

[0094] Step 6: Manage the track based on the track status update in Step 5;

[0095] Step 7: Output the vehicle's trajectory.

[0096] It should also be noted that this embodiment provides a detailed analysis of each of the above steps, as follows:

[0097] In step 2, the track information includes the target's longitudinal position, lateral position, longitudinal velocity, lateral velocity, covariance matrix, length, width, height, heading angle, id, source, age, lifeSpan, coastSpan, and status.

[0098] II. In step 2, before the motion compensation begins, all state values ​​of the trajectory are in the vehicle's coordinate system of the previous frame, where the origin of the coordinate system is located at the center of the vehicle's front bumper, forward is the positive x-axis, and left is the positive y-axis. Therefore, the change in the vehicle's motion state coordinate system is as follows:

[0099] dx = vEgo * dt dθ = yawRate * dt

[0100] Where dx is the distance the coordinate system moves forward, dθ is the angle the coordinate system rotates (positive to the left), vEgo is the vehicle speed, yawRate is the vehicle yaw rate (positive to the left), and dt is the execution cycle.

[0101] Therefore, motion compensation for the trajectory is performed as follows:

[0102] x=(x k1 -dx)*cos(dθ)-y k1 *sin(dθ)

[0103] y = (x k1 -dx)*sin(dθ)+y k1 *cos(dθ)

[0104] v x =v xk1 *cos(dθ)-v yk1 *sin(dθ)

[0105] v y =vxk1 *sin(dθ)+v yk1 *cos(dθ)

[0106] Wherein, x k1 , y k1 , v xk1 , v yk1 are the longitudinal position, lateral position, longitudinal velocity, lateral velocity of the track before motion compensation, respectively, x x , y y , v x , v y are the longitudinal position, lateral position, longitudinal velocity, lateral velocity of the track after motion compensation, respectively.

[0107] Before starting each frame tracking method, all state values of the track are in the ego vehicle coordinate system of the last frame. Since the ego vehicle moves, the coordinate system changes. Through the above motion compensation, the state values of the track are converted to the new ego vehicle coordinate system, which prepares for the subsequent association update.

[0108] III. In step 3, before the prediction of the track state, the track state is in the time of the last frame, and the observation is in the time of the current frame. The purpose of Kalman prediction is to keep the state of the track and the state of the observation in the same time domain. In the method of the embodiment, the observation refers to the network detection target and the clustered target. Therefore, the formula of Kalman prediction is:

[0109]

[0110]

[0111] Wherein, is the track state vector after motion compensation, that is: {x, y, v x , v y};

[0112] is the covariance matrix of the track of the last frame, A is the state transition matrix of the track, and a uniform motion model is adopted, so:

[0113]

[0114] After the above Kalman prediction, the ego vehicle track state and the observation state are kept in the same time domain.

[0115] IV. In step 4, the association of the track and the network detection target, specifically includes the following:

[0116] The distance matrix of the track array and the network detection target array is calculated in turn, and the calculation method is as follows:

[0117] dis=dX T *S -1*dX

[0118] wherein dX is the difference between the state vector of the track and the state vector of the network detection target; S is the sum of the covariance matrix of the track and the covariance matrix of the network detection target; the association threshold is set as disThre, when dis is less than disThre, then the nearest neighbor algorithm is used to associate the track and the network detection target one by one;

[0119] The network detection target that is not successfully associated is newly tracked, the longitudinal position, the lateral position, the length, the width, the height, the heading angle and the covariance matrix of the newly tracked track are consistent with the longitudinal position, the lateral position, the length, the width, the height, the heading angle and the covariance matrix of the network detection target, and the track source information is assigned with the cnn attribute.

[0120] The updated life state track lifespan is added by 1, the updated coast state track enters the life state and initializes the lifespan, and the newly tracked track enters the life state and initializes the lifespan. The initialization of the lifespan means that the lifespan is set to 1.

[0121] The track with the successfully associated source information is subjected to Kalman update, the source information of the track refers to the source when the track is created, if the track is created by the network detection target, the source information is set to cnn, and if the track is created by the clustering target, the source information is set to clu.

[0122] The Kalman update further includes shape mutation compensation for the network detection target or the clustering target with a large difference in length and width before the Kalman update; the lateral and longitudinal positions of the input network detection target and the clustering target refer to the position of a target center point, and the lateral and longitudinal positions of the track also refer to the position of a target center point. Since the point cloud detected by the actual laser radar is blocked by an object, the length and width attributes of the network detection result or the clustering result of the same object will change, which causes the center point position of the input network detection target or the clustering target to fluctuate, and if the fluctuating position is used for update, the track tracking will be unstable, and even the disassociation phenomenon will occur, so the shape mutation compensation is needed.

[0123] The shape mutation compensation is to calculate the coordinates of four corner points pTrack a , pTrack b , pTrack c , pTrack d of each track according to the track center point position and the length and width attributes and the heading angle, and the corner points pCNN a , pCNN b , pCNNc pCNN d The coordinates are then used to calculate the distance between each track corner and each network-detected target corner. If the distance is less than a threshold, the track corner and the network-detected target corner are associated.

[0124] If the number of associated corner points is less than 2, shape mutation compensation fails and Kalman update is not performed; if the number of associated corner points is greater than or equal to 2, the two track corner points with the best association effect and the corresponding corner points of the two network detection targets are found (the better the association effect, the smaller the association distance between the corner points); a reference edge without mutation is determined based on the coordinates of the two network detection target corner points; the length mutation value dLength and the width mutation value dWidth are calculated based on the length and width attributes of the track and the length and width attributes of the network detection targets, and the new center point of the network detection targets is obtained before Kalman update is performed.

[0125] like Fig. 2 As shown, assuming the network with the best correlation performance detects the target corner point is pCNN a and pCNN b If the reference edge is the edge containing the length of the target, then pCNN will be used. c and pCNN d The new corner point pCNN is obtained by translating dWidth along the width direction. cnew and pCNN dnew According to pCNN a pCNN b pCNN cnew pCNN dnew The new center point of the target detected by the network is calculated before Kalman update; if the reference edge is the edge containing the width of the target, then pCNN is updated. c and pCNN d Translate along the length direction to obtain dLength to the new corner point pCNN cnew and pCNN dnew According to pCNN a pCNN b pCNN cnew pCNN dnew The center point of the new network detection target is calculated before Kalman update is performed.

[0126] Specifically, the Kalman update method described in this embodiment is as follows:

[0127] First, calculate the Kalman gain:

[0128]

[0129] Then, the trajectory state vector and covariance matrix are updated:

[0130]

[0131]

[0132] wherein, Z is the state vector of the network detection target or the state vector of the cluster target; H is an observation matrix, which converts the state domain to the observation domain:

[0133]

[0134] wherein, I is a 4-order unit matrix.

[0135] V. In step 5, the association of the track and the cluster target is consistent with the relationship mode of the track and the network detection target, and specifically includes the following:

[0136] The distance matrix of the track array and the cluster target array is calculated in turn, and the calculation method is as follows:

[0137] dis = dX T *S -1 *dX

[0138] wherein, dX is the difference between the state vector of the track and the state vector of the cluster target; S is the sum of the covariance matrix of the track and the covariance matrix of the cluster target; the association threshold is set as disThre, and when dis is less than disThre, the nearest neighbor algorithm is used for 1-to-1 association of the track and the cluster target.

[0139] For the cluster target that is not successfully associated, a new track is created, and the longitudinal position, lateral position, length, width, height, heading angle and covariance matrix of the new track are consistent with those of the cluster target, and the track source information is assigned with the clu attribute.

[0140] The updated life state track lifespan is increased by 1, the updated coast state track enters the life state and initializes lifespan, and the newly created track enters the life state and initializes lifespan. Initializing lifespan means that lifespan is set to 1.

[0141] The track with clu source information is subjected to Kalman update, and the track source information refers to the source when the track is created. If it is created by the network detection target, the source information is set to cnn, and if it is created by the cluster target, the source information is set to clu.

[0142] The Kalman update further comprises shape mutation compensation for a cluster target with a large gap in length and width from the track; the horizontal and vertical position of the network detection target and the cluster target input in the embodiment refers to the position of a target center point, and the horizontal and vertical position of the track also refers to the position of a target center point. Since the point cloud detected by the actual laser radar is blocked by the object during operation, the length and width attributes of the network detection result or the cluster result of the same object will change, which leads to fluctuations in the center point position of the input network detection target or cluster target. If the fluctuating position is used for updating, it will lead to unstable track tracking, and even disassociation phenomenon, so shape mutation compensation is needed.

[0143] The shape mutation compensation is to calculate the coordinates of four corner points pTrack a , pTrack b , pTrack c , pTrack d of each track according to the center point position, length and width attributes and heading angle of the track, and the coordinates of four corner points pCNN a , pCNN b , pCNN c , pCNN d of each cluster target can be calculated in the same way, and then the distance between each track corner point and each cluster target corner point is calculated. If the distance is less than a threshold, the track corner point and the cluster target corner point are associated.

[0144] If the number of associated corner points is less than 2, the shape mutation compensation fails and the Kalman update is not performed; if the number of associated corner points is greater than or equal to 2, the two track corner points and the corresponding two cluster target corner points with the best association effect are found (the better the association effect, the smaller the distance between the corner points); a reference edge without mutation is determined according to the coordinates of the two cluster target corner points; the length mutation value dLength and the width mutation value dWidth are calculated according to the length and width attributes of the track and the length and width attributes of the cluster target, and the center point of the new cluster target is obtained to perform Kalman update.

[0145] As shown in Fig. 2 , assuming that the cluster target corner points with the best association effect are pCNN a and pCNN b , if the reference edge is the length of the target, pCNN c and pCNN d are translated in the width direction by dWidth to obtain new corner points pCNN cnew and pCNN dnew , and pCNN a , pCNN b , pCNN cnew , pCNNdnew Calculate the center point of the new cluster target and then perform Kalman update; if the reference edge is the edge where the target width is located, then pCNN c and pCNN d Translate along the length direction to the new corner point pCNN cnew and pCNN dnew Calculate the center point of the new cluster target and then perform Kalman update. a , pCNN b , pCNN cnew , pCNN dnew

[0146] Specifically, the Kalman update method in the embodiment is as follows:

[0147] First, calculate the Kalman gain:

[0148]

[0149] Then, update the track state vector and the covariance matrix:

[0150]

[0151]

[0152] Wherein, Z is the state vector of the network detected target or the state vector of the cluster target; H is an observation matrix, which converts the state domain to the observation domain:

[0153]

[0154] Wherein, I is a 4-order unit matrix.

[0155] Six, in step 6, the track management is specifically as follows: the track includes lifeSpan and coastSpan attributes, the lifeSpan attribute is reduced to 0, the track is in a coasted state, and the coastspan of the track is initialized; the coastspan of the track is initialized to 100, and if the coastSpan of the coasted track is reduced to 0, the track is deleted.

[0156] ​The flow of this step can achieve the effect of preventing ID_switch by maintaining the track in coast state: under a general tracking algorithm, assuming that the id of a track is a, due to the fact that the target is temporarily blocked in reality, the algorithm does not receive the network detection result and the clustering result of the target, and thus the track is deleted when the lifeSpan decreases to 0; after a period of time, the target is detected again by the network or clustering, and at this time, a new track is generated in the algorithm, and the id of the track is b, which is the ID_switch phenomenon of the general tracking algorithm for tracking the same target; compared with the general tracking algorithm, the algorithm of the embodiment introduces the concepts of coast state and coastSpan, when the target is temporarily blocked and the lifeSpan decreases to 0, the track enters the coast state, although the algorithm does not output the track, the track is maintained in the track list and the track id remains unchanged; when the target is not blocked and is detected by the network or clustering after a period of time, the track is associated and updated and reenters the life state and is output, and the track id still remains a.

[0157] Seven, in step 7, the track output from the vehicle further includes processing of overlapping tracks;

[0158] The judgment of track overlap refers to the overlapping area of the two-dimensional rectangular box of the projection of two tracks in the horizontal plane, and the overlapping area is greater than the threshold iou_thre, and then the two tracks are defined as overlapping tracks; since the detection effect of the network detection target is stable and the confidence is high, the tracking effect of the track with the source as the cnn is obviously better than that of the track with the source as the clu, and thus the overlapping track processing adheres to the principle of giving priority to the cnn; since the detection effect of the clustering target often appears that a large clustering rectangular box is unstable and is split into multiple small clustering rectangular boxes, and thus the overlapping track processing also adheres to the principle of giving priority to the track with large area; thus, if the track of the network detection target (cnn) overlaps with the track of the clustering target (clu), the track of the clustering target is deleted, if the tracks of the network detection target overlap with each other, the track with small area is deleted, and if the tracks of the clustering target overlap with each other, the track with small area is deleted, and the track area mentioned above is the length multiplied by the width of the track.

[0159] Thus, the output of the track is that all the tracks age after the track overlap processing is completed are added by 1, and the tracks with age greater than 5 and in the life state are output.

[0160] Compared with the prior art, the embodiment disclosed above has the following advantages:

[0161] 1. The track formed by the laser radar tracking of the application integrates network detection results and point cloud clustering results, and eliminates the drawbacks of using single network detection or clustering results for tracking in the prior art, i.e., the target may have uneven position and speed, long and wide jumps, and track ID switching;

[0162] 2. The process of the application adds a track management process, and the concepts of coast state and coastSpan set by the track management prevent track ID switching logic;

[0163] 3. The application sets a shape mutation compensation process before Kalman update, and performs Kalman update for successful shape mutation compensation, effectively reducing the phenomenon of unstable track tracking or even disassociation caused by the fluctuation of input network detection targets or clustering targets, and improving the stability of the track.

[0164] The above examples are only used to illustrate the technical solutions of the application and not to limit it, and other modifications or equivalent replacements of the technical solutions of the application made by those skilled in the art should be covered in the scope of the claims of the application, as long as they do not deviate from the spirit and scope of the technical solutions of the application.

Claims

1. A joint tracking method based on laser point cloud network detection and clustering targets, characterized in that: The method is based on two arrays sent by an upstream laser radar perception, and outputs a track array of a vehicle to realize tracking of the vehicle. The two arrays sent by the upstream laser radar perception are a network detection target array and a clustering target array, which are independent of each other. The network detection target array stores a plurality of network detection targets, and the clustering target array stores a plurality of clustering targets. The network detection target and the clustering target both contain state information of the target, each track in the track array contains state information of a target and information of a track corresponding to the target, and a network detection target or a clustering target with a large difference in length and width from a track is subjected to shape mutation compensation. The shape mutation compensation is to calculate distances between each track corner point and each network detection target corner point, and if the distance is less than a threshold value, the track corner point and the network detection target corner point are associated; or distances between each track corner point and each clustering target corner point are calculated, and if the distance is less than a threshold value, the track corner point and the clustering target corner point are associated. If the number of associated corner points is less than 2, the shape mutation compensation fails, and Kalman update is not performed; if the number of associated corner points is greater than or equal to 2, two track corner points with the smallest distance between the associated corner points and two network detection target corner points corresponding to the two track corner points are found, or two track corner points with the smallest distance between the associated corner points and two clustering target corner points corresponding to the two track corner points are found. 2.The method of claim 1, wherein: According to the coordinates of the two network detection target corner points or the two clustering target corner points, a reference edge without mutation is determined; length mutation values and width mutation values are calculated according to the length and width attributes of the track and the length and width attributes of the network detection target, and a new center point of the network detection target is obtained for Kalman update, or length mutation values and width mutation values are calculated according to the length and width attributes of the track and the length and width attributes of the clustering target, and a new center point of the clustering target is obtained for Kalman update. The state information of the target includes longitudinal position, transverse position, covariance matrix, length, width, height, and heading angle of the target.

3. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 1 or 2, characterized in that: The information of the track includes id, source, age, lifeSpan, coastSpan, and status of the track. The specific steps of the method include the following: Step 1: receiving a network detection target array and a clustering target array sent by an upstream laser radar perception of a vehicle; Step 2: performing motion compensation on all tracks of the vehicle in the last frame to convert state values of the tracks into a new self-vehicle coordinate system; Step 3: performing Kalman prediction on the tracks converted in step 2 to keep the state of the track and the state of the observation in the same time domain; Step 4: associating the predicted tracks with network detection targets; If the association is successful and there is no shape mutation, the network detection target is used to perform Kalman update on the track state; if the association is successful and there is shape mutation and the mutation compensation is successful, the compensated network detection target is used to perform Kalman update on the track state; if the association is successful and there is shape mutation and the mutation compensation fails, no operation is performed; and if the association fails, a track is created; Step 5: associating the tracks updated with the network detection targets with clustering targets. If the association is successful and there is no shape mutation, the Kalman update is performed on the track state using the clustering target; if the association is successful and there is shape mutation and the mutation compensation is successful, the Kalman update is performed on the track state using the compensated clustering target; if the association is successful and there is shape mutation and the mutation compensation fails, no operation is performed; and if the association fails, a track is created; Step 6: track management based on the track state update in step 5; Step 7: outputting the track of the ego vehicle.

4. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 3, characterized in that: In step 2, before the motion compensation, all state values of the track are in the coordinate system of the ego vehicle in the last frame, wherein the origin of the coordinate system is located at the center of the front bumper of the ego vehicle, the forward direction is the positive direction of the x-axis, and the left direction is the positive direction of the y-axis, and then the motion state coordinate system of the ego vehicle is changed to: dx = vEgo * dt dθ = yawRate * dt wherein dx is the distance of the forward movement of the coordinate system, dθ is the angle of the rotation of the coordinate system, vEgo is the speed of the ego vehicle, yawRate is the yaw rate of the ego vehicle, and dt is the execution period; Then, the motion compensation of the track is as follows: x = (x k1 -dx)*cos(dθ)-y k1 *sin(dθ) y = (x k1 -dx)*sin(dθ) + y k1 *cos(dθ) v x = v xk1 *cos(dθ) - v yk1 *sin(dθ) v y = v xk1 *sin(dθ) + v yk1 *cos(dθ) Where, x k1 y k1 v xk1 v yk1 These represent the longitudinal position, lateral position, longitudinal velocity, and lateral velocity of the trajectory before motion compensation, along with their x, y, and v values. x v y These are the longitudinal position, lateral position, longitudinal velocity, and lateral velocity of the trajectory after motion compensation.

5. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 4, characterized in that: In step 3, before the prediction of the track state, the track state is in the time of the last frame, and then the formula of the Kalman prediction is: wherein, is the motion compensated track state vector, i.e. {x, y, v x v y} ; Ck-1is the covariance matrix of the previous track, A is the state transition matrix of the track, a uniform motion model is adopted, so: After the Kalman prediction, the track state and the observation state are kept in the same time domain.

6. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 5, characterized in that: In steps 4 and 5, the association of the track with the network detection target and the association of the track with the clustering target are the same, and specifically include the following: The distance matrix of the track array and the network detection target array or the clustering target array is calculated in sequence, and the calculation is as follows: dis = dX T S -1 dX wherein dX is the difference between the state vector of the track and the state vector of the network detection target, or the difference between the state vector of the track and the state vector of the distance target; and S is the sum of the covariance matrix of the track and the covariance matrix of the network detection target, or the sum of the covariance matrix of the track and the covariance matrix of the clustering target; The association threshold is set as disThre, when dis is less than disThre, the nearest neighbor algorithm is used to perform the 1:1 association of the track and the network detection target, or the 1:1 association of the track and the clustering target; The network detection target or the clustering target that is not successfully associated is newly created, and the network detection target or the clustering target track that is successfully associated is Kalman updated.

7. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 6, characterized in that: The Kalman update further includes shape mutation compensation for the network detection target or the clustering target that has a large difference in length and width from the track.

8. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 7, characterized in that: The Kalman update mode is as follows: First, the Kalman gain is calculated: Then, the track state vector and the covariance matrix are updated: wherein Z is the state vector of the network detection target or the state vector of the clustering target; and H is an observation matrix that converts the state domain to the observation domain: wherein I is a 4-order unit matrix. 9.The method of claim 3, wherein: In step 6, the track management is specifically as follows: the track includes the lifeSpan and coastSpan attributes, the lifeSpan attribute decreases to 0, the track is in the coasted state, the coastSpan of the track is initialized, and the coasted state track is deleted when the coastSpan decreases to 0.

10. The joint tracking method for network detection and clustering targets based on laser point cloud according to claim 3, characterized in that: In step 7, the output of the track of the ego vehicle further comprises processing of overlapping tracks, specifically: The overlapping area of the two-dimensional rectangle projected by the track in the horizontal plane is used for judgment, and if the overlapping area exceeds a threshold, the track is defined as an overlapping track; If the track of the network detection target overlaps with the track of the clustered target, the track of the clustered target is deleted; if the tracks overlap within the track of the network detection target, the track with smaller area is deleted; if the tracks overlap within the track of the clustered target, the track with smaller area is deleted; the track area is the length multiplied by the width of the track; The output of the track is that all tracks age is added by 1, and the track with age greater than 5 and in the life state is output.

Images