Upload
others
View
1
Download
0
Embed Size (px)
Citation preview
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]
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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]
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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]
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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)
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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)
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
Formulation - Multi-robot Belief Space Planning
15
Formulation - Multi-robot Belief Space Planning
§ 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:
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
( ) ( )( )1ˆ[ ] ,b N X -= LP P P§ Maximum a posteriori (MAP) inference:
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
17
Formulation - Multi-robot Belief Space Planning§ Probability distribution function (pdf) of multi robot at planning time :tk
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
( ) ( )( )1ˆ[ ] ,b N X -= LP P P§ Maximum a posteriori (MAP) inference:
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
18
Formulation - Multi-robot Belief Space Planning§ Probability distribution function (pdf) of multi robot at planning time :tk
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
( ) ( )( )1ˆ[ ] ,b N X -= LP P P§ Maximum a posteriori (MAP) inference:
19
Formulation - Multi-robot Belief Space Planning§ 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
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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 rk k k v v v v k l
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
21
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 rk k k v v v v k l
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
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
22
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 rk k k v v v v k l
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
'
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
25
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
26
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
27
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
28
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
29
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
30
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
31
§ 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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
Algorithm Overview – Prior Correlation
Belief at time k
§ With prior correlation at time k
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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)
Algorithm Overview – Prior Correlation
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
41
Results - Basic scenario
2 robots:25 candidate paths
4 robots:25 candidate paths
42
Results - Basic scenario
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
43
§ Statistical study of 50 runs (2 and 4 robots):
§ More than twice efficient§ Same results as Standard
approach
Results - Basic scenario
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
Results – Larger Scale Scenario – Planning 2Candidate paths
SLAM
Chosen paths
47
Results – Larger Scale Scenario – Planning 3Candidate paths
SLAM
Chosen paths
48
Results – Larger Scale Scenario – Final Result
SLAM
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
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
T. Regev, Multi-Robot Decentralized Belief Space Planning in Unknown Environments.Graduate Seminar, July 2016
52
Introduction – Localization
§ Navigation in known environment or with GPS.
Localization: Where am I?
What happens when map is unknown and without GPS?