Smooth Obstacle Avoidance Path Planning For Autonomous Vehicles
Smooth Obstacle Avoidance Path Planning For Autonomous Vehicles
Smooth Obstacle Avoidance Path Planning For Autonomous Vehicles
Abstract—Obstacle avoidance and overtaking are important map and a localization system). The most common used
manoeuvres for autonomous driving. The collision avoidance with methods for global path planning are Dijkstra [7], A∗ and
a vehicle, a pedestrian or any obstacle and the generation of Rapidly-exploring Random Tree RRT [8]. These methods are
a feasible continuous curvature trajectory represent the major
problems faced by researchers to provide a safe path planning time-consuming as the calculation of the whole trajectory in
solution. This paper presents an algorithm able to avoid vehicles real time is slow especially when the map is wide and thus, it
or obstacles by proposing a smooth local modified trajectory of can hardly be applied in real time especially for autonomous
a global path. The proposed method is based on the combination cars and when obstacles are dynamic.
of a parameterized sigmoid function and a rolling horizon. Local approaches restrict the trajectory calculation only
The major advantage of this method is the reactivity to the
obstacle motion and the generation of a smooth trajectory in on a limited time/distance window by taking the perceived
a low execution time. The lateral and the longitudinal safety environment into account. The local path is limited to the
distances are easily parameterized when generating the avoidance perception range of the sensors or camera. These methods are
trajectory. Simulation results show that the algorithm performs used as the whole path is unknown in the majority of cases.
collision avoidance manoeuvres for static and dynamic obstacles The reference trajectory is calculated simultaneously when the
in an effective way. The method is validated by applying the
avoidance approach on real measured trajectory. displacement is started. Using these methods, the navigation
Index Terms—autonomous vehicle, path planning, obstacle can be ensured in a partially known environment and also with
avoidance, sigmoid function. dynamic obstacles.
Several local methods based on interpolating the trajectory
I. INTRODUCTION by mathematical expressions are proposed in the literature.
Autonomous driving is currently an active research sub- Clothoid curves [9], [10], polynomial curves [11], Bézier
ject especially for perception and collision avoidance. An curves and spline curves [12], [13] are some interesting
autonomously driven vehicle must, first of all, be able to examples of these methods. Clothoid curves are used for
determine its path with respect to the perceived static and instance in tentacle methods [10]. The latter is based on the
dynamic environment. After that, it plans the movement to selection of one trajectory from a set of candidate trajectories
be carried out to achieve its objective by respecting a certain using an optimization criterion. Yet, its major issue is the
number of constraints related to the environment (presence long computation time because a great number of paths
of obstacles) and vehicle dynamics (planning stage). Thus, is generated at each step before the selection of the best
the complete process that allows a vehicle to navigate on candidate through optimization. Polynomial, Bézier and Spline
its environment can be summed up in highly interdependent curves are able to generate a smooth trajectory by linking
modules: perception (including localization), planning and different way-points representing the free-space navigation for
control [1]. Trajectory planning for autonomous vehicles has the vehicle. A better compromise must be reached between
been studied in different fields like robotics, autonomous cars, the order of the mathematical function and the computation
unmanned vehicles, underwater vehicles and drones [2]–[6]. time. Sigmoid-based curve is a local planning method based
The main challenge when generating a trajectory is ensuring on an easily configurable path [14]. The parameters of the
its feasibility by taking into consideration several constraints developed sigmoid function are able to adjust the smoothness
related to vehicle dynamics, the road profile, etc. The need of the obstacle avoidance manoeuvre and the safety distance
of a low execution time is also an essential factor in order between the ego vehicle and the leading vehicle to be avoided.
to be implemented in real time. We can distinguish two In local planning, planning area or horizon computation is
categories of path planning approach for obstacle avoidance: performed such as in the Horizon Planning Approach which
the global planning and the local planning. The principle consists in planning the trajectory by dividing the space into
of global approaches is to determine a complete movement regions [15], [16]. This allows quickly computing trajectories
between an initial position and a final position from a priori by dividing the problem into sub-problems. The horizon can
known environment information (e.g. provided by a digital be expressed in time [17] or in distance [18], [19].
P1
The inputs of the function are the obstacle position and the
position of the vehicle. Then, the lateral offset is calculated
depending on these parameters. In [22], the method proposed
by the authors is limited to static obstacle avoidance. The
execution time which is an important criterion is also not
evaluated. This method can be improved by adapting it in real
Fig. 1. Architecture of the control of the vehicle proposed by [1]. time and relating the planned trajectory distance to the sensors
detection capabilities.
In this study, we focus on the trajectory generation module
by supposing that the ego vehicle, i.e. the car which has B. Horizon planning approach
to perform the lane change, is accurately localized and that The Horizon Planning approach (HP) consists in computing
the perception module provides the distance between the ego the path by dividing the drivable space into convex regions
[15]. The trajectory of each region is computed as the vehicle the next navigation area is automatically performed as the
moves forward. The main advantage is the low execution vehicle is moving forward. Thanks to the perception layer, the
time and the reactivity to the obstacle movement. This allows obstacle position is defined at every step. When considering
quickly computing trajectories and divides the problem into moving objects, the next sampling trajectory is recalculated to
sub-problems but requires treating the continuity problem be adapted to the new obstacle position.
between each planned path. HP approach is widely used in
B. Rolling horizon planning approach
the case of autonomous unmanned vehicles [22]. This method
is useful when the environment is partially unknown. The The horizon length depends on the detection range of the
dimension of each horizon must be correlated to the sensors sensors and can be configured. It can be also correlated to
perception range in order to be reactive to the sensed obstacles the speed of the car and be adaptive at real time. All horizon
or the leading vehicle to be avoided. vectors have the same length (Fig. 4); at the first iteration
(iteration 1), the 1st horizon vector which represents the
III. PROPOSED APPROACH next local planned trajectory is calculated. When the vehicle
position reaches the horizon step distance, the second horizon
A. Principle of the approach
path is calculated for the next planned trajectory. Notice that
The approach developed in this paper extends the use of a common part of each vector is recalculated at each iteration
the sigmoid-based path planning either to static or dynamic in order to update the trajectory for the obstacles position
obstacles perceived in the driving scene. Our proposed algo- changes.
rithm is based on the two previously mentioned principles by
using a configurable sigmoid function to provide an efficient
and feasible path to the vehicle in a given horizon. The path
takes into consideration the position and the motion of the
perceived obstacles or the leading vehicle.
The algorithm is constituted of different parts able to react
to any detected obstacle on the road. The initial reference
path of the vehicle can be provided from a digital map
database or a recorded map. The inputs of the algorithm are
the vehicle position and the obstacle positions which represent
the perception and localization parts as illustrated in Fig. 3. Fig. 4. The proposed rolling horizon planning approach.
The local path is then calculated depending on the desired
The horizon step distance must be also well chosen as
parameter of smoothness and the two parameters of safety
it concerns the distance from which the trajectory must be
distance to be respected during the avoidance manoeuvre. This
recalculated and updated. The ratio between the horizon length
parametric smooth variation is necessary to maintain a level
and the step length represents the number of points to be
of safety and comfort to the passenger of the vehicle and to
calculated in each horizon vector. Evidently, when the step
respect the vehicle dynamics when doing a manoeuvre. The
length is small, the position vector (X, Y ) at each horizon
safety distance when avoiding obstacle can be configurable by
contains more points and will be more precise in the generation
taking into consideration the vehicle velocity. The way-point
of the trajectory. The algorithm finishes when the last horizon
calculated represents the position to reach when avoiding the
vector reaches the limit of the complete trajectory vector. In
obstacle/vehicle based on the generated avoidance path.
the next sub-section, the steps of the algorithm are detailed.
C. Execution steps
The algorithm is decomposed into different steps as illus-
trated in Fig. 5.
The first step is to sense the existence of obstacles in
the scene. If an obstacle is detected, its position (X0 , Y0 )is
recorded. Then, the distance separating the initial local path
from the first obstacle is calculated. And for each horizon, this
vector of the Euclidean distances d(N ) between the vector
of the initial trajectory (X(N ), Y (N )) and the obstacle, is
calculated (N represents the index of the elements of the
horizon vector). The vector of the circular trajectory Yr (N )
is then generated by applying the equality between d(N ) and
Fig. 3. Path-planning strategy.
the desired lateral safety distance S if d(N ) ≤ S (Fig. 6).
The goal is to build a safety circular area around the obstacle
The main objective is to calculate the next local path via
following (2):
the Rolling Horizon Planning Approach depending on the p
perception area of the vehicle sensors. The calculation of Yr (N ) = (S 2 − (X(N ) − X0 )2 ) + Y0 . (2)
IV. SIMULATION RESULTS
In this section, the simulation results of the obstacle
avoidance method for left-hand overtaking direction will be
presented in different scenarios. The right-hand overtaking
direction is also possible by modifying the way-point of the
curve to the right side of the obstacle. The execution time
which represents a crucial parameter when implementing the
approach in real time is evaluated. The simulations presented
in this section are carried out using Matlab software. The
computer used in simulation has a processor Intel® Core™
i7 − 7820HQ CPU @ 2.90 GHz and 16 Go of RAM.
A. Influence of the smoothness parameter on the curvature
The parameter C in equation (3) has a direct influence on
Fig. 5. Execution steps of the algorithm.
the smoothness of the generated path. This will provide an
adaptive curvature of the path which can be correlated to the
vehicle speed and dynamic behaviour to provide a feasible path
for the vehicle. Simulation tests have been conducted using
two values of parameter C = 0.5 and C = 1. The smaller
value of C gives a smoother curve as shown in the Fig. 7.
This simulation illustrates also the influence of this parameter
on the curvature of the trajectory. The maximum curvature ρ
is reduced from 0.2 for C = 1 to ρ = 0.06 for C = 0.5.
As depicted in Fig. 7, the curve reaches the point P2 in Sm
meters before the obstacle which is chosen at 10 meters for
Fig. 6. Illustration of the initial and the avoiding trajectories in a local path this example. The safety lateral deviation S is chosen to be 4
and the way-point determination in presence of an obstacle. meters.
(a)
(b)
(c)
Fig. 10. Simulation results of a real path by placing a circular shaped obstacle
on the reference trajectory plotted in blue (a). The new path after applying
the avoiding method is in red. In (b) a zoom-in of the avoidance part of the
path. A blue grid is plotted on the figure with a resolution of 5 meters.