# A Strategy for Integrated Multi-Demands High-Performance Motion Planning Based on Nonlinear MPC
## 2 Problem Description  
This section initiates the formulation of the NMPC planner under the TMO strategy. Subsequently, it delves into the issue of redundancy and conflict, providing an in-depth analysis through the examination of three typical scenarios. The idea of TMO strategy is aggregating the fundamental demands into the objective function through a weighted manner, and transforming the high-priority demands function to the constraints directly. This strategy is commonly used in many papers [8; 14; 15; 16; 17]. The multi-objective NMPC planner under the TMO strategy is represented as follows:  
\[\underset{\mathbf{x},\mathbf{u}}{min}\sum_{i=0}^{N_{p}-1}\left(q_{1}f_{ track}^{i}+q_{2}f_{comf}^{i}+q_{3}f_{fuel}^{i}\right) \tag{1a}\] \[\text{s.t.}\ \ \mathbf{x}(0)=\mathbf{x}_{0}\] (1b) \[\mathbf{x}^{i+1}=f\Big{(}\mathbf{x}^{i},\mathbf{u}^{i}\Big{)},\ i=0\dots N_{p}-1\] (1c) \[\mathbf{u}_{min}\leq\mathbf{u}^{i}\leq\mathbf{u}_{max},\ i=0\dots N_{p}-1\] (1d) \[C_{dyna}^{i}<0,\ i=0\dots N_{p}-1\] (1e) \[C_{colli}^{i}<0,\ i=0\dots N_{p}-1\] (1f) \[C_{lane}^{i}<0,\ i=0\dots N_{p}-1\] (1g) \[C_{red}^{i}<0,\ i=0\dots N_{p}-1\] (1h) \[C_{speed}^{i}<0,\ i=0\dots N_{p}-1 \tag{1i}\]  
where \(N_{p}\) is the number of discretization points in prediction horizon. (1b) is the initial condition for vehicle state \(\mathbf{x}\). \(\mathbf{x}=\big{[}v_{x}\ v_{y}\ \omega\ s\ e_{1}\ e_{2}\big{]}^{T}\), in which \(v_{x},\ v_{y}\) are longitudinal and lateral speed, respectively. \(\omega\) is the angular speed. \(s,e_{1},e_{2}\) are the position coordinates in Frenet coordinates [8]. \(s\) denotes the longitudinal driving distance. \(e_{1},e_{2}\) are the lateral position and heading error with respect to the reference trajectory, respectively. (1a) is the objective function, in which \(f_{track}^{i}\), \(f_{comf}^{i}\) and \(f_{fuel}^{i}\) describe the objectives of tracking, comfort, and fuel economy at \(i\)-th time step, respectively. \(q_{1},\ q_{2}\) and \(q_{3}\) are their weights. \(f_{track}=(e_{1}-e_{1d})^{2}+e_{2}^{2}+(v_{x}-v_{xd})^{2}\), where \(e_{1d}\) and \(v_{xd}\) are the desired lateral position and longitudinal speed, respectively. \(f_{comf}\) is the sum of the square of acceleration and jerk [14]. \(f_{fuel}\) is the empirical formula of fuel consumption [21]. (1c) is the vehicle dynamics function [23]. (1d) is the physical control limits, and \(\mathbf{u}\) is the control input, including the driving force and steering angle of the front wheel. (1e)-(1i) describe the constraints of dynamic stability, collision avoidance, solid lane line, red light, and max speed limit, respectively. The specific models of these constraints are referred to [20]. To elucidate and analyze the issue of redundancy and conflict among driving demands, three typical scenarios are established, as illustrated in Figure 2. The initial states are depicted in the figure, with the red light 10 s remaining, and a maximum speed limit of 60 km/h. In the first two scenarios, the obstacle vehicles (OVs) are engaged in lane-keeping maneuvers at a constant speed. In the third scenario, OV1 abruptly changes lanes, decelerating to merge into the middle lane. In planner (1), the TMO strategy mandates collision avoidance and adherence to solid lane lines as constraints for all scenarios. However, these constraints may prove ineffective in certain situations. In the scenario depicted in Figure 2a, the solid lane line is positioned within the side lanes, while the tracking demand necessitates that the subject vehicle (SV) travels along the centerline of the middle lane. If the tracking performance is optimal, the constraint related to the solid lane line becomes redundant. Additionally, all OVs are operating within their respective lanes, rendering collision avoidance constraints unnecessary. Similarly, constraints pertaining to dynamic stability and speed limits become redundant. Conversely, in the scenario illustrated in Figure 2c, OV1 abruptly changes lanes and decelerates, necessitating a rapid reaction from the SV to avert a collision.