# Optimization of Vehicular Trajectories under Gaussian Noise Disturbances

^{*}

## Abstract

**:**

## 1. Introduction and Motivation

_{f}. Additionally, we minimize some other mobility parameters like the lateral speed at the end of the path (to reduce the impact of sudden inertial changes). Finally, we carry out a study on how diversion from the optimum trajectory due to Gaussian noise in measurement sensors can be minimized thanks to the well-known the Kalman Filter technique [5]. Since this mathematical optimization framework will be extended for cooperative collision avoidance (CCA) in a future work, we also discuss how it can be implemented with our proposal. That is, we show here the results of a first step within our general goal, focusing on different functionals and providing useful insight on the resulting trajectories that can be expected from them, as well as a simple analysis on how noise disturbances can affect trajectories and possible solutions to this undesirable effect. In summary, the main contributions of this paper are:

- A discussion on the different ways to compute optimized real-time maneuvers for a high-speed moving vehicle subject to timing constraints (the maneuver must be performed in a maximum time interval of t
_{f}). - The evaluation of functionals including the minimization of the final lateral speed. By keeping the final lateral speed (at t
_{f}) as low as possible, the possibility of continuing in the optimum lateral position is also maximized. - A preliminary discussion on the accuracy of the computed trajectories by an evaluation of the discretization factor N (number of stages in which the trajectory is divided into).
- An analysis on how trajectories could be affected by random Gaussian noise, and the application of Kalman Filter theory to minimize the impact of unwanted deviations from the optimum path.

## 2. Related Work

## 3. Problem Statement and Results

#### 3.1. Scenario Description and Formulation

_{f}seconds after the maneuver has begun (see Figure 1). During this interval, the vehicle will try to adjust its mobility evolution according to a set of constraints and optimizing a given functional. In this particular case, our goal would be to achieve a given degree of safety while keeping the maneuver as comfortable as possible. To combine these two features, we look for the maximization of the lateral distance with respect to the lateral boundaries of the road, while minimizing the final lateral speed at the end of the path, all within a given time interval of t

_{f}seconds. Maximizing the lateral distance implies in this case leaving the maximum possible road width on both sides of the vehicle, thus minimizing the probability of colliding with the lateral protections. On the other hand, by simultaneously minimizing the lateral speed at t

_{f}, we can change the general appearance of the trajectory in such a way that the inertial impact on the mobility of the passengers is reduced when reaching the optimum lateral position.

_{1}(t) refers to the position of vehicle 1 in axis x at time t; v

_{1}(t) denotes the current speed of vehicle 1 in axis x as a function of t; and a

_{1}(t) defines the lateral acceleration of vehicle 1 along time. We take a

_{1}(t) as the control variable that we have to manipulate in real time in order to modify the trajectory traced by the vehicle under consideration. As was specified before, we assume that these mobility equations only govern the lateral displacement of the vehicle (we consider the longitudinal speed to be constant and fixed to 120 km/h during the whole trajectory).

**Lateral acceleration restrictions.**The absolute value of the lateral acceleration cannot take a value higher than the limit c(vi) m/s^{2}, where vi is the constant longitudinal speed of the vehicle and c(·) is a function of the longitudinal speed.**Lateral position restrictions.**The vehicle can only have a lateral displacement inside the width limits of the road.

**Final lateral distance maximization and final lateral speed minimization**. In this case we want to minimize the final variance of the lateral distances left by the vehicle after t_{f}, while minimizing the lateral speed at the end of the trajectory. The equation corresponding to this functional takes the form:**Final lateral distance maximization.**In this case we skip the minimization of the lateral speed at the end of the trajectory. We only perform here the maximization of the final lateral distance.**Instantaneous lateral distance maximization and final lateral speed minimization.**This functional aims at maximizing the instantaneous lateral distance while minimizing the lateral speed at the end of the trajectory.**Instantaneous lateral distance maximization.**In this case, we skip the minimization of the lateral speed at the end of the trajectory, but we maximize the instantaneous lateral distance (during the maneuver).

#### 3.2. Final Lateral Distance Maximization

_{D1}and J

_{D2}for the configuration of parameters in Table 1, where the instantaneous lateral distance of the vehicle along the trajectory is not maximized, but the final (at t

_{f}) lateral distance to the lateral protections is maximized. If we obtain the mobility evolution (acceleration, speed and position evolution along the trajectory) for two different values of t

_{f}(10 and 2 seconds), we can notice from Figure 2 and Figure 3 that for longer time intervals to execute the maneuver, the vehicle does not need to reach the maximum acceleration stated by the model’s restrictions. On the other hand, for lower values of t

_{f}, the steering maneuver needs to use the maximum allowable values of the lateral acceleration to maximize the functional at the end of the trajectory. The most illustrative differences between both functionals can be read from Figure 2 and Figure 3, where we see that J

_{D1}(blue) reaches a higher lateral peak speed than J

_{D2}just at the middle of the time period. The explanation for this fact is that by flipping between two opposite values of the acceleration (see Figure 2 and Figure 3) until the final position is reached, the vehicle can get there with null lateral speed. However, for J

_{D2}, the speed will increase more smoothly from the first instant, but at the cost of not having null lateral speed at the end of the trajectory, which could lead to a further potential risk because of the inherent inertial dynamics.

Evaluation parameter | Meaning | Value |
---|---|---|

N | Discretization factor | 20 |

X_{0 } | Initial lateral position | 1 m |

V_{0 } | Initial lateral speed | 0 m/s |

a_{0 } | Initial lateral acceleration | 0 m/s_{2 } |

W | Road width | 20 m |

v_{i } | Longitudinal speed | 120 km/h |

c(v_{i}) | Maximum absolute lateral acceleration | 3 m/s_{2 } |

_{f}). Analyzing the results in these figures, we can conclude that functional J

_{D1}can reach the optimum lateral position at a later time but with a null lateral speed, whereas through functional J

_{D2}it is possible to reach the optimum lateral position earlier, but at the expense of having a non-zero speed. The first one will not have problems to follow the trajectory, but for the second one, safety relies on how will the inertial mobility be from the instant t

_{f}onwards.

#### 3.3. Instantaneous Lateral Distance Maximization

_{D3}and J

_{D4}). As we can infer from Figure 6 and Figure 7, in this case the main objective consists of updating the lateral position as soon as possible in order to meet the requirements set by the functionals.

_{D1}and J

_{D2}there could effectively be lateral collisions, but they are less prone to disturbances, because lateral distance is maximized regarding only the final lateral positions.

_{D3}and J

_{D4}, we can see that for longer time intervals (Figure 8) until reaching the final position, a vehicle focus on achieving the final lateral position very quickly (thus the rapid changes in acceleration, which oscillate between the two extreme values although there is enough time to make a smoother maneuver). Besides, it is surprising that for both functionals the final lateral speed reached at the end of the path is null, and both functionals provide very similar behaviors. On the other hand, if we analyze Figure 9, we see that for very short time intervals, the evolution of mobility is very similar to what we saw for the functionals J

_{D1}and J

_{D2}.

#### 3.4. Discretization Influence

_{N}(t), in comparison with the N

_{ref}= 100 reference trajectory x

_{Nref}(t).

_{D1}needs a higher value for the discretization factor than that of J

_{D2}to achieve the same level of accuracy. Due to the need of higher accelerations for the trajectories obtained by J

_{D1}in comparison with those calculated by J

_{D2}, a higher value for the discretization factor N is thus required to keep an acceptable accuracy. On the other hand, both functionals regarding instantaneous lateral distance maximization (J

_{D3}and J

_{D4}) show that they need a higher value of the discretization factor to reach the same level of accuracy. This is explained by the same reasons as regards the comparison between J

_{D1}and J

_{D2}, that is, the greater acceleration values of the trajectories, the higher N must be to maintain accuracy.

#### 3.5. Kalman Filter for Trajectory Smoothing

- The shape of the traced path due to possible deviations from the optimum course, see Subsection 3.5.1.
- The sensors’ measurement on the position and speed at a fixed time t, see Subsection 3.5.2.

#### 3.5.1. State Variability

**F**defines the state transition model applied to the previous state x (k − 1);

**B**denotes the control-input applied to the controls

**u**(k);

**w**(k) is the process noise, which is assumed to be drawn from a zero mean multivariate normal distribution with covariance

**Q**(k):

#### 3.5.2. Measurements Variability

**H**is the observation model that maps the true state space onto the observed space; and v (k) is the observation noise, which is assumed to be zero mean Gaussian white noise with covariance

**R**(k):

**w**(k) (Gaussian noise process of mean 0 and covariance

**Q**(k), Equation (10)). Furthermore, we consider that there exists some sort of measurement sensing error, represented by variable

**v**(k) (Gaussian noise process of mean 0 and covariance

**R**(k), Equation (12)).

**F**,

**B**and

**H**matrices by extracting the correspondence of both the controls and the state variables with Equations (10) and (12). After some easy operations we get:

_{z}

^{2 }and the state dispersion variance σ

_{x}

^{2 }affect the evolution of trajectories. With the Kalman Filter, it is possible to minimize the counterproductive effect of both noise processes by using two trajectory regeneration procedures enumerated next:

**Filtering.**By means of this process, the Kalman Filter predicts the new values x (k) of the states taking into account the states’ history until the instant (k − 1). (Although the term “Kalman Filter” regards all techniques to reduce the influence of noise on the states of a dynamical system, we must not get confused with Filtering, which, as well as Smoothing, describes a specific procedure to fix or reduce trajectory dispersion caused by any noise process.)**Smoothing.**In this second process, the Kalman Filter estimates the new values x (k) of the states taking into account, apart from the states’ history until the instant (k − 1), the current measurement of the states: z (k).

_{f}= 10 s to reach the optimum lateral position) is affected by the presence of additive Gaussian noise in the states and in the sensor measurements. We will establish a state dispersion variance σ

_{x}

^{2 }= 0.1 for all the examples and will focus only on the influence of noise produced by measurement sensors (measurement variance σ

_{z}

^{2}). We will include a comparison between the True Trajectory (optimum determined trajectory), the measured trajectory (sensed trajectory while executing the maneuver), and the filtered and smoothed trajectory for the two proposed performance measures J

_{D1}and J

_{D2}, and for a set of values of the measurement variance σ

_{z}

^{2}= {0.1, 1, 5}. If we now draw our attention to Figure 11, we can fairly understand how measurement noise affects the shape of trajectories by producing a dispersion of the optimum path due to the sensed positions along the course (mild dispersion for σ

_{z}

^{2}= 0.1, medium dispersion for σ

_{z}

^{2}= 1 and high dispersion for σ

_{z}

^{2}= 5). We can notice that both Filtering and Smoothing reduce the dispersion effect, but Smoothing reduces the still significant variability that the filtered trajectory obtains, by using the measurement of the present instant z (k). Differences between J

_{D1}and J

_{D2}, if present, are not clear in Figure 11.

**Figure 11.**Kalman filter effect on trajectories for J

_{D1}and J

_{D2}under measurement noise (σ

_{z}= {0.1, 1, 5}).

_{z}

^{2 }∈ [0,5], for both J

_{D1}and J

_{D2}. Due to the variability to which the MSE is subject, we have used regression techniques to represent the Degree-2 polynomial, which averages the MSE evolution during the evaluated interval. This will make the visualization of graphs easier for comparison purposes.

_{f}in order to quantify how far the trajectory ends from the desired position. The associated graph will represent this magnitude for the same interval σ

_{z}

^{2 }∈ [0,5] as in the last case, and will show the percentage, with respect to the total width of the road W , of distance far from the optimum lateral position.

_{D1}.

**Figure 12.**Degree-2 polynomials for regression of MSE under measurement noise (σ

_{z}∈ {0.1, 1, 5}), for J

_{D1}.

**Figure 13.**Percentage of distance with respect to the lateral optimum position under measurement noise (σ

_{z}∈ {0.1, 1, 5}), for J

_{D1}.

_{z}∼ 0.1, sensing noise does not affect the trajectory remarkably, since the measured, filtered and smoothed paths obtain a very low MSE with respect to the optimum trajectory. As σ

_{z}increases, it is easily noticeable how necessary it is to use at least Filtering and, if possible, Smoothing in order to correct the path dispersion introduced by noise. Surprisingly, the LDP shows very similar results for both Filtering and Smoothing, since both tend to reach the optimum final position regardless of the evolution of the trajectory. Analyzing now Figure 14 and Figure 15, we can come to the same conclusions as for the J

_{D1}functional. More importantly, we can see that differences between using Filtering and Smoothing for J

_{D1}and J

_{D2}are not as remarkable. This implies that the shape of the traced trajectory does not influence the performance of the Kalman filter.

**Figure 14.**Degree-2 polynomials for regression of MSE under measurement noise (σ

_{z}∈ {0.1, 1, 5}), for J

_{D2}.

**Figure 15.**Percentage of distance with respect to the lateral optimum position under measurement noise (σ

_{z}∈ {0.1, 1, 5}), for J

_{D2}.

#### 3.6. Connection to Cooperative Collision Avoidance (CCA)

## 4. Conclusions

_{f}. We have proposed and evaluated four different functionals and discussed their advantages and drawbacks. Functionals J

_{D1}and J

_{D2}provide a better stability during the trajectory because they only update the mobility parameters according to the final lateral position. J

_{D3}and J

_{D4}, however, are based on updating the lateral position in terms of the instantaneous distance, which requires faster changes in the mobility evolution. On the other hand, whereas J

_{D1}is really useful for higher values of t

_{f}(since apart from reaching the optimum position it arrives with null lateral speed), J

_{D2}reaches the last position with a speed always higher than zero. However, for low distances until the optimum position (i.e. low values of t

_{f}), J

_{D2}can get to this position earlier, while the other functional focuses its attention on reaching the last position at zero speed, not caring as much about how far is the vehicle from the optimum position. From these results, we can deduce that in general optimizing in terms of the final lateral distance can be better to avoid very rapid changes in acceleration, which could imply the need to have higher values for the discretization factor N (at the expense of more computation overhead). However, we have not explored the great multiplicity of scenarios that appears from this premise, therefore using functionals J

_{D3}and J

_{D4}(or a derivation of them) might be more convenient. In a future work, we will investigate a wider range of scenarios to completely characterize the performance of the four functionals (and possible extensions).

## Acknowledgments

## References

- National Spanish Traffic Administration. Annual Report of Accidents; DGT (Direccion General de Trafico), Ministry of inner affairs, Government of Spain: Madrid, Spain, 2010. Available online: http://www.dgt.es/portal/es/seguridad_vial/estadistica/publicaciones/anuario_estadistico/ (accessed on 19 December 2012).
- Ili, S.; Albers, A.; Miller, S. Open innovation in the automotive industry. R&D Manag.
**2010**, 40, 246–255. [Google Scholar] - IEEE (Institute of Electrical and Electronics Engineers), IEEE Draft Standard for Wireless Access in Vehicular Environments (WAVE)-Networking Services; 1609.1-4; IEEE: New York, NY, USA, 2010.
- Dingle, P.; Guzzella, L. Optimal emergency maneuvers on highways for passenger vehicles with two-and four-wheel active steering. In Proceedings of American Control Conference (ACC) Woburn, MA, USA, 30 June 2010-July 2010; pp. 5374–5381.
- Kalman, R.E. A new approach to linear filtering and prediction problems. J. Basic Eng.
**1960**, 82, 35–45. [Google Scholar] [CrossRef] - Venkatraman, A.; Bhat, S.P. Optimal planar turns under acceleration constraints. In Proceedings of 45th IEEE Conference on Decision and Control, San Diego, CA, USA, 13–15 December 2006; pp. 235–240.
- Anisi, D.A.; Hamberg, J.; Hu, X. Nearly time optimal paths for a ground vehicle. J. Control Theory Appl.
**2003**, 1, 2–8. [Google Scholar] [CrossRef] - Tjahjana, H.; Pranoto, I.; Muhammad, H.; Naiborhu, J. The numerical control design for a pair of Dubin’s vehicles. In Proceedings of the International Conference on Intelligent Unmanned System (ICIUS 2007), Bali, Indonesia, 24–25 October 2007.
- Naranjo, J.E.; Gonzalez, C.; Garcia, R.; de Pedro, T.; Haber, R.E. Power-steering control architecture for automatic driving. IEEE Trans. Intell. Transp. Syst.
**2005**, 6, 406–415. [Google Scholar] - Naranjo, J.E.; Gonzalez, C.; de Pedro, T.; Garcia, R.; Alonso, J.; Sotelo, M.A.; Fernandez, D. AUTOPIA architecture for automatic driving and maneuvering. In Proceedings of Intelligent Transportation Systems Conference, Madrid, Spain, 17–20 Septemper 2006; pp. 1220–1225.
- Kirk, D.E. Optimal Control Theory: An Introduction; Prentice-Hall: London, UK, 1971. [Google Scholar]
- Stratonovich, R.L. Topics in the Theory of Random Noise; Gordon and Breach: New York, NY, USA, 1963. [Google Scholar]
- Thrun, S.; Burgard, W.; Fox, D. Probabilistic Robotics; MIT Press: Cambridge, MA, USA, 2005. [Google Scholar]
- Eichler, S. Performance evaluation of the IEEE 802.11p WAVE communication standard. In Proceedings of 2007 IEEE 66th Vehicular Technology Conference (VTC-2007 Fall), Munich, Germany, 30 September-3 October 2007; pp. 2199–2203.
- Vinel, A. 3GPP LTE versus IEEE 802.11p/WAVE: Which technology is able to support cooperative vehicular safety applications? IEEE Wirel. Commun. Lett.
**2012**, 1, 125–128. [Google Scholar]

© 2013 by the authors; licensee MDPI, Basel, Switzerland. This article is an open-access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

## Share and Cite

**MDPI and ACS Style**

Tomas-Gabarron, J.-B.; Egea-Lopez, E.; Garcia-Haro, J.
Optimization of Vehicular Trajectories under Gaussian Noise Disturbances. *Future Internet* **2013**, *5*, 1-20.
https://doi.org/10.3390/fi5010001

**AMA Style**

Tomas-Gabarron J-B, Egea-Lopez E, Garcia-Haro J.
Optimization of Vehicular Trajectories under Gaussian Noise Disturbances. *Future Internet*. 2013; 5(1):1-20.
https://doi.org/10.3390/fi5010001

**Chicago/Turabian Style**

Tomas-Gabarron, Juan-Bautista, Esteban Egea-Lopez, and Joan Garcia-Haro.
2013. "Optimization of Vehicular Trajectories under Gaussian Noise Disturbances" *Future Internet* 5, no. 1: 1-20.
https://doi.org/10.3390/fi5010001