A two-layer switching based trajectory prediction method

Safety-critical situations in road traﬃc often result from incorrect estimation of the future behavior of other road users. Therefore, many Advanced Driver Assistance Systems (ADAS) need prediction models to ensure safety. Physical prediction models offer the advantage of general use and work quite well for short prediction horizons, while for longer periods of time, maneuver-based models offer better performance which, however, strongly depends on the data used to train them. An additional challenge for prediction is the fact that the surrounding traﬃc can change its path, i.e. for safety not only one maneuver should be considered but regular updates are required. Against this background, we propose a method that uses three physics-based predictions – corresponding to different prediction assumptions and models – combined with possible maneuver-based trajectories derived from environmental knowledge. Continuous monitoring is used to select the most likely of the three physics-based models. This choice then inﬂuences the environment-based prediction and the output of both models is fused after-wards. The output of the resulting Multiple Model Trajectory Prediction (MMTP) has been validated with measured data from two different scenarios – a city junction and a highway – with a good prediction performance and without the need for special measurements as commonly required for maneuver-based prediction.


Introduction
Nowadays, the number of vehicles equipped with ADAS is steadily increasing.For these systems, it is important to have knowledge of the traffic and the road topology.In order to fully exploit the potential of most ADAS systems, a prediction of other vehicles' behavior is also required.Trajectory prediction is covered very well in the literature.Most of the available solutions focus either on short prediction horizons based on physical prediction [12][13][14][15]20] , or on longer prediction horizons using maneuver-based models [4][5][6]16,19,21] .The main problem with the current solutions is their limited scope, for example they are mostly optimized for highways.Purely physical predictions are limited to simple straight streets and a short horizon.Under these conditions, they provide quite good information about the trajectory for the near future.For intersection situations or curvy streets, these methods cannot be used for a longer prediction period, as they sometimes provide unrealistic predictions.Maneuver-based models, on the other hand, provide possible solutions for long prediction periods, but the major disadvantage is that a huge amount of measured data from many vehicles must be available for the calculations (as these methods are usually learning-based).Such data is available for highways [8] , but for smaller intersections in cities (which may have not standardized structures), the available amount of data is not rich enough to develop specific models.Therefore, the question arises whether these methods can be combined as already shown in [20] to obtain better results for short as well as for longer prediction horizons, with the additional condition of not being dependent on prerecorded maneuver data.In this paper, a trajectory prediction method is presented which combines environmental information with the physical prediction, leading to promising results in different scenarios.
More specifically, we use physics-based trajectories from different vehicle models and combine the most probable ones with a possible outlet trajectory for the given environment.This combination allows for taking advantage of the positive characteristics of each prediction method.Section 2 gives a brief overview of the approach and in Section 3 , the physical prediction, marked red in Fig. 1 , is discussed.After that, Section 4 deals with possible exit trajectories of intersections and highways, marked yellow in Fig. 1 .Section 5 explains the MMTP in more detail, marked green in Fig. 1 .Finally, the used data are described in Section 6 , leading to the validation results in Section 7 .Section 8 concludes all outcomes.

Overview
As our prediction approach consists of several interlinked stages, Fig. 1 gives an overview of the individual components.The process is described for one single vehicle during the whole paper, but can be applied to an arbitrary amount of vehicles in parallel.For all formulations, k indicates the current step in time with the time t defined as t = k • T s where T s is the sampling time ( 100 ms indicate the three possible lanes a vehicle could use.These are calculated with the help of environmental knowledge in an optimization.The number of possible trajectories varies depending on the traffic situation.Then, these trajectories are compared to the selected physical prediction, again by using the square norm.The closest trajectory is denoted as for the following step.Now, in order to calculate the final predicted trajectory , the two predictions are added weighted with a confidence function μ.

Physics-based prediction
As mentioned, physics-based motion models are required in our prediction approach.In this section, different concepts are introduced which are based on the physical equations originating from Dubins vehicle [13] .

Vehicle-models and prediction
Too simplified abstractions of Dubins vehicle like constant velocity (CV) and constant acceleration (CA) models [12] are not sufficient to reflect complex behaviors in junction situations, as at least the rotation around the z -axis needs to be considered in those cases.Therefore, the previously mentioned CTRV, CTRA and STM prediction models are considered within this work.The CTRV-model is obtained by assuming a constant velocity and turn rate applied to the temporally discretized Dubins model and is often used in tracking-systems [15] .The CTRA-model goes one step further by modeling the velocity as a depending variable of a constant acceleration.The STM includes not only the orientation, but also the steering angle, taking into account the simplified positioning of the front wheels.As visualized in Fig. 2 , the origin of the models is the continuous in time Dubins vehicle.To be usable for our method, it needs to be discretized first, this is explained in more detail in [12,15,20] .Note that for this work the inputs are not linearly separable from the state equations.Therefore, they are displayed in bold and red in the following equations.
Here x k and y k represent the Cartesian coordinates, θ k is the rotation angle of the vehicle, v k is the longitudinal velocity and ω k is the turn-rate of the vehicle.The system equations are The extension of CTRV-model is the CTRA-model, which has following state space vector The only new variable is a k which stands for the longitudinal acceleration of the vehicle.The system equations extend to The last model uses the relationship between the steerable front and the rigid rear axle.The rotation takes place around a momentary pole with the distance tan (δ k ) • D a to the center of the rear axle, where D a stands for the center distance, seen in Fig. 3 .Therefore, the state space vector results to which was extended by the steering angle δ k .The system equations are Measured data obtained by sensors never fully reflect the ground truth due to various influences [3] .It is also not possible to measure all required states, so the remaining states ( θ k , a k , δ k , ω k ) serve as a basis for the following investigations.

Model-switching
The three presented models have different advantages in different segments of a trajectory.In order to find the estimated trajectory that best describes the true one, it is necessary to evaluate the models in an adaptive way.The idea is to use the estimated states ˆ x with the three models.This enables an evaluation of the models over n ev steps, as the corresponding measured values are known.The evaluation is carried out using the quadratic norm of the error E k , see (8) .The predicted values ˆ x (•)  k are marked with a hat from here on.The vector of the trajectory T k and the vector of the prediction P k , which consist of the Cartesian coordinates are denoted as The square norm is defined as . Now, as sketched in (9) , the minimum of the vector E k is determined to find the best possible physical model for the current situation.
This model (in Fig. 4 it is for example STM) is then used to calculate a predicted trajectory which ranges n ph steps into the future.This prediction already delivers good results for a short prediction horizon.The selected physical model is used as the starting point for the multiple model trajectory prediction which will be further explained in Section 5 .

Environmental prediction
The second component of our method consists of a theoretical trajectory for leaving the situation.Concrete examples are shown in Fig. 5 in the form of an exemplary intersection and a three-lane highway.To calculate these trajectories, it is necessary to have information about the surroundings e.g.via Open Street Map [11] .A big advantage of our method is that only the output tangents of a traffic situation are required, which are easier to determine than explicit points of exit and therefore also not so prone to errors.
Starting from the point p meas k , Fig. 5 (a) shows exemplary tra- , and for leaving the intersection.that may result for the highway scenario.The calculation of these trajectories requires some effort, which is shown in the following subsection.

Calculation of the possible trajectories
As displayed in Fig. 5 , there are various options for completing the scenario based on p meas k .In the case of the intersection, it is possible to leave via the northern, eastern or southern exit.In the case of the highway, it is possible to change lanes to the left or right or to keep in the middle lane.The possible outlet trajectories are determined by an optimization.To set up the optimization task, topological information of the intersection and highway is required to determine possible output tangents.In the following explanation only intersections are discussed, because the highway can be viewed as a simplified intersection (in terms of outlet tangents).It is not reasonable to define fixed endpoints for the exit of the junction, as the optimization solution would get infeasible in case of minor deviations.Apart from that the distance from p meas k to the respective endpoints is typically not equal and therefore the number of optimization steps must also be different.The solution to solve these two problems is a coordinate transformation from x − y coordinates to η − ξ coordinates which are aligned to a respective outlet tangent as shown in (10) .The angle θ out is defined as the angle between the Y-axis and the respective outlet tangents of the intersection, as shown in Fig. 6 .ξ points in direction of the outlet tangent.This transformation simplifies the cost function, as it only has to be minimized in direction of η.
First the optimization problem shall be defined and then the individual components are discussed in more detail.
W T describes the transposed weighting vector of the individual elements and is used for fine adjustment.The smoothing J SMO (k ) and reference J REF (k ) terms consist of the following costs.
The reference costs determine the number of optimization steps, because J REF (k ) has a minimum at the exit of the intersection.There, the distance to the tangent η becomes minimal ( η → 0 ) and the vehicle enters in the direction of the tangent ( θ k − θ out → 0 ).With the smoothing costs, the vehicle-jerk is minimized which guarantees natural driving behavior.This is done by punishing the changes in speed and rotation.(11b) represents the system equations, where the STM was chosen for this task.
A in condition (11c) defines the admissible range of acceleration, which was selected with a min = −5 m / s 2 and a max = 2 m / s 2 , similar to [1] .V describes the admissible range of velocity, which is selected to [0 , 2 • v 0 ] .Condition (11d) expresses that δ must be an element of the admissible range of steering angle, which is defined as follows.T and speed v k it is no longer possible to leave the intersection on the exit to the south, therefore this option can be removed.
The maximum steering angle δ max (v k ) at a point k is calculated as a function of the current speed v k , as formulated in (16) .Using this procedure, a physically possible trajectory can be calculated in which the vehicle does not lose traction.
The maximum lateral acceleration a max lat was selected from Manfred Mitschke [10] as 4 m / s 2 .The relationship in ( 16) can be explained with the help of Kamm's circle, see [10] .This method delivers good results at a macroscopic view.The biggest advantage, however, is that impossible (infeasible) trajectories are recognized and can therefore be excluded as shown in Fig. 7 .
The result of this presented method are several possible trajectories, as for example in Fig. 7 .All optimization results within this work were obtained using CasADi [2] with the nonlinear solver IPOPT [18] .

Trajectory selection
After calculating the set of topologically possible solutions, the best one is selected with the help of from Section 3 .The criterion for that choice is the distance to the physical prediction which shall be minimal as it is assumed that the physical trajectory approximately describes the direction of the true trajectory and so the correct exit of the intersection can be recognized.As in (8) , the square norm is used to calculate the trajectory distance.The south-trajectory is not valid as also shown in Fig. 8 and the west-trajectory could be excluded in advance due to the assumption that the vehicle does not turn at a city intersection.
In ( 17) n ph reflects the prediction horizon, which corresponds to the length of the prediction trajectory.The physical prediction is defined as . The result of the possible output trajectory selection is the trajectory with smallest distance to the physical prediction , as also shown in Fig. 1 .The background of the selection procedure is exemplarily displayed in Figs. 8 and 9 for the intersection scenario Car 11.

Multiple model trajectory prediction
The task is now to combine the advantages of the two selected trajectories (physical and environmental) from the previous Sections 3 and 4 .A trust function μ is introduced for this purpose,  which reflects the respective trust over the prediction horizon.If two trajectories shall be combined, both have to be weighted with their corresponding trust function in order to obtain a physically possible solution, assuming that Here, μ PHY (k ) and μ ENV (k ) reflect the trust in the physical and environmentally possible solution from Sections 3 and 4 .As visualized in Fig. 10 , the function was selected to represent a sigmoidal shape, similar to the approach in [20] .
The trust in the physics-based predicted trajectory decreases sharply with increasing prediction horizon and runs into 0 from k ≈ 80 while the trust in the environmentally possible outlet trajectory increases with increasing k .
The parameters a and c are used to fine-tune the confidence range.The parameter a influences the slope of the sigmoid function while c specifies the shift of the function.The selected parameter values are listed in Table 1 .
It is important to note that this parameter setting is used both for the intersection as well as the highway scenario, since generic applicability is to be guaranteed.Both parameters were tuned manually based on the recorded data.Now, it is possible to add two trajectories to obtain a combined solution The vector P k consists of the coordinates x k and y k and reflects the result of the predicted trajectory.Overview of the junction [11] where the data were recorded.

Data collection
To validate the approach, two measurement data sets were used.The junction data set was recorded at the uncontrolled junction 'Freistädter Straße -Hauptstraße' in Linz, Austria ( Fig. 11 , 48 • 18 56 . 2 N 14 • 16 49 .7 E).The highway data set was recorded during test drives on highways between Austria and Italy.

Evaluation and results
The developed prediction model is now evaluated with the data sets introduced in Section 6 .An exemplary trajectory of the intersection data set is displayed in Fig. 13 .For this trajectory (at time step k ), the CTRV model showed the smallest deviation from the measured values in the evaluation period and was chosen as the physical trajectory . The prediction trajectory is determined with the help of the most probable outlet trajectory . Fig. 15 shows the exemplary application of the prediction algorithm on the highway data.In this case (at time step k ), the STM is selected as the best physical model for the prediction which is combined with the most probable outlet tra- . In both cases, the MMTP trajectory is superior compared to the trajectories resulting from the single methods.Additional results are shown in Figs. 14 and 16 .
The single prediction methods are now compared based on the validation error  with the exception that for the study of the performance in highway cases, only the validation error in Y-direction is evaluated (dropping the first term in ( 24) ).This appears more sensible for these cases as the longitudinal displacement (especially for the physics-based prediction) would be too dominant otherwise.Fig. 17 shows the validation error in Y-direction for the situation from Fig. 15 (Car 9).As clearly visible, the physical prediction is better for a short prediction horizon and the possible outlet trajectory provides better results for long prediction horizons.
The MMTP approach outperforms the other two methods by taking benefit of the their respective advantages.Table 2 lists the performance of the single prediction methods based on the respective maximum of the validation error and further confirms this observation.
To move away from exemplary results to a general validation of our prediction method, all available trajectories in the recorded data sets were analyzed.In Fig. 18       was assumed to be straight, leading to a bias during validation.
Comparable results for highways can be found in [6] and also [16] , where a data-based approach was used.A comparison for junction scenarios is not trivial due to general variations in geometry and also error metrics, see [7,9,17] .

Conclusions
We presented an approach that serves as a precise prediction method for multiple scenarios, exemplary applied on intersections and highways.The simple structure as well as the independence of a data base for learning maneuvers results in a wide range of possible applications and the accurate prediction performance over a long horizon enables a robust reaction on possibly hazardous situations for arbitrary ADAS systems.The step-wise framework opens up the opportunity to further expand the approach, e.g. by adding more complex physical prediction models in the first stage.
In future works it is planned to merge this prediction approach with other ADAS systems to further evaluate the performance of the method.

Declaration of Competing Interest
The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.

,Fig. 2 .
Fig. 2. Representation of the relationship between the vehicle models.

Fig. 5 (
Fig. 5 (b) shows exemplary trajectories T left k + n ph k +1 , T center k + n ph k +1 and T right + n ph k +1

Fig. 7 .
Fig. 7. Exclusion of an infeasible trajectory.The picture shows a scenario where based on the position p meas k = [ x k , y k ]

Fig. 8 .
Fig. 8. Cost vector for the intersection scenario Car 11 with two possible solutions, as the south-trajectory yields no valid result.

Fig. 9 .
Fig. 9.Possible exit trajectories to leave the intersection for the intersection scenario Car 11.In this case only two possibilities (north-and east-trajectory) exist.The south-trajectory is not valid as also shown in Fig.8and the west-trajectory could be excluded in advance due to the assumption that the vehicle does not turn at a city intersection.

Fig. 10 .
Fig. 10.Trust of the respective trajectories P PHY and P ENV for the addition in the MMTP.

Fig. 11 .
Fig. 11.Overview of the junction[11] where the data were recorded.
the blue bars describe the 25% and 75% quantiles while their baseline represents the median of the error E V, (•) k (24) for each step k over all data sets.The orange bars indicate the maximum error for the respective prediction

Fig. 18 .
Fig. 18.Error plots of several trajectories of the intersection (upper subplot) and highway data set (lower subplot).

Table 1
Parameters of the selected sigmoid function.
ph / 3 .82 Shift in the sigmoid function

Table 2
Resulting performance.
step.It turns out that the model provides very good results over a long prediction horizon and keeps the maximum error low.The prediction period is limited by the measurement data, because lane changes on the highway as well as crossing an intersection do not require a longer prediction.The errors at the end of the prediction can be traced back to measurement errors, as the highway