Optimum Control for Path Tracking Problem of Vehicle Handling Inverse Dynamics

Abstract

Accurate tracking of a given path is one of the primary factors in the maneuverability of a vehicle and is also an important topic in autonomous vehicle research. To solve the problem of vehicle path tracking, the problem must first be transformed into an optimal control problem. Then, a symplectic pseudospectral method (SPM) based on the third-generation function of symplectic theory and pseudospectral discretization is proposed to efficiently solve the nonlinear optimal control problems. Finally, the results obtained by the proposed algorithm are compared with those obtained by the Gauss pseudospectral method (GPM). The simulation results show that the proposed method can effectively solve the vehicle path tracking problem. Furthermore, the vehicle can track the given path controlled by the proposed algorithm with higher accuracy and greater applicability than other methods.

1. Introduction

Because of their convenience and comfort, vehicles are rapidly becoming indispensable to people’s lives. At this stage, global vehicle ownership is increasing annually. Additionally, it is difficult to decrease the incidence of traffic accidents because of the continuous increase in vehicle ownership. Traffic accidents are considered the “first public hazard” at present. The number of yearly traffic accident deaths has begun to exceed that caused by diseases and wars. As a key part of automated driving technology, vehicle motion control is at the end of the chain. Motion control captures the main behavior of the intelligent system and is the bridge between the vehicle and other modules. It plays an important role in supporting and realizing the algorithmic architecture of the auto-driving system. Based on the vehicle state and the planned trajectory, the motion control module controls the vehicle traveling along the planned path, including the speed, steering, and driving/braking systems. Obstacle avoidance path planning is an important technology in the field of automatic driving. It relies on various onboard systems to obtain road environment and self-driving information, using algorithms to plan a safe path so the vehicle can avoid collisions and bypass obstacles. The path tracking system ensures that the vehicle can accurately follow the expected path. Autonomous obstacle avoidance reduces injuries to passengers as well as economic and property losses caused by accidents. Furthermore, as the main interactor of intelligent transportation, it can alleviate the congestion caused by traffic accidents and reduce unnecessary losses. [1,2].

The problem of vehicle path tracking has been widely studied. A brief review is presented in the following.

Levy et al. [3] proposed a cooperative controller using a nonlinear model predictive control approach for dozens of autonomous vehicles without considering lane markings. In order to improve the accuracy of path tracking in intelligent vehicles, Shi et al. [4] proposed an intelligent vehicle path-tracking control method based on improved model predictive control combined with hybrid proportional-integral-derivative control theory. Awad et al. [5] introduced an integrated path-tracking control strategy for autonomous vehicles. To improve the path-tracking control performance of the intelligent vehicle under critical maneuvers, Sun et al. [6] introduced a novel control strategy. Liu et al. [7] presented the Gauss pseudospectral method (GPM) method to solve the vehicle path-tracking problem. Some researchers have focused on the nonlinear system but have not proposed further improving the solution’s accuracy.

Zhou et al. [8] pioneered the use of an adaptive inverse controller to offset the dynamics of the steering system’s backlash. Homolla et al. [9] proposed an encapsulated trajectory tracking controller. Hashemi et al. [10] presented a novel integrated control framework by considering the combined-slip effect, wheel dynamics, and tire force nonlinearities for driver-assistance systems. Fan et al. [11] proposed the synthesized safety distance for lane changing to study lane-changing time points. However, some models ignored the effect of kinematic changes on the tire. So related tire mechanics and the robustness of the algorithm should be studied.

Tan et al. [12] presented a robust model predictive control-based strategy for the path tracking of 4WS4WD vehicles considering external disturbances. Jiang et al. [13] proposed a hierarchical control strategy to realize the path-tracking control of six-wheel independent drive UGV based on differential steering. In order to overcome the structural and unstructured uncertainties, Jiang et al. [14] presented a model-free predictive control strategy using particle swarm optimization. However, most of the multi-degrees of freedom vehicle models were simplified. Therefore, a nonlinear model that can truly reflect the vehicle condition should be adopted.

In order to improve the path-tracking accuracy of the intelligent vehicle at high speed and establish a practical track-tracking controller, Shi et al. [15] established a vehicle monorail model to achieve the intelligent vehicle’s fifth-order polynomial obstacle avoidance path tracking based on the MPC algorithm. Vieira et al. [16] proposed an algorithm for path planning of car-like mobile robots without movement interruption and avoiding any obstacles along the way. Li et al. [17] designed a new obstacle avoidance path planner to solve the limitation of the traditional local obstacle avoidance path planner in excessive obstacle avoidance. However, most of them focused on the linear model, and they usually neglected the effect of solving methods from dynamic simulation. So, models that can truly reflect the vehicle motion condition and effective solving methods should be adopted.

Aiming to solve the problem of large solution errors and low accuracy, we first establish an optimal control model for trajectory planning. To achieve an efficient solution, under the indirect method′s framework and based on the third-generation function, high-precision pseudospectral interpolation is used for the first-order necessary conditions of the optimal control problem. Thus, a symplectic pseudospectral method is constructed.

This paper proposes a Newton symplectic pseudospectral optimization algorithm, in conjunction with the Newton iterative method, to solve the vehicle path tracking problem. The remainder of the paper is organized as follows. Section 2 presents the problem of vehicle path tracking. Section 3 provides the Newton symplectic pseudospectral algorithm, and Section 4 presents numerical simulations and experimental verification. Finally, Section 5 concludes the paper and provides future research directions.

2. Mathematical Model of Vehicle Path-Tracking Problem

2.1. Mathematical Model of Vehicle Path Tracking Problem

The nonlinear 7-DOF vehicle model in Figure 1 describes the vehicle lane change problem.

In state–space form, it is [7]:

$$\begin{array}{l}\dot{u}=v\omega +\frac{F_{x\mathrm{f}}\cos \delta -F_{y\mathrm{f}}\sin \delta +F_{x\mathrm{r}}-F_{\mathrm{f}}-F_{\mathrm{w}}}{m}\\ \dot{v}=-u\omega +\frac{F_{y\mathrm{f}}\cos \delta +F_{y\mathrm{r}}+F_{x\mathrm{f}}\sin \delta }{m}\\ \dot{\omega }=\frac{a{F}_{y\mathrm{f}}\cos \delta -b{F}_{y\mathrm{r}}+a{F}_{x\mathrm{f}}\sin \delta }{I_z}\\ \dot{\delta }=p\\ \dot{p}=-\frac{k_1{\xi }_1}{I_{w}u}v-\frac{k_1{\xi }_{1}a}{I_{w}u}\omega +\frac{\left(k_1{\xi }_1-k_w\right)}{I_w}\delta -\frac{c_w}{I_w}p+\frac{T_{sw}i}{I_w}\\ \dot{x}=u\cos \theta -v\sin \theta \\ \dot{y}=v\cos \theta +u\sin \theta \end{array}$$

(1)

The state variable is $\mathit{x}(t)={\left[u(t),v(t),r(t),x(t),y(t)\right]}^{\mathrm{T}}$, and the control variable is $\mathit{u}(t)={[\dot{v}]}^{\mathrm{T}}$. Then, Equation (2) can be obtained by simplifying Equation (1).

$$\dot{\mathit{x}}=f[\mathit{x}(t),u(t)]$$

(2)

2.2. Optimal Control Object of Lane Change Motion Planning Problem

The vehicle path-tracking problem can be regarded as the optimal control problem in control theory. Therefore, the minimum lateral distance error of tracking the prescribed path is set as the control object:

$$J={\displaystyle {\int }_{t_0}^{t_f}\left({(\frac{y-y_d}{\widehat{E}})}^2+{(\frac{z}{{\widehat{T}}_{sw}})}^2\right)dt={\displaystyle {\int }_{t_0}^{t_f}L(\mathit{x},\mathit{u},t)dt}}$$

(3)

where $t_0$ and $t_f$ are the initial and final time; $y_d$ is the desired path; $\widehat{E}$ = 0.3 m is the standard threshold of the lateral distance error of $y-y_d$; ${\widehat{T}}_{sw}$ = 8 N·m is the standard threshold of the steering torque.

2.3. Constraints

The initial and terminal states are described as:

$$\mathit{x}(t_0)={[u(t_0),0,0,0,0]}^T$$

(4)

$$\mathit{x}(t_f)={[u(t_f),0,0,x(t_f),y(t_f)]}^T$$

(5)

When the braking maneuver is applied to decelerate the vehicle, the constraints on $F_{x\mathrm{f}}$, $F_{x\mathrm{r}}$ can be rewritten in the following manner:

$$\begin{array}{l}{F}_{x\mathrm{f}}\ge -\frac{\mu mg(b+\mu h_{\mathrm{g}})}{a+b}\\ F_{x\mathrm{r}}=\frac{a-\mu h_{\mathrm{g}}}{b+\mu h_g}{F}_{x\mathrm{f}}\end{array}\}$$

(6)

Therefore, the optimal path-tracking problem can be described as:

$$\min J;s.t.\dot{\mathit{x}}=f[\mathit{x}(t),u(t),t],h\le 0$$

(7)

where $h$ expresses the inequality constraint.

3. Newton Symplectic Pseudospectral Method

The terminal time and corresponding optimal trajectory are solved by the Newton symplectic pseudospectral method (NSP), which is based on the Newton iteration and the third kind of generating function as well as the symplectic theory, to obtain the optimal solution corresponding to the optimal trajectory. The specific calculation steps are:

Step 1: The final time in Equation (7) and the iteration accuracy of the Newton iteration method are initialized. The constrained time–energy optimal control problem in Equation (7) is transformed into an optimal control problem with constraints and a fixed final state and time using the quasi-linearization technique, which can be described as:

$$\begin{array}{l}\min J^{[k+1]}=\frac{1}{2}{\displaystyle \underset{t_0}{\overset{t_f}{\int }}({(U^{[k+1]})}^T{R}^{[k+1]}{U}^{[k+1]})dt}\\ s.t.\{\begin{array}{l}{\dot{X}}^{[k+1]}=A{}^{[k]}{X}^{[k+1]}+B{}^{[k]}{U}^{[k+1]}+w{}^{[k]}\\ C{}^{[k]}{X}^{[k+1]}+D{}^{[k]}{U}^{[k+1]}+V{}^{[k]}+a{}^{[k]}=0\end{array}\end{array}$$

(8)

where $\alpha$ is a non-negative compensation vector, and the number of state variables is set as $n_s$, $k=0,1,\cdots ,{(\cdot )}^{[k]}$ represents the calculation results of variables in the kth iteration. For convenience, superscripts indicating the number of iterations are omitted below.

For the above optimal control model, Lagrange operator $\lambda$ and parameter multiplier vector $\beta$ are introduced to unconstrain the original problem. The parameter multiplier vector satisfies ${\alpha }^T\beta =0,\beta \ge 0$. The objective function can be expressed as:

$$J={\displaystyle \underset{t_0}{\overset{t_f}{\int }}(H-{\lambda }^T\dot{X})dt}$$

(9)

where the Hamiltonian function is:

$$H=\frac{1}{2}{U}^{T}RU+{\lambda }^T(AX+BU+W)+{\beta }^T(CX+DU+V+\alpha )$$

(10)

To minimize the objective function J, the system must satisfy both the control and Hamilton canonical equations.

Then, the control equation is:

$$\frac{\partial H}{\partial U}=UR+B^T\lambda +D^T\beta =0$$

(11)

The Hamilton canonical equation is:

$$\dot{X}=\frac{\partial H}{\partial \lambda },\dot{\lambda }=-\frac{\partial H}{\partial X}$$

(12)

Since the Hamilton function is a function of state variables, control variables, Lagrange multiplier vectors, parameter multiplier vectors, and compensation vectors, and the control variable can be represented by the Lagrange multiplier vector and parameter multiplier vector according to Equation (11), the Hamilton function is a function of four independent variables: state variable, Lagrange multiplier vector, parameter multiplier vector, and compensation vector.

Step 2: Initializing the iteration parameters $k=0$, and setting the initial guess solutions of variables, control variables, and covariates as $X^{[0]}$, $U^{[0]}$, and ${\lambda }^{[0]}$, respectively.

Step 3: Combining $X^{[k]}$, $U^{[k]}$, and ${\lambda }^{[k]}$, the solution of Equation (8) is calculated by the third kind of symplectic pseudospectral method. The specific calculation steps follow.

The time interval $T=[t_0,t_f]$ is discretized into P intervals. The jth interval is $T^j=[t_{j-1},t_j],j=1,2,\cdots ,P$. The jth interval is interpolated by the N^j-dimension Legendre polynomial $L^j(\tau )$; that is, the number of LGL collocations is N^j. Then $X$, $\lambda$, $\beta$, and $\alpha$ can be uniformly expressed as follows:

$$v^j(\tau )={\displaystyle \sum _{l=0}^{N^j}{v}_l^j}\frac{({\tau }^2-1){\dot{L}}^j(\tau )}{N^j(N^j+1)(\tau -{\tau }_l^j)L_l^j}$$

(13)

where $v$ represents any variable in $X$, $\lambda$, $\beta$, and $\alpha$.

Equation (14) can be obtained by differential solving for the state expression.

$$\frac{d{X}_k^j(\tau )}{d\tau }={\displaystyle \sum _{l=0}^{N^j}{X}_l^j{D}_{kl}^j}$$

(14)

where $D_{kl}^j={\dot{L}}_l^j({\tau }_k^j)$ is the pseudospectral differential matrix in the jth interval.

Since the third-generation function is only about the function of and, it can be regarded as an independent variable, and the standing value can be obtained at other interpolation points.

$$\{\begin{array}{l}\frac{\partial S_0^j}{\partial {\lambda }_0^j}=X_0^j\\ \frac{\partial S_m^j}{\partial {\lambda }_m^j}=0,m=1,2,\cdots ,N^j\\ \frac{\partial S_m^j}{\partial X_m^j}=0,m=0,1,\cdots ,N^j-1\\ \frac{\partial S_{N^j}^j}{\partial X_{N^j}^j}={\lambda }_{N^j}^j\end{array}$$

(15)

Then, Equation (16) can be obtained.

$$K^j{\sigma }^j+{\xi }^j{\beta }^j+{\gamma }^j=r^j$$

(16)

According to the control equation, $U=g(X,\lambda ,\beta )$, the boundary constraint equation can be further summarized as:

$$C^j{X}^j-H^j{\lambda }^j-M^j{\beta }^j+V^j+{\alpha }^j=0$$

(17)

Then, Equation (18) can be obtained by further collation:

$$\{\begin{array}{l}{K}^j{\sigma }^j+{\xi }^j{\beta }^j+{\gamma }^j=r^j\\ {\Gamma }^j{\sigma }^j-M^j{\beta }^j+{\widehat{V}}^j+{\alpha }^j=0\\ {({\alpha }^j)}^T{\beta }^j=0,{\alpha }^j\ge 0,{\beta }^j\ge 0\end{array}$$

(18)

where ${\Gamma }^j=[-H^j,H^j]$.

The solution form of the whole interval can be obtained by assembling the results from a single interval.

$$\{\begin{array}{l}K\sigma +\xi \beta +\gamma =r\\ \Gamma \sigma -M\beta +V+\alpha =0\\ {\alpha }^T\beta =0,\alpha \ge 0,\beta \ge 0\end{array}$$

(19)

where the coefficient matrix K is a symmetric sparse matrix.

Then, the state variables and covariate variables can be obtained.

$$\sigma =-K^{-1}\xi \beta -K^{-1}(\gamma -r)$$

(20)

Thus, the solution to the optimal control problem can be obtained. Equation (21) can be obtained by substituting the solution into the boundary constraint equation.

$$Y\beta +q=\alpha ,{\alpha }^T\beta =0;\alpha \ge 0,\beta \ge 0$$

(21)

where $Y=\Gamma K^{-1}\xi +M,q=\Gamma K^{-1}(\gamma -r)-V$.

Since $\alpha$ and the value of variable $\beta$ are unknown in the equation, and they meet the orthogonal relationship, it can be regarded as a standard linear complementary problem. The Lemke method is used to solve the equation. The value of $\alpha$ and $\beta$ can be obtained, and then the solutions $X^{[k+1]}$ and $U^{[k+1]}$ in this iteration are also obtained.

Step 4: Whether the convergence condition $\left|(X^{[k+1]}-X^{[k]})/X^{[k+1]}\right|\le \epsilon$ is being satisfied (where $\epsilon$ is the calculation accuracy) is judged. If the convergence condition is satisfied, the iteration is stopped, and the solution in this iteration is taken as the optimal solution of the optimal control problem. On the contrary, it is set that $k=k+1$ to return to Step 3.

Step 5: According to the optimal solution corresponding to the optimal control problem, the Hamilton function at the end of the optimal trajectory is obtained. If it is satisfied $\left|H(t_f)+w_k\right|\le \rho$, the state and control variables and end time corresponding to the optimal trajectory in Step 4 are the optimal solutions of Equation (7). On the contrary, the Newton iteration method shall be adopted according to the cross-sectional conditions $H(t_f)+w_k=0$ of the end to obtain a new end time and return to Step 1.

The flow of the NSP algorithm is shown in Figure 2.

4. Numerical Simulations and Experimental Verification

4.1. Numerical Simulations

In order to verify the effectiveness of the algorithm, the paper uses the double-lane-change test road as the reference path for simulation analysis. The initial guesses of state and control variables at every time node are all set to be the values of state and control variables at the initial boundary; $\epsilon$ and $\rho$ are set to be 10⁻⁴ and 10⁻³ respectively.

4.1.1. A: 25 km/h

Figure 3 shows the simulation results under 25 km/h for tracking a double lane change road test.

Figure 3a,b show the simulation results of the lateral distance and lateral deviation. From Figure 3a,b, the lateral deviation is smaller, indicating that the vehicle can track the desired path well with the control of the proposed method.

Figure 3c indicates that the vehicle creates a larger yaw angle at the second and third corners when tracking the desired double lane change test road.

Figure 3d–f show that the vehicle needs a larger steering angle and generates a larger sideslip angle and yaw rate when entering and exiting the corners of the double lane change road test, indicating harder sledding for the driver to manipulate the vehicle and a larger degree of tension at the corners.

4.1.2. B: 45 km/h

Figure 4 shows the simulation results at 45 km/h for tracking a double lane change on the test road.

As the vehicle speed increases, the difference compared with the 25 km/h condition becomes more apparent.

Figure 4a,b shows the simulation results of the lateral distance and lateral deviation. From Figure 4a,b, we see that the vehicle can still track the desired path well with the proposed method. Figure 4c–f show that the trend of the curves is similar to that of Figure 3. However, when the vehicle enters and exits the corners of the double lane change road test, the corresponding peak values of the yaw rate, yaw angle, and steering angle were greater than at 25 km/h.

4.1.3. C: 65 km/h

Figure 5 shows the simulation results at 65 km/h for tracking the double lane change; Figure 5a,b show the lateral distance and lateral deviation. Figure 5a,b show that the vehicle can track the desired path well with the control of the proposed method. When tracking the given path, the error between the reference and simulation results increases at higher speeds. Figure 5c–f indicate that the vehicle generates a larger yaw angle and sideslip and needs a bigger steering angle when passing the double lane change test, indicating the difficulty for the driver to manipulate the vehicle and the occurrence of the tail flick.

Figure 6 shows the error between the reference and simulation results at different driving speeds. As the speed increases, the error between the reference and simulation results increases. However, the rate of change from 45 km/h to 65 km/h is larger than the rate of change from 25 km/h to 45 km/h, which indicates that the control performance at 45 km/h is better than at 65 km/h.

4.2. Efficiency Verification

In order to verify the efficiency of the algorithm, we compared it with the Gauss pseudospectral method (GPM) provided in Ref. [7] under the same parameter settings. The specific comparison results are shown in

Table 1. The comparison result of calculation time.

Calculation Accuracy (m)	Time of Calculation (s)
	Mean		Standard Deviation
	NSP	GPM	NSP	GPM
10−2	13.54	32.46	3.39	5.52
10−3	18.65	41.35	4.08	6.35
10−4	22.69	46.83	4.66	6.77
10−5	26.41	49.96	5.04	6.99
10−6	30.78	56.47	5.46	7.45

, and the intuitive comparison is shown in Figure 7. It can be seen from the analysis of Table 1 and Figure 7 that, under the same calculation accuracy, the calculation efficiency of the algorithm in this paper is higher. The calculation time required by the algorithm in this paper is less than that of the Gauss pseudospectral method, which has greater advantages in calculation efficiency than that of the Gauss pseudospectral method.

The single-lane change condition is a common scene for vehicle driving. Figure 8 shows the simulation results under different road friction coefficients at $u=90\mathrm{km}/\mathrm{h}$.

From Figure 8a, it can be seen that as the road friction coefficient decreased, the vehicle required a larger longitudinal distance to complete the single-lane change process. Figure 8b shows that the driver should establish steering options earlier to complete the single-lane change process on roads with a lower road friction coefficient. Meanwhile, Figure 8c shows that the yaw rate trend is consistent with the steering angle. As the road friction coefficient decreases, there are early occurrences of yaw rates, and the yaw rate is smaller.

4.3. Experimental Verification

A ground test was conducted to obtain related test data, such as the yaw rate and steering wheel angle, to verify the feasibility of the simulated results.

The measurement equipment and the real test vehicle used in the tests are described in Figure 9 and Figure 10, respectively [18].

Figure 11 shows the real vehicle test values and the numerical simulation results with the proposed algorithm. Figure 11 shows that, for the results of the sideslip angle and yaw rate, there are errors between the test and numerical simulation values. This is because the mathematical model used for numerical simulation can not fully describe the real vehicle. However, the curves in Figure 11a–f consistently illustrate the correctness of the proposed method.

The experimental results show that the algorithm proposed in this paper can solve the optimal control problem of vehicle path tracking with high accuracy, and the corresponding control variables change relatively smoothly. The algorithm has low requirements for the initial values of covariates and control variables, which has strong operability and applicability and is convenient for practical applications.

5. Conclusions

This article presents an optimal control method for path-tracking problems. Firstly, based on a nonlinear 7-DOF vehicle model, in view of the low computational efficiency of the traditional algorithm, the symplectic pseudospectral method based on the third-generation function of symplectic theory and pseudospectral discretization was established. Then, based on the MATLAB simulation platform, the effectiveness, and accuracy of the proposed estimation algorithm were verified under different conditions and compared with the traditional algorithm. The simulation and experiment results indicated that the mean absolute error of the sideslip angle and the mean absolute error of the yaw rate of the algorithm proposed in this paper were both smaller than the value of the GPM algorithm, indicating good calculation accuracy. And also, the mean and the standard deviation of the time of calculation of the proposed algorithm under the same calculation accuracy were both smaller than the value of the GPM algorithm, indicating a higher calculation efficiency.

In the proposed algorithm, the setting of the initial guess solution will affect the algorithm’s convergence. Improving the quality of initial guess solutions to further improve the algorithm′s convergence rate is the basis for engineering applications and the direction of subsequent research. In addition, further algorithm optimization is needed to improve its computational efficiency.