Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie...
Transcript of Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie...
![Page 1: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/1.jpg)
Belief space planning assuming maximum likelihood observations
Robert Platt
Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez
Computer Science and Artificial Intelligence Laboratory,Massachusetts Institute of Technology
June 30, 2010
![Page 2: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/2.jpg)
Planning from a manipulation perspective
(image from www.programmingvision.com, Rosen Diankov )
• The “system” being controlled includes both the robot and the objects being manipulated.
• Motion plans are useless if environment is misperceived.
• Perception can be improved by interacting with environment: move head, push objects, feel objects, etc…
![Page 3: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/3.jpg)
The general problem: planning under uncertainty
Planning and control with:
1. Imperfect state information2. Continuous states, actions, and
observations
most robotics problems
N. Roy, et al.
![Page 4: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/4.jpg)
Strategy: plan in belief space
1. Redefine problem:
“Belief” state space
2. Convert underlying dynamics into belief space dynamics
start
goal
3. Create plan
(underlying state space) (belief space)
![Page 5: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/5.jpg)
Related work
1. Prentice, Roy, The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance, IJRR 2009
2. Porta, Vlassis, Spaan, Poupart, Point-based value iteration for continuous POMDPs, JMLR 2006
3. Miller, Harris, Chong, Coordinated guidance of autonomous UAVs via nominal belief-state optimization, ACC 2009
4. Van den Berg, Abeel, Goldberg, LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information, RSS 2010
![Page 6: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/6.jpg)
Simple example: Light-dark domain
11 ttt uxxUnderlying system:
ttt xwxz Observations:
underlying state
action
observation
observation noise
start
goal
25,0;~ xxNxw tt
State dependent noise:“dark” “light”
![Page 7: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/7.jpg)
Simple example: Light-dark domain
start
goal
11 ttt uxxUnderlying system:
ttt xwxz Observations:
underlying state
action
observation
observation noise
“dark” “light”
25,0;~ xxNxw tt
State dependent noise:
Nominal information gathering plan
![Page 8: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/8.jpg)
Belief system
ttt uxfx ,1
ttt xwxgz
Underlying system:
Belief system:• Approximate belief state as a Gaussian
ttt mb ,
(deterministic process dynamics)
state
(stochastic observation dynamics)
action
observation
ttt mxNbxP ,;|
![Page 9: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/9.jpg)
Similarity to an underactuated mechanical system
x
Acrobot
m
b
Gaussian belief:
State space:
Underactuated dynamics: uf ,, ???
Planning objective:
0
gx
0g
g
xb
![Page 10: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/10.jpg)
Belief space dynamics
start
goal
tttttt muzFm ,,,, 11Generalized Kalman filter:
![Page 11: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/11.jpg)
Belief space dynamics are stochastic
unexpected observation
BUT – we don’t know observations at planning time
start
goal
Generalized Kalman filter: tttttt muzFm ,,,, 11
![Page 12: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/12.jpg)
Plan for the expected observation
Plan for the expected observation:
Generalized Kalman filter:
Model observation stochasticity as Gaussian noise
tttttt muzFm ,,,, 11
nmuzFm tttttt ,,,ˆ, 11
We will use feedback and replanning to handle departures from expected observation….
![Page 13: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/13.jpg)
Belief space planning problem
T
tt
Tt
k
iiT
TiT RuunnubJ
11:11,Minimize:
Minimize covariance at final state
• Minimize state uncertainty along the directions.in
Find finite horizon path, , starting at that minimizes cost function:
Action cost• Find least effort path
Subject to:
Trajectory must reach this final state
goalT xm
Tu :1 1b
![Page 14: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/14.jpg)
Existing planning and control methods apply
Now we can apply:• Motion planning w/ differential constraints (RRT, …)• Policy optimization• LQR• LQR-Trees
![Page 15: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/15.jpg)
Planning method: direct transcription to SQP
1. Parameterize trajectory by via points:
2. Shift via points until a local minimum is reached:• Enforce dynamic constraints during
shifting
3. Accomplished by transcribing the control problem into a Sequential Quadratic Programming (SQP) problem.• Only guaranteed to find locally optimal solutions
![Page 16: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/16.jpg)
Example: light-dark problem
• In this case, covariance is constrained to remain isotropic
X
Y
![Page 17: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/17.jpg)
Replanning
goal
• Replan when deviation from trajectory exceeds a threshold:
r
2rmmmm T
m
m
New trajectory
Original trajectory
![Page 18: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/18.jpg)
Replanning: light-dark problem
Planned trajectory
Actual trajectory
![Page 19: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/19.jpg)
Replanning: light-dark problem
![Page 20: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/20.jpg)
Replanning: light-dark problem
![Page 21: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/21.jpg)
Replanning: light-dark problem
![Page 22: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/22.jpg)
Replanning: light-dark problem
![Page 23: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/23.jpg)
Replanning: light-dark problem
![Page 24: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/24.jpg)
Replanning: light-dark problem
![Page 25: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/25.jpg)
Replanning: light-dark problem
![Page 26: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/26.jpg)
Replanning: light-dark problem
![Page 27: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/27.jpg)
Replanning: light-dark problem
![Page 28: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/28.jpg)
Replanning: light-dark problem
![Page 29: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/29.jpg)
Replanning: light-dark problem
![Page 30: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/30.jpg)
Replanning: light-dark problem
![Page 31: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/31.jpg)
Replanning: light-dark problem
![Page 32: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/32.jpg)
Replanning: light-dark problem
Originally planned path
Path actually followed by system
![Page 33: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/33.jpg)
Planning vs. Control in Belief Space
A plan A control policy
Given our specification, we can also apply control methods:
• Control methods find a policy – don’t need to replan
• A policy can stabilize a stochastic system
![Page 34: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/34.jpg)
Control in belief space: B-LQR
In general, finding an optimal policy for a nonlinear system is hard.
• Linear quadratic regulation (LQR) is one way to find an approximate policy
• LQR is optimal only for linear systems w/ Gaussian noise.
Belief space LQR (B-LQR) for light-dark domain:
![Page 35: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/35.jpg)
Combination of planning and control
Algorithm:
1. repeat
2.
3. for
4.
5. if then break
6. if belief mean at goal
7. halt
1:1:1 _, bplancreatebu TT
Tt :1
tttt bubcontrollqru ,,_
0 tt bb
![Page 36: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/36.jpg)
Conditions:
1. Zero process noise.2. Underlying system passively critically stable3. Non-zero measurement noise.4. SQP finds a path with length < T to the goal belief region from
anywhere in the reachable belief space.5. Cost function is of correct form (given earlier).
Theorem:
• Eventually (after finite replanning steps) belief state mean reaches goal with low covariance.
Analysis of replanning with B-LQR stabilization
![Page 37: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/37.jpg)
Laser-grasp domain
![Page 38: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/38.jpg)
Laser-grasp: the plan
![Page 39: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/39.jpg)
Laser-grasp: reality
Initially planned path
Actual path
![Page 40: Belief space planning assuming maximum likelihood observations Robert Platt Russ Tedrake, Leslie Kaelbling, Tomas Lozano-Perez Computer Science and Artificial.](https://reader036.fdocuments.us/reader036/viewer/2022081519/56649e4a5503460f94b3d6cb/html5/thumbnails/40.jpg)
Conclusions
1. Planning for partially observable problems is one of the keys to robustness.
2. Our work is one of the few methods for partially observable planning in continuous state/action/observation spaces.
3. We view the problem as an underactuated planning problem in belief space.