Automated vehicles (AVs) have become a critical link in the development of intelligent transportation systems (ITS) owing to their vast potential to enhance safety, reduce energy consumption, and optimize traffic flow [1], [2]. The general hierarchical scheme of an AV consists of three layers: ① perception layer, ② planning layer, and ③ control layer. These layers rely on sensing, network [3], processing of complex algorithms, and actuation implemented by electrical and/or electronic (E/E) systems. This study focuses on the planning and control layers of the AV system, which assumes that the perception layer works satisfactorily.

With the increase of advanced functionalities included in AVs, safety during their operational phase is of paramount importance for the road vehicles industry. However, there have been several fatal accidents involving AVs [4], [5], which underscore the importance and urgency of guaranteeing their safety. The reasons for the above accidents can be attributed to three typical safety issues faced by AVs: ① functional safety (ISO 26262) [6], ② safety of the intended functionality (SOTIF, ISO 21448) [7], and ③ cybersecurity (ISO 21434) [8]. Among these three, the SOTIF stands out as both a current academic research hotspot and an immediate challenge for AV applications. The SOTIF aims to handle potentially hazardous behaviors (PHBs), including insufficiencies or limitations related to the specifications, performance, and situational awareness, with or without reasonably foreseeable misuse, and surrounding impacts (e.g., other users, passive infrastructure, weather, and electromagnetic interference).

Given this consideration, this study deduced that the current motion planning and control techniques also suffer from issues that fall within the scope of the SOTIF. For example, uncertainties such as model mismatches will inevitably lead to control errors in the future while the planning layer does not consider the impact of these errors within the planning cycle. Moreover, AVs are affected by environmental uncertainty, and their actuators and sensors have performance limitations.

Fig. 1(a) shows an example scenario where uncertainties may lead to PHBs in automated driving. To date, however, most research on motion planning and control has not fully considered these uncertainties, and the desired trajectory is generally optimized individually by the planning layer. Therefore, as shown in Fig. 1(b), the main focus of this study is to reduce the possible hazardous/unknown areas in automated driving scenario categories. Specifically, this study generates and tracks a trajectory that can avoid obstacles and corresponding uncertainties, and mitigate unavoidable crashes.

The planning layer is the core that directly impacts driving safety. This layer is responsible for deciding the optimal driving behavior and generating a collision-free local trajectory to be followed at each instant. Heuristic approaches, including A* [9], Dijkstra [10], and rapidly exploring random tree (RRT) [11], were first applied to robot path planning. However, the heuristic approaches have difficulty generating smooth trajectories with continuous curvature. Therefore, curve-based methods, including the polynomial [12], Bezier-based [13], spline curve [14], and generation-selection-based [15] methods, were utilized to generate smooth trajectories. The quintic polynomials were proved as the jerk-optimal connections between a start state and an end state [12]. Moreover, the curve-based methods require a further assessment of the generated trajectories to select the optimal trajectory. Therefore, optimization-based methods, such as artificial potential field (PF) method [16], can be used to obtain smooth and curvature continuous trajectories. Nevertheless, the trajectories generated by the PF and gradient descent methods may converge toward local minima, resulting in rough and unexpected trajectories. Additionally, some artificial intelligence (AI)-based motion planning methods using historical data have emerged recently, such as reinforcement learning (RL) and deep learning (DL) [17]. The AI-based techniques are challenging to interpret and require high hardware costs for online applications. Considering the trade-off between computational cost and optimality, this study uses a quintic polynomial for smooth curve constructions and a PF function considering the obstacle sizes for discrete assessment.

The control layer is the core that directs the AV's actuators to follow the reference trajectories optimized by the planning layer. State-of-the-art approaches, such as dynamical game-based control [18], robust control [19], optimal control [20], RL-based control [21], and model predictive control (MPC) [22], [23], [24], are commonly applied to trajectory-tracking tasks. The main disadvantage of most of these approaches is that the AV's critical safety and actuator constraints are not explicitly considered. Moreover, robust control can be computationally intensive and behaves too conservatively, whereas RL-based control requires improved interpretability. By contrast, MPC has been extensively studied and applied to AV systems owing to its ability to systematically exploit prediction information and handle multiple-input multiple-output (MIMO) under constraints.

Nevertheless, the traditional MPC could not handle uncertainties such as model mismatches and sensor noises. Therefore, schemes considering the impact of uncertainties in future evolution were studied. The common approaches are robust MPC (RMPC) [25] with a bounded set description and stochastic MPC (SMPC) with a probabilistic description [26]. However, it should be noted that uncertainties such as model mismatches are difficult to describe using probability. Hence, we consider the RMPC more promising than the SMPC in AV control tasks.

The RMPC schemes address all possible uncertainties by considering the worst case. Min–max RMPC [27] was first proposed by solving dynamical programming (DP) at each time step; however, it is computationally expensive. Subsequently, a more efficient tube-based RMPC (TRMPC) [28] was developed to guarantee the robustness and stability of the constraint satisfaction with a lower computational cost. The TRMPC scheme creates a condition similar to the real state being tightened in a tube. Compared to the traditional MPC, the online computational burden of the TRMPC is slightly increased.

Therefore, the TRMPC scheme has been applied widely in the AV control field, such as lateral control for AVs [29], trajectory tracking for autonomous race cars [30], active safety control for four-wheel steering [31], fault-tolerant control [32], and platoon control [33].

However, traditional TRMPC also faces difficulties in practical applications. For example, the high cost of set operations often limits computation to only a few time steps, which prevents the evaluation of the uncertainties in future evolution.

Inspired by the existing literature on motion planning and control, the main contribution of this study is to propose a safe motion planning and control (SMPAC) framework for AVs to guarantee the safety of automated driving under multi-dimensional uncertainties. Fig. 2 shows the overall schematic of the study.

The core idea of SMPAC can be summarized as follows:

(1) For the control layer, we develop a flexible zonotopic TRMPC (FTMPC) controller by leveraging an efficient reachability analysis on the uncertainties. The controller can converge the future evolution of all possible uncertainties into a minimal robust positive invariant (mRPI) set.

(2) For the planning layer, we propose a concept of safety sets, which describes the geometric boundaries that ego vehicle/obstacles may reach by considering the bounded multi-dimensional uncertainties from the control layer. The safety sets guarantee that the real trajectories of the ego vehicle are always constrained in a safety tube.

Furthermore, the proposed SMPAC is a general scheme with interchangeable planning methods. Therefore, we believe that SMPAC has potential applications in other robotic systems, including but not limited to mobile robots and unmanned underwater vehicles.

The remainder of this study is presented as follows. Section 2 introduces the preliminaries for the set operations and zonotopes. In Section 3, the dynamic model with uncertainties is developed, and the FTMPC scheme is proposed. Section 4 presents the motion planning approach with safety sets to cope with multi-uncertainties. The co-simulations (MATLAB/Simulink, R2021b, MathWorks, USA; CarSim, 2019 version, Mechanical Simulation Corporation, USA) and hardware-in-the-loop experiments (HIL) performed to validate the proposed SMPAC are described in Section 5. Finally, the conclusions are summarized in Section 6.

This study revolves around a set theory, hence the following three basic set operations are introduced first.

For a linear matrix $\mathit{L}\in {\mathbb{R}}^{n_l\times n}$, sets ${\mathbb{S}}_1,{\mathbb{S}}_2\subset {\mathbb{R}}^n$, and elements $s_1,s_2\in {\mathbb{R}}^n$,

Linear map:

Minkowski sum:

Pontryagin difference:

A bounded polyhedral set is a polytope. The zonotope shown in Fig. 3 (a) is a class of centrally-symmetric polytopes.

The zonotope is also an affine transformation of the hypercube. An n-order zonotope ( $\mathbb{Z}\subset {\mathbb{R}}^n$) transformed from an s-order hypercube ( ${\mathbb{B}}^s\subset {\mathbb{R}}^s$) is described by

where $\mathit{c}\in {\mathbb{R}}^n$ is the center and $\mathit{G}=\left[{\mathit{g}}_1,{\mathit{g}}_2,\dots ,{\mathit{g}}_s\right]\in {\mathbb{R}}^{n\times s}$ is the generator. In this study, the zonotope is abbreviated as $\mathbb{Z}=〈\mathit{c},\mathit{G}〉$.

Moreover, zonotopes hold the following essential properties.

For ${\mathit{c}}_1,{\mathit{c}}_2\in {\mathbb{R}}^n,{\mathit{G}}_1,{\mathit{G}}_2\in {\mathbb{R}}^{n\times s}$, the linear map and Minkowski sum of two zonotopes are computed by

The Frobenius norm of the generator denotes the size of the zonotope $\mathbb{Z}=〈c,G〉$:

The interval hull $\square \mathbb{Z}$ of a zonotope $\mathbb{Z}$ is computed by

where $\text{r}\mathrm{s}\left(\mathit{G}\right)$ a diagonal matrix such that $\mathrm{rs}{\left(\mathit{G}\right)}_{\mathit{ii}}={\sum }_{j=1}^s\left|{\mathit{G}}_{\mathit{ij}}\right|$, $i=1,\dots ,n$. Fig. 3(b) illustrates the geometric interpretation of the interval hull $\square \mathbb{Z}$.

The P-radius of a zonotope $\mathbb{Z}$ is defined by

where $\mathit{P}={\mathit{P}}^{\mathrm{T}}\succ 0$ is a symmetric and positive definite matrix. $\mathit{P}$ is set as the identity matrix in this study, such that $\sqrt{P_{\text{r}}\left(\mathbb{Z}\right)}$ represents the Euclidean distance.

Furthermore, all defined sets in this study are represented by zonotopes. The main reasons are summarized as follows:

(1) Zonotopes guarantee that all sets defined in this study are compact, convex, and contain the origin in their interior.

(2) The zonotopic set operations in Eqs. (5), (6) enable an exact, fast, and visual exploration of the system evolution in the FTMPC scheme.

(3) Table 1 illustrates that the zonotopes provide higher accuracy than ellipsoids while having lower time complexity than polytopes [34].

The FTMPC controller for uncertainties is introduced in this section. The process includes modeling the AV and uncertainties, determining known constraints, evaluating the evolution of the uncertain system, and developing a varying zonotopic tube.

Considering the trade-off between the model accuracy and computational cost, a simplified vehicle model consisting of a 1-degree of freedom (1-DOF) longitudinal model and a 2-DOF bicycle model is used under certain assumptions [35]. Fig. 4 depicts the vehicle model with the longitudinal, lateral, and yaw dynamics governed by

where $\mathit{XOY}$ is the inertial coordinate system for the ground and $\mathit{xoy}$ is the vehicle coordinate system. For the $\mathit{xoy}$ body-fixed coordinates, $m$ is the vehicle mass, $l_{\text{f}}$ and $l_{\text{r}}$ are the front and rear wheelbase, $I_z$ is the moment of inertia through the center of gravity about the yaw axis, $v_x$ and $v_y$ are the vehicle's longitudinal and lateral velocities, $\psi$ is the vehicle heading angle, $\delta$ is the steering angle of the front wheels, ${\mathit{F}}_{\mathit{x}\mathrm{T}}$ is the total longitudinal tire force, and ${\mathit{F}}_{\mathit{xi}}$ and ${\mathit{F}}_{\mathit{yi}}$ ( $i=\left\{\text{f},\text{r}\right\}$) are the longitudinal and lateral forces of the front/rear tires, respectively.

Assuming that the lateral acceleration $a_y$ and vehicle sideslip angle $\beta$ are small, and the vertical tire load is constant, for simplicity, the lateral tire forces ${\mathit{F}}_{\mathrm{yf}}$ and ${\mathit{F}}_{\mathit{y}\mathrm{r}}$ are considered to be linear with the tire slip angles ${\alpha }_{\text{f}}\left(\mathrm{f}\mathrm{r}\mathrm{o}\mathrm{n}\mathrm{t}\mathrm{t}\mathrm{i}\mathrm{r}\mathrm{e}\right)$ and ${\alpha }_{\text{r}}$ (rear tire):

where $c_{\text{f}}$ and $c_{\text{r}}$ are the corner-stiffnesses of the front and rear tires, respectively.

Leveraging an error model is a more effective approach for tracking a pre-planned trajectory. As shown in Fig. 5, $e_v$, $e_y$, and $e_{\psi }$ are the tracking errors of the longitudinal velocity, lateral distance, and heading angle with respect to the reference trajectory, respectively. According to the kinematic relationship, ${\dot{e}}_v$, ${\dot{e}}_y$, ${\ddot{e}}_y$, ${\dot{e}}_{\psi }$, and ${\ddot{e}}_{\psi }$ are defined as

where ${\dot{v}}_{x,\mathrm{des}}=v_{y,\mathrm{des}}{\dot{\psi }}_{\mathrm{des}}+a_{x,\mathrm{des}}$, $v_{y,\mathrm{des}}$ is the desired lateral velocity, $a_{x,\mathrm{des}}$ is the desired longitudinal acceleration, ${\dot{\psi }}_{\mathrm{des}}=\frac{v_x}{R}$ is the desired yaw rate, and $R$ is the radius of the reference trajectory.

According to the state variable dynamics in Eqs. (10), (11), as well as the tracking error expression in Eq. (12), the continuous real AV model can be described as follows

where ${\mathit{x}}_e={\left[e_v,e_y,{\dot{e}}_y,e_{\psi },{\dot{e}}_{\psi }\right]}^{\mathrm{T}}$ is the state, ${\mathit{u}}_e={\left[F_{\mathit{x}\mathrm{T}},\delta \right]}^{\mathrm{T}}$ is the control input, and ${\mathit{w}}_{\mathrm{des}}={\left[{\dot{v}}_{x,\mathrm{des}},{\dot{\psi }}_{\mathrm{des}}\right]}^{\mathrm{T}}$ denotes the information from the reference trajectory. The additive disturbance ${\mathit{w}}_{\mathrm{c}}$ generalizes the internal and external uncertainties caused by all the adverse factors. Here, the internal uncertainties mainly include the coupled dynamics of longitudinal and lateral (the $v_y\dot{\psi }$ term in Eq. (12)) time-varying parameters, and unmodeled dynamics. The external uncertainties mainly include sensor noises and environmental disturbances. The disturbance matrix ${\mathit{B}}_3$ is determined under scenarios encompassed by the operational design domain (ODD) of automated driving.

The presence of the ${\mathit{B}}_2{\mathit{w}}_{\mathrm{des}}$ term in Eq. (13) prevents the tracking errors from all converging to zero. Hence a feedforward term of the control inputs is designed to ensure a zero steady-state error. The steady-state ${\mathit{x}}_{\mathrm{ss}}$ and steady-input ${\mathit{u}}_{\mathrm{ss}}$ are obtained by the final value theorem,

where $\rho =\frac{1}{R}$ is the reference curvature, $l=l_{\mathrm{f}}+l_{\text{r}}$ is the wheelbase, and $K_V=\frac{l_{\text{r}}m}{c_{\text{f}}l}-\frac{l_{\text{f}}m}{c_{\text{r}}l}$ is the understeer gradient.

Substitute Eq. (16) into Eq. (13) and define $\mathit{x}={\mathit{x}}_e-{\mathit{x}}_{\mathrm{ss}}$, ${\mathit{u}}_{\mathrm{\Delta }}={\mathit{u}}_e-{\mathit{u}}_{\mathrm{ss}}$. The zero steady-state error model can be described by

with the full-output $\mathit{y}={\mathit{C}}_{\mathrm{c}}\mathit{x}$, ${\mathit{C}}_{\mathrm{c}}={\mathit{I}}_{n_x}$, ${\mathit{I}}_{n_x}$ is an identity matrix.

By the zero-order hold method, the discrete real AV system to be controlled can be expressed as

with the output ${\mathit{y}}_k=\mathit{C}{\mathit{x}}_k$, where ${\mathit{x}}_k\in {\mathbb{R}}^{n_x}$, ${\mathit{u}}_k\in {\mathbb{R}}^{n_u}$, $\mathit{A}=e^{{\mathit{A}}_{\mathrm{c}}{T}_{\text{s}}}\in {\mathbb{R}}^{n_x\times n_x}$, $\mathit{B}={\int }_0^{T_{\mathrm{s}}}{e}^{{\mathit{A}}_{\mathrm{c}}{T}_{\text{s}}}\mathrm{d}t{\mathit{B}}_1={\sum }_{k=0}^{\infty }\frac{{\left({\mathit{A}}_{\mathrm{c}}\right)}^k{\left(T_{\text{s}}\right)}^{k+1}}{\left(k+1\right)!}{\mathit{B}}_1\in {\mathbb{R}}^{n_x\times n_u}$, $\mathit{w}={\int }_0^{T_{\mathrm{s}}}{e}^{{\mathit{A}}_{\mathrm{c}}{T}_{\text{s}}}\mathrm{d}t{\mathit{B}}_3{\mathit{w}}_{\mathrm{c}}\in {\mathbb{R}}^{n_w}$, $\mathit{C}={\mathit{C}}_c\in {\mathbb{R}}^{n_y\times n_x}$, $T_{\text{s}}$ is the sample time.

Moreover, the real AV system in Eq. (18) is subject to zonotopic hard constraints, that is, $\forall k\in N^{+}$, ${\mathit{x}}_k\in \mathbb{X}=〈\mathbf{0},{\mathit{G}}_x〉\subseteq {\mathbb{R}}^{n_x}$, ${\mathit{u}}_k\in \mathbb{U}=〈\mathbf{0},{\mathit{G}}_u〉\subseteq {\mathbb{R}}^{n_u}$, and $\mathit{w}\in \mathbb{W}=〈\mathbf{0},{\mathit{G}}_w〉\subseteq {\mathbb{R}}^{n_w}$. The zonotope generators ${\mathit{G}}_x\in {\mathbb{R}}^{n_x\times n_a}$ and ${\mathit{G}}_u\in {\mathbb{R}}^{n_u\times n_b}$ are known matrices determined by the characteristics of the AV system, which include the following aspects:

Trajectory tracking constraints:

where $r_w$ is the lane width, $r_v$ is the width of the ego vehicle, $e_v^{\max }$ and $e_{\psi }^{\max }$ are the maximum values of the velocity error ( $e_v$) and heading angle error ( $e_y$), respectively.

Handling stability constraints: The vehicle sideslip angle $\beta$ has the empirical constrained boundary and the yaw rate $\dot{\psi }$ has a limit:

where the tire-ground adhesion coefficient $\mu$ (lateral for ${\mu }_y$) is assumed to be known for the controller, and $g$ is the gravity acceleration.

Actuator constraints: The control inputs $F_{\mathrm{xT}}$, $\delta$ and their increments $\mathrm{\Delta }{F}_{\mathit{xT}}$, and $\mathrm{\Delta }\delta$ have boundaries:

Furthermore, the disturbance $\mathit{w}$ consists of the internal disturbance ${\mathit{w}}_{\mathrm{in}}\in {\mathbb{W}}_{\mathrm{in}}$ and the external disturbance ${\mathit{w}}_{\mathrm{ex}}\in {\mathbb{W}}_{\mathrm{ex}}$, that is, $\mathbb{W}={\mathbb{W}}_{\mathrm{in}}\oplus {\mathbb{W}}_{\mathrm{ex}}$. Here, ${\mathit{w}}_{\mathrm{in}}$ can be determined by comparing the responses of the real model (CarSim model) and the nominal model in Eq. (22) within the ODD.

Figs. 6 (a)–(c) show the visual projection of ${\mathbb{W}}_{\mathrm{in}}$ in three-dimensional (3D) and two-dimensional (2D) views.

Assuming that the ${\mathit{w}}_{\mathrm{ex}}$ (e.g., sensor noise boundary) is known, the generator ${\mathit{G}}_w\in {\mathbb{R}}^{n_x\times n_c}$ of the $\mathit{w}$ can be determined. Fig. 6 (d) shows the visual relationship of $\mathbb{W}$, ${\mathbb{W}}_{\mathrm{in}}$, and ${\mathbb{W}}_{\mathrm{ex}}$.

The discrete-time real system in Eq. (18) satisfies the finite time controllability condition [36]. In this section, the real system is divided into a nominal system and an uncertain system, which are handled separately.

We extract the deterministic part of the real system in Eq. (18), and the nominal model without considering the disturbance $\mathit{w}$ is defined as

where all top marks $\widehat{a}$ represent the corresponding nominal variables. The nominal control input ${\widehat{\mathit{u}}}_k$ is obtained by solving an MPC optimization problem for the nominal dynamics.

We remove the deterministic part (Eq. (22)) from the real system in Eq. (18), and the uncertain model composed of the disturbance $\mathit{w}$ can be obtained,

where ${\stackrel{\sim }{\mathit{x}}}_k={\mathit{x}}_k-{\widehat{\mathit{x}}}_k$, ${\stackrel{\mathit{\sim }}{\mathit{u}}}_k={\mathit{u}}_k-{\widehat{\mathit{u}}}_k$, and all top marks $\stackrel{\sim }{a}$ represent the corresponding uncertain variables. The state feedback gain solved by the linear quadratic regulator (LQR) is used to guarantee the stability of the uncertain system, that is, $\mathit{K}=\mathrm{LQR}\left(\mathit{A},\mathit{B}\right)$, ${\stackrel{\sim }{\mathit{u}}}_k=\mathit{K}{\stackrel{\sim }{\mathit{x}}}_k$. The FTMPC control law applied to the real system in Eq. (18) is then expressed as

Hence, the uncertain system in Eq. (23) is rewritten as a stable autonomous system:

where the feedback gain matrix $\mathit{K}$ makes ${\mathit{A}}^{\prime }=\mathit{A}+\mathit{B}\mathit{K}\in {\mathbb{R}}^{n_x\times n_x}$ Schur stable.

The future evolution law of the uncertain system is evaluated by leveraging the reachability analysis.

According to the zonotope properties in Eqs. (5), (6), the evolution of the uncertain system (Eq. (25)), ${\stackrel{\sim }{\mathit{x}}}_{k+h\mid k}\in {\stackrel{\sim }{\mathbb{X}}}_{k+h\mid k}$, ${\stackrel{\sim }{\mathit{u}}}_{k+h\mid k}\in {\stackrel{\sim }{\mathbb{U}}}_{k+h\mid k}$, $\mathbf{0}$ refer to the zero matrix) with time steps $h\in N^{+}$ is revealed as:

Proposition 1. The control law in Eq. (24) stabilizes the uncertain system in Eq. (23).

Proof 1. Fig. 7(a) draws all zonotopes of ${\stackrel{\sim }{\mathbb{X}}}_{k+h\mid k}$ in the finite horizon ( $h=1000$) with CORA [37] to visualize the evolution of the uncertain system in Eq. (23). In Fig. 7(a), the boundary lines of the zonotopes become denser as the time steps increase.

Moreover, based on the definition of the zonotope size in Eq. (7), the size change rate of the uncertain state constraints ${\stackrel{\sim }{\mathbb{X}}}_{k+h|k}$ is defined as

Fig. 7(b) shows that the $R_{k+h}^{\mathrm{size}}$ decreases with the time steps rapidly, which indicates that the difference between two adjacent zonotopes will quickly converge to zero.

Consequently, owing to the Schur matrix ${\mathit{A}}^{\prime }$, the uncertain states constraints ${\stackrel{\sim }{\mathbb{X}}}_{k+h|k}$ in Eq. (26) will converge rapidly in a bounded set ( $\text{mRPI}\subseteq {\mathbb{R}}^{n_x}$). Hence, the $\mathit{K}{\stackrel{\mathbf{\sim }}{\mathit{x}}}_k$ term in the control law (Eq. (24)) stabilizes the uncertain system in Eq. (23).

According to the nominal model in Eq. (22), the prediction model of the nominal system over the prediction horizon $N_{\mathrm{p}}$ can be expressed as

where $N_{\mathrm{c}}$ is the control horizon, ${\mathrm{\Theta }}_k\in {\mathbb{R}}^{\left(n_x\times N_{\mathrm{p}}\right)\times \left(n_u\times N_{\mathrm{c}}\right)}$,

with the evolution of the ${\stackrel{\sim }{\mathbb{X}}}_{k+i|k}$ and ${\stackrel{\sim }{\mathbb{U}}}_{k+i|k}$, it yields that the MPC optimization problem for the nominal dynamics (nominal MPC) is subject to two tightened constraint sets, that is, ${\widehat{\mathbb{X}}}_k=\mathbb{X}\ominus {\stackrel{\sim }{\mathbb{X}}}_k$, ${\widehat{\mathbb{U}}}_k=\mathbb{U}\ominus {\stackrel{\sim }{\mathbb{U}}}_k=\mathbb{U}\ominus \mathit{K}{\stackrel{\sim }{\mathbb{X}}}_k$.

Remark 1. According to Eq. (26), as the disturbance set $\mathbb{W}$ increases, the uncertain constraint ${\stackrel{\sim }{\mathbb{X}}}_k$ increases, and the controller conservatism strengthens. If the disturbance set $\mathbb{W}$ acts too significant, either $\mathbb{X}\ominus {\stackrel{\sim }{\mathbb{X}}}_k$ or $\mathbb{U}\ominus \mathit{K}{\stackrel{\sim }{\mathbb{X}}}_k$ will be an empty set, rendering the controller unsolvable. For the $\mathbb{W}$ determined in subsection 3.2, the controller is sufficient.

To increase the quadratic programming (QP) feasible region of the nominal MPC and reduce conservatism, a flexible tube with varying cross-sections is designed at a negligible increase in computational cost [29]. The state and control tubes are parameterized with matrices ${\widehat{\mathit{G}}}_k^x$ and ${\widehat{\mathit{G}}}_k^u$ as

where ${\widehat{\mathbb{X}}}_{k+N_{\mathrm{p}}|k}=\mathbb{X}\ominus {\stackrel{\sim }{\mathbb{X}}}_{k+N_{\mathrm{p}}|k}=〈\mathbf{0},{\widehat{\mathit{G}}}_{k+N_{\mathrm{p}}|k}^x〉,{\widehat{\mathbb{U}}}_{k+N_{\mathrm{c}}|k}=\mathbb{U}\ominus \mathit{K}{\stackrel{\mathbf{\sim }}{\mathbb{X}}}_{k+N_{\mathrm{c}}|k}=〈\mathbf{0},{\widehat{\mathit{G}}}_{k+N_{\mathrm{c}}|k}^u〉$.

The final optimization of the nominal MPC is expressed as

where $\rho \in {\mathbb{R}}^{n_{\epsilon }\times n_{\epsilon }}$ is the weight matrix of slack $\epsilon$, ${\mathit{Q}}_{\mathrm{c}}\in {\mathbb{R}}^{\left(n_x\times N_p\right)\times \left(n_x\times N_p\right)}$, and ${\mathit{R}}_{\mathrm{c}}\in {\mathbb{R}}^{\left(n_u\times N_{\mathrm{c}}\right)\times \left(n_u\times N_{\mathrm{c}}\right)}$ are the weight matrices. Additionally, the final zonotopic constraint can be transformed to half-space polytopic constraints by Eq. (4) to make the QP optimization faster and more efficient [38].

Proposition 2. The control law in Eq. (24) stabilizes the nominal system in Eq. (22).

Proof 2. The nominal system in Eq. (22) has been proved to be exponentially stable under the assumption that the initial nominal state ${\widehat{\mathit{x}}}_t$ at each sample time ( $t>0$) is optimized as a parameter of the control law [28]. The optimization problem is subject to a significant constraint of ${\mathit{x}}_t\in {\widehat{\mathit{x}}}_t\oplus \text{mRPI}$, that is, the optimal initial nominal state ${\widehat{\mathit{x}}}_t^{*}$ and optimal control sequence ${\widehat{\mathbf{U}}}_t^{\ast }$ at time $t$ are computed by

In this study, ${\widehat{\mathit{x}}}_t$ is derived from the real state and control input of the previous time step to measure the uncertain state ${\stackrel{\mathbf{\sim }}{\mathit{x}}}_t$ and determine the internal disturbance set in subsection 3.2, that is, ${\widehat{\mathit{x}}}_t=\mathit{A}{\mathit{x}}_{t-1}+\mathit{B}{\mathit{u}}_{t-1}$. The uncertain state ${\stackrel{\mathbf{\sim }}{\mathit{x}}}_t$ at time $t$ satisfies that ${\stackrel{\sim }{\mathit{x}}}_t={\mathit{x}}_t-{\widehat{\mathit{x}}}_t=\mathit{w}\in \mathbb{W}$. The real state ${\mathit{x}}_t$ at time $t$ also satisfies the above significant constraint, that is, ${\mathit{x}}_t={\widehat{\mathit{x}}}_t+\mathit{w}\in {\widehat{\mathit{x}}}_t\oplus \mathbb{W}\subset {\widehat{\mathit{x}}}_k\oplus \text{mRPI}$. Hence the nominal MPC term (Eq. (32)) in the control law (Eq. (24)) stabilizes the nominal system in Eq. (22).

This section introduces a motion planning algorithm that considers uncertainties in the Frenet Frame. The process consists of safety sets, trajectory generation, ranking, and collision detection.

Because the uncertain system evolution is demonstrated by reachability analysis in subsection 3.3, the mRPI can effectively describe all possible uncertainties caused by the tracking layer of the ego vehicle. Moreover, the uncertain areas of the mRPI may cause a potential collision. Therefore, a concept of safety sets is proposed to describe the geometric boundaries that the ego vehicle and obstacles may reach. The safety sets will be developed and applied in subsequent planning sections to guarantee motion planning security.

Assuming that the effects of the acceleration variables are ignored, the safety set of the ego vehicle mainly considers the shape, location, and heading angle $\psi$.

(1) Shape and location: The ego vehicle's shape and location set ${\mathbb{V}}_{\mathrm{sl}}$ consists of the physical dimension set ${\mathbb{V}}_{\mathrm{pd}}$ and the uncertainty of the tracking errors $\mathrm{m}\mathrm{R}\mathrm{P}{\mathrm{I}}_{12}$.

For the former ${\mathbb{V}}_{\mathrm{pd}}$, the zonotope shown in Fig. 8(a) is used to enclose the physical dimension of the stationary ego vehicle. This zonotope is defined as the physical dimension set ${\mathbb{V}}_{\mathrm{pd}}\subseteq {\mathbb{R}}^2$, which can effectively represent the original shape and location of the ego vehicle.

For the latter, the vehicle tracking errors are constrained by the first and second dimensions of the mRPI set, a result emanating from the introduced FTMPC in Section 3. Accordingly, $\mathrm{m}\mathrm{R}\mathrm{P}{\mathrm{I}}_{12}$ is defined as the reachable set of the ego vehicle’s longitudinal and lateral tracking errors. The reachable set can be computed within a finite safety horizon $N_{\mathrm{s}\mathrm{a}\mathrm{f}\mathrm{e}}$ or an infinite horizon based on the evolution of the uncertain system.

Therefore, ${\mathbb{V}}_{\mathrm{sl}}={\mathbb{V}}_{\mathrm{pd}}\oplus \mathrm{m}\mathrm{R}\mathrm{P}{\mathrm{I}}_{12}=〈{\mathit{c}}_e,{\mathit{G}}_{\mathrm{sl}}〉\subseteq {\mathbb{R}}^2$. Fig. 8 (b) shows the visual relationship of the $\mathrm{m}\mathrm{R}\mathrm{P}{\mathrm{I}}_{12}$, the original vehicle set ${\mathbb{V}}_{\mathrm{pd}}$, and the shape and location set ${\mathbb{V}}_{\mathrm{sl}}$.

(2) Heading angle $\psi$: The heading angle should be considered in the ego vehicle set when the ego vehicle is driving. Therefore, a rotation matrix is introduced to modify the generator ${\mathit{G}}_{\mathrm{sl}}$ of the ${\mathbb{V}}_{\mathrm{sl}}$:

As shown in Fig. 9 (a), the rotation function ${\mathit{R}}_m\left(\psi \right)$ should modify the generator of ${\mathbb{V}}_{\mathrm{sl}}$ without changing its center. Thus, the ego vehicle set with a heading angle $\psi$ can be represented by the matrix multiplication of the rotation matrix ${\mathit{R}}_m\left(\psi \right)$ and the generator ${\mathit{G}}_{\mathrm{sl}}$, that is, $〈{\mathit{c}}_e,{\mathit{R}}_m\left(\psi \right){\mathit{G}}_{\mathrm{sl}}〉$.

>

Remark 2. Moreover, the heading angle error $e_{\psi }$ is constrained by the fourth dimension of the mRPI, that is, $e_{\psi }^{\min }\le e_{\psi }\le e_{\psi }^{\max }$. Therefore, the ego vehicle set with the heading angle error ${\mathbb{V}}_{e_{\psi }}$ can be simplified as the union of ${\mathbb{V}}_{e_{\psi }}^{\min }$ and ${\mathbb{V}}_{e_{\psi }}^{\max }$:

Finally, ${\mathbb{V}}_{e_{\psi }}$ is reduced to an interval hull $\square {\mathbb{V}}_{e_{\psi }}$ and $\square {\mathbb{V}}_{e_{\psi }}$ is rotated with the heading angle $\psi$. The ego vehicle set ${\mathbb{V}}_s$ is obtained and shown in Fig. 9 (b).

The observation (or prediction) of the $i$th ( $i=1,\cdots ,n_o$) obstacle ( $\mathrm{o}\mathrm{b}{\mathrm{s}}^i$) by the ego vehicle has a bounded error [39], which can be described by an obstacle error set ${\mathbb{O}}_e^i\subseteq {\mathbb{R}}^{n_x}$. Similar to the process of the ego vehicle set, we first define a physical dimension set ${\mathbb{O}}_{\mathrm{pd}}^i\subseteq {\mathbb{R}}^2$ for the $i$th obstacle. Afterward, the $i$th obstacle set with uncertainties ${\mathbb{O}}_s^i\subseteq {\mathbb{R}}^2$ can be computed with the obstacle physical dimension set ${\mathbb{O}}_{\mathrm{in}}^i$, obstacle error set ${\mathbb{O}}_e^i$, and obstacle heading angle ${\psi }_o^i$.

In the Frenet coordinate system, the $s$-axis is set as the centerline of the rightmost lane, and the $d$-axis is perpendicular to the $s$-axis. The origin is merged with the origin of the Cartesian coordinate system and moves along the $s$-axis with the ego vehicle.

The quintic polynomial is proved as the jerk-optimal connection between the start state at $t_0$ and the end state at $t_1$ [12]. Therefore, the motions in the $s$ and $d$ directions are described by quartic and quintic polynomials:

where $a_{0\sim 4}$ and $b_{0\sim 5}$ are undetermined coefficients, and $t$ is the time. The start and end states are expressed as

In addition, all the start states are set to be the same as the final vehicle state of the last control cycle to ensure continuity. The end states $\ddot{s}\left(t_1\right)$, $\dot{d}\left(t_1\right)$, and $\ddot{d}\left(t_1\right)$ are set to zero to ensure vehicle stability. Two sequences of the end velocity $\dot{s}\left(t_1\right)$ and the end $d$ position $d\left(t_1\right)$ are designed to ensure that the candidate trajectories can cover all lanes. The following matrix equation is then obtained by taking an orthogonal combination of the two sequences of $\dot{s}\left(t_1\right)$ and $d\left(t_1\right)$:

where ${\mathit{T}}_1$, ${\mathit{P}}_1$, ${\mathit{S}}_1$ and ${\mathit{T}}_2$, ${\mathit{P}}_2$, ${\mathit{S}}_2$ are presented in (Appendix A), respectively. The $p$th ( $p=1,2,\cdots ,n_s$) column of the matrix ${\mathit{P}}_1$ corresponds to the polynomial parameters of $s\left(t\right)$ under the condition of $\dot{s}{\left(t_1\right)}^{\left(p\right)}$, and $0\le \dot{s}{\left(t_1\right)}^{\left(p\right)}\le \dot{s}{\left(t_1\right)}^{\max }$. The average acceleration $\frac{\dot{s}{\left(t_1\right)}^{\left(p\right)}-\dot{s}\left(t_0\right)}{t_1-t_0}$ should satisfy comfortable acceleration limits. The $q$th ( $q=1,2,\cdots ,n_d$) column of the matrix ${\mathit{P}}_2$ corresponds to the polynomial parameters of $d\left(t\right)$ under the condition of $d{\left(t_1\right)}^{\left(q\right)}$, and $d{\left(t_1\right)}^{\left(q\right)}$ should not exceed the road boundary.

As shown in Fig. 10, a cluster of the $n_t$ candidate trajectories can be obtained by solving ${\mathit{P}}_1$ and ${\mathit{P}}_2$, where $n_t=n_s\times n_d$.

The candidate trajectories are discretely sampled and then ranked for risk assessment ( $J_{\mathrm{risk}}$), ride comfort ( $J_{\mathrm{com}}$), and trajectory stability ( $J_{\mathrm{sta}}$). The total cost function ( $J_{\mathrm{total}}$) is defined as

where $k_{\mathrm{risk}}$, $k_{\mathrm{com}}$, and $k_{\mathrm{sta}}$ are the corresponding weights.

The risk assessment index $J_{\mathrm{risk}}$ is considered from two perspectives of road boundaries (crossable or non-crossable lane lines) and obstacles of various sizes (e.g., other vehicles, accidentally dropped cargo).

(1) Road boundary description: The periodic PF of the road boundaries $U_{\mathrm{road}}$ is generated by a trigonometric function to deal with multi-lane roads. The $U_{\mathrm{road}}\in \left[0,1\right]$, and can be written as

where $s$ and $d$ are the Frenet coordinates of the ego vehicle, $r_{\mathrm{w}}$ is the lane width, $R_A\left(d\right)$ is the amplitude varying with $d$, $p_m\in \left[0,1\right]$ denotes the risk value at the crossable edge lines, and $d_{\mathrm{cn}}$ is the $d$ coordinate of the leftmost lane centerline. Fig. 11 shows the PF of the road.

(2) Obstacle description with the obstacle set ${\mathbb{O}}_s^i$: The $i$ th obstacle PF shown in Figs. 12(a) and (b) is generated by an exponential function. In which, the obstacle PF in Eq. (41) mainly considers the relative velocity ${\mathrm{\Delta }}_v\left(i\right)$ and the obstacle size.

$U_{\mathrm{obs}}\left(i\right)\in \left[0,1\right]$, and can be written as