**4.1 Stochastic control formulation**

In this approach, the ego vehicle (Car 1, from **Figure 3**) has to first decide if it is feasible to overtake the HDV ahead (Car 2) once it gets "close enough". In this simplified formulation, it is assumed that the Car 1 is able to measure its own relative velocity with respect to the HDVs with some additive noise. If the decision to overtake is made, then the AV has to generate the exact trajectory it will take to perform the overtake maneuver. An overview of these steps is outlined henceforth.

*y*3ð Þ¼ *k* 3*d=*2 (42) *θ*3ð Þ¼ *k* 0 (43)

*z*1ð Þ¼ *k x*2ð Þ� *k x*1ð Þþ *k n*1ð Þ*k* (45)

*z*1ð Þ¼ *k x*3ð Þ� *k x*1ð Þþ *k n*2ð Þ*k* (46)

*z*1ð Þ¼ *k v*2ð Þ� *k v*1ð Þþ *k n*3ð Þ*k* (47)

*z*1ð Þ¼ *k v*3ð Þ� *k v*1ð Þþ *k n*4ð Þ*k* (48)

*<sup>n</sup>*¼<sup>0</sup> (44)

*<sup>i</sup>*ð Þ*k* . There

To keep the problem as general as possible, it is assumed that at time step k, the AV has all the history of its past states, linear velocity and its relative position and velocity w.r.t the other two cars with some additive white Gaussian noise

Ið Þ¼ *<sup>k</sup> <sup>x</sup>*1ð Þ *<sup>n</sup>* , *<sup>y</sup>*1ð Þ *<sup>n</sup>* , *<sup>θ</sup>*1ð Þ *<sup>n</sup>* , *<sup>v</sup>*1ð Þ *<sup>n</sup>* , *<sup>z</sup>*1ð Þ *<sup>n</sup>* , *<sup>z</sup>*2ð Þ *<sup>n</sup>* , *<sup>z</sup>*3ð Þ *<sup>n</sup>* , *<sup>z</sup>*4ð Þ *<sup>n</sup> <sup>n</sup>*¼*<sup>k</sup>*

*Models and Methods for Intelligent Highway Routing of Human-Driven and Connected…*

Here, *ni*(k) are white Gaussian Processes with mean 0 and variances *σ*<sup>2</sup>

initial longitudinal coordinates of the three cars i.e. *x*1ð Þ 0 , *x*2ð Þ 0 and *x*3ð Þ 0 .

is an assumption placed on the independence of f g *ni*ð Þ*k <sup>k</sup>*≥<sup>1</sup> from the distribution of

Upon consideration of the control problem that the AV has to solve, it becomes apparent rather quickly that the AV simply doesn't have to decide on linear and angular velocities. If that were the case, then the Car 1 could simply wait till Car 3 passes and then overtake Car 2 by having a velocity greater than Car 2. That is not an optimal solution for all possible scenarios. Therefore,the first thing that the AV needs to do is to get better estimates of position and velocity of Cars 2 and 3 rather than using the raw noisy data. With the better estimates of positions and velocities of the HDVs, the AV can perform a feasibility analysis to see if it is feasible to overtake the Car 2 or not. If the Car 1 deems the overtake maneuver to be infeasible, it can resort to the waiting strategy. If, however, Car 1 decides to overtake, then it has to decide on the time to start overtaking. Once the time to start overtaking is finalized, then the AV has to plan a trajectory that it will take to perform the maneuver such that it will not violate any safety margins. Finally, it needs to generate control commands i.e. linear and angular velocities to execute the overtaking maneuver. One typical problem that could arise is that the vehicle may have to return to its original lane, after starting the overtaking maneuver, but this

A sample trajectory with a constant linear velocity for the AV is displayed in **Figure 4**. Between *k* ¼ 0 and *k* ¼ *T*1, Car 1 is approaching Car 2. At *k* ¼ *T*1, Car 1 decides to run the feasibility analysis and decides to overtake Car 2. Between *k* ¼ *T*1 and *k* ¼ *T*2, Car 1 has a positive constant angular velocity resulting in motion towards the adjacent lane. At *k* ¼ *T*2, Car 1 has reached the divider between the lanes so it switches to a negative angular acceleration having the same magnitude as

(AWGN). Therefore,

*DOI: http://dx.doi.org/10.5772/intechopen.94332*

*4.1.2 Control problem*

*4.1.3 Sample trajectory*

**137**

scenario is beyond the discussion of this chapter.

where

In the formulation below, it is assumed that the width of each of the lanes is defined to be a constant value *d* and the minimum safety distance between cars is *L*. A collision is defined in terms of AV violating the minimum safety distance threshold with respect to the centroids of the cars. The positive velocities are defined towards the right in **Figure 3** and positive *θ* is defined counterclockwise relative to the velocity vector of the car. Finally, the sets of admissible linear and angular speeds for the cars are considered to be finite.

## *4.1.1 Modelling*

The states of Car i are *xi*, *yi* and *θ<sup>i</sup>* which respectively correspond to the longitudinal coordinate, lateral coordinate and orientation of Car i. The inputs to Car i are *vi* and *wi*, the linear and angular velocities respectively of Car i respectively. An index *k* is used to denote the time step.

Considering the initial states of the vehicles, it is assumed that initially at *k* ¼ 0, Car 1's longitudinal coordinate is a random variable distributed normally with *μ* ¼ 0 and *σ* ¼ Σ<sup>1</sup> while the lateral coordinate is fixed at the center of the bottom lane i.e. *y*1ð Þ¼ 0 *d=*2 and facing forward i.e. *θ* ¼ 0. As for Car 2, it is assumed that initially at *k* ¼ 0, *x*<sup>2</sup> is distributed normally with *μ*<sup>2</sup> ¼ *x*~<sup>2</sup> and *σ* ¼ Σ<sup>2</sup> where Σ<sup>2</sup> is s.t. *x*2ð Þ 0 >*x*1ð Þ 0 while the lateral coordinate is at *y*2ð Þ¼ 0 *d=*2 and facing forward i.e. *θ* ¼ 0, identical to Car 1. Finally, for Car 3, it is assumed that initially at *k* ¼ 0, *x*<sup>3</sup> is distributed normally with *μ*<sup>3</sup> ¼ *x*~<sup>3</sup> and *σ* ¼ Σ<sup>3</sup> where Σ<sup>3</sup> is s.t. *x*3ð Þ 0 >*x*2ð Þ 0 while the lateral coordinate is fixed at the center of the top lane i.e. *y*3ð Þ¼ 0 3*d=*2 and facing reverse (since *v*<sup>3</sup> < 0) i.e. *θ* ¼ 0.

As for the dynamics of the vehicles, it is assumed that cars 2 and 3 keep travelling along the same lane i.e. *ω*<sup>2</sup> and *ω*3, the angular velocities of cars 2 and 3 respectively, are identically zero for all time steps *k*≥0.

Based on the assumptions above, the dynamics for the cars are defined by the equations below:

$$\varkappa\_1(k+1) = \varkappa\_1(k) + \nu\_1(k)\cos\left(\theta\_1(k)\right) \tag{35}$$

$$y\_1(k+1) = y\_1(k) + v\_1(k)\sin\left(\theta\_1(k)\right) \tag{36}$$

$$
\theta\_1(k+1) = \theta\_1(k) + o(k) \tag{37}
$$

$$
\varkappa\_2(k+1) = \varkappa\_2(k) + \nu\_2(k) \tag{38}
$$

$$y\_2(k) = d/2\tag{39}$$

$$\theta\_2(k) = \mathbf{0} \tag{40}$$

$$
\pi\_3(k+1) = \pi\_3(k) + \upsilon\_3(k)\tag{41}
$$

*Models and Methods for Intelligent Highway Routing of Human-Driven and Connected… DOI: http://dx.doi.org/10.5772/intechopen.94332*

$$
\mathcal{Y}\_3(k) = \mathbf{3}d/2\tag{42}
$$

$$\theta\_3(k) = \mathbf{0} \tag{43}$$

To keep the problem as general as possible, it is assumed that at time step k, the AV has all the history of its past states, linear velocity and its relative position and velocity w.r.t the other two cars with some additive white Gaussian noise (AWGN). Therefore,

$$\mathcal{L}(k) = \left\{ x\_1(n), y\_1(n), \theta\_1(n), v\_1(n), z\_1(n), z\_2(n), z\_3(n), z\_4(n) \right\}\_{n=0}^{n=k} \tag{44}$$

where

**4.1 Stochastic control formulation**

speeds for the cars are considered to be finite.

reverse (since *v*<sup>3</sup> < 0) i.e. *θ* ¼ 0.

equations below:

**136**

henceforth.

*4.1.1 Modelling*

In this approach, the ego vehicle (Car 1, from **Figure 3**) has to first decide if it is feasible to overtake the HDV ahead (Car 2) once it gets "close enough". In this simplified formulation, it is assumed that the Car 1 is able to measure its own relative velocity with respect to the HDVs with some additive noise. If the decision to overtake is made, then the AV has to generate the exact trajectory it will take to perform the overtake maneuver. An overview of these steps is outlined

*Models and Technologies for Smart, Sustainable and Safe Transportation Systems*

In the formulation below, it is assumed that the width of each of the lanes is defined to be a constant value *d* and the minimum safety distance between cars is *L*. A collision is defined in terms of AV violating the minimum safety distance threshold with respect to the centroids of the cars. The positive velocities are defined towards the right in **Figure 3** and positive *θ* is defined counterclockwise relative to the velocity vector of the car. Finally, the sets of admissible linear and angular

The states of Car i are *xi*, *yi* and *θ<sup>i</sup>* which respectively correspond to the longitudinal coordinate, lateral coordinate and orientation of Car i. The inputs to

Considering the initial states of the vehicles, it is assumed that initially at *k* ¼ 0, Car 1's longitudinal coordinate is a random variable distributed normally with *μ* ¼ 0 and *σ* ¼ Σ<sup>1</sup> while the lateral coordinate is fixed at the center of the bottom lane i.e. *y*1ð Þ¼ 0 *d=*2 and facing forward i.e. *θ* ¼ 0. As for Car 2, it is assumed that initially at

Car i are *vi* and *wi*, the linear and angular velocities respectively of Car i

*k* ¼ 0, *x*<sup>2</sup> is distributed normally with *μ*<sup>2</sup> ¼ *x*~<sup>2</sup> and *σ* ¼ Σ<sup>2</sup> where Σ<sup>2</sup> is s.t.

*x*2ð Þ 0 >*x*1ð Þ 0 while the lateral coordinate is at *y*2ð Þ¼ 0 *d=*2 and facing forward i.e. *θ* ¼ 0, identical to Car 1. Finally, for Car 3, it is assumed that initially at *k* ¼ 0, *x*<sup>3</sup> is distributed normally with *μ*<sup>3</sup> ¼ *x*~<sup>3</sup> and *σ* ¼ Σ<sup>3</sup> where Σ<sup>3</sup> is s.t. *x*3ð Þ 0 >*x*2ð Þ 0 while the lateral coordinate is fixed at the center of the top lane i.e. *y*3ð Þ¼ 0 3*d=*2 and facing

As for the dynamics of the vehicles, it is assumed that cars 2 and 3 keep travel-

Based on the assumptions above, the dynamics for the cars are defined by the

*x*1ð Þ¼ *k* þ 1 *x*1ð Þþ *k v*1ð Þ*k* cosð Þ *θ*1ð Þ*k* (35)

*y*1ð Þ¼ *k* þ 1 *y*1ð Þþ *k v*1ð Þ*k* sin ð Þ *θ*1ð Þ*k* (36)

*θ*1ð Þ¼ *k* þ 1 *θ*1ð Þþ *k ω*ð Þ*k* (37)

*x*2ð Þ¼ *k* þ 1 *x*2ð Þþ *k v*2ð Þ*k* (38)

*x*3ð Þ¼ *k* þ 1 *x*3ð Þþ *k v*3ð Þ*k* (41)

*y*2ð Þ¼ *k d=*2 (39)

*θ*2ð Þ¼ *k* 0 (40)

ling along the same lane i.e. *ω*<sup>2</sup> and *ω*3, the angular velocities of cars 2 and 3

respectively. An index *k* is used to denote the time step.

respectively, are identically zero for all time steps *k*≥0.

$$
\pi\_1(k) = \pi\_2(k) - \pi\_1(k) + n\_1(k) \tag{45}
$$

$$x\_1(k) = x\_3(k) - x\_1(k) + n\_2(k)\tag{46}$$

$$
\sigma\_1(k) = \nu\_2(k) - \nu\_1(k) + \nu\_3(k) \tag{47}
$$

$$z\_1(k) = \upsilon\_3(k) - \upsilon\_1(k) + n\_4(k)\tag{48}$$

Here, *ni*(k) are white Gaussian Processes with mean 0 and variances *σ*<sup>2</sup> *<sup>i</sup>*ð Þ*k* . There is an assumption placed on the independence of f g *ni*ð Þ*k <sup>k</sup>*≥<sup>1</sup> from the distribution of initial longitudinal coordinates of the three cars i.e. *x*1ð Þ 0 , *x*2ð Þ 0 and *x*3ð Þ 0 .

### *4.1.2 Control problem*

Upon consideration of the control problem that the AV has to solve, it becomes apparent rather quickly that the AV simply doesn't have to decide on linear and angular velocities. If that were the case, then the Car 1 could simply wait till Car 3 passes and then overtake Car 2 by having a velocity greater than Car 2. That is not an optimal solution for all possible scenarios. Therefore,the first thing that the AV needs to do is to get better estimates of position and velocity of Cars 2 and 3 rather than using the raw noisy data. With the better estimates of positions and velocities of the HDVs, the AV can perform a feasibility analysis to see if it is feasible to overtake the Car 2 or not. If the Car 1 deems the overtake maneuver to be infeasible, it can resort to the waiting strategy. If, however, Car 1 decides to overtake, then it has to decide on the time to start overtaking. Once the time to start overtaking is finalized, then the AV has to plan a trajectory that it will take to perform the maneuver such that it will not violate any safety margins. Finally, it needs to generate control commands i.e. linear and angular velocities to execute the overtaking maneuver. One typical problem that could arise is that the vehicle may have to return to its original lane, after starting the overtaking maneuver, but this scenario is beyond the discussion of this chapter.

#### *4.1.3 Sample trajectory*

A sample trajectory with a constant linear velocity for the AV is displayed in **Figure 4**. Between *k* ¼ 0 and *k* ¼ *T*1, Car 1 is approaching Car 2. At *k* ¼ *T*1, Car 1 decides to run the feasibility analysis and decides to overtake Car 2. Between *k* ¼ *T*1 and *k* ¼ *T*2, Car 1 has a positive constant angular velocity resulting in motion towards the adjacent lane. At *k* ¼ *T*2, Car 1 has reached the divider between the lanes so it switches to a negative angular acceleration having the same magnitude as

algorithm such that it provides strict guarantees in regards to collision avoidance while the other is a stochastic algorithm that that yields a small collision probability with the advantage of shorter overtaking time. Moreover, the expected behavior of the human driver is modeled using a stochastic model based on martingales. It is shown that if the human driver is non-aggressive, the stochastic algorithm will yield a shorter overtaking time and if the driver is aggressive, the behaviors for stochastic

*Models and Methods for Intelligent Highway Routing of Human-Driven and Connected…*

The key takeaway from this section is that there is a need to place focus on explicitly considering the role of human drivers on the road while developing algorithms for autonomous vehicles. It was shown by the study of the simple car overtaking example that there are scenarios where the need to model human drivers on the road increases manifold. The algorithm presented in Section 4.2.2 explicitly models the behavior of human-drivers with martingales and provides overtaking algorithms with safety guarantees. The solution complexity of this approach is rather high due to the reachability analysis-based solutions so further research could be directed at improving the solution complexity or coming up with approaches that yield lower complexity while maintaining the safety guarantees. Furthermore, there is a prospect of exciting research in the direction of modeling human behavior with other approaches which may lead to various other interesting algorithms. The aim of this section was to provide motivation to the reader to explore research avenues that incorporate explicit modelling of human driving patterns yielding algorithms that will expedite the introduction of Autonomous Vehicles onto our

The advent of improved communication, sensing and control in modern day vehicles and infrastructure creates a lot of opportunities to improve the efficiency and safety in many highway processes. Key areas of interest involve traffic routing and management, optimal highway merging and intelligent overtaking behaviours. This chapter examined some of the methods used in these areas and discussed the various improvements and shortcomings of each of them. The implementation of the algorithms discussed in this chapter, would lead to modern transportation systems becoming more effective, productive and safe. While there are many other methods worthy of merit not discussed in this chapter, the areas covered should give the reader a broad understanding of the extent of possibilities in this field and also spark further thinking which may lead to the generation of innovative new

and the robust algorithms will be identical.

*DOI: http://dx.doi.org/10.5772/intechopen.94332*

**4.3 Summary**

roads.

**5. Conclusions**

solutions.

**139**

**Figure 4.** *Sample trajectory for car overtaking problem.*

before, until the car reaches the center of adjacent lane at *k* ¼ *T*3. Between *k* ¼ *T*3 and *k* ¼ *T*5, the angular acceleration remains at 0 for a straight line motion in opposite lane for overtaking. A negative angular acceleration having the same magnitude as before is applied between *k* ¼ *T*5 and *k* ¼ *T*6 followed by positive angular acceleration between *k* ¼ *T*6 and *k* ¼ *T*7. Car 1 continues its motion in a straight line after *k* ¼ *T*7 with zero angular acceleration.

#### **4.2 Solution methods**

There were quite a few assumptions made to obtain the simplified model discussed above and some of those assumptions could be removed in order to obtain a rather complicated yet general framework. For instance, it might not be possible for the AV to store all the history of past states and actions so an attempt at modelling with limited information could be made. With this disclaimer, a brief overview of some of the possible solution methods for this problem is presented below.

#### *4.2.1 Minimizing probability of collision*

In this approach, taken from [42], it is assumed that the AV will travel at a constant speed throughout the overtaking maneuver. The estimates of positions and velocities of HDVs are obtained using Kalman filtering and a constant N is introduced to characterize the behavior of the AV with higher values corresponding to higher level of aggressiveness. Two feasible sets for the linear and angular velocities respectively of Car 1 are obtained by ensuring that the estimated position of AV after performing the maneuver will stay outside the estimated minimum safety region around the HDVs. Using the feasibility sets, probability of collision of Car 1 with the HDVs is obtained and the decision to overtake is based on those probabilities. If the decision to overtake is made, the linear and the angular velocities are chosen to minimize the probability of collision and the maneuver is performed as detailed in Section 4.1.3.

#### *4.2.2 Reachability analysis-based with martingale-based HDV modelling*

In this approach, taken from [43], the focus is on obtaining safety guarantees while overtaking. In this approach, the restriction on the constant speed of HDVs is also lifted, which was alluded to previously in Section 4.2. There are two different reachability analysis-based algorithms presented: one is a robust time-optimal

*Models and Methods for Intelligent Highway Routing of Human-Driven and Connected… DOI: http://dx.doi.org/10.5772/intechopen.94332*

algorithm such that it provides strict guarantees in regards to collision avoidance while the other is a stochastic algorithm that that yields a small collision probability with the advantage of shorter overtaking time. Moreover, the expected behavior of the human driver is modeled using a stochastic model based on martingales. It is shown that if the human driver is non-aggressive, the stochastic algorithm will yield a shorter overtaking time and if the driver is aggressive, the behaviors for stochastic and the robust algorithms will be identical.
