1. Multiple UGV Simultaneous Localization
and Map Sharing
Autonomous Systems Lab
Advanced Controls and Sensors Group
by
Akash Borate

2. Introduction
• Cooperation between human teams and UGV teams.
• Use of cooperative robots on assisting humans in distributed
activities.
• Mobile UGVs used in Search and Rescue (SaR) missions in urban
catastrophic incidents.

3. Approach
• Independent agents divide the task of building a global map.
• Centralized coordination architecture.
• Iterative construction of map through data obtained from range
sensors.
• Metric Maps represented by 2D occupancy grids.

4. Simultaneous localization & Mapping Overview
Classical Slam
• Navigation success is dependent on results of four stages : perception,
localization, cognition and motion control.
• Sensors used in Mobile Robotics can be divided in exteroceptive and
proprioceptive sensors.
• The main energy sensors used in Mobile Robotics are the Light Detection And
Rangings (LIDARs), sonar, radars and the Global Navigation Satellite Systems
(GNSSs).

5. • SLAM : Repetitively improving an existing map, while at the same time localizing
the UGV within that map.
• A good filtering algorithm used can reduce noise from signals.
• The Extended Kalman Filter (EKF) is intended to overcome that difficulty and use
odometry and the observation of spacial references.
• RBPF effective way to solve SLAM problems.
Filter
Type Advantages Disadvantages
Extended Kalman
Filter SLAM
Linear update of the observations
using fractioned updates;
Through sub mapping it is possible
to obtain a solution in constant
time;
It assumes a Gaussian error which
does not exist always;
The non-linear models linearization
can cause divergences;
Rao-
Blackwellized
Particle Filter
Simultaneously keeps several
hypotheses;
With a large number of samples it
can efficiently represent non-linear
or non-Gaussian models;
It estimates the robot's entire
trajectory online;
The number of particles
exponentially
increases with the state's size;

6. Map Representation
• It is important for autonomous UGVs to learn and maintain models of
the environment.
• Main problem to deal with the high dimensionality of the entities
being mapped.
• Detailed 2D geometric maps require huge memory.
• Topological entities rooms, doors, corridors may not require much
memory.
• Two major distinct paradigms for indoor mapping.
• Grid based Maps
• Topological Maps

7. Grid-based Maps:
• Accurate metric maps similarly to a blueprint of the environment.
• Requires precise location of the robot.
• Common example of metric maps is Occupancy grids.

8. Topological Maps:
• Produce graph-like maps more efficiently.
• Vertices correspond to important places or landmarks.
• Difficulties in recognizing similar places that look alike.

9. Implementation of Multi – UGV SLAM
• Algorithm consists of diverse modules.
• Framework used to support Multi UGVs SLAM.
• Each individual UGV runs its own SLAM.
• Receiving partial map, map alignment and merging both maps.
Map

10. Multi – UGV System in ROS
• Robot Operating System (ROS) is
used as the development
framework.
• Uses a publish/subscribe
semantics architecture and a
central master.
• Each node reports its register
information back to the roscore.
• Execution of algorithms are
controlled by such ROS nodes,
which can be programmed in C++,
Python,

11. • Challenge to overcome :UGVs
should have a different namespace
• Use of the tf_prefix parameter is
the recommended when sharing
reference frame transforms.
• Remap topics where the different
UGVs use the same node internally.
Map_exchange
Defining a prefix will turn things like the command velocity
reference of the robot (typically named “/cmd_vel”) into
“drrobot_master/cmd_vel” and “drrobot_slave/cmd_vel”
automatically, depending on which UGV it is.

13. Simultaneous Localization and Mapping approach
Rao-Blackwellized particle filter grid mapping algorithm
Gmapping
• Key idea of the Rao-Blackwellized particle filter for SLAM is to estimate the joint
posterior
p(x1:t, m | z1:t, u1:t−1)
about the map m and the trajectory x1:t = x1, . . . , xt of the robot.
• The process can be summarized by the following four steps:
Sampling: obtained from the generation
Importance Weighting: is assigned to each particle
Resampling: Replacement to their importance weight.
Map Estimation: based on trajectory and history of observation

14. • Require a high number of particles .
• Increase in Computational Complexity.
• Depletion problem associated with the particle filters.
• Therefore, an adaptive resampling technique has been developed
• Taking into account not only robot's motion, but also its most recent
observations.
• Most recent sensor observation and computes a Gaussian
approximation to efficiently obtain the next generation of particles.
• Gaussian parameters are given by the following equations:

15. • Using this distribution, the weight of the ith particle is:
• Resampling is necessary since only a finite number of particles are
used to approximate the target distribution.
• Equation computes the effective number of particles Neff, which is a
trigger that defines when the resampling step should be performed:
• The algorithm resamples each time Neff drops below a given
threshold of N/2, being N the number of particles.

16. • Gmapping is a laser-based
RBPF-SLAM algorithm.
• Gmapping is the most stable
and commonly-used package
with recognized results.
• This package provides a laser-
based SLAM as a ROS node
called slam_gmapping
• The slam_gmapping node
transforms each incoming scan
into the odometry frame of the
robot.
• Shows the average rate of
messages of each transform
and the existing frames.

17. Scan matching algorithm Hector Mapping
• Scan matching is the process of aligning laser scans with each other or with an
existing map.
• The algorithm uses a Gauss-Newton approach for optimization, and formulates
the optimized rigid transformation as
where and denotes the world coordinates.
• Above equation is minimized as Gauss-Newton equation
with Hessian matrix

18. • Gaussian Newton equation rely on the gradient of the map at some
coordinate .
• The gradient can be approximated via bilinear filtering as
where x, y, x0, x1, y0 and y1 are the map coordinates of the integer values P,

19. • An approximation of the match uncertainty can be computed as the
inverse Hessian scaled by some sensor specific constant:
• Procedure is based on gradient descent, getting stuck in local minima
is a source of error.
• Hector SLAM seeks to minimize these effects where the map is stored
at different resolutions.

20. • Hector SLAM works with grid maps, and its underlying principle is
scan matching.
• First LIDAR scan is written to the map. Subsequent LIDAR scans are
matched with the map.
• Transform tree diagram with the coordinate frames associated with
master and slave UGV

21. Map Alignment
• Two different stages: map alignment
and map merging.
• In the alignment stage, the objective
is to find the transformation
between the different reference
systems of the local maps.
• The 2D aligning method computes
the transformation between the two
local maps, which consists of three
alignment parameters: translation in
x and y and the rotation
• ROS provides a package .for
alignment called mapstitch.

22. Map Merging
• Combining the partial maps of all robots into a global map.
• Assume an occupancy grid map M is a matrix with r rows and c columns.
• The spatial location of cell is indicated as . Given two maps, M1 and
M2, and a rigid transformation T.
• The second UGV's environment map is transformed using
• The transformed occupancy grid map of the second UGV , can then be fused with
the first UGV's map.

23. • Stable package available in ROS, called occupancy_grid_utils was
employed.
• When the alignment function returns a possible transform between
the maps to a common referential, that transform may be considered
valid or rejected.

24. Experiments and Results
Basic Simulation results
• Simulation results using the Stage, the ground truth of the
test arena is presented below.
• The UGV is depicted in blue and the seven blocks are shown
in shades of white to black

25. • Figures help us to understand the basic principles of the LIDAR
technology and how the 270 degree vision identifies obstacles
around.
• The goal of this simulation is to test the gmapping and hector_slam
packages.

26. Real World Results
• The proposed results are tested with a team of two Jaguar 4x4
platforms.
• The Jaguar 4x4 is a small lightweight differential wheeled robot ideal
for indoor laboratory or classroom use.
• The robot is equipped with front sonar array and a Laser Hokuyo
URG-04lx-UG014 as seen in below images.

27. • The robots are placed in the arena of approximately 100 square
meters.
• Some photos of such space, located at the Autonomous Systems Lab
126 at University of Texas at Arlington Research Institute.
Arena 1

28. Panoramic View of
Arena 1

29. • The goal of this experiment, is to obtain a global map of the arena
from two partial maps retrieved by the team of two Jaguar 4x4.
Partial map built by drrobot_master Partial Map built by drrobot_slave
Whole map built by merging

30. • In Arena 2 we change the obstacles from which master travels and put extra
obstacle in the room.
Arena 2

31. • Also changed the position of master slave.
Partial map built by drrobot_master Partial Map built by drrobot_slave
Whole map built by merging