Keywords

1 Introduction

Lateral-longitudinal integrated control is an important research direction in autonomous vehicle motion control [1, 8]. Integrated control can more fully consider the coupling characteristics between the longitudinal and lateral dynamics of the vehicle, thereby having a higher performance potential [6]. However, due to the increase in the number of system states and the nonlinearity of the system, the design of control law becomes more challenging, and the computational load during usage is also increased.

Another important research direction is to design motion controllers with safety guarantee. Due to the tracking error and lag, collisions may still occur even if path planning includes safety constraints. While MPC does offer safety guarantee, it comes with a significant computational burden [4, 5]. Therefore, designing a vehicle lateral-longitudinal integrated controller that ensures safety and has a low computational burden is of significant importance.

Control Lyapunov function (CLF) given by Artstein in [3] is an essential tool for the stable control design of affine nonlinear system. Later, based on the idea of CLF and the foundation of barrier certificates theory [9], Wieland introduced the control barrier function (CBF) in [10], a widely favored approach for designing safety control laws among scholars. However, the aforementioned works can only address problems with relative degree 1. Exponential stablilizing control Lyapunov function (ES-CLF) were introduced in [2] to acheive stability of zero dynamics, and later evolved into exponential control Lyapunov function (ECLF) which was commonly used in the stability design of higher-order systems. Inspired by ECLF, exponential control barrier functions (ECBF) were first introduced in [7] as a way to easily enforce high relative-degree safety constraints.

The contribution of this paper is to propose an explicitly solvable autonomous vehicle motion controller with lateral-longitudinal integrated characteristic and safety guarantee, achieved by using exponential control Lyapunov function to address high-order tracking problem and using exponential control barrier function to address high-order constriant problem. We performed simulations using a high-fidelity dynamics model in CarSim for simple urban traffic scenarios. The simulation results demonstrate that our designed controller has good trajectory tracking performance, safety guarantee, low computational burden and a certain level of robustness.

2 Contribution

2.1 Autonomous Vehicles Trajectory Tracker Using Exponential Control Lyapunov Function

The vehicle dynamics model used in the controller design is a six-state system with relative degree 2, given by the following equation:

$$\begin{aligned} \begin{bmatrix} \dot{X} \\ \dot{Y} \\ \dot{\varphi } \\ \dot{U} \\ \dot{V} \\ \dot{\omega } \end{bmatrix} = \begin{bmatrix} U \cos \varphi - V \sin \varphi \\ U \sin \varphi + V \cos \varphi \\ \omega \\ V \omega \\ -U \omega + \frac{k_f}{m} \left( \frac{V + l_f \omega }{U}\right) + \frac{k_r}{m} \left( \frac{V - l_r \omega }{U}\right) \\ \frac{l_f k_f}{I_z} \left( \frac{V + l_f \omega }{U}\right) - \frac{l_r k_r}{I_z} \left( \frac{V - l_r \omega }{U}\right) \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & -\frac{k_f}{m}\left( \frac{V+l_f \omega }{U}\right) \\ 0 & -\frac{k_f}{m} \\ 0 & -\frac{l_f k_f}{I_z} \end{bmatrix} \begin{bmatrix} a \\ \delta \end{bmatrix} \end{aligned}$$
(1)

All the variables are illustrated in Fig. 1. To facilitate future use, define \(x = \begin{bmatrix}x_1& x_2\end{bmatrix}^T \), \(x_1 = \begin{bmatrix}X& Y& \varphi \end{bmatrix}^T \) and \(x_2 = \begin{bmatrix}U& V& \omega \end{bmatrix}^T \). The reference trajectory is a time dependent sequence of reference state \(x_1^{\textrm{ref}}(t)=\begin{bmatrix}X^{\textrm{ref}}(t) &Y^{\textrm{ref}(t)} &\varphi ^{\textrm{ref}(t)}\end{bmatrix}^T\) which is located at a distance L from the perpendicular foot of mass center along the reference path, it is also illustrated in Fig. 1. L is proportionally related to the reference speed, and the scale factor is set to 0.5.

Fig. 1.
figure 1

Vehicle dynamics model and design of reference trajectory

Full size image

For tracking problem with time-varying reference state \(x^{\textrm{ref}}(t)\), to use ECLF for tracker design, we can treating time-varying reference states as time-varying equilibria. The neglect of the derivatives of reference states may result in some loss of tracking performance, but the impact is minimal, especially when the derivatives is not significant. The original exponential control Lyapunov function for autonomous vehicles trajectory tracker is designed as follows

$$\begin{aligned} V(x) = (x_1 - x_1^{\textrm{ref}})^T \begin{bmatrix}0.3 & 0& 0\\ 0& 0.3 & 0\\ 0& 0& 500\end{bmatrix} (x_1 - x_1^{\textrm{ref}})\ \end{aligned}$$
(2)

It can be determined that the relative degree of \( V \) is 2 under the system (1). According to the theory of ECLF, we can establish a linear system as follows

$$\begin{aligned} \dot{\eta }_v(x) &= F \eta _v(x) + G \mu , \end{aligned}$$
(3a)
$$\begin{aligned} V(x) &= C \eta _v(x) \end{aligned}$$
(3b)

where \(\eta _v(x) = \begin{bmatrix} V(x) \\ \dot{V}(x) \end{bmatrix} = \begin{bmatrix} V(x) \\ L_f V(x) \end{bmatrix} \), \(F = \begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\), \(G = \begin{bmatrix}0 \\ 1\end{bmatrix}\), \(C = \begin{bmatrix}1 & 0\end{bmatrix}\) and \(\mu = L_f^2\,V(x) + L_g L_f V(x) u\). Using linear control design techniques, we can choose \(\mu = -K_v \eta _v(x)\), with \(K_v \) ensuring that \(A_v = F - G K_v\) has eigenvalues with negative real parts. Furthermore, we can obtain the following control law

$$\begin{aligned} u^{\textrm{track}} = (L_g L_f V(x))^{+}(-K_v \eta _v(x)-L_f^2\,V(x)) \end{aligned}$$
(4)

Under the assumption that ego vehicle and the obstacle are treated as point masses, the constraint h(x) can be formulated as follows

$$\begin{aligned} h(x) = \left( X-X_{\textrm{obs}}\right) ^2 + \left( Y-Y_{\textrm{obs}}\right) ^2 - d_{\textrm{safe}}^2 < 0 \end{aligned}$$
(5)

where \(X_{\textrm{obs}}\) and \(Y_{\textrm{obs}}\) are the coordinates of the obstacle, and \(d_{\textrm{safe}}\) is the minimum safe distance that must be maintained between the vehicle and any obstacles. We construct the following ECBF by h(x)

$$\begin{aligned} B(x)= k^{\textrm{b}}h(x) = k^{\textrm{b}}[d_{\textrm{safe}}^2 - \left( X-X_{\textrm{obs}}\right) ^2 - \left( Y-Y_{\textrm{obs}}\right) ^2] \end{aligned}$$
(6)

where \(k^{\textrm{b}} \in \mathbb {R}_{+}\). Likewise, the relative degree of \( B \) under the system (1) is also 2. According to the theory of ECBF, we can establish a linear system as follows

$$\begin{aligned} \dot{\eta }_b(x) &= F \eta _b(x) + G \mu , \end{aligned}$$
(7a)
$$\begin{aligned} B(x) &= C \eta _b(x) \end{aligned}$$
(7b)

where \(\eta _b(x) = \begin{bmatrix} B(x) \\ \dot{B}(x) \end{bmatrix} = \begin{bmatrix} B(x) \\ L_f B(x) \end{bmatrix}\), \(F = \begin{bmatrix}0 & 1\\ 0 & 0\end{bmatrix}\), \(G = \begin{bmatrix}0 \\ 1\end{bmatrix}\), \(C = \begin{bmatrix}1 & 0\end{bmatrix}\) and \(\mu = L_f^2 B(x) + L_g L_f B(x) u\). Likewise, we can choose \(\mu \ge -K_b \eta _b(x)\), with \(K_b \) ensuring that \(A_b = F - G K_b\) has eigenvalues with negative real parts. Furthermore, we can obtain the following control law

$$\begin{aligned} u^{\textrm{safe}} = (L_g L_f B(x))^{+}(-K_b \eta _b(x) + \tau -L_f^2 B(x)) \end{aligned}$$
(8)

where \(\tau \in \mathbb {R}_{+}\). The final step is to fuse \(u^{\textrm{track}}\) and \(u^{\textrm{safe}}\), resulting in a controller that achieves both tracking performance and safety. We adopt the method proposed by Wieland in [10], replacing CBF with ECBF. The method is as follows

$$\begin{aligned} u ={\left\{ \begin{array}{ll}u^{\textrm{track}} & \text{ if } B(x) \le -\epsilon , \\ \sigma u^{\textrm{track}} + (1-\sigma )u^{\textrm{safe}} & \text{ if } B(x) \in (-\epsilon , 0), \\ u^{\textrm{safe}} & \text{ if } B(x) \ge 0,\end{array}\right. } \end{aligned}$$
(9)

where \(\sigma = -2 \left( \frac{B(x)}{\epsilon } \right) ^3 - 3 \left( \frac{B(x)}{\epsilon } \right) ^2 + 1\), \(\epsilon \in \mathbb {R}_{+}\).

3 Simulation

The controller parameters are selected as follows: \(K_v = \begin{bmatrix}2& 3\end{bmatrix}\), \(K_b = \begin{bmatrix}2& 3\end{bmatrix}\), \(k_b = 0.2\), \(\tau = 1\) and \(\epsilon = 0.25\). The control object in the simulation is the CarSim high-fidelity vehicle dynamics model.

3.1 Pure Trajectory Tracking

Pure trajectory tracking simulation including straight through intersection and right turn through intersection. In the multi-lane setup, the reference driving speed is established at \(10 \ \mathrm {m/s}\). However, this speed is adjusted to \(8 \ \mathrm {m/s}\) at intersections, and further reduced to \(5 \ \mathrm {m/s}\) when executing turns at intersections. The simulation results are shown in Fig. 2 and Fig. 3.

Fig. 2.
figure 2

Parameters in Variable-Speed Straight-Line Scenario

Full size image
Fig. 3.
figure 3

Parameters in Variable-Speed Right-Turn Scenario

Full size image
Fig. 4.
figure 4

Parameters in Straight-Line Collision Avoidance Scenario

Full size image

3.2 Trajectory Tracking with Collision Avoidance

The vehicle is traveling in a straight line when a stationary obstacle suddenly appears ahead. \(d_{\textrm{safe}}\) is set to 6 m. It was observed that the vehicle executed a reasonable braking maneuver, ultimately coming to a stop at a distance of 6.24 m from the obstacle. The simulation results are shown in Fig. 4.

4 Conclusion

The simulation results demonstrate that our designed controller has good trajectory tracking performance, safety guarantee, low computational burden and a certain level of robustness. As a next step, we aim to propose an integrated optimization method for the tunable parameters in the controller to enhance its optimality.