Multi-Robot Decentralized Belief Space Planning in Unknown ...Multi-Robot Decentralized Belief Space...

Multi-Robot Decentralized Belief Space Planningin Unknown Environments

Tal RegevUnder the supervision of Assistant Prof. Vadim Indelman

Graduate Seminar, July 2016

International Conference on Intelligent Robots and Systems (IROS 2016), Accepted

T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016

2

Introduction – Application

§ Navigation and mapping in unknown environment with multiple robots (MR)

Navigation and mapping with MR: Why is it interesting?

Autonomous cars Space exploration Search & rescue operations[google.com] [nasa.gov]

[spectrum.ieee.org]


3

Introduction - SLAM

§ Navigation and mapping in unknown environment without GPS§ Simultaneous localization and mapping (SLAM)

Estimation and Perception: Where am I? What is the surrounding environment?

PerceptionEstimation

[societyofrobots.com]


4

Introduction - SLAM

§ Represent robot knowledge in a graph model– Vertices represent the variables. For example location of robot

– Edges represent constrains between variables, also known as factors

§ Allows computationally efficient probabilistic inference, given data§ For example, pose estimation given sensor data (e.g. images)

PerceptionEstimationFactor Graph


5

Introduction - Planning

§ Navigation and mapping in unknown environment without GPS§ Key components for autonomous operation include

– Estimation and Perception: Where am I? What is the surrounding environment?

– Planning: What to do next?

§ Belief space planning (BSP) - fundamental problem in robotics

Perception

Planning

Estimation


6

Introduction – Multi Robot SLAM

§ Navigation and mapping in unknown environment with multiple robots (MR)– Robust and faster exploration/mapping– Higher accuracy in a multi robot collaborative framework

[Indelman ‘16 CSM, ‘14 ICRA][J. Dong et al. ‘15 ICRA]


7

Introduction - Multi Robot Belief Space Planning

§ Belief space planning in unknown environment with multiple robots (MR)§ Reason about uncertainty within planning and consider collaboration

between robots

[Indelman ‘15 ISRR]

Start Start


8

§ Solving multi-robot BSP is in particular challenging– Involves considering all combinations of candidate paths of all robots– Existing approaches typically assume environment/map is known

Red Robot

Green Robot

Blue Robot

Purple Robot

Related Work – Belief Space Planning (BSP)


9

Related Work – Sampling Methods

§ Sampling Methods– Discretize the environment– Candidate trajectories

§ Existing approaches– Rapidly exploring random trees (RRT)

– Rapidly exploring random graph (RRG)– Probabilistic road map (PRM)

PRM


10

§ Recent works enable operation in unknown environments [Kim et al. ‘14 IJRR] [Indelman et al. ‘15 IJRR] [Indelman ‘15 ISRR]

§ In particular, reason about future observations of unknown environments within multi-robot belief space planning [Indelman ‘15 ISRR]

– Existing approaches typically assume environment/map is known

Related Work – Belief Space Planning (BSP)


11

Related Work – Announced Path Approach– Involves considering all combinations of candidate paths of all robots

Candidate paths

Chosen path

1st iteration

Robot r

Calculations are performed from scratch at each iteration

'rPCandidate

paths Chosen path

2nd iterationRobot r

Candidate paths

rnewP

Chosen path

Robot r’

Candidate paths

rPChosen path

Robot r’

§ A common (sub-optimal) iterative approach to reduce computational effort: [Atanasov ‘14 TRO] [Levine ’13 JAIS]

12

Contribution

§ Each time the announced path from some robot r’ changes, each robot r has to re-evaluate its candidate paths

§ Key idea– Not all paths are

impacted due to change in the announced paths

– Impacted paths can be efficiently re-evaluated by reusing calculations


13

§ Belief for robot r at a future time 𝑡"#$:

k + 1k k + l. . .

Planning time

l-th look ahead step

. . . k + L

( )0: 0: 1| ,r r r rk l k l k l k lb X p X Z u+ + + + -é ùë û B

§ 𝑍&:"#$( - Observations available to robot r

§ 𝑋"#$( - Poses and landmarks estimated by robot r

§ 𝑢&:"#$+,( - Controls of robot r

Formulation - Multi-robot Belief Space Planning


14

§ Optimal controls for all R robots:

§ Multi-robot objective function:

§ Belief for robot r at a future time 𝑡"#$:

k + 1k k + l. . .

Planning time

l-th look ahead step

. . . k + L

( )0: 0: 1| ,r r r rk l k l k l k lb X p X Z u+ + + + -é ùë û B

argmin ( )J=ÂU

U U

1 1( ) ( [ ], )

L Rr r rl k l k l

l rJ c b X u+ +

= =

é ù= ê úë ûååEU


15


§ Probability distribution function (pdf) of multi robot at planning time :tk

1 1

( ),

0: 0: 1 ,1 1 { , }

[ ] ( | , ) ( | , ) ( | ) ( | , )r

l l l l i j

LRr r r r r r r r r

k k k v v v v k l i j v vr l i j

b p X p x x u p Z X p z x x- -

¢ ¢- +

= =

é ù= ×ê ú

ê ûëÕ Õ Õ

P

P Z U

1 , ,( , , ), ( , , )r r r r r r r ri i i i i j i j i jx f x u w z h x x v+ = =

§ State transition and observation models:

§ - Some specific candidate paths for all robots ', ,rré ù= ë ûKP PPP

( ) ( )( )1ˆ[ ] ,b N X -= LP P P§ Maximum a posteriori (MAP) inference:


16

Formulation - Multi-robot Belief Space Planning§ Probability distribution function (pdf) of multi robot at planning time :tk

1 1

( )

1 10: 0:

{ , }

,,1 ( | , ) ( | ) ( | , )( | , )[ ] l l l l i j

r

r r r r r r r r rL

v v v

R

r l iv k l ik k k j v v

j

p x x u p Z X p z x xpb X- -

¢ ¢+

= =- ×

é ù= ê ú

ê ûëÕ Õ Õ

P

Z UP

Prior

( ), ,

,1 1 { , }

( )rLR

r local r rl i j

r l i jk

¢

= =

é ùL = + L + Lê ú

ê úûL

ëå å å

P

P



17


1 11 {

,0:

( )

, },

10: 1 ( | , )( | , ) ( | , )( )[ ] |

r

l il l l j

Lr r r r rv v v v k

r r r rk k

R

rk i j vl

lv

i j

p x x u pp X p z x xb Z X- - +

=

¢

=

¢- ×

é ù= ê ú

ê ûëÕ Õ Õ

P

Z UP

Prior Local information

,,

1 {

(,

1 , }

)

( )rL

r locall

Rr ri j

r il jk

= =

¢é ùL = + + Lê úê ú

Lû

Lë

å ååP

P



18


1 10,,

{:

( )

1 ,: 1

10

}

( | , ) ( | , )[ ] ( | , )( | )l l l

r

il j

r r r r rk k k v v v v k l

r r rLR

r

ri j v v

i jl

p z x xb p X p x x u p Z X- -- +

= =

¢ ¢×é ù

= ê úê ûë

Õ Õ ÕP

Z UP

Prior Local information

( ), ,

,{ , }11

( )rL

r localk l

l

r ri

ir j

R

j= =

¢Lé ù

L = + +ê úêë

Lû

Lú

å ååP

P

Mutual observations


19


1 1

( ),

0: 0: 1 ,1 {1 , }

( | , ) ( | , ) ( | ) ( | , )[ ]r

l l l l i j

Lr r r r r r r r r

k k k v v v v k l i j v vl

R

r i j

p X p x x u p Z X pb z x x- -

¢ ¢- +

==

×é ù

= ê úê ûë

Õ Õ ÕP

Z UP

Factor graph


20

Factor Graph Inference

§ Joint state :,r r¢P P

'

1 1

1 1

( )

1

( )

0: 0: 1

' ' ' ' ' ,

1 1 { , },

( | , ) ( | , ) ( | )

( | , ) ( | ) (

[ ,

)

]

| ,

r

r

l l l l

l l l l i j


r r r r r r r r rv v

Lr r

l

L R

l r i jv v k l i j v v

p X p x x u p Z X

p x x u p Z X p z x

b

x

- -

- -

- +

¢ ¢

=

+

¢

= =

é ù×ë û

é ù×ë û

=

é ùê úê ûë

Õ

Õ Õ Õ

P

P

ZP P U

§ Joint state :', rnr ewP P


21



'

1 1

1 1

( )

1

( )

0: 0: 1

' ' ' ' ' ,

1 1 { , },

( | , ) ( | , ) ( | )

( | , ) ( | ) (

[ ,

)

]

| ,

r

r

l l l l

l l l l i j



Lr r

l

L R


p X p x x u p Z X

p x x u p Z X p z x

b

x

- -

- -

- +

¢ ¢

=

+

¢

= =

é ù×ë û

é ù×ë û

=

é ùê úê ûë

Õ

Õ Õ Õ

P

P

ZP P U

1 1

'

1 1

( )' ' ' ' ' ,

,1 1 { ,

( )

0:

}

0: 11

( | , ) ( | ) ( | , )

( | , ) ( |[ , ] , ) ( | )l l l l

r

l l l l i j

r

r r r r rk k k v

L Rr r r r r r r r rv v v v k l i j v v

l

Lr r

ne v v v k l

r j

wl

i

p x x

p X p x x u p

u p Z X p z

b

x

Z X

x

- -

- -

-¢

=

¢ ¢+

= =

+

é ùé ù× ê úë

é ù

û ê

×ë=

ë

û

ûÕ Õ Õ

ÕP

P

UP P Z

§ Joint state :', rnr ewP P Does not change


22



'

1 1

1 1

( )

1

( )

0: 0: 1

' ' ' ' ' ,

1 1 { , },

( | , ) ( | , ) ( | )

( | , ) ( | ) (

[ ,

)

]

| ,

r

r

l l l l

l l l l i j



Lr r

l

L R


p X p x x u p Z X

p x x u p Z X p z x

b

x

- -

- -

- +

¢ ¢

=

+

¢

= =

é ù×ë û

é ù×ë û

=

é ùê úê ûë

Õ

Õ Õ Õ

P

P

ZP P U

'

1 1

1 1

(

( )' ' ' ' '

1

11

0:

)

0:( | , ) ( | , ) ( |

( | , )

[

| )

)]

(

,

r

l l l

l l l

r

l

l

Lr r r r r r r

k k k v v v

Lr r

newl

r r rv v v v k l

v k l

l

p X p x

p x x u p Z X

u pb x Z X

-

-

-

-

¢

=- +

+=

é ù×ë û=

é ù×ë û

Õ

Õ

P

P

Z UP P

§ Joint state :', rnr ewP P Does not change


23

Algorithm Overview – High Level

§ Iterate over vertices in previous and new announced paths§ Identify involved vertices in multi-robot factors – collect into set 𝑉./0§ Identify and mark involved paths from all candidate paths

§ Key Idea– Not all paths are impacted due to change in the announced paths– Impacted paths can be efficiently re-evaluated by reusing calculations


24

§ Key Idea

– Iterate vertices 𝑣(2that belong to 𝒫(4 or 𝒫/56(4

• Find all nearby vertices 7𝑣(} ⊆ 𝑉( to 𝑣(2

• Add 𝑣. to 𝑉./0– Identify multi-robot factors to be

added or removed– Mark all candidate paths that go

through vertex

Algorithm Overview – Details


25

§ Key Idea





through vertex



26

§ Key Idea





through vertex



27

§ Key Idea





through vertex



28

§ Key Idea





through vertex



29

§ Key Idea





through vertex



30

§ Key Idea





through vertex



31

§ Key Idea





through vertex



32

§ Non-marked paths– Calculate once the change

– Add to old objective function

1

Lr r

ll

J c¢ ¢=

D DåB

rJ ¢D

Algorithm Overview


33

. .

( ) ( )

k l k l

k l k l f FG f FGf FG f FGf t t f t t

f f

+ +

¢+ + Î Î¢Ï Ï

£ £

¢L = L - L + Lå å

From previous step!

§ Non-marked paths– Calculate once the change

– Add to old objective function

1

Lr r

ll

J c¢ ¢=

D DåB

rJ ¢D

§ Marked paths– Iterate over vertices in 𝑉./0, add or remove multi-robot factors– Evaluate objective function from new Information matrix

( ), ,

,1 1 { , }

( )rLR

r local r rk l i j

r l i j

¢

= =

é ùL = L + L + Lê ú

ê úë ûå å å

P

P

Our method Standard method

Algorithm Overview

34

Algorithm Overview – Prior Correlation

Belief at time k

§ No prior correlation at time k

35


Belief at time k

§ With prior correlation at time k


36

§ So far – robots’ beliefs at time k were assumed to be not correlated§ In practice, correlation may exist, e.g.:

– Robots have observed a mutual scene (or landmarks)– Robots made a direct observation of each other

§ Our approach handles these cases as well (next slides)


37

Algorithm Overview – Prior Correlation§ Two possible cases

– With multi-robot factors– Without multi-robot factors

38

Algorithm Overview – Prior Correlation§ Two possible cases

– With multi-robot factors– Without multi-robot factors

39

Algorithm Overview – Prior Correlation§ If covariance changes significantly,

mark all paths§ Otherwise, do not mark

Correlation

Covariance at time k

§ Two possible cases– With multi-robot factors– Without multi-robot factors

40

Results

§ Scenario: multiple robots autonomously navigating to goals in unknown environment

§ Simulation results– 25 candidate paths per robot– PRM– No GPS

§ Next slides:– Basic scenario: In-depth study for single goal– Larger scale scenario: multiple goals and planning sessions, SLAM in

between


41

Results - Basic scenario

2 robots:25 candidate paths

4 robots:25 candidate paths

42



43

§ Statistical study of 50 runs (2 and 4 robots):

§ More than twice efficient§ Same results as Standard

approach


44

Results – Larger Scale Scenario§ Each robot has multiple goals§ Multiple planning sessions§ SLAM session given calculated robot paths (actions)§ Two first robots start with high position uncertainty

4 robots: 25 candidate paths

45

Results – Larger Scale Scenario – Planning 1Candidate paths

SLAM

Chosen paths

46


SLAM

Chosen paths

47


SLAM

Chosen paths

48

Results – Larger Scale Scenario – Final Result

SLAM


49

Conclusions and Future Work

§ Collaborative multi-robot belief space planning in unknown environments§ Contribution:

– Identify impacted paths due to change in announced paths– Efficiently re-evaluate belief only for impacted paths– One-time re-calculation for all non-impacted paths– Performance study in simulation

§ Future work includes: – Concept may be generalized to other BSP approaches– Implement method in an incremental setting (e.g. RRG, RRT)– Extend approach to active cooperative localization and target tracking

50

Results

51

Results


52

Introduction – Localization

§ Navigation in known environment or with GPS.

Localization: Where am I?

What happens when map is unknown and without GPS?

Multi-Robot Decentralized Belief Space Planning in Unknown ...Multi-Robot Decentralized Belief Space...

Documents

Transcript of Multi-Robot Decentralized Belief Space Planning in Unknown ...Multi-Robot Decentralized Belief Space...