## 4 Application and Analysis
### Motion Planner Design by NMPC  
With the framework of NMPC, several motion planners designed by using SDM, the kinematic model and the single-track model are compared under different driving scenarios. In order to tackle the constraints of obstacle conveniently, vehicle models are extended with respect to the curvilinear coordinate system as shown in Figure 10 [33]:  
The NMPC motion planner is formulated as:  
\[F(x,u)=\frac{1}{2}\int_{t_{0}}^{t_{f}}\Bigl{(}\|y_{i}-y_{des}\|_{Q}^{2}+\|u(t) \|_{p}^{2}+\|\Delta u(t)\|_{k}^{2}\Bigr{)}\text{d}t \tag{13}\]  
Figure 10: Notation used in NMPC planner. Figure 9: Comparative results: (**a**) longitudinal speed; (**b**) longitudinal coupling component of lateral force; (**c**) lateral speed; (**d**) yaw rate. \[\dot{x}=\left[ \begin{array}{c}\frac{2\zeta_{yf}\cdot\left(y_{y}+l_{f}\omega- \delta_{f}y_{x}\right)\cdot\sin\left(\delta_{f}\right)}{m\overline{w}_{x}}+ \frac{\overline{r}_{T}}{m}+v_{y}\omega\\ \frac{2\zeta_{yf}\cdot\left(\delta_{f}v_{x}-v_{y}-l_{f}\omega\right)\cdot\cos \left(\delta_{f}\right)}{m\overline{w}_{x}}+\frac{2\zeta_{yf}\cdot\left(y_{y} -l_{f}\omega\right)}{m\overline{w}_{x}}-v_{x}\omega\\ \frac{2\zeta_{yf}\cdot l_{f}\cdot\left(\delta_{f}v_{x}-v_{y}-l_{f}\omega\right) \cdot\cos\left(\delta_{f}\right)}{l_{z}\overline{v}_{x}}+\frac{2\zeta_{yf} \cdot\left(v_{y}-l_{f}\omega\right)\cdot l_{f}}{l_{z}\overline{v}_{x}}\\ \frac{v_{x}\cos\left(\delta_{z}\right)-v_{y}\sin\left(\varepsilon_{2}\right)}{ l-k_{e}}\\ v_{x}\sin\left(\varepsilon_{2}\right)+v_{y}\cos\left(\varepsilon_{2}\right)\\ \omega-k\varsigma\end{array} \right] \tag{14}\]  
\[x(t_{0})=\overline{x}_{0} \tag{15}\]  
\[h_{o}(x(t),u(t))\leq 0\quad o=0,\cdots,M \tag{16}\]  
\[-\overline{u}\leq u(t)\leq\overline{u} \tag{17}\]  
\[x_{min}\leq x(t)\leq x_{max} \tag{18}\]  
where \(x(t)=\left[v_{x},v_{y},\omega,s,e_{1},e_{2}\right]\), \(u(t)=\left[F_{T},\delta_{f}\right]\), \(y(t)=\left[v_{x},e_{1},e_{2}\right]\), \(e_{1}\) and \(e_{2}\) are the lateral position error and the heading error with respect to the reference lane, \(s\) denotes the longitudinal driving distance and \(k\) is the curvature of the reference lane. The cost function (13) includes the tracking errors, the control input and its increment. The symbols \(Q\), \(P\), and \(R\) are the positive semi-definite weighting matrices. Three kinds of constraints described by (14)-(18) are considered. Equation (15) is the constraint for the initial states, (16) is the spatiotemporal elliptical constraint for obstacle avoidance, (17) and (18) limit the control input and state, respectively. \(M\) is the maximum number of obstacles. When using different vehicle models, only (14) should be replaced by the corresponding model. This optimization problem is defined over the predictive horizon \(T\) with the predictive step size \(N\). As shown in Figure 11, the simulation system composed of \"A: Perception\", \"B: Motion planning\" and \"C: Vehicle model\", which communicate with each other via a control area network [34; 35]. The virtual reality simulation software \"PreScan\" in part \"A\" is responsible for simulating the driving scenario and transmits the information of the traffic environment to \"B\". The NMPC motion planner is solved by the sequential quadratic programming (SQP) with the real-time iterative framework in part \"B\", which runs on a PC (Quad-core ARM A57 CPU, 3 GHz, and 256-core NVIDIA Pascal GPU). The outputs of the NMPC motion planner are act as the inputs of CarSim, which is a vehicle dynamics simulation software. Both the driving force \(F_{T}\) and the front tire steering angle \(\delta_{f}\) are converted into the actuator control signals. More precisely, the driving force is converted into throttle angle and braking pressure through longitudinal inverse dynamics model [36]. The steering angle of the front tire is converted into steering wheel angle through the steering coefficient. Figure 11: Block diagram of connection among modules of simulation system.