Embedded Intelligent Systems

for Autonomous Navigation

Frankfurt University of Applied Sciences

Prof. Dr. Peter Nauth
 TurtleBot 3 Burger
LDS 01
LIDAR = Light Detection and Ranging

IMU, 3-axis

Encoder

05.04.2021 Embedded Intelligent Systems / P. Nauth 2

 TurtleBot 3 Burger

05.04.2021 Embedded Intelligent Systems / P. Nauth 3

 TurtleBot 3 Burger

05.04.2021 Embedded Intelligent Systems / P. Nauth 4

 Embedded System / TurtleBot

05.04.2021 Embedded Intelligent Systems / P. Nauth 5

 LIDAR / TurtleBot

05.04.2021 Embedded Intelligent Systems / P. Nauth 6

 LIDAR / TurtleBot

05.04.2021 Embedded Intelligent Systems / P. Nauth 7

 LIDAR / TurtleBot

Raspberry Pi
USB Connector

05.04.2021 Embedded Intelligent Systems / P. Nauth 8

 TurtleBot 3 Burger

05.04.2021 Embedded Intelligent Systems / P. Nauth 9

 Motors / TurtleBot

Asynchronous
Serial
Communication
(8bit, 1stop, No
Parity)

05.04.2021 Embedded Intelligent Systems / P. Nauth 10

Autonomous Navigation / TurtleBot

LDS 01 Mapping
Measure
LIDAR Map
Update Correct

Encoder Predict Position

Motors Localization

SLAM: Simultaneous Localization and Mapping

05.04.2021 Embedded Intelligent Systems / P. Nauth 11

 Occupancy Grid Map
Yellow Cells:
Free space,
Weight is low,
e.g. = 1

Black Cells:
Obstacles,
Weight is high,
e.g. = 100.000
14.08.2018 12
Autonomous Intelligent Systems / Nauth
 Path Planning: A* Algorithm

05.04.2021 13
Embedded Intelligent Systems / P. Nauth
 1 Axis Model
Robot moves from p(k)
to p(k+1) whereby
D(k): Distance travelled
within k to k+1
Θ(k): Orientation at k
p(k) = (x(k), y(k), Θ(k))
Θ(k) + ΔΘ(k):
p(k+1)
Orientation at k+1

05.04.2021 Embedded Intelligent Systems / P. Nauth 14

 1 Axis Model, Difference Equation

Δθ(k)/2 valid, if orientation changes gradually from θ(k) to θ(k) + Δθ(k) within T
T is time intervall the robot needs to change from k to k+1

05.04.2021 Embedded Intelligent Systems / P. Nauth 15

 1 Axis Model, State Space Equation
• Replace the 3 kinematik equations for x(k+1),y(k+1),θ(k+1)
by state space equation:

p(k+1) = F p(k) + G u(k) + n(k)

• p(k+1) = (x(k+1), y(k+1), θ(k+1)) (State = Robot Pose)
p(k) = (x(k), y(k), θ(k))
u(k) = (D(k)cos(θ(k)+Δθ(k)/2)),D(k)sin(θ(k)+Δθ(k)/2)),Δθ(k))

• F: State Transition Matrix (robot dynamics),e.g. identity matrix

• G: Input Transition Matrix, e.g. identity matrix
• n(k): Noise

05.04.2021 Embedded Intelligent Systems / P. Nauth 16

 Autonomous Navigation
Following a Path by using State Space Equation:
1. Based on Start Position and Final Position, plan
optimal path (trajectory) and discretize it in K time
intervalls of ΔT. Total time is T = k ΔT.
2. Persuit Point is set to next state p*(k+1) on path.
3. Robot executes action a(k) by moving to p*(k+1) by
applying a u(k) calculated by closed loop control.
4. Robot estimates (predicts) it‘s new state p(k+1) by
using State Space Equation. D(k) and Δθ(k) are
measured by encoders.
05.04.2021 Embedded Intelligent Systems / P. Nauth 17
 Autonomous Navigation
Final Position:
Real State: Table in kitchen
Robot
True pose
at k+1 Trajectory planned
p*(k+1)
Real State:
True pose Predicted State p(k+1):
at k Estimated pose due to input u(k)
State p(k): Belief State
Belief State

Input u(k) for calculation of

predicted state is measured
by encoders
Start Position:
Bed room

05.04.2021 Embedded Intelligent

Seite Systems
18 / P. Nauth
 Autonomous Navigation
Problems:
1. Position estimation is affected by error for it is
based on a kinematic model and u(k) data
derived from encoders

2. Map of environment might not be available or

might be affected by errors.

05.04.2021 Embedded Intelligent Systems / P. Nauth 19

 Autonomous Navigation
Solution:
• Combine position estimation (equation,
encoders) and position measurement (LIDAR,
camera, ..)
• Create and improve map of environment
-> Simultaneous Localization and Mapping (SLAM)

05.04.2021 Embedded Intelligent Systems / P. Nauth 20

 SLAM Algorithms
• Hector SLAM
LIDAR (2-D); Point Clouds -> Grid Map
• G Mapping SLAM
LIDAR (2-D) + Odometrie; Point Clouds -> Grid M.
• RTAB Map SLAM
RGB-D Sensor (3-D, e.g. Kinect) + LIDAR + Odom;
Point Clouds -> Graph Based Map (->Loop Closr)
• EKF – SLAM
LIDAR (2-D); Landmarks / Features; Extended
Kalman Filter
05.04.2021 Embedded Intelligent Systems / P. Nauth 21
 EKF - SLAM
Localization (Estimate pose of robot):

• Estimate with high precision the pose p(k+1)

the robot has reached,

• if an input u(k) has been applied to the robot

in pose p(k)

05.04.2021 Embedded Intelligent Systems / P. Nauth 22

 EKF - SLAM
Mapping (Create a map):

• Detect Landmarks / Features

• Estimate their position
• Update position estimation of features in map
(If positions of landmarks known a-priori,
no mapping required)

Remarks: Features are dominant parts

environment such as edges

05.04.2021 Embedded Intelligent Systems / P. Nauth 23

 EKF - SLAM

Landmark 1 (e.g. retroreflector stripes) or

Feature 1 at position xL1(k+1), yL1(k+1),

Final Position:
Real State: Robot
Table in kitchen
True pose at k+1 Laser Scanner
p*(k+1) Trajectory planned
Real State:
True pose Predicted State p(k+1):
at k Estimated pose due to input u(k)
Belief State
State p(k):
Belief State
Input u(k) for calculation of predicted
state is measured by encoders
Start Position:
Bed room
05.04.2021 Embedded Intelligent
Seite Systems
24 / P. Nauth
 EKF - SLAM
• State = Robot Pose and Landmark Positions
p(k+1) = (x(k+1), y(k+1), θ(k+1), xL1(k+1), yL1(k+1), …)
p(k) = (x(k), y(k), θ(k ), xL1(k), yL1(k), ….)

• Input
u(k) = (D(k) cos(θ(k)+Δθ(k)/2)),
D(k) sin(θ(k) +Δθ(k)/2)),
Δθ(k))
D(k) and Δθ(k) measured by Encoder (Odometrie)

05.04.2021 Embedded Intelligent Systems / P. Nauth 25

 EKF - SLAM
• p(k+1) = F p(k) + G u(k) + n(k) State Equation
• z(k) = H p(k) + w(k) Measurement Equation
(= Observor Equation)
• p(k+1) and p(k): States, u(k): Input
n(k): Process noise, w(k): Measurement noise
• F: State Transition Matrix (robot dynamics)
G: Input Transition Matrix
H: Measurement Transition Matrix
Maps state to measurements (observations)

05.04.2021 Embedded Intelligent Systems / P. Nauth 26

 EKF - SLAM
• State Prediction
p(k+1) = F p(k) + G u(k)
• Measurement Prediction
z(k+1) = H p(k+1) Estimated distance to LMs
• Measurement Residual
v(k+1) = zm(k+1) – z(k+1) zm: Measured dist. to LMs
• Updated State Predicition
^p(k+1) = p(k+1) + K(k+1) v(k+1)
K(k+1): Kalman Gain (weights the correction impact)

05.04.2021 Embedded Intelligent Systems / P. Nauth 27

 EKF - SLAM
K(k+1): Kalman Gain Calculation
• Prediction of P(k+1)
P(k+1) = F P(k)F + N(k)
• Updated Prediction
T -1
K(k+1) = P(k+1)H(HP(k+1)H + W(k+1))
^P(k+1) = P(k+1) – K(k+1)HP(k+1)
• Covariances
P(k) is covariance of state x(k)
N(k) is covariance of process noise n(k)
W(k) is covariance of measuremenF noise w(k)

05.04.2021 Embedded Intelligent Systems / P. Nauth 28

 EKF - SLAM
K(k+1): Kalman Gain
• K weights correction in update of
robot position and landmark positions
• K is updated each step k based on ratio of
process noise v and measurement noise w
• The higher the measurement noise the lower the
weights in K
• If a LM is not observable at a step k, it‘s weights in
K must be set to 0
05.04.2021 Embedded Intelligent Systems / P. Nauth 29
 EKF - SLAM
K(k+1): Kalman Gain
• Full SLAM (see [6]):
- updates weights by means of statistical
approaches such as Kalman filter
- corrects robot position and LM positions
• Static SLAM (see example below):
- uses fixed weights
- corrects robot position only

05.04.2021 Embedded Intelligent Systems / P. Nauth 30

 EKF - SLAM

Landmark 1 at position xL1(k+1), yL1(k+1),

(e.g. retroreflector stripes)
Updated
prediction zm z Final Position:
Real State: ^p(k+1) Table in kitchen
Persuit Point =
True pose at k+1 Planned State
Trajectory planned
p*(k+1)
Robot
Predicted State p(k+1):
Real State:
Estimated pose due to input u(k),
True pose at k
Belief State
State p(k):
Belief State
Input u(k) is calculated by a moving to a
point approach, e.g. closed loop approach
(e = p*(k+1) – p(k))
Start Position: or measured by odometrie sensors
Bed room
05.04.2021 Embedded Intelligent
Seite Systems
31 / P. Nauth
 SLAM and Navigation
Next pose at trajectory to be reached
p*(k+1) u(k) Real p(k+1)
Controller Robot
- KD, KΔθ Kinematics
p(k)
Current pose

zm
Prediction Predicted Update LMs
p(k+1) Prediction
k+1 Updated
→k ^p(k+1)

Start Position
Path
Final Position
Planning
Map

05.04.2021 Embedded Intelligent Systems / P. Nauth 32

 Autonomous Navigation, Comparison
with Markov Decision Process (MDP)
s(0)
a0(0) 0.7
s(0)
0.3 a0(0)
s(1)
Belief State
a1(0) s(1)
1.0 Obstacle Avoidance
a0(1)
0.5 a0(1) a1(1)
s(2)
Belief State
a1(2)
0.5 s(2)

Markov Decision Process Decision Process in

(3 states, 2 actions) Autonomous Navigation

05.04.2021 Embedded Intelligent Systems / P. Nauth 33

 Static SLAM Example with 2 LM
x
y
θ
p(k) = xL1 F=E
(3 + 2n) x 1 yL1 (3 + 2n) x (3 + 2n)

xL2
yL2

05.04.2021 Embedded Intelligent Systems / P. Nauth 34

 Static SLAM Example with 2 LM

D(k)cos(θ(k)+Δθ(k)/2) 100
u(k)= D(k)sin(θ(k)+Δθ(k)/2) 010
3x1 Δθ(k) 001
G= 000
(3 + 2n) x 3 000
000
000

05.04.2021 Embedded Intelligent Systems / P. Nauth 35

 Static SLAM Example with 2 LM

1 0 0 -1 0 0 0 x - xL1
H= 0 1 0 0 -1 0 0 H p(k) = z(k) = y - yL1
1 0 0 0 0 -1 0 x - xL2
0 1 0 0 0 0 -1 y - yL2
2n x (3 + 2n) 2n x 1

05.04.2021 Embedded Intelligent Systems / P. Nauth 36

 Static SLAM Example with 2 LM

k1 0 k2 0 k1 ΔxL1 + k2 ΔxL2
0 k1 0 k2 k1 ΔyL1 + k2 ΔyL2
0 0 0 0 0
K= 0 0 0 0 K v(k+1) = 0
0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
(3 + 2n) x 2n (3 + 2n) x 1

05.04.2021 Embedded Intelligent Systems / P. Nauth 37

 Obstacle Avoidance
1. Static Obstacles:
Path Planning Algorithms (Aktiv Path Planning such as A*
using Global map) plans trajectories around obstacles.
2. Dynamic Obstacles:
Obstacles detected during navigation (Local Map) cause
to sidestep and get back to planned trajectory, e.g. by
Bug Algorithm (called Passive Path Plng. if straight line
used as path)

05.04.2021 Embedded Intelligent Systems / P. Nauth 38

 References
(1) https://www.turtlebot.com/

05.04.2021 Embedded Intelligent Systems / P. Nauth 39