Motion planning under uncertainty has received considerable attention in the past decade because uncertainty is an important concern when dealing with real-world robotic systems and environments. The uncertainty typically originates from noisy actuation, noisy and partial sensor measurements, and uncertainty about the environment. Sampling-based planners have been proposed to explicitly account for motion uncertainty (Kewlani et al., 2009; Alterovitz et al., 2007). Another class of planners has explicitly focused on uncertainty in the environment and obstacles contained therein (Burns and Brock, 2007; Guibas et al., 2008). Motion planning under both motion and sensing uncertainty is actually an instance of a partially observable Markov decision process (POMDP) (Roy et al., 1999), which suffer from the curse of dimensionality. Efficient point-based value iteration methods have
but these approaches are still computationally expensive and cannot be used for steerable needle planning and control because of the discretization of the action and observation space.
A large body of work has appeared recently that assumes Gaussian models for motion and sensing uncertainty. These approaches use a combination of a Kalman filter (Simon, 2006) for state estimation and standard feedback control techniques (Bertsekas, 2007) to compensate for uncertainty. A few approaches explicitly incorporate uncertainty in sampling-based motion planning to compute high quality motion plans in terms of safety (Prentice and Roy, 2009; van den Berg et al., 2011; Bry and Roy, 2011). (Platt et al., 2010; van den Berg et al., 2012) use iterative trajectory optimization methods to efficiently solve for a locally optimal plan and an associated control policy. There have been attempts to treat deformations as a sort of uncertain perturbation and compensating for deformations using feedback controllers (van den Berg et al., 2010), but these methods do not explicitly account for large deformations and displacement of the obstacles and the target configuration. To our best knowledge, there is no prior work that offers a principled solution for the motion planning under uncertainty problem in deformable environments with obstacles.
In the presence of uncertainty in planning and execution, it is important to compute motion plans that optimize a user-specified objective to evaluate the quality of the motion plans. Prentice and Roy (Prentice and Roy, 2009) compute plans that minimize the maximum uncertainty along the motion plan. Van den Berg et al. (van den Berg et al., 2011) propose metrics to evaluate motion plans based on the probability of collision and uncertainty at the target configuration. (Bry and Roy, 2011; Vitus and Tomlin, 2011) also aim to minimize the probability of collision of the robot during runtime execution. All these approaches characterize uncertainty in terms of a priori Gaussian distributions of the robot state, but assume for the sake of simplicity that the distributions along the plan are independent. This is an incorrect assumption because it does not account for possible collisions with obstacles during plan execution (Greytak, 2009). It is important to be able to accurately compute these metrics without relying on computationally expensive Monte-Carlo simulations to obtain a good estimate (Lambert et al., 2008; du Toit and Burdick, 2011).
4.3
Objective
We consider a steerable needle being inserted into deformable tissue. We assume that time is discretized into stages of equal duration and a controlutfrom its control input spaceU =Rnu is
executed at each time stept. Our objective, formalized below, is to compute a motion plan and associated feedback controller to maximize the probability of success.
Our planner requires as input a simulator of the coupled deformable system that models the motion of the needle and deformations in the surrounding tissue (Sec. 1.3). Given a model of anatomical structures such as glands, vessels, and bones in the environment, a set of simulation parameterss, and a control inpututfor the robot, the simulatorgcomputes the expected motions of the robot and deformations in the environment. We define the state space of the deformable system to beY. A deformable system stateyt∈ Y at timetencodes all necessary information to save the state of the simulator and be able to restart it, including deformations, dynamics parameters, friction states, configuration of the needle tip etc. Formally, the simulatorgevolves as:
yt+1=g[yt,ut,s]. (4.1)
The simulatorgacts as a deterministic function, but the deformations and motions of the robot and environment may be highly uncertain. The material properties of deformable objects (e.g. Young’s modulus and Poisson’s ratio (Nealen et al., 2006)), interaction parameters such as friction, and deviation from commanded actuation are all uncertain and can significantly affect the motion of the needle tip and deformations in the environment. Due to this uncertainty, we assumescomes from a known distributionSand we assume that the expected value ofsis0for brevity. Selecting different values ofs∼Sallows us to use the deterministic simulator to consider variations in outcome.
The dimension of the deformable system state spaceY is very high, possibly containing thou- sands of elements for large, complex meshes of deformable objects (Chentanez et al., 2009). Comput- ing optimal control policies directly in this high-dimensional state space would be computationally prohibitive. Furthermore, while the initial model of the environment may be known, it is unlikely that it will be possible to sense and track the entire deformation over time when executing a plan.
a reduced-dimensional spaceX ∈Rnx that only contains the attributes that define the state of the
needle (the position and orientation of the needle tip) and/or points in the deformable environment that are necessary for collision detection.
We assume thatX ⊂ Y. Given a statex?t ∈ X at timetand the applied control inputu?t ∈ U, theexpectedstate at timet+ 1,x?t+1 ∈ X, evolves as:
x?t+1 =f[x?t,u?t,s], (4.2)
where f is based on the simulator g (Eqn. (4.1)). It is important to note that during the actual execution of a given plan, the true (unknown) state xt departs from the expected output of the deterministic simulation because of uncertainty arising from noisy control inputs and uncertainty in the deformation models. The uncertainty in the true statextis modeled by adding a stochastic noise termmt, which is assumed to be zero-mean Gaussian with varianceMt. It is important to note that in contrast to the kinematic models considered in Chapters 2 and 3, the functionfis based on a physically-based simulation and does not assume a base kinematic motion model for the needle tip. During the actual plan execution, we also assume that sensors provide us with partial and noisy information about the state according to a given stochastic observation model:
zt=h[xt,nt], nt∼ N[0, Nt], (4.3)
whereztis the measurement obtained at timetthat relates to the true statextthrough functionh, andntis the measurement noise that we assume is drawn from a zero-mean Gaussian distribution with known varianceNt.
To account for uncertainty during the execution of a plan, we assume that the needle is controlled in a closed-loop fashion using the linear quadratic Gaussian (LQG) feedback controller and state estimator framework (Stengel, 1994). The LQG controller uses a linear quadratic regulator (LQR) control law that operates on the estimate of the robot state and aims to keep the robot close to the nominal plan and uses a Kalman filter for state estimation.
Our objective of planning and control for the needle in deformable environments can now be formally stated as follows:
Model
Linearization
LQG
Controller
Select high
quality plan
Deformable
Simulator
Actual Plan
Execution
Motion
Planner
Inputs
Figure 4.2: Schematic overview of our method. The deformable simulator is used for motion planning, model linearization and for selecting a plan that maximizes the probability of success.
Objective: Given a start statexstart∈ X and a target regionXtarget ⊂ X, generate a motion plan and
associated feedback controller that maximizes the probability of successfully avoiding obstacles and reaching the target region.
Input: Deformable system simulatorg, deformable system model parameterssand their distribution
S, sensor observation model, start statexstart, and target regionXtarget ⊂ X.
Output: A motion plan composed of a series of states and corresponding control inputs, π : (x?0,u?0, . . . ,x?t,u?t, . . . ,x?`),0 ≤ t < ` where`is the number of discrete stages in the plan and
x?t+1 = f[x?t,u?t,0]. Also, x?0 = xstart,x?` ∈ Xtarget, and the associated feedback controller for handling uncertainty arising from simulation errors, noisy sensing, and unpredictable actuation.