Mode Switching Control Using Lane Keeping Assist and Waypoints Tracking for Autonomous Driving in a City Environment

Abstract

This paper proposes a mode switching supervisory controller for autonomous vehicles. The supervisory controller selects the most appropriate controller based on safety constraints and on the vehicle location with respect to junctions. Autonomous steering, throttle and deceleration control inputs are used to perform variable speed lane keeping assist, standard or emergency braking and to manage junctions, including roundabouts. Adaptive model predictive control with lane keeping assist is performed on the main roads and a linear pure pursuit inspired controller is applied using waypoints at road junctions where lane keeping assist sensors present a safety risk. A multi-stage rule based autonomous braking algorithm performs stop, restart and emergency braking maneuvers. The controllers are implemented in MATLAB^® and Simulink™ and are demonstrated using the Automatic Driving Toolbox™ environment. Numerical simulations of autonomous driving scenarios demonstrate the efficiency of the lane keeping assist mode on roads with curvature and the ability to accurately track waypoints at cross intersections and roundabouts using a simpler pure pursuit inspired mode. The ego vehicle also autonomously stops in time at signaled intersections or to avoid collision with other road users.

There is an increasing demand for methods that will enable cars to drive with high levels of autonomy, while dealing with dynamical, control and safety constraints. This paper considers the mode switching control of autonomous vehicles with an emphasis on path following, under constraints on the trajectory including traffic light signals, road users and the need to maintain a safe distance from preceding vehicles. The application under consideration is the simulation of a vehicle navigating a distance of 500 m in a city environment, including cross intersections, roundabouts and traffic lights, before reaching a car park. Real-life driving scenarios often require hybrid systems with the ability to switch between controllers when the control objectives change. Mode switching is often implemented between adaptive cruise control (ACC) laws, as in Zhenhaia et al. ( 1 ) where cruise, following or approach modes were successfully selected with their distance, speed and acceleration references under real traffic conditions. Ioannou and Chien ( 2 ) developed ACC for platooning applications by implementing a switching logic in a look ahead model. Faster traffic flows were obtained compared with driver models. A similar headway switching logic was extended in Raza and Ioannou ( 3 ) to vehicle following and platooning in automated highway systems, with emergency situations handling. In Zhang and Ioannou ( 4 ), autonomous cruise control was further developed with the ability to change lanes and vary speed limits depending on the traffic conditions. In Kesting et al. ( 5 ), ACC was used together with a traffic dependent switching logic. In Hegyi et al. ( 6 ), model predictive control (MPC) was used for the optimal combination of flow metering and speed limits to reduce the overall congestion time. Platooning is beyond the scope of this paper, however. Here a time headway is only considered with respect to a maximum of one lead vehicle at any time. Other aspects of autonomy may involve the ability to avoid obstacles, using either MPC or path planning methods, such as jump point search (JPS) ( 7 , 8 ). Autonomous braking is increasingly considered because of its proven impact on driving safety through testing procedures ( 9 ). Autonomous braking algorithms often employ partial braking to decelerate, followed by full braking to avoid collision, as in Bae et al. ( 10 ) where a tradeoff was achieved between ride comfort and safety. In Kusano et al. ( 11 ), a 39% reduction in the number of fatal injuries was obtained by combining forward collision warning with autonomous braking. This capability was extended in Lee et al. ( 12 ) to the ability to brake autonomously on roads with curvature. Autonomous driving strategies were also developed to account for traffic light signals. Predictive cruise control was used in Asadi and Vahidi ( 13 ) where upcoming traffic signal predictions were exploited to improve fuel economy and reduce trip time by adjusting speeds to reach intersections when traffic lights turn green. In Kamal et al. ( 14 ), MPC was used to optimize fuel economy on roads with signalized intersections.

This paper focuses on path following on roads with curvature and signalized intersections with the ability to stop but also restart autonomously. Lane keeping and maneuvering using MPC methods has been the subject of increased research interest in recent years. Predictive control is also increasingly being considered using active set or interior point methods, with hybrid affine models where a mode is typically selected based on the discrete states of a state machine and on discrete events being triggered when those states exceed thresholds ( 15 ). In Abdelmoniem et al. ( 16 ), a Stanley lateral MPC controller was used to optimize lateral and yaw errors, while avoiding obstacles during double lane changes. In Xu et al. ( 17 ), MPC was performed with autonomous lane keeping using lane keeping assist (LKA) sensor points. Other approaches have focused on reducing computational demand using an explicit MPC approach where a multiparametric quadratic optimization problem is solved offline. This allows for pre-computed gains to be used in different state space regions for real-time implementation, as shown in Lee and Chang ( 18 ) and Kamat ( 19 ). Nonlinear and time varying MPC were also used in Falcone et al. ( 20 ) to maintain stability during active steering control on slippery roads up to relatively high speeds.

The ability to adapt MPC to unexpected emergencies or to changing paths constraints is important to many driving scenarios in city environments. The latter was shown in Bujarbaruah et al. ( 21 ), where adaptive MPC was used to control the steering at constant speeds using a set membership-based switching strategy to deal with curvature changes during autonomous lane keeping. In Qu et al. ( 22 ), MPC was used with five driving modes depending on a congestion dependent emergency coefficient and on the estimated acceleration of a lead vehicle, with improvements compared with single mode MPC in driving comfort and safety. In Xue et al. ( 23 ), adaptive MPC was used for automated fallback maneuvers in presence of sensor malfunctions. Adaptive MPC formulations with responsibility-sensitive safety distance constraints were introduced in Chai et al. ( 24 ). The robustness of adaptive MPC to scenario linked uncertainty, including driving at a junction, was investigated in Leurent et al. ( 25 ). A chance constrained scenario MPC approach was used to change lanes safely in Cesari et al. ( 26 ). MPC was used in Liu et al. ( 27 ) for path planning by enforcing collision avoidance constraints while maintaining driving comfort by incorporating a lane dependent potential field within the cost function. A hybrid MPC approach was used as an ACC method in Corona et al. ( 28 ) where the objective was to follow a lead vehicle. In Di Cairano et al. ( 29 ), a switched MPC controller was developed to switch between local linear MPC controllers depending on tire conditions. A switched MPC controller was also proposed in Sun et al. ( 30 ) with mode switching between tracking error models, respectively based on heading error, heading error rate and sideslip. Mode switching with simpler controllers is less common, with few exceptions as in Shakouri and Ordys ( 31 ), where ACC uses nonlinear MPC for throttle control and more reactive proportional plus integral (PI) control for the more urgent autonomous braking and an additional switching logic is used to transition automatically from driver commands to ACC. The switching between MPC controllers is therefore increasingly considered, but mode switching between MPC and simpler, more reactive controllers has not received sufficient attention. Another reason to switch to more reactive controllers is the known possible adverse effect of certain sensors, such as LKA systems, as reported by Thatcham Research in Grover et al. ( 32 ) for scenarios requiring a lane change or to stop following lanes. In this paper, mode switching is applied between adaptive MPC and a simpler pure pursuit controller, which is known to be efficient at low speeds. In pure pursuit, a virtual target is assumed. The steering command is then calculated either from a required curvature and look ahead distance from the vehicle’s rear axle to the virtual target position, as in Yang et al. ( 33 ), or from a line of sight (LOS) requirement from the center of gravity (CoG) of the vehicle to the virtual target, as in Chen and Zhu ( 34 ). In this case, the car model still accounts for steering commands being applied with the front wheels, while heading is defined from the CoG.

The main contributions of the paper are as follows:

• A mode switching cruise and steering controller is applied with an adaptive MPC LKA mode on the main road to optimize path following and a safer and more reactive waypoints-based linear pure pursuit controller at road intersections, where this simpler approach is known to be efficient because of the shorter distances and lower speeds at those locations.

• The mode switching controller only uses LKA sensors on the main road where they are safe. It uses waypoints instead of LKA sensing at road junctions including cross intersections and roundabouts, where the discontinuities in lane patterns otherwise present the safety risk of an interruption in LKA sensor data, potentially leading to a wrong lane, even assuming a large LKA sensor field of view (FoV). Driving at junctions using LKA indeed presents lane keeping safety risks similar to the ones reported in Grover et al. ( 32 ).

• An autonomous braking logic is proposed, which differentiates between stopping accurately for traffic lights and emergency braking for other road users (ORUs) with additional safety margins, followed by a safe restart mode in both cases.

• The pure pursuit law projects the heading toward a virtual target between the current and next waypoints, which exploits ideas from driver models and differs from conventional LOS pure pursuit where the next waypoint is tracked.

The solutions are developed using features of the Automatic Driving Toolbox™ in MATLAB^®/Simulink™ 2020a, including LKA, optical and radar sensors and Model Predictive Control toolbox libraries, as well as the Driving Scenario Designer app. It is noteworthy that the LKA block in Simulink™ systematically fails at junctions and that the proposed approach circumvents this issue, which opens new prospects for users of the Automatic Driving Toolbox™ in MATLAB^®, particularly for path following. The supervisory mode switching control, pure pursuit inspired mode and autonomous braking algorithms were all developed as part of the InnovateUK Assured Parking project (reference: 105095). One of the objectives of the project is to evaluate and develop city driving and parking capabilities for autonomous vehicles. The road model has similar features to the ASSURED CAV City at the HORIBA MIRA campus, Nuneaton, UK. A desired itinerary is realized using waypoints centered on the desired lanes with desired speeds, based on speed limits and arcs connecting waypoints on the roads that intersect.

The remainder of this paper is organized as follows. The equations of vehicle dynamics are described in the next section. The supervisory switching logic is introduced in the following section for the autonomous driving problem under consideration. A mode switching steering controller is then presented before being extended to steering and speed control. An autonomous braking controller is introduced as an additional control mode to stop for traffic lights or ORUs before restarting. The numerical simulation analysis is then presented, which demonstrates the effectiveness of the proposed mode switching controller with adaptive LKA MPC and pure pursuit waypoint tracking modes for path following on roads with curvature, cross intersections and roundabouts and of the autonomous braking mode using onboard obstacle sensing and traffic light information. This is followed by a discussion on the generality of the approach before concluding the paper.

Equations of Vehicle Dynamics

The lateral and longitudinal vehicle dynamics are described by a “bicycle” type model similar to the one in Falcone et al. ( 20 ) and given by:

$$\begin{array}{c}\stackrel{··}{y}=-\stackrel{·}{x}r+\frac{F_y}{m}\hfill \\ \stackrel{·}{r}=\frac{M_z}{I_{\mathrm{zz}}}\hfill \\ r=\stackrel{·}{\psi }\hfill \end{array}$$

(1)

$$\stackrel{··}{x}=\stackrel{·}{y}r+\frac{F_x}{m}$$

(2)

where $x$ , $y$ are the Cartesian coordinates of the position vector in two dimensions, $r$ is the yaw rate, $I_{\mathrm{zz}}$ is the yaw inertia, $F_x$ , $F_y$ are the forces acting along the longitudinal and lateral body axes, $M_z$ is the yawing moment and $\psi$ is the yaw angle. The yawing moment is a function of tire dependent forces and arm lengths and is given by $M_z=F_{\mathrm{yr}}{l}_r+F_{\mathrm{yf}}{l}_f$ , where $l_r$ and $l_f$ are respectively the rear and front tire arm lengths with respect to the CoG of the car. $F_{\mathrm{yr}}$ and $F_{\mathrm{yf}}$ are the forces on the rear and front tires, respectively, and are given in Falcone et al. ( 20 ). They depend on tire stiffness, steering angle, velocity components $\stackrel{·}{x},\stackrel{.}{y}$ and yaw rate r. The forces F_x and F_y are linked to the steering, acceleration and deceleration commands as follows:

$$\begin{array}{c}\frac{F_x}{m}=\frac{{\delta }_{\tau }-F_{\mathrm{Brake}}}{m}\hfill \\ F_y=F_{\mathrm{yr}}+F_{\mathrm{yf}}\hfill \end{array}$$

(3)

where ${\delta }_{\tau }$ is the throttle force acting along the longitudinal axis and $F_{\mathrm{Brake}}$ is the deceleration force applied by the brakes.

The model can be compactly written in vector form as:

$$\stackrel{\mathbf{.}}{\mathbf{x}}=\mathit{f}(\mathbf{x},\mathbf{u})$$

(4)

with:

$$\begin{array}{c}\mathbf{x}={[x,\stackrel{·}{x},y,\stackrel{·}{y},\psi ,r]}^T\hfill \\ \mathbf{u}={[\frac{F_x}{m},\delta ]}^T\hfill \end{array}$$

For convenience, $\mathbf{p}={[x,y]}^T$ is defined as the position vector, $D=\frac{F_{\mathrm{brake}}}{m}$ as the deceleration obtained by autonomous braking and V as the longitudinal velocity. In practice, $\frac{F_x}{m}=\frac{{\delta }_{\tau }}{m}$ during path following when the throttle is used. When autonomous braking is used, the throttle is automatically disabled and $\frac{F_x}{m}=-D$ .

Supervisory Mode Switching Decision Layer for Autonomous Vehicle Control

The autonomous car in this study is required to perform the following autonomously:

• Switch between adaptive MPC and linear pure pursuit inspired waypoint tracking using the Autonomous Steering and Acceleration Supervisory Controller (ASASC) described in Algorithm 2 to drive on roads with junctions and only use LKA when it is safe. Note that the Steering Controller Arbitrator (SCA) of Algorithm 1 assumes steering at a constant speed and was developed as a control design stage before being extended to ASASC.

• Brake using the Multi-Stage Autonomous Braking System (MS-ABS) described in Algorithm 3 to adhere to road traffic regulations and stop in the case of emergency caused by the presence of ORUs in the path of the vehicle. The obstacle detection and pose estimation were obtained using sensor fusion from optical and radar sensors at the front of the ego vehicle.

• Restart when the traffic light is green or when the way is clear again, also using MS-ABS.

The supervisory decision layer determines which of the acceleration, steering and braking controllers are activated based on three sets of activation thresholds:

• Thresholds specifying forward crossing/collision warning time $t_c$ before traffic lights or obstructions created by ORUs,

• Thresholds specifying the time gap $t_h$ that will determine the safety distance s with respect to preceding vehicles,

• Lane keeping thresholds l and l_c to impose vertical deviation constraints and to indicate if LKA sensing is safe to use, respectively.

A forward collision warning (FCW) status variable determines if, without deceleration, collision with another road user will occur. Likewise, a forward stop line crossing warning (FSLCW) status variable determines if the traffic line will be crossed when the light is amber or red. The calculation of FCW time is described in the multi-stage braking section. The positions of the vertices of quadrilateral road intersection areas at the junctions are assumed to be obtained using the onboard map. Those regions will be taken to be rectangular in the numerical simulation section. The supervisory algorithm can be summarized as follows for scenarios requiring steering, acceleration and braking commands:

if (Green light) and (no road users detected within the safety distance)

 FCW status = 0 (No FCW)

 FSLCW = 0 (No FSLCW)

 braking status = 0 (No braking)

 call the mode switching controller for steering/acceleration (ASASC, Algorithm 2)

else if (amber or red traffic light) or (road user detected)

 call the autonomous braking controller (MS-ABS, Algorithm 3)

end if

Note that within MS-ABS (Algorithm 3), FCW status will be set to 1 before braking status is set to 1 (autonomous braking). The throttle in ASASC (Algorithm 2) is then set to 0 during multi-stage braking. When the braking status is reset to 0 (green light), the brakes are released, and the throttle command of ASASC is reactivated. A finite state machine enables the car to start again when the light turns back to green and if there is no obstacle in front of the vehicle.

Mode Switching Control for Path Following with Steering Inputs

The control design for the autonomous vehicle control is done in stages, where the lane following is first formulated for steering at constant speeds before being extended to acceleration and steering commands. The mode switching steering controller, which is presented in this section, has two modes: a LKA MPC controller on the main road and a waypoints tracking pure pursuit inspired steering controller at road intersections. The next two subsections respectively describe the adaptive MPC mode and the pure pursuit inspired waypoints tracking mode in the constant speed case.

Adaptive MPC Steering Controller Mode

This subsection is a precusor to the steering and acceleration controller. It focuses on steering and assumes that the speed is constant, making it possible to evaluate the steering performance independently. This preliminary simplifying assumption will be relaxed in the proposed 3DoF controller.

A reduced order control system is therefore considered first, with $\mathbf{u}=\delta ,\mathbf{x}=[x,y,\psi ,r]$ . The optimization problem under consideration is the minimization of a quadratic cost function under control effort and lateral error limitations:

$$\begin{array}{c}\underset{\delta }{min}J={\int }_0^{\infty }({\mathbf{e}}^{\mathbf{T}}\mathbf{Qe}+{\mathbf{u}}^{\mathbf{T}}\mathbf{Ru})\mathrm{dt}\hfill \\ \multicolumn{1}{c}{\begin{array}{c}\mathrm{st}.\left|\delta \right|<{\delta }_{\mathrm{Max}}\hfill \\ \multicolumn{1}{c}{\left|e_y\right|<l}\end{array}}\end{array}$$

(5)

where e = x_d–x and x_d is a desired state vector trajectory, $e_y$ is the lateral error with respect to the lane center, ${\delta }_{\mathrm{Max}}$ is the maximum steering angle and $l$ is a constraint on maximum lateral deviation from the path. The coordinates x_d, y_d are obtained from the LKA sensor when LKA is sufficiently accurate.

Using adaptive MPC, it is possible to incorporate controller and state constraints. The prediction model is adapted to changing operating conditions. The plant model is linearized by a first order Taylor expansion around operating points $(\overline{\mathbf{x}},\overline{\mathbf{u}})$ along the road curvature. In adaptive MPC, the nominal operating point $(\overline{\mathbf{x}},\overline{\mathbf{u}})$ evolves with time and the linearization matrices are updated:

$$\begin{array}{c}\stackrel{·}{\mathbf{x}}={\mathbf{A}}_{\mathbf{c}}\left(\overline{\mathbf{x}}\right)\mathbf{x}+{\mathbf{B}}_{\mathbf{c}}\mathbf{u}+\mathbf{N}\left(\overline{\mathbf{x}}\right)\hfill \\ \multicolumn{1}{c}{\mathbf{y}={\mathbf{C}}_{\mathbf{c}}\mathbf{x}}\end{array}$$

(6)

$${\mathbf{A}}_{\mathbf{c}}\left(\overline{\mathbf{x}}\right)={\left[\frac{\partial \mathit{f}}{\partial \mathbf{x}}\right]}_{\overline{\mathbf{x}}},{\mathbf{B}}_{\mathbf{c}}\left(\overline{\mathbf{x}}\right)={\left[\frac{\partial \mathit{f}}{\partial \mathbf{u}}\right]}_{\overline{\mathbf{x}}}=\left[\begin{array}{ccc}0\hfill & \frac{1}{m}\hfill & 0000\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\frac{K_f}{m}0\frac{l_f{K}_f}{I_{\mathrm{zz}}}\hfill \end{array}\right]$$

The matrix C_c is $I_{6\times 6}$ in the full 3DoF model and a concatenation of $I_{2\times 2}$ and $0_{2\times 2}$ matrix in the special case of a steering only model. After first order discretization, the discrete state and control matrices A_k, B_k remain constant around the operating point and do not necessarily change at each time k. Matrices A_k and B_k are hereinafter referred to as A and B.

The predicted states for i = 1, N where N is the prediction horizon, can then be written as a function of past states over a prediction horizon and past control inputs over a control horizon N_c:

$$\begin{array}{cc}\multicolumn{1}{c}{\mathbf{x}(k+i|k)=}& {\mathbf{A}}^i\mathbf{x}\left(k\right)\hfill \\ +\left[\begin{array}{ccc}{\mathbf{A}}^{i-1}\hfill & {\mathbf{A}}^{i-2}\hfill & \begin{array}{cc}\dots \hfill & \mathbf{I}\hfill \end{array}\hfill \end{array}\right]\mathbf{B}{\mathit{U}}_c\left(k\right),i=1,N\hfill \end{array}$$

(7)

$${\mathit{U}}_c\left(k\right)={\mathit{U}}_c(k-1)+\Delta {\mathit{U}}_c\left(k\right)$$

where ${\mathit{U}}_c\left(k\right)=\left[\begin{array}{c}\mathbf{u}(k|k)\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{\Delta \mathbf{u}(k+N_c-1|k)}\end{array}\right],\Delta {\mathit{U}}_c\left(k\right)=\left[\begin{array}{c}\Delta \mathbf{u}(k|k)\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{\Delta \mathbf{u}(k+N_c-1|k)}\end{array}\right]$

For $n_u$ inputs, assuming no lead vehicle, the optimal vehicle path following control problem can be written as a quadratic programming problem minimizing:

$$\begin{array}{c}\begin{array}{c}J=\sum _{i=1}^N\Vert \mathbf{x}(k+i|k)-{\mathbf{x}}_{\mathrm{ref}}\left(k\right){\Vert }_Q^2+\sum _{j=1}^N\Vert \mathbf{\Delta }\mathbf{u}(k+i|k){\Vert }_R^2\hfill \\ \multicolumn{1}{c}{\mathrm{st}.\left|u_j\right|<{u_j}_{\mathrm{Max}},j=1,n_u}\end{array}\hfill \\ \multicolumn{1}{c}{\left|e_y\right|<l}\end{array}$$

(8)

Pure Pursuit Steering Controller Mode

The conventional stabilizing steering controller at cross intersections or when entering and leaving roundabouts in the constant speed case will simply consist of a PI controller:

$${\delta }_{\mathrm{PI}}\left(t\right)=k_p({\psi }_c\left(t\right)-\psi \left(t\right))+k_i{\int }_0^t({\psi }_c\left(\tau \right)-\psi \left(\tau \right))d\tau$$

(9)

where k_p and k_i are the proportional and integral controller gains, respectively, and $\psi$ and ${\psi }_c$ are the actual and desired yaw angles, respectively. The desired yaw ${\psi }_c$ is derived using a LOS pure pursuit inspired approach, which is recursively applied at the current position between the current waypoint i and the next waypoint i+1, for i = 1, n_p, where n_p is the number of waypoints. Pure pursuit is also naturally complementary to MPC as it presents an analogy with it, given the tendency to look ahead with a LOS. A similar approach was proposed in Chen and Zhu ( 34 ) but the virtual target in that paper was simply the next waypoint. In this paper, the target can be adjusted depending on the relative positions with respect to the current and next waypoints. A weighting parameter $\lambda$ is defined here as:

$$\lambda =\frac{〈\Delta \mathbf{p},\Delta {\mathbf{p}}_{\mathit{waypoints}(\mathit{i},\mathit{i}+1)}〉}{{\Vert \Delta {\mathbf{p}}_{\mathit{waypoints}(\mathit{i},\mathit{i}+1)}\Vert }^2}$$

(10)

where 〈.,.〉 represents the scalar product, $\mathbf{\Delta }\mathit{p}$ is the relative position with respect to current waypoint i and ${\mathbf{p}}_{\mathit{waypoints}(\mathit{i},\mathit{i}+1)}$ is the position vector from waypoint i to waypoint $i+1$ . The projection parameter $\lambda$ is designed to be closer to 1 when the vehicle is closer to p_i+1 and closer to zero when the vehicle is closer to p_i. This safety improvement, which exploits driver models available in Mathworks examples, makes the controller more reactive between waypoints. The target position can then be obtained as:

$${\mathbf{p}}_T=(1-\lambda ){\mathbf{p}}_i+\lambda {\mathbf{p}}_{i+1}$$

(11)

where ${\mathbf{p}}_i$ and ${\mathbf{p}}_{i+1}$ respectively represent the current and next waypoint positions. A similar approach is used to develop realistic drivers’ models by Mathworks. The desired heading is then finally given by:

$${\mathrm{\psi }}_c=\mathrm{atan}\left(\frac{\Delta y}{\Delta x}\right)$$

(12)

where $\Delta x\mathrm{and}\Delta y$ are respectively the longitudinal and lateral components of the vector ${\mathbf{p}}_{\mathbf{T}}-\mathbf{p}$ . Note that the desired heading could also have been written as a function of look ahead distance and road curvature.

The waypoints are taken to be at the center of the desired lane on the road map and on the arcs joining roads at road intersections. The desired speed is constant. This condition will later be relaxed with the variable speed controller and the same pure pursuit law will be used to track variable (piecewise constant) desired speeds. In the next subsection, the constant speed mode switching steering controller will combine the two controllers from the last two subsections.

SCA

The objective of steering control is to follow a desired path including junctions and only change steering with a constant speed. The mode selection switches to the adaptive MPC steering controller mode on the main road before reaching road intersections, whenever the lane keeping MPC and waypoint tracking are sufficiently different (above safety threshold $\epsilon$ ) which is typically the case with road curvature and the LKA sensing indicates a correct lane, which is characterized by a cross track error $\left|e_y\right|$ smaller than a threshold $l_c$ . LKA is used by default while operating in the adaptive MPC mode because of its accuracy and the threshold $\epsilon$ is kept small to avoid unnecessary switching to waypoints on curved main roads. The threshold $\epsilon$ is only used to activate the simpler PI control when the road is straight. To avoid accidental switching on the main road, it is possible to require differences smaller than $\epsilon$ for the last few time steps in memory, but this was found not to be necessary for the selected scenarios with road curvature. At intersections and when entering or leaving roundabouts, the pure pursuit inspired waypoints tracking controller mode is used for path following.

A set-based approach is used to define road intersection areas where pure pursuit PI control is used. Let F denote the full road map, in other words $F=\{(x,y)\in R^2,\underset{¯}{x}\le x\le \overline{x}\mathrm{and}\underset{¯}{y}\le y\le \overline{y}\}$ , where $\underset{¯}{x},\overline{x}$ , $\underset{¯}{y},\overline{y}$ are respectively the minima and maxima of x and y in the road map area. Let J denote the union of all intersection areas ${\mathrm{\Gamma }}_j$ in the map, either between two roads or between a road and a roundabout:

$$\mathrm{\Gamma }=\bigcup _{j=1}^{N_J}{\mathrm{\Gamma }}_j$$

(13)

where $N_J$ is the number of road intersections on the desired path, ${\mathrm{\Gamma }}_j=\{(x,y)\in R^2,{\underset{¯}{x}}_j\le x\le {\overline{x}}_j\mathrm{and}{\underset{¯}{y}}_j\le y\le {\overline{y}}_j\}$ represents a road intersection box (rectangular area in simulation examples) and ${\underset{¯}{x}}_j,{\underset{¯}{y}}_j,{\overline{x}}_j,{\overline{y}}_j$ are elements of F, respectively representing the minima and maxima of x and y in that box area.

The mode switching steering algorithm is summarized below:

Algorithm 1: SCA Algorithm

In the SCA, steering control is applied at a constant speed to evaluate the steering performance independently from speed control and uses adaptive LKA MPC mode in $F\backslash \mathrm{\Gamma }$ (main roads) provided that LKA is enabled by other safety and accuracy improvement thresholds. It uses the PI control mode in $\mathrm{\Gamma }$ (road intersection areas) or if LKA is disabled because of other safety and accuracy thresholds. The MS-ABS of Algorithm 3 (autonomous braking and restart) cannot be activated from the SCA (Algorithm 1), as that would imply the use of variable speeds. This mode switching approach will be generalized to variable speeds in ASASC (Algorithm 2), from which MS-ABS can then be activated.

if ( $\mathbf{p}\in F\backslash \mathrm{\Gamma }$ and $\left|e_y\right|\le l_c$ and | $\delta$ _mpc– $\delta$ _PI|> $\epsilon$ )

 Apply adaptive MPC , $\delta$ _mpc, to solve Equation 5 (with LKA by default)

else if ( $\mathbf{p}\in \mathrm{\Gamma }$ or $\left|e_y\right|>l_c$ or | ${\delta }_{\mathrm{mpc}}$ – $\delta$ _PI| $\le \epsilon$ )

 Apply the waypoint tracking PI pure pursuit controller of Equation 9

end if

Mode Switching Controller with Steering and Acceleration Commands

The mode switching controller of this section also has an adaptive MPC LKA mode and a linear waypoint tracking pure pursuit mode; however, both control modes have two inputs, steering and acceleration. The state vector $\mathbf{x}={[x,\stackrel{·}{x},y,\stackrel{·}{y},\psi ,r]}^T$ and the control input vector $\mathbf{u}={[F_x,\delta ]}^T$ will therefore have full dimension.

The control objective of ASASC (Algorithm 2) is to follow a desired path with variable desired speeds when necessary (i.e., linearly decreasing desired speeds at the junctions when leaving the previous road or roundabout and linearly increasing desired speeds when entering the next road or roundabout). The ego vehicle will also maintain a safe minimum distance headway $s$ from the lead vehicle, specified as a constant time headway $t_h$ . $\Delta \delta \mathrm{and}\Delta \tau$ are defined respectively as the differences between MPC and pure pursuit inspired PI waypoint tracking for the steering and throttle commands. The mode selector will switch to the adaptive MPC steering and acceleration controller whenever the differences in absolute value of $\Delta \delta \mathrm{and}\Delta \tau$ between the lane keeping MPC and the pure pursuit waypoints tracking control inputs exceed thresholds ${\epsilon }_1\mathrm{on}\Delta \delta {\epsilon }_2\mathrm{on}\Delta \tau ,$ and that the vehicle is not at a road intersection area, such that LKA sensing is reliable. Otherwise, waypoints-based pure pursuit inspired PI control with acceleration and steering commands is used to control speed and yaw to the desired virtual target position. That the pure pursuit controller is reactive is another safety advantage at junctions compared with MPC.

Adaptive MPC Steering and Acceleration Controller Mode

The adaptive MPC algorithm will be identical to the one in Equations 6 to 15, but with additional constraints.

$$\begin{array}{c}\underset{\mathit{u}}{min}J={\int }_0^{\infty }({\mathbf{e}}^{\mathbf{T}}\mathbf{Qe}\mathbf{+}{\mathbf{u}}^{\mathbf{T}}\mathbf{Ru})\mathrm{dt}\hfill \\ \multicolumn{1}{c}{\begin{array}{c}\begin{array}{c}\mathrm{st}\left|\delta \right|<{\delta }_{\mathrm{Max}}\hfill \\ \multicolumn{1}{c}{\tau <{\tau }_{\max }}\end{array}\hfill \\ \multicolumn{1}{c}{\begin{array}{c}\left|e_y\right|<l\hfill \\ \multicolumn{1}{c}{\left|d_{\mathrm{EL}}\right|>s\left(t_h\right)}\end{array}}\end{array}}\end{array}$$

(14)

where l is an upper bound constraint on the cross track error $\left|e_y\right|$ , $d_{\mathrm{EL}}$ is the additional constraint on the distance headway from the ego to the lead vehicle and, as previously defined, $\mathbf{u}={[\mathbf{\delta },\mathbf{\tau }]}^{\mathit{T}}$ as it can be assumed that braking is not active and $F_x=\frac{\mathbf{\tau }}{\mathit{m}}$ when this controller is applied. Autonomous braking is only used in the autonomous braking mode.

Linear Steering and Speed Controllers in the Waypoints Tracking Mode

The conventional steering control law for the 3DoF controller is still given by Equation 9 and the controller for longitudinal speed $V$ is likewise given by:

$${\delta }_{\tau \mathrm{PI}}=S_{\mathrm{brake}}(k_p(V_c\left(t\right)-V\left(t\right))+k_i{\int }_0^t(V_c\left(\tau \right)-V\left(\tau \right))d\tau )$$

(15)

where $V_c$ is the desired longitudinal speed.

Note that the throttle will be multiplied by a status variable $S_{\mathrm{brake}}$ that will be equal to 1 nominally but will be switched to zero when a braking maneuver is activated ( $F_{\mathrm{brake}}\ne 0)$ because of traffic lights turning amber or red or to prevent collision with ORUs impeding the vehicle’s path.

The 3DoF mode switching algorithm, summarized in the ASASC (Algorithm 2), uses steering and acceleration commands to control speed, position and heading.

Algorithm 2: ASASC

This algorithm only runs when MS-ABS (Algorithm 3) is not running (FCW status = 0, braking status = 0). For compactness, $\delta \mathrm{and}\tau$ are both denoted $u_i$ in this algorithm, with i = 1 and i = 2 for the steering and throttle components of u, respectively. The sets $\mathrm{\Gamma },\mathrm{F}$ are defined as in the steering case.

if ( $\mathbf{p}\in F\backslash \mathrm{\Gamma }$ and $\left|e_y\right|\le l_c$ and $|u_{i\mathrm{mpc}}-u_{i\mathrm{PI}}|>{\epsilon }_i,i=\{1,2\})$

 Apply adaptive MPC u_mpc steering and acceleration that solves Equation 14

else if ( $\mathbf{p}\in \mathrm{\Gamma }$ or $\left|e_y\right|>l_c$ or $|u_{i\mathrm{mpc}}-u_{i\mathrm{PI}}|\le {\epsilon }_i,i=\{1,2\})$

 Apply waypoint tracking PI controllers from Equations 9, 15

end

Note that in the adaptive MPC mode, the control input vector u(k) is applied at the current time step k. A loop predicts and stores inputs and outputs over the prediction horizon N and the control horizon N_c for possible future use (if MPC mode remains active) by solving Equation 14.

Autonomous Braking with Start–Stop Sequencing

The traffic light signals are programmed as a periodic finite state machine sequence. The proposed MS-ABS mode can be activated at any time to allow the car to stop for road users in the path of the ego vehicle, detected using optical and radar sensors, or to stop for traffic lights when necessary. The vehicle is also programmed to restart when the traffic lights turn back to green or when there are no more road users in the path of the ego vehicle. The methodology and algorithms for autonomous braking and start–stop sequencing are detailed hereinafter. Without loss of generality, autonomous braking is assumed to take place on straight parts of the road circuit, which is a realistic assumption near traffic lights. Autonomous braking is implemented using a finite state machine to select realistic progressive deceleration stages in the range from 0 to 1 g, with values above 0.4 g considered as hard braking.

Multi-Stage Braking Algorithm

The algorithm is activated when a time to collision (TTC) or time to traffic light (TTTL) becomes shorter than the forward collision/intersection warning time. The FCW and FSLCW times are given by:

$$\mathrm{FCW}\mathrm{time}=\frac{V}{D_{\mathrm{FCW}}}$$

(16a)

$$\mathrm{FSLCW}\mathrm{time}=\frac{V}{D_{\mathrm{FSLCW}}}$$

(16b)

where V is the norm of vector $\mathit{V}=\stackrel{·}{x}\overrightarrow{i}+\stackrel{·}{y}\overrightarrow{j}$ and $\overrightarrow{i},\overrightarrow{j}$ are respectively unit vectors along the $x$ and $y$ axes of the road map and $D_{\mathrm{FCW}}$ is the FCW deceleration, representing the nominal deceleration that an average driver would apply. Similarly, the FSLCW time will be needed to calculate the TTTL and this deceleration will be chosen such that $D_{\mathrm{FSLCW}}\le D_{\mathrm{FCW}}$ . For the simulated scenarios, traffic lights were placed at locations where the road is straight and along the $x$ or $y$ axis and nominal decelerations were also assumed for simplicity. Multi-stage braking uses predefined deceleration commands $D_i$ , i = 1, n, such that $D_{i+1}<D_i$ and $D_n$ is the maximum deceleration at the last stage n. In the numerical simulation analysis, n is taken to equal 3. Each deceleration stage has an associated braking time $t_{\mathrm{bi}}$ :

$$t_{\mathrm{bi}}=\frac{V}{2D_i},i=1,n$$

(17)

The division by two in Equation 17 is sometimes overlooked in commercial software but is necessary to stop at the correct position. Indeed, for a constant deceleration stage to reduce the initial speed $V$ to zero, the average of the decreasing speed over the deceleration time is $\frac{V}{2}$ , assuming that the vehicle was not initially accelerating. The TTTL is given by:

$$\mathrm{TTTL}=\frac{\Vert \mathbf{p}-{\mathbf{p}}_{\mathrm{TL}}\Vert }{V}$$

(18)

where ${\mathbf{p}}_{\mathrm{TL}}$ is the desired CoG position of the car to stop at the traffic light stop line. Note that the TTTL simply reduces to $\frac{x-x_{\mathrm{TL}}}{\stackrel{·}{x}}$ when the traffic light is along the $x$ axis, for example. The automamous braking logic is similar for pedestrians and ORUs but the difference is that exact stopping position is not enforced. Instead, the breaking time for ORUs is given by:

$$t_{\mathrm{bi}\mathrm{ORU}}=k\frac{V}{2D_i}$$

(19)

where k is a gain that can be tuned empirically and depends on the speed limit and the detection sensors’ specifications. Geometrically, detection is possible if the ORU is within the sensor FoV and range $\frac{l_w}{\tan \left(\mathrm{FoV}\right)}\le d_{V-O}\le r_{\max },$ where $r_{\max }$ is the maximum sensor range, $d_{V-O}$ is the distance to the ORU and $l_w$ is lane width. The additional time needed to account for the vehicle not yet being aligned with a crossing pedestrian and that the braking distance is smaller than the detection range can be calculated as $\frac{r_{\max }-\frac{l_w}{\tan \left(\mathrm{FoV}\right)}}{\frac{V}{2}}$ . In practice, an analytical formula helps to inform a first guess, but the value of k will be tuned manually because of the uncertainty on obstacle detection, which is characterized by the sensor detection probability and the scenario.

The autonomous braking algorithm for ORUs in Algorithm 3 is otherwise very similar to stopping for traffic lights, with the differences being that TTTL is replaced by the TTC and that the restart condition (light turning back to green) is replaced by a condition to check that the road user has been outside detection range for more than a set time, t_restart.

The multi-stage braking algorithm is summarized in Algorithm 3 as follows:

Algorithm 3: Multi-Stage Autonomous Braking Algorithm (MS-ABS)

This algorithm is run:

• If (traffic light is amber or red and ${\epsilon }_{\mathrm{SL}}<d_{V-\mathrm{SL}}<L$ ), where ${\epsilon }_{\mathrm{SL}}$ is a distance ahead of the stop line that is too short to enable the ego vehicle to stop if the light changes to amber. L is the maximum distance from the stop line to start monitoring the traffic lights if the required $D\ge D_{\min }$ . L will be taken to be equal to the range of the optical sensor, or

• If an ORU is suddenly detected and emergency braking is required.

There is an additional 0.1 s detection time delay to detect traffic lights or ORUs. That delay is implemented by default within the Automatic Driving Toolbox radar and optical sensors blocks in the case of road users and was verified with HORIBA MIRA to be realistic. To account for this and stop before the traffic light stop line, the car stop line is taken to be −0.5 m before the traffic light stop line. Let n be the number of braking stages.

FCW status = 1 or FSLCW = 1 (collision or stop line warning, autonomous braking algorithm activated)

 while i $\le n$ (n = 3 in three-stage braking)

  if (TTTL< $t_{\mathrm{bi}}$ ) and (TTC< $t_{b{i}_{\mathrm{ORU}}}$ )

    $D=D_i$ , (Note that $D_{i+1}>D_i$ )

    (use Equation 17 to compute $D_i$ if $\mathrm{TTTL}<t_{\mathrm{bi}}$ and $\mathrm{TTC}\ge t_{\mathrm{biORU}}$ ,

    otherwise use Equation 19)

   Braking status = 1 (Deactivate throttle controller of Algorithm 2: ${\delta }_T=0$ )

    if (car not stopped)

      $i=i+1$

    else if car stopped

     FCW status = 0

     FSLCW = 0

     Apply $D_0$ to keep the car stationary (assuming zero or low road inclination)

      if traffic light is green or ORU out of collision area for duration t_restart

       D = 0 (Disable autonomous braking)

       Braking status = 0 (Reactivate throttle controller of Algorithm 2)

      end if (green light road user out of risk)

    end if (car stop condition)

  end if (TTTL or TTC condition)

 end while

Note that for single stage braking (n = 1) in the MS-ABS, the condition to compute TTTL (or TTC) is:

$$-\frac{D}{2}\mathrm{TTT}{L}^2+V\mathrm{TTTL}+\Vert \mathit{p}-{\mathit{p}}_{\mathit{TL}}\Vert =0$$

(20)

where D is the nominal deceleration value (typically 4 m.s⁻²). In the simulation analysis, only longitudinal axis components of vectors p and V (with norm V) are used with a traffic light along that axis.

The autonomous braking algorithm was implemented using Stateflow™ and MATLAB^®/Simulink™ to implement the state machine part of the logic that differentiates between the cases of traffic lights and ORUs.

Numerical Simulation Analysis

The vehicle parameters used for the simulation analysis are mass, m = 1575 kg, yaw inertia, I_zz = 2875 $\mathrm{mN}{s}^2$ , and right and left arm lengths given by l_r = 1.6 m and l_f = 1.2 m, respectively. The front and rear tire stiffness are K_f = 19.6 kN/rad and K_r = 33 kN/rad, respectively. Lane width is 3.75 m. Initial conditions and desired speeds are scenario dependent. All scenarios are designed using the Driving Scenario Designer app within MATLAB^® and are preloaded with waypoints centered within the desired lanes on the desired itinerary. Functionalities of the Automatic Driving Toolbox™ are used for LKA and obstacle sensing. All three scenarios presented in this section have an ego vehicle with feedback control and may include other actors, such as lead cars or ORUs (pedestrians), with predefined paths.

The path following objective is to track a desired trajectory with waypoints centered within the lane. This is done using adaptive MPC when LKA is enabled on the main road (F\ $\mathrm{\Gamma }$ ) and using waypoints tracking PI control for cruise and pure pursuit steering control at road intersections and when entering and leaving roundabouts ( $\mathrm{\Gamma })$ , where the lane pattern is typically temporarily discontinued, leading to a failure in LKA sensing. An additional time constant of 0.5 s is added between the throttle input and the vehicle. In SCA and ASASC (Algorithms 1 and 2), the lateral MPC constraints are a maximum lateral deviation in absolute value l = 0.5 m, and a maximum steering angle in absolute value ${\delta }_{\max }=$ 0.6 rad. A threshold l_c = 1.5 m is also used as a safety feature to indicate the unlikely but risky possibility of an unintended lane change. The number of waypoints is scenario dependent, with more waypoints and finer spacing at the junction areas.

Obstacle detection sensors with moderate technical specifications are only used in scenarios 2 and 3 where speed is variable. The radar sensor on the vehicle has a range of 100 m and a FoV of 19.85 degrees to detect lead vehicles at a relatively close range. The optical sensor has a larger FoV of 43.6 degrees and a range of 150 m. Those technical specifications are within the usual range for modern obstacle sensors in autonomous vehicles, see Müller ( 35 ). The sensor detections were derived for realistic simulated pedestrian motion, which was defined using the Driving Scenario Designer app.

Scenario 1: Switched Controller with Lane Keeping MPC Steering and Waypoint Steering Modes

For this scenario, the SCA (Algorithm 1) is used and simulation time of $t_f$  = 54 s. The total number of waypoints is 37, most of which are used as a backup on the main road in case LKA sensing fails or at the roundabout. In this scenario, the set $\mathrm{\Gamma }$ is the union of two intersection areas, $N_J$  = 2, between the roundabout and the two roads in Figure 1 and $N_J$  = 2. The MPC prediction horizon is 4 s and the control horizon is 0.2 s. The weights of the MPC controller are equal to 1 for lateral tracking and 0.1 for the control input. The desired speed is assumed to be fixed at 17 m.s⁻¹ (38 mph). The gains of the PI steering mode are k_p = 1, k_i = 0.02 in the steering scenario. The MPC constraints are a lateral deviation $e_y$ of +/−0.5 m, a steering command in the range [−0.6, 0.6] rad. The threshold $\epsilon =0.002$ of similarity between linear waypoint tracking and LKA MPC is low, such that LKA MPC is used on the main road (F\ $\mathrm{\Gamma })$ , where there are no lane pattern issues and MPC is more efficient in accommodating for road curvature. Figure 2 shows the times when the LKA MPC controller is used (index = 1) and when the pure pursuit waypoint tracking mode is used (index = 0). The latter corresponds to the ego vehicle entering and leaving the roundabout, from 37 s to 39 s, then from 46 s to 51 s, respectively. It can be seen in Figure 3 that the steering command remains within the steering constraints and steering angle $\delta$ reaches a maximum of ${\delta }_{\max }=$ 0.6 rad at the activation time of the waypoint tracking mode. Figure 4 shows that the lateral deviation changes sign when the curvature changes direction and remains bounded within +/−0.5 m.

Figure 1.

Comparison between waypoint tracking (a) and lane keeping assist (b) as an adaptive model predictive control (MPC) mode for Algorithm 1 on the main road: (a) vehicle path using lane centered waypoints, (b) vehicle path using switched control with lane keeping assist on the main road.

Figure 2.

Mode index for steering only control, where 1 indicates that lane keeping assist model predictive control (LKA MPC) is activated and 0 indicates that waypoint tracking is activated.

Figure 3.

Steering angle input using the switched steering controller.

Figure 4.

Lateral deviation using the switched steering controller.

In Table 1, the performance metrics show that the use of the mode switching controller with adaptive MPC and linear stabilizing modes has enhanced efficiency in both lateral integrated absolute error (IAE) ${\int }_0^{\mathrm{tf}}\left|y_e\right|\mathrm{dt}$ and control effort ${\int }_0^{\mathrm{tf}}\left|\delta \left(t\right)\right|\mathrm{dt}$ . Both controllers meet constraints but the path following performance is enhanced using the SCA. The use of waypoints improves yaw error, which is of less practical significance than the lateral position error.

Table 1.

Performance Metrics for the Switched Steering Controller Simulation with and without Lane Keeping Assist (LKA) on the Main Road

Performance metric	Switched steering controller (Steering Controller Arbitrator)	Switched steering controller with waypoint tracking (no LKA) in both MPC/PI modes
Integral of steering control effort ${\int }_0^{\mathrm{tf}}\left|\delta \left(t\right)\right|\mathrm{dt}$	3.41	5.15
Yaw IAE =  ${\int }_0^{\mathrm{tf}}\left|{\psi }_e\right|\mathrm{dt}$	3.2	2.95
Lateral IAE =  ${\int }_0^{\mathrm{tf}}\left|y_e\right|\mathrm{dt}$	17.95	21.21
Min.–max. lateral deviation $e_y$	[−0.5, 0.6]	[−0.6, 0.6]
Min.–max. yaw	[−0.5, 0.5]	[−0.5, 0.5]

Note: IAE = integrated absolute error; MPC = model predictive control; PI = proportional plus integral [control]; min. = minimum; max. = maximum.

Scenario 2: Switched Controller with Steering and Acceleration Inputs in MPC and Waypoint Tracking Modes

In this subsection, ASASC is used and speed is controlled to a target speed history, which is constant at 21 m.s⁻¹ on the main road (47 mph) or adjusted to keep a safe distance from the lead car. Desired speeds at the cross intersection consist of a linear deceleration when approaching the junction followed by a linear acceleration out of the junction. The full simulation uses 29 waypoints in total, including waypoints on the main road in case LKA sensing failed and waypoints at the junction. The additional constraints compared with scenario 1 are a prescribed time headway $t_h=1.5\mathrm{s}$ from a lead vehicle and an acceleration in the range [−3, 2] m.s⁻². The thresholds of similarity ${\epsilon }_1,{\epsilon }_2$ between steering and acceleration using waypoint tracking and LKA MPC are both equal to 0.002, such that LKA MPC is used on the main road (F\ $\mathrm{\Gamma })$ , where there are no lane pattern issues and $\mathrm{\Gamma }$ is the junction area in this example where $N_J$  = 1.

The car is shown to follow the road curvature at the center of the lane and turns at the junction where the lanes are discontinued before joining the correct road and lane and reactivating LKA. The MPC weights on longitudinal tracking, change in acceleration and steering commands are all equal to 0.1, whereas the weight on lateral error is equal to 1. The prediction horizon is 3 s, the control horizon is 0.2 s, the sampling time period is 0.1 s. The gains of the PI controllers are k_p = 1, k_i = 0.05 for steering and k_p = 1.1, k_i = 0.1 for speed control. Figure 5 shows that the ego car follows the road curvature accurately and remains centered on the lane. In Figure 6, the mode switching controller is shown to perform better when the adaptive MPC mode uses LKA sensing compared with the use of waypoints during that mode on the main road. The same waypoint tracking pure pursuit controller is used at the junction in both cases. Note that LKA simply cannot be used for comparison at junctions using the Automatic Driving Toolbox^TM. It was indeed found that LKA sensing systematically fails at junctions, which would also be the case in practice, because of lane discontinuities and lane recognition issues, even with a wide LKA sensor FoV. Using waypoints at junctions is therefore a safety improvement.

Figure 5.

Path following on curved trajectory with a junction using the switched 3DoF controller.

Figure 6.

Comparison at the intersection between lane keeping assist (a) and waypoint tracking (b) as an adaptive model predictive control (MPC) mode for Algorithm 2 on the main road.

In Figure 7, the ego vehicle remains above the prescribed safety distance using MPC while the lead vehicle is detected. The control logic switches from mode index = 1 to waypoint tracking (mode index = 0) for 1 s at time t = 31 s, when the ego vehicle reaches the junction, before LKA is enabled when the ego vehicle exits the junction. Figure 8 shows that the acceleration commands remain within the prescribed maximum acceleration limits of +/−2 m.s⁻². Figure 9 shows that gentle steering commands are used by the adaptive MPC controller mode of ASASC (Algorithm 2) compared with the steering only controller of Algorithm 1 (SCA); steering then reaches and recovers from a peak in steering but remains within prescribed steering constraints. In Figure 9, the lateral deviation remains within +/−0.5 m.

Figure 7.

Distance to lead car and safety distance.

Figure 8.

Acceleration commands using the 3DoF switched controller.

Figure 9.

Lateral deviation (a) and steering angle (b) using the 3DoF switched controller (in radian).

All controller constraints were met during the MPC stage. A linear speed deceleration and acceleration profile was used when entering and exciting road junctions. In Table 2, good tracking performance is obtained by adding speed control to the mode switching controller. The MPC LKA stage is more accurate, as predicted, in yaw and lateral control, but the performance of the controller remains acceptable in integrated control inputs and tracking error when the linear stabilizing control mode is incorporated in the full simulation.

Table 2.

Performance Metrics for the Switched Controllers with Steering and Acceleration Commands

Performance metric	ASASC controller during MPC LKA stage only before the junction (t = 0 to tf = 27.5 s)	ASASC controller during full simulation time (t = 0 to tf = 40 s)	Switched controller with waypoints tracking in both MPC/PI modes (t = 0 to t = 40 s)
Integral of steering control effort ${\int }_0^{\mathrm{tf}}\left|\delta \left(t\right)\right|\mathrm{dt}$	0.37	0.78	0.92
${\int }_0^{\mathrm{tf}}\left|{\delta }_{\tau }\left(t\right)\right|\mathrm{dt}$	15.9	26.25	34.55
Yaw IAE =  ${\int }_0^{\mathrm{tf}}\left|{\psi }_e\right|\mathrm{dt}$	0.158	0.305	0.412
Lateral IAE =  ${\int }_0^{\mathrm{tf}}\left|y\right|\mathrm{dt}$	1.3	2.9	3.41

Note: ASASC = Autonomous Steering and Acceleration Supervisory Controller; IAE = integrated absolute error; MPC LKA = model predictive control lane keeping assist; PI = proportional plus integral [control].

Scenario 3: Autonomous Braking for Traffic Tights and a Pedestrian

In this scenario, MS-ABS (Algorithm 3) is used on a straight road along the Y-axis of the driving circuit. The traffic light stop line y-position coordinate is 8 m and the initial ego car y-position is at −30 m. The traffic light is located before a road intersection and a pedestrian y-position was set at a 50 m in a collision course, after the road intersection. The pedestrian speed along the X-axis is 1.4  $\mathrm{m}.{\mathrm{s}}^{-1}$ . The number of braking stages was selected to be $n$  = 3 as it was found to be more accurate than single stage braking. The distance from the vehicle CoG to the front of the vehicle is 2.5 m. A standard sensor detection probability of 0.9 was used for both optical and radar sensors in the Driving Scenario Designer app to account for the possibility that pedestrian detection may not be systematic and immediate. The multiplicative margin k in Equation 18 was taken to be equal to 1.4 to brake safely for the pedestrian in this typical scenario where the maximum speed is 12 m.s⁻¹ (assuming a 30 mph speed limit). This speed limit dependent gain was determined empirically and was found to work successfully on other scenarios with the same speed limit and sensor characteristics. The ability to restart when the traffic lights switch to green is also demonstrated. The maximum acceleration is conservatively set at 2 m.s⁻². The effect of sensor update time period $t_s$ is evaluated by numerical simulations, assuming that both the camera and radar have the same operating frequency $f_s=\frac{1}{t_s}$ . This operating frequency is typically between 10 Hz and 20 Hz for current obstacle sensing technology (see Müller [35]) but the effect of this parameter is evaluated to assess the robustness to an unexpectedly low sensor frequency.

Initially, the traffic light is green (car running ASASC on a straight portion of the road map for simplicity). The traffic light then switches to amber, then red. The FCW time is calculated for a deceleration of 4 m.s^–2. The traffic light is amber for 3 s before switching to red for 13 s, then amber and red for 2 s before turning back to green for 15 s, which is a typical sequence.

Figure 10 shows that the autonomous braking strategy uses multiple stages for the traffic light, but quickly selects either an intermediate deceleration of 6.8  $\mathrm{m}.{\mathrm{s}}^{-2}$ if t_s = 0.1 s or t_s = 0.3 s or the highest level of deceleration available of 9.8  $\mathrm{m}.{\mathrm{s}}^{-2}$ for a degraded sensor specification of t_s = 0.5 s. The maximum 1 g deceleration level is realistic for modern vehicles equipped with anti-lock brake systems ( 36 ).

Figure 10.

Deceleration during stop–start sequences for different sensor update time periods.

Figure 11 shows that the vehicle stops at the correct (traffic light stop line) position when the traffic light switches to red, then starts successfully again when the light switches back to green. After that, the vehicle successfully stops again safely at a CoG position 3.6 m before the crossing pedestrian and avoids a collision with the pedestrian for t_s = 0.1 s and t_s = 0.3 s. The distance from the front of the vehicle is 1.1 m in both cases. The same figure also shows, however, that the collision risk is not avoided with t_s = 0.5 s with both x and y positions differences below 0.1 m. This is explained by the pedestrian being detected on time (around 23.5 s) when t_s = 0.1 s or t_s = 0.3 s, but slightly too late (approximately t = 23.8 s) with t_s = 0.5 s. The vehicle automatically restarts when the pedestrian is no longer on a collision course.

Figure 11.

Sequence with autonomous braking for traffic light then pedestrian road user for different sensor update time periods.

Discussion on the Generality of the Proposed Approach

The proposed mode switching control approach was designed under commonly used assumptions such as zero road inclination and straight roads near traffic lights. However, the approach is systematic, provided that regions are defined at road junctions for the application of the pure pursuit controller. In simulation scenarios, the number of junctions $N_J$ was either taken to be equal to one or two, but the theory extends to larger road maps (larger $N_J$ ). All mode switching control algorithms were successfully tested on several road layouts with scenarios involving different speed limits and junctions. To further automate the process, it is possible to store certain parameters, such as the gain k of Equation 19, in a lookup table to automatically change them when the speed limit changes. As future work, alternatives to the pure pursuit algorithm can be considered for the waypoints tracking at junctions to further enhance performance at the expense of simplicity. The MPC mode can also be further enhanced for higher speed driving circuits or more complex junctions. The use of artificial intelligence was beyond the scope of the paper, but it could also be considered to further improve the mode switching.

Conclusions

This paper has demonstrated uninterrupted, accurate, autonomous path following in a city environment using threshold-based mode switching control with steering and acceleration commands. The first control mode is applied on the main roads and uses LKA based on adaptive MPC for accurate path following at desired cruise speeds. The second mode uses a safer and more reactive pure pursuit inspired controller with waypoints at junctions including cross intersections and roundabouts to maneuver at a reduced velocity at these locations where LKA sensing is less reliable. An autonomous braking mode with a stop-start logic overrides either mode to stop and restart accurately at traffic lights or to avoid collisions with ORUs. Numerical simulations show that the switched controller meets the prescribed constraints on road curvature, acceleration, steering commands, time headway and lateral deviation. The proposed approach is systematic, provided that the speed limits are known and that road intersection areas are defined. This software development enhances the closed loop control demonstrations capabilities provided with the Automatic Driving Toolbox^TM by including junctions and roundabouts and only employing LKA capabilities when it is safe.