Keywords

1 Introduction

In recent years, path planning and tracking system have attracted strong attention for various automotive applications. Path planning and tracking are the key components of autonomous driving systems to generate a smooth, collision-free trajectory and optimal control command, which can cope with danger that may arise from the surrounding environments (González et al. 2016).

Some path planning and tracking methods have been applied in autonomous vehicles. Among these approach, model predictive control method can simultaneously consider the mathematical model of the controlled object system and the constraints of states. Ji et al. (2017) proposed a path planning and control framework based on model predictive control for obstacle avoidance, a collision-free path is generated by creating artificial potential fields method based on the information of obstacles and road. Wang and Müller (2016) introduced a new distance-direction based method with the potential field for obstacle avoidance. Wang et al. (2018) combined the virtual-leader potential field and the vehicle-vehicle potential field to generate path for UGV. Huang et al. (2019) combined physics knowledge with artificial potential field method, the resistance value is assigned to each edge according to the potential function. Then, find a collision-free path according to the direction of the current. Elmi and Efe (2018) used two different potential field functions for road boundary and obstacles to calculate the collision-free path. However, the artificial potential field approach is softly constrained and may not find a locally optimal solution.

In this paper, we use the advantages of model predictive control to establish system state constraints and form a unified path planning and control framework. By sampling the lateral and longitudinal of the reference road, an efficient search-based method is proposed to build safe driving corridor. Furthermore, considering the constraints of vehicle dynamics and driving corridor, a multi-constrained model predictive controller is proposed. The entire controller architecture is shown as Fig. 1 (Fig. 2).

Fig. 1.
figure 1

System architecture.

Full size image
Fig. 2.
figure 2

Vehicle model in road coordinate.

Full size image

2 System Model

2.1 Kinematic Error Model in Road Coordinate System

In this section, the kinematic error model in road coordinate is introduced to describe the relationship of vehicle with respect to the reference road, which shown as Fig. 1. The current point \(P_{c}\) is located at the center of mass of the vehicle. The reference point \(P_{s}\) is corresponding to the current point on the reference road. The arc length s represents the distance along the reference path. The relative relationship between the current point and the reference point can be modeled as follows:

$$ \begin{aligned} \dot{e}_{\varphi } & = \dot{\varphi } - v_{x} k(s) \\ \dot{l}_{s} & = v_{x} \sin (e_{\varphi } ) + v_{y} \cos (e_{\varphi } ) \\ \end{aligned} $$
(1)

where \(\varphi\) and \(\varphi_{s}\) are the vehicle yaw angle of the point \(P_{c}\) and \(P_{s}\) respectively. \(k(s)\) is the curvature of the reference point. \(l_{s}\) is the lateral displacement error and \(e_{\varphi }\) is the heading error. \(v_{x}\) and \(v_{y}\) are the longitudinal speed and lateral speed respectively.

2.2 Dynamic Model

The lateral dynamic equation of the vehicle can be obtained at a certain speed. The vehicle’s lateral motion equation is described as follows:

$$ mv\dot{\beta } + 2\left( {k_{f} + k_{r} } \right)\beta + \left( {mv + \frac{2}{v}\left( {l_{f} k_{f} - l_{r} k_{r} } \right)} \right)w_{r} = 2k_{f} \delta $$
(2)

While the vehicle’s yaw motion equation along the x y plane is:

$$ I_{z} \dot{\gamma } + \frac{{2(l_{f}^{2} k_{f} + l_{r}^{2} k_{r} )}}{v}\gamma + 2(l_{f} k_{f} - l_{r} k_{r} )\beta = 2l_{f} k_{f} \delta $$
(3)

where \(m\) is the vehicle mass, \(\delta\) is the front wheel angle, \(l_{f}\) denotes the distance between the center of the mass and the front axle. \(l_{r}\) denotes the distance between the center of the mass and the rear axle. \(I_{{\text{z}}}\) is the yaw inertia moment. \(k_{f}\) and \(k_{r}\) are the cornering stiffness values of the front and rear tires, respectively.

Assuming that the sideslip angle and the heading angle error is small, the lateral error model can be converted as follows:

$$ \dot{l}_{s} = v_{x} e_{\varphi } + v_{y} = v_{x} e_{\varphi } + v_{x} \beta $$
(4)

Combining the kinematic error model with the dynamic model of vehicle, and the linearized model can be directly used to predict the future state. The state of the system is \(x = [\begin{array}{*{20}c} {l_{s} } & {e_{\varphi } } & \beta & \gamma \\ \end{array} ]^{T}\), The state equation of the vehicle model can be represented as:

$$ \left[ \begin{gathered} l_{s} \hfill \\ e_{\varphi } \hfill \\ \beta \hfill \\ \gamma \hfill \\ \end{gathered} \right]^{\prime } = \left[ {\begin{array}{*{20}l} 0 \hfill & {v_{x} } \hfill & {v_{x} } \hfill & 0 \hfill \\ 0 \hfill & 0 \hfill & 0 \hfill & 1 \hfill \\ 0 \hfill & 0 \hfill & {\frac{{ - 2\left( {k_{f} + k_{r} } \right)}}{{mv_{x} }}} \hfill & {\frac{{ - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right)}}{{mv_{x}^{2} }} - 1} \hfill \\ 0 \hfill & 0 \hfill & {\frac{{ - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right)}}{{I_{z} }}} \hfill & {\frac{{ - 2\left( {k_{f} l_{f}^{2} + k_{r} l_{r}^{2} } \right)}}{{I_{z} v}}} \hfill \\ \end{array} } \right]\left[ \begin{gathered} l_{s} \hfill \\ e_{\varphi } \hfill \\ \beta \hfill \\ \gamma \hfill \\ \end{gathered} \right] + \left[ {\begin{array}{*{20}l} 0 \hfill \\ 0 \hfill \\ {\frac{{2k_{f} }}{{mv_{x} }}} \hfill \\ {\frac{{2k_{f} l_{f} }}{{I_{z} }}} \hfill \\ \end{array} } \right]\delta + \left[ {\begin{array}{*{20}c} 0 \\ { - v_{x} k(s)} \\ 0 \\ 0 \\ \end{array} } \right] $$
(5)

Convert the above formula to:

$$ \dot{x}_{m} = A_{m} x + B_{m} u + C_{m} $$
(6)

The system model is discreted as:

$$ x_{k + 1} = A_{k} x_{k} + B_{k} u_{k} = (I + A_{m} T)x_{k} + B_{m} Tu_{k} + C_{m} T $$
(7)

where T is the sampling time, \(x_{k}\) and \(x_{k + 1}\) are the state values at steps k and k + 1 respectively.

3 Method

3.1 Construction of Collision-Free Driving Corridors

In order to build the collision-free driving corridors, we sample various points along the lateral and longitudinal directions of the road. The longitudinal sampling distance is \(\Delta s\), The lateral sampling distance is \(\Delta l\). These points are connected to form the graph structure. Then, we search these sampling points to get the collision-free driving corridors, which satisfies the shortest search distance without collision, meanwhile, taking into account the speed of the vehicle, and make a certain expansion of obstacles, as shown in Fig. 3.

Fig. 3.
figure 3

Collision-free driving corridors.

Full size image

Firstly, the expansion between two adjacent nodes must satisfy certain conditions. The limit of heading angle is expressed as follows:

$$ l_{\min }^{^{\prime}} \le \frac{{l_{i} - l_{i - 1} }}{\Delta s} \le l_{\max }^{^{\prime}} $$
(8)

Secondly, the process of expanding from the current node requires the selection of the least costly node. The cost from the current node to the next node is designed as:

$$ C = C_{r} + C_{c} + C_{o} $$
(9)

where \(C_{r}\) is the reference cost, \(C_{c}\) is the continue cost and \(C_{o}\) is the obstacle cost.

These costs are designed as follows:

$$ C_{r} = (l_{i} - l_{ref} )^{2} $$
(10)
$$ C_{c} = (\frac{{l_{i} - l_{i - 1} }}{\Delta s})^{2} $$
(11)
$$ C_{o} = \left\{ {\begin{array}{*{20}l} {(d_{\max } - d_{obs} )^{2} ,} \hfill & {d_{obs} \le d_{\max } } \hfill \\ {0,} \hfill & {otherwise} \hfill \\ \end{array} } \right. $$
(12)

where \(l_{i}\) and \(l_{i - 1}\) is the lateral distance of current node and last node respectively, \(l_{ref}\) is the reference lateral distance. \(d_{obs}\) is the distance from the current node to the nearest obstacle and \(d_{\max }\) is the is the safe distance.

As a result, the searched path is shown as red line in Fig. 3. Based on the optimal nodes, we found the safe driving boundary towards the left boundary and the right boundary respectively, which shown as the blue lines.

3.2 Multi-constraints Model Predictive Controller

Motion planning and control must ensure planned trajectories do not collide with any obstacles or road edges. At the same time, it needs to meet the vehicle stability constraints, as well as cornering constraints.

According to the collision-free driving corridors, we established the safety constraints of vehicle. For each node, the lateral constraints are represented as:

$$ l_{i}^{\min } + 1/2w + l_{buffer} < l_{i} < l_{i}^{\max } - 1/2w - l_{buffer} $$
(13)

where \(l_{i}^{\min }\) and \(l_{i}^{\max }\) are the lateral maximum and minimum values of lateral distance, w is the width of vehicle. \(l_{buffer}\) is the safety buffer.

According to the predicted trajectory of the vehicle model, considering the lateral position and heading of the path tracking and the magnitude of the control input front wheel angle. In order to ensure the stability of the vehicle, it is necessary to limit the steering angle of the vehicle, the constraint of the actuator is introduced as:

$$ abs(\delta ) \le \min (\tan^{ - 1} (\frac{{(l_{f} + l_{r} )a_{y\_\max } }}{{v_{x}^{2} }}),\delta_{\max } ) $$
(14)

where \(a_{y\_\max }\) is the maximum lateral acceleration. \(\delta_{\max }\) is the limitation of steering angle.

That is, the vehicle state and the control input constraint are solved as follows optimization:

$$ \min J = \sum\limits_{i = 0}^{{N_{p} }} {\left\| {Q(y(k + i\left| {k) - y_{ref} (k + i\left| k \right.))} \right.} \right\|^{2} } + \sum\limits_{i = 0}^{{N_{p} - 1}} {\left\| {Ru(k + i\left| k \right.)} \right\|^{2} } $$
(15)
$$ {\text{Subject to}}:\;y_{k}^{\min } \le y_{k} \le y_{k}^{\max } \;{\text{and}}\;u^{\min } \le u_{k} \le u^{\max } $$
(16)

Furthermore, we converted Eq. (16) to the form of quadratic programming. This optimal problem can be implemented by using the Operator Splitting Quadratic Program (OSQP) solver with low computation.

4 Simulation Results

Simulation results of single obstacle and double static obstacles avoidance are shown in Fig. 4 and Fig. 5, the obstacles are represented by red rectangles and the autonomous vehicle is blue rectangles. The ego vehicle is driving at 15 m/s. The steering wheel angle is shown in Fig. 5. The figures show that the vehicle can avoid these obstacles and ensure safe driving. At the same time, the wheel angle of the ego vehicle is within the limits of vehicle dynamics. The proposed controller can meet the requirements of safety and vehicle dynamics model constraints simultaneously.

Fig. 4.
figure 4

Results of obstacles avoidance.

Full size image
Fig. 5.
figure 5

Single obstacle avoidance

Full size image

5 Conclusion

The proposed path planning and tracking framework established the collision-free driving corridor constraints for model predictive controller, which can effectively avoid static obstacles and ensure the safe driving of autonomous vehicle. Simulation results verify the effectiveness of the proposed method for obstacle avoidance.