Mission Planning Problem

0 downloads 0 Views 9MB Size Report
IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 2 ... Plan paths between task locations/areas ... Proposed approach for PRM-based path planning:.
Probabilistic Roadmaps And Ant Colony Optimization For UAV Mission Planning F. Adolf, A. Langer, L. Silva, F. Thielecke

Folie 1

6th IFAC Symposium on Intelligent Autonomous Vehicles 2007, Toulouse Vortrag > Autor > Dokumentname > 09.11.2005

The Helicopter Systems ARTIS UAV Family

Magnetometer

Camera

Sonar

Computer Vision

Power Supply

Flight Controller Telemetry

GPS IMU

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 2 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

The Helicopter Systems (cont.) Depending on mission and workload of an operator, a Ground Control Station (GCS) should provide different ways to command the UAV. What the onboard Flight Control computer can „do for us: !  Accept command from GCS !  GCS command types vary from !  Positions e.g. Speech input „ARTIS fly 10 meters north !

 Velocities/Paths

!

e.g. Joystick  Behavior Sequences e.g. Sequence of (complex) commands

(offline) Mission Plans

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 3 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Mission Planning Context !

 ARTIS focusses on Low-Altitude ( Slide 4 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Motivation UAV

Sequence of behavior commands

Generate „Ready-To-Fly Mission Plans on GCS ID 20070510 TO -5 HV 0 0 -3 180 avoidance on WT 10 HV 33.3 3.51 -7 28.9 HV 37.1415 1.63484 -10 90 avoidance off WT 5 object tracker on HV 37.1415 48.688 -10 90 HV 37.1415 48.688 -10 180 HV 31.81 48.688 -10 180 HV 26.4785 1.63484 -10 180 HV 26.4785 1.63484 -10 90 HV 26.4785 48.688 -10 90 HV 26.4785 48.688 -10 180 HV 21.147 48.688 -10 180 object tracker off LD WO

Mobile Ground Control Station IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 5 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Mission Planning Problem Given: !  Environment (a.k.a. World Model) with known obstacles !  Set of waypoints at which some task is performed !  One or more UAVs (agents) that can solve tasks To be solved:

Task Planning

!

 Assign tasks to agents

!

 Schedule tasks

!

 Plan paths between task locations/areas

“I need the costs…”

Path Planning

Highly coupled problem domains: Task Planning

“…and me, the task ordering”

Path Planning IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 6 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Problem Given: !  Initial and a final robot configuration (x, y, z, θ) !  Algorithm requirements: !  Multi-agent support. !  Completeness: Find solutions if one exists. !  Fast computation: Onboard use intended. To be found: !  Collision free path: Set of intermediate configurations to connect initial and final configuration together.

Finding paths in obstacle-constrained, threedimensional environment with optimal (here: shortest) length has been proven to be NP-hard [Canny, et al., 1987]

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 7 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Problem Given a mission description: !   Mission !   Set of tasks to be accomplished (by a team). !   Task dependence. !   Task priority. !   Task !   Task is defined by set of waypoints. !   Variety of objectives (e.g. search specified area, take pictures at specific point) !   Waypoint / Polygonal Area !   Distinct objective for one agent. !   Specific area for which waypoints have to be planned. !   Agent !   UAV with state vector (e.g. position, task status) !   Each UAV’s capabilities (e.g. “UAV1 has a camera”)

Find (close-to-)optimal task assignment: 1. Agents Cooperativeness 2. Sharing tasks allowed 3. Tasks tight restrictions IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 8 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Mission Planning Approach 1.  User sets up tasks (single waypoints and/or waypoint areas). 2.  Path Planner produces an overall path cost matrix. 3.  Task Planner orders tasks (near) optimal. 4.  Path Planner determines path (for each agent). 5.  Mission Planner generates sequence of behavior commands.

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 9 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Approach PRM-based Path Planning

Use Probabilistic Roadmap (PRM) [Petersson 2006] !  Trade off: Optimality vs. Short runtimes !  Probabilistically complete [Hsu, 1999] !  Sufficient sampling iterations: Finding a solution tends to 1.0 Proposed approach for PRM-based path planning: !  Pre-processing: Probabilistic Roadmap !  Graph search (Dijkstra, A*) !  Post-processing: Smoothing steps

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 10 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Approach 1: Build PRM

Probabilistic Roadmap (PRM), [Petersson 2006] !  Build search graph for random 3D points which reflects the connectivity of search space:

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 11 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Approach 2: Graph Search

3D World Model of Hannover Airport

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 12 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Approach 3: Post-processing

Addressing a known issue: !  Planning within narrow passages Example Scenario -  Area: 800 m x 1400 m -  Blocks create corridors of 20 m width -  Safety distance: 5 m

More samples are not enough! IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 13 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Approach 3: Post-processing

Addresses known issues !  Random samples may fail to find points within narrow passages, limiting the algorithm´s performance in constrained environments Solution: !  Perform „bridge tests to generate samples within narrow areas:

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 14 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Path Planning Performance ! ! ! !

 PRM average building runtine (1000 samples): 1.8s  Max. distance of ~100m between sample points  Graph search average runtime: 48 ms  Smoothing runtime: 9 ms Planned path (orange) and smoothed one (red):

Underlying PRM:

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 15 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach Overview

Tasks

Group of UAVs (Agents) Agent 1 Agent 2

Search Task 1

Multi-Agent Mission

Mosaiking 1

Agent 3 Agent 4

Search Task 2

Input

Live Video 1

Task Refinement (Prim’s Algorithm)

Eliminate potential assignment conflicts

Task Assignment

Solve task ordering problem

(Ant Algorithm)

Task Sharing Agent 1

T11

T12

T1k

Agent 2

T21

T22

T2l

Agent n

Tn1

Tn2

Tnp

Planner Output

Mission Schedule (Group Plans)

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 16 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach 1a: Task Refinement

!

 Initial Refinement !

 Accessing Capability Matrix

!

 Build set of possible tasks

!

 Ex. Two Agents and Three Tasks Potential Task Assignments

Agent1 -Video Camera -near-IR Vision System

Agent2 -Photo Camera -12 min Flight Endurance

Capability Mapping Tasks Agent

1 2

1

2

3

1 0

0 1

1 1

Agents 1 2

Possible Tasks 1 and 3 2 and 3 Conflict!

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 17 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach 1b: Conflict Solving

!

 Using Spanning Tree Algorithm [Prim] Chosen D

Visited {D, A} 1

Visited {D, A, F, B, E}

Visited {D, A, F}

5

3

2

Visited {D, A, F, B, E, C} 6

Visited {D, A, F, B} 4

Resulting Graph

Visited All 7

8

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 18 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach 1b: Conflict Solving

!

 Assign task through obtained graph

!

 Example 2 Agents 7 Tasks !

  Agent 1: Tasks 1, 2, 3, 6

!

  Agent 2: Tasks 3, 4, 5, 6, 7

Spanning trees for each Agent

Initial Graph Task 3 Task 1

Task 3

Task 4 Task 1

Task 6

Agent 1

Conflicting Tasks

Agent 2

Task 4

Task 6 Task 6 Task 5

Task 2

Task 5

Task 2

Task 3

Agent 1

Task 7

Agent 2

Task 7

Task 6 Cost Agent 1: Edge(A1,T2)+Edge(T2,T6) Cost Agent 2: Edge(A2,T6) IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 19 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach 2: Task Ordering per Agent

Ant-Algorithm to solve (global) optimization problems !  Shortest paths exploration !  Travelling Salesman Problem !  Sequential Ordering Problem !  …

Ex: Shortest Path Exploration (Courtesy of Ralph Weires) IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 20 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Approach 2: Task Ordering per Agent

Graph

Algorithm

Based on behavior of real ants: Depositing and sensing pheromone trails

STEP 1: Initialization For all Edges: τ ij

= τ max

STEP 2: Walk Method

Stochastic Method + Fastness + Near-optimal solutions - No-optimal solution guaranteed

For each Artificial Ant: -Choose the vertices -Return the cost of the path -Get the best ant for updating

vj

vi

Pheromone

STEP 3: Update

All edges: τ ij = (1 − ρ )τ ij For all Edges in the best tour:

Δτ = 1 / f ( Sbest ) ⇒ τ ij = τ ij + Δτ ,τ min ≤ τ ij ≤ τ max

Parameters

Key tuning parameter

α: β: ρ:

Exp. for Heuristic Factor Exp. for Pheromone Factor Evaporation Rate

pbest : Prob. Best Solution n:

avg =

τ max

n 2 1

Δ

HF (v i , v j ) =

1 = ρ f ( S opt )

Δ

τ max (1 − pbest )

Heuristic Factor Pheromone Factor

PF (v i , v j ) =τ < vi ,v j >

n

Number of Vertices opt Best tour found so far S :

τ min =

τ:

m = min{n,10}

Pheromone Factor

1 cos t (v i , v j )

(avg − 1) n pbest

p(next = v j ) =

HF (v i , v j )α × PF (v i , v j ) β

∑ HF (v , v i

k

)α × PF (v i , vk ) β

vk ∈UV

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 21 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Planning Performance Uses “best” parameters known for Sequential Ordering Problems:

Example scenario with 30 tasks

pbest = 0.05, β = 2, α = 1 n = nº Tasks , m = min{10, n}

Five UAV’s: Estimated mission execution time ~40% Three UAV’s: Estimated mission execution time ~60% One UAV: Estimated Time ~= 100%

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 22 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Mission Planner

Implementation for GCS 3D World Model of DLR Braunschweig

Pirouette around an object

Object Search/ Mosaiking Area

Start

Landing

Spline-based fast flight segment

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 23 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Mission Planner

Implementation for GCS (cont.) Non-optimized mission Optimized mission planplan

3D World Model of DLR Braunschweig

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 24 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Summary Feasible approach for UAV mission planning (low-alt. domain) Two-tier planning approach: !  Fast, near-optimal 3D path planner !  Multi-agent capable task planner w/ heterogeneous task Mission Planner has been implemented for ARTIS ground contorl station

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 25 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Future Work The following limits pose yet another challenge !  Spline-based fast flight implemented but not yet considered by the path planner explicitly. !

 Research is underway to enhance PRM for online path re-planning

!

(e.g. disappearing obstacles).  Evaluate applicability of an onboard distributed task planner as opposed to this centralized task planning approach.

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 26 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Thank You!

Questions?

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 27 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

APPENDIX

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 28 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Addressing known issues

!

 Planning within narrow passages

Example Scenario -  Area: 800 m x 1400 m -  Blocks create corridors of 20 m width -  Safety distance: 5 m

… more sampling only is not enough!

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 29 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Approach Description !

 Benchmark tests: path optimality Preprocessing time

8 7 6 5 4 3 2 1 0

70 60 50 Time (ms)

Time (s)

Path planning time

40 30 20 10 0

500

1000

1500

2000

500

Number of PRM points

Path length

20

Length (m)

Time (ms)

15 10 5 0 1000

1500

1500

Number of PRM points

2000

Number of PRM points

Smoothing time

500

1000

2000

Non-optimized path Optimized path

1350 1300 1250 1200 1150 1100 1050 1000 950 900 500

1000

1500

2000

Number of PRM points

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 30 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Addressing known issues

!

 Towards online path planning !

 Given a previously calculated PRM, a single path planning query is

done fast enough for needed path recalculations !

 But what if the environment changes?

!

 New obstacle ! PRM pruning

!

 New PRM !path replanning

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 31 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Task Sharing - Search Pattern

Finish Point

Start Point

Method 1

Finish Point

Method 2

Start Point

Clearly, Method 1 is more efficient than Method 2 !! Number of points planned are always even!!

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 32 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Overall Performance !

  Assess Assignment approaches: 1.  Modified Prim’s Algorithm !

Scenario

 Load Balancing Idea

2.  Spanning Tree Algorithm 3.  Nearest Neighbour Heuristic !

!

  Analyze: !

 Total execution time (J1)

!

 Total flight length (J2)

!

 Computation time to plan (time) !  Machine: Pentium, 2.4 GHz, 512 MB RAM

  Benchmark Scenarios: !

  A1: 3 Agents 20 tasks. No-sharing

!

  A2: 3 Agents 20 tasks. Sharing

!

  A3: 5 Agents 25 tasks. Sharing

Task Planner Simulation environment

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 33 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Overall Performance - Results

Mission Time [s]

J1

Comparative

1000 900 800 700 600 500 400 300 200 100 0

Method 1 Method 2 Method 3

A1

A2

A3

Mission

J2

Methods

J1

J2

time

1 x (2 and 3)

+4%

+8%

-8%

2 x (1 and 3)

-10%

0%

+10%

3 x (1 and 2)

+14%

-8%

-2%

Mission Length [m]

3000 2500

Method 1

2000

Method 2

1500

Method 3

1000 500 0 A1

A2

A3

Mission

Time [s]

Algorithm Performance

1.80 1.60 1.40 1.20 1.00 0.80 0.60 0.40 0.20 0.00

Method1 Method2 Method3

A1

A2

Method 2 outperforms others methods in cost. However it takes ~10% more time to give the final assignment.

A3

Missions

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 34 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Ant Algorithm - Performance “Best” Parameters known for Sequential Ordering Problems:

pbest = 0.05, β = 2, α = 1 n = nº Tasks , m = min{10, n}

Convergence Analysis: “Seed”: Random Tour

“Seed”: Heuristic Tour

~0.91

~0.81

ρ

Small : ~55% better solutions

~150 iterations ~20 iterations

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 35 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007

Conflict Solver - Heuristic Solution !

 Example 2 Agents 5 Tasks !

  Agent 1: Tasks 1, 2, 3

!

  Agent 2: Tasks 2, 3, 4, 5

Task 5

Task 3

Task 1

Task 2

8

13

Task 1

12

5

5

15

Task 1

13

13 12

12 Task 2

7 Agent 2

Agent 2

Agent 1

Task 4

Agent 2

Agent 1

Task 4 Task 5

Task 3

Task 3

Task 4

3

Task 3 Task 5

15 13 Task 2

Agent 1

6

15

6

Task 5

Task 3

Task 2

Agent 1

Task 1

Task 5

Task 3

15

13

Task 4

Task 4

Task 4

Task 1

Task 1

Task 2

Agent 2

Task 5

Agent 1

Task 2

Agent 2

Agent 1

Agent 2

IAV 2007, Session 4B “Aerial robotics!, Decision” > Slide 36 Florian-M. Adolf > Systems Automation > Institute of Flight Systems > German Aerospace Center (DLR) > 4th Sep.2007