Cooperative Frontier-Based ExplorationStrategy for Multi-Robot System

N. Mahdoui, V. Frémont and E. Natalizio

Séminaire Systèmes Robotiques en Interaction26 juin, 2018.

Séminaire SyRI – 26 juin, 2018 1

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Plan

l Introduction

l Exploration strategy

l MAV fleet coordination

l Simulation study

l Conclusions

Séminaire SyRI – 26 juin, 2018 2

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

IntroductionContext

Séminaire SyRI – 26 juin, 2018 3

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

IntroductionApplications

Search and rescue.(

http://www.onyxstar.net/\search-and-rescue-by-drone/)

Infrastructure inspection.( https://uavamerica.com/

infrastructure-inspection-a-customer-perspective/)

Reconnaissance and surveillance.( http://www.directindustry.fr/prod/riegl-lms/

product-15822-1937769.html )

Exploration.( http:

//www.geologyin.com/2014/08/drones-for-geology.html)

Séminaire SyRI – 26 juin, 2018 4

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

IntroductionMotivations

Fleet of MAVs deployment+ Scalability

+ Limited failure risks

+ Performances increase

+ Less time execution

+ Area coverage improvement

SENSOR? EXPLORATION? COORDINATION?DATA? WHEN? TO WHOM?

Séminaire SyRI – 26 juin, 2018 5

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

IntroductionMotivations

Fleet of MAVs deployment+ Scalability

+ Limited failure risks

+ Performances increase

+ Less time execution

+ Area coverage improvement

SENSOR? EXPLORATION? COORDINATION?DATA? WHEN? TO WHOM?

Séminaire SyRI – 26 juin, 2018 5

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

IntroductionMotivations

Fleet of MAVs deployment+ Scalability

+ Limited failure risks

+ Performances increase

+ Less time execution

+ Area coverage improvement

SENSOR? EXPLORATION? COORDINATION?DATA? WHEN? TO WHOM?

Séminaire SyRI – 26 juin, 2018 5

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Planl Introduction

l Exploration strategym Map constructionm Frontier points and information gain computationm Frontier points processingm Goal assignment

l MAV fleet coordination

l Simulation study

l Conclusions

Séminaire SyRI – 26 juin, 2018 6

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Map construction

3D Point cloud

T1−→

3D voxels

T2−→

2D cells

T1: Occupancy grid mapping frameworkT2: 2D projection onto the plane z = 0

Séminaire SyRI – 26 juin, 2018 7

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points and information gain computation

Information gain• The information gain of Fij

represents the number of unknowncells adjacent to it

Séminaire SyRI – 26 juin, 2018 8

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points and information gain computation

Frontier points• Frontier points are cells that lie

between unknown and either freeor occupied∗ cells

∗ will be discarded later

Information gain• The information gain of Fij

represents the number of unknowncells adjacent to it

Séminaire SyRI – 26 juin, 2018 8

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points and information gain computation

Frontier points• Frontier points are cells that lie

between unknown and either freeor occupied∗ cells

∗ will be discarded later

Information gain• The information gain of Fij

represents the number of unknowncells adjacent to it

Séminaire SyRI – 26 juin, 2018 8

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points processing

TwoMAVs:

ThreeMAVs:

Frontier points of local maps.

Frontier points in overlap. Candidate frontier points.

Delete the frontier points adjacent to occupied cells.

Séminaire SyRI – 26 juin, 2018 9

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points processing

TwoMAVs:

ThreeMAVs:

Frontier points of local maps. Frontier points in overlap.

Candidate frontier points.

Delete the frontier points adjacent to occupied cells.

Séminaire SyRI – 26 juin, 2018 9

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points processing

TwoMAVs:

ThreeMAVs:

Frontier points of local maps. Frontier points in overlap. Candidate frontier points.

Delete the frontier points adjacent to occupied cells.

Séminaire SyRI – 26 juin, 2018 9

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Frontier points processing

TwoMAVs:

ThreeMAVs:

Frontier points of local maps. Frontier points in overlap. Candidate frontier points.

Delete the frontier points adjacent to occupied cells.

Séminaire SyRI – 26 juin, 2018 9

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMap construction Frontier points and information gain computation Frontier points processing Goal assignment

Goal assignmentmulti-MAV

Input : Candidate targets tj ∈G with their respective information gain I(tj ), states ofMAVs si in the cluster C, number of MAVs in the cluster nc

Output : θ(i , j) assignment of MAVi with target jAssigned targets G =;while MAVi still without target goal do

// Compute the utility of reaching each target:while tj ∉G do

U(tj )= I(tj )exp(−λ.(dmin(si , tj )+nc/∑nc

k=1,k 6=i (dmin(sk , tj ))))

end// Compute the target that maximizes the utility:tj′ = argmaxtj ∈G∩G U(tj )

// Assign target j′ to MAVi :θ(i , j′)// Schedule the information gain of the remaining candidate targets:while tj ∈G∩G do

if dist(tj′ , tj )<= thresholdRange thenI(tj )= I(tj )(1−exp(−γ.‖tj′ − tj ‖))

endend// Remove the target goal tj′ :G =G∪ tj′

end

Séminaire SyRI – 26 juin, 2018 10

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Planl Introduction

l Exploration strategy

l MAV fleet coordinationm Clusters and status selectionm Exchanged datam MAVs interactionm Strategies to face some issues

l Simulation study

l Conclusions

Séminaire SyRI – 26 juin, 2018 11

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsClusters and status selection Exchanged data MAVs interaction Strategies to face some issues

Clusters and status selection

Séminaire SyRI – 26 juin, 2018 12

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsClusters and status selection Exchanged data MAVs interaction Strategies to face some issues

Exchanged data

Data 1• Identification Number id

• State si

Data 2• Frontier points fi ,j with their

respective information gain I(fi ,j )

Data 3• Assigned target tk

Note: All MAVs’ status are initialized to Leader.

Séminaire SyRI – 26 juin, 2018 13

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsClusters and status selection Exchanged data MAVs interaction Strategies to face some issues

MAVs interaction

Séminaire SyRI – 26 juin, 2018 14

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsClusters and status selection Exchanged data MAVs interaction Strategies to face some issues

Strategies to face some issues

Communication loss/failure• If the Explorer does not receive any

assigned target from the Leader after aperiod τ, it selects its own one

MAV getting stuck• MAVs’ status are always updated

• A new Leader is selected at eachiteration during the mission

Séminaire SyRI – 26 juin, 2018 15

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Plan

l Introduction

l Exploration strategy

l MAV fleet coordination

l Simulation studym Implementation detailsm Exploration evaluationm Evaluation with an Ad Hoc network

l Conclusions

Séminaire SyRI – 26 juin, 2018 16

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Implementation details

http://www.ros.org/http://gazebosim.org/http://wiki.ros.org/ardrone_autonomy

FoV π/3 Trade-off pa-rameter λ

0.2

Dimension 8m∗8m Min distancebetween fron-tiers d

0.3

Resolution 0.05m Range to re-duce the Ig[Rmin ,Rmax ]

[1m,3m]

Linear veloc-ity

[0.1,0.3] Theta veloc-ity

[0.1,0.3]

Initial posi-tion

One MAV: {MAV1(1,1)} ;

two MAVs: {MAV1(1,1);MAV2(2,2)} andThree MAVs: {MAV1(2,2);MAV2(0,0);MAV3(−2,−2)}

Séminaire SyRI – 26 juin, 2018 17

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Implementation details

http://www.ros.org/http://gazebosim.org/http://wiki.ros.org/ardrone_autonomy

FoV π/3 Trade-off pa-rameter λ

0.2

Dimension 8m∗8m Min distancebetween fron-tiers d

0.3

Resolution 0.05m Range to re-duce the Ig[Rmin ,Rmax ]

[1m,3m]

Linear veloc-ity

[0.1,0.3] Theta veloc-ity

[0.1,0.3]

Initial posi-tion

One MAV: {MAV1(1,1)} ;

two MAVs: {MAV1(1,1);MAV2(2,2)} andThree MAVs: {MAV1(2,2);MAV2(0,0);MAV3(−2,−2)}

Séminaire SyRI – 26 juin, 2018 17

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Exploration evaluationRatio of explored space during time for one MAV

(a) 3% rate(5s)

(b) 20% rate(28s)

(c) 31% rate(50s)

(d) 40% rate(69s)

(e) 50% rate(75s)

(f) 60% rate(83s)

(g) 70% rate(98s)

(h) 81% rate(127s)

(i) 90% rate(133s)

(j) 99% rate(202s)

Séminaire SyRI – 26 juin, 2018 18

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Exploration evaluationProjected 2D map in a coordinated exploration with a team ofone, two and three MAVs

(a) One MAV (b) Two cooperative MAVs (c) Three cooperative MAVs

Green, blue and red markers and arrows define the initial position and the trajectory of MAV1, MAV2and MAV3, respectively.

Séminaire SyRI – 26 juin, 2018 19

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Exploration evaluationTotal time spent during exploration and total distance traveledby each MAV in the fleet

# MAVs Total traveled dis-tance (m)

Total time explor-ing (s)

One MAV MAV1 22.69±2.74 197.85±17.34

Two MAVs MAV1 15.12±2.35 133.85±7.83MAV2 9.35±1.81

Three MAVs MAV1 10.91±1.04 126.42±14.08MAV2 7.75±1.9MAV3 8.30±1.71

Séminaire SyRI – 26 juin, 2018 20

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Frontier points processingExplored area space and frontier points evolution

A:

B:

One MAV Two cooperative MAVs Three cooperative MAVs

A: Explored space rate with one, two and three MAVs

B: The evolution of frontier points and candidate targets number during cooperative exploration

Séminaire SyRI – 26 juin, 2018 21

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Frontier points processingExplored area space and frontier points evolution

A:

B:

One MAV Two cooperative MAVs Three cooperative MAVs

A: Explored space rate with one, two and three MAVsB: The evolution of frontier points and candidate targets number during cooperative exploration

Séminaire SyRI – 26 juin, 2018 21

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Evaluation with an Ad Hoc networkImplementation details

One ROS core on multiplemachines:

+ Easy to implement

− Vulnerable regarding the ROScore

− Reliable network required

Multiple ROS cores on multiplemachines (multi-master):

+ Independent ROS cores

+ Robust to network failure

− Cores synchronizationrequired

http://wiki.ros.org/multimaster_fkieNetwork Time Protocol (NTP)

Séminaire SyRI – 26 juin, 2018 22

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Evaluation with an Ad Hoc networkImplementation details

One ROS core on multiplemachines:

+ Easy to implement

− Vulnerable regarding the ROScore

− Reliable network required

Multiple ROS cores on multiplemachines (multi-master):

+ Independent ROS cores

+ Robust to network failure

− Cores synchronizationrequired

http://wiki.ros.org/multimaster_fkieNetwork Time Protocol (NTP)

Séminaire SyRI – 26 juin, 2018 22

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsImplementation details Exploration evaluation Evaluation with an Ad Hoc network

Evaluation with an Ad Hoc networkComparison of exchanged data size of 2D occupancy gridversus frontier points

ResultThe data exchanged using the proposedstrategy are 10 times less than those of astrategy that makes MAVs exchange thewhole local map

Demovideo/Final_with_com_two_t.mp4

Séminaire SyRI – 26 juin, 2018 23

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Plan

l Introduction

l Exploration strategy

l MAV fleet coordination

l Simulation study

l Conclusions

Séminaire SyRI – 26 juin, 2018 24

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Conclusions• Frontier-based strategy

→ Exploration time

• Novel utility function

→ traveled distance

• Leader-based approach

→ Communication issues

• Frontier points exchange

→ Exchanged data size

Future work• Large scale evaluation

• Relative localization estimation

Séminaire SyRI – 26 juin, 2018 25

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Conclusions• Frontier-based strategy

→ Exploration time

• Novel utility function

→ traveled distance

• Leader-based approach

→ Communication issues

• Frontier points exchange

→ Exchanged data size

Future work• Large scale evaluation

• Relative localization estimation

Séminaire SyRI – 26 juin, 2018 25

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Conclusions• Frontier-based strategy

→ Exploration time

• Novel utility function

→ traveled distance

• Leader-based approach

→ Communication issues

• Frontier points exchange

→ Exchanged data size

Future work• Large scale evaluation

• Relative localization estimation

Séminaire SyRI – 26 juin, 2018 25

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Conclusions• Frontier-based strategy

→ Exploration time

• Novel utility function

→ traveled distance

• Leader-based approach

→ Communication issues

• Frontier points exchange

→ Exchanged data size

Future work• Large scale evaluation

• Relative localization estimation

Séminaire SyRI – 26 juin, 2018 25

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Conclusions• Frontier-based strategy

→ Exploration time

• Novel utility function

→ traveled distance

• Leader-based approach

→ Communication issues

• Frontier points exchange

→ Exchanged data size

Future work• Large scale evaluation

• Relative localization estimation

Séminaire SyRI – 26 juin, 2018 25

Introduction Exploration strategy MAV fleet coordination Simulation study Conclusions

Thank you for your attention.

Séminaire SyRI – 26 juin, 2018 26

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

Mapping process

OctomapThe octomap is a 3D mapping framework based on a hierarchical data structure (Octree).The 3D space is recursively subdivided until attending minimum voxel size (resolution).

• Compact memoryrepresentation

• Fast data access

• Obstacle avoidance Hornung, A.; Wurm, K. M.; Bennewitz, M.; Stachniss, C.& Burgard, W. OctoMap: An efficient probabilistic 3Dmapping framework based on octrees AutonomousRobots, Springer, 2013, 34, 189-206

Séminaire SyRI – 26 juin, 2018 27

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

The utility function

Utility function

U(tj)= I(tj)exp(−λ.(dmin(si , tj)+nC/∑nC

k=1,k 6=i(dmin(sk , tj))))

where:I(tj ): information gain of the target goal tjλ: tuning parameter for trade-off between rapid exploration and filling in detailsnC : number of neighbors in the cluster Cdmin(si , tj ): shortest distance from robot i’s state to the candidate target goal

Séminaire SyRI – 26 juin, 2018 28

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

Path planning and control

move_base package• Velocity commands

• Global and local planner

• Dijkstra algorithm

Séminaire SyRI – 26 juin, 2018 29

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

ROS data exchange

ROS frames for two MAVs.

ROS topics for two MAVs.

Topics for data exchange• ID_informtion : Robot ID (int) and its

state (geometry_msgs/PoseStamped)

• Frontiers_to_leader_topic: frontiers(geometry_msgs/Point) and theircorresponding information gain(int)

• Target_goal_information : state of thefuture target (geometry_msgs/Pose)

• Ids_of_reached_goals: informationfor acknowledgment(int)

Séminaire SyRI – 26 juin, 2018 30

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

ROS data exchange

ROS frames for two MAVs.

ROS topics for two MAVs.

Topics for data exchange• ID_informtion : Robot ID (int) and its

state (geometry_msgs/PoseStamped)

• Frontiers_to_leader_topic: frontiers(geometry_msgs/Point) and theircorresponding information gain(int)

• Target_goal_information : state of thefuture target (geometry_msgs/Pose)

• Ids_of_reached_goals: informationfor acknowledgment(int)

Séminaire SyRI – 26 juin, 2018 30

Introduction Exploration strategy MAV fleet coordination Simulation study ConclusionsMapping process The utility function Path planning and control ROS data exchange

ROS data exchange

ROS frames for two MAVs.

ROS topics for two MAVs.

Topics for data exchange• ID_informtion : Robot ID (int) and its

state (geometry_msgs/PoseStamped)

• Frontiers_to_leader_topic: frontiers(geometry_msgs/Point) and theircorresponding information gain(int)

• Target_goal_information : state of thefuture target (geometry_msgs/Pose)

• Ids_of_reached_goals: informationfor acknowledgment(int)

Séminaire SyRI – 26 juin, 2018 30