A Simplified Vehicle Dynamics Model for Motion Planner Designed by Nonlinear Model Predictive Control

Abstract

Motion planning by considering it as an optimal problem is an effective and widely applicable method. Its comprehensive performance greatly depends on the vehicle dynamics model, which is highly coupled and nonlinear, especially under the dynamical scenarios and causes much more consumption of computation resources for the numerical optimization. To increase the real time performance of the motion planner designed by nonlinear model predictive control (NMPC), a unified and simplified vehicle dynamics model (SDM) is presented to make a balance between the accuracy and complexity for dynamical driving scenarios. Based on the statistical analysis results of naturalistic driving conditions, a unified nonlinear vehicle dynamics model is set up, which considers the tyre cornering characteristic and is also applicable to conditions with large turning angle. After the validation of this coupled dynamics model (CDM) by comparisons with other widely used models under a variety of conditions, the coupling effect is analyzed according to the transfer functions, which are obtained by linearizing CDM at equilibrium points. Furthermore, SDM is derived by ignoring the weak part of the coupling effect. The accuracy of SDM is validated by several comparative studies with other models and it is further applied to design a motion planner by NMPC to validate its contribution on the performance improvement under dynamical driving conditions.

1. Introduction

The growing of transportation demand has brought great challenges to road safety and efficiency. Automatic driving is regarded as the most promising solution due to its potential to replace human driver by an intelligent system [1,2]. As a key part of the automatic driving system, motion planner has been designed by several types of methods including artificial potential field (APF) [3,4], sampling-based method [5,6] and optimal control method [7,8].

The APF generates the control force by the gradient of the potential field composed of the target’s attraction and the repulsion energy of obstacles. The advantage of APF is the low cost of computation, but it can hardly consider the future driving performance and the constraints of vehicle dynamics. The fundamental of the sampling-based method is to convert the continuous planning problem into a discrete space and then search for a sequence of points meeting the task requirements. The rapidly-exploring random tree is one of the commonly used ones because of its better real time performance [9]. However, the constraints of vehicle dynamics also cannot be dealt with effectively because of its inherent random mechanism. Moreover, the generated control command by the motion planner easily exceeds the vehicle’s executable ability without consideration of vehicle dynamics. Accordingly, the actual state trajectory of vehicle will deviate from the desired one, which causes the degradation of driving performances and even traffic accidents [10]. To solve this problem, optimal control methods have been widely used to design the motion planner in recent years. Among them, model predictive control (MPC) has received more attention, because it can optimize multiple objectives in finite time horizon, meanwhile handle nonlinear dynamics and inequality constraints [11].

Essentially, MPC is a model based approach, whose performance is greatly affected by the accuracy and complexity of the model. A high-order nonlinear vehicle model is often used to study the handle limits of vehicles [12]. To analyze the influence of the arrangement of the propulsion system, the mass and the suspension on the lap time of racing cars, Yu et al. used a 14-DOF vehicle model running on a parallel computation platform with 8 CPUs on each node [13]. For the application to motion planning, Chen et al. compared three types of vehicle dynamics models, i.e., bicycle model, 8-DOF model and 14-DOF model, with the commercial model “CarSim” and concluded that the high-fidelity model is beneficial to the performance of motion planner, but its requirement of computation resource goes beyond the ability of on-board processors significantly [14].

To reduce the consumption of computation resources, some researchers devoted to the development of more efficient numerical methods for motion planning. Liu et al. discretized the bicycle model with variable time-steps [15] and Guo et al. accelerated the numerical optimization by selection a better initial value, which is roughly optimized with less steps [16]. Another way is to select a simple and low order vehicle model according to the working condition. When designing the automatic parking system, Li et al. optimized the parking trajectory using the kinematic model, regardless of the tire’s slip and cornering characteristic [17]. This model was also used in the design of motion planner for obstacle avoidance, roundabout entry, intersection crossing and lane change [18]. However, the kinematic model only can describe the vehicle behavior finely under the condition that both longitudinal and lateral accelerations are small enough. For the application to motion planning, Polack et al. studied the limitation of the kinematic model and gave out its application range quantitatively [19].

Another vehicle model widely used in motion planning is the vehicle dynamics model. Carvalho et al. combined a modified nonlinear Fiala tire model with the bicycle model to design the lane keep controller at high speed [20]. Since vehicle speed appears in the denominator of this model and its small angle assumption, it is not applicable to such conditions as stop-and-go and large turning angle. Liu et al. have compared the motion planning performances using five different kinds of vehicle dynamics models and found that each one is only suitable for some types of driving scenarios [21].

Though some researchers have tried to extend the kinematic model [22] and the dynamics model [23] to a wider scope of work conditions by different discretization methods and parameters, these studies evaluated from the system performance under some special scenarios, and the accuracy and application range of model itself is not improved in essence. Moreover, the designed motion planner becomes more sensitive to disturbances under the extended conditions. With the increasing of the intelligent level of automatic driving systems, the motion planner should deal with a variety of driving scenarios [24]. Limited by the application range of each vehicle model, a practical way is to design multiple motion planners, which are scheduled according to the current type of driving condition [25,26]. This will increase the complexity of automatic driving systems. Moreover, the discontinuity in system state and control command, and oscillation among different planners are also unsolved problems. A unified and fine model describing the vehicle behavior in all normal driving conditions is beneficial to the comprehensive performance of motion planning and the practical application of optimal control methods on the design of motion planner.

In this study, a unified and simplified vehicle dynamics model (SDM) is proposed to design a practical motion planner in any normal driving scenarios. By considering the coupling effect in longitudinal and lateral directions, a coupled dynamics model (CDM) is set up, whose accuracy is validated by comparisons with the kinematic model and single-track model under a variety of normal driving conditions. CDM considers the tire cornering characteristic, meanwhile is also applicable to conditions with large turning angle. To reduce the complexity of CDM, the coupling effect is analyzed numerically according to the transfer functions obtained by linearizing at equilibrium points, and the weak part of the coupling effect is ignored to derive SDM. Then, the accuracy of SDM is verified by comparing with CDM, kinematic and single-track models, and applied to design the motion planner by NMPC to validate its contribution on the performance improvement.

The rest paper is organized as follows: Section 2 introduces the modelling process of CDM, which is simplified by the coupling effect analysis in Section 3; In Section 4, SDM is adopted to design the motion planner by NMPC to show its contribution on the performance improvement and Section 5 concludes the paper.

2. Modeling of Vehicle Dynamics for Dynamical Driving Conditions

Because big data has shown its uniquely powerful ability to reveal, model and understand driving process. The naturalistic driving data is used to derive the possible range of normal driving conditions statistically, which CDM is set up to describe and analyze the vehicle dynamics under such conditions.

2.1. Statistical Analysis of Normal Driving Conditions

The natural driving data are collected from a number of equipped vehicles driven under naturalistic conditions over an extended period of time. Though there already exit several public naturalistic driving datasets [27,28], most parameter values about the vehicle dynamics model are missing and so the natural driving data collected by ourselves in Chongqing, China is used in this study.

As shown in Figure 1, the trajectory includes highways, urban expressways, urban common roads and several roundabouts. The total length of one round route is about 30 km and the average driving time is about one hour. During the experiments, the driver is required to drive the experimental vehicle normally according to his driving habits. The driver can change lanes to overtake or drive freely according to the traffic flow. A total of 20 sections of driving data are recorded and the statistical results are shown in Figure 2.

It is found from Figure 2 that:

(1) The range of the front wheel steering angle is comparative large in the speed range from $4\mathrm{m}/\mathrm{s}$ to $10\mathrm{m}/\mathrm{s}$ (See Figure 2a). The single-track model is inapplicable at such conditions because of its assumption about small steering angle [23];

(2) As shown in Figure 2b,d, the maximum yaw rate and lateral acceleration reach about $40\mathrm{degree}/\mathrm{s}$ and $3.5\mathrm{m}/{\mathrm{s}}^2$ in the speed range from $4\mathrm{m}/\mathrm{s}$ to $10\mathrm{m}/\mathrm{s}$. Under such conditions, the kinematic model is also inapplicable, because the cornering characteristic of tire is non-ignorable. The model error becomes significant when the speed is bigger than $5\mathrm{m}/\mathrm{s}$ [22];

(3) The vertical load transfer along the longitudinal direction can be ignored, because the longitudinal acceleration is small enough (See Figure 2c) [29];

(4) Since the absolute value of the lateral acceleration is smaller than ${4\mathrm{m}/\mathrm{s}}^2$, the tire works in the linear range and the vertical load transfer along the lateral direction also can be ignored (See Figure 2d) [21].

2.2. Vehicle Dynamics Model Considering Coupling Forces

For the automatic driving system with a high intelligent level, its motion planner should deal with a variety of dynamical scenarios. A unified, concise and accurate enough model is critical for the motion planning performances, such as computation consumption, tracking error and etc. In this section, a nonlinear and complicated model ensuring accuracy is firstly setup for all natural driving conditions, and then it is simplified by sensitivity analysis in Section 3 to make a balance between the accuracy and complexity.

Considering the analysis results about the driving conditions in Section 2.1 that the vertical load transfer along both longitudinal and lateral directions can be ignored, and the tire works in the linear range. The tire force and tire slip angle in the front and rear can be computed as [29]:

$$F_{yf}=-2C_{yf}{\alpha }_f$$

(1)

$$F_{yr}=-2C_{yr}{\alpha }_r$$

(2)

$${\alpha }_f=\frac{v_y+l_f\omega }{v_x}-{\delta }_f$$

(3)

$${\alpha }_r=\frac{v_y-l_r\omega }{v_x}$$

(4)

According to the force analysis in Figure 3, CDM is described by following first-order partial differential equations:

$${\dot{v}}_x=\frac{F_T\cos \left({\delta }_f\right)}{m}+\frac{2C_{yf}}{m}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\sin \left({\delta }_f\right)+v_y\omega$$

(5)

$${\dot{v}}_y=\frac{F_T\sin \left({\delta }_f\right)}{m}-\frac{2C_{yf}}{m}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\cos \left({\delta }_f\right)-\frac{2C_{yr}}{m}·\frac{v_y-l_r\omega }{v_x}-v_x\omega$$

(6)

$$\dot{\omega }=\left[\frac{F_T\sin \left({\delta }_f\right)}{I_z}-\frac{2C_{yf}}{I_z}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\cos \left({\delta }_f\right)\right]·l_f+\frac{2C_{yr}}{I_z}·\frac{v_y-l_r\omega }{v_x}·l_r$$

(7)

The parameters and variables of CDM are defined in

Table 1. Definition of parameters and variables.

Symbol	Description	Value
$m$	Vehicle mass	$1460\mathrm{kg}$
$I_z$	Yaw moment of inertia	$1943\mathrm{kg}·{\mathrm{m}}^2$
$C_{yf},C_{yr}$	Cornering stiffness of front/rear tire	$\mathrm{54,600}\mathrm{N}/\mathrm{rad}$
$l_f,l_r$	Distance from central of gravity to front/rear axle	$1.17\mathrm{m}$/$1.77\mathrm{m}$
${\alpha }_f,{\alpha }_r$	Sideslip angle at the of front/rear tire	/
$F_{yf},F_{yr}$	Lateral forces at the of front/rear tire	$/$
$v_x,v_y$	Longitudinal/lateral velocity in vehicle frame $\left(x,y\right)$	$/$
$v,v_f,v_r$	Vehicle velocity at central/front/rear	$/$
$F_T$	Longitudinal driving force	$/$
$\omega$	Yaw rate	$/$
$\beta$	Vehicle slip angle	$/$
$\phi$	Heading angle	$/$
${\delta }_f$	Front wheel steering angle	$/$

, which the vehicle parameters are from Changan CS55 with automatic speed and steering control.

2.3. Validation and Analysis of CDM

To verify the accuracy of CDM, it is compared with the kinematic model and the single-track model in the natural driving scenarios. The kinematic model is purely based on geometric relationships that works well at low speed. The single-track model considered the tire force with a small angle assumption. Both the kinematic and single-track models have been validated clearly, which can effectively describe vehicle dynamics in them owe application range and the equations are in the Appendix A.

According to the characteristics of the kinematic and single-track models [15,22], and the natural driving conditions analyzed in Section 2.1, the following typical ones are selected from the natural driving data and the statistical results of model errors are shown in Figure 4:

• Condition 1: The vehicle moved at low speed ($v\le 5 \mathrm{m}/\mathrm{s}$) and the range of the steering angle is $\left|{\delta }_f\right|\ge {35}^{°}$, which is comparatively larger.
• Condition 2: The vehicle moved in the speed range from $5 \mathrm{m}/\mathrm{s}$ to $20 \mathrm{m}/\mathrm{s}$ and the steering angle is bigger than ${15}^{°}$. According to the analysis in Section 2.1, both the kinematic and single-track models are inapplicable to such conditions.
• Condition 3: The vehicle moved at high speed ($v\ge 20 \mathrm{m}/\mathrm{s}$) and the steering angle satisfies $\left|{\delta }_f\right|\le 3^{°}$.

It is known from Figure 4a that the longitudinal speed error of the single-track model is bigger than another two vehicle models in Condition 1, because the projection of the lateral force of the tyre on the longitudinal driving direction has a significant influence on the longitudinal vehicle dynamics when the steering angle is big enough. Moreover, it is found from Figure 4b,c that both lateral speed error and yaw rate error of the kinematic model increase with the velocity, and the maximum errors of the lateral speed and yaw rate reach 0.12 $\mathrm{m}/\mathrm{s}$ and 1.92 $\mathrm{deg}/\mathrm{s}$ respectively in Condition 3. Because the kinematic model ignores the cornering characteristic of tire, which becomes very important to the vehicle lateral dynamics when the lateral force of tyre is large. Summarizing the above analysis, the single-track model and the kinematic model are only applicable to some of the natural driving conditions. Comparatively, CDM achieves a fine accuracy under all the evaluation conditions.

Though CDM is more accurate than the kinematic model and the single-track model by considering the coupling effect of tyre forces in longitudinal and lateral directions, its nonlinearity becomes stronger because of the projection of forces. This requires much more discrete points to fit the nonlinear vehicle dynamics when designing the motion planer by such methods as NMPC. The consumption of computation resources for numerical optimization increases sharply with the dimension of optimization variables [30]. This poses great challenges on the practical application because of the limited computation resource of on-board units. To make a balance between the accuracy and nonlinearity, CDM is simplified by the sensitive analysis in Section 3.

3. Simplification of CDM

The coupling force generated by the projection is the main difference between CDM and the single-track or kinematic model. For simplification, the relationship between different coupling forces in (5)~(7) is analyzed numerically under three typical conditions as shown in Figure 5.

As shown in Figure 6c, the projection of the lateral tyre force on the longitudinal driving direction varies with ${\delta }_f$, and the maximum value reaches 354 $\mathrm{N.}$ This directly leads to the oscillation of vehicle longitudinal speed (See Figure 5a), which is consistent with the analysis in Section 2.3. Under Condition 2 and 3, though the longitudinal control input, $F_T$, is different, the yaw rate remains unchanged. Because the lateral coupling force varies in a small range (See Figure 6d), which is far less than the longitudinal coupling force. From the above analysis, it is possible to simplify CDM by ignoring the minor coupling part.

3.1. Coupling Effect Analysis

The coupling effect is analyzed by the amplitude-frequency characteristic of CDM, which is obtained by linearization at equilibriums [31].

$$\begin{array}{l}\mathsf{\Delta }\dot{\xi }\left(t\right)=A_{k,0}\mathsf{\Delta }\xi \left(t\right)+B_{k,0}\mathsf{\Delta }u\left(t\right)\\ A_{k,0}={A|}_{{\xi }_0,u_0}={\frac{\partial f}{\partial \xi }|}_{{\xi }_0,u_0}, B_{k,0}={B|}_{{\xi }_0,u_0}={\frac{\partial f}{\partial u}|}_{{\xi }_0,u_0}\\ \mathsf{\Delta }\xi \left(t\right)=\xi \left(t\right)-{\xi }_0\left(k\right), \mathsf{\Delta }u\left(t\right)=u\left(t\right)-u_0\left(k\right), \xi ={\left[v_x,v_y,\omega \right]}^T, u={\left[F_T,{\delta }_f\right]}^T\end{array}$$

(8)

To evaluate the influence of inputs to the outputs at the same scale, the transfer function is normalized according to the analysis results about natural driving conditions in Section 2.1:

$$\begin{gathered} G{\left(s\right)}^{\prime }=\left[\begin{array}{cc}\frac{\mathsf{\Delta }{v}_x\left(s\right)}{\mathsf{\Delta }{F}_T\left(s\right)}·\frac{{\overline{F}}_T}{{\overline{v}}_x}& \frac{\mathsf{\Delta }{v}_x\left(s\right)}{\mathsf{\Delta }{\delta }_f\left(s\right)}·\frac{{\overline{\delta }}_f}{{\overline{v}}_x}\\ \frac{\mathsf{\Delta }\omega \left(s\right)}{\mathsf{\Delta }{F}_T\left(s\right)}·\frac{{\overline{F}}_T}{\overline{\omega }}& \frac{\mathsf{\Delta }\omega \left(s\right)}{\mathsf{\Delta }{\delta }_f\left(s\right)}·\frac{{\overline{\delta }}_f}{\overline{\omega }}\end{array}\right] \\ \begin{array}{cccc}{\overline{v}}_x=30 \mathrm{m}/\mathrm{s},& {\overline{F}}_T=m·a\left(v_x\right),& {\overline{\delta }}_f=\delta \left(v_x\right),& \overline{\omega }=38 \mathrm{deg}/\mathrm{s}\end{array} \end{gathered}$$

(9)

where $s$ is the Laplacian operator, $\delta \left(v_x\right)$ and $a\left(v_x\right)$ are obtained from the 3$\mathsf{\sigma }$ boundary in Figure 2a,c, respectively.

Since the closed-loop system composed of the human and vehicle is a low-pass one with a cut-off frequency smaller than 1 Hz [32], only the low frequency band is considered. The vehicle state space under natural driving conditions is divided into $\mathrm{22,500}$ equilibrium points with the step size, 0.2, for $v_x\in \left[0,30\right]\mathrm{m}/\mathrm{s}$ and ${\delta }_f\in \left[0,30\right]\mathrm{degree}$.

3.1.1. Influence of ${\delta }_f$ on Longitudinal Vehicle Dynamics

As shown in Figure 7a, the amplitude-frequency characteristic from ${\delta }_f$ to $v_x$ is similar to the first-order inertial system. Thus, the open-gain and cut-off frequency are selected to measure the coupling strength of ${\delta }_f$ to the longitudinal vehicle dynamics.

It is found from Figure 7c,d that both the open-loop gain and cut-off frequency decrease with $v_x$, while increase with ${\delta }_f$, and the minimum open-loop gain is greater than 1 dB, which implies that ${\delta }_f$ has an obvious effect on the vehicle longitudinal dynamics. Furthermore, the amplitude-frequency characteristic from ${\delta }_f$ to $v_x$ is compared with that from $F_T$ to $v_x$ in Figure 7b, where the solid lines represent the amplitude-frequency characteristic from $F_T$ to $v_x$ and the dashed ones denote that from ${\delta }_f$. to $v_x$. It is found that ${\delta }_f$. has the same order of magnitude of effect on the longitudinal dynamics as the $F_T$. This implies that compared with $F_T$, the projection of the tyre lateral force on the longitudinal direction is big enough and non-ignorable.

3.1.2. Influence of $F_T$ on Lateral Vehicle Dynamics

It can be found from Figure 8a that the open-loop gain of the amplitude-frequency characteristic from $F_T$ to $\omega$ is negative under most conditions and decreases with the increase of ${\delta }_f$ and $v_x$. Under low speed and small steering angle conditions, the maximum open-loop gain is only 17 dB. In Figure 8b, the solid and dashed lines represent the amplitude-frequency characteristic from ${\delta }_f$ to $\omega$ and $F_T$ to $\omega$, respectively. It is further found that the amplitude of the amplitude-frequency characteristic from $F_T$ to $\omega$ is at least 37 dB less than that from ${\delta }_f$ to $\omega$. Summarizing the above analysis, it can be concluded that the projection force of $F_T$ on the lateral dynamics is comparative weak and can be ignored.

3.2. Simplification and Validation of SDM

Based on the analysis results in Section 3.1, CDM is simplified to obtain SDM as

$${\dot{v}}_x=\frac{F_T}{m}+\frac{2C_{yf}}{m}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\sin \left({\delta }_f\right)+v_y\omega$$

(10)

$${\dot{v}}_y=-\frac{2C_{yf}}{m}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\cos \left({\delta }_f\right)-\frac{2C_{yr}}{m}·\frac{v_y-l_r\omega }{v_x}-v_x\omega$$

(11)

$$\dot{\omega }=-\frac{2C_{yf}}{I_z}·\left(\frac{v_y+l_f\omega }{v_x}-{\delta }_f\right)·\cos \left({\delta }_f\right)·l_f+\frac{2C_{yr}}{I_z}·\frac{v_y-l_r\omega }{v_x}·l_r$$

(12)

A typical driving condition, including acceleration, deceleration and steering operations, is selected from the natural driving data to validate the accuracy of SDM by comparing with CDM, the kinematic model and the single-track model. The comparative results are shown in Figure 9, where the single-track model and kinematic model are denoted by “STM” and “KM”, respectively.

In Figure 9a, the longitudinal speeds of CDM, SDM, KM, STM are almost consistent with the real vehicle. But the lateral speeds and yaw rates of KM and STM deviate from the real vehicle greatly during $20–30\mathrm{s}$ (See Figure 9c,d), because the kinematic and single-track models ignore the coupling effect on the longitudinal and lateral directions. As shown in Figure 9b, the coupling force along the longitudinal direction reaches $137\mathrm{N}$. Comparatively, SDM describes vehicle lateral and longitudinal dynamics finely and is simpler than CDM. This is beneficial to reduce the computation burden of the motion planner, meanwhile ensuring the optimization performance.

4. Application and Analysis

In this section, SDM is adopted to design a motion planner by NMPC to show its contribution on the performance improvement.

4.1. 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\left(x,u\right)=\frac{1}{2}{{\displaystyle \int }}_{t_0}^{t_f}\left(\Vert y_i-y_{des}{\Vert }_Q^2+\Vert u{\left(t\right)\Vert }_P^2+\Vert \Delta u{\left(t\right)\Vert }_R^2\right)\mathrm{d}t$$

(13)

$$\dot{x}=\left[\begin{array}{c}\frac{2C_{yf}·\left(v_y+l_f\omega -{\delta }_f{v}_x\right)·\sin \left({\delta }_f\right)}{m{v}_x}+\frac{F_T}{m}+v_y\omega \\ \frac{2C_{yf}·\left({\delta }_f{v}_x-v_y-l_f\omega \right)·\cos \left({\delta }_f\right)}{m{v}_x}+\frac{2C_{yr}·\left(v_y-l_r\omega \right)}{m{v}_x}-v_x\omega \\ \frac{2C_{yf}·l_f·\left({\delta }_f{v}_x-v_y-l_f\omega \right)·\cos \left({\delta }_f\right)}{I_z{v}_x}+\frac{2C_{yr}·\left(v_y-l_r\omega \right)·l_r}{I_z{v}_x}\\ \frac{v_x\cos \left(e_2\right)-v_y\sin \left(e_2\right)}{1-k{e}_1}\\ v_x\sin \left(e_2\right)+v_y\cos \left(e_2\right)\\ \omega -k\dot{s}\end{array}\right]$$

(14)

$$x\left(t_0\right)={\overline{x}}_0$$

(15)

$$\begin{array}{cc}{h}_o\left(x\left(t\right),u\left(t\right)\right)\le 0& o=0,\cdots ,M\end{array}$$

(16)

$$-\overline{u}\le u\left(t\right)\le \overline{u}$$

(17)

$$x_{min}\le x\left(t\right)\le x_{max}$$

(18)

where $x\left(t\right)=\left[v_x,v_y,\omega ,s,e_1,e_2\right]$, $u\left(t\right)=\left[F_T,{\delta }_f\right]$, $y\left(t\right)=\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.

Some vehicle parameters have been listed in Table 1, the other parameters of the NMPC motion planner are presented in

Table 2. Parameters of NMPC motion planner.

Symbol	Value	Symbol	Value
$N$	$60$	$T$	$3\mathrm{s}$
$Q$	$\mathrm{diag}\left(0.844, 1,40\right)$	$P$	$\mathrm{diag}\left({10}^{-5}, 62.5\right)$
$R$	$\mathrm{diag}\left({10}^{-4},90\right)$	$\overline{u}$	${\left[4000 \mathrm{N} 32 \mathrm{deg}\right]}^{\mathrm{T}}$
$\Delta u$	${\left[4000\mathrm{N}/\mathrm{s}63\mathrm{deg}/\mathrm{s}\right]}^{\mathrm{T}}$	$x_{min}$	${\left[0 \mathrm{m}/\mathrm{s},-\infty \mathrm{m}/\mathrm{s},0 \mathrm{rad}\right]}^{\mathrm{T}}$
$x_{max}$	${\left[33\mathrm{m}/\mathrm{s},\infty \mathrm{m}/\mathrm{s},2 \mathrm{pi} \mathrm{rad}\right]}^{\mathrm{T}}$	/	/

.

4.2. Motion Planning Results and Analysis

According to the characteristics of SDM and the natural driving conditions analyzed in Section 2.1, two typical driving scenarios are used to verify the improvement of NMPC-SDM in accuracy and real-time performance.

• Scenario 1: The vehicle cruises on an S-shaped road and the relationship between the curvature and the desired speed is illustrated in Figure 12 [37]. This scenario is designed to verify the improvement of the control accuracy.
• Scenario 2: The vehicle moves at high speed and makes a lane change to avoid obstacles. The desired speed is $28\mathrm{m}/\mathrm{s}$ and the obstacle vehicles move at $3\mathrm{m}/\mathrm{s}$ and $10\mathrm{m}/\mathrm{s}$, respectively.

As shown in Figure 13a, the maximum lateral error of SDM is only $0.08\mathrm{m}$, which is reduced by $38\%$ and $55\%$ compared with the single-track model and the kinematic model, respectively. Moreover, the longitudinal speed error of SDM is only $0.11\mathrm{m}/\mathrm{s}$, which is also reduced by $71\%$ and $74\%$ (See Figure 13b). Summarizing the statistical results of tracking errors, it is concluded that NMPC-SDM has the best tracking control performance compared with the motion planners designed using the single-track model and kinematic model.

There is no obstacle in scenario 1 and so the consumption of computation resources is much less. Besides the tracking control performance, the consumption of computation resources is further analyzed in Scenario 2, where the motion planner should find the optimal solution to avoid collisions with two obstacle vehicles, meanwhile tracking the desired speed as much as possible. The comparative simulations are shown in Figure 14 and Figure 15.

It is found from Figure 14a that the running time of NMPC-SDM is smaller than NMPC-CDM and larger than NMPC-KM overall. Compared with NMPC-CDM, the average running time of single cycle of NMPC-SDM is reduced from $0.91\mathrm{s}$ to $0.74\mathrm{s}$ (See Figure 14b). Though it is larger than NMPC-STM and NMPC-KM, both of the motion planners designed by the single-track and kinematic models sacrifice the tracking performance as shown in Figure 15.

As shown in Figure 15a–c, NMPC planners make lane change decision within $s\in \left[100,200\right]\mathrm{m}$ to avoid the obstacle vehicle (OV1) successfully. After overtaking the OV1, both NMPC-SDM and NMPC-STM return to the middle of the lane, but the NMPC-KM hits on the curb. Because the slip angle of tire reaches $5^{°}$ (See Figure 15d) and the longitudinal coupling force is nearly $250\mathrm{N}$ (See Figure 15e) that KM has a large predictive error under such a condition. The maximum lateral position error of NMPC-KM even reaches $2.8\mathrm{m}$ (See Figure 15g). This implies that the vehicle will invade other undesired lanes, which is very dangerous, especially under high speed conditions. The maximum lateral position error of NMPC-STM is $1.5\mathrm{m}$ (See Figure 15g), because a comparative large steering angle (See Figure 15c) is required to realize the collision avoidance while STM is only applicable to the conditions where the steering angle is small. Comparatively, NMPC-SDM has a good tracking performance of speed and reference lane, whose error are only $0.2\mathrm{m}/\mathrm{s}$ and $0.7\mathrm{m}$, respectively (See Figure 15f,g).

5. Conclusions

To make a balance of the motion planning performance between tracking control and consumption of computation resources under all possible natural driving conditions, this paper proposes a simplified vehicle dynamics model by sensitive analysis. The comparative simulation and numerical analysis results show that:

• The coupling effect in longitudinal and lateral directions is non-ignorable and should be considered when designing a unified motion planner for all possible natural driving conditions.
• The simplified vehicle dynamics model makes a good balance between the model accuracy and complexity, and it can describe vehicle dynamical behavior under both large steering angle and high speed conditions.
• By using SDM, the designed NMPC motion planner has a better comprehensive performance including the consumption of computation resources, the longitudinal speed tracking error and collision avoidance.