8/3/2019 Robust Motion Planning PDF
1/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada
Robust Motion Planning for Marine VehiclesMatthew Greytak
Franz Hover
July 8, 2008
8/3/2019 Robust Motion Planning PDF
2/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada2
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
8/3/2019 Robust Motion Planning PDF
3/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada3
Motivations (2) Two solutions to autonomous vehicle navigation
Waypoint navigation: drive between a pre-defined set of waypoints using aline-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 difficult, open-loop plans
We would like to combine the ositive features of both solutions
8/3/2019 Robust Motion Planning PDF
4/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada4
Outline
Problem definition
Motion planning framework
Uncertainty evolution and risk predictions
Experimental results
Conclusions
8/3/2019 Robust Motion Planning PDF
5/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada5
Problem Definition
Marine vehicle trajectory planning is a constrained infinite-dimensional problem
Dynamic constraints: vehicle limitations (underactuated, non-
minimum phase, velocity limits, control limits)
Kinematic constraints: obstacles
Continuous input space: thruster commands Objective function: time, distance, control energy
Simplifications
Change input space from thruster commands to discrete maneuvers
Trajectories are concatenations of maneuvers
Optimization over a continuous space is replaced by a discrete graphsearch
Maneuver Automaton motion planning framework has been usedsuccessfully for autonomous helicopters
8/3/2019 Robust Motion Planning PDF
6/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada6
Maneuvers
Store a library of maneuvers withknown 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
specified amount of time Speed control panel
MP Trim
8/3/2019 Robust Motion Planning PDF
7/26July 8, 2008
Robust Motion Planning for Marine VehiclesISOPE 2008 Conference, Vancouver, Canada
7
Waypoint Path Following
Driving to a waypoint is a type of maneuver (known nominalresult, starts and ends at a speed control setpoint)
Velocity feedback and position feedback: robust to disturbancesand 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
8/3/2019 Robust Motion Planning PDF
8/26July 8, 2008
Robust Motion Planning for Marine VehiclesISOPE 2008 Conference, Vancouver, Canada
8
ccbwaabwebab
Motion Plans
Concatenate maneuvers into motion plans through shared speedcontrol setpoints
Represent motion plans as letter sequences
Vector of duration times: preset for motion primitives, variable for trims
a b cd e f
g h i
w1 w2
8/3/2019 Robust Motion Planning PDF
9/26July 8, 2008
Robust Motion Planning for Marine VehiclesISOPE 2008 Conference, Vancouver, Canada
8
ccbwaabw
Motion Plans
Concatenate maneuvers into motion plans through shared speedcontrol setpoints
Represent motion plans as letter sequences
Vector of duration times: preset for motion primitives, variable for trims
a b cd e f
g h i
ebab
w1 w2
8/3/2019 Robust Motion Planning PDF
10/26July 8, 2008
Robust Motion Planning for Marine VehiclesISOPE 2008 Conference, Vancouver, Canada
8
ccbw
Motion Plans
Concatenate maneuvers into motion plans through shared speedcontrol setpoints
Represent motion plans as letter sequences
Vector of duration times: preset for motion primitives, variable for trims
a b cd e f
g h i
ebab aabw
w1 w2
8/3/2019 Robust Motion Planning PDF
11/26
8/3/2019 Robust Motion Planning PDF
12/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in thediscrete framework
Use the A* search algorithm to find 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
8/3/2019 Robust Motion Planning PDF
13/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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
8/3/2019 Robust Motion Planning PDF
14/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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
8/3/2019 Robust Motion Planning PDF
15/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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)
Eliminate plans thatare guaranteed to beworse than thecurrent feasible planto the goal
High Cost
8/3/2019 Robust Motion Planning PDF
16/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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)
Eliminate plans thatare guaranteed to beworse than thecurrent feasible planto the goal
High Cost
8/3/2019 Robust Motion Planning PDF
17/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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)
Eliminate plans thatare guaranteed to beworse than thecurrent feasible planto the goal
High Cost
8/3/2019 Robust Motion Planning PDF
18/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada9
Searching for Plans
There are infinite possible concatenations of maneuvers, even in the
discrete framework
Use the A* search algorithm to find 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)
8/3/2019 Robust Motion Planning PDF
19/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada10
Uncertainty Evolution
(t) = A(t)(t)+ (t)AT
(t)+W
Trajectories divergeunder open loop control
Trajectories converge
under closed loopcontrol
Trajectory variance ismodeled by the Riccatiequation
Analytic solutions for allmaneuvers
Predictions used toevaluate the risk for anygiven plan
8/3/2019 Robust Motion Planning PDF
20/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada11
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
Add overall collision probability to the costfunction
1 std dev
envelope
8/3/2019 Robust Motion Planning PDF
21/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada
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 obstaclecorners 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
12
Experimental Results
8/3/2019 Robust Motion Planning PDF
22/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada
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 obstaclecorners 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
12
Experimental Results
8/3/2019 Robust Motion Planning PDF
23/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada
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 obstaclecorners 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
12
Experimental Results
8/3/2019 Robust Motion Planning PDF
24/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada13
Conclusions
Motion planning using a discrete set of maneuvers is acomputationally efficient solution for marine vehicle navigation
A* finds the optimal path within the motion planning framework
Robustness to disturbances and modeling error is improved byadding waypoints to the maneuver library
Use an analytic prediction of the trajectory uncertainty toestimate the risk associated with each plan considered by A*
Incorporate the risk prediction into the cost function to generatepaths that are safe and efficient
8/3/2019 Robust Motion Planning PDF
25/26
July 8, 2008Robust Motion Planning for Marine Vehicles
ISOPE 2008 Conference, Vancouver, Canada14
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
8/3/2019 Robust Motion Planning PDF
26/26
July 8 2008Robust Motion Planning for Marine Vehicles
15
Questions?
Top Related