Vehicle track reconstruction and fusion method based on sparse long-distance traffic detection equipment

Abstract

The invention relates to a vehicle track reconstruction and fusion method based on sparse long-distance traffic detection equipment, and belongs to the technical field of intelligent traffic. According to the method, on the basis of a Newell car-following model and an expanded reverse car-following model thereof, the tracks of multiple vehicles in the whole road section can be reconstructed, parameters of the car-following model of each vehicle are calibrated by minimizing the travel time error of the reconstructed track of each vehicle, and finally the vehicle tracks collected and reconstructed by adjacent long-distance traffic detection equipment are fused by using a particle filter. Under different traffic flow states, the method can work stably, accuracy and integrity of trajectory reconstruction are improved remarkably, accurate vehicle trajectories can be obtained stably under the scene that long-distance traffic detection equipment is sparsely arranged, and the method is used for road traffic flow state monitoring and provides important support for overall traffic management and optimization.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent transportation, and particularly relates to a vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection devices. Background Art

[0002] In an intelligent transportation system, the collection and analysis of vehicle trajectory data are the keys to realizing vehicle-road collaboration. Vehicle trajectories can record the continuous position coordinates of all vehicles on a specific length of urban roads or highways over a period of time, that is, the longitude and latitude of the vehicles, from which rich traffic flow information such as vehicle driving paths, speeds, and accelerations can be deduced. Based on the analysis and judgment of vehicle trajectory data, traffic flow statistics, traffic carbon emissions and energy consumption accounting, traffic signal optimization, traffic accident reconstruction, driving behavior analysis, etc. can be carried out to optimize traffic control and for research. In an intelligent highway system, new roadside perception devices such as high-definition cameras and radar-vision integrated machines are used to collect vehicle trajectories. Such devices can detect and track multiple vehicle trajectories in a relatively long detection area, and obtain information such as vehicle models, positions, speeds, and lanes where the vehicles are located. Such devices are collectively referred to as long-distance traffic detection devices. Restricted by the equipment procurement cost, long-distance traffic detection devices usually cannot cover all sections in the construction project of intelligent highways, but are arranged at intervals according to actual needs. Therefore, for vehicle trajectories outside the device detection area, vehicle trajectory reconstruction methods must be used to obtain them.

[0003] Vehicle trajectory reconstruction refers to the process of collecting vehicle trajectories of local sections using multiple roadside fixed traffic flow detection devices with non-overlapping coverage ranges, or in cooperation with mobile traffic flow detection devices such as fixed and floating vehicles, and inferring vehicle trajectories outside the device coverage range based on this. In previous studies, vehicle trajectory reconstruction mostly relied on cross-section traffic detection devices such as loop detectors and geomagnetic detectors, as well as a small number of probe vehicles (for example, connected probe vehicles, GPS probe vehicles). Cross-section traffic detection devices usually can only collect information when vehicles pass through the device installation cross-section, and cannot obtain time-varying motion information of the vehicles. The number of probe vehicles is limited, and the collection of information involves issues such as personal privacy, so the collection is relatively difficult.

[0004] Therefore, how to overcome the deficiencies of the existing technology is an urgent problem to be solved in the current technical field of intelligent transportation. Summary of the Invention

[0005] The purpose of the present invention is to solve the deficiencies of the existing technology, and provide a vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection devices. This method is based on the Newell car-following model and its extended inverse car-following model, can reconstruct the trajectories of multiple vehicles on the entire section, calibrate the car-following model parameters of each vehicle by minimizing the travel time error of the reconstructed trajectories of each vehicle, and finally use a particle filter to fuse the vehicle trajectories collected and reconstructed by adjacent long-distance traffic detection devices.

[0006] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device, comprising the following steps:

[0008] Step 1, using the long-distance traffic detection device to obtain the vehicle detection data of multiple vehicles in its detection area in real time, so as to obtain the vehicle trajectories of multiple vehicles;

[0009] Step 2, according to the vehicle trajectories collected by the long-distance traffic detection device in real time, reconstruct the vehicle trajectories upstream and downstream of the device detection area;

[0010] Step 3, according to the reconstructed vehicle trajectories in Step 2, calculate the travel time error of the reconstructed trajectories, and taking the minimization of the travel time error of the reconstructed trajectories as the goal, calibrate the parameters of the Newell car-following model and the inverse car-following model for each vehicle;

[0011] Step 4: Use a particle filter to fuse two different reconstructed trajectories of the same vehicle reconstructed by adjacent long-distance traffic detection devices in the same area to obtain a complete and smooth vehicle trajectory.

[0012] Further, in Step 1, the vehicle detection data includes the position, speed, and acceleration information of the vehicle, as well as the vehicle length, color, vehicle type, and license plate number information.

[0013] Further, the specific method of Step 2 is:

[0014] Regarding the reconstructed trajectories as spatio-temporal offsets of the collected trajectories according to the Newell car-following model and the inverse car-following model;

[0015] For the target vehicle of trajectory reconstruction, the trajectory upstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple vehicles in front of it; the trajectory downstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple vehicles behind it;

[0016] For the vehicle trajectories in the blind area of trajectory reconstruction of the Newell car-following model and the inverse car-following model, use the speed recursion method for reconstruction.

[0017] Further, for the upstream trajectory reconstruction of the detection area, when reconstructing the target vehicle trajectory, according to the Newell car-following model, the vehicle trajectory segment not detected by the long-distance traffic detection device is regarded as the spatio-temporal offset of the trajectories of multiple vehicles in front of the device detected by the device, and the offset formula is:

[0018] x n (t + τ n ) = x n-1 (t) - δ n

[0019] In the formula, x n(t + τ n ) is the nth vehicle, i.e., the position of the following vehicle at time t + τ n ; x n-1 (t) is the (n - 1)th vehicle, i.e., the position of the preceding vehicle at time t; τ n is the time offset of the nth vehicle; δ n is the spatial offset of the nth vehicle;

[0020] Further, for the trajectory reconstruction downstream of the detection area, when reconstructing the trajectory of the target vehicle, according to the inverse car-following model extended based on the Newell car-following model, the trajectories of undetected vehicles are regarded as the spatio-temporal offsets of the trajectory segments of multiple following vehicles behind them, and the offset formula is:

[0021] x n (t - τ n ) = x n+1 (t) + δ n

[0022] In the formula, x n (t - τ n ) is the nth vehicle, i.e., the position of the following vehicle at time t - τ n ; x n+1 (t) is the (n + 1)th vehicle, i.e., the position of the preceding vehicle at time t; τ n is the time offset of the nth vehicle; δ n is the spatial offset of the nth vehicle.

[0023] Further, for the vehicle trajectories that cannot be reconstructed based on the Newell car-following model and the Newell inverse car-following model, the speed recursion method is used for trajectory reconstruction; this method uses the arithmetic mean to estimate the vehicle speed on the target section, and the vehicle speed calculation formula between adjacent cross-section detection points is:

[0024]

[0025] In the formula, x i , x i+1 are the position coordinates of two adjacent detection points upstream and downstream of the road respectively, x is the spatial position coordinate of the target vehicle on the road, where x i ≤ x ≤ x i+1 , v(x i+1 ) and v(x i ) are the vehicle speeds detected by the adjacent upstream detection point and the downstream detection point respectively; with the help of the instantaneous vehicle speeds detected by adjacent long-distance traffic detection equipment and the endpoint speeds of the missing positions of the trajectories reconstructed according to the Newell car-following model, the missing trajectories can be reconstructed using the speed recursion method.

[0026] Further, the specific method of step three is:

[0027] (3.1) Set the initial spatial lag parameter:

[0028] Set different spatial lag parameters for the Newell following model of each vehicle:

[0029] X = {δ1, δ2, δ3,..., δ j ,..., δ m}

[0030] Where X is the set of spatial lag parameters of all vehicles, and δ j is the spatial lag value of the j-th vehicle, and m is the number of vehicle trajectories detected by the long-distance traffic detection device;

[0031] (3.2) Reconstruct the vehicle trajectories using the given parameters:

[0032] Reconstruct the trajectories of each vehicle in turn using the given set of spatial lag parameters X. The reconstructed vehicle trajectories are expressed as:

[0033] Y(X) = {y(δ1), y(δ2), y(δ3),..., y(δ j ),..., y(δ m )}

[0034] Where Y(X) is the set of vehicle trajectories reconstructed using the set of vehicle spatial lag parameters X, and y(δ j ) is the trajectory of the j-th vehicle reconstructed using the spatial lag parameter δ j of the j-th vehicle;

[0035] (3.3) Calculate the vehicle travel time error:

[0036] The travel time error obtained from the vehicle trajectory reconstruction is:

[0037] e(δ j ) = |t(δ j ) rec - t(j) real |

[0038] Where e(δ j ) is the travel time error of the trajectory reconstructed for the j-th vehicle, and t(δ j ) rec is the travel time of the trajectory reconstructed for the j-th vehicle using the given spatial lag parameter δ j , and t(j) real is the actual observed travel time of the j-th vehicle, where j = 1, 2, 3,..., m;

[0039] (3.4) Update the spatial lag parameter:

[0040] Set the optimization objective of reconstructing each vehicle's trajectory as minimizing the travel time error of the reconstructed trajectory of the target vehicle. The objective function is:

[0041]

[0042] In the formula, E(X) is the sum of travel time errors of all vehicle trajectories reconstructed using the set X of vehicle spatial lag parameters; find the optimal spatial lag parameter δ for each vehicle based on the objective function j After that, update the spatial lag parameters of each vehicle, and repeat steps (3.2) to (3.3) until the optimal set X of vehicle spatial lag parameters is solved, making the value of E(X) minimized;

[0043] (3.5) Output the optimization result of vehicle trajectory reconstruction

[0044] Reconstruct the trajectory of each vehicle using the calculated set X of vehicle spatial lag parameters, thereby improving the accuracy of vehicle trajectory reconstruction.

[0045] Furthermore, the specific method of step four is:

[0046] (4.1) Design the state space model:

[0047] In the state space model of the particle filter, it includes a state vector and an observation vector;

[0048] The state vector x t Describes the kinematic characteristics of the vehicle at time t, x t Is expressed as:

[0049]

[0050] In the formula, l t Is the position of the vehicle at time t, v t Is the speed of the vehicle at time t;

[0051] Assume that the acceleration a t-1 Is the system process noise, and establish the discrete state dynamics equation of vehicle motion:

[0052]

[0053] In the formula, T is the time frame interval of data acquisition, l t-1 Is the position of the vehicle at time t - 1, v t-1 Is the speed of the vehicle at time t - 1, a t-1 Is the acceleration of the vehicle at time t - 1;

[0054] The observation vector y t Is:

[0055]

[0056] The observation equation is expressed as:

[0057]

[0058] Wherein, is the observed value of the reconstructed trajectory of the upstream vehicle at time t, is the observed value of the reconstructed trajectory of the downstream vehicle at time t, and are respectively the position and speed of the trajectory , and are respectively the position and speed of the trajectory , R is the observation noise;

[0059] In the process of vehicle motion state transition, the following constraint conditions are added: after state transition, the vehicle position state at the current time step is greater than or equal to the vehicle position state at the previous time step; after state transition, the vehicle speed state at the current time step is greater than or equal to zero and less than the maximum highway speed limit; after state transition, the distance between the position of the vehicle at the current time step and the position of the vehicle in front should be greater than or equal to the minimum stopping distance;

[0060] (2) Adjust the particle weights to fuse vehicle trajectories

[0061] In the particle filter, the formula for updating the particle weights of trajectory fusion is:

[0062]

[0063] Wherein, is the particle weight of the previous state, is the particle weight calculated based on the observed value of the upstream reconstructed trajectory, is the particle weight calculated based on the observed value of the downstream reconstructed trajectory, t is the current time step, and t3 - t2 is the total time length of the fused trajectory; and are calculated according to the weight update formula of the particle filter;

[0064] After each round of particle weight update, the weighted average of each particle is taken to obtain the estimated value of the fused trajectory point at the current moment.

[0065] In the present invention, the long-distance traffic detection devices are arranged in a sparse manner, there are intervals between the detection areas of adjacent devices, and each detection device can collect the trajectories of multiple vehicles in its detection area in real time. The vehicle trajectory reconstruction model can be used to input the vehicle trajectories in the detection area of the device and reconstruct the vehicle trajectories of the entire road section.

[0066] Since each vehicle maintains a different minimum stopping distance from the vehicle in front, the space lag parameters in the Newell following model and the inverse following model for each vehicle are different. By setting different space lag parameters for the Newell following model and the inverse following model for each vehicle, the differences in drivers' following behaviors can be fully considered, enabling the reconstructed trajectory to more accurately restore the vehicle following behavior, thereby improving the accuracy and reliability of trajectory reconstruction.

[0067] The method for calibrating the parameters of the vehicle following model in the present invention is used to more realistically describe the following behaviors of each vehicle. By minimizing the travel time error of the vehicle reconstructed trajectory to calibrate the space lag parameters of each vehicle, the calibrated parameters are used for trajectory reconstruction, thereby improving the accuracy of trajectory reconstruction.

[0068] In the present invention, the reconstructed vehicle trajectories of the same vehicle by adjacent long-distance traffic detection devices on the same road section are different, so it is necessary to fuse and process the vehicle trajectories reconstructed by adjacent devices. The particle filter can be used to fuse accurate vehicle trajectories.

[0069] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0070] Under different traffic flow states, the method of the present invention can work stably, significantly improving the accuracy and integrity of trajectory reconstruction.

[0071] The experimental results show that the MAE of the trajectory reconstructed using long-distance traffic detection devices is 8.18% lower than that of the trajectory reconstructed using 15% penetration probe vehicles combined with cross-section traffic detection devices, and it can stably obtain vehicle reconstructed trajectories with the same accuracy.

[0072] The present invention fully considers the influence of driver differences. The MAE of the trajectory obtained by the trajectory reconstruction method considering driver differences is on average 27.17% lower than that of the trajectory reconstruction method without considering driver differences, effectively improving the accuracy of trajectory reconstruction.

[0073] The vehicle trajectory fusion method based on the particle filter proposed by the present invention enables the trajectories reconstructed by adjacent long-distance traffic detection devices on the same road section for the same vehicle to be completely and smoothly fused and spliced. When the layout spacing is 200m, the MAE of the fused trajectory is on average reduced by 60.08% compared with the MAE of the trajectory reconstructed by a single device in the experiment, further improving the accuracy of the reconstructed trajectory and ensuring that multiple long-distance traffic detection devices arranged at intervals have the ability to continuously reconstruct vehicle trajectories over a large range.

[0074] The present invention can stably obtain accurate vehicle trajectories in the scenario of sparsely arranged long-distance traffic detection devices, which can be used in vehicle-road collaborative systems, providing important support for overall traffic management and optimization. Description of the Drawings

[0075] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required for the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0076] Figure 1 is the flowchart of the vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection equipment;

[0077] Figure 2 is the schematic diagram of equipment layout and collected trajectory;

[0078] Figure 3 is the schematic diagram of trajectory reconstruction based on the Newell following model;

[0079] Figure 4 is the schematic diagram of the trajectory reconstruction area of the speed recursion method;

[0080] Figure 5 is the schematic diagram of vehicle trajectory fusion research;

[0081] Figure 6 is the trajectory map obtained by vehicle trajectory reconstruction and fusion;

[0082] Figure 7 is the diagram of the change of trajectory reconstruction and fusion error with the equipment layout spacing. Specific embodiments

[0083] The following further describes the present invention in detail in conjunction with the embodiments.

[0084] Those skilled in the art will understand that the following embodiments are only used to illustrate the present invention and should not be regarded as limiting the scope of the present invention. For those not specified in the embodiments regarding specific technologies or conditions, they shall be carried out according to the technologies or conditions described in the literature in the art or according to the product specifications. For those materials or equipment not specified as the manufacturer, they are all conventional products that can be obtained by purchase.

[0085] Embodiment 1

[0086] As Figure 1 shown, the vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection equipment includes the following steps:

[0087] Step 1, use the long-distance traffic detection equipment to obtain the vehicle detection data of multiple vehicles in its detection area in real time, so as to obtain the vehicle trajectories of multiple vehicles;

[0088] Step 2, reconstruct the vehicle trajectories upstream and downstream of the equipment detection area according to the vehicle trajectories collected by the long-distance traffic detection equipment in real time;

[0089] Step 3: Reconstruct the vehicle trajectories according to Step 2, calculate the travel time error of the reconstructed trajectories, and calibrate the parameters of the Newell car-following model and the inverse car-following model for each vehicle with the goal of minimizing the travel time error of the reconstructed trajectories;

[0090] Step 4: Use a particle filter to fuse two different reconstructed trajectories of the same vehicle reconstructed by adjacent long-distance traffic detection devices in the same area to obtain a complete and smooth vehicle trajectory.

[0091] Embodiment 2

[0092] As Figure 1 shown, a vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection devices includes the following steps:

[0093] Step 1: Use long-distance traffic detection devices to obtain vehicle detection data of multiple vehicles in their detection areas in real time, so as to obtain the vehicle trajectories of multiple vehicles;

[0094] Step 2: Reconstruct the vehicle trajectories upstream and downstream of the device detection area according to the vehicle trajectories collected by the long-distance traffic detection devices in real time;

[0095] Step 3: Reconstruct the vehicle trajectories according to Step 2, calculate the travel time error of the reconstructed trajectories, and calibrate the parameters of the Newell car-following model and the inverse car-following model for each vehicle with the goal of minimizing the travel time error of the reconstructed trajectories;

[0096] Step 4: Use a particle filter to fuse two different reconstructed trajectories of the same vehicle reconstructed by adjacent long-distance traffic detection devices in the same area to obtain a complete and smooth vehicle trajectory.

[0097] In Step 1, the vehicle detection data includes the position, speed, and acceleration information of the vehicle, as well as the vehicle length, color, vehicle type, and license plate number information.

[0098] The specific method of Step 2 is as follows:

[0099] Regard the reconstructed trajectories as the spatio-temporal offsets of the collected trajectories according to the Newell car-following model and the inverse car-following model;

[0100] For the target vehicle of trajectory reconstruction, the trajectory upstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple preceding vehicles; the trajectory downstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple following vehicles;

[0101] For the vehicle trajectories in the blind areas of the Newell car-following model and the inverse car-following model trajectory reconstruction, use the speed recursion method for reconstruction.

[0102] For the upstream trajectory reconstruction of the detection area, when reconstructing the trajectory of the target vehicle, according to the Newell following model, the vehicle trajectory segment not detected by the long-distance traffic detection device is regarded as the spatio-temporal offset of the trajectories of multiple leading vehicles detected by the device. The offset formula is:

[0103] x n (t + τ n ) = x n-1 (t) - δ n

[0104] In the formula, x n (t + τ n ) is the position of the nth vehicle, i.e., the following vehicle, at time t + τ n ; x n-1 (t) is the position of the (n - 1)th vehicle, i.e., the leading vehicle, at time t; τ n is the time offset of the nth vehicle; δ n is the spatial offset of the nth vehicle.

[0105] For the downstream trajectory reconstruction of the detection area, when reconstructing the trajectory of the target vehicle, according to the inverse following model extended based on the Newell following model, the undetected vehicle trajectory is regarded as the spatio-temporal offset of the trajectory segments of multiple following vehicles behind it. The offset formula is:

[0106] x n (t - τ n ) = x n+1 (t) + δ n

[0107] In the formula, x n (t - τ n ) is the position of the nth vehicle, i.e., the following vehicle, at time t - τ n ; x n+1 (t) is the position of the (n + 1)th vehicle, i.e., the leading vehicle, at time t; τ n is the time offset of the nth vehicle; δ n is the spatial offset of the nth vehicle.

[0108] For the vehicle trajectory that cannot be reconstructed based on the Newell following model and the Newell inverse following model, the speed recursion method is used for trajectory reconstruction; this method uses the arithmetic mean to estimate the vehicle speed on the target section. The vehicle speed calculation formula between adjacent cross-section detection points is:

[0109]

[0110] In the formula, x i and x i+1 are the position coordinates of two adjacent detection points upstream and downstream of the road respectively, x is the spatial position coordinate of the target vehicle on the road, where x i≤x≤x i+1 , v(x i+1 ), v(x i ) are the vehicle speeds detected at adjacent upstream and downstream detection points respectively; by means of the instantaneous vehicle speeds detected by adjacent long-distance traffic detection devices and the endpoint speeds of the missing positions of the trajectories reconstructed according to the Newell car-following model, the missing trajectories can be reconstructed by using the speed recursion method.

[0111] The specific method of step three is as follows:

[0112] (3.1) Set the initial spatial lag parameter:

[0113] Set different spatial lag parameters for the Newell car-following model of each vehicle:

[0114] X = {δ1, δ2, δ3,..., δ j ,..., δ m}

[0115] In the formula, X is the set of all vehicle spatial lag parameters, δ j is the spatial lag value of the j-th vehicle, and m is the number of vehicle trajectories detected by the long-distance traffic detection device;

[0116] (3.2) Reconstruct the vehicle trajectories using the given parameters:

[0117] Use the given set of spatial lag parameters X to reconstruct the trajectories of each vehicle in turn. The reconstructed vehicle trajectories are expressed as:

[0118] Y(X) = {y(δ1), y(δ2), y(δ3),..., y(δj),..., y(δm)}

[0119] In the formula, Y(X) is the set of vehicle trajectories reconstructed using the set of vehicle spatial lag parameters X, and y(δ j ) is the trajectory of the j-th vehicle reconstructed using the spatial lag parameter δ j of the j-th vehicle;

[0120] (3.3) Calculate the vehicle travel time error:

[0121] The vehicle travel time error obtained from the vehicle trajectory reconstruction is:

[0122] e(δ j ) = |t(δ j ) rec - t(j) real |

[0123] In the formula, e(δ j ) is the vehicle travel time error obtained from the reconstruction of the j-th vehicle's trajectory, and t(δ j) rec Reconstruct the travel time, \(t(j)\), of the \(j\)-th vehicle's trajectory using the given spatial lag parameter \(\delta\). j The travel time obtained by reconstructing the trajectory of the \(j\)-th vehicle, \(t(j)\). real The travel time actually observed for the \(j\)-th vehicle, where \(j = 1, 2, 3, \ldots, m\).

[0124] (3.4) Update the spatial lag parameter:

[0125] Set the optimization objective for reconstructing each vehicle's trajectory as minimizing the travel time error of the target vehicle's reconstructed trajectory. The objective function is:

[0126]

[0127] In the formula, \(E(X)\) is the sum of the travel time errors of all vehicle trajectories reconstructed using the set \(X\) of vehicle spatial lag parameters. Find the optimal spatial lag parameter \(\delta\) for each vehicle based on the objective function j After that, update the spatial lag parameters of each vehicle and repeat steps (3.2) to (3.3) until the optimal set \(X\) of vehicle spatial lag parameters is solved, minimizing the value of \(E(X)\).

[0128] (3.5) Output the optimization result of vehicle trajectory reconstruction

[0129] Reconstruct the trajectory of each vehicle using the calculated set \(X\) of vehicle spatial lag parameters to improve the accuracy of vehicle trajectory reconstruction.

[0130] The specific method of Step Four is:

[0131] (4.1) Design the state space model:

[0132] The state space model of the particle filter includes a state vector and an observation vector.

[0133] The state vector \(x\) t Describes the kinematic characteristics of the vehicle at time \(t\), \(x\) t Is expressed as:

[0134]

[0135] In the formula, \(l\) t Is the position of the vehicle at time \(t\), \(v\) t Is the speed of the vehicle at time \(t\).

[0136] Assume the acceleration \(a\) t-1 Is the system process noise, and establish the discrete state dynamics equation of vehicle motion:

[0137]

[0138] In the formula, \(T\) is the time frame interval of data acquisition, \(l\) t-1The position of the vehicle at time t-1, v t-1 The speed of the vehicle at time t-1, a t-1 Is the acceleration of the vehicle at time t-1;

[0139] The observation vector y t Is:

[0140]

[0141] The observation equation is expressed as:

[0142]

[0143] In the formula, Is the observed value of the reconstructed trajectory of the upstream vehicle at time t, Is the observed value of the reconstructed trajectory of the downstream vehicle at time t, And Are respectively the position and speed of the trajectory , And Are respectively the position and speed of the trajectory , R is the observation noise;

[0144] In the process of vehicle motion state transition, the following constraint conditions are added: after state transition, the vehicle position state at the current time step is greater than or equal to the vehicle position state at the previous time step; after state transition, the vehicle speed state at the current time step is greater than or equal to zero and less than the highway speed limit; after state transition, the distance between the position of the vehicle at the current time step and the position of the vehicle in front should be greater than or equal to the minimum stopping distance;

[0145] (2) Adjust the particle weights to fuse the vehicle trajectories

[0146] In the particle filter, the formula for updating the particle weights for trajectory fusion is:

[0147]

[0148] In the formula, Is the particle weight of the previous state, Is the particle weight calculated based on the observed value of the upstream reconstructed trajectory , Is the particle weight calculated based on the observed value of the downstream reconstructed trajectory , t is the current time step, and t3–t2 is the total time length of the fused trajectory; And Are calculated according to the weight update formula of the particle filter;

[0149] After each round of particle weight update, the weighted average of each particle is taken to obtain the estimated value of the fused trajectory point at the current moment.

[0150] Application Example

[0151] Figure 1 The following is a flowchart of the vehicle trajectory reconstruction and fusion method based on sparse long - distance traffic detection equipment provided by the present invention. Next, each step will be introduced in detail.

[0152] Step 1: The long - distance traffic detection equipment obtains the vehicle position, vehicle speed, vehicle type, license plate number, and lane information of multiple vehicles in its detection area in real - time, and extracts the vehicle trajectories of multiple vehicles from them.

[0153] 1. Data collection

[0154] As Figure 2 is a schematic diagram of the equipment layout scenario. Through long - distance traffic detection equipment such as radar - vision integrated machines or high - definition cameras installed on roadside poles or gantries, the full - structured dynamic information of vehicles in the equipment detection area is obtained in real - time. The long - distance traffic detection equipment collects the position, speed, and acceleration information of each passing vehicle in each time frame at a time - frame interval of 0.1 seconds, and identifies the vehicle length, color, vehicle type, and license plate number information. The data collected by the long - distance traffic detection equipment is transmitted to the edge - computing equipment installed in a supporting manner to process the multi - vehicle data collected by the long - distance traffic detection equipment in real - time, and extract the vehicle trajectory data in the equipment detection area from it in real - time.

[0155] 2. Travel - time calculation

[0156] First, the vehicle trajectory data extracted by each edge - computing equipment is transmitted to the total server in real - time. According to the license plate number and vehicle type information of the vehicles collected by each equipment, the data collected by different equipment for the same vehicle is matched and unified. Then, according to the time and speed information of the same vehicle collected by adjacent equipment, the travel time of the vehicle passing through adjacent two equipment is calculated, so that each vehicle can find the optimal spatial offset later, making the difference between the travel time of the reconstructed trajectory and the actual travel time the smallest.

[0157] Step 2: According to the vehicle trajectories collected by the long - distance traffic detection equipment in real - time, the vehicle trajectories upstream and downstream of the equipment detection area are reconstructed.

[0158] 1. Reconstruction of the upstream trajectory in the detection area

[0159] As Figure 3 shown, when reconstructing vehicle trajectory n, according to the Newell car - following model, the vehicle trajectory segment 1' not detected by the long - distance traffic detection equipment can be regarded as the spatio - temporal offset of the trajectory segment 1 of vehicle n - 1 detected by the equipment. Among them, the spatial offset δ n is:

[0160]

[0161] where k j is the blockage density, L is the length of the road section, and N j is the number of vehicles on the road during blockage.

[0162] The time offset τ of trajectory 1′ n is:

[0163]

[0164] where is the time when the trajectory reconstruction target vehicle (the nth vehicle) reaches the boundary of the equipment detection area, is the time when the vehicle in front of the target vehicle (the (n - 1)th vehicle) travels beyond the boundary of the equipment detection area, and δ is the spatial offset.

[0165] Using the spatio-temporal offset, the trajectory 1′ of vehicle n can be inferred from the trajectory segment 1 of vehicle n - 1. Similarly, the trajectory segment of its following vehicle n - 1 can be inferred using the trajectory segment 2 of vehicle n - 2 (the second vehicle in front of the target vehicle), and further, the trajectory segment 2′ of vehicle n can be inferred based on the trajectory segment obtained by vehicle n - 1. And so on, by means of multiple vehicles in front of the target vehicle n, all the vehicle trajectory segments upstream of the equipment detection area for the target vehicle n can be gradually inferred.

[0166] 2. Trajectory Reconstruction Downstream of the Detection Area

[0167] Similar to the principle of reconstructing the trajectory of the target vehicle upstream of the equipment detection area, when reconstructing all the vehicle trajectories downstream of the target vehicle n, according to the Newell inverse car-following model, the undetected vehicle trajectory can be regarded as the spatio-temporal offset of the trajectory segments of multiple following vehicles behind it. The offset formula is:

[0168]

[0169] where k j is the blockage density, L is the length of the road section, and N j is the number of vehicles on the road during blockage. The time offset τ n is:

[0170]

[0171] where is the time when the vehicle behind the target vehicle (the (n + 1)th vehicle) reaches the position at the downstream boundary of the detection area, is the time when the target vehicle (the nth vehicle) reaches the downstream boundary of the detection area, δ is the spatial offset, and l is the longitudinal detection area of the long-distance traffic detection equipment.

[0172] 3. Trajectory Reconstruction in the Missing Area

[0173] It should be noted that inFigure 4 The vehicle trajectory within the lower left dashed triangle cannot be reconstructed based on the Newell car-following model because there is no preceding vehicle trajectory data when the first vehicle starts to enter. Similarly, since there is a lack of following vehicle trajectory data when the last vehicle exits the detection area, for Figure 4 the trajectory in the upper right triangle area, it cannot be reconstructed based on the Newell inverse car-following model. For Figure 4 the partial trajectories within the triangle area that cannot be reconstructed, the velocity recursion method is used for trajectory reconstruction. The vehicle speed calculation formula between adjacent cross-section detection points is:

[0174]

[0175] In the formula, x i , x i+1 are the positions of two adjacent detection points upstream and downstream of the road respectively, x is the spatial position of the target vehicle on the road, where, x i ≤ x ≤ x i+1 , v(x i+1 ) and v(x i ) are the vehicle speeds detected by the adjacent upstream detection point and downstream detection point respectively. With the help of the instantaneous vehicle speeds detected by adjacent long-distance traffic detection equipment and the endpoint speeds at the missing positions of the trajectories reconstructed based on the Newell car-following model and its inverse car-following model, the missing trajectories can be reconstructed using the velocity recursion method.

[0176] Step 3: According to the vehicle trajectories reconstructed in Step 2, calculate the travel time error of the reconstructed trajectories, and calibrate the car-following model parameters of each vehicle with the aim of minimizing the travel time error of the reconstructed trajectories.

[0177] 1. Set the initial space lag parameter

[0178] Set different space lag parameters for the Newell car-following model of each vehicle:

[0179] X = {δ1, δ2, δ3,..., δ j ,..., δ m}

[0180] In the formula, X is the set of all vehicle space lag parameters, δ j is the space lag value of the jth vehicle, and m is the number of vehicle trajectories detected by the long-distance traffic detection equipment.

[0181] 2. Reconstruct the vehicle trajectories using the given parameters

[0182] Use the given set of space lag parameters X to reconstruct the trajectories of each vehicle in turn. The reconstructed vehicle trajectories are expressed as:

[0183] Y(X) = {y(δ1), y(δ2), y(δ3),..., y(δ j ),..., y(δ m )}

[0184] Wherein, Y(X) is the set of vehicle trajectories reconstructed using the set X of vehicle spatial lag parameters, and y(δ j ) is the trajectory of the j-th vehicle reconstructed using the spatial lag parameter δ j of the j-th vehicle.

[0185] 3. Calculate the vehicle travel time error

[0186] The true vehicle travel time can be calculated from the vehicle data collected at the edge of the adjacent device detection area. Define the travel time error obtained by reconstructing the vehicle trajectory as:

[0187] e(δ j ) = |t(δ j ) rec - t(j) real |

[0188] Wherein, e(δ j ) is the travel time error of the trajectory reconstructed for the j-th vehicle, and t(δ j ) rec is the travel time obtained by reconstructing the trajectory of the j-th vehicle using the given spatial lag parameter δ j , and t(j) real is the actual observed travel time of the j-th vehicle, where j = 1, 2, 3,..., m.

[0189] 4. Update the spatial lag parameter

[0190] Set the optimization objective of reconstructing each vehicle's trajectory to minimize the travel time error of the reconstructed trajectory of the target vehicle. Based on the objective function, find the optimal spatial lag parameter δ j for each vehicle, and then update the spatial lag parameter of each vehicle to minimize the sum of the travel time errors e(δ j ) of the reconstructed trajectories of each vehicle. The objective function is:

[0191]

[0192] Wherein, E(X) is the sum of the travel time errors of all vehicle trajectories reconstructed using the set X of vehicle spatial lag parameters. Update the spatial lag parameter δ j of each vehicle, and repeat steps (2) to (3) until the optimal set X of vehicle spatial lag parameters is solved to minimize the value of E(X).

[0193] 5. Output the optimization result of vehicle trajectory reconstruction

[0194] Reconstruct the trajectory of each vehicle using the calculated set X of vehicle spatial lag parameters, thereby improving the accuracy of vehicle trajectory reconstruction.

[0195] Step 4: Use a particle filter to fuse two different reconstructed trajectories of the same vehicle reconstructed by adjacent long-distance traffic detection devices in the same area to obtain a complete and smooth vehicle trajectory.

[0196] 1. Design a state space model

[0197] In the application of a particle filter, the state vector and the observation vector together constitute the state space model of the system. Among them, the state vector describes the specific state of the system at a certain moment, used to understand the operating condition of the system at the current moment and predict its future development trend. The measurement vector describes the measurement data actually obtained by the system, used to compare and match with the state vector, so as to realize the estimation and correction of the system state. By continuously updating and correcting the state vector, the particle filter can gradually approach the true state of the system and achieve an accurate estimation of the system state.

[0198] The state vector describes the kinematic characteristics of the vehicle. As Figure 5 shown, the detection area of the upstream device 1 is from l1 to l2, and the detection area of the downstream device 2 is from l3 to l4. The vehicle trajectory between l2 and l3 is the trajectory not detected by the device. For Figure 5 the trajectory not detected by the device between t2 and t3 in t is regarded as a discrete state with state variables such as the position l t and velocity v t at time step t. The state vector x

[0199]

[0200] Assume that the acceleration a t-1 is the system process noise. Establish the dynamic equation of the discrete state of vehicle motion according to the kinematic relationship:

[0201]

[0202] In the formula, T is the time frame interval of data acquisition.

[0203] The observation vector describes the measured value of the vehicle motion state. At time t, the measured value is the measured trajectory reconstructed based on the data collected by the upstream device and the downstream device ( Figure 5 the trajectory obtained by the upstream device 1 and the trajectory obtained by the downstream device 2 in ), which are respectively represented by

[0204]

[0205] The observation equation can be expressed as:

[0206]

[0207] Wherein, and are the position and velocity of the trajectory respectively, and are the position and velocity of the trajectory respectively, and R is the observation noise.

[0208] In addition, during the actual driving process of the vehicle, the speed is greater than or equal to zero, and the following distance between vehicles is greater than the minimum stopping distance. Therefore, the following constraint conditions are added during the vehicle motion state transition: after the state transition, the vehicle position state at the current time step is greater than or equal to the vehicle position state at the previous time step; after the state transition, the vehicle speed state at the current time step is greater than or equal to zero and less than the highway speed limit; after the state transition, the distance between the position of the vehicle at the current time step and the position of the preceding vehicle should be greater than or equal to the minimum stopping distance.

[0209] 2. Adjust the particle weights to fuse the vehicle trajectories

[0210] In the particle filter, it is necessary to adjust the particle weights with the help of the measured values. Regarding the trajectory reconstruction points as the measured values, there are two measured values at each state transition time step, and the particle weights need to be updated according to these two measured values. As Figure 5 shown, since the reconstruction trajectory error is smaller closer to the device detection area, when updating the particle weights, it is set that during the process of time t increasing from t2 to t3, the particle weight of the upstream reconstruction trajectory observation value in the trajectory fusion calculation increases linearly, and the particle weight of the downstream reconstruction trajectory observation value decreases linearly. The sum of the particle weights of the upstream and downstream reconstruction trajectories for the same particle at the same moment is set to 1, so that when t is equal to t2, the particle weight of is 1, and when t is equal to t3,

[0211]

[0212] Wherein, is the particle weight calculated based on the upstream reconstruction trajectory observation value , is the particle weight calculated based on the downstream reconstruction trajectory observation value , t is the current time step, and t3 - t2 is the total time length of the fusion trajectory. and It is calculated according to the weight update formula of the particle filter. Among them, The calculation formula of is:

[0213]

[0214] In the formula, is the particle weight of the previous state, is the proposal density function,

[0215] is the state transition probability, is the likelihood function.

[0216] The calculation formula of is:

[0217]

[0218] In the formula, is the particle weight of the previous state, is the proposal density function,

[0219] is the state transition probability, is the likelihood function.

[0220] After each round of particle weight update, the weighted average of each particle is taken to obtain the estimated value of the fused trajectory point at the current moment.

[0221] Combining steps one to four, using Figure 1 the vehicle trajectory data collected by the long-distance traffic detection device, the vehicle trajectory is reconstructed and fused. The result is as Figure 6 shown. Using the vehicle trajectory reconstruction and fusion method proposed by the present invention, a complete vehicle trajectory can be obtained, and it is close to the Figure 4 true vehicle trajectory shown. Changing the layout spacing between the two devices, the relationship between the trajectory reconstruction and fusion error and the device layout spacing is as Figure 7 shown. As the layout spacing increases, the reconstruction and fusion trajectory error increases.

[0222] Combining steps one to three, Table 1 shows the average value of the reconstruction trajectory errors of all vehicles obtained by each method. Comparing the experimental results, it can be seen that the trajectory reconstruction method considering driver differences after optimization in this chapter has a MAE 72.48% lower than that of the method using only the section traffic detection device and reconstructing the trajectory by the speed recursion method; it has a MAE 8.18% lower than that of the trajectory reconstruction method using 15% penetration detection vehicles combined with the section traffic detection device and adopting the Newell car-following model.

[0223] Table 1 Comparison of vehicle trajectory reconstruction errors

[0224] Trajectory reconstruction method MAE (m) MAPE (%) RMSE (m) Cross-section traffic detection equipment 24.89 4.98 29.97 Probe vehicle combined with cross-section traffic detection equipment (5%) 24.11 4.82 28.04 Probe vehicle combined with cross-section traffic detection equipment (10%) 15.06 3.01 17.35 Probe vehicle combined with cross-section traffic detection equipment (15%) 7.46 1.49 8.99 Long-distance traffic detection equipment (before optimization) 9.60 1.92 15.66 Long-distance traffic detection equipment (after optimization) 6.85 1.37 11.15

[0225] Combined with Steps 1 to 4, a vehicle trajectory fusion experiment is conducted using the sub-dataset of the Jinan Ring Expressway. The average errors of the reconstructed trajectories and the fused trajectories of all vehicles from upstream and downstream devices in the trajectory reconstruction area (the area from 2,900 meters to 3,100 meters of the road section) are calculated respectively. The results are shown in Table 2. It can be seen that under various lanes and traffic volumes, compared with the reconstructed trajectories from individual upstream and downstream devices, the MAE of the fused trajectories is reduced by an average of 60.08%.

[0226] Table 2 Trajectory Errors of Reconstructed and Fused Sub-dataset of Jinan Ring Expressway

[0227]

[0228] The complete vehicle trajectory data obtained by reconstruction can be applied to the vehicle-road collaborative system. First of all, the reconstructed vehicle trajectory data can support real-time traffic management decisions, including optimizing traffic signal control and adjusting intersection traffic flow distribution, so as to maximize the utilization rate of road capacity and reduce traffic congestion. Secondly, the analysis of reconstructed vehicle trajectories can help identify and solve road network bottleneck problems, optimize road planning and design, and improve the overall operation efficiency and safety of the traffic system. In addition, these data can also be used for driving behavior evaluation and accident reconstruction, providing a scientific basis for accident investigation and traffic safety improvement.

[0229] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification only illustrates the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A vehicle trajectory reconstruction and fusion method based on sparse long-distance traffic detection devices, characterized in that It includes the following steps: Step 1: Use long-distance traffic detection equipment to obtain vehicle detection data of multiple vehicles in its detection area in real time, so as to obtain the vehicle trajectories of multiple vehicles; Step 2: According to the vehicle trajectories collected by the long-distance traffic detection equipment in real time, reconstruct the vehicle trajectories upstream and downstream of the equipment detection area; Step 3: According to the vehicle trajectories reconstructed in Step 2, calculate the travel time error of the reconstructed trajectories, and aim to minimize the travel time error of the reconstructed trajectories to calibrate the parameters of the Newell following model and the inverse following model of each vehicle; Step 4: Use a particle filter to fuse two different reconstructed trajectories of the same vehicle reconstructed by adjacent long-distance traffic detection equipment in the same area to obtain a complete and smooth vehicle trajectory.

2. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 1, characterized in that In Step 1, the vehicle detection data includes the position, speed, acceleration information of the vehicle, as well as the vehicle length, color, vehicle type and license plate number information.

3. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 1, wherein The specific method of Step 2 is: Regard the reconstructed trajectory as the spatio-temporal offset of the collected trajectory according to the Newell following model and the inverse following model; For the target vehicle of trajectory reconstruction, the trajectory upstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple leading vehicles; The trajectory downstream of its detection area is regarded as the spatio-temporal offset of the trajectories of multiple following vehicles; For the vehicle trajectories in the blind area of the Newell following model and the inverse following model trajectory reconstruction, use the speed recursion method for reconstruction.

4. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 3, wherein For the upstream trajectory reconstruction of the detection area, when reconstructing the target vehicle trajectory, according to the Newell following model, the vehicle trajectory segment not detected by the long-distance traffic detection equipment is regarded as the spatio-temporal offset of the trajectories of multiple leading vehicles detected by the equipment, and the offset formula is: x n (t + τ n ) = x n-1 (t) - δ n where x n (t + τ n ) is the position of the nth vehicle, i.e., the position of the following vehicle at time t + τ n ; x n-1 (t) is the position of the (n - 1)th vehicle, i.e., the position of the preceding vehicle at time t; τ n is the time offset of the nth vehicle; δ n is the space offset of the nth vehicle.

5. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 3, characterized in that For the downstream trajectory reconstruction of the detection area, when reconstructing the target vehicle trajectory, according to the inverse following model extended based on the Newell following model, regard the undetected vehicle trajectory as the spatio-temporal offset of the trajectory segments of multiple following vehicles behind it, and the offset formula is: x n (t - τ n ) = x n+1 (t) + δ n Where x n (t - τ n ) is the position of the nth vehicle, i.e., the position of the following vehicle at time t - τ n ; x n+1 (t) is the position of the (n + 1)th vehicle, i.e., the position of the leading vehicle at time t; τ n is the time offset of the nth vehicle; δ n is the space offset of the nth vehicle.

6. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 3, characterized in that For the vehicle trajectories that cannot be reconstructed according to the Newell following model and the Newell inverse following model, use the speed recursion method for trajectory reconstruction; this method uses the arithmetic mean to estimate the vehicle speed of the target section, and the vehicle speed calculation formula between adjacent section detection points is: where x i and x i+1 are the position coordinates of two adjacent detection points upstream and downstream of the road respectively, x is the spatial position coordinate of the target vehicle on the road, where x i ≤ x ≤ x i+1 , v(x i+1 ) and v(x i ) are the vehicle speeds detected by the adjacent upstream detection point and the downstream detection point respectively; by means of the instantaneous vehicle speed detected by the adjacent long-distance traffic detection equipment and the endpoint speed of the missing position of the trajectory reconstructed according to the Newell car-following model, the missing trajectory can be reconstructed by using the speed recursion method.

7. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 1, characterized in that The specific method of Step 3 is: (3.1) Set the initial spatial lag parameter: Set different spatial lag parameters for the Newell following model of each vehicle: X = {δ1, δ2, δ3,..., δ j ,..., δ m} where X is the set of all vehicle spatial lag parameters, and δ j is the spatial lag value of the j-th vehicle, and m is the number of vehicle trajectories detected by the long-distance traffic detection device; (3.2) Reconstruct the vehicle trajectories using the given parameters: Use the given set of spatial lag parameters X to reconstruct the trajectories of each vehicle in turn, and the reconstructed vehicle trajectories are expressed as: Y(X) = {y(δ1), y(δ2), y(δ3),..., y(δ j ),..., y(δ m )} where Y(X) is the set of vehicle trajectories reconstructed using the set X of vehicle spatial lag parameters, and y(δ j ) is the trajectory of the j-th vehicle reconstructed using the spatial lag parameter δ j of the j-th vehicle; (3.3) Calculate the vehicle travel time error: The travel time error obtained from the vehicle trajectory reconstruction is: e(δ j ) = |t(δ j ) rec - t(j) real | where \(e(\delta j )\) is the trajectory travel time error obtained by reconstructing the \(j\)-th vehicle, and \(t(\delta j ) rec is the travel time obtained by reconstructing the trajectory of the \(j\)-th vehicle using the given spatial lag parameter \(\delta j \), and \(t(j) real is the actual observed travel time of the \(j\)-th vehicle, where \(j = 1, 2, 3, \ldots, m\); (3.4) Update the spatial lag parameter: Set the optimization objective of each vehicle trajectory reconstruction to minimize the travel time error obtained from the reconstructed trajectory of the target vehicle, and the objective function is: where \(E(X)\) is the sum of travel time errors of all vehicle trajectories reconstructed using the set \(X\) of vehicle space lag parameters; finding the optimal space lag parameter \(\delta\) for each vehicle based on the objective function j After that, update the space lag parameters of each vehicle and repeat steps (3.2) to (3.3) until the optimal set \(X\) of vehicle space lag parameters is obtained, minimizing the value of \(E(X)\); (3.5) Output the optimization result of the vehicle trajectory reconstruction Use the calculated set of vehicle spatial lag parameters X to reconstruct the trajectories of each vehicle, so as to improve the accuracy of vehicle trajectory reconstruction.

8. The vehicle trajectory reconstruction and fusion method based on a sparse long-distance traffic detection device according to claim 1, characterized in that The specific method of Step 4 is: (4.1) Design the state space model: The state space model in the particle filter includes a state vector and an observation vector; State vector x t Describes the kinematic characteristics of the vehicle at time t, x t Is expressed as: where \(l\) t is the position of the vehicle at time \(t\), and \(v\) t is the speed of the vehicle at time \(t\); Assume the acceleration a t-1 is the system process noise, and establish the discrete-state dynamics equation of vehicle motion: where T is the time frame interval for data acquisition, l t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1, a t -1 is the acceleration of the vehicle at time t-1; Observation vector y t is as follows: The observation equation is expressed as: In the formula, is the observed value of the reconstructed trajectory of the upstream vehicle at time t, is the observed value of the reconstructed trajectory of the downstream vehicle at time t, and are the position and velocity of the trajectory respectively, and are the position and velocity of the trajectory respectively, and R is the observation noise; Add the following limiting conditions during the process of vehicle motion state transition: after the state transition, the vehicle position state at the current time step is greater than or equal to the vehicle position state at the previous time step; after the state transition, the vehicle speed state at the current time step is greater than or equal to zero and less than the highway speed limit; after the state transition, the distance between the position of the vehicle at the current time step and the position of the vehicle in front should be greater than or equal to the minimum stopping distance. (2) Adjust the particle weights to fuse the vehicle trajectories In the particle filter, the formula for updating the particle weights for trajectory fusion is: In the formula, is the particle weight of the previous state, is the particle weight calculated based on the upstream reconstructed trajectory observation value is the particle weight calculated based on the downstream reconstructed trajectory observation value t is the current time step, and t3–t2 is the total time length of the fusion trajectory;​ After each round of particle weight update, the weighted average of each particle is taken to obtain the estimated value of the fused trajectory point at the current moment.

Images