Robust Motion Planning PDF

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


2/26


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


3/26



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


4/26



Outline

Problem definition

Motion planning framework

Uncertainty evolution and risk predictions

Experimental results

Conclusions


5/26



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


6/26



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


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/26July 8, 2008


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


9/26July 8, 2008


8

ccbwaabw

Motion Plans




a b cd e f

g h i

ebab

w1 w2


10/26July 8, 2008


8

ccbw

Motion Plans




a b cd e f

g h i

ebab aabw

w1 w2


11/26


12/26



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


13/26



Searching for Plans

There are infinite possible concatenations of maneuvers, even in the

discrete framework





cost-to-go (h)



High Cost


14/26



Searching for Plans


discrete framework





cost-to-go (h)



High Cost


15/26



Searching for Plans


discrete framework





cost-to-go (h) Expand the node with the lowest estimated total cost f = g + h


Eliminate plans thatare guaranteed to beworse than thecurrent feasible planto the goal

High Cost


16/26



Searching for Plans


discrete framework








High Cost


17/26



Searching for Plans


discrete framework








High Cost


18/26



Searching for Plans


discrete framework







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)


19/26



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


20/26



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


21/26



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


22/26





azimuthing thruster








12



23/26





azimuthing thruster








12



24/26



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


25/26



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


26/26

July 8 2008Robust Motion Planning for Marine Vehicles

15

Questions?

Robust Motion Planning PDF

Documents