View
227
Download
5
Embed Size (px)
Citation preview
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]