See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/353314517

Autonomous Driving Software for FS-AI 2021

Technical Report · July 2021

DOI: 10.13140/RG.2.2.26045.18406

CITATIONS READS
0 263

15 authors, including:

Michael Samy Hannalla Ahmed Tarek

Ain Shams University The British University in Egypt
1 PUBLICATION 0 CITATIONS 3 PUBLICATIONS 2 CITATIONS

SEE PROFILE SEE PROFILE

Salma Ibrahim
Ain Shams University
1 PUBLICATION 0 CITATIONS

SEE PROFILE

All content following this page was uploaded by Michael Samy Hannalla on 17 July 2021.

The user has requested enhancement of the downloaded file.

 Autonomous Driving Software for FS-AI 2021
Ain Shams University, ASU Racing Team
Michael Hannalla, Haidy Sorial, Ahmed Tarek, Salma Ibrahim, Ahmed Hesham, Islam Ayman,
Laila Elgenedi, Abdelrahman Ayman, Ahmed Samy, Omar Fadl, Kareem Alsawah,
Aly Alkady, Nesma Walid, Hassan Amr, Moataz Elashery
Abstract—Self driving cars field is a very active area of
research that impacts and benefits everyday life on roads. This
document demonstrates the phases undergone by our team to
develop an autonomous vehicle, to participate and compete
in the 2021 Formula Student AI competition. We include the
state-of-the-art schemes we studied and the diverse methods we
endeavored to approach our problem.
D ISCLAIMER
This paper is written as an autonomous design report to be
submitted to FS-AI judges in Formula Student competition.
Content may include mentioning trademarks and manufactur-
ers for sensors, computers, or software. These manufacturers
do not supervise or endorse any of the content in this paper.
I. I NTRODUCTION
The aim of this work is to provide state of the art approaches
in solving the problems facing autonomous driving starting
from the generalized perception in most of weather conditions
and robust to the scale of the image semantic content moving
through providing the car with conscious strategic maneuver
when required. The work would be tested against standards
benchmarks in measuring the performance of self-driving
vehicles and partnered with the industrial pioneers in the field
of autonomous vehicles.
II. S YSTEM OVERVIEW
This section provides an overview on the hardware and
sensors used, high level architecture of our autonomous system
from the very high level perception to the lowest level of
control and commands sent to the ADS-DV1 via the CAN
bus. Fig. 1 shows the high level architecture of our software.
We use Velodyne VLP-16 LiDAR, FLIR Backfly S monocular
camera, and ZED stereo camera for perception. Along with an
IMU, GPS, and feedback from wheel and steering encoders
for state estimation. Perception sensors are used in different
pipelines for cone detection and drivable space estimation.
State estimates and cone detections are fed to the SLAM
module that fuses all these together to output an optimal state
estimate and a map. The map and pose estimate are then taken
by the planning module to output waypoints that the navigation
module sends control commands via the CAN bus to track and
follow these waypoints.
All of our software is based on ROS installed on Ubuntu.
Our computing devices are the prefitted InCarPC and an
NVIDIA Jetson TX2. Computers and sensors are connected
together to be allowed to communicate using an Ethernet
network.
III. P ERCEPTION
This section demonstrates the different cone detection
pipelines and utilities we used in our system, mentioning both
laser based and vision based methods.
A. LiDAR Pointcloud Pre-processing
1) Field of View Trimming: Due to the LiDAR’s placement
on the vehicle and the wide surrounding, there are too many
irrelevant points which we need to get rid of. We start by
applying pass-filters to trim the field-of-view, a longitudinal
to remove the vehicle points and anything far away, and a
lateral one to limit the side field.
2) Adaptive Ground Removal: After applying the FOV
trimming, we need to filter out the ground points. We apply an
adaptive algorithm that acclimates to changes in the inclination
of the ground. We use a RANSAC algorithm to segment the
ground plane from the cones’planes; treating the ground points
as outliers and leaving us with the non-ground points.
3) Clustering: For point cloud clustering, we made a choice
using the Euclidean distance Clustering Algorithm by the
means of a K-D tree as the base spatial locator class for
nearest neighbor estimation. This approach is implemented by
subdividing the space into boxes of fixed widths, or in a more
general case an octree data structure. This method is very fast
to build and gives us a useful representation of the data in
every resultant 3D-box.
4) Cone Reconstruction: The next reasonable step is to
reconstruct the clusters obtained, as carrying out the ground
removal process causes the loss and elimination of some
cone points along with the ground points.Unfortunately, this
dwindles the already modest number of points used in cone
detection. This is solved by restoring a cylinder-shaped surface
area of points with a diameter equal to that of the cone width
around each cluster using points of the antecedent point-cloud
(before ground removal). This way, we can assure that most of
the mistakenly revoked points are retrieved, so our detection
and colour pattern estimation processes can transpire smoothly
and more accurately.
1 ADS-DV is the shared formula vehicle that FS-AI DDT (Dynamic Driving
Task) competitors use to deploy their autonomous systems on during the
competition. This vehicle has a prefitted ZED camera and an InCarPC
computer.
 Fig. 1. High-level software architecture
5) Filtration: After the cluster reconstruction, each cluster
should be passed through two filters to ascertain the fact that
all the clusters used for detection are truly cones. This is
done by the means of two filters; the Rule Based Filter and
the Z-Centroid Filter. The former depends on calculating
the expected number of points in a cone according to its
distance from the vehicle, cone dimensions, and the LiDAR
specifications, then comparing this value to the substantial
cluster number of detected points. If the difference between
the two numbers does not exceed a certain threshold, then
the cluster passes by the latter filter. Since all cones are of
equal dimensions, consequently they should all have the same
centroid position. However, the variance in their distances
from the vehicle results in having a certain range for the
centroid values instead of one specific distinct number. This
filter assures that all cone cluster centroids lie within that
range. Otherwise, the cluster is not regarded as a cone and
it does not undergo the colour estimation process.
B. Image Object Detection
As the tracks are delineated by blue, yellow and different
size orange cones, our goal is to localize the cones and
differentiate between their different colours with high accuracy
and low latency for safe maneuvering through the track. We
tackle the detection challenge using the state-of-the-art model,
YOLOv3 [1] [2] architecture. To have our model fit our
specific problem, we train the model using open source racing-
cones dataset, which we modified to also differentiate between
different cone-colours.
C. Mono-LiDAR Cone Detection Pipeline
In this section, we fused between two sensors: a camera
and LiDAR. The LiDAR as a stand-alone sensor gets accurate
depth and localization, while the camera as a stand-alone
sensor gets accurate colors. By fusing the two sensors, we
leverage the measurements coming out from the two sensors
individually. We mainly rely on projection of the LiDAR
points on the camera’s image plane to get the pose of the
cones and their colors.
Let’s denote C as the incoming clusters from the LiDAR
pre-processing step as a stacked matrix of column vectors and
each column C i as i-th cluster in the incoming clusters from
the LiDAR pre-processing step.
Where L C are the clusters given in the LiDAR’s coordinate
frame and N represents the number of points in the point
cloud. We then need to transform the points from the LiDAR
frame to the camera frame using simple homogeneous trans-
formation as follows:
C
C = C TL ∗ L C
Where C C are the clusters transformed to the camera’s
coordinate frame and C TL ∈ SE(3) represents the homo-
geneous transformation between the LiDAR frame and the
camera frame.
We then need to project these points from the 3D camera
frame to the image plane as follows (subject to matrix broad-
casting and vectorization):
      
U 1 fx 0 X c used to correct the state estimates. GPS was used to correct
=Z + x
V 0 fy Y cy
Where U is a stacked row vector for u-coordinates for
clusters projected on the image plane, V is another stacked row
vector for v-coordinates for clusters projected on the image
plane, f x is x-axis focal length, f y is y-axis focal length, cx
and cy are u-coordinate and v-coordinate of the center pixel
on the image plane.
Since we already have both the bounding box coordinates
- from the object detector module - and the projected LiDAR
clusters on the image plane, we use bounding box coordinates
to find which cluster from the LiDAR are belongs to the
bounding box. By reaching this points, we have got the 3D
coordinates of the object defined inside the bounding box.
D. Stereo Cone Detection Pipeline
In order to localize cones in the world frame, this pipeline
has two main components; first of them being the cones’
bounding boxes and the second being the disparity map. Since
the bounding boxes, accompanied with its colour, are already
obtained from object detection module, this section focuses on
obtaining 3D coordinates of the cones.
We start by rectifying the left and right images to remove
any distortion. Then disparity matching is computed with
reference to the left frame with a confidence score for every
disparity-pixel. For every cone we obtain a disparity region
by indexing the disparity map using the cone’s bounding
box coordinates. The disparity region may contain different
values for the cone so we choose the pixel with the highest
confidence score as it being the most reliable. Using disparity
value, bounding box center coordinates and intrinsic camera
parameters we calculate the cone extrinsic parameters by using
the stereopsis equations.
IV. S TATE E STIMATION
A. Motion Estimation
A standard extended Kalman filter is used to predict
and hcorrect the system states. The proposed state vector
iT
x = Px , Py , ψ, vx , vy , ψ̇ ∈ R6 where Px and Py are the
vehicle’s position, vx and vy are the longitudinal & lateral
velocities respectively, and ψ is vehicle’s yaw angle.
In our proposed system, a constant acceleration process
model is used since jerk is close to a zero mean Gaussian
distribution. Hence, the positions and velocities are propagated
using the acceleration and yaw rate from the IMU, resulting
in the following process model.
T T T
[v̇x , v̇y ] = [ax , ay ] + ψ̇ [vy , −vx ] + nv
ψ̇ = nψ̇ (1)
ȧ = na
For the correction step of the EKF, multiple sensors were
position as well as the local longitudinal and lateral velocities
of the vehicle with the following measurement model.
   
Pxgps Px
 Pygps   Py 
 vxgps  =  vx cos ψ − vy sin ψ
    + ngps (2)

vygps vx sin ψ + vy cos ψ
A magnetometer was used to correct vehicle yaw estimate.
Before correction, calibration is done for a variable duration
to register initial heading measurement by averaging all the
readings within this duration and use it as reference for later
estimates. This will ensure that magnetometer heading is
always zero at starting point.
LiDAR scans were also used to achieve odometry through
Advanced LOAM implementation based on the paper [3] by
J. Zhang and S. Singh. Additionally, visual odometry from
ZED stereo camera was also used to correct vehicle’s position
& yaw angle to overall provide a reliable and redundant
system for estimating vehicle states.
In order to achieve sensor redundancy and protect the
autonomous system from sensor failure, wheel encoders were
used to correct local longitudinal and lateral vehicle velocities
as well as yaw rate through the implementation of the tire slip
model [4] but with a constant slip assumption.
B. SLAM
As our system has to navigate autonomously in an unseen
environment; we don’t know the shape or length of the race-
track. This leads us to deploy SLAM algorithms in our system.
Our system needs to map cones of the track and estimate the
vehicle position within these landmarks of interest (cones).
Our algorithm is based on FastSLAM 2.0 algorithm proposed
in [5] which is a probabilistic approach to SLAM based on the
concept of Rao-blackwellization. Rao-blackwellization is the
splitting of the SLAM problem into pose estimation problem
and independent landmarks estimation using N number of
Kalman filters, one for each landmark as its position state
estimator. Where N is the number of mapped landmarks
(cones).
Due to the challenges imposed by FS-AI, we modified
the base FastSLAM in the literature to suit our needs and
requirements.
First, we made our landmark estimator estimate, not only
landmark’s position, but also estimate its color - yellow cone or
blue cone for example in our case - and account for perception
uncertainties. Landmark’s color matters in our system because
we then pass this map to the planning module that decides
which path to follow.
Also, as the main core for SLAM to provide correct
estimates for landmarks and poses, the vehicle should be able
to decide if its perception system is seeing again a previously
mapped landmark. We used the Mahalanobis distance metric
to decide whether this is a new landmark or a landmark previ-
ously mapped and being seen again, we chose the Mahalanobis
distance to account for measurement uncertainties.
For loop closure, we exploit the prior knowledge that the
race track starts and ends by a special type of cones (mentioned
in FS-AI rule-book). Once we detect these cones and localize
them, we can know that this is where the zero position of
the vehicle started with full certainty. This triggers our loop
closure algorithm to optimize and refine the map that was
incrementally drifting before loop closure due to odometry
and measurement inaccuracies.
As the vehicle is moving along the track for the first time,
we take the outputs from FastSLAM and construct a pose
graph that gets optimized once loop closure conditions are
met. We used a pose graph optimization algorithm to optimize
the map after loop closure. This optimized map then is used
in subsequent race laps (in challenges that has two or more
laps) by the vehicle to only localize in it and doesn’t perform
map updates. Thus shutting down the SLAM process and only
performing localization.
The optimized map is then used by a Monte Carlo based lo-
calization algorithm to determine the vehicle’s location within
this map. This gives us the capability for faster pose estimate
update rate thus allowing faster navigation.
V. PATH P LANNING
Our path planning problem is defined as finding suitable
goal way-points within the track in order for the navigation
pipeline to move the vehicle correctly.
The path planning module has two inputs. The map (cone xy
coordinates in the global frame), cone colour, and the vehicle
pose (xy coordinate and its heading angle) also in the global
frame. In each time-step, the input map consists of all cones
seen by the vehicle in this time-step and all cones seen by
the vehicle in previous time-steps. Thus, preprocessing and
filtering of this map is necessary.
A. Map Preprocessing
First, the map is transformed to the local frame of the
vehicle. Then, only the cones within a specific distance and
position from the vehicle are chosen. So the planning process
will only be done on cones which are near the vehicle and are
in a specific x-Coordinate in the vehicle’s local frame. Those
cones are then transformed back to the global frame and then
are separated using their colour (yellow, blue, etc..)
B. Delaunay Triangulation
The preprocessed cones are then given to a triangulation
meshing technique called Delaunay triangulation which is used
to extract all midpoints between all of these cones. Delaunay
triangulation is also used in constructing an edges matrix,
which is a matrix consisting of all pairs of cones making
the midpoints. The midpoints matrix and the edges matrix are
synchronized together and used in both the cost function and
the constraints.
C. Cost Parameters
Those midpoints are then given costs and passed through a
cost function so that the midpoints with the lowest costs are
chosen as goal way-points. Those way-points are then passed
to the navigation module.
1) Distances: The first cost parameter is the distance be-
tween each midpoint and the vehicle pose in the current time-
step. This is calculated as a euclidean distance. Then all the
distances between the vehicle and all midpoints are passed
through a softmax function to make all costs between zero
and one.
2) Heading Angles: Secondly, the angle between the vehi-
cle heading vector and each vector between the vehicle and
each midpoint is calculated. Thus, midpoints that require a
small steering angle or no steering at all are given a low cost.
All angle costs are passed through a softmax function as well.
3) Voronoi meshing: Voronoi Meshing is a geometric mesh-
ing technique that constructs polygons around the cones given
certain geometric constraints. Some of these polygons are
unbounded. Thus, all midpoints that are positioned inside or
on the edge of bounded polygons are given a zero cost, and
all midpoints that are placed outside the bounded polygons are
given a high cost(unity).
D. Cost Function
After calculating all the different costs for each midpoint,
a cost function is constructed to calculate the total cost for
each midpoint where each cost parameter is multiplied with
its respective weight. Then, a specific number of the lowest
cost midpoints are returned. The cost of each midpoints is as
follows:
(Wd * Distances) + (Wh * Headings) + (Wv * VoronoiOutput)
E. Constraints
Only one constraint is performed using the edges matrix. As
a result of the track layout, a way-point can not be between
two blue cones or between two yellow cones. Any midpoint
between two cones with the same colours is either outside the
track or on the track boundaries. Thus, all these midpoints are
neglected.
F. Edge Cases
If the preprocessing of cones outputs less than 3 cones
or only cones from the same colour, the algorithm will not
function correctly because of its constraints and because of the
meshing constraints. Thus, all of these edge cases are handled
to make sure the planning process computes smoothly with no
struggles.
 VI. NAVIGATION C ONTROL
We implemented geometric control to use for the first lap
while mapping, after the first lap the vehicle can speed up
using model predictive control (dynamic control) to maximize
speed and test its boundaries. We participate in the DDT
category and the ADS-DV has two control modes, speed
control will be used for geometric control while torque control
used for model predictive control.
A. Geometric Control
PID control for longitudinal control [6] and pure pursuit or
Stanley for lateral control [7].
1) Pure Pursuit: Simple lateral control algorithm, dated
back in history to the pursuit of missile to a target. It simply
aligns the heading of the vehicle to a target at a set distance
on the path.
2) Stanley: We used a Stanley controller for lateral control
as it tries to minimize the cross-track error (distance from the
given path) and the heading error (difference between current
heading and the tangent to the path’s heading).
B. Model Predictive Control
1) Problem Formulation: We need to convert our Optimal
Control Problem (OCP) to a non-linear problem (NLP) to
solve it and get the minimum for our optimization variables
using multiple shooting with Runge-Kutta 4 discretization.
Cost Function
Running (stage) Costs: path tracking of the reference track
saved after the first lap
2 2
`(x, u, ∆u) = kxu − xr kQ +ku − ur kR +k∆u − ∆ur kRrate
(3)
Constraints
We have a set of constraints limiting the states and controls,
in addition to the equality constraints on the dynamics of the
vehicle to avoid non continuous states. Also, we added track
boundaries as an inequality constraint to avoid going out of the
track where, for example, the steering angle limit represents
the physical limit of the steering system. [8]
PN −1
minimize JN (x0 , u) = k=0 ` (xu (k), u(k), ∆u(k))
u,x
subject to:
xu (k + 1) = f (xu (k), u(k))
2 2 2
(Xk − Xcen ,k ) + (Yk − Ycen ,k ) ≤ RTrack ,k
xu (0) = x0
u(k) ∈ U, ∀k ∈ [0, N − 1]
xu (k) ∈ X, ∀k ∈ [0, N ]
2) Prediction Model:
Three models were used, kinematic model, dynamic model,
and a fusion between both models according to the vehicle’s
View publication stats
velocity vx to get more robust control actions for both low
and high speeds.
x̃˙ = λf˜dyn ˜ ˙ ˜
(x̃, ũ) + (1 − λ)fkin (x̃, ũ, ũ)= f(x̃, ũ, u̇)
vx −vx, blend min (6)
λ = min max vx, blend max −vx, blend min
,0 ,1
By using our fusion factor λ, we guarantee that we will get
the best performance for both kinematic model and dynamic
models [9].
ACKNOWLEDGMENT
This project is supported by Autotronics Research Lab, Ain
Shams University, Egypt, and Embotech.
R EFERENCES
[1] J. Redmon, S. Divvala, R. Girshick, and A. Farhadi, “You only look
once: Unified, real-time object detection,” in Proceedings of the IEEE
Conference on Computer Vision and Pattern Recognition (CVPR), June
2016.
[2] M. Bjelonic, “YOLO ROS: Real-time object detection for ROS,”
github.com/leggedrobotics/darknet ros, 2016–2018.
[3] J. Zhang and S. Singh, “Loam : Lidar odometry and mapping in real-
time,” Robotics: Science and Systems Conference (RSS), pp. 109–111, 01
2014.
[4] V. Fors, Autonomous Vehicle Maneuvering at the Limit of Friction.
Linköping University Electronic Press, 2020, vol. 2102.
[5] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “Fastslam 2.0: An
improved particle filtering algorithm for simultaneous localization and
mapping that provably converges,” Proc. IJCAI Int. Joint Conf. Artif.
Intell., 06 2003.
[6] Kiam Heong Ang, G. Chong, and Yun Li, “Pid control system analysis,
design, and technology,” IEEE Transactions on Control Systems Technol-
ogy, vol. 13, no. 4, pp. 559–576, 2005.
[7] J. Snider, “Automatic steering methods for autonomous automobile path
tracking,” 04 2011.
[8] M. Dawood, M. Abdelaziz, M. Ghoneima, and S. Hammad, “A nonlinear
model predictive controller for autonomous driving,” 02 2020, pp. 151–
157.
2 [9] J. Kabzan, M. I. Valls, V. J. Reijgwart, H. F. Hendrikx, C. Ehmke,
M. Prajapat, A. Bühler, N. Gosala, M. Gupta, R. Sivanesan et al., “Amz
driverless: The full autonomous racing system,” Journal of Field Robotics,
vol. 37, no. 7, pp. 1267–1294, 2020.
(4)
(5)