T-ITS-16-03-0129
Abstract— Artificial potential fields and optimal controllers
are two common methods for path planning of autonomous
vehicles. An artificial potential field method is capable of
assigning different potential functions to different types of
obstacles and road structures, and plans the path based on these
potential functions. It does not however, include the vehicle
dynamics in the path planning process. On the other hand, an
optimal path planning controller integrated with vehicle
dynamics plans an optimal feasible path that guarantees vehicle
stability in following the path. In this method, the obstacles and
road boundaries are usually included in the optimal control
problem as constraints, and not with any arbitrary function. A
model predictive path planning controller is introduced in this
paper such that its objective includes potential functions along
with the vehicle dynamics terms. Therefore, the path planning
system is capable of treating different obstacles and road
structures distinctly while planning the optimal path utilizing
vehicle dynamics. The path planning controller is modeled and
simulated on a CarSim vehicle model for some complicated test
scenarios. The results show that, with this path planning
controller, the vehicle avoids the obstacles and observes road
regulations with appropriate vehicle dynamics. Moreover, since
the obstacles and road regulations can be defined with different
functions, the path planning system plans paths corresponding to
their importance and priorities.
Index Terms— Path Planning, Autonomous Vehicles, Road
Vehicles, Model Predictive Control, Artificial Potential Field,
Vehicle Dynamics and Control.
I. I
NTRODUCTION
large percentage of car accidents is caused by driver
errors [1]. A fully autonomous driving could reduce such
accidents significantly. Besides, it increases the comfort
of traveling by obviating the need for a driver. However, if it
is intended to replace a driver, an autonomous system should
be intelligent enough to handle different driving scenarios for
various obstacles and road regulations. Planning the vehicle’s
path based on road regulations and obstacles is performed in
the path planning module of an autonomous vehicle.
Y. Rasekhipour is with University of Waterloo, Waterloo, ON, N2L 3G1
Canada. (phone: 519-888-4567 x37494, e-mail: yrasekhi@uwaterloo.ca)
A. Khajepour is with University of Waterloo, Waterloo, ON, N2L 3G1
Canada. (e-mail: akhajepour@uwaterloo.ca)
S.K. Chen is with General Motors Company, Warren, MI, 48090-9055,
USA, (e-mail: shih-ken.chen@gm.com)
B. Litkouhi is with General Motors Company, Warren, MI, 48090-9055,
USA, (e-mail: bakhtiar.litkouhi@gm.com)
Developing such a module so that it is able to plan an
appropriate path for any combination of obstacles and road
structures is an ongoing research subject.
Path planning has been widely studied in robotics for
obstacle avoidance [2-4]. For autonomous road vehicles, road
structures and regulations should also be considered in the
path planning in addition to obstacles. Moreover, considering
the vehicle dynamics and tires’ and actuators’ limitations at
the path planning level makes the planned path more feasible
to be tracked by the vehicle. The main advanced path planning
methods developed for autonomous road vehicles are artificial
potential field methods, random search methods, and optimal
control methods.
Artificial potential field method generates a potential field
based on Potential Functions (PFs) of obstacles, road
structures, and goal. It plans the path by moving in the descent
direction of the field. Then, a path tracking module calculates
the vehicle inputs required to track the path [5,6]. The main
advantage of this method over the other path planning
methods is its low calculation cost even with complex PFs for
obstacles and road structures. Considering vehicle dynamics in
the path tracking module improves the ability of the vehicle in
tracking the path. Jie et al. [7] introduces a model predictive
path tracking controller to consider the vehicle dynamics and
actuators’ limitations in its path tracking. However, it is
possible that the planned path is not feasible to be tracked by
the vehicle since the vehicle dynamics and its limitations are
not considered in path generation [8]. Noto et al. [9] considers
the vehicle dynamics in generating the reference path. To plan
the path, it calculates steering angle commands that move the
vehicle in the potential field descent direction and satisfy the
vehicle’s dynamics constraints. It then, uses a path tracking
controller to follow the planned path. Although it finds a path
satisfying the vehicle dynamics, the path is not the optimal
path in terms of vehicle dynamics.
Optimal controllers are also used for path planning on
structured roads. The approach for considering obstacles in a
two dimensional space for obstacle avoidance is a challenge
for this path planning method. Shildbach et al. [10] designs a
scenario-based model predictive controller with two levels.
The higher level module determines the reference lane and
speed by calculating a time-to-lane-change. At the lower level,
the model predictive controller tracks the reference lane and
speed and keeps the vehicle at the safe distance from the
obstacles by staying in the safe interval of the reference lane.
Carvalho et al. [11] calculates the Signed Distance (SD)
between the vehicle and an obstacle and generates an
approximate linear constraint based on this distance for
A Potential-Field-based Model Predictive Path
Planning Controller for Autonomous Road
Vehicles
Yadollah Rasekhipour, Amir Khajepour, Shih-Ken Chen, Bakhtiar Litkouhi
A
© IEEE 2017. Rasekhipour, Y., Khajepour, A., Chen, S.-K., & Litkouhi, B. (2016). A Potential Field-
Based Model Predictive Path-Planning Controller for Autonomous Road Vehicles. IEEE Transac
1
tions on
Intelligent Transportation Systems, 18(5), 1255–1267. https://doi.org/10.1109/TITS.2016.2604240
T-ITS-16-03-0129
2
obstacle avoidance. The predicted obstacles and their
corresponding constraints are probabilistic. The road structure
is also considered as constraints on the vehicle position. A
chance-constrained model predictive controller is used to
solve the problem in two dimensions. Carvalho et al. [12]
considers different vehicle models, driver models, and
environment models to simulate different optimal control path
planning methods in [10] and [11]. It also simulates a tube-
based model predictive controller introduced in [13] for
obstacle avoidance in an unstructured path. The method
considers the obstacles as ellipse-shaped constraints and keeps
the vehicle robustly far from the obstacle while following a
desired path by solving a nonconvex optimal control problem.
Gao et al. [14] includes obstacle avoidance costs in the cost
function of a model predictive path planning controller. The
obstacle avoidance cost is calculated for each obstacle as a
function of the longitudinal distance from the vehicle to the
obstacle and whether the obstacle is in the sight of the vehicle.
The model predictive controller is nonlinear to solve the two-
dimensional obstacle avoidance problem with this obstacle
avoidance cost. Moreover, the optimal control problem
considers all the obstacles with the same function and does not
include road regulations.
In this paper, a model predictive controller is developed for
path planning of autonomous vehicles which avoids obstacles
and observes road regulations by including obstacles’ and
road’s PFs in the objective function of the optimal controller.
It has the merits of both potential field and optimal control
path planning techniques. In another word, it is able to
consider any PF for obstacles and road structures while
calculating the optimal path based on the obstacles, road
structures, and vehicle dynamics. Other optimal control path
planning systems usually consider the obstacles and road
boundaries as constraints [10-13] or consider one cost function
for all of them [14], and therefore, treat all of them in the same
way despite their different characteristics. However, the
proposed method allows considering different types for
obstacles and road structures in the optimal control problem
and treating them according to their characteristics. For
instance, the presented autonomous vehicle passes a speed
bump on its side when possible and cross it otherwise, while it
stops behind a high profile stone if passing it on its side is not
possible. Besides, the presented optimal control problem
solves the two-dimensional obstacle avoidance problem
through a quadratic model predictive controller, for which
there are efficient algorithms solving the problem with lower
computational cost than the existing algorithms for nonlinear
model predictive controllers used in [13,14].
This paper is organized as follows. In Section II, the
structure of an autonomous vehicle system and its different
modules are presented, and the relationship between the path
planning module and the other modules are explained. In
Section III, the path planning problem, the vehicle dynamics
model, and the PFs for different types of obstacles and road
structure are defined, and the path planning optimal control
problem is formulated. In Section IV, the path planning
system is evaluated with a high fidelity CarSim simulation
under several complicated scenarios, and the results are
presented and discussed. Section V concludes the paper.
II. O
VERALL VEHICLE SYSTEM
Even though this paper focuses on the path planning module
in an autonomous vehicle system, there are more necessary
modules, as shown in Fig. 1. In this paper, it is assumed that
the path planning module receives the reference vehicle speed
and the reference lane from the mission planning module. The
mission planning module may generate these reference signals
according to road regulations, planned vehicle route, and
flows of the lanes [15-17]. The path planning module also
receives the shape, position, and velocity of the obstacles, the
road structure, and the regulations from the perception module
[18-20], and the vehicle states from the estimation module
[21-23]. The goal of the path planning module is to plan a path
following the commands of the mission planning module
while meeting the road regulations, avoiding the obstacles,
and having a stable vehicle dynamics. The path planning
module generates the front steering angle and the total
longitudinal force commands. These choices of commands
correspond to the driver commands, which include steering
wheel angle and the gas/brake pedal positions, so that for a
semi-autonomous vehicle, switching between the autonomous
system and the driver can be performed simply. The path
planning system is explained in the following section.
Fig. 1. Block diagram of the autonomous system.
III. PATH PLANNING
This section presents a path planning system for
autonomous road vehicles. First, a vehicle dynamics model is
presented. Next, PFs for obstacles and road lane markers are
defined. Then, the model predictive path planning problem is
generated based on the vehicle model and PFs.
A. Vehicle Dynamics Model
A bicycle model is used to model the vehicle dynamics. The
notation used in the vehicle model is shown in Fig. 2. The
equations of motion of the bicycle model are:
(
)
=
,
(1)
(
+
)
=
+
,
(2)
=
,
(3)
= ,
(4)
= cos sin ,
(5)
= cos + sin ,
(6)
T-ITS-16-03-0129
3
where , and denote the longitudinal velocity, lateral
velocity, and yaw rate of the vehicle at its center of gravity, ,
and are the longitudinal and lateral position and heading
angle of the vehicle in the global coordinate,
and
are
the total lateral forces of the front and rear tires,
is the total
longitudinal force of tires, is the vehicle’s mass, and
is
the vehicle’s momentum of inertia around its vertical axis.
The vehicle is assumed to have a front steering system. A
linear tire model is used for the lateral tire forces [13]:
=
=
+
,
(7)
=
=
,
(8)
in which
and
are the sideslip angles of the front and rear
tires, and is the steering angle. Moreover,
and
denote
the cornering stiffness values of the front and rear tires,
respectively, which are obtained similar to [13].
Fig.2. Vehicle bicycle model.
The vehicle linear dynamics can then be obtained by
linearizing (1) - (8) around the vehicle’s operating point:
= +
,
(9)
=
[
]
,
(10)
=
[
]
,
(11)
where is the state vector,
is the input vector, is the state
matrix, and is the input matrix. The model is discretized by
zero order hold method to be utilized as the model of the
model predictive path planning controller.
B. Potential Field
A potential field is a field generated by obstacle and goal
PFs to lead the vehicle toward the goal while keeping it away
from the obstacles. A goal PF has a minimum at the goal so
that the goal attracts the vehicle, and an obstacle PF has a
maximum at the obstacle position so that the obstacle repulses
the vehicle. In this paper, the task of leading the vehicle
towards its goal is performed by the tracking terms in the
objective function of the path planning controller. Therefore,
the potential field generated here is repulsive only, and is
constructed of obstacle PFs. A PF is defined for the lane
markers to prevent the vehicle from going out of its lane and
the road (
). Two PFs are also defined for two categories of
obstacles: obstacles that cannot be crossed like a vehicle
(
), and the one that can be crossed like a bump (
). The
potential field is the sum of the PFs:
=
+
+
,
(12)
where indices , , and denote the i
th
non-crossable obstacle,
the j
th
crossable obstacle, and the q
th
lane marker, respectively.
The presented functions below are some sample functions;
other functions can be used for modeling other road
regulations and obstacles. The presented method can handle
any PF that is twice differentiable.
1) Non-crossable obstacles
Some obstacles should not be crossed since they are either
important themselves like a pedestrian or can cause a damage
to the vehicle, like a vehicle obstacle or a high profile object.
A hyperbolic function of the distance between the vehicle and
the obstacle is used to generate the potential field caused by
this kind of obstacle. The rate of change of the function
strictly increases as the distance to the obstacle position
decreases, and it approaches to infinity, which prevents the
vehicle from crossing the obstacle. Schulman et al. [24] uses
the SD between the vehicle shape and the obstacle shape for
collision avoidance. The SD is the minimum distance of the
shapes if there is no contact between the shapes, or the
negative of the penetration distance if there are contact points.
More information on the signed distance can be found in [25].
The PF is generated as a function of the SD,
:
(
,
)
=
,
,
(13)
where
and
are intensity and shape parameters of the PF,
respectively. In addition, the vehicle needs to have a larger
distance to the obstacle in the longitudinal direction than the
lateral direction. Therefore, the SD is normalized by the safe
longitudinal and lateral distances from the obstacle,
and
, which are defined as:
=
+
+
2
,
(14)
=
+ sin
+
sin
+
2
.
(15)
The safe longitudinal distance includes the minimum
longitudinal distance,
, the distance spanned by the vehicle
during the safe time gap,
, and the distance due to the
longitudinal velocity difference between the vehicle and the
obstacle [26]. The safe lateral distance includes the minimum
lateral distance,
, and the lateral distance spanned by the
vehicle and the obstacle during the safe time gap if they have
the constant heading angles of
toward each other, and the
distance due to the lateral velocity difference between the
vehicle and the obstacle. The safe time gap compensates for
the vehicle response time, and its value is assigned
T-ITS-16-03-0129
4
accordingly. Besides,
is the longitudinal velocity of the i
th
obstacle,
is the comfortable acceleration, and
and
are the approaching velocities in the longitudinal and
lateral directions. In each direction, the approaching velocity
is set to the velocity difference between the vehicle and the
obstacle if they are approaching and to zero otherwise.
Moreover, zero SD results in an infinite PF. In addition,
with this PF, the vehicle would have no longitudinal response
to the obstacle approaches from the side, if the longitudinal
component of the SD is zero, while a driver would brake in
this situation. These issues are resolved with a modification in
the calculation of the SD; if the longitudinal distance between
the vehicle and the obstacle is less than a threshold,
, it is
set to
with the obstacle being ahead.
If the vehicle and the obstacle are approaching each other,
there is a region around the obstacle where the vehicle cannot
avoid a collision. The longitudinal and lateral collision
distances,
and
, are defined as the maximum distances
from the obstacle in the longitudinal and lateral directions at
which the collision cannot be avoided. In each direction, the
collision distance is the distance required to change the
approaching velocity to zero by modifying the vehicle velocity
with the maximum acceleration,
:
=
2
,
(16)
=
2
.
(17)
The intensity and shape parameters of (13) are calculated by
assigning the safe potential parameter,
, and the accident
potential parameter,
, to the PF at the safe distance and the
collision distance, respectively:
=
= 1
=
.
(18)
It is notable that for being at the safe distance from the
obstacle, the vehicle just needs to be at the safe distance in
either lateral or longitudinal direction. The same expression
holds for the collision distance. Therefore, the collision SD,
, is the maximum of the corresponding SD of the
longitudinal collision distance and the corresponding SD of
lateral collision distance. The potential field of an obstacle
vehicle located at
,
=(20,3.5)m and moving at the
same speed as the vehicle at 80Km/h is shown in Fig. 3.
2) Crossable obstacle
Some obstacles can be crossed without any damage, but it is
preferred not to cross them, if possible, like a low profile
object or a bump on the road. The PF of such an obstacle is
defined with an exponential function:
(
,
)
=
(
,
)
,
(19)
where
is the normalized SD between the vehicle and the
obstacle calculated similar to (13)-(15).
and
are also the
intensity and shape parameters, which are calculated similar to
(14)-(18) except that the uncomfortable potential parameter,
, is assigned to the PF at the collision distance.
The exponential function repulses the vehicle from the
obstacle everywhere because of its positive gradient. But, at
positions close to the obstacle, the gradient decreases as the
distance to the obstacle decreases, which allows the vehicle to
cross the obstacle. Figure 4 shows the potential field generated
by this function for a similar situation to that of Fig. 3.
Fig. 3. Non-crossable obstacle potential field.
Fig. 4. Crossable obstacle potential field.
3) Road lane boundaries
In a structured road, the vehicle should not cross the road
lane markers unless a lane change is desired. To avoid
undesirable lane marker crossings, PFs are defined for lane
markers:
(
,
)
=
(, )
(, ) <
0
(
,
)
>
,
(20)
where
is the SD of the vehicle from the lane marker,
is
the allowed distance from the lane marker, index = ,
denotes the right or left lane marker, and
, is the intensity
parameter calculated by assigning the lane marker potential
parameter,
, to the PF at zero SD.
If a lane keeping is intended, the right and left lane markers
are the ones on which the PFs are implemented. If a lane
change is intended, the PF is not implemented on the lane
marker that can be crossed for the lane change. It is
implemented on the next lane marker instead.
The lane marker PFs are defined with quadratic functions,
and their gradients increase linearly as the SD decreases.
Therefore, the vehicle can cross the lane markers to any
T-ITS-16-03-0129
5
extent, but the farther the vehicle goes from the middle of the
lane the harder the PF pushes it toward there. Figure 5 shows
the road PF for a lane change maneuver on a two lane road.
Fig. 5. Lane changing road potential field.
C. Path planning
In this section, a model predictive path planning controller
is developed with the presented vehicle dynamics model. The
presented potential field for obstacles and road regulations is
added to the controller objective to include the general
obstacle avoidance and road regulation observation to the
model predictive path planning system. With this objective,
the path planning system has the vehicle dynamics
consideration of an optimal control path planning method and
the generality of a potential field method in considering
different functions for the obstacles and road structures.
The model predictive controller predicts the response of the
vehicle up to a horizon, and optimizes the vehicle dynamics,
command following, obstacle avoidance, and road regulations
observation up to that horizon based on the predicted values.
For this optimal control problem, it is assumed that the desired
lane and speed are predefined. Therefore, the desired lateral
position, which is the center of the desired lane, and the
desired longitudinal velocity are the outputs to be tracked:
=
[
]
,
(21)
=
[
]
,
(22)
=
1
2
+
,
(23)
where is the output matrix tracking the desired output
matrix,
,
is the desired lateral position,
is the
desired vehicle speed,
is the lane width,
is the lateral
offset of the road compared to a straight road, and
is the
index number of the desired lane counted from the right.
There are some road regulations on the minimum and
maximum speed limits that the vehicle should not violate.
Moreover, since the tire longitudinal and lateral forces cannot
exceed the friction ellipse, the model predictive controller
should consider this limitation in its prediction to have an
accurate prediction. Therefore, constraints are applied on the
vehicle speed and tire forces to restrict their changes:
< <
,
(24)
+
< 1, = , ,
(25)
where
is the maximum total longitudinal tire force,
, for = , , is the maximum front or rear lateral tire
force, and
and
are the minimum and maximum
speed limits. In most cases, there is no minimum speed limit,
so it is set to zero, and the desired speed is assigned to the
maximum speed limit. It is notable that the constraints on the
tire forces limit the sideslip angles to remain in intervals in
which the tires’ lateral forces behave almost linearly [13].
The constraints of (24) and (25) are applied in the optimal
control problem as soft constraints. A soft constraint can be
violated, but its violation is penalized. A slack variable is
added to the constraint equation to allow some violation and
constructs a penalty term in the objective function of the
optimal control problem to penalize the violation. It is notable
that although surpassing the tire ellipses is physically
impossible, the constraints on the tire forces are considered
soft to avoid possible feasibility issues due to errors in
estimated vehicle states. Moreover, these constraints are
quadratic and cannot be used in a quadratic optimal control
problem. Each of the elliptical constraints is approximated by
affine constraints through approximating the ellipse by an
octagon inscribed in it.
The optimal control problem of the path planning is:
min
,
,
+
,
,
+
,
+
,
,
+
,
(26)
s.t. (= 1, … ,
)
,
=
,
+
,
,
(27)
,
=
,
+
,
,
(28)
,
=
,
+
,
,
(29)
,
+
,
(30)
0,
(31)
=
,
+ 1 ,
= 1, … ,
/
,
(32)
<
,
<
,
(33)
<
,
,
<
,
(34)
,
=
,
, >
,
+
,
= 1, … ,
/
,
(35)
,
=
(
1
)
,
(36)
,
=
(
)
,
(37)
where + , index denotes the predicted value at steps
ahead of the current time ,
is the prediction horizon, and
is the vector of slack variables at steps ahead of the
current time. The objective function includes the predicted
potential field, and quadratic terms of tracking, inputs,
changes in inputs, and slack variables with weighting matrices
, , , and , respectively. The states are predicted through
(27), which is obtained by discretizing (9) to obtain
and
as the discrete state and input matrices. Equation (28)
calculates the tracking outputs, where and are the output
and feedforward matrices. The speed constraint (24) and the
constraints of the octagon approximation of (25) are presented
in (30), where
is the vector of soft constraint variables and
