# A Strategy for Integrated Multi-Demands High-Performance Motion Planning Based on Nonlinear MPC
## 3 Hierarchical and Multi-Domains Strategy Based on Driving Demands
### Design of Constraints Set and Objective Function  
To make the planner adaptively remove the ineffective constraints and adjust the conflict items, several risk indicators are defined:  
\[\begin{gathered} I_{d}=\begin{cases}0,\;L_{dyna}\leq 0\\ 1,\;L_{dyna}>0^{\prime}\;I_{c}=\begin{cases}0,\;L_{colli}\leq 0\\ 1,\;L_{colli}>0\end{cases}\\ I_{l}=\begin{cases}0,\;L_{lane}\leq 0\\ 1,\;L_{lane}>0^{\prime}\end{cases},\;I_{r}=\begin{cases}0,\;L_{red}\leq 0\\ 1,\;L_{red}>0^{\prime}\end{cases}\\ \end{gathered}\\ \end{gathered} \tag{11}\]  
where \(I_{d},\;I_{c},\;I_{l},\;I_{r},\;I_{s}\) are parameters indicating whether the driving demands are satisfied. They indicate the dynamic stability, collision avoidance, solid lane line, red light, and max speed limit, respectively. The values of those parameters equal to 1 means having risks of violating the demands. The constraints set of the driving demands are designed as:  
\[\mathbf{I}\times\mathbf{C}\leq 0 \tag{12}\]  
where \(\mathbf{I}=\text{diag}\;(I_{d},(1-I_{d})I_{c},(1-I_{d})(1-I_{c})I_{l_{r}}\;(1-I_{ d})(1-I_{c})I_{r},(1-I_{l})(1-I_{d})(1-I_{c})\)\(I_{s})\). \(\mathbf{C}=\begin{bmatrix}C_{dyna},\;\;C_{colli},\;\;C_{lane},\;\;C_{red},\;\;C_{speed} \end{bmatrix}^{\mathcal{T}}\). \(\mathbf{C}\) is the demand function vector. \(\mathbf{I}\) is used to activate or deactivate some constraint. To avoid conflict between constraints (such as collision avoidance and lane boundary), instead of activating all constraints simultaneously, we choose to activate part of these constraints according to the working conditions. This can be achieved by defining the constraints as \(\mathbf{I}\times\mathbf{C}<0\).  
Figure 6: Illustration of variables in evaluation function of collision avoidance.  
The objective function is designed as:  
\[q_{1}f_{track}+q_{4}(1-I_{d})\Big{(}q_{2}f_{comf}+q_{3}f_{fuel}\Big{)}+q_{5}I_{d}I_ {c}C_{colli} \tag{13}\]  
where \(q_{1}\sim q_{5}\) are weights of different items. The definitions of the items in Equation (13) are consistent with those in Equations (1) and (11). When the SV is at risk of collision, the desired states for the tracking demand undergo modification. Initially, the HMD strategy assesses whether the detection points are situated within areas deemed to be at risk of collision (as illustrated in Figure 7). If all detection points fall within these collision-prone areas, the desired lateral position remains unaltered. However, the desired speed is adjusted to match that of the OV. Conversely, if any detection point is outside the risky area, the desired lateral position is realigned to the centerline of the lane corresponding to the detection point's location, as depicted in Figure 7.  
It is important to emphasize that when the side lane and the current lane of the SV are separated by a solid lane line (as depicted in Figure 7), the side lane is not considered under typical circumstances. The side lane is only taken into account in emergent situations, which are defined as instances where the SV faces the risk of collision and loss of stability. In such cases, if there are two detection points meeting the criteria, the HMD strategy gives priority to the one located in the left lane adjacent to the SV.