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