SLAM &

Navigation

Textboo
k
P.
312~360 1
Contents
I. Navigation and
components

II. SLAM Practice

III SLAM
. Application
IV. SLAM Theory

V. Navigation
Practice
VI. Navigation
Application Textboo
k
P.
VII. Navigation Theory 312~360 2
Let’s start !
This topic is
SLAM,
Navigation!
3
What
? ㅡ
ㅡ ;;
Let’s go a little
easier!
Path Finding
How is it?
Path
… Path 「 Noun 」

1. A way beaten, formed,

or trodden by the feet
persons or animals
2. A narrow walk or way
3. A route, course, or track
along
which something moves
Path Finding...
Path 「 Noun 」

1. A way beaten, formed,

or trodden by the feet
persons or animals
2. A narrow walk or way
3. A route, course, or track
along
which something moves
 What if there
is no compass &
map?
http://www.pinterest.com/jerodor/cagaduisimo/
I’m here.
 Travelers with the Sun, the Moon, and
stars

 Compass, one of Four great Chinese

inventions

 Development of Compass
 Magnetic compass
 Front compass
 GPS

 Map
Big Dipper, by Magnus Manske, Public Domain pixabay.com, CC0
 Imagine it!
Find Path in the dark
 Path Finding of
Robot
(From now on,
Let’s learn more in detail)
What you need for path finding!
Avoiding
Obstacles…

Arrived
Let’s go
safely!
to B from
A
What you need for path finding!
Avoiding
Obstacles…

Arrived
Let’s go
safely!
to B from
A

① Position: Measuring/estimating the robot’s position

② Sensing: Measuring obstacles such as walls and objects
③ Map: Maps with road and obstacle information
➃ Path: Calculate optimal path to the destination and follow
① Position: Measuring/estimating the robot’s
position
 GPS (Global Positioning
System)
 Error
 Weather
 Outdoor

 Indoor Positioning Sensor

 Landmark (Color, IR Camera)
 Indoor GPS
 WiFi SLAM
 Beacon
Estimote (Beacon) StarGazer Vicon MX
① Position: Measuring/estimating the robot’s
position
 Dead Reckoning
 Using the encoder value of both wheel axes
 Calculate moving distance and rotation angle, and then estimate
position
 Floor slip, mechanical, cumulative error
 Position compensation with inertial
sensor, filter such as IMU
 Kalman filter...
TurtleBot 3

 Required Information
 Encoder value E on both wheel
axes
(Recalculated as gear ratio for motor shaft)
 Distance between wheels D
 ① Position: Measuring/estimating the robot’s
position
Dead Reckoning (𝑥𝑘+1 , 𝑦
𝑘+1, 𝜃
Position & Orientation after 𝑣𝑙 = 𝑙
𝐸 c − 𝐸 𝑙p
𝑇
(radian/sec)
∙
180
π

) 𝑘+1
movement
𝑒
Calculation 𝑣𝑟 = 𝑟
𝐸 c − 𝐸𝑟p
∙
π
𝑇 180
 Linear velocity: v (radian/sec)
𝑒
 Angular velocity: w 𝑉𝑙 = 𝑣𝑙 ∙ r (meter/sec)

 Use Runge-Kutta 𝑉𝑟 = 𝑣𝑟 ∙ r (meter/sec)

𝑉𝑟 + 𝑉 𝑙
Formula 𝑣𝑘 =
2
(𝑥𝑘+1 , 𝑦
𝑘+1, 𝜃 (meter/sec)
Position & Orientation before

) 𝑘+1
movement
 Approximate value
𝑉𝑟 − 𝑉 𝑙
𝜔𝑘 =
of moved position x, y 𝐷
(radian/sec)
∆s = 𝑣𝑘𝑇𝑒 ∆θ = 𝜔𝑘𝑇𝑒
 Rotation angle θ

∆𝜃
𝑥 = 𝑥𝑘 + ∆𝑠 cos 𝜃𝑘
+ 𝑘+1
(x, y) 2

∆𝜃
𝑦 = 𝑦𝑘 + ∆𝑠 sin 𝜃𝑘
D
+
r
𝑘+1
x
2
y
θ 𝑘+1 = 𝜃𝑘 + ∆𝜃
② Sensing: Measuring obstacles such as walls and
objects
Distance Sensor
 LRF, ultrasonic sensor, infrared distance sensor

 Vision Sensor
 Stereo camera, mono camera, omni-directional
camera

 Depth camera
 SwissRanger, Kinect-2
 RealSense, Kinect, Xtion, Carmine(PrimeSense),
Astra
③ Map: Maps with road and obstacle
information Robots need a map to find a path!

 Map
 Digital maps for infrastructure such as
roads!
 Maps of hospitals, cafes, companies,
homes?
 Maps of unknown, collapsed hazardous
areas?

pixabay.com, CC0
③ Map: Maps with road and obstacle
information
 Robots need a map to find a path!

 Map
 Digital maps for infrastructure such as
roads!
 Maps of hospitals, cafes, companies,
homes?
 Maps of unknown, collapsed hazardous
areas?

 Map? Let’s make it!

 pixabay.com, CC0
④ Path: Function to calculate optimal path to destination
and travel
• Navigation
• Localization / Pose
estimation
• Path search and planning

• Dynamic Window Approach

(DWA)
• A* algorithm (A Star)
• Potential Field
• Particle Filter (t1) (t2)

• Graph

(t3) (t4) https://students.cs.byu.edu/~cs470ta, http://vimeo.com/3423169

① ② ③ ➃
Position Sensing Map Path

pixabay.com, CC0, www.willowgarage.com CC BY 3.0

① ② ③ ➃
Position Sensing Map Path

Position+Sensing → Position+Sensing+Map →
Map Path
SLAM Navigation
pixabay.com, CC0, www.willowgarage.com CC BY 3.0
SLA
M
25
Gmapping
• One of SLAM method published in OpenSLAM, packaged in
ROS
• Author: G. Grisetti, C. Stachniss, W. Burgard
• Feature: Rao-Blackwellized particle filter, Decreased
number of particles, grid map

• Hardware Constraints
• X, Y, Theta Speed Command
• Differential drive mobile robot
• Omni-wheel robot
• Odometry
• Measuring sensor: 2D plane measurable sensor(LRF, LiDAR,
Kinect, Xtion, etc.)
• Rectangular and circular robots https://www.openslam.org/gmapping.html
Mapping: Gmapping + TurtleBot3
• Software Preparation
• http://emanual.robotis.com/docs/en/platform/turtlebot3/pc_setup
/
• http://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setu
p/
• http://emanual.robotis.com/docs/en/platform/turtlebot3/opencr_s
etup/

• Turtlebot3 Packages
• https://github.com/ROBOTIS-GIT/turtlebot3
• https://github.com/ROBOTIS-GIT/turtlebot3_msgs
• https://github.com/ROBOTIS-GIT/turtlebot3_simulations
• https://github.com/ROBOTIS-GIT/turtlebot3_applications
Mapping: Gmapping + TurtleBot3
• http://turtlebot3.robotis.com/en/latest/slam.
html
• Run Master (Remote PC)
$ roscore

• TurtleBot3 and Sensor Drive

(SBC)
$ export TURTLEBOT3_MODEL=burger (or waffle or waffle_pi)
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

• RViz, TurtleBot3 Remote Control, Mapping

$ (Remote PC)
export TURTLEBOT3_MODEL=burger (or waffle or waffle_pi)
$ roslaunch turtlebot3_slam turtlebot3_slam.launch
$ rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
$ rosrun map_server map_saver -f ~/map
Mapping: Gmapping + TurtleBot3

https://youtu.be/7mEKrT_cKWI
29
Mapping: Gmapping + TurtleBot3
• Completed
map 2D Occupancy Grid Map (OGM)

• White = Free area where robot can move

• Black = Occupied area where robot can not

move

• Gray = Unknown area

Mapping: Gmapping & Cartographer +
TurtleBot3

https://youtu.be/lkW4-dG2BCY
Mapping: Gmapping & Cartographer +
TurtleBot3

https://youtu.be/lkW4-dG2BCY
Mappin
g

How about it, easy?

https://mirror.enha.kr/
Mapping

If you want to develop a service or mobile

robot with
SLAM & Navigation as basic function,
Just use SLAM and Navigation as they
are and spend more time in your
desired area!
How about it, easy? I look forward to seeing your
unique robot that is not in the
world

https://mirror.enha.kr/
Mapping

If you want to develop a service or mobile

robot with
SLAM & Navigation as basic function,
Just use SLAM and Navigation as they
are and spend more time in your
desired area!
How about it, easy? I look forward to seeing your
unique
If robottothat
you want is not
study morein about
the SLAM &
world
Navigation, All software is opened.
See it , understand it, and add
functions! There is no better
textbook than practice

https://mirror.enha.kr/
Process of SLAM Related Nodes
① sensor_node ③ ② ①
turtlebot3_core turtlebot3_teleop sensor_node
② velocity
turtlebot3_teleo geometry_msgs/Twist

p odom
tf/tfMessage sensor_msgs/LaserScan
③ base_footprint
tf scan
turtlebot3_core ④
④ base_link

slam_gmapping gmapping
(slam_gmapping)
base_scan
⑤ map_server ⑤
map_server
map (map_saver)
nav_msgs/OccupancyGrid
map.pgm & map.yaml
Localization | Kalman filter, Particle filter, Graph, Bundle
adjustment
• Kalman filter
• A recursive filter that tracks the state of an object in a linear
system containing noise
• Based on Bayesian probability

• Prediction
• Estimate the state of the current state
from the previous state by assuming the
model

• Update
• Using the error between the predicted
value of the previous step and the actual
measured value obtained by the external
instrument !
http://en.wikipedia.org/wiki/Kalman_filter, CC0
 Localization | Kalman filter, Particle filter, Graph, Bundle
adjustment
• Particle Filter
• A particle filter is a technique that
predicts through simulation based
on a trial-and-error method. Estimated
values are represented as particles
with probabilistic representation.

1) Initialization (t1) (t2)

2) Prediction
3) Update
4) Pose
Estimation
5) Resampling

(t3) (t4)
S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. MIT Press, Cambridge, MA, 2005.
Navigatio
n
39
Navigation + TurtleBot3
• http://turtlebot3.robotis.com/en/latest/navigation
.html
• Run Master (Remote PC)
$ roscore

• TurtleBot3 and Sensor Drive

(SBC)
$ export TURTLEBOT3_MODEL=burger (or waffle or waffle_pi)
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

• RViz, TurtleBot3 Remote Control, Navigation

$ (Remote PC)
export TURTLEBOT3_MODEL=burger (or waffle or waffle_pi)
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
$ rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz
Navigation

https://youtu.be/VYlMywwYALU
Navigation
• Dynamic Window Approach (Mainly used in local plan)
• How to choose the speed at which you can quickly reach the target point
while avoiding the obstacles that can collide with the robot in the ‘velocity
of the space’
search 𝑣 𝑣𝑚𝑎
robot 𝑠 𝑥

𝑣 (linear velocity), 𝜔 (angular

Collision
Area

𝑉
𝑉
• Dynamic Window

𝑠 : Available speed 𝑉
(𝑉𝑑)

𝑉
• velocity)
𝑟
• 𝑎 : Permissible speed range
range 𝑎
𝑉
• 𝑉𝑟 : Speed range in dynamic
𝐺(𝑣,𝜔) = 𝜎 𝛼 ∙ ℎ𝑒𝑎𝑑𝑖𝑛𝑔 𝑣, 𝜔 + 𝛽 ∙ 𝑑𝑖𝑠𝑡 𝑣,
𝑐
window 𝜔 𝑚𝑖 ∆𝑡 ∙ 𝜔 𝑚𝑎
𝜔 + 𝛾 ∙ 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦 𝑣, 𝜔
•
𝑛 𝑎𝑚𝑎𝑥 𝑥

∆𝑡 ∙
robot, the objective function G finds 𝑣, 𝜔 at which 𝑎 𝑚𝑎𝑥
• Given the direction, speed, and impact of the

the objective function is maximized

Dynamic Window
D. Fox, W. Burgard and S. Thrun, The dynamic window approach to collision avoidance, IEEE Robotics & Automation Magazine
Dynamic Window Approach (DWA)

Goal
Dynamic Window Approach (DWA)

Goal
Dynamic Window Approach (DWA)

Goal
Mapping

If you want to develop a service or mobile

robot with
SLAM & Navigation as basic function,
Just use SLAM and Navigation as they
are and spend more time in your
desired area!
How about it, easy? I look forward to seeing your
unique
If robottothat
you want is not
study morein about
the SLAM &
world
Navigation, All software is opened.
See it , understand it, and add
functions! There is no better
textbook than practice

https://mirror.enha.kr/
 Practice Time!
“SLAM /
Navigation”
47
Preparation of TurtleBot3 Simulation Development
Environment
• Official TurtleBot3 wiki reference
• http://turtlebot3.robotis.com
• Basic package installation (for the use of 3D simulator
Gazebo)
$ sudo apt-get install ros-kinetic-joy ros-kinetic-teleop-twist-joy ros-kinetic-teleop-twist-keyboard ros-
kinetic-laser-proc ros-kinetic-rgbd-launch ros-kinetic-depthimage-to-laserscan ros-kinetic-rosserial-arduino
ros-kinetic-rosserial-python ros-kinetic-rosserial-server ros-kinetic-rosserial-client ros-kinetic-rosserial-msgs
ros-kinetic-amcl ros-kinetic-map-server ros-kinetic-move-base ros-kinetic-urdf ros-kinetic-xacro ros-kinetic-
compressed-image-transport ros-kinetic-rqt-image-view ros-kinetic-gmapping ros-kinetic-navigation

$ cd ~/catkin_ws/src/
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
$ cd ~/catkin_ws && catkin_make
48
TurtleBot3 in Gazebo

https://youtu.be/xXM5r_SVkWM
49
Run Virtual Robot with Gazebo
• Control virtual robot on 3D simulator Gazebo
• Robots can be controlled with ‘Turtlebot3_teleop_key’ node
• Possible to check sensor value mounted on robot on Gazebo
via Rviz
• 2D Laser Range Sensor, Camera, Depth Camera, IMU, etc.
$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch

50
Run Virtual Robot with Gazebo

51
Virtual SLAM with Gazebo
 Run
Gazebo
$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
 Run
SLAM
$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_slam turtlebot3_slam.launch
 Run
RViz
$ export TURTLEBOT3_MODEL=waffle_pi
$ rosrun rviz rviz -d `rospack find turtlebot3_slam`/rviz/turtlebot3_slam.rviz
 TurtleBot3 Remote
Control
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

 Run Map
Server
$ rosrun map_server map_saver -f ~/map

52
Virtual SLAM with Gazebo

53
Virtual Navigation with Gazebo
 Run
Gazebo
$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch

 Run
Navigation
$ export TURTLEBOT3_MODEL=waffle_pi
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

 Run RViz & Setting

Destination
$ export TURTLEBOT3_MODEL=waffle_pi
$ rosrun rviz rviz -d `rospack find turtlebot3_navigation`/rviz/turtlebot3_nav.rviz

54
Virtual Navigation with Gazebo

55
Question
Time!
 Advertisement #1

Download link
Language:
English, Chinese, Japanese,
Korean

“ROS Robot Programming”

A Handbook is written by TurtleBot3
Developers
 Advertisement #2

AI Research Starts Here

ROS Official Platform
TurtleBot3 is a new generation mobile robot that’s modular, compact and
customizable. Let's explore ROS and create exciting applications for
education, research and product development.

Direct Link
 Advertisement #3

www.robotsource.org
The ‘RobotSource’ community is the space for people making robots.

We hope to be a community where we can share knowledge about

robots, share robot development information and experiences, help
each other and collaborate together. Through this community, we
want to realize open robotics without disti nguishing between
students, universities, research institutes and companies.

Join us in the Robot community ~

END
.