# Chapter 6. Trajectory execution layer

The algorithms presented in the previous section compute the vehicle actions to be carried out in order to achieve the required automation task according to all inputs (automation level, driver automation level request, set of trajectories, relevant detected targets, vehicle positioning, trajectory & state limits, vehicle state). The outputs of this function are primarily the trajectory and the speed constraints. This trajectory has to be realized by a control system considering the fuel economy, the speed constraints and vehicle parameters.

Another task of the trajectory execution layer is the execution of the calculated control commands (motion vector) and to provide feedback about the actual status. The motion vector contains the selected trajectory that has to be followed including the desired longitudinal and lateral movement of the vehicle.

The execution of the motion vector is distributed among the intelligent actuators of the vehicle. The task distribution and the harmonization of the intelligent actuators are carried out by the integrated vehicle (powertrain) controller.

## 6.1. Longitudinal control

The main task of the longitudinal control is the calculation and execution of an optimal speed profile. It is basically a speed control (cruise control) which tries to hold speed set-point given by the driver, but it could be extended by distance control (ACC) and the road inclinations in front of the vehicle. This control method and the extension to a platoon is detailed in the following subsections.

### 6.1.1. Design of speed profile

The purpose is to design speed trajectory, with which the longitudinal energy, thus fuel requirement can be reduced. If the inclination of the road and the speed limits are assumed to be known, the speed trajectory can be designed. By choosing the speed fitting in these factors the number of unnecessary accelerations and brakes can be reduced.

The road ahead of the vehicle is divided into several sections and reference speeds are selected for them, see Figure 87. The rates of the inclinations of the road and those of the speed limits are assumed to be known at the endpoints of each section. The knowledge of the road slopes is a necessary assumption for the calculation of the velocity signal. In practice the slope can be obtained in two ways: either a contour map which contains the level lines is used, or an estimation method is applied. In the former case a map used in other navigation tasks can be extended with slope information. Several methods have been proposed for slope estimation. They use cameras, laser/inertial profilometers, differential GPS or a GPS/INS systems, see [72], [73], [74]. An estimation method based on a vehicle model and Kalman filters was proposed by [75].

The simplified model of the longitudinal dynamics of the vehicle is shown in Figure 88. The longitudinal movement of the vehicle is influenced by the traction force ${F}_{l}$ as the control signal and disturbances ${F}_{d}$. Several longitudinal disturbances influence the movement of the vehicle. The rolling resistance is modeled by an empiric form ${F}_{r}={F}_{z}{f}_{0}\left(1+\kappa {\stackrel{˙}{\xi }}^{2}\right),$ where ${F}_{z}$ is the vertical load of the wheel, ${f}_{0}$ and $\kappa$ are empirical parameters depending on tyre and road conditions and $\stackrel{˙}{\xi }$ is the velocity of the vehicle, see GOTOBUTTON GrindEQbibitem24 . The aerodynamic force is formulated as ${F}_{aer}=0.5{C}_{w}\rho {A}_{0}{\stackrel{˙}{\xi }}_{rel}^{2},$ where ${C}_{w}$ is the drag coefficient, $\rho$ is the density of air, ${A}_{0}$ is the reference area, ${\stackrel{˙}{\xi }}_{rel}$ is the velocity of vehicle relative to the air. In case of a lull ${\stackrel{˙}{\xi }}_{rel}=\stackrel{˙}{\xi }$, which is assumed in the paper. The longitudinal component of the weighting force is ${G}_{x}=mgsin\alpha$, where m is the mass of the vehicle and $\alpha$ is the angle of the slope. The acceleration of the vehicle is the following: $\stackrel{¨}{\xi }=\left({F}_{l}-{F}_{d}\right)/m$, where $m$ is the mass of the vehicle, $\xi$ is the position of the vehicle, and are the traction force and the disturbance force (${F}_{d}={F}_{r}+{F}_{aer}+{G}_{x}$), respectively.

Although between the points may be acceleration and declaration an average speed is used. Thus, the rate of accelerations of the vehicle is considered to be constant between these points. In this case the movement of the vehicle using simple kinematic equations is: ${s}_{1}={\stackrel{˙}{\xi }}_{0}\left({\stackrel{˙}{\xi }}_{1}-{\stackrel{˙}{\xi }}_{0}\right)/\stackrel{¨}{\xi }+{\left({\stackrel{˙}{\xi }}_{1}-{\stackrel{˙}{\xi }}_{0}\right)}^{2}/2/\stackrel{¨}{\xi }$, where ${\stackrel{˙}{\xi }}_{0}$ is the velocity of vehicle at the initial point, ${\stackrel{˙}{\xi }}_{1}$ is the velocity of vehicle at the first point and ${s}_{1}$ is the distance between these points. Thus the velocity of the first section point is the following: ${\stackrel{˙}{\xi }}_{1}^{2}={\stackrel{˙}{\xi }}_{0}^{2}+2\stackrel{¨}{\xi }{s}_{1}={\stackrel{˙}{\xi }}_{0}^{2}+\frac{2}{m}{s}_{1}\left({F}_{l1}-{F}_{d1}\right)$. The velocity of the first section point ${\stackrel{˙}{\xi }}_{1}^{2}$ is defined as the reference velocity ${v}_{ref,1}^{2}={\stackrel{˙}{\xi }}_{1}^{2}$. This relationship also applies to the next road section: ${\stackrel{˙}{\xi }}_{2}^{2}={\stackrel{˙}{\xi }}_{1}^{2}+2\stackrel{¨}{\xi }{s}_{2}$. It is important to emphasize that the longitudinal force ${F}_{l1}$ is known only the first section. Moreover, the longitudinal forces ${F}_{li},i>1$ are not known during the traveling in the first section. Therefore at the calculation of the control force it is assumed that additional longitudinal forces will not act on the vehicle, i.e., the longitudinal forces ${F}_{li},i>1$ will not affect the next sections. At the same time the disturbances from road slope are known ahead. Similarly, the velocity of the vehicle can be formalized in the next $n$ section points. Using this principle a velocity-chain, which contains the required velocities along the way of the vehicle, is constructed. At the calculation of the control force it is assumed that additional longitudinal forces ${F}_{li},i\in \left[2,n\right]$ will not affect the next sections. The velocities of vehicle are described at each section point of the road by using similar expressions to GOTOBUTTON GrindEQequation22 . The velocity of the ${n}^{th}$ section point is the following: . It is also an important goal to track the momentary value of the velocity. It can also be considered in the following equation: ${\stackrel{˙}{\xi }}_{0}^{2}\to {v}_{ref,0}^{2}.$

The ${F}_{di}$ disturbance force can be divided in two parts: the first part is the force resistance from road slope ${F}_{di,r}$, while the second part ${F}_{di,o}$ contains all of the other resistances such as rolling resistance, aerodynamic forces etc. We assume that ${F}_{di,r}$ is known while ${F}_{di,o}$ is unknown. ${F}_{di,r}={G}_{x}=mg\text{sin}{\alpha }_{i}$depends on the mass of the vehicle and the angle of the slope ${\alpha }_{i}$. When the control force ${F}_{l1}$ is calculated, only ${F}_{d1,o}$ influences the vehicle of all of the unmeasured disturbances. In the control design the effects of the unmeasured disturbances are ignored. The consequence of this assumption is that the model does not contain all the information about the road disturbances, therefore it is necessary to design a robust speed controller. This controller can ignore the undesirable effects. Consequently, the equations of the vehicle at the section points are calculated in the following way:

 ${\stackrel{˙}{\xi }}_{0}^{2}={v}_{ref,0}^{2}$ (1)
 ${\stackrel{˙}{\xi }}_{0}^{2}+\frac{2}{m}{s}_{1}{F}_{l1}-\frac{2}{m}{s}_{1}{F}_{d1,o}={v}_{ref,1}^{2}+\frac{2}{m}{s}_{1}{F}_{d1,r}$ (2)
 ${\stackrel{˙}{\xi }}_{0}^{2}+\frac{2}{m}{s}_{1}{F}_{l1}-\frac{2}{m}{s}_{1}{F}_{d1,o}={v}_{ref,2}^{2}+\frac{2}{m}\left({s}_{1}{F}_{d1,r}+{s}_{2}{F}_{d2,r}\right)$ (3)

$⋮$

 ${\stackrel{˙}{\xi }}_{0}^{2}+\frac{2}{m}{s}_{1}{F}_{l1}-\frac{2}{m}{s}_{1}{F}_{d1,o}={v}_{ref,n}^{2}+\frac{2}{m}\sum _{i=1}^{n}\text{}{s}_{i}{F}_{di,r}$ (4)

The vehicle travels in traffic and it may happen that the vehicle is overtaken. Because of the risk of collision it is necessary to consider the preceding velocity on the lane:

 ${\stackrel{˙}{\xi }}_{0}^{2}\to {v}_{lead}^{2}.$ (5)

The number of the segments is important. For example in the case of flat roads it is enough to use relatively few section points because the slopes of the sections do not change abruptly. In the case of undulating roads it is necessary to use relatively large number of section points and shorter sections, because it is assumed in the algorithm that the acceleration of the vehicle is constant between the section points. Thus, the road ahead of the vehicle is divided unevenly, which is consistent with the topography of the road.

In the following step weights ${\gamma }_{1},{\gamma }_{2},...,{\gamma }_{n}$ are applied to reference speeds. An additional weight $Q$ is applied to the momentary speed. An additional weight $W$ is applied to the leader speed. While the weights ${\gamma }_{i}$ represent the rate of the road conditions, weight $Q$ has an essential role: it determines the tracking requirement of the current reference velocity ${v}_{ref,0}$. By increasing $Q$ the momentary velocity becomes more important while road conditions become less important. Similarly, by increasing $W$ the road conditions and the momentary velocity become negligible. The weights should sum up to one, i.e. ${\gamma }_{1}+{\gamma }_{2}+...+{\gamma }_{n}+Q+W=1$.

Weights have an important role in control design. By making an appropriate selection of the weights the importance of the road condition is taken into consideration. For example when $Q=1$ and the control exercise is simplified to a cruise control problem without any road conditions. When equivalent weights are used the road conditions are considered with the same importance, i.e., $Q={\gamma }_{1}={\gamma }_{2}=...={\gamma }_{n}$ and $W=0$. When $W=1$ and only the tracking of the preceding vehicle is carried out. The optimal determination of the weights has an important role, i.e., to achieve a balance between the current velocity and the effect of the road slope. Consequently, a balance between the velocity and the economy parameters of the vehicle is formalized.

By summarizing the above equations the following formula is yielded:

 ${\stackrel{˙}{\xi }}_{0}^{2}+\frac{2}{m}{s}_{1}\left(1-Q-W\right){F}_{l1}-\frac{2}{m}{s}_{1}\left(1-Q-W\right){F}_{d1,o}=\vartheta$ (6)

where the value $\vartheta$ depends on the road slopes, the reference velocities and the weights

 $\vartheta =W{v}_{lead}^{2}+Q{v}_{ref,0}^{2}+\sum _{i=1}^{n}\text{}{\gamma }_{i}{v}_{ref,i}^{2}+\frac{2}{m}\sum _{i=1}^{n}\text{}{s}_{i}{F}_{di,r}\sum _{j=i}^{n}\text{}{\gamma }_{j}.$ (7)

In the final step a control-oriented vehicle model, in which reference velocities and weights are taken into consideration, is constructed. The momentary acceleration of the vehicle is expressed in the following way: ${\stackrel{¨}{\xi }}_{0}=\left({F}_{l}-{F}_{d,o}-{F}_{d1,r}\right)/m$ where ${F}_{d1,r}=mg\text{sin}\alpha$. Equation (6) is rearranged:

 ${\stackrel{˙}{\xi }}_{0}=\lambda$ (8)

where the parameter $\lambda =\sqrt{\vartheta -2{s}_{1}\left(1-Q-W\right)\left({\stackrel{¨}{\xi }}_{0}+gsin\alpha \right)}$ is calculated based on the designed $\vartheta$. Consequently, the road conditions can be considered by speed tracking. The momentary velocity of vehicle $\stackrel{˙}{\xi }$ should be equal to parameter $\lambda$, which contains the road information. The calculation of $\lambda$ requires the measurement of the longitudinal acceleration ${\stackrel{¨}{\xi }}_{0}$.

### 6.1.2. Optimization of the vehicle cruise control

In the following step the task is to find an optimal selection of the weights in such a way that both the minimization of control force and the traveling time are taken into consideration. Equation (6) shows that ${F}_{l1}$ depends only on the weights in the following way:

 ${F}_{l1}={\beta }_{0}\left(Q\right)+{\beta }_{1}\left(Q\right){\gamma }_{1}+{\beta }_{2}\left(Q\right){\gamma }_{2}+\dots +{\beta }_{n}\left(Q\right){\gamma }_{n}$ (9)

Since ${\beta }_{i}$ depends on the weight $Q$, therefore ${F}_{l1}$ depends on the weights $Q$ and ${\gamma }_{i}$. The longitudinal control force must be minimized, i.e., $|{F}_{l1}|\to Min!$. Instead, in practice the ${F}_{l1}^{2}\to Min!$ optimization is used because of the simpler numerical computation. Simultaneously, the difference between momentary velocity and modified velocity must be minimized, i.e., $|{v}_{ref,0}-{\stackrel{˙}{\xi }}_{0}|\to Min!$

The two optimization criteria lead to different optimal solutions. In the first criterion the road inclinations and speed limits are taken into consideration by using appropriately chosen weights $\overline{Q},{\overline{\gamma }}_{i}$. At the same time the second criterion is optimal if the information is ignored. In the latter case the weights are noted by $\stackrel{˘}{Q},{\stackrel{˘}{\gamma }}_{i}$. The first criterion is met by the transformation of the quadratic form into the linear programming using the simplex algorithm. It leads to the following form: ${\overline{F}}_{l1}^{2}\left(\overline{Q},{\overline{\gamma }}_{i}\right)={\left({\beta }_{0}\left(\overline{Q}\right)+{\beta }_{1}\left(\overline{Q}\right){\overline{\gamma }}_{1}+{\beta }_{2}\left(\overline{Q}\right){\overline{\gamma }}_{2}+\dots +{\beta }_{n}\left(\overline{Q}\right){\overline{\gamma }}_{n}\right)}^{2}$ with the following constrains $0\le \overline{Q},{\overline{\gamma }}_{i}\le 1$ and $\overline{Q}+{\sum }^{\text{​}}\text{}{\overline{\gamma }}_{i}=1-W$. This task is nonlinear because of the weights. The optimization task is solved by a linear programming method, such as the simplex algorithm.

The second criterion must also be taken into consideration. The optimal solution can be determined in a relatively easy way since the vehicle tracks the predefined velocity if the road conditions are not considered. Consequently, the optimal solution is achieved by selecting the weights in the following way: $\stackrel{˘}{Q}=1$ and ${\stackrel{˘}{\gamma }}_{i}=0,i\in \left[1,n\right]$.

In the proposed method two further performance weights, i.e., ${R}_{1}$ and ${R}_{2}$, are introduced. The performance weight ${R}_{1}$ ($0\le {R}_{1}\le 1$) is related to the importance of the minimization of the longitudinal control force ${F}_{l1}$ while the performance weight ${R}_{2}$ is related to the minimization of $|{v}_{ref,0}-{\stackrel{˙}{\xi }}_{0}|$. There is a constraint according to the performance weights ${R}_{1}+{R}_{2}=1$. Thus the performance weights, which guarantee balance between optimizations tasks, are calculated in the following expressions:

 $Q={R}_{1}\overline{Q}+{R}_{2}\stackrel{˘}{Q}={R}_{1}\overline{Q}+{R}_{2}$ (10)

${\gamma }_{\text{i}}={R}_{1}{\overline{\gamma }}_{\text{i}}+{R}_{2}{\stackrel{˘}{\gamma }}_{\text{i}}={R}_{1}{\overline{\gamma }}_{\text{i}}$, i=1,n(11)

Based on the calculated performance weights the speed can be predicted.

The tracking of the preceding vehicle is necessary to avoid a collision, therefore $W$ is not reduced. If the preceding vehicle accelerates, the tracking vehicle must accelerate as well. As the velocity increases so does the braking distance, therefore the following vehicle strictly tracks the velocity of the preceding vehicle. On the other hand it is necessary to prevent the velocity of the vehicle from increasing above the official speed limit. Therefore the tracked velocity of the preceding vehicle is limited by the maximum speed. If the preceding vehicle accelerates and exceeds the speed limit the following vehicle may fall behind.

Remark 3.1 Look-ahead control considering efficiency and safe cornering

The maximum cornering velocity for each section can be calculated in advance knowing the designed path of the vehicle. If the speed limit ${v}_{ref,i}$ at section point $i$ exceeds the safe cornering velocity then the speed limit ${v}_{ref,i}$ is substituted with the smallest value concerning skidding or rollover, i.e.,

 ${v}_{ref,i}^{mod}=\text{min}\left({v}_{ref,i};{v}_{skid,i};{v}_{roll,i}\right).$ (12)

### 6.1.3. Implementation of the velocity design

The control system can be realized in three steps as Figure 89 shows. The aim of the first step is the computation of the reference velocity. The results of this computation are the weights and the modified velocity which must be tracked by the vehicle. In the second step the longitudinal control force of the vehicle (${F}_{l1}$) is designed. The role of the high-level controller is to calculate this required longitudinal force. In third step the real physical inputs of the system, such as the throttle, the gear position and the brake pressure are generated by the low-level controller.

In the proposed method the steps are separated from each other. The reference velocity signal generator can be added to the upper-lower Adaptive Cruise Control (ACC) system. It is possible to design a reference signal generator unit almost individually, and to attach it to the ACC system. Thus the reference signal unit can be designed and produced independently from automobile suppliers, only a few vehicle data are needed. The independent implementation possibility is an important advantage in practice. The high-level controller calculates positive and negative forces as well, therefore the driving and braking systems are also actuated. Figure 90 shows the architecture of the low-level controller.

The engine is controlled by the throttle, which could be a butterfly gate or a quantity of injected fuel. The engine-management system and the fuel-injection system have their own controllers, thus in the realization of the low-level controller only the torque-rev-load characteristics of the engine are necessary. In this case the rev of the engine is measured, the required torque is computed from the longitudinal force of the high-level controller, thus the throttle is determined by an interpolation step using a look-up table. The position of the automatic transmission is determined by logic functions, thus it depends on the fuel consumption and the maximum rev of the engine. The pressures on the cylinders of the wheels increase during braking. At normal traveling the ABS actuation is not necessary, in case of an emergency the optimal driving is overwritten by the safety requirements. The necessary braking pressure for the required braking force is computed from the ratios of the hydraulic/pneumatic parts.

In the simulation example a transportational route with real data is analyzed. The terrain characteristics and geographical information are those of the M1 Hungarian highway between Tatabánya and Budapest in a $56$-$km$-long section. In the simulation a typical F-Class truck travels along the $56km$ route. The mass of the 6--gear truck is $2023kg$ and its engine power is $300kW$ ($402hp$). The regulated maximal velocity is $130km/h$, but the road section contains other speed limits (e.g. $80km/h$ or $100km/h$), and the road section also contains hilly parts. Thus, it is an acceptable route for the analysis of road conditions, i.e., inclinations and speed limits. Publicly accessible up-to-date geographical/navigational databases and visualisation programs, such as Google Earth and Google Maps, are used for the experiment.

Figure 91: Real data motorway simulation

In this example two different controllers are compared. The first is the proposed controller, which considers the road conditions such as inclinations and speed limits and is illustrated by solid line in the figures, while the second controller is a conventional ACC system, which ignores this information and is illustrated by dashed line. Figure 91 shows the time responses of the simulation.

Figure 91(a) shows that the motorway contains several uphill and downhill sections. Figure 91(b) shows the velocity of the vehicle with speed limits in both cases. The conventional ACC system tracks the predefined speed limits as accurately as possible and the tracking error is minimal. In the proposed method the speed is determined by the speed limits and simultaneously it takes the road inclinations into consideration according to the optimal requirement. Figure 91(c) shows the required longitudinal force. The high-precision tracking of the predefined velocities in the conventional ACC system often requires extremely high forces with abrupt changes in the signals. As a result of the road conditions less energy is required during the journey in the proposed control method, see Figure 91(f). The proposed method requires smaller energy ($47.2MJ$) than the conventional method ($55.6MJ$), and the energy saving is $8.4MJ$, which is $15.1\text{%}$. The fuel consumption can also be calculated by using the following approximation: $V=\left({F}_{l1}{\stackrel{˙}{\xi }}_{0}\right)/\left(\eta {L}_{h}{\rho }_{fuel}\right)$ where $\eta =0.25$ is the efficiency of the driveline system, ${L}_{h}=47.3MJ/kg$ is the heat of combustion and ${\rho }_{fuel}=730kg/{m}^{3}$ is the density of petrol. The fuel consumption of the conventional system is $6.44l$ while that of the proposed method is $5.47l$, which results in $0.97l$ reduction in fuel consumption in the analyzed $56km$ length section.

Remark 3.2 Speed control based on the preceding vehicle

In addition to the consideration of road conditions it is also important to consider the traffic environment. It means that the preceding vehicle must be considered in the reference speed design because of the risk of collision. Thus, all of the kinetic energy of the vehicle is dissipated by friction. This estimation of the safe stopping distance may be conservative in a normal traffic situation, where the preceding vehicle also brakes, therefore the distance between the vehicles may be reduced. In this paper the safe stopping distance between the vehicles is determined according to the 91/422/EEC, 71/320/EEC UN and EU directives (in case of ${M}_{1}$ vehicle category, the velocity in $km/h$): ${d}_{st}=0.1{\stackrel{˙}{\xi }}_{0}+{\stackrel{˙}{\xi }}_{0}^{2}/150$. It is also necessary to consider that without the preceding vehicle the consideration of safe stopping distance is neither possible nor necessary. The consideration of the preceding vehicle is determined by $W$. The following example analyses the incidence when another vehicle overtakes the vehicle with an adaptive control proposed in the paper or the vehicle catches up with a preceding vehicle.

In the simulation example, the preceding vehicle is slower, however, in the second part its velocity is higher than that of the follower vehicle. Furthermore, in the example the preceding vehicle also exceeds the official speed limit ($110km/h$). Figure 92(a) and Figure 92(b) show that in the first part of the simulation the follower vehicle approaches the preceding vehicle taking the braking distance into consideration, while in the second part the follower vehicle avoids exceeding the speed limit and falls behind. This velocity control is achieved by using the value of $\mathcal{W}$ as it is shown in Figure 92(d). In the first part of the simulation the weight is increased by the risk incident while in the second part it is reduced by the increasing distance. The solution requires radar information, which is available in a conventional adaptive cruise control vehicle. This simulation example shows that the designed control system is able adapt to external circumstances.

Figure 92: Adaptive control systems with a preceding vehicle

### 6.1.4. Extension of the method to a platoon

The main idea behind the design is that each vehicle in the platoon is able to calculate its speed independently of the other vehicles. Since traveling in a platoon requires the same speed, the optimal speed must be modified according to the other vehicles. In the platoon, the speed of the leader vehicle determines the speed of all the vehicles. The goal is to determine the common speed at which the speeds of the members are as close as possible to their own optimal speed. In the case of a platoon each vehicle has its own optimal reference speed ${\lambda }_{j}$. Moreover, speeds of the vehicles are not independent of each other, because the speed of the leader ${\lambda }_{1}$ influences the speed of every member of platoon ${\stackrel{˙}{\xi }}_{0,j}$. The goal is to find an optimal reference speed for the leader ${\overline{\lambda }}_{1}$.

It is important to note that there is an interaction between the speeds of the vehicles in a platoon. If a preceding vehicle changes its speed, the follower vehicles will modify their speeds and track the motion of the preceding vehicle within a short time. The members of the platoon are not independent of the leader, therefore it is necessary to formulate the relationship between the speed of the ${j}^{th}$ platoon member ${\stackrel{˙}{\xi }}_{0,j}$ and the leader and the preceding vehicles. It is formulated with a transfer function with its input and output. The output ${Y}_{j}=ℱ{\left[\begin{array}{lll}ℒ\left({\stackrel{¨}{\xi }}_{0,j}\right)\hfill & ℒ\left({\stackrel{˙}{\xi }}_{0,j}\right)\hfill & ℒ\left({\xi }_{0,j}\right)\hfill \end{array}\right]}^{T}$ contains the information, i.e., the position, speed and acceleration of the vehicle, which is sent to the follower vehicles. The input ${U}_{j}=\mathcal{G}{\left[\begin{array}{lllll}ℒ\left({\stackrel{¨}{\xi }}_{0,j-1}\right)\hfill & ℒ\left({\stackrel{˙}{\xi }}_{0,j-1}\right)\hfill & ℒ\left({\xi }_{0,j-1}\right)\hfill & ℒ\left({\stackrel{˙}{\xi }}_{0}\right)\hfill & ℒ\left({\xi }_{0}\right)\hfill \end{array}\right]}^{T}$ contains the position, speed, the acceleration of the preceding vehicle and the leader. The transfer function between ${U}_{j}$ and ${Y}_{j}$ is ${G}_{j,cl}={K}_{j}{G}_{j}/\left(1+{K}_{j}{G}_{j}\right)$ with the controller of the ${j}^{th}$ vehicle ${K}_{j}$ and its longitudinal dynamics ${G}_{j}$. Similarly, the effect of the leader and the preceding vehicles on the ${\left(j+1\right)}^{th}$ platoon member is formulated: ${Y}_{j+1}={G}_{j+1,cl}{U}_{j+1}$, where , which finally leads to . Consequently, the speed of the ${j}^{th}$ vehicle is determined by the next formula: . The value of ${\stackrel{^}{G}}_{j}$ is used for the computation of the optimal reference speed of the platoon ${\overline{\lambda }}_{1}$.

Finally the required reference speed of the leader vehicle ${\overline{\lambda }}_{1}$ is designed. The aim of the design is that the generated speeds of all the vehicles ${\stackrel{˙}{\xi }}_{0,j}$ are as close to as their modified reference speed ${\lambda }_{j}$ as possible:

 $\sum _{j=1}^{m}\text{}|{\lambda }_{j}-{\stackrel{˙}{\xi }}_{0,j}|\to Min.$ (13)

Since the speed of the ${j}^{th}$ vehicle is formulated as ${\stackrel{˙}{\xi }}_{0,j}={\stackrel{^}{G}}_{j}{\overline{\lambda }}_{1}$, the following optimization form is used: $\sum _{j=1}^{m}\text{}{\left({\lambda }_{j}-{\stackrel{^}{G}}_{j}{\overline{\lambda }}_{1}\right)}^{2}=\sum _{j=1}^{m}\text{}{\lambda }_{j}^{2}+\sum _{j=1}^{m}\text{}{\left({\stackrel{^}{G}}_{j}{\overline{\lambda }}_{1}\right)}^{2}-2\sum _{j=1}^{m}\text{}\left({\lambda }_{j}{\stackrel{^}{G}}_{j}{\overline{\lambda }}_{1}\right)\to 0$, where ${\stackrel{^}{G}}_{j}=\prod _{k=1}^{j-1}\text{}{\stackrel{^}{G}}_{k}$. It can be stated that in GOTOBUTTON GrindEQequation48 the only unknown variable is ${\overline{\lambda }}_{1}$. The optimization leads to the following equation: The solution of the optimization problem can be achieved. The deduction of the optimization of ${\overline{\lambda }}_{1}$ is as follows:

 ${\overline{\lambda }}_{1}=\frac{{\sum }_{j=1}^{m}\text{}\left({\lambda }_{j}{\prod }_{k=1}^{j-1}\text{}{\stackrel{^}{G}}_{k}\right)}{{\sum }_{j=1}^{m}\text{}{\left({\prod }_{k=1}^{j-1}\text{}{\stackrel{^}{G}}_{k}\right)}^{2}}$ (14)

It means that the leader vehicle must track the required reference speed ${\overline{\lambda }}_{1}$.

## 6.2. Lateral control

### 6.2.1. Design of trajectory

Government transportation agencies have been evaluating several studies in the field of highway road planning in respect to horizontal curve design. Road design manuals determine a minimum curve radius for a predefined velocity, road superelevation and adhesion coefficient, see [76], [77]. The calculation is based on assuming the vehicle to move on a circular path, where the vehicle is subject to centrifugal force that acts away from the center of the curve as illustrated in Figure 94, see [78]. The slip angle $\beta$ is assumed to be small enough for the lateral force to point along the path radius, and the longitudinal acceleration is also small enough not to degrade the lateral friction of the vehicle considerably.

The mass of the vehicle $m$ along with the road superelevation (cross slope) $\epsilon$ and the side friction between the tire and the road surface $\mu$ counterbalance the centrifugal force. Assuming that there is the same $\mu$ at each wheel of the vehicle the sum of the lateral forces is: ${\sum }^{\text{​}}\text{}{F}_{y}=mg\left(\mu +\epsilon \right)$, where $g=9.81m/{s}^{2}$ is the gravitational constant. At cornering the dynamics of vehicle is described by the equilibrium of the two forces: $m{v}^{2}/R=mg\left(\mu +\epsilon \right)$, where $R$ is the radius of the curve. Assuming that the road geometry is known by on-board devices such as GPS, it is possible to calculate a safe cornering velocity.

The following relationship holds for the maximum safe cornering velocity regarding the danger of skidding out of the corner:

 ${v}_{skid}=\sqrt{Rg\left(\mu +\epsilon \right)}$ (15)

(15) is also used in crash reconstruction and is referred to as the Critical Speed Formula (CSF), see [79], [80]. In the so-called yaw mark method the critical speed of the vehicle is determined by using the calculated radius of the vehicle path from the tire skid marks left on the road.

Note that in the calculation of the safe cornering velocity the value of the side friction factor $\mu$ plays a major role. This factor depends on the quality and the texture of the road, the weather conditions and the velocity of the vehicle and several other factors. The estimation of $\mu$ has been presented by several important papers, see e.g. [81], [82], [83]. However, these estimations are based on instant measurements, thus they are not valid for look-ahead control design, where the friction of future road sections should be estimated.

In road design handbooks the value of the friction factor is given in look-up tables as a function of the design speed, and it is limited in order to determine a comfortable side friction for the passengers of the vehicle. For the calculation of safe cornering velocity these friction values give a very conservative result.

A method to evaluate side friction in horizontal curves using supply-demand concepts has been presented by [84]. Here the side friction has an exponential relation with the design speed as follows:

 $\mu ={F}_{o,v}\text{exp}\frac{v-{v}_{skid}}{{S}_{p}}$ (16)

where ${F}_{o,v}$ and ${S}_{p}$ are constant values depending on the texture of the pavement. Note that ${F}_{o,v}$ is the estimated reference friction at a measurement speed of $v$. Thus, by using (15) and (16) the maximum safe cornering speed can be determined on a given road surface along with the side friction factor, as it is illustrated inFigure 95. The intersections of the supply and demand curves give the safe cornering velocities and the corresponding maximum side frictions. Note that whereas the friction supply only depends on the velocity of the vehicle, the friction demand is a function of the velocity and the curve radius as well.

The relationship between the curve radius and safe cornering velocity can be observed in Figure 5. The data points of the diagram are given by the intersections of the supply and demand curves. It shows that the safe cornering velocity increases with the curve radius. However, the relationship is not linear, i.e by increasing an already big cornering radius results in only moderated growth of the safe cornering velocity.

Rollover danger estimation and prevention control methods have already been studied by several authors, see [85], [86], [87]. A quasi-static analysis of maximal safe cornering velocity regarding the danger of rollover has been presented by [88]. Assuming a rigid vehicle and using small angle approximation for superelevation ($\text{sin}\epsilon \approx \epsilon$, $cos\epsilon \approx 1$), a moment equation can be written for the outside tires of the vehicle during cornering as follows:

 $m\frac{{v}^{2}}{R}h-mg\epsilon h+{F}_{zi}b-mg\frac{b}{2}=0$ (17)

where $h$ is the height of the center of gravity, $b$ is the track width, ${F}_{zi}$ is the load of the inside wheel at cornering.

The vehicle stability limit occurs at the point when the load ${F}_{zi}$ reaches zero, which means the vehicle can no longer maintain equilibrium in the roll plane. Thus by reorganizing (17) and substituting ${F}_{zi}=0$ the rollover threshold is given as follows:

 ${v}_{roll}=\sqrt{\frac{Rg}{2h}\left(b+2\epsilon h\right)}$ (18)

Thus, to ensure safe cornering of the vehicle in a cornering maneuver without the danger of skidding or rollover, the velocity of the vehicle has to chosen to meet the two constraints defined by (15) and (16).

Another important task is to calculate the radius of curves ahead of the vehicle in order to define the safe cornering velocities in advance. The road ahead of the vehicle can be divided into $n$ number of sections. The goal is to calculate the curve radius at each $n$ section points ahead of the vehicle to determine the safe cornering velocities corresponding to the curve radius.

The calculation of the cornering radius ${R}_{j}$, $j\in \left[1,n\right]$ is as follows. It is assumed that the global trajectory coordinates $x$ and $y$ of the vehicle path are known. Considering a sufficiently small distance the trajectory of the vehicle around a section point can be regarded as an arc, as it is shown in Figure 97. The arc can be divided into $k$ data points.

The length of the arc can be approximated by summing up the distances between data points: These distances are calculated as: The length of the chord ${d}_{j}$ is calculated as follows: Knowing the length ${s}_{j}$ and ${d}_{j}$, $j\in \left[1,n\right]$ it is possible to calculate a reasonable estimation of the curve radius ${R}_{j}$. Note that the length of the arc must be chosen carefully. A too short section with fewer data points may give an unacceptable approximation of the radius. On the other hand a too big distance can be inappropriate as well, since then the section may not be approximated by a single arc. The number of data points selected is also important, and by increasing the number $k$, the accuracy of the following calculation can be enhanced. The angle of the arc ${\phi }_{j}$ is as follows . The length of the chord ${d}_{j}$ is also expressed as a function of the radius: ${d}_{j}=2{R}_{j}\text{sin}\left({\phi }_{j}/2\right)$. Expressing the radius ${R}_{j}$ the following equation is derived:

 ${R}_{j}=\frac{{d}_{j}}{2\text{sin}\left(\frac{{s}_{j}}{2{R}_{j}}\right)}$ (19)

This expression can be transformed by introducing ${x}_{j}={s}_{j}/2{R}_{j}$ and using the Taylor series for the approximation of the $\text{sin}{x}_{j}$ function, i.e., $\text{sin}\left({x}_{j}\right)\cong {x}_{j}-{x}_{j}^{3}/6$. Then the following expression is gained for the radius:

 ${R}_{j}=\sqrt{\frac{{s}_{j}^{3}}{24\left({s}_{j}-{d}_{j}\right)}}$ (20)

The curve radius ${R}_{j}$, $j\in \left[1,n\right]$ can be calculated at each section point ahead of the vehicle path. The calculation method is validated through the CarSim simulation environment. Here, the vehicle follows the desired path while the curve radius is being measured and at the same time the calculation method is running giving a close approximation of the real value. The comparison of the real and the calculated radius is shown in Figure 98.

By calculating the radius of the curve using (20) the safe cornering velocity of the vehicle can be determined by using (15) and (18). This velocity can be considered as the maximum velocity that the vehicle is capable of in a corner without the danger of slipping and leaving the track or rolling over. It is important to state that in severe weather conditions this safe velocity may be smaller than that of the speed limit, thus the consideration of the maximum safety velocity in the cruise control design is essential.