Conditioned Score-Based Models for Learning Collision-Free Trajectory Generation
Abstract
Planning a motion in a cluttered environment is a recurring task autonomous agents need to solve.
This paper presents a first attempt to learn generative models for collision-free trajectory generation based on conditioned score-based models.
Given multiple navigation tasks, environment maps and collision-free trajectories pre-computed with a sample-based planner, using a signed distance function loss we learn a vision encoder of the map and use its embedding to learn a conditioned scorebased model for trajectory generation.
A novelty of our method is to integrate in a temporal U-net architecture conditioning variables such as the latent representation of the environment and task features, using a cross-attention mechanism.
We validate our approach in a simulated 2D planar navigation toy task, where a robot needs to plan a path that avoids obstacles in a scene.
Introduction
Recent advances in new architectures and training methods of diffusion and score-based models (SBMs) have shown impressive results in image and text-to-image generation [24, 4, 23, 10, 20].
One field where these models are still not fully explored is robotics [7, 27].
Due to the multimodality and dimensionality of sensory data, e g. inputs from vision or robot trajectories, it can be worth to study diffusion models as components of intelligent robots.
Figure 1 pictures an autonomous robot cleaning a house floor.
One crucial task is to plan collision-free motions (paths) between two locations.
With access to an environment map, sampling-based methods, such as variants of Probabilistic Road Maps (PRM)
[9] and Rapidly exploring Random Trees (RRT) [13], are commonly used.
By replanning over multiple scenarios, the robot observes data that is often unstructured, such as point clouds of tables, chairs or shelves.
It is therefore natural to reuse this collected information if it needs to replan in a new situation.
To speed up planning, several methods have proposed neural motion planners that encode environment and motion information using neural networks, which are afterwards used as priors for planners [17, 19].
In this work, we leverage recent developments in SBMs and Signed Distance Fields (SDFs) [16] to build a generative model for collision-free trajectory generation, conditioned on an environment map and task relevant features.
We summarize our contributions as: (1) learn an environment encoder based on an SDF loss to encode geometrical properties; (2) introduce a conditioning model for planning - the Conditional Temporal U-Net - extending the work of Janner et al.
[7] with ideas from Rombach et al.
[23]; (3) learn a generative SBM of collision-free trajectories given a new environment and task; (4) show first results in a simulated 2D navigation task.
Score-Based Models for robotics
Few works have explored using score-based and diffusion models in robotics.
Janner et al.
[7] presented the Diffuser , a diffusion model for trajectory planning based on temporal convolutions, which enforces temporal ordering and locality.
We use their Temporal U-Net architecture as a component in our work.
Wang et al.
[31] used diffusion as regularizers and policies for step-based offline reinforcement learning (RL).
In robotics, Urain et al.
[27] used diffusion to learn cost functions for jointly optimizing motion and grasping poses.
Motion planning We consider two methods for motion planning: sampled-based and optimizationbased.
Both strategies can be augmented with better sampling distributions learned from previous data to guide and speed up planning.
Sampling-based motion planning includes classical algorithms such as PRM [9], RRT [13] and RRT* [12].
Several works have proposed learning conditional sampling distributions using the environment and task information as context variables, e.g. in [6] the environment is given as an occupancy map.
Similar to our work, Deep Sampling-based Motion
Planner (DeepSMP) [17] learns an autoencoder given a point cloud.
Instead, we learn an encoder trained on an SDF loss, which has two advantages - there is no need to learn a decoder for reconstruction; and we get access to the environment’s SDF and its gradient, which can be used as an extra cost for planning.
Other works [17, 19, 18, 30] learn a conditioned one-step neural planner, and to sample different solutions they add dropout to ensure stochasticity.
Contrarily, we learn a trajectory distribution model to easily introduce the notion of smoothness, which is important in robot motions.
Optimization-based motion planning includes methods that optimize a trajectory either via gradient optimizers or stochastic optimization, e.g. Covariant Hamiltonian Optimization for Motion Planning (CHOMP) [21], Stochastic Trajectory Optimization for Motion Planning (STOMP) [8] or Gaussian
Process Motion Planner (GPMP) [15].
These methods often use an uninformed initial distribution at the start of optimization.
In CHOMP a straight trajectory connects the initial and final points, STOMP uses a distribution, whose mean is a straight line, but has high entropy in the middle of the trajectory, and decreasing entropy towards the initial and final points, GPMP uses a Gaussian Process Prior.
Learning better initial distributions can thus speed up these methods, as shown in [28].
Our conditioned SBM can be used a prior distribution for both sampling and optimization-based planners.
Background 2.1 Motion Planning as Inference
Let s ∈ S
⊆
R d encode the state of a robot (agent) and its environment.
In motion planning, a trajectory is represented in discrete-time with horizon H as a sequence of states τ ≜ ( s 1 , . . .
, s H ) ∈
R H × d .
It is common to optimize τ given a context C , which can include an occupancy map, obstacle locations, start and final positions, etc.
Optimization-based motion planning formulates the problem as trajectory optimization τ ∗ =
arg min τ (cid:80) i
c i ( τ , C ) , where c i are different costs related to trajectory smoothness, obstacle avoidance or goal reaching [28].
The connection between trajectory optimization and probabilistic inference is well established [1, 26, 14].
Following the notation in [7], the probability of a trajectory (a random variable) factorizes (up to a normalizing constant) as ˜ p ( τ |C )
∝ p ( τ |C ) h ( τ |C ) , where p ( τ |C ) is a prior distribution (that will be will be learned from data) and h ( τ |C ) is a task-specific distribution, e.g. a delta distribution for the starting state of the trajectory.
Trajectory optimization computes the maximum-a-posteriori solution τ ∗ = arg max τ log ˜ p ( τ |C ) .
On the other hand, in inference we sample from ˜ p
τ |C ) , which allows to get multiple solutions.
Langevin dynamics [32] is a common approach to sample from ˜ p , for which we need ∇ τ log ˜ p ( τ |C ) =
∇ τ log p ( τ |C ) + ∇ τ h ( τ |C ) .
We propose to approximate the gradient of the log-prior distribution on trajectories with a conditioned SBM s θ ( τ , c )
[Equation]
2.2 Score-Based Models as Trajectory Generative Models
Several generative modelling techniques, such as Generative Adversarial Networks (GANs) [3],
Variational Auto Encoders (VAEs) [11] and Normalizing Flows (NFs) [22], are trained to maximize the data log-likelihood. Sampling is done by applying deterministic transformations to a random variate from a simple distribution.
Instead, SBMs are implicit models [5, 29, 24], which perturb the data with diffusion and learn to reconstruct it by denoising using the score-function - the gradient of the log-probability w.r.t.
the input.
Sampling is done by iteratively transforming a sample from a simple distribution following the (parametrized) score-function s θ ( τ t , t ) ≈ ∇ τ t log p ( τ t , t ) , where
[Figure]
[Figure]
t ∈ [0 , 1] is the time of the denoising step.
The goal is to move an initial noisy sample τ 1 to a sample from the data distribution τ 0 .
SBMs can be trained with Denoising Score Matching (DSM) by minimizing
[Equation]
[Equation] where p ( τ t | τ , t ) = N (cid:0) τ t ; τ , σ 2 ( t ) I (cid:1) is the density of the perturbed trajectory at time t , λ ( t ) =
( a 2 t − 1) / (2 log a ) and σ ( t ) =
(cid:112) λ ( t ) .
Due to space constraints, for more details we refer the reader to [25].
In [7] a discrete-time denoising diffusion probabilistic model is used instead [4].
Conditioned Score-Based Generator for Collision-Free Trajectories
Given a dataset of environments E , task features g (e.g. initial and final positions) and trajectories D
= { ( τ i , E i , g i ) }
Ni =1 , we model a conditional distribution p ( τ | c ) .
The environment E is given as an occupancy map, which is transformed into latent features with a vision encoder f ϕ ( E ) parametrized by ϕ .
We create a context embedding c
= c θ ( f ϕ ( E ) , g ) , which is used as a conditioning variable for the SBM.
We propose to learn an implicit representation of p ( τ | c ) by modelling a conditioned score function s θ ( τ t , t, c )
≈ ∇ τ t log p ( τ t , t | c ) , implemented as a conditional temporal U-Net .
The network architecture uses the Diffuser from [7] as an unconditional model, and we introduce conditioning by using a cross-attention mechanism at the end of each temporal residual block of the U-Net, similar to [23].
We experimented with other types of conditioning, e.g. concatenating the context embedding, trajectory and time, but found that cross-attention produced better results.
Learning the vision encoder and score models The overall architecture is depicted in Fig.
Extra details can be found in App.
B. The vision model takes as input an image E (black-and-white occupancy map), and produces latent features f ϕ ( E ) .
With access to the distribution of environments and ground-truth signed distance values sdf gt ( E, x ) for a point x ∈ R 2 , we pretrain the vision encoder, a CNN with parameters ϕ , by minimizing an SDF L 1 -loss
[Equation]
where ˆ sdf ξ is the learned SDF .
We use a shallow SDF network, to enforce the vision encoder to represent as much of the environment features.
The latent representation f could also be learned with a VAE, but this model would be larger (due to the decoder) and would not give access to the SDF , which we use for ranking samples.
With a pretrained vision model, the conditioned SBM is trained by sampling a batch of environments, tasks and trajectories, and minimizing the score loss with DSM
[Equation]
Following [25, 23], the context encoder c θ and the conditioned SBM s θ are jointly trained.
We experimented with other types of architectures, such as learning jointly the vision features and the score-model, but found it to be more difficult to train.
Also, our solution is modular, allowing to use other types of pre-trained modules needed for robotics without retraining everything, e.g. one could use dense object features to create movements conditioned on a particular object.
RRT* Trajectories
Sampled Trajectories
Signed Distance Field − 10 −− 10 − 10 −10 −
Trajectory generation and ranking Given a new environment and task we compute the context vector c .
To generate trajectories we solve a reserve SDE by using the learned SBM and the EulerMaruyama solver.
In practice, we sample a noisy trajectory τ 1 ∼ N (cid:0) 0 , 0 . 5( a 2 − 1) I (cid:1) and iteratively run τ t − ∆ t = τ t + a 2 t s θ ( τ t , t, c )∆ t + a t √ ∆ t z , where ∆ t is a time discretization (in our case ∆ t = 1 / 500 ), a = 1 . 1 and z ∼ N ( 0 , I ) .
In a real world scenario we are able to execute only one trajectory.
Therefore, we propose a simple heuristic metric to select and rank trajectories – discard the ones if any point along them has a negative SDF , which means collision with an obstacle.
Afterwards, we select the trajectory that has the shortest path.
Even though this does not guarantee the optimal trajectory is found, it can be a good first heuristic prior for motion optimization methods.
2D Planar Navigation Experiments
Our simulated setup includes an image of an environment and a robot located at an initial position (lower left) that needs to move to a final position (upper right) while avoiding obstacles (Fig. 3).
The datasets consist of an image that describes a 2D environment, different tasks for each environment where tasks are different start and end points, and RRT* generated collision-free trajectories.
The training set has 50 k environments, each having 10 random tasks and one RRT* demonstration.
To train the trajectory generator, we used an horizon of 32 steps, hence τ ∈ R 32 × 2 .
The validation set corresponds to 5% of the total data.
For each environment an SDF for the obstacles is created as well.
Fig.
3 shows the results in the validation set of the R ECTANGLES AND C IRCLES dataset.
The left plot shows the trajectories generated with RRT*, for different goal positions.
Thinner trajectories in the center plot are sampled with our learned conditioned SBM, and thicker trajectories are the ones considered after ranking.
The right plot shows the learned SDF .
Notice how these trajectories traverse regions with positive SDF values.
In the center plot some of the green trajectories cross the obstacle, which can be partially explained by errors in the learned SDF .
Nevertheless, these trajectories are discarded by our heuristic.
As a quantitative measure, we evaluated our method across all validation environments and obtained around 90% of collision free trajectories.
More experiments can be seen in Figures A.1 and A.2, where we observe that the model produces diverse (multimodal) trajectories.
Conclusion and Future Work
In this work we presented an architecture for a conditional generative score-based model, with applications to collision-free trajectory generation for planar navigation.
To encode a conditional distribution, we augmented a temporal U-Net model with a conditioning mechanism based on crossattention.
As contextual variables we use task features and a latent encoding of an environment map, which is trained with a signed distance function loss.
First experiments of our method in a simulated 2D planar navigation task show promising results to encode collision-free trajectory generation.
In future work, we will incorporate our learned model as a prior to speed up sample-and optimization-based planners and extend our approach to robots arms with more degrees of freedom.
We believe that due to the modelling power of score-based methods, they can be used as learned components of intelligent robotic systems.
Acknowledgments and Disclosure of Funding
This work was funded by the German Federal Ministry of Education and Research (project IKIDA, 01IS20045). Authors are with the Institute for Intelligent Autonomous Systems (IAS) of the Technische Universität Darmstadt (TU Darmstadt). Jan Peters is affiliated with the German Research Center for AI (DFKI), Research Department: Systems AI for Robot Learning, Hessian.
AI and the Centre for Cognitive Science of TU Darmstadt. Emails: {joao,julen,jan}@robot-learning.de, mark.baierl@stud.tu darmstadt.de A Additional Results
[Figure]
RRT* Trajectories
Sampled Trajectories
[Figure]
[Figure]
−
[Figure]
−
[Figure]
−
[Figure]
−
[Figure]
−
[Figure]
−−
[Figure]
−
[Figure]
−
[Figure]
−
[Figure]
−
[Figure]
−−
[Figure]
−
[Figure]
−
[Figure]
−−
[Figure]
−−
[Figure]
The temporal U-Net architecture and dimensions are the same as in the original work [7]. Tabledetails the other components of the model. More details on the architecture, training, hyperparameters, etc., can be found in the accompanying code upon publication.
[Table]
C Background on Deep Signed Distance Functions
Given an environment consisting of free-space and obstacles [Table] Signed Distance Functions (SDFs) are continuous mappings that compute the distance from a point in space to the closest obstacle surface
SDF ( x ) ∈ R [2].
In our current approach x ∈ R[-2 , but in general it is a point in 3 D. If SDF ( x ) < 0 ,the point lies inside of the obstacle, SDF ( x ) > 0 it lies outside, and if SDF ( x ) = 
-]
[Table]
but in general it is a point in 3 D. If SDF ( x ) < 0 , the point lies inside of the obstacle, SDF ( x ) > 0 it lies outside
[Table]
and if SDF ( x ) = 0 it lies on the surface.
To generalize to multiple shapes, deep SDFs [16] use deep neural networks to learn both the latent representation z S of a shape S and encode the distance function, ˆ SDF ξ ( z S , x ) ≈ SDF S ( x ) .
They use an auto-decoder network to learn the latent shape embedding, and jointly train this network and the SDF network to learn the geometric details of an object.
Note that the SDF network should be responsible for learning only a metric, and the latent embedding should encode the geometrical properties of the object.
We leverage this idea to learn a geometry encoding of our 2D environments.