Robust Motion Planning for Marine Vehicles

Matthew Greytak Franz Hover July 8, 2008

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Motivations (1)
Example scenario: autonomous harbor patrols
Small, autonomous vehicles deployed in a harbor for surveillance, chemical sensing, etc. Destinations handed down from a supervisor (human or other) Dock and undock autonomously Traverse large open spaces easily Robust to disturbances and modeling error

Assumptions
Underactuated vessel Vessel position known Obstacle locations known Stable speed controller for surge and yaw

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Motivations (2)
Two solutions to autonomous vehicle navigation
Waypoint navigation: drive between a pre-dened set of waypoints using a line-of-sight path following algorithm, with straight and curved paths

Simple, fast planning, robust, good for open environments Not suitable for constrained environments Motion planning: combine low-level maneuvers to steer around obstacles Agile trajectories, guaranteed feasibility Planning more difcult, open-loop plans

We would like to combine the positive features of both solutions

July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 3

Outline
Problem denition Motion planning framework Uncertainty evolution and risk predictions Experimental results Conclusions

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Problem Denition
Marine vehicle trajectory planning is a constrained innitedimensional problem
Dynamic constraints: vehicle limitations (underactuated, nonminimum phase, velocity limits, control limits) Kinematic constraints: obstacles Continuous input space: thruster commands Objective function: time, distance, control energy

Simplications
Change input space from thruster commands to discrete maneuvers Trajectories are concatenations of maneuvers Optimization over a continuous space is replaced by a discrete graph search

Maneuver Automaton motion planning framework has been used successfully for autonomous helicopters
Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

July 8, 2008

Maneuvers
Store a library of maneuvers with known position and velocity changes Each maneuver starts and ends at a speed control setpoint Maneuver duration and position change are functions of the speed control gains Closed loop velocity, open loop position: susceptible to disturbances and modeling error Initial error propagates through maneuver Motion primitives move between setpoints Trims stay in one setpoint for a specied amount of time
July 8, 2008

MP

Trim

Speed control panel

6

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Waypoint Path Following

Driving to a waypoint is a type of maneuver (known nominal result, starts and ends at a speed control setpoint) Velocity feedback and position feedback: robust to disturbances and modeling error Asymptotic convergence to the path leg Initial error does not propagate through the maneuver At the waypoint, the along-track position is known exactly Span large open areas without long trims or concatenation

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Motion Plans
Concatenate maneuvers into motion plans through shared speed control setpoints Represent motion plans as letter sequences Vector of duration times: preset for motion primitives, variable for trims w1 w2

a d g

b e h

c f i

ebab aabw ccbw

July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 8

Motion Plans
Concatenate maneuvers into motion plans through shared speed control setpoints Represent motion plans as letter sequences Vector of duration times: preset for motion primitives, variable for trims w1 w2

a d g

b e h

c f i

ebab aabw ccbw

July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 8

Motion Plans
Concatenate maneuvers into motion plans through shared speed control setpoints Represent motion plans as letter sequences Vector of duration times: preset for motion primitives, variable for trims w1 w2

a d g

b e h

c f i

ebab aabw ccbw

July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 8

Motion Plans
Concatenate maneuvers into motion plans through shared speed control setpoints Represent motion plans as letter sequences Vector of duration times: preset for motion primitives, variable for trims w1 w2

a d g

b e h

c f i

ebab aabw ccbw

July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 8

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

Eliminate plans that are guaranteed to be worse than the current feasible plan to the goal

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

Eliminate plans that are guaranteed to be worse than the current feasible plan to the goal

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

High Cost

Eliminate plans that are guaranteed to be worse than the current feasible plan to the goal

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Searching for Plans

There are innite possible concatenations of maneuvers, even in the discrete framework Use the A* search algorithm to nd the optimal motion plan
Goal-directed graph search using an estimate of the minimum cost-to-go At the current node, add all compatible collision-free maneuvers For each added maneuver, evaluate the path cost (g) and the estimated cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h End when the expanded node is at the goal (within a tolerance)

A* speed highly dependent on cost-to-go estimate h Optimal plan by what metric?

Minimum time? (g = duration) Time balanced with risk? (g = duration + risk)

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

Uncertainty Evolution
(t) = A(t)(t) + (t)AT (t) + W
Trajectories diverge under open loop control Trajectories converge under closed loop control Trajectory variance is modeled by the Riccati equation Analytic solutions for all maneuvers Predictions used to evaluate the risk for any given plan
July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 10

Planning for Risk

For each plan:
Compute the cross-track position variance Evaluate the probability of hitting the nearest obstacles to the nominal path Contract the position variance with each obstacle passing
1 std dev envelope

Add overall collision probability to the cost function

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

July 8, 2008

11

Experimental Results
Tests performed in the MIT Towing Tank with a 1.25-meter autonomous ship model with a single azimuthing thruster

Mission: pull away from the dock, then drive down the tank while avoiding obstacles Waypoints automatically placed off obstacle corners and at the goal Motion plan: back away from wall, rotate in place, then use waypoints to get to the goal Divergence/convergence of 5 runs is consistent with predicted uncertainty evolution
July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 12

Experimental Results
Tests performed in the MIT Towing Tank with a 1.25-meter autonomous ship model with a single azimuthing thruster

Mission: pull away from the dock, then drive down the tank while avoiding obstacles Waypoints automatically placed off obstacle corners and at the goal Motion plan: back away from wall, rotate in place, then use waypoints to get to the goal Divergence/convergence of 5 runs is consistent with predicted uncertainty evolution
July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 12

Experimental Results
Tests performed in the MIT Towing Tank with a 1.25-meter autonomous ship model with a single azimuthing thruster

Mission: pull away from the dock, then drive down the tank while avoiding obstacles Waypoints automatically placed off obstacle corners and at the goal Motion plan: back away from wall, rotate in place, then use waypoints to get to the goal Divergence/convergence of 5 runs is consistent with predicted uncertainty evolution
July 8, 2008 Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada 12

Conclusions
Motion planning using a discrete set of maneuvers is a computationally efcient solution for marine vehicle navigation A* nds the optimal path within the motion planning framework Robustness to disturbances and modeling error is improved by adding waypoints to the maneuver library Use an analytic prediction of the trajectory uncertainty to estimate the risk associated with each plan considered by A* Incorporate the risk prediction into the cost function to generate paths that are safe and efcient

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

13

Future Work
Incorporate model accuracy into the risk assessment Learn the dynamic model during the task, and plan accordingly Add a real-time replanner to monitor the vehicles progress and provide better solutions as they become available

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

14

Questions?

July 8, 2008

Robust Motion Planning for Marine Vehicles ISOPE 2008 Conference, Vancouver, Canada

15