Implementation of Multi-Goal Motion Planning Under ... - ARAS

(1)

Implementation of Multi-Goal Motion Planning Under Uncertainty on a Mobile Robot

ICRoM 2017

November 20, 2017 Ali Noormohammadi Asl

Hamid D. Taghirad Amirhossein Tamjidi

Dept. of Electrical Engineering K. N. Toosi University of Technology Advanced Robotics and Automated Systems (ARAS)

(2)

16 Introduction Problem Statement

Multi-goal Motion Planning POMDP Problem Formulation

TSP-FIRM TSP-FIRM Graph Obstacles in TSP-FIRM

TSP-FIRM for a Nonholonomic Robot

TSP-FIRM Elements

Experimental Results Implementation Details Offline Phase Online Phase

Conclusion

Advanced Robotics and Automated Systems (ARAS)

Introduction Problem Statement

Multi-goal Motion Planning POMDP

Problem Formulation TSP-FIRM

TSP-FIRM Graph Obstacles in TSP-FIRM

TSP-FIRM for a Nonholonomic Robot TSP-FIRM Elements

Experimental Results Implementation Details Offline Phase

Online Phase Conclusion

(3)

Introduction 2 Problem Statement

TSP-FIRM Elements

Conclusion

Introduction

Motion planning under uncertainty

I Finding a collision-free motion for the robot system from an initial configuration to goal configuration.

I The uncertainty is closely intertwined with most robotic applications.

I Uncertainy emanates from three main sources:

I Model (motion) uncertainty

I Observation (sensor) uncertainty

I Environment (map) uncertainty

I Ignoring uncertainty in planning and decision making may well cause improper results and even failure.

(4)

16 Introduction 2 Problem Statement

TSP-FIRM Elements

Conclusion

Motion planning under uncertainty

I Finding a collision-free motion for the robot system from an initial configuration to goal configuration.

I The uncertainty is closely intertwined with most robotic applications.

I Uncertainy emanates from three main sources:

I Model (motion) uncertainty

I Observation (sensor) uncertainty

I Environment (map) uncertainty

I Ignoring uncertainty in planning and decision making may well cause improper results and even failure.

(5)

3 Multi-goal Motion Planning POMDP Problem Formulation

TSP-FIRM Elements

Conclusion

Problem Statement

Multi-goal Motion Planning

I Finding a policy for the robot to traverse through a number of goal points.

1 Start

2

3

4

5

6

7

(6)

TSP-FIRM Elements

Conclusion

Multi-goal Motion Planning

I Simple TSP: A primitive solution in the absence of uncertainty.

1 Start

2

3

4

5

6

7

(7)

TSP-FIRM Elements

Conclusion

Problem Statement

Multi-goal Motion Planning

I Simple TSP: A primitive solution in the absence of uncertainty.

I The TSP problem should be solved in belief space.

I The path between each two goal points is in not the direct line connecting them.

I The costs are not deterministic.

I The graph is not symmetric.

(8)

Multi-goal Motion Planning POMDP 4 Problem Formulation

TSP-FIRM Elements

Conclusion

POMDP

Partially Observable Markov Decision Processes

I Motion planning under uncertainty is an instance of the problem of sequential decision making under uncertainty.

I Partially Observable Markov Decision Processes (POMDP) is a general framework for sequential decision making under uncertainty.

J(·) =min

Π

∞

X

k=0

E[c(b_k, π(b_k))]

π^∗=argmin

Π

∞

X

k=0

E[c(bk, π(bk))]

s.t. b_k+1=τ(b_k, π(b_k),z_k), z_k ∼p(z_k |x_k)

(9)

Multi-goal Motion Planning POMDP Problem Formulation 5 TSP-FIRM

TSP-FIRM Elements

Conclusion

Problem Statement

Problem Formulation

I Multi-goal motion planning formulation using TSP and POMDP frameworks.

{p,Π}min

N

X

i=1 N

X

j=1

pij

∞

X

k=0

E

cj bⁱ_k, π b_kⁱ

s.t.

N

X

j=1

pij=1 (i6=j,i∈V)

N

X

i=1

p_ij=1 (i6=j,j∈V)

Xp_ij≤ |s| −1 (s⊂V , 2≤ |s| ≤N−2) pij∈ {0,1} (i,j)∈A

bk+1=τ(bk, π(bk),zk)

I This optimization problem is intractable to solve.

(10)

Multi-goal Motion Planning POMDP Problem Formulation 5 TSP-FIRM

TSP-FIRM Elements

Conclusion

Problem Formulation

I Multi-goal motion planning formulation using TSP and POMDP frameworks.

{p,Π}min

N

X

i=1 N

X

j=1

pij

∞

X

k=0

E

cj bⁱ_k, π b_kⁱ

s.t.

N

X

j=1

pij=1 (i6=j,i∈V)

N

X

i=1

p_ij=1 (i6=j,j∈V)

Xp_ij≤ |s| −1 (s⊂V , 2≤ |s| ≤N−2) pij∈ {0,1} (i,j)∈A

bk+1=τ(bk, π(bk),zk)

I This optimization problem is intractable to solve.

(11)

TSP-FIRM 6 TSP-FIRM Graph Obstacles in TSP-FIRM

TSP-FIRM Elements

Conclusion

TSP-FIRM

I Feedback based Information RoadMap is a multi–query graph based algorithm for path planning under uncertainty.

I FIRM helps to simplify the complex POMDP problem into a tractable MDP problem.

I We use FIRM to solve the TSP optimization problem in the belief space.

(12)

TSP-FIRM Elements

Conclusion

TSP-FIRM Graph

1. Forming a Probabilistic RoadMap (PRM) including the goal points and the sampled nodes.

2. design a stabilizer (node controller) for each node.

I each TSP-FIRM node is a small region

B={b:kb−b⁰k ≤}around the sampled beliefb⁰. 3. Designing TSP-FIRM graph edges (local controllers).

I Each edge is a concatenation of the node controller and the edge controller.

(13)

TSP-FIRM Elements

Conclusion

TSP-FIRM

TSP-FIRM Graph

I A graph with the set of nodes and the set of edges is obtained.

I Motion planning over the entire belief space is reduced to planning over the TSP-FIRM graph.

1 2

3

4

5

6

7

(14)

TSP-FIRM TSP-FIRM Graph

8 Obstacles in TSP-FIRM TSP-FIRM for a Nonholonomic Robot

TSP-FIRM Elements

Conclusion

Obstacles in TSP-FIRM

Obstacles

There are three types of obstacles.

1. Completely known obstacles 2. Areas which are potentially obstacle 3. Completely unknown obstacles

One-step-cost











C_i^g(B,u) =0 if B∈B_goalⁱ

or (F happens) and(B∈/ Fsus)

C_i^g(B,u)≥β > ε >0 if (B∈F_sus) C_i^g(B,u)≥ε >0 if otherwise

(15)

TSP-FIRM Elements 9 Experimental Results

Implementation Details Offline Phase Online Phase

Conclusion

TSP-FIRM for a Nonholonomic Robot

TSP-FIRM Elements

TSP-FIRM Nodes

I Designing a stationary Kalman filter (SKF) for each sampled node.

I Constructing TSP-FIRM node with the center b_c^j ≡

v^j,P_s^j .

B^j =n

b≡(x,P) : x −v^j

< δ₁,

P−P_s^j

_m< δ₂o

(16)

Conclusion

TSP-FIRM Elements

Local Controller

I Designing a Kalman filter for the robot position estimation.

I Designing a switching controller for the stabilizer (node controller).

I Designing an LQG controller for the edge controller to drive the robot from a node to another one.

(17)

Conclusion

TSP-FIRM for a Nonholonomic Robot

TSP-FIRM Elements

Transition Cost and Probability

Using the sequential Monte Carlo method for computing transition costs and probabilities.

I Estimation accuracy,Φ^ij =E hPT

k=1tr

wP_k^iji .

I Stopping time of local controller,Tˆ^ij =E T^ij

.

I The time robot moves in the high-risk area,Tˆ_obs^ij =E T^ij

.

C Bⁱ, µ^ij

=ξ₁Φîj+ξ₂Tˆîj+ξ₃Tˆ_obsîj

(18)

Conclusion

TSP-FIRM Elements

Transition Cost and Probability

Using the sequential Monte Carlo method for computing transition costs and probabilities.

I Estimation accuracy,Φ^ij =E hPT

k=1tr

wP_k^iji .

I Stopping time of local controller,Tˆ^ij =E T^ij

.

I The time robot moves in the high-risk area,Tˆ_obs^ij =E T^ij

.

C Bⁱ, µ^ij

=ξ₁Φîj+ξ₂Tˆîj+ξ₃Tˆ_obsîj

(19)

TSP-FIRM Elements

Experimental Results Implementation Details 12 Offline Phase Online Phase

Conclusion

Experimental Results

Implementation Details

Motion Model

Differential wheeled mobile robot: MELON

Xk+1=f(Xk,uk,wk) =





xk+ (Vk+nv)δtcosθk

yk+ (Vk+nv)δtsinθk

θ_k+ (w_k+n_w)δt





Qk=diag

ηvVk+σ^V_b2

,(ηwwk+σ_b^w)²

(20)

TSP-FIRM Elements

Conclusion

Motion Model

Differential wheeled mobile robot: MELON

Xk+1=f(Xk,uk,wk) =





x_k+ (V_k+n_v)δtcosθ_k yk+ (Vk+nv)δtsinθk

θk+ (wk+nw)δt





Q_k=diag

ηvV_k+σ^V_b2

,(ηww_k+σ_b^w)²

Sensor Model

Black and white patterns: ArUco markers

jzk= ^jdk

,atan2 ^jd2_k,^jd1_k

−θT

+^jv,^jv∼ N 0,^jR

jRk=diag ηrd

^jdk

+ηrφ|φk|+σ_b^r2

, ηθd

^jdk

+ηθφ|φk|+σ^θ_b2

(21)

TSP-FIRM Elements

Conclusion

Experimental Results

Map

Second floor of the Electrical Engineering department at K. N.

Toosi University of Technology.

(22)

TSP-FIRM Elements

Experimental Results Implementation Details

14 Offline Phase Online Phase

Conclusion

Offline Phase

I Choosing goal points

I Generating graph

(23)

TSP-FIRM Elements

Experimental Results Implementation Details

14 Offline Phase Online Phase

Conclusion

Experimental Results

Offline Phase

I Choosing goal points

I Generating graph

I Calculating transition cost, transition probability and failure probability

I Finding the best way between each two goal points

I Solving TSP:[1,2,3,4,5,6,7,1]

[1,10,9,8,2,11,12,3,14,5,15,6,22,7,21,17,16,4,13,3,12, 11,2,8,9,10,1]

(24)

TSP-FIRM Elements

Experimental Results Implementation Details Offline Phase

15 Online Phase Conclusion

Online Phase

The robot starts executing the offline obtained policyBUT

Challenges in the Online Phase

I Change in map

I Deviation from planned path

I Observation loss

I Robot Kidnapping

An online replanning algorithm is proposed to update map, graph, and policy

(25)

TSP-FIRM Elements

Conclusion 16

Conclusion

I Modeling multi-goal motion planning problem as an asymmetric traveling salesman problem (ATSP) in the belief space.

I Presenting algorithms for the offline generation of TSP-FIRM graph and online execution.

I Generilizing the algorithms for nonholonomic mobile robots.

I Implementation on a physical mobile robot in a real environment.