STEAP: simultaneous trajectory estimation and planning
Mustafa Mukadam Jing Dong Frank Dellaert Byron Boots




STEAP



Mukadam et al.


	Mustafa Mukadam 

	mmukadam3@gatech.edu 

	

	Jing Dong 

	jdong@gatech.edu 

	

	Frank Dellaert 

	frank@cc.gatech.edu 

	

	Byron Boots 

	bboots@cc.gatech.edu 

	

	*Equal contribution to this article 

	1Institute for Robotics & Intelligent Machines,
	Georgia Institute of Technology,
	Atlanta, GA, USA


STEAP: simultaneous trajectory estimation and planning
	
	
	

    
	Mustafa Mukadam1*
	Jing Dong1*
	Frank Dellaert1
	Byron Boots1

    
====================================================================






We present a unified probabilistic framework for simultaneous trajectory estimation and planning (STEAP). Estimation and planning problems are usually considered separately, however, within our framework we show that solving them simultaneously can be more accurate and efficient. The key idea is to compute the full continuous-time trajectory from start to goal at each time-step.  While the robot traverses the trajectory, the history portion of the trajectory signifies the solution to the estimation problem, and the future portion of the trajectory signifies a solution to the planning problem. Building on recent probabilistic inference approaches to continuous-time localization and mapping and continuous-time motion planning, we solve the joint problem by iteratively recomputing the maximum a posteriori trajectory conditioned on all available sensor data and cost information. Our approach can contend with high-degree-of-freedom (DOF) trajectory spaces, uncertainty due to limited sensing capabilities, model inaccuracy, the stochastic effect of executing actions, and can find a solution in real-time. We evaluate our framework empirically in both simulation and on a mobile manipulator.







§ INTRODUCTION


Trajectory estimation and planning are both important capabilities for autonomous robot navigation. Trajectory estimation is fundamentally backward-looking: the robot estimates a trajectory of previous states that are consistent with a history of noisy and incomplete sensor data. Conversely, planning is fundamentally forward looking: starting from an estimate of its current state, the robot optimizes a trajectory of future states to minimize a cost function and achieve a feasible solution.

In this work, we provide a unified approach to trajectory estimation and planning. Our key insight is that both these problems are inherently variants of trajectory optimization and can therefore be combined to remove the redundancy present in a traditional two step process. The idea is to compute the complete continuous-time trajectory from start to goal at each time-step, such that given the current time-step, the solutions to the estimation problem (history of the trajectory) and the planning problem (future of the trajectory) automatically fall out. Additionally, performing this joint optimization allows information to flow between estimation and planning resulting in mutual benefits. This problem can be quite difficult to solve; the robot must contend with a potentially high-degree-of-freedom (DOF) trajectory space, uncertainty due to limited sensing capabilities, model inaccuracy, and the stochastic effect of executing actions. For the solution to be practical, it must be generated in (faster than) real-time. 



We propose a solution to the problem of simultaneous trajectory estimation and planning (STEAP) by viewing trajectory optimization as probabilistic inference and building on recent approaches to continuous-time localization and mapping <cit.> and continuous-time motion planning <cit.>. We represent the continuous-time trajectory as a function mapping time to robot states and model the trajectory distribution as a Gaussian process (GP) <cit.>. At each time-step, we incrementally (re)estimate the entire continuous-time trajectory, as new sensor data or cost information is encountered, by iteratively recomputing the maximum a posteriori (MAP) trajectory conditioned on all the available sensor data and cost information. In general, sensor data can include various measurements from proprioception (encoders, inertial measurement unit (IMU), etc) or perception for external information (cameras, LIDARs, etc). On the other hand, cost information can include full or partial information know before (start, goal, map of the environment, etc) or constraints and information encountered during execution (changing goal, orientation constraints on the end effector for a portion of the trajectory when for example a cup with liqiud is held, etc).

We formulate the STEAP problem on a single probabilistic graphical model and seek the MAP function with incremental inference <cit.>. This allows us to exploit the underlying sparsity of the problem and avoid re-solving it from scratch as new information is encountered. In our approach the trajectory is only updated where required, dramatically reducing the overall computational burden and enabling a faster-than-real-time solution. We also provide theoretical insight on the connections between our approach and various methods in mapping, estimation, and planning in the context of solving them as inference on graphical models. To better accommodate mobile manipulation problems, we build on recent work by <cit.> on continuous-time trajectory estimation on SE(3) and extend <cit.> to plan trajectories on Lie groups. We implement our framework for solving STEAP and perform several experiments to evaluate our approach in simulation and on the Vector mobile manipulator (Fig. <ref>), and show that our framework is able to incrementally integrate real-world sensor data and directly update its trajectory estimate and motion plan in real-time.
This paper is an extended and revised version of our conference paper <cit.>. In particular,


  * We summarize the incremental inference via Bayes trees approach <cit.> in Section <ref>, specifically in the context of solving the STEAP problem.

  * We provide a detailed explanation on formulating sparse GPs on Lie groups <cit.> in Section <ref>.

  * We present updated experiments on a harder dataset for the planar robot and new experiments with a 18-DOF PR2 robot in simulation.

  * We provide further insight in to our approach by adding a discussion on limitations and future work in Section <ref>.






§ RELATED WORK


By viewing trajectory estimation and motion planning as inference, we are able to borrow and combine tools from different areas of robotics. The Simultaneous Localization and Mapping (SLAM) community has focused on efficient optimization algorithms for many years. One of the more successful approaches is the Smoothing and Mapping (SAM) family of algorithms <cit.> that formulates SLAM as inference on a factor graph <cit.> and exploits the sparsity of the underlying large-scale linear systems to perform inference efficiently. Given new sensor data, incremental Smoothing and Mapping (iSAM) <cit.> exploits the structure of the problem to efficiently update the solution rather than resolving the entire problem from scratch. Recently, <cit.> introduced a continuous-time formulation of the SAM problem, in which the robot trajectory is a function that maps any time to a robot state. The problem of estimating this function along with landmark locations has been dubbed simultaneous trajectory estimation and mapping (STEAM). This approach was further extended in <cit.> to take advantage of the sparse structure inherent in the STEAM problem, in <cit.> to efficiently and incrementally update the solution, and in <cit.> to 4D mapping problems. The resulting algorithms speed up solution time and can be viewed as continuous-time analogs of the original square-root SAM algorithm in <cit.> and the iSAM2 algorithm in <cit.>.

While probabilistic inference is frequently used as a foundation for state estimation and localization, it is only recently that these techniques have been used for planning. The duality between linear estimation and control has long been established <cit.>, but solutions to estimation and control problems have, for the most part, evolved independently within their own subfields. In the last decade this has begun to change. The optimization-inference duality has been shown to extend to planning and optimal control <cit.> with some early work in this direction looking at solving Markov decision processes (MDP) <cit.>. Several researchers have recently proposed a probabilistic inference perspective on planning and control problems, leveraging expectation maximization <cit.>, expectation propagation <cit.>, KL-minimization <cit.>, and efficient inference on factor graphs <cit.>. Interestingly, the incremental inference technique <cit.> used in <cit.> to solve replanning problems is the same as originally used in <cit.> to solve SLAM problems. We exploit this idea to solve our more general class of simultaneous trajectory estimation and planning problems.

Efficient replanning algorithms for navigation are an active area of research <cit.>, but most previous work is difficult to extend to real, high-dimensional systems, is computationally expensive, or does not incorporate uncertainty in the robot's state estimate. Recent work in simultaneous localization and planning (SLAP) attempts to unify robot localization and planning, with early work using HMMs <cit.>, more recent approaches designed for dynamic environments <cit.>, and new approaches <cit.> that combine state estimation and model predictive control (MPC) <cit.>. Unfortunately, these approaches are too computationally expensive due to the MPC style re-evaluation of the new plan, which is compounded with high DOF systems in cluttered environments. In this work, we tackle the simultaneous trajectory estimation and planning (STEAP) problem within a unified probabilistic inference framework. The STEAP problem can be considered as a generalization of the SLAP problem in that the goal of STEAP is to compute the full continuous-time trajectory conditioned on observations and costs in both the past and the future. By contrast, SLAP only computes the current state estimate and the new plan.





§ BACKGROUND: TRAJECTORY OPTIMIZATION AS PROBABILISTIC INFERENCE


Following previous work on both STEAM problems <cit.> and Gaussian process motion planning <cit.>, we view the problem of estimating or optimizing continuous-time trajectories as probabilistic inference. We represent the trajectory as a continuous-valued function mapping time t to robot states θ(t). The goal is to find the maximum a posteriori (MAP) continuous-time trajectory given a prior distribution on the space of trajectories and a likelihood function.



 §.§ Trajectory prior

A prior distribution over trajectories can be defined as a vector-valued Gaussian process θ(t) ∼GP(μ(t), 𝐊(t, t')), where μ(t) is a vector-valued mean function and 𝐊(t,t') is a matrix-valued covariance function. For any collection of times t = { t_0, …, t_N}, θ has a joint Gaussian distribution

    θ≐[ θ_0     θ_N ]^⊤∼N(μ, 𝐊)

with mean vector μ and covariance kernel 𝐊 defined as


    μ≐[ μ(t_0)        μ(t_N) ]^⊤,  𝐊≐  [𝐊(t_i, t_j)]|_ij, 0 ≤ i,j ≤ N.

The prior distribution is then defined by the GP mean μ and covariance 𝐊

    p(θ) ∝exp{ - 1/2∥θ - μ∥^2_𝐊}.

The prior encodes information about the system that is known a priori. For example, in robotic state estimation problems, a structured GP prior may encourage trajectories to follow known system dynamics, e.g. the robot velocity changes smoothly <cit.>. In motion planning, the prior is selected to encourage higher-order derivatives of the system configuration to be minimized <cit.>. The prior we use in our implementation is detailed in Section <ref>.



 §.§ Likelihood function

The likelihood function encodes information about a particular problem instance. For example, in STEAM problems, the likelihood function encourages posterior trajectories to be consistent with proprioceptive or landmark observations <cit.>, while in motion planning problems the likelihood function encourages posterior trajectories to be collision-free <cit.>. 

Let 𝐞 be a collection of random binary events. Examples of events include collision, receiving a sensor measurement, or reaching a goal. The likelihood function is the conditional distribution l(θ ; 𝐞) = p(𝐞 | θ), which specifies the probability of events 𝐞 given a trajectory θ. We define the likelihood as a distribution in the exponential family

    l(θ ; 𝐞) ∝exp{ - 1/2∥h(θ,𝐞) ∥^2_Σ}

where h(θ,𝐞) can be any vector-valued cost function with covariance matrix Σ. The specific likelihood used in our implementation is detailed in Section <ref>.





 §.§ Computing the MAP trajectory

The posterior distribution of the trajectory given the events can be written in terms of the prior and the likelihood using the Bayes rule

    p(θ | 𝐞) ∝ p(θ)  p( 𝐞 | θ).

Then, we can compute the maximum a posteriori (MAP) trajectory

    θ^*
       = _θ{ p(θ | 𝐞) } = _θ{ p(θ)  p( 𝐞 | θ) }
       = _θ{- log( p(θ)
    p( e| θ) ) }
       = _θ{1/2∥θ - μ∥^2_𝐊 + 1/2∥h(θ,𝐞)
    ∥^2_Σ}

where Eq. (<ref>) follows from Eq. (<ref>) and Eq. (<ref>). The MAP estimation problem can therefore be reduced to a nonlinear least squares problem and can be solved with tools like Gauss-Newton or Levenberg-Marquardt.

When solving estimation and planning simultaneously, we encounter new measurements and/or cost information during online execution, thus changing the likelihood. A naïve approach to contending with this new information would be to resolve the problem in Eq. (<ref>) repeatedly. However, this is very inefficient and computationally expensive for an online setting. In the following sections, we formulate the inference problem on graphical models that allow us to exploit the underlying sparsity of the problem (Section <ref>-<ref>), and then we use incremental inference techniques that allow us to iteratively update the solution only where needed resulting in a computationally efficient approach (Section  <ref>).





§ MAPPING, ESTIMATION, AND PLANNING WITH FACTOR GRAPHS


The MAP trajectory computation in Section <ref> can be executed efficiently by exploiting known structure in the problem. This is accomplished by representing the posterior distribution as a factor graph. With a factor graph <cit.> any distribution can be factored into a product of functions that is organized as a bipartite graph G = {Θ, F, E}. This graph consists of variable nodes Θ≐{θ_0, … , θ_N}, factor nodes F≐{ f_0, … , f_M }, and edges E that connect the two types of nodes.

In our case, the variables are a set of instantaneous robot states along the trajectory, and the factors are conditional probability distributions on variable subsets Θ_i of Θ. Therefore, we can write the posterior distribution as a product of the factors

    p(θ | 𝐞) ∝∏_i=0^M f_i (Θ_i).

The precision matrix of this distribution also encodes the connectivity in the graph. Consequently, a sparse factor graph structure yields a sparse precision matrix, which can be exploited to make the computation in Eq. (<ref>) efficient <cit.>.

We can further write the posterior distribution as a product of prior factors and likelihood factors,

    p(θ | 𝐞) ∝ p(θ)  p( 𝐞 | θ) ∝ f^prior(Θ) f^like(Θ).

In the remainder of this section, we will use this general formulation to illustrate relationships between prior work in mapping, estimation, and planning. Then, we will extend this idea and connect it with our proposed work in the next section.



  
SAM: We begin with the smoothing and mapping (SAM) <cit.> problem, an early work that uses factor graphs to address the state estimation and mapping problem in robotics. The goal is to estimate the full posterior trajectory in the past given all measurements. The factor graph used in SAM is 


    p(θ_est | 𝐞) ∝ f^prior f^meas,


where θ_est signifies the history portion of the trajectory to be estimated, f^prior = f^prior(θ_0) is the prior on the first state, and f^meas is the likelihood of all sensor measurements, which itself factors as


    f^meas = ∏_i f^meas_i(Θ_i).


Unary measurement factors can refer to odometer, GPS or IMU measurements, while higher order factors on a subset of states (Θ_i) often represent landmark observations.



  
STEAM: Like SAM, simultaneous trajectory estimation and mapping (STEAM) <cit.> addresses trajectory estimation problems. The key difference is that in STEAM, the trajectory is no longer treated as a discrete sequence of states Θ, but rather a continuous-time trajectory sampled from a GP. The prior is a joint distribution on the full trajectory f^prior = f^gp(Θ), yielding a factor graph


    p(θ_est | 𝐞) ∝ f^gp f^meas.




  
GPMP2: GPMP2 <cit.> is a probabilistic inference framework for solving planning problems. It utilizes the GP trajectory representation from STEAM to find collision free future trajectories that satisfy the GP prior. Unlike SAM and STEAM, however, the likelihood is not based on sensor measurements, but rather the likelihood of a trajectory being free from collision with obstacles. The collision factor is defined as


    f^obs = ∏_i f^obs_i(θ_i).


A fixed start and goal state (this can also be an end effector goal in workspace) is also required in planning problem, and therefore can be incorporated in to the likelihood. So factors to fix start and goal configurations are also employed


    f^fix = f^start(θ_0) f^goal(θ_N).


The full factor graph of GPMP2 is, therefore


    p(θ_plan | 𝐞) ∝ f^gp f^obs f^fix

where θ_plan signifies the future portion of the trajectory to be planned.



  
SLAP: In real robotics applications, it is frequently the case that both estimation and planning problems must be solved. One  approach to tackling this problem is SLAP <cit.>. Although previous work in this area does not employ factor graphs, we reformulate SLAP using them here to illustrate its relation to other problems. SLAP can be viewed as splitting the inference problem into two factor graphs, an estimation graph and a planning graph, defined by


    p(θ_est | 𝐞)    ∝ f^prior f^meas, 
    
    p(θ_plan | 𝐞)    ∝ f^prior f^curr f^obs f^goal.


If a continuous-time trajectory representation like GPs are employed, we can replace f^prior with f^gp. SLAP solves the estimation graph first to find an estimate of the current state f^curr (θ_curr) and then uses it to initialize and solve the planning problem.



  

We summarize the factorization of these various problems in Table. <ref>, with their factor graphs shown in Fig. <ref>.








§ SIMULTANEOUS TRAJECTORY ESTIMATION AND PLANNING 


In this paper, we present simultaneous trajectory estimation and planning (STEAP), where the task is to perform inference on the entire factor graph from start to goal at once, in contrast to SLAP, which would solve the estimation and planning graph sequentially. We optimize the full trajectory θ = θ_est∪θ_plan represented by the GP prior in Section <ref> given all sensor data and cost information collected in to a single likelihood. Compared to prior work discussed in the previous section, the likelihood here is interpreted more broadly to represent events than happen in the past and in the future, all together. The STEAP factor graph is defined as,


    p(θ | 𝐞) ∝ f^gp f^meas f^obs f^fix.


We define these factors in Section <ref>. Given the current time-step, the solutions to the estimation and planning problems automatically fall out. During online execution as new measurement or cost information is encountered, the likelihood, and by extension the factor graph, can be updated appropriately. Performing inference on the new graph then provides the updated estimation and replanning solutions. We explain this procedure with a simple toy example in Section <ref>. STEAP has the following major advantages:

(i) Optimization of a single graph allows information to flow between the two sub-graphs of estimation and planning, which is not possible with SLAP. This increases performance in both estimation and planning, and provides mutual benefit. The collision-free likelihood of both the past and the future part of the graph encourages the estimated past trajectory to remain in areas without obstacles, since a successfully traversed trajectory would not have passed through obstacles. This helps contend with noisy (or drops in) raw measurements and reduces trajectory estimation errors. Similarly, the trajectory estimation information corrects the estimate of the current robot position, providing feedback for the planned future trajectory. 

(ii) The number of variables in a STEAP factor graph does not change much during execution i.e. only a few factors are added in each step. This allows for very efficient incremental inference using the Bayes tree data structure <cit.>. Updating the solution with Bayes trees only requires a small fraction of the runtime compared to reoptimizing the full graph from scratch. Additional discussion of incremental inference using the Bayes tree is in Section <ref>.






 §.§ STEAP factor definitions




  §.§.§ The Gaussian process prior factor


A Gaussian RBF kernel defines a prior distribution of trajectories with no pairwise independences. In other words, all states are connected to a single GP prior factor, f^gp = f^gp(Θ). This prior cannot be factored, and destroys the problem's sparsity, making inference computationally expensive.
However, in the context of STEAM problems, <cit.> showed that certain types of GP priors generated by linear time varying (LTV) stochastic differential equations (SDEs), are sufficient to model Markovian robot trajectories. These priors are highly structured, and factor according to

    f^gp = ∏_i f^gp_i(θ_i, θ_i+1)

where any GP prior factor connects to only its two neighboring states, forming a (Gauss-Markov) chain. This is shown in Fig. <ref> (a) where states (white circle) form a chain by connecting to GP prior factors (black circle). In GPMP2 <cit.>, the GP prior on trajectories is generated by a LTV-SDE defined on a vector space (Fig. <ref> (b)). GP priors have also been formulated with non-linear SDEs <cit.> and on the SE(3) Lie group <cit.>. 

If the robot configuration is in vector space ℝ^n, similar to GPMP2, STEAP can use the GP prior defined in <cit.>. But we develop STEAP for mobile manipulators that have their configuration space defined by a Lie group product θ_i∈ SE(2)×ℝ^n where n is the degree-of-freedom of the arm and the SE(2) Lie group defines a planar translation and rotation (yaw) for the mobile base. We employ a constant velocity i.e. noise-on-acceleration model to define a non-linear SDE that generates our GP prior. See Section <ref> for details about the GP prior.



  §.§.§ Obstacle factor

All obstacle factors are constructed similar to GPMP2 <cit.> except that they are defined for the Lie group configuration space. The obstacle factors evaluate collision cost using a hinge loss function and a signed distance field of the environment. See <cit.> for details.



  §.§.§ Start and goal factor

These are multivariate Gaussian factors

    f^start(θ_0) = exp{-1/2∥θ_0 - θ_start∥^2_Σ_fix}
    
    	f^goal(θ_N) = exp{-1/2∥θ_N - θ_goal∥^2_Σ_fix}

with the mean as the start or goal and a small covariance Σ_fix, and are used to tie down the trajectory at the start and goal locations. When the trajectory has finished execution, the goal factor is replaced with the pose measurement factor so that the final posterior update gives the final trajectory estimate.



  §.§.§ Measurement factor

There are many types of sensors that provide different measurements, and thus many types of measurement factors have been proposed by the SLAM community <cit.>. For example, measurements from an inertial measurement unit (IMU) can be incorporated into the factor graph with pre-integrated IMU factors <cit.>, and visual landmark measurements from a camera can be incorporated with Schur complement factors <cit.>.

For the sake of simplicity, in the remainder of this paper we use a multivariate Gaussian measurement factor for the state measurement 

    f^meas_i(θ_i) = exp{-1/2∥θ_i - μ^meas_i ∥^2_Σ_meas}

where the measurement queried from sensors has mean μ^meas_i with covariance Σ_meas. The multivariate Gaussian is a typical noise assumption for many sensors. For example, GPS can provide coordinate and velocity measurements in Cartesian space with covariance <cit.>, and 2D/3D laser scanners can provide a raw localization with covariance from a 2D/3D point-cloud with the ICP algorithm <cit.>. Note that, although we only use multivariate Gaussian measurement factors in our examples and evaluations, the STEAP framework is general and can incorporate any type of measurement factors.



 §.§ A STEAP example

We use an example, illustrated in Fig. <ref>, to describe how works using Algorithm <ref> that is complemented by the block diagram in Fig. <ref>. Note that the small size of the graph in Fig. <ref> is just for illustration, in practice our approach can handle much larger graphs (see Section <ref> for the graph sizes used in our experiments). In this example, a robot with stochastic dynamics starts at time-step t_0 and needs to reach the goal at time-step t_4 while avoiding any obstacles.

First, we construct a factor graph that will reflect the prior distribution. A small, sparse set of robot states are connected via GP factors that collectively form the prior distribution of a continuous-time trajectory. Then, we add a start and a goal factor with a small covariance (to tie the trajectory down at the start and goal) and obstacle factors. In practice, there are also multiple binary obstacle factors present between any two states (omitted here for clarity) that use GP interpolation to project the cost between any two states back on to those states and allow the trajectory to stay sparse but still reason about obstacles between the sparse states (see <cit.> for details). The start, goal and obstacle factors together form the likelihood. We can find the mode of the posterior shown in blue at the top level of Fig. <ref>, which is inherently a special case of our approach providing the solution to the GPMP2 motion planning problem, since no measurement factors are present and there is no state estimation yet at this step.

	

Next, the planned solution between t_0 and t_1 is upsampled to a desired resolution with GP interpolation, checked for safety, and then executed on the robot. The ground-truth trajectory is illustrated in green. Since the system is stochastic, execution is noisy. We make an observation to generate a measurement factor and insert it into the graph at t_1. This new factor is combined with the old likelihood to produce the updated likelihood. Using the Bayes tree to efficiently organize computation, we generate a new MAP solution. Note that, in this case, the factor graph is changed by adding only one measurement factor, so the incremental inference performed using the Bayes tree will be very fast. The red portion of the trajectory is an estimate of the trajectory traversed by the robot until current time-step t_1 and the blue portion of the trajectory is the replanned solution to the goal. This whole process is then repeated (steps are shown from top to bottom in Fig. <ref>) until the robot reaches the goal at t_4. At t_4 again we have a special case of our approach that provides a solution to the trajectory estimation problem (STEAM), but with extra obstacle factors.








§ INCREMENTAL INFERENCE WITH THE BAYES TREE DATA STRUCTURE


In Section <ref>, we discussed how to solve the MAP inference problem as non-linear least squares optimization. But one significant drawback of using non-linear optimization like Gauss-Newton or Levenberg-Marquardt to solve inference on factor graphs is that they are iterative methods, and with every iteration the problem must be completely linearized and resolved (the cost on every factor will be evaluated and every variable is updated), even if the factor graph is mostly unchanged. 

To reduce these redundant calculations, <cit.> proposed efficient incremental updates of non-linear least square problems with the Bayes tree data structure. When re-solving a graph with only minor changes (in variables or factors), only the parts of the Bayes tree associated with the changes will be updated, leaving most of the Bayes tree unchanged. By updating the solution in this incremental manner, the efficiency of inference is significantly improved.

Since only a very small portion of the STEAP factor graph changes (few variables with new measurement factors) at each time-step, we convert the factor graph into a Bayes tree, and update the tree incrementally. By utilizing this efficient incremental inference technique we get a significant performance boost, and easily achieve real-time performance, as illustrated in our experiments.

In this section, we first give a brief overview of the Bayes tree and its relation to factor graphs, discuss how to perform incremental inference on Bayes tree, and then give a detailed example to show how to use a Bayes tree to perform incremental inference for a STEAP problem. Readers are encouraged to refer to the iSAM2 paper <cit.> for a more detailed and general explanation.




 §.§ Building a Bayes tree from a factor graph




A Bayes tree is a directed tree-structured graphical model which is derived from a Bayes net that has very close relation to junction tree. Both the Bayes tree's and junction tree's nodes are cliques of a Bayes net, but the Bayes tree is directed to reflect the conditional relations in factored probability density.

Before we officially define the Bayes tree, we first introduce the variable elimination algorithm <cit.>, which converts a factor graph into a Bayes net. For a given factor graph, we first choose an ordering of variables. Although any variable ordering works for the explanation here, different orderings generate Bayes nets with different numbers of edges, and, in general, a smaller number of edges is better for reducing computation. Choosing the optimal ordering is a NP-hard problem. To contend with this problem, several approximation heuristics have been proposed. We use COLAMD <cit.> to estimate a close-to-optimal ordering.

Given a factor graph and a variable ordering, we eliminate each variable by Algorithm <ref> and factorize the probability density over all variables to


    p(θ) = ∏_j p(θ_j | S_j),


where S_j ⊂θ is the separator of θ_j. Note that the factorized probability density in Eq. (<ref>) meets the conditional dependencies of a Bayes net, so, by elimination, we convert a factor graph to a Bayes net. Fig. <ref> shows an example of running elimination Algorithm <ref> on a GPMP2 planning factor graph with reverse variable ordering, which is actually the optimal variable ordering in this case.



For a Bayes net generated by Algorithm <ref>, we extract all cliques C_k from the Bayes net and build a Bayes tree by defining each node by one clique C_k. For each node of the Bayes tree, we further define the conditional density p(F_k|S_k), where S_k is the separator variables S_k, by S_k = C_k ∩Π_k intersecting between C_k and C_k's parent node Π_k, and the frontal variables F_k, by F_k = C_k \ S_k. The clique is written as C_k = F_k : S_k. The probability density of a Bayes tree is defined by the joint density of all nodes


    p(θ) = ∏_k p_C_k(F_k | S_k).


The algorithm to convert a Bayes net to a Bayes tree is summarized in Algorithm <ref>, and the Bayes tree example of the toy planning factor graph from Fig. <ref> is illustrated in Fig. <ref>. Here, the example is straightforward: the Bayes net has 4 cliques, {θ_0, θ_1}, {θ_1, θ_2}, {θ_2, θ_3} and {θ_3, θ_4}, so the Bayes tree has 4 nodes.








 §.§ Incremental inference on Bayes tree


Given a Bayes tree generated from a factor graph, we can efficiently add new factors and perform incremental inference. One of the most important properties of the Bayes tree is that if a factor, involving a variable θ_i, is added in the Bayes tree, only the cliques between the cliques containing θ_i and the root of the tree (assume the root of the tree is at the top) will be affected. The sub-tree below the cliques containing θ_i will remain unchanged during incremental inference. This means that by updating the Bayes tree a large part of the computation that is redundant and involves only untouched cliques, can be prevented compared to solving a full new non-linear least squares optimization problem.

The procedure to update a Bayes tree with new factors is stated in Algorithm <ref>. Given a set of new factors, we first find all the cliques which contain the variables involved in those new factors, and reinterpret the sub-tree as a factor graph. After adding new factors in the factor graph, we perform elimination on the factor graph to get the corresponding Bayes net, and further convert the Bayes net in to a Bayes tree. Finally, we attach the untouched sub-tree to the updated sub-tree, to get the final updated tree. The procedure is further explained by a toy example in Fig. <ref>. In this example we add a unary factor to a planning factor graph at θ_2. As explained in the previous sub-section, we have the Bayes net of the factor graph in Fig. <ref>(b), where the dashed boxes mark the affected part of the tree. We see that the clique θ_0:θ_1 remains untouched during the whole update procedure.








 §.§ Using the Bayes tree in STEAP






We are now ready to discuss how the Bayes tree can be used to speed up STEAP. At the beginning of the STEAP algorithm, the factor graph is the same as the planning factor graph in GPMP2 <cit.>. A Bayes tree is constructed from the factor graph using the approach discussed in Section <ref>. After construction of the Bayes tree, the factor graph is no longer needed or maintained, since all remaining steps are performed directly on the Bayes tree. When the Bayes tree needs to be updated with new measurement factors, we update the tree and perform incremental inference using the approach described in Section <ref>. An example is shown in Fig. <ref> that corresponds to the STEAP example in Section <ref>.

As mentioned in Section <ref>, one of the advantages of STEAP, compared to other trajectory estimation and planning approaches, is that it can use Bayes trees to gain a significant speed up while performing incremental inference. In the example in Fig. <ref>, we see that for each step of STEAP, only part of the Bayes tree is updated leaving the remaining tree unchanged. Therefore, incremental inference is more efficient while maintaining similar accuracy compared to batch inference, which needs to perform all the steps (linearization, solving linear systems, etc.) on the full graph. We also gain additional efficiency improvement from the iSAM2 <cit.> implementation of the Bayes tree (which we use) that avoids excessive linearization operations by fluid relinearization. We will see in the experimental section how the Bayes tree and iSAM2 improve the efficiency of STEAP compared to other approaches.





§ GP PRIORS




 §.§ GP priors on vector space


GP priors on vector-valued system states θ(t) can be generated by linear time-varying stochastic differential equations (LTV-SDEs) <cit.>

    θ̇(t) = 𝐀(t)θ(t) + 𝐮(t) + 𝐅(t)𝐰(t),

where 𝐮(t) is the known system control input, 𝐰(t) is white process noise, and both 𝐀(t) and 𝐅(t) are time-varying system matrices. The white process noise is represented by 

    𝐰(t) ∼ GP(0, 𝐐_Cδ(t-t')),

where 𝐐_C is the power-spectral density matrix, which is a hyperparameter <cit.>, and δ(t-t') is the Dirac delta function. The mean and covariance of the GP is computed by taking the first and second order moments of the solution to Eq. (<ref>)

    μ(t)    = Φ(t,t_0) μ_0 + ∫_t_0^tΦ(t,s) 𝐮(s) ds
    𝐊(t,t')    =  Φ(t,t_0) 𝐊_0 Φ(t',t_0)^⊤+ 
       ∫_t_0^min(t,t')Φ(t,s) 𝐅(s) 𝐐_C 𝐅(s)^⊤Φ(t',s)^⊤ ds

where μ_0 is the initial mean value of first state, 𝐊_0 is the covariance of first state, and Φ(t, s) is transition matrix.



  §.§.§ Constant velocity GP prior


The constant-velocity GP prior is generated by a LTV-SDE with a noise-on-acceleration model

    𝐩̈(t) = 𝐰(t),
 
where 𝐩(t) is the N-dimensional vector-valued position (or pose) variable of trajectory, if the system has N degrees of freedom. To convert this prior into the LTV-SDE form of Eq. (<ref>), a Markov system state variable is defined

    θ(t) ≐[  𝐩(t); 𝐩̇(t) ],

The prior in Eq. (<ref>) then can easily be converted into a LTV-SDE in Eq. (<ref>) by defining

    𝐀(t) = [ 0 1; 0 0 ],  𝐮(t) = 0,  𝐅(t) = [ 0; 1 ].




  §.§.§ GP prior factor


Given the LTV-SDE formulation defined in Eq. (<ref>), we define the GP factor between any two states at t_i and t_i+1 by <cit.>

    f^gp_i(θ_i, θ_i+1) = exp{ - 1/2Φ(t_i+1, t_i)θ_i - θ_i+1^2_𝐐_i,i+1}

where

    Φ(t, s) = [       1 (t-s) 1;       0       1 ], 
    𝐐_i,i+1 = [ 1/3Δ t_i^3 𝐐_C 1/2Δ t_i^2 𝐐_C; 1/2Δ t_i^2 𝐐_C      Δ t_i 𝐐_C ].




  §.§.§ Constant time GP interpolation


The posterior mean of the trajectory at any time τ can be approximated by Laplace's method and expressed in terms of the current trajectory θ at time points t <cit.>:

    θ(τ) = μ(τ) + 𝐊(τ,  t)𝐊^-1(θ-μ).

Although the interpolation in Eq. (<ref>) naïvely requires O(N) operations, with structured kernels θ(τ) can be computed in as fast as O(1) <cit.>.






 §.§ GP prior on Lie groups


The sparse GP prior defined in GPMP2 <cit.> works well for robot manipulators since their configurations can be defined using a vector space. But not all robots' configuration spaces can be well represented with a vector space. For example, the orientations of rigid body in 3D space cannot be represented by vectors without singularity (Euler angle) or extra degrees (quaternion). Lie groups offer more general robot configuration space representations. For example, SE(2) can represent position and orientation for a planar base of a mobile manipulator, and SE(3) can represent position and orientation for an aerial vehicle. Prior work <cit.> proposed the sparse GP prior on SE(3) which is useful for trajectory estimation. In this section we extend the sparse GP priors on Lie groups <cit.> that will be useful for more general configuration representations. This section is an overview of GP priors on Lie groups, a more detailed discussion can be found in our technical report <cit.>.

A N-dimensional matrix Lie group G is a sub-group of the general linear group and defines a smooth differentiable manifold whose local tangent space is described by its associated Lie algebra 𝔤 <cit.>. For example, the Lie algebra of SE(2) is defined by a skew symmetric matrix. We can switch between them using the exponential map exp : 𝔤→ G and the logarithm map log : G →𝔤 and to convert elements in local coordinate of G to Lie algebra and vice versa we can use the hat operator ∧ : ℝ^N →𝔤 and the vee operator ∨ : 𝔤→ℝ^N respectively <cit.>.




  §.§.§ Constant velocity GP prior


We define a constant velocity GP prior here to match the vector space prior in GPMP2 <cit.>, but just like the vector space, other priors can also be applied here. Let T ∈ G represent a state in G, such that T(t) defines a continuous-time trajectory in G. To generate the GP we need to first construct a stochastic differential equation (SDE) with a Markovian state <cit.>. Let that state be θ(t) ≐{ T(t), ϖ(t) }, with the SDE as a double integrator noise-on-acceleration model that will yield a constant velocity prior,

    ϖ̈(t) = 𝐰(t),   𝐰(t) ∼ GP(0, 𝐐_Cδ(t-t'))

where ϖ(t) is the `body-frame velocity' variable defined by

    ϖ(t) ≐ (T(t)^-1Ṫ(t))^∨.

and 𝐰(t) is a white noise, zero mean Gaussian process and power-spectral density matrix 𝐐_C <cit.>. Since ∀ T ∈ G,  T^-1Ṫ∈𝔤 <cit.>, we can apply the ∨ operator on T(t)^-1Ṫ(t). However, unlike GPMP2 <cit.>, this SDE is non-linear. Fortunately through linearization we can convert it to the linear time-varying SDE (LTV-SDE) that GPMP2 uses.




  §.§.§ Local linearization


With linearization on the Lie group around any T_i, we can define both a local GP and a LTV-SDE on the linear tangent space to leverage the constant-velocity GP prior. A local GP at any time t, t_i ≤ t ≤ t_i+1 will be

    T(t) = T_i exp(ξ_i(t)^∧),   ξ_i(t) ∼N(0, 𝐊(t_i, t))

where the local pose variable ξ_i(t) ∈ℝ^N 
around T_i is 

    ξ_i(t) ≐log( T_i^-1 T(t) )^∨.

Then the local LTV-SDE is defined using the local pose similar to Eq. (<ref>) as a double integrator noise-on-acceleration model to give a constant velocity prior

    ξ̈_̈ï(t) = 𝐰(t),   𝐰(t) ∼ GP(0, 𝐐_Cδ(t-t'))

with the local Markovian state for the LTV-SDE as 

    γ_i(t) ≐{ξ_i(t), ξ̇_̇i̇(t) }.

To show the equivalence between the original nonlinear SDE and the local approximation we use the identity <cit.>

    T(t)^-1Ṫ(t) = ( 𝐉_r(ξ_i(t)) ξ̇_i(t) ) ^∧

where 𝐉_r is the right Jacobian of G. Following from Eq. (<ref>) we get

    ξ̇_i(t) = 𝐉_r(ξ_i(t))^-1ϖ(t)

If the time interval between any t_i and t_i+1  is small then the approximation

    ξ̇_i(t) ≈ϖ(t)

is good. Then linear SDE Eq. (<ref>) is a good approximation of non-linear SDF Eq. (<ref>), so we linearize the SDE and we can apply LTV-SDE GP prior by <cit.>.




  §.§.§ GP prior factor

 We define the GP factor between any two states at t_i and t_i+1 with their local pose using the linearized SDE formulation on the Lie group manifold

    f   ^gp_i(θ_i, θ_i+1) 
       = exp{ - 1/2Φ(t_i+1, t_i)γ_i - γ_i+1^2_𝐐_i,i+1}

where the logarithm map provide transformation from SE(2) to ℝ^3 and vice versa with the exponential map, and 

    Φ(t, s) = [       1 (t-s) 1;       0       1 ], 
    𝐐_i,i+1 = [ 1/3Δ t_i^3 𝐐_C 1/2Δ t_i^2 𝐐_C; 1/2Δ t_i^2 𝐐_C      Δ t_i 𝐐_C ].

We use this GP prior in our current implementation.







§ IMPLEMENTATION DETAILS


We implement STEAP within the PIPER <cit.> package using ROS <cit.> and GPMP2 <cit.> and have open-sourced the code. Fig. <ref> shows a block diagram of the framework. The offline phase assimilates (i) robot-specific information including model and physical parameters, (ii) problem definitions and optimization parameters, and (iii) a pre-generated signed distance field (SDF) of the environment, which is assumed to be static. In the online phase, this information is passed to our central module, , that solves problems and communicates with the  (simulated or physical) with sensors, and the  that takes raw sensor measurements and outputs a noisy pose estimate for the robot that can be interpreted by the .

Note that in our framework, the  is free to be any source of raw or processed sensor information, as long as suitable factors are defined to fuse sensor information in the factor graph, e.g. GPS, LIDAR, monocular, or stereo camera data. In our implementation we use a depth image-based localization algorithm, detailed in Section <ref>.




 §.§ STEAP module


This module uses the robot and problem configuration. In the first step, the module initializes the problem by constructing an initial factor graph, performing inference to get the first planned solution, and constructing a Bayes tree for future iterations. During every other step, the module performs incremental inference by updating the Bayes tree directly. At every step, the replanned solution is upsampled and checked for safety, and is sent to the . When the  returns a current pose measurement, new sensor measurement factors are added to the Bayes tree and the updated posterior is evaluated. This procedure repeats until the full trajectory completes execution or exits due to collision failure. Given the generic implementation of this module, our framework can be used on any simulated or real robot as long as the robot information is provided in the offline phase.





 §.§ Robot module


This module consists of the robot API and controllers that can understand and execute the trajectory passed to it by the  for either a real robot or an interface with Gazebo for a simulated robot. Sensors on the robot pass information to the . Note that in our simulator, adjustable Gaussian noise is mixed in both system dynamics (more precisely vehicle velocities) and sensor measurements (more precisely depth measurements of depth camera simulator), to simulate the real-world stochasticity. 




 §.§ Mapper


Obstacle factors require a signed distance field (SDF) to calculate obstacle cost. Although we can calculate SDFs from CAD models in simulation, we will not have these models available in most real-world environments. Therefore, we build SDFs from sensor data. We use depth scans from depth sensors (a simulated depth camera in simulation and a PrimeSense, shown in Fig. <ref>, in real-world experiments), and an occupancy grid mapping approach <cit.> to generate the SDF. The space is first discretized into small cells, and a probability of occupancy p_o is assigned to each cell, with initial p_o = 0.5 (since we have no information). All p_os are updated by sensor measurements, and after all sensor data is received, we assume cells with p_o≥ 0.5 are occupied, and cells with p_o < 0.5 are unoccupied. Note that we assume cells with p_o = 0.5, which indicates no depth measurement available, are occupied, since it is safer to assume that locations never observed are occupied by obstacles. True camera poses are provided by a motion capture system during the mapping process. After occupancy grid mapping, we calculate the signed distance field by the efficient distance transformation <cit.>. We use CUDA <cit.> to implement occupancy grid mapping and distance transformation, allowing us to achieve real-time performance on scenes roughly of size 8m×6m×1.5m with 3cm resolution. A map constructed with this approach is shown in Fig. <ref>.






 §.§ Localization module


The Localization module reads raw sensor data from the , calculates a pose estimate of the robot, and provides this information to the . Here it's free to choose any Localization algorithm. We choose an ICP-style iterative approach, which similar to tracking in KinectFusion <cit.> but operates on the full SDF generated by the  rather than the truncated SDF (TSDF) in KinectFusion. We use CUDA to implement and parallelize the tracking module to achieve real-time performance. Although additional sensor data like RGB images, odometry, and laser scans are available to the robot, we only use depth images from PrimeSense in our experiments.







 §.§ Computational complexity


Since our approach can involve performing inference on arbitrary factor graphs and incremental inference on their corresponding Bayes trees, the computational complexity depends on the problem and the structure of the graph or tree and how the tree changes over time. Considering the case of the experiments in this paper where the graphs are chain-like and measurement factors are unary, the complexity of the batch step is O(TD^2), where T is the number of time-steps (or states) and D is the dimension of the system state. The time complexity O(TD^2) comes from solving the block-tridiagonal linear system with block size D × D.

In the online step, the time complexity of the incremental update is O(VD^2), where V is the total number of variables (or states) in all the affected cliques of the Bayes tree starting from the root of the tree. In the beginning since all states in the Bayes tree are affected, V is the same as T and the time complexity is equal to the batch step complexity. When the robot is moving along the trajectory, V is much smaller, as shown in Fig. <ref>.

The time complexity for STEAP discussed above only applies to cases of chain-like graphs (as shown in Fig. <ref>) that have a start-to-goal variable ordering. In the presence of non-unary measurement factors (for example, higher-order measurement factors like landmark observations) that connect to states across greater time-intervals, or in the case of loop-closures, the time complexity will be greater. In general, the time complexity increases as the sparsity in the graph decreases, and is also dependent on the variable ordering selected by the COLAMD heuristics.





§ EVALUATION


We evaluate our framework with simulation and real-world experiments.[A video of experiments is available at <https://youtu.be/lyayNKV1eAQ>] The simulation benchmark is performed on two datasets: a planar 2-link mobile arm in several randomly generated 2D environments as shown in Fig. <ref>, and a 18-DOF PR2 robot in a simulated indoor environment as shown in Fig. <ref>. The real-world experiments are performed on a mobile manipulator, with an omni-drive base and a 6-DOF Kinova JACO2 arm, in an indoor environment as shown in Fig. <ref>.

In our experiments we compare our proposed approach, simultaneous trajectory estimation and planning (), against an open loop execution (). In  the initial inference solves the GPMP2 planning problem and the planned trajectory is executed without any estimation or replanning until the final time-step or a collision. In our simulation experiments, we also compared against simultaneous localization and planning () that uses the current measurement factor in the graph, but updates only a truncated version of the graph associated with the future states to replan. This graph also uses the GP prior factors from .








 §.§ Benchmark with a planar 2-link mobile arm


For this benchmark we use a simulated 2-link planar mobile arm with base of size 1m × 0.7m and link length 0.6m in an environment of size 30m × 20m. The robot is equipped with a simulated 2D laser scanner for localization using ICP. The environment is populated with 20 randomly generated obstacles of size 1m × 1m. The graph consists of 30 states from start to goal with 5 interpolated binary obstacle factors between any two states. We compare ,  and  across different amounts of robot dynamics noise (n_dyn), implemented as uniform bounded additive noise (m/s) to the robot velocity, and camera measurement noise (n_cam), implemented as additive Gaussian noise (m^-1) when receiving depth information from the camera on the robot. Each setting is run with 40 distinct seeds (each seed yields a new environment) to account for stochasticity, which are kept the same across all three scenarios. In each trial we record if the trajectory successfully finishes without collision (success), the distance from the goal (goal error) at the end of execution, and L_2 norm of the ground-truth trajectory with the estimated trajectory (estimation error).






The results for this benchmark are summarized in Table <ref>–<ref>. The goal and estimation error are aggregates of runs where success is true (the robot reached the goal without colliding with any obstacles). As expected,  exhibits a low success rate that drops further with an increase in n_dyn.  has lower success rates compared to ; a possible reason is the low precision trajectory estimation from  confuses the robot. Comparatively  has a higher success rate than  and , and also follows the decreasing trend with increasing n_dyn.

The goal error in  is much lower than  and , with the help from high precision trajectory estimation. The trajectory estimation error is significantly smaller with  compared to  and the difference between them increases with increasing n_cam. This can be attributed to simultaneously solving the trajectory estimation and planning problems; the motion plan helps in providing a better estimate of the robot's trajectory and in turn a better estimate of the trajectory helps in generating a better motion plan.




 §.§ Benchmark with 18-DOF full-body PR2




We additionally evaluated our approach on a simulation dataset in 3D, with a 18-DOF PR2 robot in an indoor setting. The PR2 has two 7-DOF arms (without grippers), a 3-DOF mobile base, 1-DOF linear actuator that moves the torso in the vertical direction, and a 2D laser scanner on the robot base (10cm above the ground level) used for localization. The environment consists of two cabinets, a desk and several random obstacles so that the laser scanner receives non-trivial readings when the robot is facing outwards. The benchmark consists of a total of 6 tasks, each with a start and a goal state, and the robot is tasked with reaching the goal states from the start states. The environment and an example problem from the benchmark is shown in Fig. <ref>. We compare ,  and  across different n_dyn and n_cam settings, with a graph of 40 states from start to goal. Each setting is run for 120 trials (6 tasks with 20 distinct seeds) and similar metrics as the 2D benchmarks are recorded.






The results for this benchmark are summarized in Table <ref>-<ref>, which support the finding in the previous 2D benchmark.
We see that  has higher success rates compared to  and . This is a result of the lower estimation errors (compared to ) and better quality plans achieved through joint inference.  has a significantly lower estimation errors with larger n_cam. The goal error in  and  are smaller than  since both are feedback approaches that can reduce the error when approaching the goal. We also observe that  has a smaller performance drops caused by a high value of both n_dyn and n_cam, than  and .

We also report timing results for this benchmark. On average  takes about 17ms per time-step, which is significantly faster than SLAP with an average of about 130ms per time-step.







 §.§ Experiments with a real robot


Real-world experiments were performed in an 8m×6m indoor environment. Various obstacles (desks, sofas and small objects like boxes and cans) are placed in the environment, to simulate domestic scenes. During the experiments, ground-truth robot trajectories are recorded by an Optitrack motion capture system. A photo of the robot traversing the environment and a map of the environment can be found in Fig. <ref> and Fig. <ref>, respectively.
Our implementation runs on a desktop computer equipped with Intel 4.0GHz quad-core CPU, 32GB memory and one NVIDIA Titan X GPU. Robot sensor data is streamed to the desktop over WiFi, and STEAP commands are streamed back to robot after processing. 

We design 2 problems for performance evaluation. In each problem the robot begins from a start configuration and is tasked with reaching the goal configuration. For both problems the graph consists of 50 states from start to goal with 2 interpolated binary obstacle factors between any two states. Fig. <ref> shows a screenshot when  the robot is running STEAP for problem 1.
To evaluate the performance of our STEAP implementation, we performed 10 runs for each problem, in which 5 runs switch the start and goal configurations. We record the planned, estimated and ground-truth trajectories and calculate the same performance criteria as in simulation: success rate, final goal error, and trajectory estimation error.



Table. <ref> reports performance in these real-world experiments. We first run one-time batch planning by GPMP2 and use an open loop controller to follow the planned trajectory. Since the control command execution on the omni-directional wheels is noisy, the robot base cannot follow the planned trajectory well,  so every run ends with a collision.
With the state estimation and replanning provided by STEAP, the robot can follow planned trajectories better, and compensate for perturbations. With STEAP the robot can achieve a 95% overall success rate for the given tasks, with a final translation error of about 14.2cm in problem 1, and 5.19cm in problem 2. This goal error is due to the finite-horizon trajectory setup that we use. Since if the robot overshoots when near the end of the trajectory, it may not have enough time steps left to recover. The goal error can be reduced with a receding-horizon formulation of our problem.

In addition to improving planning results, STEAP helps with trajectory estimation. We show the raw localization error in Table. <ref>. Due to the noisy depth measurements, the localization module provides poor estimates of the robot pose. Sometimes the localization module additionally fails due to the scene being out of  sensor range (for example when the robot is too close to obstacles). With STEAP, we can reduce the estimation error by about 50-60% as shown in Table. <ref>. In the video of experiments,<ref> one can see that although the raw localization positions have significant jumps between each measurement, the estimation results in STEAP are stabilized given previous sensor information and the planned trajectory.

To evaluate the efficiency of our implementation, we time the localization and STEAP modules separately. Timing results show that, in real-world experiments, localization and STEAP modules have average runtimes of 19.3ms and 76.0ms respectively, and maximum runtimes of 30.3ms and 149ms respectively, indicating that our localization implementation can easily process the depth image stream at 30Hz, and run STEAP at ∼ 10Hz. 






§ LIMITATIONS AND FUTURE WORK


The primary limitation of our current work is that it is only applicable in known, static environments. We use an existing map of the environment and precompute a signed distance field for collision checking. In dynamic environments, the map and the signed distance field would need to be constantly updated, which can be a major computational bottle neck, especially in large environments. Using techniques like incremental mapping <cit.>, incremental signed distance fields <cit.>, and dynamic tracking <cit.>, we can extend our method to perform SLAM and planning simultaneously online and handle dynamic environments.

Our current implementation is limited to holonomic systems, like the omni-directional mobile base we use in our experiments. It does not support nonholonomic and inequality constraints. But, they can be incorporated as soft constraints with small covariances, for example, the configuration and velocity limit factors <cit.>. A sequential quadratic programming type procedure would have to be set up to handle hard constraints.

Our approach is a local trajectory optimization method and is, therefore, prone to local minima. In the context of estimation, trajectory optimization rarely suffers from bad local minimas, since obtaining reasonable initial values is not hard during estimation (e.g. from odometry). In the context of planning, trajectory optimization suffers from bad local minimas (ones that are in collision) primarily in extremely cluttered or maze type environments due to the nature of the optimization methods used. Readers are encouraged to refer to <cit.> and <cit.> for some quantitative evaluations on several commonly used trajectory optimization techniques. In case of batch optimization i.e. the first inference step, several ideas like random initializations and graph-based initializations <cit.> exist to improve results. However, there are no known methods to address this problem during incremental inference using the Bayes tree, except by re-solving a new batch optimization problem, which will be computationally expensive. Developing new incremental algorithms that are better able to contend with local minima is an interesting research direction.

Our approach is capable of fusing information from multiple sensors in an asynchronous fashion using GP interpolation <cit.>, we are interested in exploring this capability more fully. Finally, as discussed in the results section, a receding horizon formulation of would help reduce goal-errors and better support navigation with exploration. We leave this as future work.





§ CONCLUSION


We formulate the problem of simultaneous trajectory estimation and planning () as probabilistic inference. By representing the prior distribution of a continuous-time trajectory and likelihood function of costs and observations with factor graphs, we can efficiently perform inference to compute the posterior distribution of the trajectory. We solve STEAP in an online setting to simultaneously estimate and smooth the trajectory history as well as replan for the future trajectory as new information is encountered. This is made possible by performing efficient incremental inference to update the previous solution. We conducted experiments in simulation and on a real mobile manipulator and showed that our framework is able to perform in real-time and robustly handle the stochasticity associated with execution. Our results demonstrate that this framework is suitable for online applications with high-degree-of-freedom systems in known, static real-world environments.




This work was partially supported by NSF NRI award 1637908 and National Institute of Food and Agriculture, USDA, award 2014-67021-22556. The authors thank Muhammad Asif Rana, David Kent, Vivian Chu, and Sonia Chernova for access to and help with the Vector robot.




spbasic




