A vehicle target tracking algorithm and system based on multi-sensor fusion

Abstract

The application relates to a vehicle target tracking algorithm and system based on multi-sensor fusion. The main steps include the following: S1, a multi-sensor module acquires sensor data; the sensor data comprises state data and observation data; S2, a target detection and tracking module allocates sensor data for multi-vehicle targets for detection and tracking, and performs state updating according to whether there is observation value, so as to acquire posterior state data and posterior covariance data; and S3, the target detection and tracking module inputs the posterior state data and the posterior covariance data into a multi-sensor fusion module, and performs time registration, track association and track fusion on each sub-target in the multi-vehicle target. The method realizes decoupling of spatial errors and time errors, so that the result is more accurate; the performance distribution weight of each sensor at the current moment is fully considered, the influence of errors on the fusion result of each target in the environment is reduced as much as possible, and the reference requirements of automatic driving can be well met.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of multi-sensor fusion algorithm, in particular to a vehicle target tracking algorithm and system based on multi-sensor fusion. BACKGROUND

[0002] Multi-sensor fusion technology refers to achieving higher precision data fusion effect by giving full play to the advantages of various sensors. The vehicle target tracking algorithm based on multi-sensor fusion (cameras, radars and lidars, etc.) is a kind of surrounding environment perception method for vehicles, which can make tracking more accurate, by acquiring surrounding environment data through sensors, then processing the original data, extracting various dimensions of information from the original data (extracting target vehicles in images, clustering point clouds to obtain obstacle distances, etc.), and then establishing a kinematic model of the target vehicle.

[0003] In the application of multi-vehicle target tracking, due to the sparse characteristics of point clouds, the point cloud clusters of multiple adjacent vehicle targets cannot be distinguished, therefore, in the prior art, a camera coupling is often added to separate the clustered point clouds through image feature extraction; in addition, lidars and millimeter wave radars have their own advantages in distance and speed measurement, so they are often used together, but multi-sensor means that the fusion result is easily affected by sensor faults. Therefore, in order to fully utilize the characteristics of multi-sensor and at the same time avoid the influence of sensor errors on the fusion result, a multi-sensor weighted fusion algorithm with error correction is often used in the prior art.

[0004] At present, the multi-sensor fusion tracking scheme similar to the above algorithm has the defect that the space error and time error cannot be decoupled, and it is difficult to ensure that all detected targets can obtain accurate values in the setting of noise, so that the fusion result is not accurate enough and the robustness cannot meet the reference requirements of automatic driving. SUMMARY

[0005] The purpose of the present application is to provide a vehicle target tracking algorithm and system based on multi-sensor fusion, which realizes the decoupling of space error and time error, makes the result more accurate, and fully considers the performance distribution weight of each sensor at the moment, minimizes the influence of errors on the fusion result of each target in this environment, and can well meet the reference requirements of automatic driving.

[0006] In order to achieve the above purpose, the technical scheme adopted by the present application is as follows:

[0007] A vehicle target tracking algorithm based on multi-sensor fusion, characterized in that it comprises:

[0008] S1, a multi-sensor module acquires sensor data; the sensor data includes state data and observation data;

[0009] S2, a target detection and tracking module assigns the sensor data to a multi-vehicle target for detection and tracking, and performs state updating according to the presence or absence of observation values, to acquire posterior state data and posterior covariance data for spatial registration;

[0010] S3, the target detection and tracking module inputs the posterior state data and the posterior covariance data into a multi-sensor fusion module, to perform time registration, trajectory association and trajectory fusion on each sub-target in the multi-vehicle target.

[0011] As a preferred embodiment of the present application, in step S1, the state data is a state vector, and the observation data is an observation vector; the state vector and the observation vector have specific forms as follows:

[0012] X = [x, y, v, θ, ω] T

[0013] Y = [R, ρ, v m θ m ] T (1)

[0014] wherein X represents a state vector; Y represents an observation vector; x and y represent the position of the target vehicle in the global coordinates; v represents the speed of the vehicle; θ represents the azimuth angle of the vehicle (the angle between the target vehicle body and the north coordinate); ω represents the angular velocity of the steering angle; R is the distance measurement of the target or clutter in the millimeter wave radar polar coordinates; ρ is the azimuth angle measurement of the target or clutter in the millimeter wave radar polar coordinates; v m is a speed scalar obtained through the Doppler effect; and θ m is a direction scalar obtained through the Doppler effect.

[0015] As a preferred embodiment of the present application, in step S1, when the multi-sensor module acquires the sensor data, it further includes a trajectory recognition processing step, specifically: acquiring sensor data at two time points at the same position to synthesize a trajectory, and performing initialization.

[0016] As a preferred embodiment of the present application, in step S2, it includes the following sub-steps:

[0017] S21, establishing a kinematic equation and a measurement equation containing unknown parameters; and acquiring an unknown parameter vector according to the kinematic equation and the measurement equation;

[0018] S22, analyzing the unknown parameter vector according to a state estimator containing unknown parameters, to acquire posterior state data;

[0019] S23, designing a minimum variance estimator to acquire posterior covariance data.

[0020] As a preferred embodiment of the present application, in step S3, the time registration method is specifically: obtaining observation data of different sensors to be registered; calculating time deviation values according to the established time kinematic equation and the observation data; and calculating time offset and updating sensor data using Kalman filtering method.

[0021] As a preferred embodiment of the present application, in step S3, the step of calculating fusion weight is further included: constructing a weight measurement model, obtaining measurement noise covariance and inputting the weight measurement model to calculate an adaptive factor.

[0022] As a preferred embodiment of the present application, in step S3, when performing trajectory association on each sub-target in the multi-vehicle target, the nearest neighbor matching principle is used to match sensor data of different sensors in the multi-sensor module for the same target value.

[0023] On the other hand, the present application also provides a vehicle target tracking system based on multi-sensor fusion, characterized in that the system comprises:

[0024] a multi-sensor module for obtaining sensor data; the sensor data includes state data and observation data;

[0025] a target detection and tracking module for receiving the sensor data, assigning the sensor data to a multi-vehicle target for detection and tracking, and updating the state according to the presence or absence of observation value to obtain posterior state data and posterior covariance data for spatial registration;

[0026] a multi-sensor fusion module for receiving the posterior state data and posterior covariance data, inputting the multi-sensor fusion module, and performing time registration, trajectory association and trajectory fusion on each sub-target in the multi-vehicle target.

[0027] In summary, the present application has the following advantages:

[0028] 1. Data fusion using multiple radars needs to consider the case of single radar failure and time error between multiple radars.

[0029] 2. Considering that vehicle acceleration state parameters are difficult to directly observe, and considering the error reading parameters of the sensor, a state estimator containing unknown parameters is set to calculate these parameters, so that other modules of automatic driving can fully utilize this information.

[0030] 3. In the multi-sensor fusion module, the performance distribution weight of each sensor at the current time is fully considered to minimize the influence of errors on the fusion results of each target in this environment.

[0031] 4. By acquiring posterior state data and posterior covariance data and time registration, the decoupling of spatial error and time error is achieved, so that the result is more accurate. BRIEF DESCRIPTION OF DRAWINGS

[0032] Figure 1 A flow chart of the method of the present application;

[0033] Figure 2 A block diagram of the system of the present application;

[0034] Figure 3 A schematic diagram of time registration in the embodiment; DETAILED DESCRIPTION

[0035] The technical solutions of the embodiments of the present application will be explained and described below in conjunction with the drawings of the embodiments of the present application. However, the following embodiments are only preferred embodiments of the present application, and are not all. Based on the embodiments in the embodiments, other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the present application.

[0036] The terms "first", "second", and the like in the description and claims of the present specification and the above drawings are used to distinguish different objects, and are not used to describe a specific order. In addition, the term "comprising" and any variation thereof is intended to cover non-exclusive inclusion. For example, a process, method, system, product or device including a series of steps or units is not limited to the listed steps or units, but can optionally include steps or units not listed, or can optionally include other steps or units inherent to the process, method, product or device.

[0037] Embodiment 1

[0038] As shown in Figure 1 , first, the multi-sensor module acquires sensor data, the sensor data includes state data and observation data, the state data is a state vector X, and the observation data is an observation vector Y. The state vector and the observation vector are specifically:

[0039] X = [x, y, v, θ, ω] T

[0040] Y = [R ρ v m θ m ] T (1)

[0041] Where X represents the state vector; Y represents the observation vector; x and y represent the position of the target vehicle in global coordinates; v represents the vehicle's velocity; θ represents the vehicle's azimuth angle (the angle between the target vehicle body and the north coordinate); ω represents the angular velocity of the steering angle; R is the range measurement of the target or clutter in millimeter-wave radar polar coordinates; ρ is the azimuth measurement of the target or clutter in millimeter-wave radar polar coordinates; v m θ is the velocity scalar obtained through the Doppler effect. m This is the directional scalar obtained through the Doppler effect.

[0042] Next, kinematic equations and measurement equations containing unknown parameters are established; the unknown parameter vector is obtained based on the kinematic equations and measurement equations.

[0043]

[0044] This method uses a variable acceleration and variable steering kinematic model to describe the vehicle state, X k Y represents the state vector at the current time k. measurement ,Y true This represents the measured value at the current moment and the actual measured value. v(t) represents the velocity relative to time t. k This represents the time parameter at that moment. Since the value of acceleration cannot be obtained directly, let the rate of change of v be α, and the rate of change of ω be ω'. Sensors in actual systems exhibit spatial biases, including distance bias and orientation bias. Distance bias refers to the systematic error μ that occurs when the sensor measures the target object. 1 Azimuth deviation refers to the azimuth angle deviation μ that occurs when the sensor antenna is aligned. 2 .

[0045] From equation (2), we can see that the unknown parameter vector is given by equation (3):

[0046] δ=[α k ω' k μ 1 μ 2 ] T (3)

[0047] Next, observation data is assigned to multiple vehicle targets for detection and tracking. Target vehicles can enter the tracking range of the vehicle's multi-sensor system at any time. Assume that target vehicle i is initialized at time k. th The trajectory is correlated based on the state information of the same location at two different times, and... Assigned to i th When tracking a target, this method will update the state based on whether there is a corresponding observation.

[0048] If there are already associated observations at this time, use the state estimator with unknown parameters to update, the specific steps are as follows:

[0049] The unknown parameter vector is analyzed according to the state estimator with unknown parameters. Specifically, the state estimator with unknown parameters is designed, which is insensitive to the set unknown parameters, can maintain the accuracy of state estimation under the condition of unknown parameters, and the form of state estimation is shown in equations (4) and (5):

[0050]

[0051] Assume that the posterior error at the last time is unbiased, then equation (5) is established.

[0052]

[0053] The purpose of the above equation is to obtain an unbiased estimate of X k+1 by giving the measurement vector Y and the nominal value k+1 At time k+1, let represent the predicted state, represent the predicted output, K k+1 is the estimator gain at time k+1, is the error in the predicted output, and the approximation is equation (6):

[0054]

[0055] where and are the linearized first-order derivatives with respect to Y and unknown parameters. Similarly, the and encountered in the following formula are the linearized first-order derivatives with respect to X and unknown parameters, where

[0056] The designed minimum variance estimator gain is, and its value is equation (7):

[0057]

[0058] where is invertible, and the prior covariance P k+1|k = F k P k|k F k T +Q, Q is the state noise covariance, and each target has a different Q. The posterior covariance of state estimation is:

[0059]

[0060] If there is no successful observation value associated with the moment, the updating of formula (6) cannot be used, and formula (7) is adopted Updating.

[0061] After the state is updated through the above process, the posterior state data and the posterior covariance data can be obtained, and the spatial registration is completed.

[0062] Next, the target detection and tracking module inputs the posterior state data and the posterior covariance data into the multi-sensor fusion module to perform time registration, trajectory association and trajectory fusion on each sub-target in the multi-vehicle target.

[0063] First, the observation data of different sensors to be registered is obtained; the time deviation value is calculated according to the established time kinematic equation and the observation data; the time offset is calculated using the Kalman filtering method and the sensor data is updated. In order to filter the time as a state, a kinematic equation related to time needs to be established, such as formula (9):

[0064]

[0065] wherein, represents the time from to The state is represented as the target The true state at the moment , but in fact the true state vector cannot be obtained, so the present application uses to replace it.

[0066] The state difference of two sensors under time deviation is given by formula (10):

[0067]

[0068] Let be the pseudo-measurement equation noise of time delay estimation, and the corresponding covariance matrix is:

[0069]

[0070] Through transformation, the pseudo-measurement equation about Vt(k) can be obtained:

[0071]

[0072] Please note that the necessary condition for the above process to be realized is It is known that in practical application, the internal clock of the laser radar and the updating algorithm are the most accurate, and in multi-sensor time synchronization, the most accurate laser radar is taken as the standard reference to synchronize other multi-sensors.

[0073] More specifically, as Figure 3As shown, assuming sensor 1, 2 produced observations of the same target in their respective sampling periods, at the kth time step, respectively and Start transmission, time-consuming In and The moment arrives at the multi-sensor fusion module, define the arrival time difference Observations are The target is Take the target state tracked by sensor 1 as an example, assuming the target motion state is uniform, the state matrix F is:

[0074]

[0075] In order to calculate the time deviation of the two sensors, two state values are needed, taking the target state tracked by the sensor as an example, The difference between the two states is formula (10).

[0076]

[0077] From equations (2), (6), (7), we change the state vector to time variable:

[0078]

[0079] The pseudo-measurement equation is:

[0080]

[0081] Calculate the new time offset Vt(k) through the Kalman filter scheme:

[0082]

[0083] From this we get the time difference between the sensors, update one of the sensor tracking state vectors:

[0084]

[0085] But the sensor has different accuracy for detecting targets at different distances, the noise covariance of the filter needs to be dynamically adjusted in size to improve the overall accuracy. Add adaptive factor γ k , the expression is given by (17):

[0086]

[0087] H k+1 , P k+1 are the innovation vector, the first order derivative of the measurement equation, and the state covariance. VV krepresent the uncertainty degree of error model. When the noise increases, the accuracy of the sensor is considered to decrease, and the fusion weight of the sensor is reduced The higher the value is, the smaller the influence of the sensor on the fusion result is

[0088]

[0089] When the sensor j th adaptive factor The larger the adaptive factor is, the smaller the corresponding is. According to the updated state and covariance, the formula (19) is obtained

[0090]

[0091] The superscript s,j represents the observation result of the s-th sensor on the j-th target. The superscript f represents the fusion variable. represents the noise covariance of the current target, represents that the covariance is adaptive and is transmitted to the sensor at the next moment.

[0092] Finally, the target detection and tracking module performs track association and track fusion on each sub-target in the multi-vehicle target. For track association of multiple sensors, whether the objects tracked by each sensor are the same target is determined by using the nearest neighbor matching principle to match the two nearest data. After matching, the posterior state of each independent target is input into the multi-sensor fusion module, j represents the type of sensor, which is divided into laser radar and millimeter wave radar in the method, and the posterior state after fusion is represented as f represents fusion, which is calculated by . After that, the result is returned to the filter as the state information X k+1 at the next moment k+2.

[0093] Embodiment 2

[0094] As shown in Figure 2 , the present application also provides a vehicle target tracking system based on multi-sensor fusion, characterized in that the system comprises:

[0095] a multi-sensor module for acquiring sensor data; the sensor data comprises state data and observation data;

[0096] a target detection and tracking module for receiving the sensor data, assigning the sensor data to the multi-vehicle target for detection and tracking, and updating the state according to whether there is an observation value to obtain posterior state data and posterior covariance data for spatial registration;

[0097] A multi-sensor fusion module is used to receive the posterior state data and posterior covariance data, and the multi-sensor fusion module is input to perform time registration, track association and track fusion on each sub-target in the multi-vehicle target.

[0098] Based on the above embodiments, more specific examples are given to illustrate the technical solutions of the present application:

[0099] This example is the process of camera / laser radar / millimeter wave radar fusion tracking multiple targets, and the operation process is as follows:

[0100] 1. Sensor data: Y = [R p v m θ m ] T During initialization and operation, point cloud data needs to exist at least at two time instants and at the same position to be considered as a track rather than noise, which controls the complexity of data association and improves the real-time performance of the system. During the initialization operation of the track, the size of the state and measurement noise covariance needs to be set, and the state noise covariance Q k|k is set by experience, and the size is:

[0101]

[0102] The azimuth angle, distance and speed resolution of the millimeter wave radar are respectively: [4°, 2.5m, 0.5m / s]; the deviations are respectively: [0.4°, 0.125m, 0.025m / s]; the azimuth angle and distance resolution of the laser radar are: [0.16°, 0.002m]; the deviations are respectively: [0.016°, 0.0002m].

[0103] Taking the millimeter wave radar as an example, the size of the corresponding observation noise covariance R can be calculated according to the detected deviation value:

[0104]

[0105] 2. According to the observation data input into the filter, the state of the target is posteriorly corrected, wherein the state is X k =[x,y,v, θ, ω] T The noise covariance matrix R and Q of each target tracking track are initialized. During the operation process, the noise covariance of each track is dynamically adjusted, and there is which represents the accuracy of the current time of the sensor. Using the filter containing the unknown parameter estimation, the Q and R matrices are substituted into equations (5), (7) and (8) to obtain the posterior state results and P k+1|k+1 which are not sensitive to errors.

[0106] 3. The multiple targets tracked by different sensors are and P k+1|k+1 The input multi-sensor fusion module matches the tracking values of the same target by different sensors according to the nearest neighbor. After matching, the posterior state of each independent target is The input multi-sensor fusion module, j represents the type of sensor, which is divided into laser radar and millimeter wave radar in this method, and the posterior state after fusion is represented as f represents fusion, which is calculated as The result will be returned to the filter as the state information X k+1 .

[0107] The above only describes the preferred embodiments of the present disclosure and the explanation of the applied technical principles. Those skilled in the art should understand that the protection scope of the present disclosure is not limited to the technical solutions formed by the specific combinations of the above technical features, and also covers other technical solutions formed by any combinations of the above technical features or equivalent features without departing from the disclosed concept. For example, the technical solutions formed by replacing the above features with the technical features disclosed in the present disclosure (but not limited to) having similar functions.

[0108] In addition, although the above discussion contains several specific implementation details, these should not be interpreted as limiting the scope of the present disclosure. Certain features described in the context of separate embodiments can also be implemented in combination in a single embodiment. Conversely, various features described in the context of a single embodiment can also be implemented separately or in any suitable sub-combination in multiple embodiments.

Claims

1. A vehicle target tracking algorithm based on multi-sensor fusion, characterized in that, The system comprises: S1, a multi-sensor module acquires sensor data; The sensor data comprises state data and observation data; S2, a target detection and tracking module assigns the sensor data to a multi-vehicle target for detection and tracking; state updating is performed according to whether there is an observation value, and posterior state data and posterior covariance data are acquired for spatial registration; The target detection and tracking module assigns observation data to multiple vehicle targets. Initialize the target vehicle at all times The trajectory is correlated based on the state information of the same location at two different times, and... Assigned to Track the target; Indicates the current time Given a state vector, design a state estimator with unknown parameters; If there is an observation value that has been successfully associated at this moment, an unknown parameter state estimator is used for updating, and an unknown parameter vector is resolved; the form of state estimation is shown in equations (4) and (5): （4） If the posterior error at the previous time step is unbiased, then an unbiased estimate of is obtained by giving the measurement vector a nominal value ; （5） At time, denotes a predicted state, denotes a predicted output, is an estimator gain at time, is an error in the predicted output, the approximation being （6） where and is the linearized first derivative with respect to Y and unknown parameters; the designed minimum variance estimator gain is with the value of (7) equation: （7） wherein is invertible, the a priori covariance , is the state noise covariance; R is the observation noise covariance matrix; The posterior covariance of state estimation is: （8） If there is no successful observation associated with this time instant, then the last successful observation is used update; S3, the target detection and tracking module inputs the posterior state data and posterior covariance data into a multi-sensor fusion module, and time registration, track association and track fusion are performed on each sub-target in the multi-vehicle target. 2.The vehicle target tracking algorithm based on multi-sensor fusion according to claim 1, characterized in that, In step S1, the state data is a state vector, and the observation data is an observation vector; the state vector and the observation vector have specific forms as follows: , where X denotes the state vector; Y denotes the observation vector; and denotes the position of the target vehicle in global coordinates; denotes the velocity of the vehicle; denotes the orientation angle of the vehicle; denotes the angular velocity of the steering angle; is the range measurement of the target or clutter in polar coordinates of the millimeter wave radar; is the azimuth angle measurement of the target or clutter in polar coordinates of the millimeter wave radar; is the velocity scalar obtained by the Doppler effect; is the direction scalar obtained by the Doppler effect.

3. The vehicle target tracking algorithm based on multi-sensor fusion according to claim 2, characterized in that, In step S1, when the multi-sensor module acquires sensor data, it further comprises a track recognition processing step, specifically: acquiring sensor data at two time points at the same position to synthesize a track, and performing initialization.

4. The vehicle target tracking algorithm based on multi-sensor fusion according to claim 3, characterized in that, In step S2, it comprises the following sub-steps: S21, a kinematic equation and a measurement equation containing unknown parameters are established; and an unknown parameter vector is acquired according to the kinematic equation and the measurement equation; S22, the unknown parameter vector is resolved according to the state estimator containing unknown parameters, and posterior state data is acquired; S23, a minimum variance estimator is designed, and posterior covariance data is acquired.

5. The vehicle target tracking algorithm based on multi-sensor fusion according to claim 4, characterized in that, In step S3, the time registration method specifically comprises: acquiring observation data of different sensors that need to be registered; calculating a time deviation value according to an established time kinematic equation and the observation data; and calculating a time offset value using a Kalman filtering method and updating sensor data.

6. The vehicle target tracking algorithm based on multi-sensor fusion according to claim 5, characterized in that, In step S3, it further comprises a fusion weight calculation step: a weight measurement model is constructed, measurement noise covariance is acquired and input into the weight measurement model, and an adaptive factor is calculated.

7. The vehicle target tracking algorithm based on multi-sensor fusion according to claim 6, characterized in that, In step S3, when track association is performed on each sub-target in the multi-vehicle target, the nearest neighbor matching principle is used to match sensor data of the same target value from different sensors in the multi-sensor module.

8. A multi-sensor fusion based vehicle target tracking system, characterized by, The system comprises: A multi-sensor module for acquiring sensor data; the sensor data comprises state data and observation data; The target detection and tracking module is used to receive the sensor data, allocate the sensor data to multiple vehicle targets for detection and tracking, update the state based on whether there are observations, and obtain posterior state data and posterior covariance data for spatial registration. Initialize the target vehicle at all times The trajectory is correlated based on the state information of the same location at two different times, and... Assigned to Track the target; Indicates the current time Given a state vector, design a state estimator with unknown parameters; If there is an observation value that has been successfully associated at this moment, an unknown parameter state estimator is used for updating, and an unknown parameter vector is resolved; the form of state estimation is shown in equations (4) and (5): （4） If the posterior error at the previous time step is unbiased, then an unbiased estimate of is obtained by giving the measurement vector a nominal value ; （5） At time, denotes a predicted state, denotes a predicted output, is an estimator gain at time, is an error in the predicted output, the approximation being （6） where and is the linearized first derivative with respect to Y and unknown parameters; the designed minimum variance estimator gain is with the value of (7) （7） wherein is invertible, a priori covariance , is the state noise covariance; R is the observation noise covariance matrix; The posterior covariance of state estimation is: （8） If there is no successful observation associated with this time instant, then the last successful observation is used update; State updating is performed according to whether there is an observation value, and posterior state data and posterior covariance data are acquired for spatial registration; A multi-sensor fusion module for receiving the posterior state data and the posterior covariance data, inputting the multi-sensor fusion module, and performing time registration, track association and track fusion on each sub-target in the multi-vehicle target.

Images