CS 326A: Motion Planning Kynodynamic Planning + Dealing with Moving Obstacles + Dealing with...

CS 326A: Motion Planning

Kynodynamic Planning+ Dealing with Moving

Obstacles+ Dealing with Uncertainty+ Dealing with Real-Time

Issues

2

Kinodynamic Planning Problem

Plan a robot’s trajectory that satisfies a dynamic equation of motion of the

form:

s’ = f(s,u)where:– s is robot’s state configuration x velocity at time t– u is control input at time t

avoids collision with moving obstacles

Outcome: (Piecewise-constant) control function u(t)

3

ProblemPlan a robot’s trajectory that satisfies a dynamic motion constraint of the

form:

s’ = f(s,u)where:– s is robot’s state configuration x velocity at time t– u is control input at time t

avoids collision with moving obstacles

Outcome: (Piecewise-constant) control function u(t)

Note similarity with non-holonomic

constraintq’ = f(q,u)

Nonholonomic vs. Dynamic Constraints

Nonholonomic constraint: f(q,q’) = 0

Dynamic constraints: f(q,q’,q’’) = 0

s = (q,q’) g(s,s’) = 0

Kinodynamic planning is done in state (configuration x velocity) space rather than in configuration space

The dimensionality of state space is twice that of configuration space

5

Similarity of Method Construction of a tree,

by integrating equation of motion with selected control inputs

However, planning occurs in state space, instead of configuration space

Hence, PRM-like planner rather than deterministic one, to deal with greater dimensionality of the space

6

air bearing

gas tank

air thrusters

Robot created to study issues in robot control and planning in no-gravityspace environment

Example: Space Robot

7

Navigation Among Moving Obstacles

8

Model, Sensing, and Control

The robot and the obstacles are represented as disks moving in the plane

The position and velocity of each disc are measured by an overhead camera every 1/30 sec

xx

yrobot

obstacles

9

Model, Sensing, and Control

The robot and the obstacles are represented as disks moving in the plane

The position and velocity of each disc are measured by an overhead camera within 1/30 sec

The robot controls the magnitude f and the orientation of the total pushing force exerted by the thrusters

xx

y

f

robot

obstacles

M

q=(x,y)

s=(q,q')

u=(f , )

1x"= f cos

m1

y"= f sinm

ff

x

y

t

Moving Obstacles in ConfigurationxTime

Space

straight line segments

Traditional PRM

12

s

g

Kinodynamic Planning with PRM in State x Time Space

The roadmap is a tree oriented along the time axis (because of the moving obstacles)

Bi-directional search?

t=1

t=2

t=3

t=2

t=1

t=4

t=2

t=3

t=4

t=5

t=3

13

s

g

PRM in State x Time Space

The roadmap is a tree oriented along the time axis

endgame region

Compare!

endgame region

Sampling Strategy

1

1

1

2 1

1

1

1

2

Goal Region

Bi-Directional Search: Forward & Backward

Integration

16

Example Run

t

x

y

Obstacle map to cylinders in configurationtime space

17

Other Examples

18

But executing this trajectory is likely to fail ...

1) The measured velocities of the obstacles are inaccurate 2) Tiny particles of dust on the table affect trajectories and

contribute further to deviation

Obstacles are likely to deviate from their expected trajectories

3) Planning takes time, and during this time, obstacles keep moving

The computed robot trajectory is not properly synchronized with those of the obstacles

The robot may hit an obstacle before reaching its goal

[Robot control is not perfect but “good” enough for the task]

19

But executing this trajectory is likely to fail ...

1) The measured velocities of the obstacles are inaccurate 2) Tiny particles of dust on the table affect trajectories and

contribute further to deviation

Obstacles are likely to deviate from their expected trajectories

3) Planning takes time, and during this time, obstacles are moving

The computed robot trajectory is not properly synchronized with those of the obstacles

The robot may hit an obstacle before reaching its goal

[Robot control is not perfect but “good” enough for the task]

Planning must take both uncertainty in world state and time constraints into account

20

Dealing with Uncertainty

The robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each time

For example, this set can be a disc whose radius grows linearly with time

t = 0 t = T t = 2T

Initial set ofpossible positions

Set of possible positions at time 2TSet of possible

positions at time T

21

Dealing with Uncertainty

The robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each time

For example, this set can be a disc whose radius grows linearly with time

t = 0 t = T t = 2T

The robot must plan to be outside this disc at time t = T

22

Dealing with Uncertainty

The robot can handle uncertainty in an obstacle position by representing the set of all positions of the obstacle that the robot think possible at each time (belief state)

For example, this set can be a disc whose radius grows linearly with time

The forbidden regions in configurationtime space are cones, instead of cylinders

The trajectory planning method remains essentially unchanged

23

Dealing with Planning Time

Let t=0 the time when planning starts. A time limit is given to the planner

The planner computes the states that will be possible at t and use them as the possible initial states

It returns a trajectory at some t , whose execution will start at t

Since the PRM planner isn’t absolutely guaranteed to find a solution within , it computes two trajectories using the same roadmap: one to the goal, the other to any position where the robot will be safe for at least an additional . Since there are usually many such positions, the second trajectory is at least one order of magnitude faster to compute

Safe Region

endgame region

Planning an Escape Trajectory

25

Are we done?

Not quite ! The uncertainty model may be itself incorrect, e.g.:

• There may be more dust on the table than anticipated

• Some obstacles have the ability to change trajectories

But if we are too careful, we will end up with forbidden regions so big that no solution trajectory will exist any more

So, it might be better to take some “risk”The robot must monitor the execution of theplanned trajectory and be prepared to re-plan a new trajectory

26

Are we done?

The robot must monitor the execution of theplanned trajectory and be prepared to re-plan a new trajectory

Execution monitoring consists of using the camera (at 30Hz) to verify that all obstacles are at positions allowed by the robot’s uncertainty model

If an obstacle has an unexpectedposition, the planner is called backto compute a new trajectory.

Note tradeoff between uncertaintymodel and number of re-plans

27

Experimental Run with Re-Planning

11 22 33

44 55 66

77

28

11 22 33

44 55 66

77

Experimental Run with Re-Planning

29

Another Experimental Run

Total duration : 40 sec

X

30

Another Experimental Run

31

Example in Simulation

8 replanning operations

32

Is this guaranteed to work?

Of course not : Thrusters might get clogged The robot may run out of air or

battery The granite table may suddenly break

into pieces Etc ...

[Unbounded uncertainty]