Non-sequential measurement fusion track updating method, device and computer equipment

Abstract

The present application relates to a method, device and computer equipment for updating a fused track of non-sequential measurement. The method includes: obtaining non-sequential measurement and the fused track state corresponding to the current moment; the moment corresponding to the non-sequential measurement is the first moment, and the current moment corresponding to the fused track state is the second moment; performing state prediction based on the fused track state to obtain a first predicted track state corresponding to the first moment; performing state prediction based on the first predicted track state corresponding to the first moment to obtain a second predicted track state corresponding to the second moment; determining equivalent measurement based on the second predicted track state and the fused track state; the equivalent measurement is the equivalent measurement corresponding to the first moment to the second moment; updating the fused track state based on the equivalent measurement and the non-sequential measurement to obtain an updated fused track state. Based on the above method, the target tracking accuracy can be improved and a large amount of storage space can be saved.

Description

Technical Field

[0001] The present application relates to the field of sensor fusion technology, and in particular to a non-sequential measurement fusion track update method, device and computer equipment. Background Art

[0002] In the field of intelligent driving technology, heterogeneous multi-sensor fusion algorithms are an important part of intelligent driving systems. Heterogeneous multi-sensor fusion algorithms fuse the perception information of multiple different types of sensors on the vehicle side, combine the characteristics of the sensors, and make the best use of the strengths and avoid the weaknesses of the sensor performance, providing richer and more accurate information for the decision-making of intelligent driving on the vehicle side. At present, with the development and maturity of sensor technology, and the demand for sensor perception performance of advanced driver assistance systems (ADAS), more and more types of sensors are deployed on the vehicle side. The fusion module on the vehicle side that executes the heterogeneous multi-sensor fusion algorithm can provide richer and more accurate perception information to meet the needs of advanced ADAS functions by receiving more diverse sensor data.

[0003] In the current traditional technology, when the vehicle actually tracks the target through sensors, due to the different processing times and sampling speeds of different types of sensors, coupled with the inherent random communication delays of different sensor information transmission channels, the sensor's perception measurement information will arrive at the vehicle-side fusion module out of order, that is, the measurement information that comes earlier in time arrives at the fusion module after the measurement information that comes later in time, which is non-sequential measurement. Non-sequential measurement will significantly affect the target tracking accuracy of the heterogeneous multi-sensor fusion algorithm. Summary of the invention

[0004] Based on this, it is necessary to provide a non-sequential measurement fusion track update method, device and computer equipment to address the above technical problems.

[0005] In a first aspect, the present application provides a method for updating a fused track of non-sequential measurements, the method comprising: obtaining non-sequential measurements and a fused track state corresponding to a current moment; the moment corresponding to the non-sequential measurements is a first moment, and the current moment corresponding to the fused track state is a second moment; performing state prediction based on the fused track state to obtain a first predicted track state corresponding to the first moment; performing state prediction based on the first predicted track state corresponding to the first moment to obtain a second predicted track state corresponding to the second moment; determining an equivalent measurement based on the second predicted track state and the fused track state; the equivalent measurement is an equivalent measurement corresponding to the first moment to the second moment; updating the fused track state based on the equivalent measurement and the non-sequential measurements to obtain an updated fused track state.

[0006] In one embodiment, performing state prediction based on the fused track state to obtain a first predicted track state corresponding to the first moment includes: acquiring a motion state transfer matrix, a coordinate system rotation change matrix, a driving distance matrix, and a process noise matrix; performing state prediction based on the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix, the process noise matrix, and the fused track state to obtain a first predicted track state corresponding to the first moment.

[0007] In one embodiment, obtaining a motion state transfer matrix, a coordinate system rotation change matrix, and a driving distance matrix includes: obtaining a target object motion model, a rotation angle of the vehicle from the first moment to the second moment, a time interval from the first moment to the second moment, and a longitudinal speed of the vehicle at the second moment; determining a motion state transfer matrix based on the target object motion model and the time interval; constructing a coordinate system rotation change matrix based on the rotation angle of the vehicle; and constructing a driving distance matrix based on the longitudinal speed of the vehicle, the time interval, and the rotation angle of the vehicle.

[0008] In one of the embodiments, the state prediction is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix, the process noise matrix and the fused track state to obtain the first predicted track state corresponding to the first moment, including: the fused track state includes fused motion state information and the corresponding fusion covariance matrix; prediction is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix and the fused motion state information to obtain the first predicted motion state information corresponding to the first moment; prediction is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the process noise matrix and the fusion covariance matrix to obtain the first predicted covariance matrix corresponding to the first moment; the first predicted track state corresponding to the first moment is determined according to the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix.

[0009] In one embodiment, determining the equivalent measurement based on the second predicted track state and the fused track state includes: the second predicted track state includes the second predicted motion state information corresponding to the second moment and the second predicted covariance matrix; the fused track state includes the fused motion state information and the corresponding fused covariance matrix; determining the observation variance of the equivalent measurement based on the second predicted covariance matrix and the fused covariance matrix; determining the filtering gain of the equivalent measurement based on the observation variance and the fused covariance matrix; determining the new information of the equivalent measurement based on the second predicted motion state information, the fused motion state information and the filtering gain; determining the covariance matrix of the new information based on the second predicted covariance matrix and the observation variance; determining the equivalent measurement based on the new information of the equivalent measurement and the covariance matrix of the new information.

[0010] In one of the embodiments, the fused track state is updated according to the equivalent measurement and the non-sequential measurement to obtain the updated fused track state, which includes: inputting the equivalent measurement, the non-sequential measurement and the fused track state into an A1 algorithm based on a one-step lag to obtain an updated fused track state.

[0011] In one of the embodiments, the equivalent measurement, non-sequential measurement and fused track state are input into the A1 algorithm based on one-step lag to obtain an updated fused track state, including: the fused track state includes fused motion state information and a corresponding fusion covariance matrix; based on the equivalent measurement and the fused motion state information, the predicted measurement at the first moment is determined; based on the predicted measurement and the non-sequential measurement, the fused track state is updated to obtain an updated fused track state.

[0012] In the second aspect, the present application also provides a fused track updating device for non-sequential measurement, the device comprising: an acquisition module, used to acquire non-sequential measurement and the fused track state corresponding to the current moment; the moment corresponding to the non-sequential measurement is the first moment, and the current moment corresponding to the fused track state is the second moment; a first prediction module, used to perform state prediction based on the fused track state, and obtain a first predicted track state corresponding to the first moment; a second prediction module, used to perform state prediction based on the first predicted track state corresponding to the first moment, and obtain a second predicted track state corresponding to the second moment; an equivalent measurement calculation module, used to determine an equivalent measurement based on the second predicted track state and the fused track state; the equivalent measurement is the equivalent measurement corresponding to the first moment to the second moment; an update module, used to update the fused track state based on the equivalent measurement and the non-sequential measurement, and obtain an updated fused track state.

[0013] In a third aspect, the present application further provides a computer device, wherein the computer device comprises a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the method for updating a fused track of any non-sequential measurement in the first aspect is implemented.

[0014] In a fourth aspect, the present application further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the computer program implements any one of the non-sequential measurement fusion track update methods in the first aspect.

[0015] The above-mentioned non-sequential measurement fusion track update method, device and computer equipment obtain the non-sequential measurement and the fusion track state corresponding to the current moment, wherein the moment corresponding to the non-sequential measurement is the first moment, and the current moment corresponding to the fusion track state is the second moment; state prediction is performed according to the fusion track state to obtain the first predicted track state corresponding to the first moment; state prediction is performed according to the first predicted track state corresponding to the first moment to obtain the second predicted track state corresponding to the second moment, and the equivalent measurement is determined according to the second predicted track state and the fusion track state, wherein the equivalent measurement is the equivalent measurement corresponding to the first moment to the second moment; finally, the fusion track state is updated according to the equivalent measurement and non-sequential measurement to obtain the updated fusion track state. The first predicted track state is predicted by the fusion track state at the current moment, and the second predicted track state is predicted by the first predicted track state. The equivalent measurement is determined according to the second predicted track state and the fusion track state, and finally the fusion track state is updated according to the equivalent measurement and non-sequential measurement, and the non-sequential measurement is applied to the fusion track state, thereby improving the target tracking accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a schematic diagram of non-sequential measurement in one embodiment;

[0017] Figure 2 It is a flowchart of a non-sequential measurement fusion track update method in one embodiment;

[0018] Figure 3 A schematic diagram of a flow chart of a track prediction method in one embodiment;

[0019] Figure 4 A schematic diagram of a flow chart of an equivalent measurement calculation method in one embodiment;

[0020] Figure 5 A schematic diagram of a multi-sensor fusion process in one embodiment;

[0021] Figure 6 A schematic diagram of a measurement update process in one embodiment;

[0022] Figure 7 is a timing diagram of non-sequential measurement in a specific embodiment;

[0023] Figure 8 The tracking comparison results of three algorithms in a specific embodiment are shown;

[0024] Fig. 9 It is a structural block diagram of a non-sequential measurement fusion track updating device in one embodiment;

[0025] Fig.10 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0027] In the embodiments of the present application, Figure 1 The figure shows a schematic diagram of non-sequential measurement. The current heterogeneous multi-sensor fusion algorithm is in the k+1th cycle, that is, between time k and time k+1. At this time, the fusion module on the vehicle side receives the perception measurement of sensor A and sensor B for the same target. That is, the perception measurement of sensor A is received. , the actual detection time is ; Receive the sensor B's sensor measurement , the actual detection time is At this time, the time of fusion track is The relationship between the three times is . Perception measurement for the above situation This is a non-sequential measurement.

[0028] In one of the embodiments, for non-sequential measurements, the non-sequential measurements are divided into three cases according to the specific arrival time of the non-sequential measurements. The first case is a single non-sequential measurement with a one-step lag, that is, a non-sequential measurement, and the non-sequential measurement is delayed by one cycle to reach the fusion module on the vehicle side; the second case is a single non-sequential measurement with multiple-step lags, that is, a non-sequential measurement, and the non-sequential measurement is delayed by multiple cycles to reach the fusion module on the vehicle side; the third case is multiple non-sequential measurements with multiple-step lags, that is, there are multiple non-sequential measurements, and the multiple non-sequential measurements are all multi-step lags. In response to the above problems, a variety of solutions are proposed: the first is a re-filtering method, which stores the measurement data and the intermediate processing results of the algorithm for a period of time in the past. When a delayed measurement is received, the filtering process is returned to the measurement moment and filtered again. The second is a data caching method, which stores sensor measurements for a period of time, sorts the measurement data before the maximum delay time by perception time, and then performs sequential filtering. The third method is to discard the delayed measurement. This method adopts the strategy of discarding the delayed measurement and does not use it to update the track state. The fourth method is the measurement prediction method. This method predicts the delayed measurement information to the current track moment, and then uses the predicted measurement to update. The fifth method is the direct update method. This method obtains the statistical information between the current track state and the delayed measurement based on the linear minimum estimation, and then directly uses the delayed measurement to update the current track state.

[0029] When actually processing non-sequential measurements, heterogeneous multi-sensor fusion algorithms need to consider target tracking accuracy, as well as the consumption of computing resources and memory resources. Although the re-filtering algorithm has the highest accuracy, its computational workload and memory resource consumption are very large, and it is not suitable for actual engineering. The data caching method reduces the amount of computation by increasing storage space while ensuring accuracy, but due to its caching mechanism, the algorithm will not use the latest measurement information, and performs poorly in scenarios with high real-time requirements. The rule of discarding delayed measurements is not applicable to the ADAS mass production field. For cost considerations, the redundancy of perception information in ADAS mass production is generally small, and the strategy of discarding data is not advisable. The measurement prediction method consumes less computing resources and storage resources, and has no delay problem, but this method requires the measurement information provided by the sensor to adapt to the prediction model, that is, the measurement is predictable. If the measurement cannot adapt to the prediction model, this method cannot be applied. The direct update method has the potential for high-precision filtering performance and controllable computational complexity. However, since the direct update method needs to record the results of algorithm iterations within multiple cycles, its storage capacity will increase rapidly with the increase of sensor delay time, the number of tracks within the algorithm, and the frequency of algorithm operation. Therefore, it is difficult to apply in engineering.

[0030] Based on the above, an embodiment of the present application also provides a fusion track update method for non-sequential measurement, which uses the predicted amount of the current track state to approximate the iterative results of multiple cycles in the past, and combines the algorithm with sequential fusion. This method can effectively solve the problem of non-sequential measurement, and greatly reduce the required storage space while ensuring accuracy, timeliness and controllable computational complexity.

[0031] In one embodiment, Figure 2 As shown, a non-sequential measurement fusion track update method is provided, comprising the following steps:

[0032] Step 201, obtaining non-sequential measurements and the fused track state corresponding to the current moment.

[0033] This method is described using the vehicle side as an example. The vehicle side is provided with various types of sensors, wherein the sensors may include: image sensors, laser radars, millimeter wave radars, ultrasonic sensors, inertial measurement units, and global positioning systems. This embodiment does not specifically limit the sensor types, and it is only necessary to set various types of sensors. During the driving of the vehicle, the vehicle side obtains measurements corresponding to various types of sensors, and each measurement corresponds to a timestamp when the measurement is collected. Among them, the sensor collects data, identifies the target object in the data, and calculates the position, velocity, and acceleration information of the target object, and uses the position, velocity, and acceleration of the target object as the measurement corresponding to the timestamp. For the same target object, the vehicle side can obtain measurements corresponding to different sensors. If the measurement obtained by the vehicle side is not a non-sequential measurement, the Kalman algorithm is used to use the measurement to directly update the fused track state of the corresponding target object. By comparing the timestamp of the acquired measurement with the timestamp of the current fused track state, it can be determined whether the measurement is a non-sequential measurement. If the timestamp of the measurement is earlier than the timestamp of the current fused track state, the corresponding measurement is a non-sequential measurement, otherwise it is not a non-sequential measurement. When it is determined that the acquired measurement is a non-sequential measurement, the non-sequential measurement and the fused track state of the same target object are acquired. Among them, the fused track state includes fused motion state information and the corresponding fused covariance matrix. The fused motion state information includes the longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the target object. The fused covariance matrix is ​​a 6×6 matrix, which represents the longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration, the relationship between the two of the six dimensions and the covariance of each dimension. After acquiring the non-sequential measurement and the fused track state of the same target object, it can be determined according to the timestamp that the time corresponding to the non-sequential measurement is the first time, the current time corresponding to the fused track state is the second time, and the first time is earlier than the second time.

[0034] Step 202, performing state prediction based on the fused track state to obtain a first predicted track state corresponding to a first moment.

[0035] After acquiring the fused track state, it is necessary to predict the first predicted track state corresponding to the first moment of the non-sequential measurement based on the fused track state. Among them, the first predicted track state includes: the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix corresponding to the first moment. The first predicted motion state information includes: the longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the target object at the first moment predicted according to the fused track state. The first predicted covariance matrix includes: the covariance matrix of the target object at the first moment predicted according to the fused track state. The representation and meaning of the first predicted covariance matrix are the same as those of the fused covariance matrix, only the corresponding time is different.

[0036] Step 203, performing state prediction according to the first predicted track state corresponding to the first moment, and obtaining a second predicted track state corresponding to the second moment.

[0037] After predicting the first predicted track state corresponding to the first moment, it is necessary to predict the second predicted track state corresponding to the second moment based on the first predicted track state. Among them, the second predicted track state includes: the second predicted motion state information corresponding to the second moment and the second predicted covariance matrix corresponding to the second moment. The second predicted motion state information includes: the longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the target object at the second moment predicted according to the first predicted track state. The second predicted covariance matrix includes: the covariance matrix of the target object at the second moment predicted according to the first predicted track state. The representation and meaning of the second predicted covariance matrix are the same as those of the fusion covariance matrix.

[0038] Step 204: Determine an equivalent measurement according to the second predicted track state and the fused track state.

[0039] The equivalent measurement is the equivalent measurement corresponding to the first moment to the second moment. The equivalent measurement includes the innovation of the equivalent measurement and the covariance matrix of the innovation. The innovation represents the deviation between the fused track state and the predicted second predicted track state.

[0040] Step 205 , updating the fused track state according to the equivalent measurement and the non-sequential measurement to obtain an updated fused track state.

[0041] After determining that equivalent measurements are obtained, the fused track state is updated in combination with the non-sequential measurements to obtain an updated fused track state.

[0042] The embodiment of the present application obtains non-sequential measurement and the fused track state corresponding to the current moment, wherein the moment corresponding to the non-sequential measurement is the first moment, and the current moment corresponding to the fused track state is the second moment; performs state prediction based on the fused track state to obtain the first predicted track state corresponding to the first moment; performs state prediction based on the first predicted track state corresponding to the first moment to obtain the second predicted track state corresponding to the second moment, and determines the equivalent measurement based on the second predicted track state and the fused track state, wherein the equivalent measurement is the equivalent measurement corresponding to the first moment to the second moment; finally, updates the fused track state based on the equivalent measurement and the non-sequential measurement to obtain the updated fused track state. Predict the first predicted track state through the fused track state at the current moment, then predict the second predicted track state through the first predicted track state, determine the equivalent measurement based on the second predicted track state and the fused track state, and finally update the fused track state based on the equivalent measurement and non-sequential measurement, and apply the non-sequential measurement to the fused track state, thereby improving the target tracking accuracy. The first predicted track state at the first moment is predicted by the current fused track state, and it is approximated as the fused track state at the first moment. The first predicted track state is directly used to update the fused track state at the current moment, without recording the results of algorithm iterations in multiple cycles. This overcomes the problem that the storage capacity of the direct update method will increase rapidly with the increase of sensor delay time, the number of tracks in the algorithm, and the frequency of algorithm operation.

[0043] In one embodiment, when predicting a first predicted track state corresponding to a first moment according to the fused track state, Figure 3 As shown, a track prediction method is provided, comprising the following steps:

[0044] Step 301, obtaining a motion state transfer matrix, a coordinate system rotation change matrix, a driving distance matrix, and a process noise matrix.

[0045] Specifically, first obtain the target object motion model, the rotation angle of the vehicle from the first moment to the second moment, the time interval from the first moment to the second moment, and the longitudinal speed of the vehicle at the second moment. Among them, the target object motion model includes a uniform motion model, a constant acceleration motion model, and a variable acceleration motion model. The target object is a target object corresponding to the non-sequential measurement. In this embodiment, the motion model of the target object is set to a constant acceleration motion model (Constant acceleration, CA). The rotation angle of the vehicle is the angle corresponding to the body of the vehicle at the first moment when the angle of the body of the vehicle at the second moment is zero degrees, that is, the rotation angle of the body in the time interval from the first moment to the second moment.

[0046] According to the target object motion model and time interval, the motion state transfer matrix is ​​determined. When the target object motion model is a constant acceleration motion model, the motion state transfer matrix is: , where kl represents the first moment, i.e., the moment corresponding to the non-sequential measurement; k represents the second moment, i.e., the moment corresponding to the fusion track state. The time interval from the first moment to the second moment is dt, where .final It is expressed as:

[0047]

[0048] A row of the above matrix represents longitudinal position, transverse position, longitudinal velocity, transverse velocity, longitudinal acceleration and transverse acceleration from left to right, and a column of the above matrix represents longitudinal position, transverse position, longitudinal velocity, transverse velocity, longitudinal acceleration and transverse acceleration from top to bottom.

[0049] According to the rotation angle of the ego vehicle, a coordinate system rotation change matrix is ​​constructed. The rotation angle of the ego vehicle is expressed as θ=yawRate×dt. Among them, yawRate represents the angular velocity of the ego vehicle at the second moment, which can be collected by the ego vehicle. dt represents the time interval from the first moment to the second moment. The final coordinate system rotation change matrix Rt is expressed as:

[0050]

[0051] A row of the above matrix represents longitudinal position, transverse position, longitudinal velocity, transverse velocity, longitudinal acceleration and transverse acceleration from left to right, and a column of the above matrix represents longitudinal position, transverse position, longitudinal velocity, transverse velocity, longitudinal acceleration and transverse acceleration from top to bottom.

[0052] The driving distance matrix is ​​constructed based on the longitudinal speed of the vehicle, the time interval and the rotation angle of the vehicle. It is expressed as:

[0053]

[0054] in, represents the longitudinal velocity of the vehicle at the second moment, dt represents the time interval from the first moment to the second moment, and θ represents the rotation angle of the vehicle. A column of the above matrix represents the longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration, and lateral acceleration from top to bottom.

[0055] Process Noise Matrix It is a preset empirical value, which represents the error between the target object motion model and the target object's true motion model, and its distribution conforms to the Gaussian distribution with a mean of 0.

[0056] Step 302 , performing state prediction based on the motion state transfer matrix, the coordinate system rotation change matrix, the travel distance matrix, the process noise matrix and the fused track state to obtain a first predicted track state corresponding to the first moment.

[0057] During the driving process, the vehicle will continuously receive the sensor measurements. The vehicle will sort the received measurements according to the size of the timestamp in one processing cycle, and process each measurement in order from small to large timestamps. That is, the fused track state is updated with the measurement corresponding to the small timestamp first, and then the fused track state is updated with the measurement corresponding to the large timestamp, so as to obtain the fused track state at the current moment. The fused track state at the second moment is expressed as as well as .in, To fuse motion state information, is the corresponding fusion covariance matrix. According to the motion state transfer matrix, coordinate system rotation change matrix, driving distance matrix, process noise matrix and fusion track state, the state prediction is performed to obtain the first predicted track state corresponding to the first moment, where the prediction includes two parts, the prediction based on the motion model and the compensation of the vehicle motion.

[0058] According to the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix and the motion state information, a prediction is performed to obtain the first predicted motion state information corresponding to the first moment. Specifically, it can be expressed as:

[0059]

[0060] in, Indicates first predicted motion state information corresponding to a first moment; represents the motion state transfer matrix; Rt represents the coordinate system rotation change matrix; represents the driving distance matrix; Indicates the fused motion state information at the second moment.

[0061] According to the motion state transfer matrix, the coordinate system rotation change matrix, the process noise matrix and the fusion covariance matrix, the prediction is performed to obtain the first prediction covariance matrix corresponding to the first moment. Specifically, it can be expressed as:

[0062]

[0063] in, represents the first prediction covariance matrix corresponding to the first moment; represents the motion state transfer matrix; Rt represents the coordinate system rotation change matrix; represents the fusion covariance matrix at the second moment; represents the process noise matrix.

[0064] After predicting the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix corresponding to the first moment, determine the first predicted track state corresponding to the first moment according to the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix. That is, the first predicted motion state information and the first predicted covariance matrix are used as the first predicted track state. Finally, the first predicted track state is approximated as the fused track state corresponding to the first moment.

[0065] In this embodiment, when updating the fused track of non-sequential measurement, the first predicted track state of the non-sequential measurement corresponding to the first moment is predicted by the fused track state at the current moment, i.e., the second moment, so as to avoid storing a large number of iterative update results at different moments during the track update process, thereby reducing the amount of data storage.

[0066] In one of the embodiments, a state prediction is performed based on the first predicted track state corresponding to the first moment to obtain a second predicted track state corresponding to the second moment. When predicting the second predicted track state through the first predicted track state, the vehicle side stores the vehicle's yaw angular velocity and the vehicle's longitudinal velocity corresponding to the first moment. The prediction method is the same as the method of predicting the first predicted track state based on the fused track state. Therefore, for the specific implementation method of predicting the second predicted track state through the first predicted track state, reference can be made to the method of predicting the first predicted track state based on the fused track state described above, which will not be repeated here. The predicted second predicted track state corresponding to the second moment includes: the second predicted motion state information corresponding to the second moment And the second prediction covariance matrix corresponding to the second moment .

[0067] In one embodiment, an equivalent measurement is determined based on the second predicted track state and the fused track state. Figure 4 As shown, a method for calculating equivalent measurement is provided, comprising the following steps:

[0068] Step 401: Determine the observation variance of the equivalent measurement according to the second prediction covariance matrix and the fusion covariance matrix.

[0069] The second predicted track state includes: the second predicted motion state information corresponding to the second moment And the second prediction covariance matrix corresponding to the second moment The fused track status includes: fused motion status information And the fusion covariance matrix The observed variance of the equivalent measurement can be expressed as:

[0070]

[0071] in, represents the observed variance of the equivalent measurement; represents the fusion covariance matrix; represents the second prediction covariance matrix.

[0072] Step 402: Determine the filter gain of the equivalent measurement according to the observed variance and the fused covariance matrix.

[0073] The equivalent measured filter gain can be expressed as:

[0074]

[0075] in, represents the filter gain of the equivalent measurement; represents the fusion covariance matrix; represents the observed variance of the equivalent measurement.

[0076] Step 403: Determine the new information of the equivalent measurement according to the second predicted motion state information, the fused motion state information and the filter gain.

[0077] The innovation of equivalent measurement can be expressed as:

[0078]

[0079] in, Indicates the new information of equivalent measurement; Indicates fusion motion state information; indicating second predicted motion state information; Represents the filter gain of the equivalent measurement.

[0080] Step 404: Determine the covariance matrix of the innovation according to the second prediction covariance matrix and the observation variance. The innovation here represents the deviation between the fused track state and the predicted second prediction track state.

[0081] The covariance matrix of the innovation can be expressed as:

[0082]

[0083] in, represents the covariance matrix of the new information; represents the second prediction covariance matrix; represents the observed variance of the equivalent measurement.

[0084] Step 405: Determine the equivalent measurement according to the innovation of the equivalent measurement and the covariance matrix of the innovation.

[0085] Specifically, the new information of the equivalent measurement and the covariance matrix of the new information finally calculated are used as the equivalent measurement.

[0086] In this embodiment, the new information represents the deviation between the fused track state and the predicted second predicted track state. By calculating the equivalent measurement, the accuracy of the fused track update of the non-sequential measurement can be further improved.

[0087] In one embodiment, after obtaining the equivalent measurement and the non-sequence measurement, the fused track state is updated to obtain the updated fused track state. Specifically, the equivalent measurement, the non-sequence measurement and the fused track state are input into the A1 algorithm based on one-step lag to obtain the updated fused track state. The A1 algorithm is an algorithm proposed by Y. Bar-Shalom et al. in 2002 to handle the out-of-sequence measurements (OOSM) problem.

[0088] Specifically, the predicted measurement at the first moment is determined based on the equivalent measurement and the fused motion state information. The predicted measurement can be expressed as:

[0089]

[0090] in, represents the predicted measurement at the first moment; represents the observation equation at the first moment, where the observation equation is the unit matrix; Indicates the third predicted motion state information at the first moment predicted based on the A1 algorithm. Specifically, the third motion state information at the first moment It can be expressed as:

[0091]

[0092] in, It represents the prediction equation from the second moment to the first moment, which is the same as the motion state transfer matrix; Indicates fusion motion state information; It means that the process error from the first moment to the second moment meets the mean of 0, which is the same as the process noise matrix; represents the covariance matrix of the new information; Indicates the new information of equivalent measurement.

[0093] After the predicted measurement is obtained, the fused track state is updated according to the predicted measurement and the non-sequential measurement to obtain an updated fused track state. Specifically, the updated fused track state includes: updated fused motion state information and an updated fused covariance matrix.

[0094] The updated fusion motion state information can be expressed as:

[0095]

[0096] in, Indicates updated fusion motion state information; Indicates fusion motion state information; Indicates non-sequential measurement; represents the predicted measurement at the first moment; Represents the gain of the new information in the A1 algorithm.

[0097] in The calculation formula is:

[0098]

[0099] in, Represents the covariance matrix of the first moment innovation in the A1 algorithm; represents the covariance matrix between the fused track state at the second moment and the non-sequential measurements at the first moment; represents the observation equation at the first moment; Indicates the third predicted motion state information at the first moment The corresponding covariance matrix; represents the observation error at the first moment; It represents the prediction equation from the second moment to the first moment, which is the same as the motion state transfer matrix; represents the fusion covariance matrix; express The covariance matrix corresponding to the part is the covariance matrix of the process noise predicted by the A1 algorithm from the second moment to the first moment; represents the cross-covariance matrix of the target state and the process error in the process of updating the fused motion state information; represents the process error with a mean of 0 from the first moment to the second moment, which is the same as the process noise matrix; represents the second prediction covariance matrix; Represents the covariance matrix of the innovation.

[0100] The updated fusion covariance matrix can be expressed as:

[0101]

[0102] in, represents the updated fusion covariance matrix; represents the fusion covariance matrix; represents the covariance matrix between the fused track state at the second moment and the non-sequential measurements at the first moment; Represents the covariance matrix of the first-moment innovation in the A1 algorithm.

[0103] Finally, according to the updated fusion motion state information And the updated fusion covariance matrix , and get the updated fusion track status.

[0104] In a specific embodiment, Figure 5 As shown, during the driving process, the vehicle obtains perception data from various types of sensor inputs, pre-processes the perception data, and determines the measurement of the corresponding perception data. Each measurement corresponds to the timestamp when the measurement is collected. The state is predicted based on the fused track state at the current moment, and the fused track state at the next moment is determined. The predicted fused track state at the next moment is associated with the obtained measurement. For example, the nearest neighbor association method based on the Mahalanobis distance can be used. After the association is completed, for the same target object, the measurement is used to update the fused track state at the current moment, and the fusion result is output after the update is completed. When updating, according to all measurements of the same target object obtained in the current time period, the fused track state at the current moment is updated in order from small to large timestamps to obtain the updated fusion result. As shown Figure 6 As shown in the figure, when updating the measurement, first determine whether the measurement is an out of sequence measure (OOSM). If the timestamp of the measurement is later than the timestamp of the fused track state, the measurement does not belong to the out of sequence measurement; if the timestamp of the measurement is earlier than the timestamp of the fused track state, the measurement is an out of sequence measurement. For measurements that do not belong to the out of sequence measurement, the fused track state is updated using the standard Kalman Filter. For the out of sequence measurement, the fused track state is updated according to the fused track update method of the out of sequence measurement. First, determine the time series of the out of sequence measurement, predict the track state at the corresponding time of the out of sequence measurement based on the fused track state at the current time, and approximate it to the track state at the corresponding time of the out of sequence measurement. Finally, the A1 algorithm is used to update the fused track state at the current time based on the track state at the corresponding time of the out of sequence measurement and the out of sequence measurement. The OOSM sequence of this method only maintains one time information, which is used to locate the timing position of the non-sequential measurement. This method can greatly reduce the amount of information maintained in the OOSM sequence, thereby reducing the algorithm's consumption of storage space. After the update step is completed, the current updated time information is entered into the OOSM sequence, and then the next sensor measurement is updated until the measurements of all currently input sensors are used, and the final fusion result is output.

[0105] The non-sequential measurement fusion track update method of the present application converts the multi-step lag problem into a single-step lag problem by means of equivalent measurement, and uses the single-step lag A1 algorithm to update the fusion track state. The direct update method is combined with the sequential fusion framework and applied to heterogeneous multi-sensor fusion tracking, overcoming the problem of limited performance of the fusion algorithm under non-sequential measurement conditions. The present application has no special requirements on the form of sensor measurement and the preset motion model of the algorithm, and has strong robustness. And under the premise of controllable algorithm accuracy, real-time performance and computational complexity, it solves the non-sequential measurement problem of heterogeneous multi-sensor fusion with less storage consumption.

[0106] In a specific usage scenario, the scenario is to track the vehicle target traveling in front of the vehicle. The vehicle target is traveling straight ahead at a speed of 35kph. The motion model of the vehicle target is assumed to be a constant acceleration motion model. The vehicle sensor is equipped with a millimeter-wave front radar and a front-view camera. The millimeter-wave radar has an operating cycle of 15Hz, a delay time of 200ms, a theoretical maximum delay of 250ms, and a size of 14kb for a radar data set. The visual camera has an operating cycle of 30hz, a delay time of about 130ms, and a size of 22kb for a visual data set. The timing of the vehicle-side fusion module receiving measurements in this scenario is as follows: Figure 7 As shown, the sensor collects data at 0ms, but the measurement corresponding to the data reaches the fusion module on the vehicle side at around 205ms. This measurement is a non-sequential measurement. The processing cycle of the fusion algorithm is 30Hz, and 64 tracks are maintained internally in the algorithm. The following analysis compares the data cache algorithm, the direct update algorithm, and the non-sequential measurement fusion track update method of this application from the perspective of accuracy, algorithm calculation consumption, and algorithm storage resource consumption. The comparison results are shown in the figure. Figure 8 As shown, based on Figure 8 The data is summarized. For accuracy, the data cache algorithm with the highest theoretical accuracy among the three algorithms is used as the benchmark, and the deviations between the other two algorithms and the data cache algorithm are counted. The data cache algorithm is used as the benchmark, and its deviation is set to 0%; the direct update algorithm is 0.06%; the non-sequential measurement fusion track update method of this application is 0.09%, and the deviation is calculated by Figure 8It can be seen that the state curves of the tracking results of the three algorithms are almost overlapped, and the state deviation degrees are all below 0.1% (deviation degree calculation method: absolute deviation / longitudinal distance, averaged over multiple frames), that is, there is almost no difference in accuracy among the three algorithms. In terms of computing resource consumption, the non-sequential measurement fusion track update method of the present application has only one more prediction step compared to the data caching method and the direct update method. This operation does not increase the complexity of the algorithm, and the time complexity of the algorithm is still O(n), that is, the three algorithms are similar in computing consumption. In terms of storage resource consumption, since the maximum delay of the two types of sensors is 250ms and the fusion processing cycle is 33ms, considering a certain amount of redundancy, the algorithm accesses data of 10 fusion cycles, then for the data caching method, it is necessary to store 10 copies of the visual data and radar data, that is, (22kb+14kb)×10=360kb of additional storage space. For the direct update algorithm, the data size of the OOSM sequence of a track is 0.18kb×10=1.8kb, of which 0.18kb is the space required for the calculation result of a single algorithm cycle. Since 64 tracks are maintained internally in the algorithm, the final storage consumption is 1.8kb×64=115kb; the OOSM sequence of a track in the non-sequential measurement fusion track update method of the present application only maintains one time information, so the storage consumption is 0.12kb×64=7.68kb. Therefore, based on the above non-sequential measurement fusion track update method of the present application, a large amount of storage space can be saved under the condition of equal accuracy and calculation amount.

[0107] It should be understood that, although the various steps in the flowcharts involved in the above-mentioned embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above-mentioned embodiments can include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0108] Based on the same inventive concept, the embodiment of the present application also provides a non-sequential measurement fusion track update device for implementing the non-sequential measurement fusion track update method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more non-sequential measurement fusion track update device embodiments provided below can refer to the limitations of the non-sequential measurement fusion track update method above, and will not be repeated here.

[0109] In one embodiment, Fig. 9 As shown, a non-sequential measurement fusion track updating device is provided, comprising: an acquisition module 100, a first prediction module 200, a second prediction module 300, an equivalent measurement calculation module 400 and an update module 500, wherein:

[0110] The acquisition module 100 is used to acquire the non-sequential measurement and the fused track state corresponding to the current moment; the moment corresponding to the non-sequential measurement is the first moment, and the current moment corresponding to the fused track state is the second moment;

[0111] A first prediction module 200, configured to perform state prediction according to the fused track state to obtain a first predicted track state corresponding to the first moment;

[0112] A second prediction module 300, configured to perform state prediction according to the first predicted track state corresponding to the first moment, to obtain a second predicted track state corresponding to the second moment;

[0113] An equivalent measurement calculation module 400 is used to determine an equivalent measurement according to the second predicted track state and the fused track state; the equivalent measurement is an equivalent measurement corresponding to the first moment to the second moment;

[0114] The updating module 500 is used to update the fused track state according to the equivalent measurement and the non-sequential measurement to obtain an updated fused track state.

[0115] The first prediction module 200 is also used to obtain the motion state transfer matrix, coordinate system rotation change matrix, driving distance matrix and process noise matrix of the vehicle; perform state prediction based on the motion state transfer matrix, coordinate system rotation change matrix, driving distance matrix, process noise matrix and fused track state to obtain the first predicted track state corresponding to the first moment.

[0116] The first prediction module 200 is further used to obtain a target object motion model, a rotation angle of the vehicle from the first moment to the second moment, a time interval from the first moment to the second moment, and a longitudinal speed of the vehicle at the second moment; determine a motion state transfer matrix according to the target object motion model and the time interval; construct a coordinate system rotation change matrix according to the rotation angle of the vehicle; and construct a driving distance matrix according to the longitudinal speed of the vehicle, the time interval, and the rotation angle of the vehicle.

[0117] The first prediction module 200 is also used to make predictions based on the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix and the fused motion state information to obtain the first predicted motion state information corresponding to the first moment; make predictions based on the motion state transfer matrix, the coordinate system rotation change matrix, the process noise matrix and the fused covariance matrix to obtain the first predicted covariance matrix corresponding to the first moment; determine the first predicted track state corresponding to the first moment based on the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix.

[0118] The equivalent measurement calculation module 400 is also used to determine the observation variance of the equivalent measurement based on the second prediction covariance matrix and the fused covariance matrix; determine the filter gain of the equivalent measurement based on the observation variance and the fused covariance matrix; determine the innovation of the equivalent measurement based on the second predicted motion state information, the fused motion state information and the filter gain; determine the covariance matrix of the innovation based on the second prediction covariance matrix and the observation variance; determine the equivalent measurement based on the innovation of the equivalent measurement and the covariance matrix of the innovation.

[0119] The updating module 500 is further used to input the equivalent measurement, non-sequential measurement and fused track state into the A1 algorithm based on one-step lag to obtain an updated fused track state.

[0120] The updating module 500 is further used to determine the predicted measurement at the first moment according to the equivalent measurement and the fused motion state information; and to update the fused track state according to the predicted measurement and the non-sequential measurement to obtain an updated fused track state.

[0121] Each module in the above-mentioned non-sequential measurement fusion track updating device can be implemented in whole or in part by software, hardware and their combination. Each of the above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the corresponding operations of each of the above modules.

[0122] In one embodiment, a computer device is provided. The computer device can be arranged at a vehicle end, and its internal structure diagram can be as follows: Fig.10As shown. The computer device includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless method can be achieved through WIFI, a mobile cellular network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a non-sequential measurement fusion track update method is implemented.

[0123] Those skilled in the art will understand that Fig.10 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0124] In one embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program. When the processor executes the computer program, any one of the non-sequential measurement fusion track update methods in the above embodiments is implemented.

[0125] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, any one of the non-sequential measurement fusion track update methods in the above embodiments is implemented.

[0126] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0127] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0128] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A non-sequential measurement fusion track updating method, characterized in that: The method comprises: Acquire non-sequential measurement and a fused track state corresponding to the current moment; the moment corresponding to the non-sequential measurement is a first moment, and the current moment corresponding to the fused track state is a second moment; Performing state prediction according to the fused track state to obtain a first predicted track state corresponding to the first moment; Performing state prediction according to the first predicted track state corresponding to the first moment to obtain a second predicted track state corresponding to the second moment; Determine an equivalent measurement according to the second predicted track state and the fused track state; the equivalent measurement is an equivalent measurement corresponding to the first moment to the second moment; The fused track state is updated according to the equivalent measurement and the non-sequential measurement to obtain an updated fused track state.

2. The method according to claim 1, characterized in that The performing state prediction according to the fused track state to obtain a first predicted track state corresponding to the first moment includes: Obtain motion state transfer matrix, coordinate system rotation change matrix, driving distance matrix and process noise matrix; State prediction is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the travel distance matrix, the process noise matrix and the fused track state to obtain a first predicted track state corresponding to the first moment.

3. The method according to claim 2, characterized in that Obtaining the motion state transfer matrix, coordinate system rotation change matrix, and driving distance matrix includes: Acquire a target object motion model, a rotation angle of the vehicle from the first moment to the second moment, a time interval from the first moment to the second moment, and a longitudinal speed of the vehicle at the second moment; Determine a motion state transfer matrix according to the target object motion model and the time interval; According to the rotation angle of the vehicle, a coordinate system rotation change matrix is ​​constructed; A travel distance matrix is ​​constructed according to the longitudinal speed of the vehicle, the time interval and the rotation angle of the vehicle.

4. The method according to claim 3, characterized in that: The state prediction is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the travel distance matrix, the process noise matrix and the fused track state to obtain the first predicted track state corresponding to the first moment, comprising: the fused track state includes fused motion state information and a corresponding fused covariance matrix; Predicting based on the motion state transfer matrix, the coordinate system rotation change matrix, the driving distance matrix and the fused motion state information to obtain first predicted motion state information corresponding to the first moment; Predicting is performed according to the motion state transfer matrix, the coordinate system rotation change matrix, the process noise matrix and the fusion covariance matrix to obtain a first prediction covariance matrix corresponding to the first moment; A first predicted track state corresponding to the first moment is determined according to the first predicted motion state information corresponding to the first moment and the first predicted covariance matrix.

5. The method according to claim 1, characterized in that Determining the equivalent measurement according to the second predicted track state and the fused track state includes: the second predicted track state includes the second predicted motion state information corresponding to the second moment and the second predicted covariance matrix; the fused track state includes the fused motion state information and the corresponding fused covariance matrix; Determining an observation variance of an equivalent measurement according to the second prediction covariance matrix and the fusion covariance matrix; Determining a filter gain of an equivalent measurement according to the observed variance and the fused covariance matrix; Determining new information of equivalent measurement according to the second predicted motion state information, the fused motion state information and the filter gain; Determine the covariance matrix of the new information according to the second prediction covariance matrix and the observed variance; An equivalent measurement is determined according to the innovation of the equivalent measurement and a covariance matrix of the innovation.

6. The method according to claim 1, characterized in that The fused track state is updated according to the equivalent measurement and the non-sequential measurement, and the updated fused track state includes: The equivalent measurement, non-sequential measurement and fused track state are input into the A1 algorithm based on one-step lag to obtain an updated fused track state.

7. The method according to claim 6, characterized in that The inputting of the equivalent measurement, the non-sequential measurement and the fused track state into the A1 algorithm based on one-step lag to obtain the updated fused track state includes: the fused track state includes fused motion state information and a corresponding fused covariance matrix; Determining a predicted measurement at a first moment according to the equivalent measurement and the fused motion state information; The fused track state is updated according to the predicted measurement and the non-sequential measurement to obtain an updated fused track state.

8. A non-sequential measurement fusion track updating device, characterized in that: The device comprises: An acquisition module, used to acquire non-sequential measurement and a fused track state corresponding to a current moment; the moment corresponding to the non-sequential measurement is a first moment, and the current moment corresponding to the fused track state is a second moment; A first prediction module, used for performing state prediction according to the fused track state to obtain a first predicted track state corresponding to the first moment; A second prediction module, used for performing state prediction according to the first predicted track state corresponding to the first moment, to obtain a second predicted track state corresponding to the second moment; An equivalent measurement calculation module, used to determine an equivalent measurement according to the second predicted track state and the fused track state; the equivalent measurement is an equivalent measurement corresponding to the first moment to the second moment; An updating module is used to update the fused track state according to the equivalent measurement and the non-sequential measurement to obtain an updated fused track state.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images