Anda di halaman 1dari 100

Creative Advanced Robotics

[ ]

Introduction of Path Planning and


Navigation of Mobile Robot and UGV
Real time Path Planning of Mobile Robot and UGV in Unknown
Environments and Its Application

School of Mechanical Engineering, Pusan National University

Prof. Min Cheol Lee


Email: mclee@pusan.ac.kr
Home page: http//mclab.me.pusan.ac.kr

Special Topics of Creative Advanced Robotics 1/100


Creative Advanced Robotics

Contents
1. Definition of Navigation ?

2. Mobile Robot and Autonomous Vehicle Technique

3. Path Planning

Global Path Planning

Local Path Planning APF with Virtual Hill

4. Simulation, Experiment and Its Application

5. Conclusion

Special Topics of Creative Advanced Robotics 2/100


Creative Advanced Robotics
Definition of Navigation
What is Navigation?
the process of monitoring and controlling the movement of
a craft or vehicle from one place to another. -wikipidia
Inertial navigation is a dead reckoning* type of navigation system that
computes its position based on motion sensors such as IMU.
Radio navigation uses radio waves to determine position by either radio
direction finding system or hyperbolic systems, such as Decca, Omega,
and LORAN-C.
Radar navigation uses radar to determine the distance from or bearing of
objects whose position is known.
Satellite navigation uses artificial earth satellite systems, such as GPS,
to determine position.
* Dead Reckoning: a method of estimating the position of a ship , aircraft , or UGV by applying
estimates of the distance and direction travelled to a previously position

Special Topics of Creative Advanced Robotics 3/100


Creative Advanced Robotics
Navigation

Special Topics of Creative Advanced Robotics 4/100


Creative Advanced Robotics
TSUKUBA CHALLENGE 2010

Special Topics of Creative Advanced Robotics 5/100


Creative Advanced Robotics

TSUKUBA CHALLENGE 2011


https://www.youtube.com/watch?v=01rXdSl1NU8

Special Topics of Creative Advanced Robotics 6/100


Creative Advanced Robotics

Navigation Technologies Path Control


Control Strategy
Detail Position Estimation / Road Following
Teaching Playback / Motion Design

Map
Measured / Self-made
Google / Provided by Local Government
Positioning - Localization
GPS
Land Marks Selection
Motion Control
Collision Avoidance / Safety

Special Topics of Creative Advanced Robotics 7/100


Creative Advanced Robotics

Navigation-Global Path Planning


UGV Competition https://www.youtube.com/watch?v=2gQ3P5BpLjQ

DARPA Grand Challenge

Autonomous Vehicle Competition


Organized by Hyundai Mobile Com., NGV
Special Topics of Creative Advanced Robotics 8/100
Creative Advanced Robotics
Path Control of Mobile Robot and Autonomous Vehicle

Technical Issue

Dijkstra Algorithm
A* and D* Algorithm

Path
Control Leader-Follower Model
Potential Field

Vision +Laser

Special Topics of Creative Advanced Robotics 9/100


Creative Advanced Robotics

Autonomous Vehicle Technique


Component Technique
Recognition of
Environment
Path Planning
Vehicle Control

Sensors for environment recognition


Special Topics of Creative Advanced Robotics 10/100
Creative Advanced Robotics

Autonomous Mobile Technique

< Autonomous Mobile Robot with Environment Recognization >

Special Topics of Creative Advanced Robotics 11/100


Creative Advanced Robotics

Autonomous Mobile Technique


Environment and Obstacle Recognition Technique

Distance Recognition Sensor(Lidar, Laser Range Finder)


Location Recognition
DGPS/RTK(Real Time Kinematics)-GPS
Environment Recognition: Vision Sensor

Recognition of driving environment


Special Topics of Creative Advanced Robotics 12/100
Creative Advanced Robotics

Autonomous Mobile Technique


Distance Recognition Sensor
Ultrasonic, Infrared Sensor: Short Distance, Low Resolution Mobile Robot

Lidar, Laser Range Finder: Long Distance, High Resolution UGV

Model LMS291 of SICK Data Acquisition Results from SICKLRF

Special Topics of Creative Advanced Robotics 13/100


Creative Advanced Robotics

Autonomous Mobile Technique


Localization (Positioning) Sensor
GPS: Low cost, Large Error( around Max. 20 m)
DGPS (Differential GPS) : Medium Cost(a Million Won ), Lower Error (4~5 m)
RTK(Real Time Kinematics)-GPS : High Cost(10 Million Over), Lowest Error

GPS Based Localization system


Special Topics of Creative Advanced Robotics 14/100
Creative Advanced Robotics

Vision Sensor
Lane Recognition,

Bumblebee2 of POINT GREY

Road recognition using vision sensor in DARPA Grand Challenge

Special Topics of Creative Advanced Robotics 15/100


Creative Advanced Robotics
Path Planning
Trapping obstacle

Robot Path Planning Start

without Human Help

z [m ]
in Known or Unknown Environments Goal

in Static or Dynamic Environments x [m] y [m]

in 2-dimentional or 3-dimentional Environments


for Arbitrary-shaped Robots
in Real-time

Special Topics of Creative Advanced Robotics 16/100


Creative Advanced Robotics

Path Planning

Global Known Environment


Path Planning
Local Unknown Environment

obstacle

Global Local Global + Local

Special Topics of Creative Advanced Robotics 17/100


Creative Advanced Robotics

Path Planning
Conventional Approaches
Complete Algorithms
for only Known Environments Dijkstra Algorithm
computationally intractable A* and D* Algorithm

Heuristic (incomplete) Algorithms


adaptable to Unknown Environments
lower complexity _ Real-time

Artificial Potential Field Approach

Special Topics of Creative Advanced Robotics 18/100


Creative Advanced Robotics
Global Path Planning
Dijkstra algorithm
a graph search algorithm that solves the single-source shortest path problem
for a graph with nonnegative edge path costs, producing a shortest path tree

Special Topics of Creative Advanced Robotics 19/100


Creative Advanced Robotics
Global Path Planning
A* search algorithm
uses a best first search and finds a least-cost path from a given initial node to one
goal node(out of one or more possible goals).

Goal

Start

Obstacle
Total Cost = g(x) +h(x)

Example of A* algorithm

Special Topics of Creative Advanced Robotics 20/100


Creative Advanced Robotics

Global Path Planning


D* search algorithm
works by iteratively selecting a node from the OPEN list and evaluating it. It then propagates
the node's changes to all of the neighboring nodes and places them on the OPEN list.
D* begins by searching backwards from the goal node. Each expanded node has a
backpointer which refers to the next node leading to the target, and each node knows the
exact cost to the target.
LOWER
h(G, X ) : path cost function states
RAISE
states h(G, X ) k (G, X )

k (G, X ) h(G, X )

Path Cost function: h(G, X )


LOWER
Key function: k (G, X ) is defined states
to the equal to the minimum of h(G, X )
Special Topics of Creative Advanced Robotics 21/100
Creative Advanced Robotics

DARPA GRAND CHALLENGE

https://www.youtube.com/watch?v=M2AcMnfzpNg
Special Topics of Creative Advanced Robotics 22/100
Creative Advanced Robotics

Global Path Planning


Path Planning Algorithm to be Applied in DARPA Urban Challenge

Path planning algorithm s of 35 teams Path planning algorithm s of top 10 in


in Urban Challenge 2007 Urban Challenge 2007

Special Topics of Creative Advanced Robotics 23/100


Creative Advanced Robotics
DRC 2015
https://www.youtube.com/watch?v=BGOUSvaQcBs

Korea Humanoid Robot - Hubo - DRC Final Winner

Special Topics of Creative Advanced Robotics 24/100


Creative Advanced Robotics
References
Cormen, Thomas; Leisernson, Charles E; Rivest, Rolald L.; Stein Clifford, "Section 24.3:
Dijkstra's algorithm". Introduction to Algorithms (Second ed.), MIT Press and McGraw-Hill,
pp. 595601, 2001.
Hart, P. E.; Nilsson, N. J.; Raphael, B. (1968). "A Formal Basis for the Heuristic
Determination of Minimum Cost Paths". IEEE Transactions on Systems Science and
Cybernetics SSC4 4 (2), pp100107.
Dechter, Rina; Judea Pearl (1985). Generalized best-first search strategies and the
optimality of A*, Journal of the ACM, Vol
Pearl, Judea (1984). Heuristics: Intelligent Search Strategies for Computer Problem
Solving. Addison-Wesley. ISBN 0-201-05594-5
Anthony Stentz, Optimal and Efficient Path Planning for Partially-Known Environments,
in Proceedings IEEE International Conference on Robotics and Automation, May 1994.
Anthony Stentz, The Focussed D* Algorithm Algorithm f or Real-Time Replanning, in
Proceedings of IJCAI-95, August 1995.

Special Topics of Creative Advanced Robotics 25/100


Creative Advanced Robotics

Local Path Planning - APF


Artificial Potential Field Approach
One of the Most Popular Technique
Providing Simple and Efficient Path Planner
Real-time Path Planning in Unknown Environment

Local minimum problem


Trap a robot before reaching it's goal
Inevitable problem in a local path planning

New approach to escape local minimum

Virtual Hill Concept

Special Topics of Creative Advanced Robotics 26/100


Creative Advanced Robotics

Local Path Planning - APF


Attractive Potential (Goal) Repulsive Potential (Obstacle)

Obstacle
Goal

Potential
Potential

y [m ]
y [m ]

x [m ]
x [m ]

Special Topics of Creative Advanced Robotics 27/100


Creative Advanced Robotics

Local Path Planning - APF


Attractive Potential (Goal) Repulsive Potential (Obstacle)
Obstacle
Goal

Potential
Potential

+
y [m ]
y [m ]

x [m ]
x [m ]

Goal Obstacle F Robot


Start
Potential

= Obstacle

y [m ]
Goal
x [m ]

Special Topics of Creative Advanced Robotics 28/100


Creative Advanced Robotics
Local Path Planning - APF
Local Minimum Problem
Robot Robot

Obstacle Obstacle
VS

Goal Goal

Global minimum (Goal) Global minimum Local minimum


Potential

Potential
y [m ] y [m ]

x [m ] x [m ]
Special Topics of Creative Advanced Robotics 29/100
Creative Advanced Robotics
Local Path Planning APF with VH
Virtual Hill Concept
Goal Obstacle Virtual Hill

6 6 6
Attractive Repulsive Extra
Potential Potential Potential
Obstacle

?
Goal
Potential
Potential

y [m ]
y [m ]

x [m ]
x [m ]
Absolutely eliminate
local minima
Special Topics of Creative Advanced Robotics 30/100
Creative Advanced Robotics
Local Path Planning APF with VH
New Approach with Virtual Hill
Start

Path Planning by
Attractive Potential and Repulsive Potential

No

No Local
Reach goal?
minimum?

Yes Yes

End Generate Virtual Hill with Extra Potential


Yes

Path Planning by Escape


Extra Potential and Repulsive Potential Local minimum area?

No

Special Topics of Creative Advanced Robotics 31/100


Creative Advanced Robotics

Local Path Planning APF with VH


Extra Potential : Definition

U ext ke1 ke 2
2

Total Potential : U U ext U rep

Special Topics of Creative Advanced Robotics 32/100


Creative Advanced Robotics

Local Path Planning APF with VH


Extra Potential
in U ext ke1 ke 2
2


: Q t * : t0 t * t and t0 t tk
Special Topics of Creative Advanced Robotics 33/100
Creative Advanced Robotics

Local Path Planning APF with VH


Direction of : a path-of-the-closest-obstacles

y
p (t i + 1 )
en

p (t i )

et

p (t i - 1 )
p C O ( t i ) = Q (t i )
eb
pC O (t i - 1) = Q C O (t i - 1)

x
z


: Q t * : t0 t * t and t0 t tk
Special Topics of Creative Advanced Robotics 34/100
Creative Advanced Robotics
Local Path Planning APF with VH
Definition of e b


B / B
eb
if B / B 0
where, B p goal pto p(t0 ) pto

k if B / B 0

y Estimated
p (t 0 ) obstacle line

eb =- k
en

et
pT O

pG oal

p pco x
en , et e n eb z
p pco
Special Topics of Creative Advanced Robotics3535/100
/ 62
Creative Advanced Robotics
Local Path Planning APF with VH
Extra Potential

2
in U ext ke1 ke 2
2

p pco

Special Topics of Creative Advanced Robotics 36/100


Creative Advanced Robotics

Local Path Planning APF with VH


Extra Force
Fext U ext ke1et 2ke 2 en Very easy to get in Real-time

y Q (t i )
p (t i ) et

en Q (t i ) = p C O (t i )
Q (t j )
p (t j )
Detected
p (t j - 1 )
Obstacles

p (t 2 ) Q (t j ) = p C O (t j )
Q (t j - 1 ) = p C O (t j - 1 )
en
et
p (t 1 )
p (t 0 )

Q (t 1 ) = p C O (t 1 )

Q (t 0 ) = p T O = p C O (t 0 )
(t 0 ) = 0
x
Special Topics of Creative Advanced Robotics 37/100
Creative Advanced Robotics

Local Path Planning APF with VH


How does it have No Local Minimum ?
1 1 1
kr 2 en 0
Frep 0

0 0

1 1 1
ke1et kr 2 2ke 2 en 0
0
F Frep Fext

ke1et 2ke 2en 0

Always F 0
At a local minimum, F = 0
Hence, New Approach has not a Local Minimum.

Special Topics of Creative Advanced Robotics 38/100


Creative Advanced Robotics

Local Path Planning APF with VH


When generate a Virtual Hill ?

If p (t )- p (t - T a ) Sa
then, generate a Virtual Hill

Start
[m]

p (t )
y

p (t - Ta)
Goal

x [m]
Special Topics of Creative Advanced Robotics 39/100
Creative Advanced Robotics

Local Path Planning APF with VH


Extra Potential Field

U ext ke1 ke 2
2

Virtual hill
Potential

Start

[m]
y
Goal Local minimum

y [m ] x [m]

x [m ]

Special Topics of Creative Advanced Robotics 40/100


Creative Advanced Robotics
Local Path Planning APF with VH
New Potential with Extra Potential
Extra Potential Repulsive Potential
Virtual hill

Potential
Potential

+
y [m ] y [m ]

x [m ]
x [m ]
Virtual hill
Potential

=
y [m ]

x [m ]

Special Topics of Creative Advanced Robotics 41/100


Creative Advanced Robotics
Local Path Planning APF with VH
Path Planner with Extra Potential
If Trapped by Local Minimum

New Potential Filed with


Conventional Potential Filed Type 2 Extra Potential

y [m ]

If Escape Local Minimum Area

Special Topics of Creative Advanced Robotics 42/100


Creative Advanced Robotics
Local Path Planning APF with VH
When return to Conventional Approach ?
If follows are satisfied
1. (pto p) Fatt 0
to do not return to the local minimum by an attractive force

2. (pto p) Frep 0
to do not return to the local minimum by a repulsive force
pC O (t K ) = Q (t K )

3. (p goal p) Frep 0 p C O ( t i ) = Q (t i )
Obstacle

p (t K ) p (t i )

to move to the goal direction by a repulsive force Just trapped by a


Just escape a local p (t 0 )
local minimum
minimum area

pC O ( t 0 ) = p T O = Q (t 0 )
then, return to the Conventional Approach
Goal

Special Topics of Creative Advanced Robotics 43/100


Creative Advanced Robotics
Local Path Planning APF with VH
Simulations ( 1 )
New Approach with the Extra Potential

Goal Start
Start

Goal

Special Topics of Creative Advanced Robotics 44/100


Creative Advanced Robotics
Local Path Planning APF with VH
Simulations ( 1 )
New Approach with the Extra Potential

Trapping obstacle Trapping obstacle


Start Goal Start
[m]

[m]
Goal
y

x [m] x [m]

Special Topics of Creative Advanced Robotics 45/100


Creative Advanced Robotics
Local Path Planning APF with VH
Simulations ( 2 )
New Approach with the Extra Potential

Start

Goal

Special Topics of Creative Advanced Robotics 46/100


Creative Advanced Robotics
Local Path Planning APF with VH
Simulations ( 2 )
New Approach with the Extra Potential

Trapping obstacle

Start
z [m]

Goal

x [m] y [m]

Special Topics of Creative Advanced Robotics 47/100


Creative Advanced Robotics
Local Path Planning APF with VH
Simulations ( 3 )
New Approach with the Extra Potential
Dynamic Environment

Start

Goal

Special Topics of Creative Advanced Robotics 48/100


Creative Advanced Robotics
Application to Mobile Robot
The Mobile Robot System for Experiment
CCD Camera

Manipulator

Ultrasonic
sensors

Special Topics of Creative Advanced Robotics 49/100


Creative Advanced Robotics

Application to Mobile Robot

Experiments ( 1 )
New Approach with the Extra Potential

Goal

Start

Special Topics of Creative Advanced Robotics 50/100


Creative Advanced Robotics

Application to Mobile Robot


Experiments ( 2 )
New Approach with the Extra Potential

Start
[m]

Goal
y

x [m]

Special Topics of Creative Advanced Robotics 51/100


Creative Advanced Robotics
Application to Mobile Robot

Special Topics of Creative Advanced Robotics 52/100


Creative Advanced Robotics
Application to UGV

Outlook of UGV for Application

Outlook of UGV

Special Topics of Creative Advanced Robotics 53/100


Creative Advanced Robotics
Application to UGV

Block Diagram for UGV Path Control


Navigation System, Obstacle Recognition System,
Vehicle Control System, Integration System for UGV Path Control

<Block diagram of UGV path control>

Special Topics of Creative Advanced Robotics 54/100


Creative Advanced Robotics

Application to UGV

Vehicle Control System

Schematic diagram of vehicle control system


Special Topics of Creative Advanced Robotics 55/100
Creative Advanced Robotics
Application to UGV

Navigation Sysem
Localization using DGPS and North Finding
Electromagnetic Compass

Schematic diagram of navigation system

Special Topics of Creative Advanced Robotics 56/100


Creative Advanced Robotics
Application to UGV

Obstacle Recognition System


Laser range Finder
Maximum obstacle recognition range 0~180 degree, 80m

Location for attaching the laser range finder


Special Topics of Creative Advanced Robotics 57/100
Creative Advanced Robotics
Application to UGV

Problems to apply conventional potential field to UGV

Path control problem of UGV

Special Topics of Creative Advanced Robotics 58/100


Creative Advanced Robotics
Application to UGV
Case 1: UGV approaches a zone of generating repulsive force
=> Generating stonger steering force to change a head direction

-1: repulsive force vector zone


-2:repulsive force vector zone same
as attractive force
-r1: zone for reducing driving speed
-r2: zone of goal position

<Potential field application for UGV>

Special Topics of Creative Advanced Robotics 59/100


Application to UGV Creative Advanced Robotics

Steering and Speed Control


case : d r1
F FREP
V Vmin Vmax Vmin ATT
If (||FREP||>0)
FATT

V Vmin Vmax Vmin max
If (||FREP||=0)
max
case : d r1
d r2
V Vmin Vmax Vmin -V: driving speed
r1 r2
-V max: maximum velocity
-V min: minimum velocity
- max: maximum steering angle
- : steering angle

<Speed control according to steering angle>


Special Topics of Creative Advanced Robotics 60/100
Creative Advanced Robotics
Application to UGV

Simulation

Obstacle

Obstacle

Special Topics of Creative Advanced Robotics 61/100


Creative Advanced Robotics
Application to UGV
Experiment Condition and Environment
1 2 r1 r2 Vmax Vmin
5m 2m 5m 2m 5.4Km/h 3.6Km/h

PNU Ground
Special Topics of Creative Advanced Robotics 62/100
Creative Advanced Robotics

Application to UGV

Experiment 1 (Obstacle Length: 4m, Width: 50cm)

Special Topics of Creative Advanced Robotics 63/100


Creative Advanced Robotics
Application to UGV

Experiment 2 (Multi Obstacle Cone type diameter:30cm)

Special Topics of Creative Advanced Robotics 64/100


Creative Advanced Robotics

Application to UGV
Experiment 3 (Dynamic Obstacle )

Special Topics of Creative Advanced Robotics 65/100


Creative Advanced Robotics

Application to Cluster Robots


Background
Research Condition
To Realize a cluster with 3 robots
Formation Type with 3 Robots
Line formation
Triangle formation
Parallel formation
Advantages of Line Formation
Most efficient navigation
Static in the environment as small and narrow area

Special Topics of Creative Advanced Robotics 66/100


Creative Advanced Robotics

Application to Cluster Robots


2.1 System
Catalogue Contents

Number of robots 3

Robot size 120 x 200 x 150 (mm)


Micro robot system
Maximum velocity 0.2 m/sec

Degree of Sensor 180 degrees

Vehicle SE-SM243 stepping motor

Processor ATmega128

Ultrasonic Sensor SRF-05

IR Sensor EL-1KL2, ST-1KLA A Side view of Robot

Power 11.1V, 850mAh Li-Po battery Special Topics of Creative Advanced Robotics 67/100
Creative Advanced Robotics

Application to Cluster Robots

Line Formation

Line formation
If the Virtual hill occurs, a cluster of robots can
be in a line and creates attractive and repulsive
force among robots.
This makes it possible to maintain a formation
and automatic driving.
Line formation

Step 1 When a robot recognizes only a goal-Leader


Step 2 When a robot recognizes a goal and one robot-1st Follower
Step 3 When a robot recognizes a goal and two robots -2nd Follower

Special Topics of Creative Advanced Robotics 68/100


Creative Advanced Robotics

Application to Cluster Robots


Step 1 Step 2

Virtual sink
Step 3
Goal

Leader

Follower1,2

Virtual hill
Flow chart line formation

Special Topics of Creative Advanced Robotics 69/100


Creative Advanced Robotics

Application to Cluster Robots


5.1 Simulation
Simulation
Arrival time according to the ultrasonic
scanning degree resolution 1 ~ 9 and
ultrasonic scanning distance resolution
1mm, 10mm (error : 5mm)
Arrival time according to the ultrasonic
scanning interval 2sec~400msec and
ultrasonic scanning distance resolution
1mm, 10mm (error : 5mm)

Special Topics of Creative Advanced Robotics 70/100


Creative Advanced Robotics

Application to Cluster Robots


Simulation

Sensor degree resolution 1


Distance resolution 1mm, 10mm

Sensor degree resolution 3


Distance resolution 1mm, 10mm

Special Topics of Creative Advanced Robotics 71/100


Creative Advanced Robotics
Experiments
Simulation

400
160

350
140

300 120
Arrived Time(s)

Arrived Time(s)
250 100

200 80

150 60
1mm 1mm
100 10mm 40 10mm
Distance Resolution Distance Resolution
50 20

0 0
0 1 2 3 4 5 6 7 8 9 10 0 200 400 600 800 1000 1200 1400 1600 1800 2000 2200
Sensing Angular Resolution Scanning Time Resolution(ms)

Arrival time according to the ultrasonic Arrival time according to the ultrasonic
scanning degree resolution scanning time interval

Special Topics of Creative Advanced Robotics 72/100


Creative Advanced Robotics

Experimental Evaluation

Initial position of robots Experimental test using multi micro robots

Special Topics of Creative Advanced Robotics 73/100


Creative Advanced Robotics

Conclusions
Artificial Potential Field Approach
Simple and efficient path planner
Local minimum problem

Virtual Hill Concept


New concept to escape local minimum
Extra potential

Simulations and Experiments


Extra Potential - absolutely eliminate local minima
Line Formation of Clustering Robot
Line formation using virtual hill concept

Special Topics of Creative Advanced Robotics 74/100


Creative Advanced Robotics
SLAM
What is SLAM ?
Where am I?

SLAM addresses two key problems in


Robotics
Robot Localization,
Where am I?
Robot Mapping,
What does the world look like?
Goal: Simultaneously estimate both
Map & Robot Location!
From Wikipedia, the free encyclopedia

Special Topics of Creative Advanced Robotics 75/100


Creative Advanced Robotics
SLAM
Definition

Simultaneous localization and mapping


(SLAM) is a technique used by
robots and autonomous vehicles
to build up a map within an unknown
environment (without a priori knowledge),
or
to update a map within a known
environment (with a priori knowledge
from a given map),
while at the same time keeping track of
their current location.
From Wikipedia, the free encyclopedia

Special Topics of Creative Advanced Robotics 76/100


Creative Advanced Robotics
SLAM Applications
Oil Pipeline Inspection
Ocean Surveying & Underwater Navigation
Mine Exploration
Coral Reef Inspection
Military Applications
Crime Scene Investigation

Special Topics of Creative Advanced Robotics 77/100


Creative Advanced Robotics
SLAM
Goal
Simultaneously estimate both Map and Robot Location.

How to estimate
It is possible to address both problems in
Stochastic Framework simultaneously.
M. Csorba : PhD Thesis - Oxford 1997
P. Newman : PhD Thesis - ACFR 1999

Probability is the key to estimate both!

Special Topics of Creative Advanced Robotics 78/100


Creative Advanced Robotics

Early work to solve SLAM

R.C. Smith, M. Self, P. Cheeseman


"On the Representation and Estimation
of Spatial Uncertainty, The
International Journal of Robotics
Research 5 (4): 5668., 1986
"Estimating Uncertain Spatial
Relationships in Robotics, UAI '86.
University of Pennsylvania, pp. 435461.,
1986
Bayesian formulation to estimate Spatial
Relationships between Landmarks +
estimate Robot Position

Special Topics of Creative Advanced Robotics 79/100


Creative Advanced Robotics

SLAM - Formalized

Special Topics of Creative Advanced Robotics 80/100


Creative Advanced Robotics

SLAM - Formalized

Special Topics of Creative Advanced Robotics 81/100


Creative Advanced Robotics

Early work to solve SLAM


p( A B) p( A B)
p( B | A) , p ( A | B)
p( A) p ( B)
Bayesian Estimation p( A B) p( A) p( B | A) p( B) p( A | B)

p( D | H ) p( H ) H : Hypothesis
p ( H | D)
p ( D) D : Data
p( D | H ) : likelihood, usually known by the experimenter

p( H ) : prior, reflects ones prior knowledge
n
p ( D) p ( D | H i ) p ( H i ) before the data are considered
i 1
p( H | D) : posterior, reflect the probability of the hypothesis
after consideration of the data

Bayesian approach : to construct the posterior probability density


function (PDF) of the state given all measurements.

Special Topics of Creative Advanced Robotics 82/100


Creative Advanced Robotics

Solutions to SLAM
Two main approaches to solving SLAM

1. Kalman Filtering Approaches


Kalman Filter approximates the posterior as a Gaussian
Smith et al. Robotics Research 1987

2. Particle Filtering Approaches


Particle Filters solve this by estimating a general PDF using sampling
techniques
FastSLAM, DP-SLAM

Special Topics of Creative Advanced Robotics 83/100


Creative Advanced Robotics

Kalman Filtering Approaches


Introduction
In 1960, R.E. Kalman published his famous paper describing a
recursive solution to the discrete-data linear filtering problem.
Since that time, due in large part to advances in digital computing
Kalman Filter Model x k A k x k 1 Buk 1 w k 1
y k H k xk v k
The process and measurement noise are assumed to be independent
(of each other), white, and with normal probability distributions

p(w) ~ N (0, Q),


p( v) ~ N (0, R).

Special Topics of Creative Advanced Robotics 84/100


Creative Advanced Robotics
Kalman Filtering Approaches
Advantages
Simple to implement
Works well in practice

Disadvantages
Assumes Uni-modal Gaussian Probability distributions
Linear Motion Model
Time complexity O(n3) due to matrix inversion

Special Topics of Creative Advanced Robotics 85/100


Creative Advanced Robotics

Kalman Filtering Approaches


Kalman Filter
Linear motion model
Zero-mean Gaussian noise, unimodal distribution
Extended Kalman Filter (EKF)
Slightly nonlinear motion model (first order approximation)
Zero-mean Gaussian noise, unimodal distribution

Unscented Kalman Filter (UKF)


Nonlinear motion model (second order general, third order gaussian)
Zero-mean Gaussian noise, unimodal distribution

Special Topics of Creative Advanced Robotics 86/100


Creative Advanced Robotics

Basic of Kalman Filter


We define x k to be our a priori state estimate at step k given
knowledge of the process prior to step k ,
And x k to be our a posteriori state estimate at step k given
measurement z k .
We can define a priori and a posteriori estimate errors
e k x k x k
e k x k x k ,
The estimate error covariance is
Pk E[ek (ek )T ]
Pk E[ek ek T ]

Special Topics of Creative Advanced Robotics 87/100


Creative Advanced Robotics

Basic of Kalman Filter


x k A k x k 1 Buk 1 w k 1
y k H k xk v k
x k x k K k (y k Hk x k ) x k A k x k 1 Buk 1

The difference (y k Hk x k ) is called the measurement innovation, or


the residual, R .
The residual reflects the discrepancy between the predicted
measurement H k x k and the actual measurement y k .
A residual of zero means that the two are in complete agreement.

K is chosen to be the gain or blending factor that minimizes a


posteriori error covariance, Pk E[ek ek T ] . ek xk xk , R y k Hk x k
Pk HT
Kk Pk E[ek ek T ek ek T ] 0
HPk HT R

Special Topics of Creative Advanced Robotics 88/100


Creative Advanced Robotics

Basic of Kalman Filter


Pk HT
Kk
HPk HT R
Approaches zero, the gain K weights the residual more heavily.
lim K k H 1
R 0

On the other hand, as the a priori estimate error covariance


approaches zero, the gain K weights the residual less heavily.

lim

Kk 0
Pk 0

As the a priori estimate error covariance Pk approaches zero the
actual measurement y k is trusted less and less, while the predicted
measurement H k x k is trusted more and more.

Special Topics of Creative Advanced Robotics 89/100


Creative Advanced Robotics

Kalman Filter Algorithm


The Kalman filter estimates a process recursively :
the filter estimates the process state at some time and then obtains
feedback in the form of measurements.
As such, the equations for the Kalman filter fall into two groups:
- time update equations: predictor equations
- measurement update equations: corrector equations
Pk E[ek (ek )T ] A k Pk 1ATk Q k
Pk E[ek ek T ] Pk K k HPk

The estimation algorithm resembles that of a predictor-corrector


algorithm for solving numerical problems as shown below.

Special Topics of Creative Advanced Robotics 90/100


Creative Advanced Robotics

Kalman Filter Algorithm

Initial state and covariance


x 0 , P0

time update

Input x k Ax k 1 Buk 1
u k 1 Pk Ak Pk 1ATk Qk

measurement update
K k Pk HT (HPk HT R)1
Measurement x k x k K k (y k H k x k ) Estimated state
yk x k
Pk Pk K k HPk

Special Topics of Creative Advanced Robotics 91/100


Creative Advanced Robotics

Extended Kalman Filter


If the process to be estimated and (or) the measurement relationship
to the process is non-linear?
xk f k (xk 1 , uk 1 , w k 1 )
Non-linear function
y k hk (xk , v k )

Independent, white Gaussian distribution noise


p(w) ~ N (0, Q),
p( v) ~ N (0, R).

The EKF is simply an adhoc state estimator that only


approximates the optimality of Bayes rule by linearization.

Special Topics of Creative Advanced Robotics 92/100


Creative Advanced Robotics

Extended Kalman Filter xk f k (xk 1 , uk 1 , w k 1 )

New governing equations that linearize an estimate y k hk (xk , v k )


xk xk A(xk 1 x k 1 ) Ww k 1
y k y k H k (xk 1 x k 1 ) Vv k

The approximate state and measurement vectors


xk f k (x k 1 , u k 1 , 0)
y k hk (xk , 0)

The Jacobian matrix of partial derivatives


f[i ] h[i ]
A[i , j ] (x k 1 , u k 1 , 0) H[i , j ] (x k , 0)
x[ j ] x[ j ]
f[i ] h[i ]
W[i , j ] (x k 1 , u k 1 , 0) V[i , j ] (x k , 0)
w[ j ] v[ j ]

Special Topics of Creative Advanced Robotics 93/100


Creative Advanced Robotics

Extended Kalman Filter

the prediction error : ex xk xk k

the measurement residual : ey y k y k k

Governing equations for an error process : linear


exk A(x k 1 x k 1 ) k
ey k H k exk k

New independent, white Gaussian distribution noise


p( k ) ~ N (0, WQWT ),
p( k ) ~ N (0, VRVT ).

Apply Kalman Filter

Special Topics of Creative Advanced Robotics 94/100


Creative Advanced Robotics

Extended Kalman Filter Algorithm

Initial state and covarience


x 0 , P0

Calculate Jacovian matrix


f A,W
h H ,V

time update
x k f (x k 1 , uk 1 ,0)
Input
u k 1 Pk Ak Pk 1ATk Wk QWkT

measurement update
K k Pk HT (HPk HT Vk RVKT )1
Measurement
x k x k K k (y k h(x k ,0)) Estimated state
yk x k
Pk Pk K k HPk

Special Topics of Creative Advanced Robotics 95/100


Creative Advanced Robotics
Kalman Filtering Approaches
EKF SLAM

Special Topics of Creative Advanced Robotics 96/100


Creative Advanced Robotics
References

Cormen, Thomas H.; Leiserson, Charles E.; Rivest, Ronald L.; Stein, Clifford , "Section
24.3: Dijkstra's algorithm". Introduction to Algorithms (Second ed.). MIT Press and
McGraw-Hill. pp. 595601, 2001.
Anthony Stentz, The Focussed D* Algorithm Algorithm f or Real-Time Replanning, in
Proceedings of IJCAI-95, August 1995.
S.Nishimura, S.Tsuda, Navigation System, J. of the Robotics Society of Japan, 1999.
Khatib, O. "Real-Time Obstacle Avoidance for Manipulators and Mobile Robots," IEEE
International Conference on Robotics and Automation, Vol. 2, pp. 500~505, 1985.
Borenstein, J. and Koren, Y., 1989, "Real-time Obstacle Avoidance for Fast Mobile
Robots," IEEE Transactions on Systems, Man and Cybernetics, Vol. 19, No. 5, pp.
1179~1187, 1989.
Hwang, Y. K. and Ahuja, N., 1992, "A Potential Field Approach to Path Planning," IEEE
Transactions on Robotics and Automation, Vol. 8, No. 1, pp. 23~32, 1992.
Min-Gyu Park, Min-Cheol Lee, "A New Technique to Escape Local Minimum in Artificial
Potential Field Based Path Planning", Int. Journal KSME, Vol.17 , No.12, pp.1876-1885,
December2003(SCIE).
Special Topics of Creative Advanced Robotics 97/100
Creative Advanced Robotics

References
Andrew Hogue, Simultaneous Localization and Mapping - SLAM, York University, 2005
Hugh Durrant-Whyte, Tim Bailey, Simultaneous Localization and Mapping (SLAM): Part I
The Essential Algorithms
Paul Newman, Kin Ho, SLAM- Loop Closing with Visually Salient Features, Oxford
University Robotics Research Group
Raja Chatila, SLAM: Representation Issues, LAAS-CNRS
Nosan Kwak, Beom-Hee Lee, Kazuhito Yokoi, Result Representation of Rao-Blackwellized
Particle Filter for Mobile Robot SLAM
Lecture 17: FastSLAM, CS 344R/393R : Robotics, Benjamin Kuipers

Special Topics of Creative Advanced Robotics 98/100


Creative Advanced Robotics
References

Special Topics of Creative Advanced Robotics 99/100


Creative Advanced Robotics

T H A N K S !!!

Special Topics of Creative Advanced Robotics

Anda mungkin juga menyukai