VIETNAM NATIONAL UNIVERSITY HO CHI MINH CITY

UNIVERSITY OF SCIENCE

ASSIGNMENT REPORT

NOVEL PATH PLANNING AND

TRAJECTORY TRACKING
ALGORITHM FOR AUTONOMOUS
VEHICLE

Instructor Dr. Le Anh Ha

Group members Vo Hoang Anh 22125022
Tran Hoang Linh 22125233
Do Minh An 21125115
Mai Chi Long Thanh 22125181
Trinh Xuan Quyen 22125165

Ho Chi Minh City, October 2022

 University of Science
Faculty of Mathematics and Computer Sciences

Contents
List of Figures .......................................................................................................................3

1 Introduction ................................................................................................................... 4
1.1 Motivation and Purpose ........................................................................................... 4
1.2 Goal ............................................................................................................................. 4

2 Project Summary ..........................................................................................................5

3 Theoretical Basis ........................................................................................................... 6

3.1 Bézier Curve ............................................................................................................... 6
3.1.1 Linear Interpolation ..................................................................................... 6
3.1.2 General Form of Bézier Curves .................................................................... 6
3.1.3 Properties....................................................................................................... 7
3.2 PID Control................................................................................................................ 7
3.2.1 Proportional Control (P) .............................................................................. 8
3.2.2 Integral Control (I) ....................................................................................... 9
3.2.3 Derivative Control (D).................................................................................. 9
3.2.4 Combined PID Control ................................................................................ 9
3.3 Artificial Potential Field ......................................................................................... 10
3.4 Genetic Algorithm ................................................................................................... 10
3.5 Gaussian Distribution ..............................................................................................11
3.6 Binary Search Algorithm .........................................................................................11

4 Methodology ..................................................................................................................12
4.1 Path Planning Algorithm ....................................................................................... 12
4.1.1 Bezier to Chromosome Encoding Strategy............................................... 12
4.1.2 Danger Map Generation ............................................................................ 13
4.1.3 Initialize Population ................................................................................... 14
4.1.4 Fitness Function ......................................................................................... 14
4.1.5 Crossover and Mutation ............................................................................. 15
4.1.6 Overal Algorithm Workflow ...................................................................... 16
4.2 Trajectory Tracking Algorithm .............................................................................. 17
4.2.1 Vehicle model .............................................................................................. 17
4.2.2 Initialization ................................................................................................ 18
4.2.3 Control loop ................................................................................................. 18
4.2.4 Overall Workflow........................................................................................22

5 Results and Simulations .......................................................................................... 23

5.1 Path Planning Algorithm .......................................................................................23
5.2 Trajectory Tracking Algorithm ..............................................................................23

6 Summary and Conclusion........................................................................................ 24

MTH251 Page 1/25

 University of Science
Faculty of Mathematics and Computer Sciences
References........................................................................................................................... 25

MTH251 Page 2/25

 University of Science
Faculty of Mathematics and Computer Sciences

List of Figures
1 Electric Cars ............................................................................................................... 4
2 Linear Interpolation .................................................................................................. 6
3 General Form of Bézier Curves................................................................................. 7
4 Some cases of Bézier Curve ....................................................................................... 7
5 Examples of different P Gain values ....................................................................... 8
6 Steady-state Error ..................................................................................................... 8
7 Examples of several values of I-Gain ....................................................................... 9
8 Examples of several values of D-Gain ..................................................................... 9
9 Combined PID Control ........................................................................................... 10
10 Artificial Potential Field ......................................................................................... 10
11 Genetic Algorithm Visualization ........................................................................... 11
12 Gaussian Distribution ............................................................................................. 11
13 Binary Search Visualization ................................................................................... 11
14 Chromosome Encoding Strategy ............................................................................ 13
15 Crossover mechanism ............................................................................................. 15
16 Add gene................................................................................................................... 15
17 Remove gene ............................................................................................................ 16
18 Edit gene .................................................................................................................. 16
19 Algorithm Workflow ............................................................................................... 16
20 Calculating Error..................................................................................................... 20
21 PID Workflow .......................................................................................................... 21
22 Overall Workflow .................................................................................................... 22
23 Generated Map ........................................................................................................ 23
24 Car following path ................................................................................................... 23

MTH251 Page 3/25

 University of Science
Faculty of Mathematics and Computer Sciences

Abstract
The goal of this project is to develop a new approach to solve two well-known
problems in robotics engineering: Path Planning and Trajectory Tracking for Au-
tomated Vehicles. Our novel approach introduced Artificial Potential Field and
Genetic Algorithm to generate a collision-free Bezier spline-based trajectory for the
automated vehicle. Then the PID control algorithm is used to help the car steer
itself to follow the generated path from the starting point to the ending point.

1 Introduction
1.1 Motivation and Purpose
With the current technological trend, cars, robots, and vehicles have been able to move
autonomously basically without any human interaction. Artificial intelligence is such
a technology that has provided electric car auto-driving and complex problem solving
capabilities to transport humans and objects to desired location. Having seen the vast
potential in technological development, we chose Path Planning and Trajectory Tracking
problems to start with with the hope of finding new solutions and being a motivational
foundation for further researches from other engineers.

(a) Tesla Cybertruck (b) Vinfast VF9

Figure 1: Electric Cars

1.2 Goal
Currently, AI is the most robust tool to provide the self-driving capability for au-
tonomous vehicles. However, since the technology requires data with high speciality
such that it may become hard to obtain (data from specified sensors, high-cost periph-
erals and devices), we aim to achieve the collision-free motion using another way, which
is Genetic Algorithm

MTH251 Page 4/25

 University of Science
Faculty of Mathematics and Computer Sciences

2 Project Summary
This project presents an advanced path planning and trajectory tracking system aimed
at optimizing navigation in complex environments. The system generates smooth, ef-
ficient paths while ensuring safe navigation through obstacle avoidance. It also tracks
trajectories with high accuracy, ensuring the system follows the planned path precisely.

Path Planning
• Bézier Curve: Used to generate smooth and continuous paths, ensuring feasibil-
ity in constrained environments.
• Artificial Potential Fields (APF): For local obstacle avoidance by generating
a recursive danger map that reflects obstacles and their danger level, guiding the
system to avoid collisions.
• Genetic Algoritm (GA): Inspired by natural selection. The algorithm evaluates
multiple possible paths, selects the most efficient ones based on fitness criteria, and
evolves the best path through successive generations.

Trajectory Tracking
• PID Controller: Ensures precise adherence to planned trajectories by making
real-time adjustments to reduce deviations caused by environmental disturbances
or system dynamics.
• Normal Distribution: Introduces noise into the system, closely mimicking real-
world conditions and enabling the algorithm to adapt to unpredictable variations
in vehicle performance and environmental factors.
• Binary Search: An algorithm designed to determine the projection of a point onto
a Bézier curve. This serves as a critical component for the PID controller, ensuring
the vehicle remains aligned with the path. Additionally, it aids the Genetic model
in validating the feasibility of the Bezier trajectory.

Tools and Libraries

• Programming Language: Python
• Librabries: Numpy, Pygame, Matplotlib
• Object-oriented Programming

Project Repository
For detailed implementation, code, and documentation on the novel robotics path-
planning and control algorithms, our project repository can be found in this link:
https://github.com/Reiko0908/NOVEL-PATH-PLANNING-ALGORITHM

MTH251 Page 5/25

 University of Science
Faculty of Mathematics and Computer Sciences

3 Theoretical Basis
3.1 Bézier Curve
Bezier curves is a set of locally generated points that can be constructed and positioned
from a set of control points. The more local control points being constructed, the
smoother curve becomes. Bezier curves are widely used in real life for tasks requiring
smooth and precise shapes. They are essential in graphic design and digital art for
creating scalable vector graphics and smooth font outlines.

3.1.1 Linear Interpolation

By definition of the Bezier Curve, Linear Interpo-
lation is the main foundation of constructing the
points on the curve. This is a method of estimat-
ing an intermediate value between two points P0
and P1, based on a parameter t that runs on the
interval [0; 1].

P(t) = (1 − t)P0 + tP1

where t=0 gives P0 and t=1 gives P1.

3.1.2 General Form of Bézier Curves

In order to construct a curve, multiple Linear In-
terpolations can be performed nested inside each Figure 2: Linear Interpolation
other. For example, interpolating P3 between P0
and P1, P4 between P1 and P2 and then do the same with P5 between P3 and P4 will
give us a quadratic Bezier Curve. In this case, P0, P1 and P2 are called ”control points”.
The general form of a Bézier curve allows it to be extended to any degree n based on
the number of control points P0, P1, . . . , Pn. The curve is defined as:
Xn
n
B(t) = (1 − t)n−itiPi
i=0
i

Where:

• B(t) is the point on the Bézier curve at parameter t ∈ [0, 1],

n
• i
= n!
i!(n−i)!
is the binomial coefficient,

• Pi is the ith control point,

• n is the degree of the Bézier curve, (n + 1) is the number of control points.

MTH251 Page 6/25

 University of Science
Faculty of Mathematics and Computer Sciences

Figure 3: General Form of Bézier Curves

Example
• n = 1, the curve is a simple straight line (single Linear Interpolation).
• n = 2, the curve is a Quadratic Bezier Curve.
• n = 3, the curve is a Cubic Bezier Curve.

(a) Quadratic B´ezier Curve (b) Cubic B´ezier Curve

Figure 4: Some cases of Bézier Curve

3.1.3 Properties
• Control Points: The curve starts at P0 and ends at Pn. Intermediate control
points (P1, P2, . . . , Pn−1) influence the shape but are not necessarily on the curve.
• Continuity: Bézier curves are continuous and smooth, making them ideal for
graphics and design applications.
• Scalability: Adding more control points increases the degree of the curve, allowing
for more complex shapes.

3.2 PID Control

PID is a closed loop algorithm use to help a system achieve a pre-determined setpoint.
For example, PID could be used to adjust the valve of a water pipe to control the water
flow inside fixed at a setpoint. The main formula of PID algorithm is:
t
de(t)
u(t) = KP · e(t) + KI · e(t)dt + KD ·
0 dt
where

MTH251 Page 7/25

 University of Science
Faculty of Mathematics and Computer Sciences

• u(t) is the variable that the system is trying to control

• e(t) is the difference between the current state of the system with the setpoint
• KP , KI, KD are Proportional Gain, Integral Gain and Derivative Gain, these are
system’s constants needed to be tuned through multiple experiments

3.2.1 Proportional Control (P)

The proportional term in the general formula adjusts the control signal based on the
error. By adjusting the Proportional Gain (KP ) high, the system would response more
quickly and aggressively to the setpoint, but it may overshoot and oscillate if the value
is too high. Setting a low gain value will make the system less responsive than needed.

P = KP · e(t)

Here are the results of too low, ideal and too high P-Gain settings:

Figure 5: Examples of different P Gain values

However, only the Proportional Control alone is not enough since it does not eliminate
the steady-state error (persistent offset) and also the time-consuming convergence to the
setpoint.

Figure 6: Steady-state Error

MTH251 Page 8/25

 University of Science
Faculty of Mathematics and Computer Sciences

3.2.2 Integral Control (I)

To get rid of the steady-state error of the slow P-Controller, the integral term could be
utilized to accumulate the error over time and therefore adjusts the control variable to
the setpoint:
t
I = KI · e(t)dt
0
The incorrectly tuned Integral gain may results in under-damped (slow reaction) system
or oscillating behavior. The following image contains some examples of the behavior of
the system if the Integral Gain is set too low, ideal and too high.

(a) I Gain too low (b) I Gain ideal (c) I Gain too high

Figure 7: Examples of several values of I-Gain

3.2.3 Derivative Control (D)

The derivative term predicts future errors by considering the rate of error change.

de(t)
D = KD ·
dt
Implementing this term will help increase the system damping effect since the rate of
change (velocity) of the error is considered. However, to high Derivative Gain may also
lead to extensive overshoot of the system and noise is also a significant factor to be
considered when tuning this constant.

(a) D Gain too low (b) D Gain ideal (c) D Gain too high

Figure 8: Examples of several values of D-Gain

3.2.4 Combined PID Control

t
de(t)
u(t) = KP · e(t) + KI · e(t)dt + KD ·
0 dt

MTH251 Page 9/25

 University of Science
Faculty of Mathematics and Computer Sciences

In order to make the system behave ro-

bustly and perfectly, PID-tuning procedure must
be conducted, in which of Proportional, In-
tegral and Derivative Gains must be deter-
mined. Each parameter has different physi-
cal influence on the system, therefore it will
be time-consumming to find the perfect conbina-
tion.

Figure 9: Combined PID Control

3.3 Artificial Potential Field
Artificial Potential Field is an approach to robot Path Planning problem that uses imagi-
nary field generated on the robot’s working environment, being a foundation for external
Path Planning algorithms. The APF often consists of two types of force:

• Attractive Force centered at the final desired position in robot’s navigation acts as
a directing tool.

• Repulsive Force placed along the edge of the obstacles helps repelling the robot
when it gets close to the obstacle, providing the collision-free capability for the
robot’s navigation.

Another intuitive representation of the APF is by

considering about the geographical meaning of it.
Each of the obstacles can be assigned a ”height”,
contributing to the total geographical terrain of the
robot’s working environment. The closer the vehi-
cles gets to an obstacle, the steeper the slope it has
to climb, the artificial gravitity will pull the vehi-
cle down, hence, repelling it away from the moun-
tain, or in this case, the obstacle. Through out this Figure 10: Artificial Potential Field
project, we will be using the geographic interpretation of APF as a supporting tool for
the Genetic Algorithm introduced.

3.4 Genetic Algorithm

Genetic Algorithm is a heuristic adaptive search algorithm, part of the Evolutionary
Algorithm which taken inspiration from the natural evolution of organisms. Genetic
Algorithms iteratively modiy the genetic encoded chromosome of each individuals in a
population with the principle of crossover and mutation, mimicking the evolution. The
population then runs through a fitness function to be evaluated, or eliminated if its
performance is terible.

MTH251 Page 10/25

 University of Science
Faculty of Mathematics and Computer Sciences

(a) Genetic Algorithm example (b) Purpose of mutation

Figure 11: Genetic Algorithm Visualization

Relying on the natural selection principle, the chromosomes that yielded better perfor-
mance (elites) will proceed to the next evolution, converging to an answer of the problem
that we are trying to solve. Also, the randomness of natural mutation would provided
the diversity of individuals that may produce better results, hence helping the model
escape out of the local minima to find a better one.

3.5 Gaussian Distribution

The Gaussian distribution, is a cornerstone of prob-
ability and statistics, distinguished by its symmet-
ric, bell-shaped curve. Its prevalence in various
natural process, which asserts that the aggregate
of numerous independent random variables. This
help the system to deal with the unexpectable real-
life conditions.
Figure 12: Gaussian Distribution
3.6 Binary Search Algorithm
Binary Search Algorithm is a method of finding an element in a sorted array. It does
this by iteratively divide the array in half, looking for the element in either left and right
partitions and ignore one them if the element lies on the other. This is a very efficient
algorithm in searching problems, especially for those with sorted values like ours because
by repeatedly cutting the problem in half, the complexity is on the logarithmic scale.

Figure 13: Binary Search Visualization

MTH251 Page 11/25

 University of Science
Faculty of Mathematics and Computer Sciences

4 Methodology
Problem Formulation
Given a car in a 2D map with predetermined start point and end point and circular
obstacles that have randomly generated radius. The goal of the project is to introduce a
collision-free Path Planning Algorithm and a Trajectory Tracking method to guide the
car along the path. The factor that we are trying to minimize is the danger of the Bezier
path. The closer the path is relative to the obstacles, the more danger the path yields.

4.1 Path Planning Algorithm

The Path Planning Algorithm involves the presence of Artificial Potential Field, Bezier
Curve and most importantly, Genetic Algorithm. The general training procedure of our
model can be described as follows:

1. Introduce a Bezier to Chromosome encoding strategy: The encoding

scheme is a universal definition of the chromosome and any latter functionality,
model training procedure will have to surround this definition.

2. Determine a general definition of path’s danger: Artificial Potential Field

will present in this part, help generating tools needed to evaluate the chromosomes
latter on.

3. Initialize the first population: Create random initial chromosomes.

4. Artificial selection: Artificial Fitness Function is defined and will be compli-

mented with APF to evaluate the population.

5. Artificial evolution: The population will undergo the crossover and mutation,
mimicking the natural evolution of animals and organisms.

6. Loop: Return to step 4 until the number ofatraining epoch runs out.

4.1.1 Bezier to Chromosome Encoding Strategy

The chromosomes of the Genetic model is defined as a sequence of control points of the
Bezier Curve which will act as genes. For instance, a curve that has P0, P1, P2 and P3
as control points will be encoded into P = {P0, P1, P2, P3} as chromosome. Since the
start and end positions of the car on the 2D map are known, we will assign the first
and last control points in the sequence corespond with them, which means the sequence
{P0, P1, P2, P3} will become {PS, P1, P2, PE}.

MTH251 Page 12/25

 University of Science
Faculty of Mathematics and Computer Sciences

Figure 14: Chromosome Encoding Strategy

4.1.2 Danger Map Generation

For the static 2D map with size H × W , with H and W are the height and width (in
pixels) of the map respectively, we will compute the level of danger of each pixel on the
map based on its distance from all surrounding obstacles

• (x, y): Coordinates of a pixel.

• (Ox,i, Oy,i): Coordinates of the i-th obstacle’s center.
• ri: Radius of the i-th obstacle (fully dangerous region).
• S: Vehicle size.
• Ri = ri + S: Outer limit radius for the i-th obstacle.
✓
• di(x, y) = (x − Ox,i)2 + (y − Oy,i)2: Distance between the pixel (x, y) and the
i-th obstacle.

• Di(x, y): Danger level contribution from the i-th obstacle.

The contribution of each obstacle to the danger level is calculated as follows:

1, if di(x, y) ≤ ri,
log10(di(x,y)−ri)
Di(x, y) = 1− log10(Ri−ri)
, if ri < di(x, y) ≤ Ri,
0, if di(x, y) > Ri.

MTH251 Page 13/25

 University of Science
Faculty of Mathematics and Computer Sciences

Cumulative Danger Level

The total danger level at a pixel (x, y) is the sum of danger level from all obstacles:
N
\_
obs

D(x, y) = min Di(x, y), 1

i=1

where Nobs is the total number of obstacles.

4.1.3 Initialize Population

At the start of the training procedure, an initial population needs to be generated.
The number of individuals or population size C will be fixed throughout the training
procedure. Each chromosome contains some initial amount of random genes.

4.1.4 Fitness Function

We defined the fitness values of the each chromosomes be the average danger level over
the associated Bezier Curve by taking the line integral and then divided by the curve’s
length.
P ⇒ B(t)
f s1
D(B(t))dl D(B(t))dt
ffit(B(t)) = L = 0
L L
where

• B(t) is the Bezier curve parametric equation with respect to t converted from P
which yields the coordinate of the local point with specific value of t

• L is the length of the Bezier curve

In computer programming, the finess values can be calculated by replacing the line inte-
gral with the sum of danger values of each discrete local point on the Bezier Curve divided
by the number of points that we are calculating with. First, we create Nbres number of
evenly spaced t values from ranging from 0 to 1. It is called ”Bezier Resolution”.
1 1 1 1
t = {0, 1 × ,2× ,3× , ..., (Nbres − 1) × , 1}
N bres N bres N bres N bres
After that, the average danger level of each chromosomes can be calculated as follows:
f
1, if ∃Oi that ∥PB(Oi) − Oi∥ ≤ ri
f =
fit 1 "£Nbres
Nbres i=0 D(⌊B(t i)⌋)

The highest possible fitness value (most inefficient chromosome) returned is 1 when
the chromosome crosses an obstacle. The ⌊B(ti)⌋ operation is used to round down
the point’s position, calculate the nearest pixel with integer value since the parametric

MTH251 Page 14/25

 University of Science
Faculty of Mathematics and Computer Sciences

equation yields floating point values. The function PB() is used to find the current point’s
projection on the Bezier Curve, which we will define later in the Trajectory Tracking
section.
After running through the finess function, the chromosomes will be separated into 2
groups: elites (fitness values < TS) and non-elites (the rest) with TS is the Separation
Threshold.

4.1.5 Crossover and Mutation

Crossover
The crossover process will then be performed separately on 2 groups: elites and non-
elites. In each group, we will select a proportion RC (crossover ratio) of chromosomes
to perform crossover. The process is defined as randomizing a crossover position on the
interval [0; min(LPA , LPB )] with LPA and LPB are the lengths of 2 parent chromosomes
respectively since the length of chromosomes throughout the training procedure may
vary.
After that, the genes behind the crossover position of the 2 parental chromosomes will
be exchanged.

Figure 15: Crossover mechanism

Mutation
There are three types of mutation going to be used: add gene, remove gene and edit gene.
The mutation process will be conducted only on a small proportion named Mutation
Ratio RM of the non-elite chromosomes. The mutation position will also be randomized.

Figure 16: Add gene

MTH251 Page 15/25

 University of Science
Faculty of Mathematics and Computer Sciences

The additional gene will be randomized in the scope of the map size

Figure 17: Remove gene

Randomly select a gene to be removed from the chromosome

Figure 18: Edit gene

The chosen gene will be re-randomized in the scope of the map size.

4.1.6 Overal Algorithm Workflow

Figure 19: Algorithm Workflow

MTH251 Page 16/25

 University of Science
Faculty of Mathematics and Computer Sciences

4.2 Trajectory Tracking Algorithm

The algorihm we introduce uses PID Controller with and the supporting Binary Search
Algorithm to hhelp the car stays on track throughout the collison-free motion. There
are several aspects need to be handle in order:

1. Introduce the vehicle’s model: Propose the 2D motion of the car mathemati-
cally.

2. Load the path: The procedure starts by loading the best chromosome saved in
the local memory that has been trained from the Genetic Model earlier. Then the
chromosome will need to be converted into the Bezier Curve

3. Control loop:

• Find the car’s projection on the Bezier Curve

• Calculate error
• Calculate PID
• Update vehicle’s state

4.2.1 Vehicle model

The general motion equation of the car can be expressed with a set of equations:
t

X(t) = Vcar · ⃗u (t)dt

0

⃗u (t) := R(ω(t)dt) · ⃗u (t)

where:

• ⃗u (t) is a unit vector, representing the direction that the car is currently heading.
(cos(α) −sin(α) l
• R(α) = is the rotation matrix.
sin(α) cos(α)
The position of the car with respect to time X(t) is obtained by continuously integrating
velocity Vcar · ⃗u ( t) with respect to time. To further simplify the problem, we define the
speed of the car to be constant, hence the equation becomes:

X(t) = X0 + Vcar · ⃗u (t)dt

The heading vector ⃗u (t) at any time t is determined by continuously rotating the vector
itself from the beginning of the simulation. The := operation means assigning the value
back to the variable. In order to control the car, helping the vehicle follow the generated
path, the introduced PID algorithm is utilized to calculate the angular velocity ωpid each
control loop.

MTH251 Page 17/25

 University of Science
Faculty of Mathematics and Computer Sciences

However, in the simulated environment, all movements are represented through mathe-
matical equations so the car would perform perfectly. In real-world scenarios, physical
vibrations and inherent errors are inevitable. Therefore, incorporating these factors into
the model is essential to validate the system’s effectiveness.

ω(t) = ωpid + ωΩ

where ωΩ is the system noise produced that we formulate as a representation of real

world vibrations and errors that obeys the Gaussian Distribution.

ωΩ = η · x

with 1
2 − 1 ( x−µ 2
N (x|µ, σ ) =√ e 2 σ )
σ 2π
where η is the maximum value of the the angular velocity noise can ever be achieved.

4.2.2 Initialization
• Initial population: Generating C chromosomes
• Car position: Coinciding the starting position.
• Car heading: Facing toward the ending position.
• Load terrain: Loading map with obstacles have been pre-created earlier.
• Load best chromosome: Executing the control points position of the chromo-
somes with the lowest fitness score throughout the epoches.

• Convert chromosome to Bézier path: Convert the saved chromosome into

Bezier Path.

4.2.3 Control loop

Find projection
Determining the projection of a point on the Bezier Curve is the back bone principle
of our PID control flow. Firstly, to minimize the computing time, we need to find a
good starting point Btmid on the curve before performing the Binary Search Algorithm
by iterating over Nbres t values in t.

tmid = arg min∥B(t) − K ∥

t∈t

where K is the point that we are trying to find projection of.

Then we know for the fact that B(tmid) is the closest point from K along the points in
the Bezer resolution, the more optimal t value that yields a more optimal solution must

MTH251 Page 18/25

 University of Science
Faculty of Mathematics and Computer Sciences
(
lies on the interval tmid −Nbres
1
; tmid + Nbres
1 .
and t = tmid + 1
Let us define tleft = tmid − Nbres
1 right Nbres The algorithm works by
repeatedly do the following operations in order:

1. Calculating the distance between K and the two middle points of the intervals
[tleft; tmid] and [tmid; tright]

tleft+tmid tmid+tright
d1 = B 2 − K and d2 = B 2 −K

2. Compare d1 and d2 to detemine which interval does the answer lies in

3. Ignore the interval that doesn’t have the answer then update the left and right
boundaries by assigning new values to tleft and tright

• tright = tmid and keep tleft the same if d1 < d2

• tleft = tmid and keep tright the same if d1 ̸= d2
+tright
4. Update tmid = tleft 2

5. Check termination condition and repeat step 1 if not satisfied.

The we define a Termination Threshold TBS if tright − tleft ≤ TBS then the algo-
rithm will yield the result B(tmid)

Calculate error
To steer the vehicle along the generated path, we first need to determine its position
beyond the current step.

1. We define point Z be the predicted position of the car by calculating it’s state
using current calulcated data in the loop.

2. Then we find the projection P of Z on the Bezier Curve.

3. Then we calculate the error Yerr, which we will be using for the PID control.

Yerr = ( ⃗u × Z⃗P ) · ∥Z − P∥

MTH251 Page 19/25

 University of Science
Faculty of Mathematics and Computer Sciences

Figure 20: Calculating Error

Calculate PID
• Propotional(P): Adjusting the control output in proportion to the error magni-
tude.The larger the error, the stronger the corrective action.

Pterm = PI · Yerr

• Integral (I): Addressing the accumulation of past errors. It is designed to elimi-

nate steady-state errors that cannot be corrected by the proportional term alone.
t

Iterm = KI · Yerr dt
0

• Derivative(D): Predicting the future behavior of the error by considering its rate
of change. This term helps to dampen oscillations and improve stability.
dYerr
Dterm = KD ·
dt
• Control Variable ωpid: Minimizing deviations from a desired target or setpoint.
t
dYerr
ωpid = KP · Yerr + KI · Yerr dt + KD ·
0 dt

MTH251 Page 20/25

 University of Science
Faculty of Mathematics and Computer Sciences

PID Workflow

Figure 21: PID Workflow

Update vehicle
• Heading: The heading vector, representing the vehicle’s orientation, is rotated
by an angle of plus a noise term to account for uncertainties or disturbances.
(cos(ω + ωΩdt) −sin(ω + ωΩdt)l
⃗u(t)
sin(ω + ωΩdt) cos(ω + ωΩdt) ·
• Noise: Introducing random disturbances into the vehicle’s steering control by gen-
erating noise based on a normal distribution. This noise simulates the real-world
unpredictability of vehicle dynamics and environmental effects.

ωΩ = η · x
• Position: The vehicle’s position is incremented in the direction of the updated
heading vector, scaled by the vehicle’s velocity and normalized by the frame rate.
This step ensures the vehicle’s movement is consistent with its velocity and the
simulation’s time resolution.

X(t) = X0 + Vcar · ⃗u (t)dt

MTH251 Page 21/25

 University of Science
Faculty of Mathematics and Computer Sciences

• Normalized heading: After the heading is updated, it is normalized to maintain

a consistent magnitude. This normalization ensures numerical stability and avoids
issues caused by cumulative floating-point errors during repeated updates.

⃗u (t)
∥⃗u (t)∥

4.2.4 Overall Workflow

Figure 22: Overall Workflow

MTH251 Page 22/25

 University of Science
Faculty of Mathematics and Computer Sciences

5 Results and Simulations

5.1 Path Planning Algorithm
Our algorithm yields a collision-free Bezier path that perform astoundingly well.

(a) 2D Map with Obstacles (b) 3D Visualizer

Figure 23: Generated Map

5.2 Trajectory Tracking Algorithm

Figure 24: Car following path

MTH251 Page 23/25

 University of Science
Faculty of Mathematics and Computer Sciences

6 Summary and Conclusion

This report presented the approach to the solve the two well-known problems: Path
Planning and Trajectory Planning by utilizing Genetic Algorithm, Artificial Potential
Field, Bezier Curve and PID Controller. The Bezier Curve has provided an efficient of
way navigation for the simulated vehicle and the PID Controller has successfully guide
the car along the generated path from the starting to the ending position. However,
although the proposed approach has provided a robust way of find and tracking paths
but there is still some limitations:

• We have only tested on static maps, dynamic maps with moving obstacles requires
more complex algorithms to make the robot navigate.

• The Bezier Curve with high number of control points (high level Bezier Curve)
possess limitations in local control, moving one control point may impact a large
portion of the generated curve. As a result, the process of crossover and mutation,
which posses large modifications in the shape of the path may produce invalid chro-
mosomes in the latter generation although their parent chromosomes performed
astoundingly well with very low fitness score and vice versa. As having mentioned
ealier the inheritance property of crossover in Genetic Algorithm will be the key
to make the model converge to the solution, and the randomness in mutation will
help the model escape from local minima. If the randomness effect out-perform
the convergence effect, it would take along time to train the model.

In sumamry, our project has successfully solve the 2 mentioned problems and could be
used as a foundation or research inspiration for other researchers and engineers. We hope
to develop more complex algorithms, starting from what we have achieved to better the
perfomance of robots

MTH251 Page 24/25

 University of Science
Faculty of Mathematics and Computer Sciences

References
[1] G. E. Ji-wung Choi, Renwick Curry. Path planning based on bezier curve for au-
tonomous ground vehicles. In Advances in Electrical and Electronics Engineering,
World Congress on Engineering and Computer Science, WCECS, San Francisco, CA,
USA, 2008. IEEE.

[2] C. Lamini, S. Benhlima, and A. Elbekri. Genetic algorithm based approach for
autonomous mobile robot path planning. Procedia Computer Science, 127:180–189,
2018.

[3] W. Y. Y. L. Z. Z. Ling Zheng, Pengyun Zeng. Bezier curve-based trajectory planning

for autonomous vehicles with collision avoidance. The Institution of Engineering and
Technology, 14:1882–1891, 2021.

[4] P. G. Luan and N. T. Thinh. Hybrid genetic algorithm based smooth global-path
planning for a mobile robot. Mechanics Based Design of Structures and Machines,
51:1758–1774, 2021.

[5] A. E. H. Mohamed Alhoseny, Alaa Tharwat. Bezier curve based path planning in a
dynamic filed using modified genetic algorithm. Journal of Computational Scienece,
pages 339–350, 2017.

[6] X. Y. Mohamed Alhoseny, Abdulaizz Shehab. Optimizing robot path in dynamic

environments using genetic algorithm and bezier curve. Yournal of Intelligent Fuzzy
Systems, 4:2305–2316, 2017.

[7] M. Pomax. A primer on bézier curves, 2020. URL https://pomax.github.io/

bezierinfo/.
[5] [3] [1] [6] [4] [2] [7]

MTH251 Page 25/25