Model predictive approach to integrated path planning and tracking for autonomous vehicles

05/09/2019 ∙ by Chao Huang, et al. ∙ 0

In the path planning problem of autonomous application, the existing studies separately consider the path planning and trajectory tracking control of the autonomous vehicle and few of them have integrated the trajectory planning and trajectory control together. To fill in this research gap, this study proposes an integrated trajectory planning and trajectory control method. This paper also studies the collision avoidance problem of autonomous by considering static and dynamic obstacles. Simulation results have been presented to show the effectiveness of the proposed control method.



There are no comments yet.


page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Autonomous vehicles have a promising future of making transportation time effortless and enabling the drivers to partake in other activities and therefore change everyday life across the world. They have the potential to dramatically reduce crashes which are caused by driver’s faults including aggressive driving, over-compensation, inexperience, slow reaction times, inattention, and various other driver shortcomings. In addition, the autonomous vehicle can provide mobility to the elderly and disabled, increase road capacity, save fuel, and lower harmful emissions [1].

Experiments have been conducted on autonomous vehicles since 1920s: promising trials took place in the 1950s and work has proceeded since then. Many large global companies such as Google, Tesla, BMW and Lexus are investing fund and resources in the research of autonomous vehicles. Google’s co-founder Sergey Brin at Recode Code Conference in Palos Verdes, California described the concept of autonomous vehicles [2]. The prototype of autonomous vehicles relies on built-in sensors and a software system to safely manoeuvre the vehicle without manual controls.

One of several key problems to be solved before autonomous vehicle becoming a reality is to determine a collision-free trajectory of the autonomous vehicles based on the sensor information of sensing and mapping module. In general, the autonomous vehicle control is divided into path planning (trajectory generation) and trajectory tracking. The path planning problem is to find a desired path for the vehicle that starts at the initial configuration and reaches the goal region while satisfying given global and local constraints. Effective path planning algorithms are what make autonomous driving genuinely feasible, safe, and fast [3]. Meanwhile, the path tracking problem is to design control laws for the individual vehicle’s actuator to achieve the selected trajectory.

The real-time path planning, however, particularly in the presence of obstacles, remains very challenging [4]. There exist many different approaches for solving the path-planning problem. The graph-search approaches such as Dijkstra algorithm [5, 6], algorithm [7], and State Lattice algorithm [8], construct graphical discretization of the vehicle’s state space and search for a shortest path using graph search methods. The incremental tree-based approaches incrementally construct a tree of reachable states from the initial state of the vehicle and then select the best branch of such a tree. The sampling based approaches have been proven to be an effective framework that is suitable for a large class of problems in domains. Existing methods in robotics such as Probabilistic Roadmap Method (PRM) [9] and the Rapidly-exploring Random Tree (RRT) [10]

are extensively tested for automated vehicles. However, these methods are computationally expensive. The interpolating curve planners implement different techniques for path smoothing and curve generation by considering feasibility, comfort, vehicle dynamics, and other parameters in the automated driving field

[11, 12].

In the path tracking controller, many of the studies assume the desired vehicle path is already known or the desired path has been planned by the off-line trajectory planner. Specifically, the autonomous vehicle is planned to follow the given trajectory, which is assumed to be collision-free and can be achieved by the vehicle [2]. In addition, tracking controller designed based on linearized or oversimplified models can ignore important nonlinear dynamics that play a major role when the vehicle is operated close to the limits of its handling capability [3].

Most of these studies separately consider the trajectory planning and trajectory control of the autonomous vehicle and few of them have integrated the trajectory planning and trajectory control together expect a few [4]

. We design a control method that combines the path planning and tracking by using a nonlinear vehicle Model Predictive (MP) algorithm in this paper. This integrated MP algorithm simultaneously optimises the front wheel angle and brake torque directly and predicts the local path in real time. The additional tracking controller is no longer required and the computational efficiency can be improved by this integrated structure. MP algorithm has been extensively applied in real-time path planning and tracking control and has been proven to generate the desired safety path. Most of these studies only use the single point mass model and assume the vehicle has a constant longitudinal speed. This assumption can hardly describe the actual vehicle dynamics and the vehicle dynamics performance may be seriously compromised when following the planned trajectory. In this paper, the MP algorithm is based on 2-degree-of-freedom (2-DOF) non-linear bicycle model. First, we calculate a Dubins path by considering the acceleration limits of the car. Compared with road centre line, the Dubins path is proven to be shortest and flexible. Then, in the optimisation cost function of the proposed MP algorithm, in addition to minimise the distance from the road centre line and maximize the distance from the road boundary and obstacles, we also consider the smoothness and comfort of the path by minimizing the yaw acceleration of the vehicle throughout the whole manoeuvre. The obstacles on the road can be classified as the static obstacles and dynamic obstacles and the proposed integrated method can control the vehicle to avoid the obstacles simultaneously in a fast and efficient manner.

The remainder of the paper is structured as follows. Section II provides models of the dynamics of vehicle. The driving manoeuvre and geometric path are designed in Section III-A and the the MP-based integrated trajectory planning and control algorithm are introduced in Section III-B. Simulation results are provided in Section IV. Finally, Section V concludes this paper.

Ii Vehicle dynamics model

The use of autonomous vehicles brings with a number of advantages compared to human operated vehicles [13], and autonomous vehicles have already been developed and produced for production, construction, and urban environments.

The dynamics of the vehicle is modeled by a bicycle model as shown in Fig. 1:


where and are the coordinates of the centre of mass (CM) in an inertial frame. and are the longitudinal and lateral speeds of CM in an inertial frame. is the inertial heading angle and is the yaw rate. and denote the vehicle’s mass and yaw inertia, respectively. and represent the distance from CM of the vehicle to the front and rear axles, respectively. and denote the longitudinal and lateral speeds of CM in the body frame , respectively. and denote the lateral tyre forces at the front and rear wheels, respectively, in the inertial frame aligned with the wheels. It is assumed that these lateral tyre forces are expressed by


where are the front and rear tyre cornering stiffness, and are the front and rear slip angles given by


with a small angle assumption.

Fig. 1: Bicycle model of vehicle


Iii Driving manoeuvre

In this paper, we design a single lane change manoeuvre. The single lane change manoeuvre is a common test for vehicle handling as it represents an essential collision avoidance manoeuvre, especially on the motorway. The road information including road width, road boundary and the obstacles information including vehicles’s speed are assumed to be known.

Iii-a Dubins path

One of the most popular path planning methods found in robotics is the geometric method. Dubins (1957) showed that, for a particle that does not reverse, the shortest paths consist of circular arcs and straight line segments. This section describes the construction of such paths suitable for the target vehicle to perform specified lateral obstacle avoidance manoeuvre.

It is known that the vehicle is capable of a maximum acceleration of [14] and this will result in a circular path with radius


where is the longitudinal speed of the vehicle, is the friction coefficient and is the gravity constant.

Fig. 2: Geometric construction of reference trajectory

Fig. 2 shows the construction of a Dubins path consisting of straight lines and circular arcs for a single lane change. The road width is

and its boundaries are represented by black solid lines. There are three obstacles (yellow vehicle, pink vehicle and blue vehicle) and it is assumed that based on the sensor information, all the obstacles can be detected by the ego vehicle and the current position and velocity of each obstacle can be accurately estimated. Therefore, the obstacle boundary is known at each time step and represented by blue dash line. It is noticed that the obstacle boundary has considered the safety gap between the ego vehicle and obstacle vehicles. The grey dash line presents the road centre line. The initial position of the ego vehicle is


By knowing the sensor information of road and obstacles and in (6), we define some waypoints like which are the keys to describe the Dubins path. Specifically, the path is designed so that the ego vehicle drives passing the initial position and follows the centre of the lower lane until reaching , which is the beginning of the maximum acceleration turn to the upper lane. Continuing the turn through , the point of the closest approach to the boundary, the vehicle reaches , from where it follows a straight path from to . The maximum acceleration turn to the lower lane, through and , brings the vehicle to the centre of the adjacent lane. From to , the vehicle starts the second turn and eventually ends to the centre of the original lane ( to ). After defining all the waypoints, the whole path containing straight lines and arcs can be completely described. It is noted that under the premise of obtaining the information of obstacles, the path can feasibly avoid static and dynamic obstacles in the lane change manoeuvre. More details can be found in [14].

Iii-B MP optimisation

However, the transition between arc and line segments entails discontinuous changes in lateral acceleration, making it impossible for real autonomous vehicle to track exactly. In this paper, MP algorithm is used to obtain control inputs and to minimize the error.

Based on the 2 DOF tyre model, the cost function of MP optimisation algorithm at time can be presented in (5). In (5), is the prediction horizon, and are the longitudinal and lateral positions of the Dubins path which are generated in Section III-A. and are the longitudinal and lateral positions of road upper boundary and and are the longitudinal and lateral positions of road lower boundary. The and are the predicted trajectory optimised by this MP algorithm. is scaling factor of attractive potential and , and are scaling factors of repulsive potential. Here, is related to the term of minimizing the distance between the vehicle position and Dubins path. and are related to the terms of maximizing the distance between the vehicle position and road boundaries. is related to the term of minimizing yaw acceleration throughout the manoeuvre. The values of these scaling factors can be adjusted according to the priority of different scenarios. The optimisation variables are control inputs, front wheel steering angle and rear wheel traction or brake torque .

The constraints are placed on the steering angle and driving or braking torque. In general, the front wheel angle of a conventional vehicle is constrained by the mechanical linkages between each wheel. According to [15], the constraints on the front wheel angle for a traditional front wheel steering vehicle is between and . For four-wheel-independent-steering vehicles [16], the front wheel angle can achieve a rotation of while driving. In this paper, the front wheel angles of Steer-by-Wire (SbW) systems can lie only within and . Any degree outside of this range is invalid and the vehicle will not be able to turn to that amount. The maximum driving torque of the real wheel of vehicle model is and the total braking torque is .

The and are the future vehicle status which can be predicted and calculated from the vehicle velocity in the previous time step based on the following discrete dynamics model:


where and are feedback values of vehicle longitudinal and lateral position in the global coordinate system in the previous time step. Here, is the time of the current time step and is the time of the next time step. and are predicted vehicle longitudinal and lateral velocities in the global coordinate system in the current time step, which can be presented as follows:


where and are predicted longitudinal and lateral velocities in the body-fixed coordinate system and is the predicted vehicle yaw angle. According to (1), , and can be predicted in (8).


In (8), , and are feedback values from actual vehicle in the current time step. and are predicted tyre front wheel side force, rear wheel side force in the previous time step. On the basis of (2), and can be predicted as follows:


In this section, the vehicle trajectory in real time can be predicted and optimised by (5)–(9) with the real-time feedback values from actual vehicle. The optimisation variables of steering angle and driving torque can be determined and input into the actual vehicle model. This method is summarized in Algorithm 1.

1:  Set prediction horizion
2:  for  to  do
3:     Generate Dubins path based on Section III-A;
4:     Solve optimisation problem (5);
5:     Go back to 2 with .
6:  end for
Algorithm 1 The proposed algorithm

Iv Simulation results

In this section, two sets of simulations are carried out by the software of MATLAB/Simulink to verify the advantages of the proposed integrated trajectory planning and tracking controller based on MP algorithm and bicycle nonlinear model. The vehicle parameters are listed in TABLE 1.

Parameter Value
TABLE I: Parameters of vehicle dynamics for simulation
Fig. 3: Vehicle desired and actual trajectory with different algorithms
Fig. 4: Yaw rate with different algorithms

The standard Matlab function fmincon with receding control strategy is implemented to solve (5) with the sampling time . The prediction horizon is .

Three algorithms for fmincon: interior point, sequential quadratic programming (SQP) and active set algorithms are compared with the traditional two-level method in Fig. 3 and Fig. 4. It is assumed that the initial velocity of the autonomous vehicle is and all the obstacles are static. under the assumption that the obstacles are static. Compared with traditional two-level method, the MP trajectories can provide a smooth and flexible path. We select the active set algorithm (red dash line) in the following simulations.

Fig. 5 and Fig. 6 show the first set of simulation where the obstacles are static. The initial velocity of the autonomous vehicle is 10 . The actual trajectory is controlled by the proposed MP algorithm based on the 2 DOF tyre model. Furthermore, the traditional two-level method is compared and shown in Fig. 5. In the traditional two-level method, the desired trajectory is assumed to be planned and known already and the two-level trajectory tracking controller is implemented to achieve the desired path. Fig. 5 also shows that the trajectories of all these two method can accurately follow the Dubins path and the proposed MP method can provide a smoother lane change path. Fig. 6 shows the vehicle yaw rate response of these methods and the proposed MP method shows more stable yaw rate response and less yaw angle change rate compared with traditional two-level method. In contrast, the yaw rate response of traditional two-level method is oscillating abruptly and the reason behind is that the desired path is pre-defined and the trajectory cannot be smoothly optimised in real time.

Fig. 5: Vehicle desired and actual trajectory in the first set of simulations
Fig. 6: Yaw rate in the first set of simulations

Fig. 7 and Fig. 8 show the second set of simulation, the autonomous vehicle is still implementing the lane change task with the same boundary condition in Fig. 2, but, the obstacles are dynamic. The initial velocity of all these three vehicles are 10 . The obstacle 1 will accelerate and drive at 10.5 and obstacle 2 will accelerate and drive at 12 and obstacle 3 will accelerate and drive at 11.5 . All the parameters of the ego vehicle are the same as that in the first set of simulation. It is noted that instead of following the pre-defined path in the first set of simulation, the traditional two-level method applies the MP method to generate the obstacle avoidance trajectory. Fig. 7 shows the actual vehicle trajectory of the proposed method and traditional methods and Fig. 8 shows the yaw rate response in the second set of simulation with dynamic obstacles. It is indicated that the proposed MP algorithm can successfully generate the avoidance trajectory and also generate a smooth lane change path.

Fig. 7: Vehicle desired and actual trajectory in the second set of simulations
Fig. 8: Yaw rate in the second set of simulations

V Conclusions

This paper proposes a real-time control method that integrates path planning and tracking based on the MP algorithm for autonomous vehicles. In the future, we are interested in extending the approach for multiple autonomous vehicles.


The authors are supported by ERATO HASUO Metamathematics for Systems Design Project (No.JPMJER1603), JST.


  • [1] D. J. Fagnant and K. Kockelman, “Preparing a nation for autonomous vehicles: 1 opportunities, barriers and policy recommendations for 2 capitalizing on self-driven vehicles 3,” Transportation Research, vol. 20, 2014.
  • [2] P. Griffiths and R. B. Gillespie, “Shared control between human and machine: Haptic display of automation during manual control of vehicle heading,” in 12th International Symposium on Haptic Interfaces for Virtual Environment and Teleoperator Systems (HAPTICS’04).   IEEE, 2004, pp. 358–366.
  • [3] B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Transactions on intelligent vehicles, vol. 1, no. 1, pp. 33–55, 2016.
  • [4] C. Shen, Y. Shi, and B. Buckham, “Integrated path planning and tracking control of an AUV: A unified receding horizon optimization approach,” IEEE/ASME Transactions on Mechatronics, vol. 22, no. 3, pp. 1163–1173, 2017.
  • [5] R. Kala and K. Warwick, “Multi-level planning for semi-autonomous vehicles in traffic scenarios based on separation maximization,” Journal of Intelligent & Robotic Systems, vol. 72, no. 3-4, pp. 559–590, 2013.
  • [6] J. Bohren, T. Foote, J. Keller, A. Kushleyev, D. Lee, A. Stewart, P. Vernaza, J. Derenick, J. Spletzer, and B. Satterfield, “Little ben: The ben franklin racing team’s entry in the 2007 darpa urban challenge,” in The DARPA Urban Challenge.   Springer, 2009, pp. 231–255.
  • [7] J. Ziegler, M. Werling, and J. Schroder, “Navigating car-like robots in unstructured environments using an obstacle sensitive cost function,” in 2008 IEEE Intelligent Vehicles Symposium.   IEEE, pp. 787–791.
  • [8] M. Rufli and R. Siegwart, “On the application of the d* search algorithm to time-based planning on lattice graphs,” in 4th European Conference on Mobile Robotics (ECMR).   Eidgenössische Technische Hochschule Zürich, 2009, pp. 105–110.
  • [9] L. Kavraki, P. Svestka, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 1994, p. 566–580.
  • [10] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,” The international journal of robotics research, vol. 20, no. 5, pp. 378–400, 2001.
  • [11] J. Horst and A. Barbera, “Trajectory generation for an on-road autonomous vehicle,” in Unmanned Systems Technology VIII, vol. 6230.   International Society for Optics and Photonics, 2006, p. 62302J.
  • [12] M. Brezak and I. Petrović, “Real-time approximation of clothoids with bounded error for path planning applications,” IEEE Transactions on Robotics, vol. 30, no. 2, pp. 507–515, 2014.
  • [13] R. Pepy, A. Lambert, and H. Mounier, “Path planning using a dynamic vehicle model,” in Information and Communication Technologies, ICTTA’06. 2nd, vol. 1.   IEEE, pp. 781–786.
  • [14] G. P. Bevan, “Development of a vehicle dynamics controller for obstacle avoidance,” Ph.D. dissertation, University of Glasgow, 2008.
  • [15] D. Karnopp, Vehicle stability.   CRC Press, 2004.
  • [16] B. Li, H. Du, and W. Li, “A novel method for side slip angle estimation of omni-directional vehicles,” SAE International Journal of Passenger Cars-Electronic and Electrical Systems, vol. 7, no. 2014-01-0303, pp. 471–480, 2014.