1. 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

2. Planning from a manipulation perspective (image from www.programmingvision.com, Rosen Diankov )www.programmingvision.com 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…

3. The general problem: planning under uncertainty Planning and control with: 1.Imperfect state information 2.Continuous states, actions, and observations most robotics problems N. Roy, et al.

4. 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)

5. 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

6. Simple example: Light-dark domain Underlying system: Observations: underlying state action observation observation noise start goal State dependent noise:“dark”“light”

7. Simple example: Light-dark domain start goal Underlying system: Observations: underlying state action observation observation noise “dark”“light”State dependent noise: Nominal information gathering plan

8. Belief system Underlying system: Belief system: Approximate belief state as a Gaussian (deterministic process dynamics) state (stochastic observation dynamics) action observation

9. Similarity to an underactuated mechanical system AcrobotGaussian belief: State space: Underactuated dynamics: ??? Planning objective:

10. Belief space dynamics start goal Generalized Kalman filter:

11. Belief space dynamics are stochastic unexpected observation BUT – we don’t know observations at planning time start goal Generalized Kalman filter:

12. Plan for the expected observation Plan for the expected observation: Generalized Kalman filter: Model observation stochasticity as Gaussian noise We will use feedback and replanning to handle departures from expected observation….

13. Belief space planning problem Minimize: Minimize covariance at final state Minimize state uncertainty along the directions. Find finite horizon path,, starting at that minimizes cost function: Action cost Find least effort path Subject to: Trajectory must reach this final state

14. Existing planning and control methods apply Now we can apply: Motion planning w/ differential constraints (RRT, …) Policy optimization LQR LQR-Trees

15. 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

16. Example: light-dark problem In this case, covariance is constrained to remain isotropic X Y

17. Replanning goal Replan when deviation from trajectory exceeds a threshold: New trajectory Original trajectory

18. Replanning: light-dark problem Planned trajectory Actual trajectory

19. Replanning: light-dark problem

32. Originally planned path Path actually followed by system

33. 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

34. 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:

35. Combination of planning and control Algorithm: 1. repeat 2. 3. for 4. 5. if then break 6. if belief mean at goal 7. halt

36. Conditions: 1.Zero process noise. 2.Underlying system passively critically stable 3.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

37. Laser-grasp domain

38. Laser-grasp: the plan

39. Laser-grasp: reality Initially planned path Actual path

40. 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.