This section interprets a safe motion planning framework that considers RIR and uncertainty of SVs’ position. A potential field based on the value of position uncertainty of SVs is built. In addition, the road boundary is also processed. Then, multi-constraints of vehicle dynamics are described. Finally, MPC based motion planning algorithm is designed to consider these factors. The overall structure of the proposed motion planning framework is shown in Figure 3. In this paper, the AV controlled by the MPC method is also referred to as the ego vehicle.

### 3.1 Potential Field for Surrounding Vehicles with Position Uncertainties Estimation

Vehicles and pedestrians are important for AVs to make decisions, which should be modeled as non-crossable obstacles. Collision with non-crossable obstacles could lead to damage or instability of the vehicle. Even worse, it might threaten human lives. In this paper, the focus is placed on non-crossable vehicles with considering the uncertainties of SVs' position. The non-crossable potential field is motivated by Ref. [34], and we include the position uncertainty in it. Based on Eq. (9), the non-crossable potential field for SVs with position uncertainty is defined as \(NPF\):

$${NPF}_{i}\left(X,Y\right)={a}_{i}{e}^{{\beta }_{i}},$$

(10)

where

$${\beta }_{i}=-{\left\{\frac{{((X-{\overline{x} }_{s})\mathrm{cos}\vartheta +(Y-{\overline{y} }_{s})\mathrm{sin}\vartheta )}^{2}}{2{\left({L}_{xi}+\sqrt{{\chi }^{2}{\lambda }_{1}}\right)}^{2}}+\frac{{(-(X-{\overline{x} }_{s}\mathrm{sin}\vartheta +(Y-{\overline{y} }_{s})\mathrm{cos}\vartheta )}^{2}}{2{\left({L}_{yi}+\sqrt{{\chi }^{2}{\lambda }_{2}}\right)}^{2}}\right\}}^{{b}_{i}},$$

where \({a}_{i}\) and \({b}_{i}\) are intensity and shape factors of the potential field, respectively, \(i\) denotes the ID of SVs, \(e\) is the exponential function, \(\vartheta\) means the heading angle of the SVs and \({L}_{xi}\) and \({L}_{yi}\) are factors related to vehicles’ shape, respectively. Other symbols are described in Section 2.

On the other hand, according to traffic rules, the ego vehicle should follow the road marker without violating road boundaries. Furthermore, the ego vehicle cannot depart from the road marker unless the lane change or overtaking command is executed. This issue can be solved by applying the method proposed by the authors' previous work [14]. Here, the quadratic function is applied to design the road boundary potential field (\(RPF\)):

$$RP{F}_{k}(X,Y)=\left\{\begin{array}{ll}{a}_{q}({S}_{Rq}(X,Y)-{D}_{a}{)}^{2}, &{S}_{Rq}(X,Y)<{D}_{a},\\ 0, &{ S}_{Rq}\left(X,Y\right)>{D}_{a},\end{array}\right.$$

(11)

where \({a}_{q}\) is the parameter of road boundaries \(RPF\), \({S}_{Rq}\) indicates the distance from the ego vehicle to road boundaries, and \({D}_{a}\) is the safety threshold between the ego vehicle and road boundaries. Based on Eq. (11), \(RPF\) will push the ego vehicle towards the center if it departs from the center of the lane.

Therefore, overall \(APF\) includes non-crossable obstacles and road boundaries and it is denoted as:

$$APF=\sum_{i}NP{F}_{i}+\sum_{k}RP{F}_{k}.$$

(12)

It can be observed that \(APF\) is nonlinear and non-convex, which increases the required time for solving the optimization problem. However, its approximated quadratic convex problem can be solved significantly faster. Consequently, the problem ought to be converted into a quadratic convex problem to reduce the calculated time. Usually, this type of problem can be solved with the Sequential Quadratic Programming (SQP) method. The main idea is to approximate a primal nonlinear and non-convex problem to a series of convex subproblems. A detailed proof of this process can be found in Ref. [13].

### 3.2 Motion Planning Framework Design

In this section, the model predictive control method is applied for the motion planning framework design. The presented *RIR* and \(APF\) are included in the MPC controller objective to achieve road regulation and obstacle avoidance while simultaneously guaranteeing stability. First, the nonlinear system described by Eq. (1) is denoted as:

$$\dot{{\varvec{x}}}=g\left({\varvec{x}},{\varvec{u}}\right),$$

(13)

where \(g\) represents vehicle dynamics, \({\varvec{x}}\) represents state variables of the system, and \({\varvec{u}}\) is the control input.

By linearizing the system at the operating point, the linear vehicle model can be expressed as:

$$\dot{{\varvec{x}}}={\varvec{A}}{\varvec{x}}+{\varvec{B}}{\varvec{u}},$$

(14)

where \({\varvec{x}}={\left[\begin{array}{cccccccc}u& v& \dot{\varphi }& \varphi & \dot{\varnothing }& \varnothing & X& Y\end{array}\right]}^\text{T}\), \({\varvec{u}}={\left[\begin{array}{cc}{\delta }_{f}& {F}_{xT}\end{array}\right]}^\text{T}\),

$${\varvec{A}}=\left[\begin{array}{cccccccc}0& \dot{\varphi }& v& 0& 0& 0& 0& 0\\ {a}_{1}& {a}_{2}& {a}_{3}& 0& 0& 0& 0& 0\\ {b}_{1}& {b}_{2}& {b}_{3}& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ {c}_{1}& {c}_{2}& {c}_{3}& 0& -\frac{{B}_{r}}{{I}_{x}}& {c}_{4}& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ \text{cos}\varphi & -\text{sin}\varphi & 0& {d}_{1}& 0& 0& 0& 0\\ \text{sin}\varphi & \text{cos}\varphi & 0& {e}_{1}& 0& 0& 0& 0\end{array}\right],$$

$${\varvec{B}}={\left[\begin{array}{cccccccc}0& \frac{{C}_{f}}{m}& \frac{{(C}_{f}+{C}_{r})}{{I}_{z}}& 0& \frac{({C}_{f}h{m}_{s})}{{I}_{x}m}& 0& 0& 0\\ \frac{1}{m}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^\text{T},$$

\({a}_{1}=\frac{({C}_{f}+{C}_{r})}{(m{u}^{2})}v-\dot{\varphi }+\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{\left(m{u}^{2}\right)}\dot{\varphi }, \quad\)
\({a}_{2}=\frac{({C}_{f}+{C}_{r})}{(mu)}, \quad\)
\({a}_{3}=u-\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{\left(m{u}^{2}\right)}.\)

\({b}_{1}=\frac{({C}_{f}{l}_{f}-{C}_{r}{l}_{r}) }{{I}_{z}{u}^{2}}v+\frac{\left({C}_{f}{l}_{f}^{2}+{C}_{r}{l}_{r}^{2}\right) }{{I}_{z}{u}^{2}}\dot{\varphi }, \quad\)
\({b}_{2}=-\frac{\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{{I}_{z}u},{b}_{3}=-\frac{\left({C}_{f}{l}_{f}^{2}+{C}_{r}{l}_{r}^{2}\right) }{{I}_{z}u}\)

\({c}_{1}=\frac{h{m}_{s}({C}_{f}{v}_{y}+{C}_{r}{v}_{y}+{C}_{f}{l}_{f}\dot{\varphi }-{C}_{r}{l}_{r}\dot{\varphi }) }{{I}_{z}m{u}^{2}},\)
\({c}_{2}=-\frac{h{m}_{s}\left({C}_{f}+{C}_{r}\right)}{{I}_{z}mu},\)
\({c}_{3}=-\frac{h{m}_{s}\left({C}_{f}{l}_{f}-{C}_{r}{l}_{r}\right)}{{I}_{z}mu},\)
\({c}_{4}=-\frac{\left({K}_{r}-gh{m}_{s}\right)}{{I}_{x}}.\)

\({d}_{1}=-v\text{cos}\varphi -u\text{sin}\varphi ,\)
\({e}_{1}=-u\text{cos}\varphi -v\text{sin}\varphi ,\)

The outputs of the system represented by Eq. (15) are defined as lateral position \(Y\) and longitudinal velocity \(u\), which are described as follows:

$${\varvec{y}}={\varvec{C}}{\varvec{x}},$$

(15)

where \({\varvec{y}}={\left[\begin{array}{cc}Y& u\end{array}\right]}^\text{T}\), \(\varvec{C}=\left[\begin{array}{cccccccc}0& 0& 0& 0& 0& 0& 0& 1\\ 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\).

The desired outputs are denoted as:

$${{\varvec{y}}}_{{\varvec{d}}{\varvec{e}}{\varvec{s}}}={\left[\begin{array}{cc}{Y}_{des}& {u}_{des}\end{array}\right]}^\text{T}.$$

(16)

Next, the zero-order hold method is applied to discrete the system represented by Eqs. (14) and (15), which can be rewritten as:

$$\begin{array}{l}{\varvec{x}}(k+1)={{\varvec{A}}}_{d}{\varvec{x}}(k)+{{\varvec{B}}}_{d}{\varvec{u}}(k),\\ {\varvec{y}}(k)={{\varvec{C}}}_{d}{\varvec{x}}(k),\end{array}$$

(17)

where \({{\varvec{A}}}_{d}\), \({{\varvec{B}}}_{d}\) and \({{\varvec{C}}}_{d}\) means the discrete matrix of \({\varvec{A}}\), \({\varvec{B}},\) and \({\varvec{C}}\), respectively, \(k\) represents the sample time.

Moreover, vehicle dynamic constraints should also be taken into account in the motion planning model to ensure that optimized trajectory can be executed by AVs. Specifically, the values of control variables should satisfy physical constraints of the actuator capacities:

$$\begin{array}{c}\left|{\delta }_{f}\right|\le {\delta }_{max},\\ {F}_{xT}\le \frac{{T}_{max}}{{R}_{w}},\end{array}$$

(18)

where \({\delta }_{max}\) is the maximum steering angle, \({T}_{max}\) is the maximum propelling torque, and \({R}_{w}\) is the radius of wheels.

Dynamic performance of the vehicle is not only restricted by the maximum propelling torque, but also by attachment conditions between tires and roads. Here, longitudinal and lateral accelerations should satisfy the following conditions:

$$\sqrt{{a}_{x}^{2}+{a}_{y}^{2}},$$

(19)

where \({a}_{x}\) and \({a}_{y}\) represent longitudinal acceleration and lateral acceleration, respectively, and \(\mu\) is the friction coefficient between the tires and the road.

Furthermore, AVs should also obey the traffic rules. Here, the velocity of the vehicle is considered constrained within the allowable range. This constraint is represented as:

$${u}_{min}\le u\le {u}_{max},$$

(20)

where \({u}_{min}\) and \({u}_{max}\) indicate minimal and maximum allowed velocity, respectively.

Finally, the risk of rollover, potential field, as well as dynamic constraints are filled into the MPC framework:

$$\begin{array}{l}\underset{\Delta u,\varepsilon }{\mathrm{min}}\sum_{k=1}^{{N}_{p}}{(\Vert {\varvec{R}}{\varvec{I}}{\varvec{R}}\left(t+k,t\right)\Vert }_{{\varvec{Q}}}^{2}+{\Vert {\varvec{y}}\left(t+k,t\right)-{{\varvec{y}}}_{{\varvec{d}}{\varvec{e}}{\varvec{s}}}\left(t+k,t\right)\Vert }_{{\varvec{P}}}^{2}\\ +{\varvec{P}}{\varvec{F}}\left(t+k,t\right)+{\Vert {\varepsilon }_{k}\Vert }_{{\varvec{R}}}^{2})+\sum_{k=1}^{Nc}{\Vert \Delta {\varvec{u}}\left(t+k,t\right)\Vert }_{{\varvec{S}}}^{2},\\ \text{s.t.},\,{\varvec{x}}(t+k)={{\varvec{A}}}_{d}{\varvec{x}}(t+k-1)+{{\varvec{B}}}_{d}{\varvec{u}}(t+k-1),\\ {\varvec{y}}(t+k-1)={{\varvec{C}}}_{d}{\varvec{x}}(t+k-1),\\ {{\varvec{u}}}_{min}(t+k-1)\le {\varvec{u}}(t+k-1)\le {{\varvec{u}}}_{max}(t+k-1),\\ \Delta {{\varvec{u}}}_{min}(t+k-1)\le \Delta {\varvec{u}}(t+k-1)\le \Delta {{\varvec{u}}}_{max}(t+k-1),\\ {\varvec{u}}\left(t\text{+}k\right)={\varvec{u}}\left(t+k-1\right),\,\forall k\ge {N}_{c},\end{array}$$

(21)

where \({N}_{p}\) is the prediction horizon, \({N}_{c}\) is the control horizon, \(t+k\) indicates the time step, \({\varvec{x}}(t+k)\) represents the predicted state values of the system, \({\varvec{y}}(t+k)\) denotes predicted outputs of the system over prediction horizon, \({{\varvec{u}}}_{min}\) and \({{\varvec{u}}}_{max}\) are the lower and upper bounds of the actuator, and \(\boldsymbol{\Delta }{{\varvec{u}}}_{min}\) and \(\boldsymbol{\Delta }{{\varvec{u}}}_{max}\) are various ranges of control variables at each moment.

Within the cost function shown in Eq. (21), matrices \({\varvec{Q}}\), \({\varvec{P}}\), \({\varvec{R}}\), and \({\varvec{S}}\) are weight matrices corresponding to the risk of rollover, tracking of the desired path, slack variables, and violation of the control input, respectively. So far, the motion planning problem has been converted into solving the optimal control issues over the prediction horizon.