A vehicle trajectory smoothing and reconstruction system based on the combination of model and data

Abstract

The present invention relates to the field of autonomous driving data processing, and in particular to a vehicle trajectory smoothing and reconstruction system based on the combination of a model and data, including: a data pre-processing module 1, a noise reduction module, a data pre-processing module 2, a trajectory reconstruction module, a data post-processing module, and a training module. The present invention embeds a kinematic model reconstruction unit into the trajectory reconstruction module, trains the trajectory reconstruction module through data, realizes the reconstruction of the trajectory and the extraction of driving actions, and designs a noise reduction module to enhance the robustness of the entire system. The system of the present invention can strictly meet the physical constraints of the vehicle model, the designed network can meet the hard constraint conditions, and can learn and extract driving actions, improving the accuracy of trajectory reconstruction and driving action extraction.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving data processing, and particularly to a vehicle trajectory smoothing and reconstruction system based on the combination of models and data. Background Art

[0002] In fields such as autonomous driving, it is often necessary to sense the trajectory information of surrounding vehicles through sensors. However, the noise of sensors is inevitable, especially in the case of pure vision perception or low-precision perception devices. The trajectory data points polluted by noise will affect downstream tasks such as prediction and decision-making, and it is also difficult to obtain driving actions (acceleration / deceleration and steering) from the trajectory data. This problem is also reflected in the following situations in real engineering: the trajectory data sets used for downstream task training usually adopt publicly available data sets collected by high-precision devices and optimized offline. When deployed on real vehicles, due to limited computing power and sensor performance, the trajectory quality drops significantly, resulting in a decline in the performance of the model after deployment.

[0003] Therefore, it is very necessary to smooth the trajectories obtained by sensors.

[0004] Existing trajectory smoothing methods include filtering methods, fitting methods, and learning methods.

[0005] Among them, filtering methods include Kalman filtering, particle filtering (refer to CN 117973167 A), mean filtering, etc. Usually, the solution efficiency is relatively high and they can run online in real time. However, these methods usually analyze individual trajectory points to obtain the optimal position of a single point. Therefore, they can only reduce noise and improve the jitter of the trajectory, but there will still be significant noise when extracting driving actions from the trajectory. Existing methods for directly obtaining driving actions through vehicle model formulas are very sensitive to noise, and trajectories with a small amount of noise may cause the driving actions to be completely submerged in the noise.

[0006] Fitting methods include polynomial fitting, spline curve fitting, Bezier curve fitting, etc. This type of method has a high degree of trajectory smoothing, but it is limited by the prior of the fitting model, and the diversity of the smoothed trajectory will decrease. In addition, when the form of the selected fitting function is inappropriate, it will cause trajectory distortion, resulting in single and distorted driving actions being extracted. The essence of the fitting problem is an optimization problem, and the solution calculation amount is relatively large. It is more difficult to solve when considering constraints, and the real-time performance is poor.

[0007] Learning methods include inverse reinforcement learning (refer to CN 117407474 A), large language models (refer to CN119377666 A), etc. Usually, they have higher flexibility, but existing models completely rely on data, it is difficult to impose hard constraints, and the reliability is difficult to guarantee. At the same time, using overly complex models is not conducive to actual deployment and use.

[0008] In addition, with the development of end-to-end autonomous driving, there is an increasing urgency to obtain the output of driving actions. The existing methods for obtaining driving actions such as acceleration, deceleration, and steering mainly collect them by arranging sensors inside the vehicle. This method has high costs and low efficiency in obtaining driving actions, and the diversity of driving actions is not as rich as the driving data on public roads. Summary of the Invention

[0009] In view of the problems existing in the prior art, the present invention proposes a vehicle trajectory smoothing and reconstruction system based on the combination of a model and data. By introducing a trajectory reconstruction module, the trajectory data is first processed into driving actions, and then a smooth reconstructed trajectory is obtained through kinematic model reconstruction, thereby realizing the reconstruction of the trajectory and the extraction of driving actions; in addition, a noise reduction module is designed to enhance the robustness of the entire system.

[0010] Technical solution of the present invention:

[0011] A vehicle trajectory smoothing and reconstruction system based on the combination of a model and data includes: a first data preprocessing module, a noise reduction module, a second data preprocessing module, a trajectory reconstruction module, a data postprocessing module, and a training module.

[0012] The system inputs a noisy trajectory and outputs a reconstructed trajectory. The reconstructed trajectory is smoother than the input trajectory and strictly satisfies the physical constraints of the kinematic model.

[0013] Specifically, the noisy trajectory first passes through the first data preprocessing module to obtain the noisy trajectory features. The first data preprocessing module rotates and translates the trajectory to reduce the trajectory distribution space and improve the learning efficiency.

[0014] The noisy trajectory features pass through the noise reduction module to output a low-noise trajectory. The noise reduction module can reduce noise and ensure the robustness of the subsequent module processing.

[0015] The second data preprocessing module processes the low-noise trajectory and outputs low-noise trajectory features and boundary conditions.

[0016] The low-noise trajectory and boundary conditions are input into the trajectory reconstruction module for processing to obtain a standard reconstructed trajectory.

[0017] The data postprocessing module restores the standard reconstruction to its original position through translation and rotation.

[0018] The training module plays a role during training and is used to train the noise reduction module, the trajectory reconstruction module, and the overall model fine-tuning training.

[0019] Further, the trajectory reconstruction module includes a driving action encoding unit and a kinematic model reconstruction unit. The denoised trajectory features are processed by the driving action encoding unit to obtain driving actions. The driving actions and boundary conditions are input into the kinematic model reconstruction unit to obtain a standard reconstructed trajectory. The driving action encoding unit is a learnable network. After being trained, the driving action encoding unit can process the low-noise trajectory features into driving actions. The kinematic model reconstruction unit is a group of networks built with deterministic operators, which can calculate the standard reconstructed trajectory from the driving actions and boundary conditions and can also propagate gradients during training.

[0020] Further, the vehicle trajectory smoothing and reconstruction system based on the combination of model and data further includes an offline kinematic model reconstruction unit.

[0021] The input and output of the offline kinematic model reconstruction unit are the same as those of the kinematic model reconstruction unit. It can also calculate the standard reconstructed trajectory according to the driving actions and boundary conditions and has higher accuracy. It can replace the kinematic model reconstruction unit to implement the function of calculating the standard reconstructed trajectory.

[0022] Beneficial effects

[0023] The present invention proposes a vehicle trajectory smoothing and reconstruction system, which can reconstruct a noisy trajectory to obtain a smooth reconstructed trajectory. Compared with traditional methods, the present invention has the following advantages:

[0024] 1. Considering the physical constraints of the vehicle model: The kinematic model reconstruction unit incorporates physical equations into the model training process. Through the design of activation functions, the network can meet the hard constraint conditions.

[0025] 2. Strong robustness of the model: By using a preposed noise reduction module, the robustness of the model is enhanced; by adding noise, missing point masks, etc., it can handle situations such as trajectory noise, trajectory frame loss, and incomplete trajectories.

[0026] 3. Improving the accuracy of trajectory reconstruction and driving action extraction: Adopting a bidirectional recursive trajectory reconstruction method enables the gradient of the trajectory error to be propagated bidirectionally during training, improving the accuracy of trajectory reconstruction and driving action extraction. In addition, the offline kinematic model reconstruction unit adopts a two-point reconstruction method to eliminate the sensitive dependence of the initial value recursive reconstructed trajectory on speed and heading, and can obtain a more accurate and robust trajectory reconstruction result. Description of the drawings

[0027] Figure 1 Schematic diagram of the system framework of the present invention;

[0028] Figure 2 Schematic diagram of the driving action encoding unit of the system of the present invention;

[0029] Figure 3Schematic diagram of the definitions of 5 operators in the splitting of the discrete model according to the embodiments of the present invention;

[0030] Figure 4 One-way recursive computational graph according to the embodiments of the present invention;

[0031] Figure 5 Two-way recursive computational graph according to the embodiments of the present invention;

[0032] Figure 6 Schematic diagram of the training unit of the noise reduction module of the system according to the present invention;

[0033] Figure 7 Schematic diagram of the training unit of the trajectory reconstruction module of the system according to the present invention;

[0034] Figure 8 Schematic diagram of the overall fine-tuning training unit of the system according to the present invention;

[0035] Figure 9 Schematic diagram of the training strategy of the system according to the present invention;

[0036] Figure 10 Schematic diagram of the trajectory smoothing effect according to the embodiments of the present invention ((a) straight trajectory (b) turning trajectory);

[0037] Figure 11 Schematic diagram of the driving action extraction effect according to the embodiments of the present invention ((a) reconstructed trajectory diagram (b) reconstructed acceleration and deceleration actions (c) reconstructed steering actions). Detailed implementation manners

[0038] The technical solution provided by the present application will be further described below in conjunction with specific embodiments and their accompanying drawings. In combination with the following description, the advantages and features of the present application will become clearer.

[0039] As Figure 1 is the overall architecture diagram of the system according to the present invention, the implementation method will be described in detail below based on this architecture diagram.

[0040] A vehicle trajectory smoothing and reconstruction system based on the combination of model and data, including a data preprocessing module I, a noise reduction module, a data preprocessing module II, a trajectory reconstruction module, an offline kinematic model reconstruction unit, a data postprocessing module, and a training module ( Figure 1 not shown in the figure).

[0041] The trajectory reconstruction module includes a kinematic model reconstruction unit and a driving action encoding unit.

[0042] The entire system inputs a noisy trajectory and outputs a reconstructed trajectory. The reconstructed trajectory is smoother than the input trajectory and strictly satisfies the physical constraints of the kinematic model.

[0043] The noise trajectory first passes through the first data preprocessing module to obtain the noise trajectory features. The main function of the first data preprocessing module is to rotate and translate the trajectory, reduce the trajectory distribution space, and improve the learning efficiency.

[0044] The noise trajectory features then pass through the noise reduction module to output a low-noise trajectory. The noise reduction module can reduce noise and ensure the robustness of the subsequent module processing, but the output low-noise trajectory does not strictly meet the physical constraints.

[0045] The second data preprocessing module processes the low-noise trajectory to output low-noise trajectory features and boundary conditions.

[0046] Both the low-noise trajectory and the boundary conditions enter the trajectory reconstruction module for processing to obtain a standard reconstructed trajectory.

[0047] The trajectory reconstruction module includes a driving action encoding unit and a kinematic model reconstruction unit. The denoised trajectory features pass through the driving action encoding unit to obtain driving actions. The driving actions and boundary conditions are input into the kinematic model reconstruction unit to obtain a standard reconstructed trajectory.

[0048] The driving action encoding unit is a learnable network. After being trained, the driving action encoding unit can process the low-noise trajectory features into driving actions.

[0049] The kinematic model reconstruction unit is a set of computational networks built with deterministic operators. It calculates the standard reconstructed trajectory according to the driving actions and boundary conditions and can propagate gradients during training.

[0050] The data post-processing module restores the standard reconstruction to its original position through translation and rotation.

[0051] The input and output of the offline kinematic model reconstruction unit are the same as those of the kinematic model reconstruction unit. It can also calculate the standard reconstructed trajectory according to the driving actions and boundary conditions and has higher accuracy. After the trajectory reconstruction module is trained, the kinematic model reconstruction unit can be replaced with the offline kinematic model reconstruction unit to obtain higher accuracy.

[0052] The training module plays a role during training and needs to call the above-mentioned first data preprocessing module, noise reduction module, second data preprocessing module, and trajectory reconstruction module.

[0053] The following details each functional module, where:

[0054] The first data preprocessing module: First, the noise trajectory data needs to be preprocessed. The trajectory is translated and rotated to move the end point of the trajectory to the origin and rotate the trajectory so that the heading angle of the end point is 0. After processing, the noise trajectory features are obtained. The purpose of this data preprocessing is to compress the trajectory distribution space and improve the learning efficiency (prior art);

[0055] The noise reduction module: After being trained, when the noise trajectory features are input, it can output the low-noise trajectory. The processing of the noise reduction module includes: first, flattening the trajectory tensor, then processing it through a multi-layer perceptron layer (mlp), and finally restoring the shape of the output tensor (unflattening).

[0056]

[0057] Among them, is the input noise trajectory feature, is the output low-noise trajectory.

[0058] The data preprocessing module II processes the low-noise trajectory output by the noise reduction module. Since driving actions are different from trajectory points and there is no spatial position dependence relationship, the coordinates of adjacent trajectory points of the low-noise trajectory are differentially calculated to obtain the low-noise trajectory features and compress the data distribution. In addition, the boundary conditions need to be calculated for use by the kinematic model reconstruction unit or the offline kinematic model reconstruction unit. The boundary conditions include the coordinates (x, y) of the trajectory boundary points and the states (v, φ) of the boundary points.

[0059] The boundary points can be the starting point, the ending point or any point on the trajectory. The number of boundary points and the selection of boundary points vary according to the requirements of the reconstruction algorithm. In the following embodiments, the initial value one-way recursive trajectory reconstruction only requires the coordinates and states of one boundary point, while the initial value two-way recursive trajectory reconstruction method requires the coordinates of two boundary points and the state of one of them.

[0060] The method for calculating the state of a certain boundary point through discrete coordinates is as follows

[0061]

[0062] In the formula and are the boundary point and its adjacent point, is the speed, is the heading angle, is the trajectory point sampling period.

[0063] The function of the trajectory reconstruction module is to process the trajectory to obtain a vehicle kinematic model that can be used to analyze the smooth trajectory of driving actions. Limited by the vehicle system and real physical conditions, the trajectory of an actual vehicle cannot have severe jitters and jumps. The function of the trajectory reconstruction module is to first obtain a driving action that meets the physical conditions, and then reconstruct the driving action into a vehicle trajectory. By completing these two operations, a trajectory that can be smoothly analyzed by the vehicle kinematic model can be obtained. These two operations are respectively completed by the driving action encoding unit and the kinematic model reconstruction unit.

[0064] The trained driving action encoding unit can process the low-noise trajectory features into driving actions.

[0065] The driving action encoding unit is composed of a multi-layer perceptron and an activation function layer, as Figure 2 shown:

[0066] The driving action encoding unit is a learnable network

[0067]

[0068] Among them, F is the functional form representation of the driving action encoding unit, represents the learnable parameters, is the driving action, including acceleration and deceleration actions and steering actions , represents the constraint parameters:

[0069] ,

[0070] Among them, is the upper bound of the vehicle acceleration and deceleration actions, is the upper bound of the steering action.

[0071] Specifically, a multi-layer perceptron mlp can be selected to obtain the intermediate features of the driving action (including the intermediate features of acceleration and deceleration and the intermediate features of steering ).

[0072]

[0073] Among them, is the low-noise trajectory feature processed by the second data preprocessing module.

[0074] The intermediate features are encoded by a multi-layer perceptron and the driving actions that meet the hard constraint limits are obtained through an activation function with upper and lower limits. The activation function layer needs to have symmetric upper and lower limits, and the hyperbolic tangent activation function can be optionally used to ensure that the output meets the hard constraints

[0075]

[0076] The constraint boundaries of acceleration and deceleration , and the constraint boundaries of steering can be selected.

[0077] The kinematic model reconstruction unit is designed in the following way:

[0078] Theoretical part (deduction)

[0079] First, start the derivation from the vehicle kinematic model:

[0080] The vehicle kinematic model with the center of the rear axle of the vehicle as the reference point can be expressed as

[0081]

[0082] where the ratio of the tangent of the front wheel steering angle and the wheelbase is defined as the equivalent steering input . It can be proved through theoretical derivation that the absolute value of the equivalent steering input is the same as the curvature of the trajectory.

[0083] Next, discretize:

[0084] Since the trajectory points are discrete and a discrete model is needed, the above model is discretized to obtain:

[0085]

[0086] where, and are the horizontal and vertical coordinates of the vehicle at the kth and (k + 1)th steps; and are the speeds of the vehicle at the kth and (k + 1)th steps; and are the heading angles of the vehicle at the kth and (k + 1)th steps; is the discrete step length, which is consistent with the sampling period in the dataset.

[0087] Next, the discrete model is split, and 5 operators are designed, including: the forward trajectory coordinate recursion operator, the forward state recursion operator, the backward trajectory coordinate recursion operator, the backward state recursion operator, and the averaging operator, where:

[0088] is the forward trajectory coordinate recursion operator,

[0089] is the forward state recursion operator,

[0090] is the backward trajectory coordinate recursion operator,

[0091] is the backward state recursion operator,

[0092] represents the averaging operator,

[0093] The above forward means starting the recursion from the trajectory coordinate point with the smallest time when reconstructing the trajectory, and the backward means starting the recursion from the trajectory coordinate point with the largest time.

[0094] The expressions of each operator are as Figure 3 shown.

[0095] The function that the kinematic model reconstruction unit needs to complete is to construct the back trajectory from the driving action

[0096]

[0097] Among them, is the driving action, is the boundary condition, is the reconstructed trajectory, is the kinematic trajectory recursion, which is a definite process without learnable parameters in the middle.

[0098] Specifically, the kinematic model reconstruction unit uses the above operators, combines the driving action and the boundary condition, and reconstructs the trajectory. There are many ways for the kinematic model reconstruction unit to reconstruct the trajectory, and different recursive reconstruction methods have different effects during error backpropagation and thus different effects during training.

[0099] As an example, the kinematic model reconstruction unit is implemented using the initial value unidirectional recursive trajectory reconstruction and the initial value bidirectional recursive trajectory reconstruction respectively.

[0100] For the initial value unidirectional recursive trajectory reconstruction, the computational graph is as Figure 4 shown. The recursion can start from the end point or the start point, and their processing processes are similar. Figure 4 The shown computational graph is the computational graph of the initial value unidirectional recursion starting from the end point:

[0101] First, the boundary conditions at the end point include the boundary point coordinates and the boundary point state . Input and into the reverse trajectory coordinate recursion operator g1 to obtain the coordinate .

[0102] Input the boundary point state and the driving action into the reverse state recursion operator g2 to obtain the state .

[0103] Similar to the recursion, input the driving action ; until is calculated until the end point position, and the complete trajectory coordinates can be obtained; … , and the state during the process; … 。

[0104] The complete trajectory coordinate points are obtained through the above recursive method, which is the standard reconstructed trajectory.

[0105] The initial value two-way recursive trajectory reconstruction has a calculation diagram as Figure 5 shown.

[0106] Similar to the initial value one-way recursive reconstruction, it can start from the end point or start from the end point. The processing process is similar and the effect is approximate, which is selected according to requirements. As Figure 5 shown, taking the recursive process starting from the end point as an example:

[0107] The first half of the recursive process is the same as the initial value one-way recursive trajectory reconstruction, and the complete trajectory coordinates are obtained ; … , and the states during the process ; … 。

[0108] Subsequently, recalculate with and as boundary conditions. Among them, uses the boundary conditions processed by the second data preprocessing module, while is obtained from the results of the forward segment-by-segment recursive process.

[0109] The purpose of using the results from the forward segment-by-segment recursive process is to ensure the consistency of the trajectory shape. The initial value two-way recursive trajectory reconstruction method takes the state of one end as the input of the other direction, so an initial value condition of a boundary point state and two boundary point coordinates is required. The forward and reverse reconstructed trajectory shapes are the same, and the average trajectory shapes calculated are also the same.

[0110] As Figure 5 shown, and are input into the forward trajectory coordinate recursion operator f1 to calculate . The boundary point state and the driving action are input into the forward state recursion operator f2 to obtain the state .

[0111] Similar recursion, the driving action ; until is calculated until the starting point position, and the complete trajectory coordinates ; …, 。

[0112] Finally, the complete trajectories obtained in the two directions are averaged to obtain the final output trajectory, that is, the standard reconstructed trajectory.

[0113] The characteristics of the above two methods are as follows:

[0114] For the single - direction recursive trajectory reconstruction method with initial value, the trajectory point error can only be propagated backward to the driving actions in front of it, while for the two - direction recursive trajectory reconstruction method with initial value, the error gradient backpropagation can be propagated from two directions.

[0115] The training module includes: a noise reduction module training unit, a trajectory reconstruction module training unit, and an overall fine - tuning training unit, which are respectively used for training the noise reduction module, the trajectory reconstruction module, and the overall fine - tuning training of the model. Specifically as follows:

[0116] As Figure 6 shown, the input of the noise reduction module training unit is the trajectory of the trajectory dataset, and the output is the pre - training parameters of the noise reduction module. The noise reduction module training unit first performs data pollution processing on the dataset trajectory: finds a high - quality trajectory dataset, slices the trajectory into the same appropriate length. Subsequently, the trajectory is polluted, and the processing means include adding various forms of random noise, randomly discarding some data, and incomplete segment data. Record the masks for the positions of the discarded and incomplete data.

[0117] The polluted noisy trajectory is used as the input of the noise reduction module, and the original high - quality trajectory data is used as the label. The lost frames and incomplete data are calculated using the masked labels.

[0118] The noise reduction module training subsystem includes: a data pre - processing module 1, a noise reduction module, and a data post - processing module. The noise reduction module training unit can obtain the pre - training parameters of the noise reduction module after training.

[0119] The loss function of the noise reduction module training unit is

[0120]

[0121] where N is the trajectory length, that is, the number of coordinate points in the trajectory, is the trajectory output by the noise reduction module, is the standard trajectory obtained after passing through the data pre - processing module 1, that is, the input trajectory of the noise reduction module.

[0122] As Figure 7As shown, the input of the trajectory reconstruction module training unit is the dataset trajectory, and the output is the pre-training parameters of the driving action encoding unit. The trajectory reconstruction module training unit inputs the dataset trajectory into the trajectory reconstruction module training subsystem. The dataset trajectory is used as a label for training. The trajectory reconstruction module training subsystem includes: a data preprocessing module 1, a data preprocessing module 2, a trajectory reconstruction module, and a data postprocessing module. After being trained, the trajectory reconstruction module training unit can obtain the pre-training parameters of the driving action encoding unit.

[0123] When training the trajectory reconstruction module, two aspects of factors are considered. One is to define the MSE error of trajectory reconstruction based on the proximity of the reconstructed trajectory

[0124]

[0125] Among them, is the trajectory output by the trajectory reconstruction module, is the standard trajectory obtained after passing through the data preprocessing module 1.

[0126] The other is the smoothness of the driving action, which is measured by the average action amount. Usually, when completing the same task, human drivers tend to use the minimum action amount rather than making high-frequency repeated adjustments. Therefore, the loss of the driving action smoothness is designed as

[0127]

[0128] Among them is the loss of acceleration and deceleration actions, is the loss of steering actions.

[0129] The final training loss function is composed of these two parts

[0130]

[0131] Among them, and are weight coefficients, and the selectable range is , in this embodiment, is selected, .

[0132] For example Figure 8As shown, the input of the overall fine-tuning training unit is the trajectory in the trajectory dataset, the pre-trained parameters of the noise reduction module, and the pre-trained parameters of the driving action encoding unit, and the output is the final parameters of the noise reduction module and the final parameters of the driving action encoding unit. The overall fine-tuning training unit processes the data of the dataset trajectory for data contamination and then inputs it into the overall fine-tuning training subsystem. The dataset trajectory is used as a label for training. The overall fine-tuning training subsystem includes: a data preprocessing module 1, a noise reduction module, a data preprocessing module 2, a trajectory reconstruction module, and a data postprocessing module. The overall fine-tuning training unit uses the pre-trained parameters of the noise reduction module and the pre-trained parameters of the driving action encoding unit as initial parameters, and can obtain the final parameters of the noise reduction module and the final parameters of the driving action encoding unit after fine-tuning training.

[0133] The loss function during the training of the overall fine-tuning training unit is the same as that of the trajectory reconstruction module training unit.

[0134] As Figure 9 shown, the training strategy is to first train the noise reduction module training unit and the trajectory reconstruction module training unit. Then, the pre-trained parameters obtained from the training are passed to the overall fine-tuning training unit for training.

[0135] In addition, since the boundary point state (v, φ) needs to be calculated indirectly through coordinates, and the initial boundary point state (v, φ) has a great influence on the trajectory reconstruction result and is prone to amplifying errors during the recurrence process. Therefore, an offline kinematic model reconstruction unit is designed, which has higher computational accuracy (but a larger amount of computation). After the model training is completed, it can replace the kinematic model reconstruction unit to complete the trajectory reconstruction (or the kinematic model reconstruction unit can also be continued to be used).

[0136] The offline kinematic model reconstruction unit adopts a two-point reconstruction method, that is, it reconstructs according to the driving action output by the driving action encoding unit and the boundary point coordinates of the starting point and the ending point. The boundary point coordinates are more accurate than the boundary point state, and the trajectory point coordinate noise has less influence on the reconstruction result. However, the two-point reconstruction method cannot directly obtain the coordinates of each intermediate point. Therefore, the boundary point state is first solved using the boundary point coordinates of the starting point and the ending point, and then the complete reconstructed trajectory is obtained by recurrence. Solving the boundary point state requires solving the following non-linear equation

[0137]

[0138] In the formula, is the starting point trajectory coordinate, is the ending point trajectory coordinate.

[0139] Perform variable substitution ,

[0140] Thus, separating the variables gives

[0141]

[0142] Subsequently, it is calculated in two cases

[0143] When The non - linear equation to be solved is

[0144]

[0145] And Or When, the equation is

[0146]

[0147] It is calculated in two cases and obtained Then solve

[0148]

[0149] It is difficult to obtain an analytical solution for the above non - linear equation, and numerical methods can be used to solve it. Therefore, it can only be reconstructed in an offline manner.

[0150] The initial state is obtained by solving 、 After that, the trajectory can be reconstructed by one - way recursive using the initial values.

[0151] The trajectory results extracted in this embodiment are as Figure 10 shown. Among them, in the figure, (a) is the straight - line trajectory, and (b) is the turning trajectory. The results show that the method proposed by the present invention can effectively reduce noise and obtain a smooth trajectory. The blue curve in the figure is the high - quality trajectory in the dataset, and the green curve is the noisy trajectory after contaminating the original data. The red curve is the reconstructed trajectory processed by the vehicle trajectory smoothing and reconstruction system described in the present invention.

[0152] The driving action results extracted in this embodiment are as Figure 11 shown. Among them, in the figure, (a) is the trajectory before and after reconstruction, (b) is the acceleration and deceleration action, and (c) is the steering action. Figure 11 The blue curve in it is the high - quality trajectory and the driving action directly calculated by the differential method. The red curve is the reconstructed trajectory and the driving action output by the driving action coding unit in the trajectory reconstruction module, and the black dotted line is the physical constraint.

[0153] From Figure 10 And Figure 11 The implementation effects of, it can be concluded that the method can effectively smooth the jittery vehicle trajectory, and the smoothed trajectory can strictly meet the vehicle physical constraints. This processing method can effectively extract driving actions.

[0154] The above description is only for the description of the preferred embodiments of the present application, and does not limit the scope of the present application in any way. Any change or modification made by any ordinary person skilled in the art according to the disclosed technical content should be regarded as an equivalent effective embodiment, and all fall within the scope of protection of the technical solution of the present application.

[0155] Appendix: Glossary of Terms:

[0156] Driving action: The action of operating a vehicle to move, including steering, accelerating, braking, etc.

[0157] Multilayer Perceptron (MLP): A feedforward neural network that contains one or more hidden layers and can learn non-linear models to classify or perform regression analysis on input data.

Claims

1. A vehicle trajectory smoothing and reconstruction system based on the combination of model and data, characterized in that Including: A first data pre - processing module, a noise reduction module, a second data pre - processing module, a trajectory reconstruction module, a data post - processing module, and a training module; The system inputs a noisy trajectory and outputs a reconstructed trajectory. The reconstructed trajectory is smoother than the input trajectory and strictly satisfies the physical constraints of the kinematic model. Specifically, The noisy trajectory first passes through the first data pre - processing module to obtain the noisy trajectory features; The first data pre - processing module rotates and translates the trajectory, reducing the trajectory distribution space and improving the learning efficiency. The noisy trajectory features pass through the noise reduction module, and a low - noise trajectory is output; The noise reduction module can reduce noise and ensure the robustness of the subsequent module processing. The second data pre - processing module processes the low - noise trajectory and outputs low - noise trajectory features and boundary conditions. The low - noise trajectory and boundary conditions are input into the trajectory reconstruction module for processing to obtain a standard reconstructed trajectory. The data post - processing module restores the standard reconstruction to its original position through translation and rotation. The training module plays a role during training and is used to train the noise reduction module, the trajectory reconstruction module, and the overall model fine - tuning training. The trajectory reconstruction module includes a driving action encoding unit and a kinematic model reconstruction unit; The noise - reduced trajectory features pass through the driving action encoding unit to obtain driving actions, and the driving actions and boundary conditions are input into the kinematic model reconstruction unit to obtain a standard reconstructed trajectory. The driving action encoding unit is a learnable network. After being trained, the driving action encoding unit can process the low - noise trajectory features into driving actions; The kinematic model reconstruction unit is a group of networks built with deterministic operators and can calculate the standard reconstructed trajectory from the driving actions and boundary conditions. The driving action encoding unit consists of a multi - layer perceptron and an activation function layer. The driving action encoding unit is a learnable network as follows Act = F(Traj; Cons; θ) Where, F represents the functional form of the driving action encoding unit, θ represents the learnable parameters, Act is the driving action, Act = [a; δ] includes the acceleration / deceleration action a and the steering action δ, and Cons represents the constraint parameters: Cons = [scale a , scale δ , Among them, scale a is the upper bound of the vehicle's acceleration and deceleration actions, and scale δ is the upper bound of the steering action; Select a multi-layer perceptron (MLP) to obtain intermediate features of driving actions, including acceleration / deceleration intermediate feature feat a and steering intermediate feature feat δ : [feat a ,feat δ = mlp(feat in ) Among them, feat in is the low-noise trajectory feature obtained by processing of the second data preprocessing module; The intermediate features are encoded by a multi - layer perceptron and pass through an activation function with upper and lower limits to obtain driving actions that meet the hard - constraint limits. The activation function layer uses the hyperbolic tangent activation function tanh() to ensure that the output meets the hard constraints. a = tanh(mlp(feat a )) scale a δ = tanh(mlp(feat δ ))scale δ Select the constraint boundary scale for acceleration and deceleration a = 10, and the constraint boundary scale for steering δ = 0.

25.

2. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 1, wherein The noise reduction module processes as follows: First, flatten the trajectory tensor, then process it through a multi - layer perceptron layer mlp, and finally restore the shape of the output tensor unflatten. Traj denoise = unflatten(mlp(flatten(Traj noise,norm ))) Among them, Traj noise,norm is the input noise trajectory feature, and Traj denoise is the output low-noise trajectory.

3. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 1, characterized in that, The second data pre - processing module processes as follows: Differential calculation is performed on the coordinates of adjacent trajectory points of the low - noise trajectory to obtain low - noise trajectory features and compress the data distribution. Calculate the boundary conditions for use by the kinematic model reconstruction unit or the offline kinematic model reconstruction unit. The boundary conditions include the coordinates (x, y) of the trajectory boundary points and the states (v, φ) of the boundary points. The method for calculating the state of a certain boundary point through discrete coordinates is as follows where (x0, y0) and (x1, y1) are the boundary point and the adjacent point of the boundary point, v is the velocity, is the course angle, and T is the trajectory point sampling period.

4. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 1, characterized in that The kinematic model reconstruction unit is designed in the following way: Theoretical part: First, start the derivation from the vehicle kinematic model: The kinematic model of the vehicle with the center of the rear axle as the reference point can be expressed as Among them, the ratio of the tangent of the front wheel steering angle to the wheelbase is defined as the equivalent steering input δ; it can be proved by theoretical derivation that the absolute value of the equivalent steering input is the same as the curvature of the trajectory; Then discretize it: Since the trajectory points are discrete and a discrete model is needed, the above model is discretized to obtain: where x k y k and x k+1 y k+1 are the horizontal and vertical coordinates of the vehicle at the k-th and (k + 1)-th steps; v k and v k+1 are the speeds of the vehicle at the k-th and (k + 1)-th steps; and are the heading angles of the vehicle at the k-th and (k + 1)-th steps; T is the discrete step length, which is consistent with the sampling period in the dataset; Next, the discrete model is split, and 5 operators are designed, including: forward trajectory coordinate recursion operator, forward state recursion operator, backward trajectory coordinate recursion operator, backward state recursion operator, and averaging operator; The above forward means that when reconstructing the trajectory, it starts from the trajectory coordinate point with the smallest time, and the backward means that it starts from the trajectory coordinate point with the largest time; The kinematic model reconstruction unit completes the function of constructing the trajectory back from the driving actions: Traj re = VKM(Act; Init) Among them, Act is the driving action, Init is the boundary condition, Traj re is the reconstructed trajectory, and VKM is the kinematic trajectory recursion. This is a deterministic process without learnable parameters in the middle; The kinematic model reconstruction unit uses the above operators, combines the driving actions and boundary conditions, and reconstructs the trajectory.

5. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 4, characterized in that The ways for the kinematic model reconstruction unit to reconstruct the trajectory include initial value single-direction recursive trajectory reconstruction and initial value two-direction recursive trajectory reconstruction. Different recursive reconstruction methods have different effects during error backpropagation and thus different effects during training.

6. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 1, wherein The training module includes: noise reduction module training unit, trajectory reconstruction module training unit, and overall fine-tuning training unit, which are used to train the noise reduction module, trajectory reconstruction module, and overall model fine-tuning training respectively; as follows: The input of the noise reduction module training unit is the trajectory of the trajectory dataset, and the output is the pre-training parameters of the noise reduction module. The noise reduction module training unit processes the data of the dataset trajectory to be polluted and then inputs it into the noise reduction module training subsystem; the dataset trajectory is used as a label for training; the noise reduction module training subsystem includes: data pre-processing module 1, noise reduction module, and data post-processing module; the noise reduction module training unit can obtain the pre-training parameters of the noise reduction module after training; The loss function of the noise reduction module training unit is where N is the trajectory length, i.e., the number of coordinate points in the trajectory, is the trajectory output by the noise reduction module, is the standard trajectory obtained after passing through the first data preprocessing module, i.e., the input trajectory of the noise reduction module; The input of the trajectory reconstruction module training unit is the trajectory of the dataset, and the output is the pre-training parameters of the driving action encoding unit. The trajectory reconstruction module training unit inputs the dataset trajectory into the trajectory reconstruction module training subsystem; the dataset trajectory is used as a label for training; the trajectory reconstruction module training subsystem includes: data pre-processing module 1, data pre-processing module 2, trajectory reconstruction module, and data post-processing module; the trajectory reconstruction module training unit can obtain the pre-training parameters of the driving action encoding unit after training; When training the trajectory reconstruction module, two factors are considered. One is to define the MSE error of trajectory reconstruction based on the closeness of the reconstructed trajectory Among them, is the trajectory output by the trajectory reconstruction module, is the standard trajectory obtained after passing through the first data preprocessing module; The other is the smoothness of the driving actions, which is measured by the average action amount. Usually, when completing the same task, human drivers tend to use the smallest action amount rather than adjusting frequently at a high frequency. Therefore, the loss of the smoothness of the driving actions is designed as Among them, Smooth a is the loss of acceleration and deceleration actions, and Smooth δ is the loss of steering actions; The final training loss function consists of these two parts Among them, β1 and β2 are weight coefficients; The input of the overall fine-tuning training unit is the trajectory of the trajectory dataset, the pre-trained parameters of the noise reduction module, and the pre-trained parameters of the driving action encoding unit, and the output is the final parameters of the noise reduction module and the final parameters of the driving action encoding unit; the overall fine-tuning training unit processes the data of the dataset trajectory to be polluted and then inputs it into the overall fine-tuning training subsystem; the dataset trajectory is used as a label for training; the overall fine-tuning training subsystem includes: a data preprocessing module 1, a noise reduction module, a data preprocessing module 2, a trajectory reconstruction module, and a data postprocessing module; the overall fine-tuning training unit uses the pre-trained parameters of the noise reduction module and the pre-trained parameters of the driving action encoding unit as initial parameters, and can obtain the final parameters of the noise reduction module and the final parameters of the driving action encoding unit after fine-tuning training; The loss function during the training of the overall fine-tuning training unit is the same as that of the trajectory reconstruction module training unit; The training strategy is to first train the noise reduction module training unit and the trajectory reconstruction module training unit, and then transfer the pre-trained parameters obtained from the training to the overall fine-tuning training unit for training.

7. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 1, characterized in that It also includes an offline kinematic model reconstruction unit; The input and output of the offline kinematic model reconstruction unit are the same as those of the kinematic model reconstruction unit. It calculates the standard reconstruction trajectory based on the driving action and boundary conditions, and has higher accuracy, replacing the kinematic model reconstruction unit to implement the function of calculating the standard reconstruction trajectory.

8. The vehicle trajectory smoothing and reconstruction system based on the combination of model and data according to claim 7, characterized in that, The offline kinematic model reconstruction unit adopts a two-point reconstruction method, that is, it reconstructs based on the driving action output by the driving action encoding unit and the boundary point coordinates of the starting point and the ending point; First, the boundary point state is solved using the boundary point coordinates of the starting point and the ending point, and then the complete reconstruction trajectory is obtained by recursion. Solving the boundary point state requires solving a non-linear equation as follows: Where: v i = v i-1 + a i-1 · T Wherein, (x1, y1) are the starting point trajectory coordinates, and (x end , y end ) are the ending point trajectory coordinates; Perform variable substitution θ = arctan(Δy / Δx) Thus, the variables are separated to obtain Subsequently, it is calculated in two cases: When The non-linear equation to be solved is: while or when, the equation is: Solve after calculating v1 in two cases The above non-linear equation is solved using numerical methods; The initial state v1 is obtained by solving, After that, the initial value is used for one-way recursive trajectory reconstruction.

Images