a framework for use in slam algorithms principle investigator: shaun egan supervisor: dr. karen...

A Framework for use in SLAM algorithms

Principle Investigator: Shaun EganSupervisor: Dr. Karen Bradshaw

SLAM is a family of algorithms

SLAM

State UpdateLandmark Tracking (Dead

reckoning)

Landmark Extraction

Data Association

Pose Tracking

Odometry

CountLeft CountRight

Odometry Algorithm

Change in wheel position:

∆ 𝑃=∆𝑐𝑑Π𝑖

Change in heading:

c: encoder countd: wheel diameteri: increments per tour

∆ 𝜃=(∆𝑟 −∆ 𝑙)

𝑎𝑑

r: right encoder countl: left encoder counta: axis wheel rationd: wheel diameter

Average heading:

𝜗=∆𝜃∗0.5+𝜃 𝑙 ast known heading

New robot position and heading

))

y=𝑦+((∆𝑃𝑙+∆𝑃𝑟 )∗0.5sin (𝜗 ))

The problem with ultrasonic range detection

The answer?

Range scans Triangulation

Triangulation

1. Add current sensor information to the sliding window2. Compare each of the new sensor range readings with all other previous readings in the

window3. Triangulate these readings to create valid points for each new sensor reading:

Calculate some required values:

∆ 𝑥=𝑠𝑟𝑥−𝑠𝑙𝑥∆ 𝑦=𝑠𝑟𝑦−𝑠𝑙𝑦𝑑2=∆ 𝑥2+∆ 𝑦 2𝑑=√ 𝑑2𝑘=

(𝑑 2+𝑟𝑙−𝑟𝑟 )2𝑑

s: sensor positionr: range value

Calculate the points of intersection

And finally, find which point is the one we want (the one visible to both sensors:

For each point (i): For each sensor (k):

𝜗=arctan (∆ 𝑦 ,∆𝑥 )∆ 𝑦=𝑃𝑖𝑦−𝑆𝑘𝑦∆ 𝑥=𝑃𝑖𝑥−𝑆𝑘𝑥

𝜗∈(𝑠𝑘𝜃−𝑠𝑘𝛿2; 𝑠𝑘𝜃+

𝑠 𝑘𝛿2

)

Range scans Triangulation

Point refinement

𝑇 𝑥=0𝑇 𝑦=0

For every point (i):

𝑇 𝑥=𝑇𝑥+𝑃𝑖𝑥𝑇 𝑦=𝑇𝑦+𝑃𝑖𝑦

Calculate the average position of points:

𝑇 𝑡=0

𝑇 𝑡=𝑇𝑡+1

Create a new landmark: new Landmark()

Point refinement:

Range scans Triangulation

Point refinement

Data association

Landmark refinement

Data association and landmark refinement:

1) The newly created landmark is added to the map

2) Compare landmarks

3) If landmarks are within a certain range of each other: Considered the same landmark Merged according to their triangulation count

phew…

• Configuration• Point• Pose• Landmark• ScanStep

CommonUtils

• Odometer• Encoder• FT_Odometer

OdometryUtils

• UltraSonic• TBFCalculator

TBFUtils

• Additional landmark extraction and data association models

• The addition of a package for localization algorithms

• The addition of a package for loop closure utilities

• Extensions for cooperative SLAM

?