Log In Sign Up

Predicted Composite Signed-Distance Fields for Real-Time Motion Planning in Dynamic Environments

We present a novel framework for motion planning in dynamic environments that accounts for the predicted trajectories of moving objects in the scene. We explore the use of composite signed-distance fields in motion planning and detail how they can be used to generate signed-distance fields (SDFs) in real-time to incorporate predicted obstacle motions. We benchmark our approach of using composite SDFs against performing exact SDF calculations on the workspace occupancy grid. Our proposed technique generates predictions substantially faster and typically exhibits an 81–97 subsequent predictions. We integrate our framework with GPMP2 to demonstrate a full implementation of our approach in real-time, enabling a 7-DoF Panda arm to smoothly avoid a moving robot.


page 1

page 3


Motion Planning in Dynamic Environments Using Context-Aware Human Trajectory Prediction

Over the years, the separate fields of motion planning, mapping, and hum...

Planning Robot Motion using Deep Visual Prediction

In this paper, we introduce a novel framework that can learn to make vis...

Simultaneous Scene Reconstruction and Whole-Body Motion Planning for Safe Operation in Dynamic Environments

Recent work has demonstrated real-time mapping and reconstruction from d...

Object-Centric Task and Motion Planning in Dynamic Environments

We address the problem of applying Task and Motion Planning (TAMP) in re...

A Scalable Framework For Real-Time Multi-Robot, Multi-Human Collision Avoidance

Robust motion planning is a well-studied problem in the robotics literat...

Rapid Motion-Planning for Dubins Vehicles under Environmental Drifts

This paper presents a rapid (i.e., (near) real time) solution to the min...

Provable Infinite-Horizon Real-Time Planning for Repetitive Tasks

In manufacturing and automation settings, robots often have to perform h...

I Introduction

To integrate autonomous systems into our daily lives, we need to ensure that they operate safely in the dynamic world around them. We must, therefore, develop robots that can adapt their movements as necessary to avoid moving obstacles. This holds whether we consider robots in a household environment avoiding humans and pets, autonomous vehicles on a road avoiding other cars and pedestrians, or robots operating in a dynamic industrial setting.

There has been significant research into motion planning; however, the current ability of robots to react in dynamic environments, with moving obstacles, is still lacking. Many approaches rely on fast re-optimisation to account for changes in the environment, yet do not account for the predicted motions of the obstacles. By neglecting predicted motions, robots encounter failure cases, as depicted in Fig. 5, where depending on the speed of the obstacle, the robot repeatedly plans to move in front of the moving obstacle. This scenario occurs because the optimisation can become trapped in a local minimum from which it cannot escape since it does not have full information of the obstacle trajectory during the early iterations. The resultant trajectory is both sub-optimal and potentially dangerous. In this paper, we show that the solution to this is to incorporate predicted obstacle motions into the planning problem.

Fig. 4: Real-time collision avoidance in a dynamic environment using our proposed framework. The 7-DOF Panda arm is tasked with a pick-up task, positioning the end-effector over a gap onto another table. During execution, a Toyota HSR travels along the gap, acting as a moving obstacle. Poses are shown in blue (start and goal configurations), green (collision-free), or red (collision). LABEL:fig:static_traj) Without environment updates during execution. LABEL:fig:one_step_traj) With environment updates and trajectory re-optimisation during execution. LABEL:fig:prediction_traj) Our method - including obstacle trajectory prediction. We incorporate ESDF predictions which are generated at . The update loop runs at a frequency of at the start and for the remainder of the trajectory.

Motion planners that would otherwise use signed-distance fields are forced to adopt alternative methods, such as binary collision costs [25], to incorporate predictions. This is because the computation time for calculating SDFs prohibits real-time usage—i.e., the ability to generate predictions at a much higher frequency than environment observations are made, typically . Our results motivate using predictions and show that we can generate real-time predictions for signed-distance fields by using composite signed-distance fields.

We integrate our method of composite SDF predictions into our novel framework which uses the GPMP2 motion planner [20] to solve for collision-free trajectories in environments with moving obstacles. We assume no prior knowledge of the moving obstacles; instead, they are segmented from the scene using sensor data and we infer their characteristics such as obstacle shape, velocity, and direction.

The key contributions of this paper are:

  • Introduction of predicted signed-distance fields for motion planning in the presence of moving obstacles.

  • Extension of GPMP2 to plan using predicted obstacle trajectories.

  • A novel framework for motion planning in dynamic environments using composite signed-distance fields.

Fig. 5: A toy example showing the inherent flaw in the execute and update method of trajectory optimisation. The task is to plan a collision-free trajectory to reach the goal state in the presence of moving obstacles. During execution, we continue to observe the latest position of the moving obstacle and update the planned trajectory. As an obstacle approaches the intersection with the current planned trajectory, this repeated optimisation can result in the planned trajectory chasing in front of the obstacle.

Ii Related Work

Motion planning in the presence of moving obstacles is an open problem, while solutions generally fall into two categories. The first is to assume full prior knowledge of the moving obstacle trajectories in the scene [18, 5, 19]. The second approach is ‘continuous re-planning’ in which the motion planner either re-optimises and adapts the current planned trajectory, or considers multiple trajectory modes at any one time, such as in ITOMP [25], and smoothly switches between them as new information is provided [12, 35]. However, there have been few works that incorporate predicted obstacle motions into the motion planning of articulated systems.

Nam et al. model obstacle motions as a random walk process and assign an artificial potential based on the probability density function of their predicted positions


. Park et al. proposed IAMP which integrates ITOMP with human motion prediction based on supervised learning

[26]. Li and Shah implement a probabilistic roadmap approach in which the roadmap is constructed using an obstacle motion prediction, essentially assuming a prior knowledge of the trajectory [15].

To further explore this problem, we first consider the environment representations used in motion planners.

Binary occupancy information enables search-based methods such as A* and D*-Lite, as well as sampling-based methods like Probabilistic Road Maps (PRMs) and Rapidly-exploring Random Trees (RRTs), to perform collision avoidance [2, 11, 9, 8, 14, 13]. In contrast, continuous optimisation-based approaches additionally require gradients so commonly use Euclidean Distance Transforms (EDTs), or Euclidean signed-distance field (ESDF), to represent the environment.

An ESDF is a 3D voxelgrid in which each cell contains the signed-distance to the closest obstacle surface. The sign denotes whether the cell is within or outside the surface boundary. In addition to providing distance information which can be used to assign collisions costs, tri-linear interpolation enables us to obtain gradients.

Motion planners such as CHOMP, TrajOpt, and GPMP2, use ESDFs to represent the environment and demonstrate them to be very effective in enabling fast planning in static environments [28, 30, 20]. However, the flaws in SDFs become apparent when we look to apply them in dynamic environments.

ITOMP considered obstacle trajectory prediction for use in dynamic environments. However, in contrast to their general approach for static collision costs, which uses EDTs, they implement a simple occupancy cost for predicted motions based on “geometric collision detection between the robot and moving obstacles” [25]. At each time-step in the optimisation a binary cost is allocated for every moving obstacle that is in collision. The reason for their approach is that EDTs are commonly considered to have CPU compute times that are not fast enough for real-time performance, thus are pre-computed and assumed to be static.

Various methods have been proposed to reduce the compute time for exact EDTs. Maurer et al. show that an exact distance transform can be calculated in linear time [17]. Approximate methods, such as the Chamfer Distance Transform (CDT) and Fast Marching Method (FMM), have also been proposed [7] to achieve fast approximations. In robotics, we often accumulate information over time; this is in contrast to areas such as medical image processing, where an ESDF will be constructed from all of the information available. This has lead to significant work being conducted into faster integration of new occupancy information into signed-distance fields. Oleynikova et al. found that Truncated Signed-Distance Fields (TSDFs) are faster to construct than Octomaps, a widely used package that maps the environment using octrees [4], and proposed, VoxBlox, a method to build an ESDF from projective TSDFs incrementally [24]. In contrast to ESDFs, TSDFs use a ‘projected distance’ metric and consider distances only within a truncated radius of surface boundaries. TSDFs are commonly used in vision for surface reconstruction [22, 32].

Despite the speed-ups achieved by state-of-the-art methods, they are not able to calculate SDFs fast enough for use in a real-time motion planner that uses what we term ‘predicted signed-distance fields’

- predictions of what the ESDF of the workspace will be for future times based on estimates of obstacle geometry, velocity, and direction. We propose using

composite signed-distance fields to generate predicted signed-distance fields in real-time.

Zucker et al. first mentioned the concept of composite SDFs, noting that distance fields are compositional under the min operation [35]. Thus the distance field computation can be reduced to a minimisation across a set of pre-computed distance field primitives. In the computer graphics community, composition of object-centric SDFs is used to unionise objects in ‘Constructive Solid Geometry’ as well as to combine the SDFs of individual objects into a single SDF of the scene for ray-marching [29].

Zucker et al. use composite SDFs in ‘CHOMP-R’ to quickly update their environment representation in response to a changing environment. However, to our knowledge, they have not been used in a predictive manner. We provide further analysis of this technique and leverage it in our novel framework which extends it to the domain of integrating predicted object trajectories into motion planning.

To demonstrate our framework, we integrate it with GPMP2; GPMP2 uses Gaussian Processes (GPs) to represent continuous-time trajectories with a small number of states and uses the GTSAM framework to exploit sparsity in the problem and perform fast probabilistic inference on factor graphs [20].

Iii Methods

Part III-A describes how we identify and segment moving obstacles from the static scene. For each moving obstacle, we infer its velocity and propagate the trajectories forward to incorporate predicted obstacle motions into the motion planning problem. This methodology enables us to plan in environments for which we have no prior knowledge of the obstacles that we will encounter.

Part III-B details our novel framework for predicting environment representations, using composite signed-distance fields, and then outlines how they can be integrated with a motion planner. We describe an experiment to examine the speed-up attained by using composite signed-distance fields, in contrast to exact computations.

In Part III-C, we describe the dynamic obstacles factors that we implement to adapt the GPMP2 motion planner to react in dynamic environments. We demonstrate its applicability by considering and comparing three scenarios of varying levels of perception.

In Part III-D, we demonstrate our fully integrated framework in a Gazebo simulation in which a 7-DoF Panda arm avoids a moving obstacle without prior knowledge of its shape or trajectory.

Iii-a Obstacle Trajectory Prediction

We consider a simple prediction strategy for the occupancy of the workspace. Consider a 3D workspace, , which comprises of a set of stationary objects, , and moving (dynamic) objects, . At a given time, , a moving object with a centroid position, , will travel at velocity . We write the collection of positions and velocities as and , where


In our simulated experiments, we assume noiseless sensor data and subsequently an accurate occupancy grid of the workspace, , at each time-step. At any given time, we can identify and isolate occupied regions of the workspace. From these regions, we calculate the centroid and list of voxels associated with each object. By comparing two successive frames, at times and , we identify which objects are moving and estimate their respective velocities from the motion of each centroid—this enables us to identify occupancy grids associated with the static scene, , and the moving obstacles, .

With centroid and velocity information, we propagate each object using a constant-velocity model to produce a predicted trajectory of the centroid. This method inherently gives accurate trajectory predictions when an object is moving in a straight line with constant velocity. However, we emphasise that this module is fully replaceable and that a more sophisticated object tracking and trajectory prediction can be implemented, e.g. a KLT tracker or Unscented Kalman Filter

[6, 27].

Iii-B Composite Signed-Distance Field Generation

We now describe our proposed framework of using composite signed-distance fields to generate fast SDF predictions of the environment at later times.

Building on the previous description of how to identify the static and moving parts of the scene, we use to calculate the static SDF, .

Next, we consider an occupancy box, , around each of the moving objects, , that we have identified in . Each occupancy box is chosen to be of equal size to , plus an additional in all directions. For each , we calculate the corresponding object SDF, . The resultant SDF is the smallest cuboid which encloses all voxels in 3D space with signed-distance values equal to or less than a distance away from the surface of the object. We introduce as a safety margin desired to be penalised in collision avoidance. When this method is integrated with a motion planner, should equal the desired safety margin around objects to be penalised plus the size of the largest collision sphere associated with the approximate robot model.

Similar to and

, we denote the stacked vector of moving SDFs at a given time as

. In practice, we may accumulate more information about the shape of each moving object, hence is a function of time. However, for demonstration, our simulations assume noiseless sensor data and so do not acquire additional shape information in subsequent steps; we thus refer to simply as .

Fig. 12: LABEL:fig:occupancy_t0, LABEL:fig:occupancy_t1 and LABEL:fig:occupancy_t2 depict a toy example of three moving spheres entering the workspace in which a static obstacle (large central sphere) is present. White regions in the workspace represent free space, while black regions are occupied. LABEL:fig:sdf_breakdown shows a SDF of the static environment, with the tracked objects in LABEL:fig:occupancy_t1 overlaid for illustration purposes. For each of the tracked objects in the scene, an SDF is calculated and associated with them. By tracking the positions of the moving objects, we infer their velocities in order to make predictions of their future positions. At these future positions, we can superpose the object SDFs onto the static SDF using a min operation - the result is a composite SDF. LABEL:fig:composite_sdf shows a composite SDF for . For comparison, LABEL:fig:exact_sdf shows the corresponding exact SDF. Critically for motion planning, the two are identical for distances up to away from the obstacle surface boundaries.

For any future time, , we can predict the occupancy of the workspace by propagating the current position, , of each moving obstacle SDF, , to its predicted position, using velocity . Similar to considering an occupancy grid as the superposition of all occupied regions, we consider the workspace SDF as the superposition of all object SDFs in the scene—this forms the basis of a composite SDF.

To calculate the resultant composite SDF for a given time, we take the minimum voxel value of all overlapping scene and object SDFs. By construction, the composite SDF is guaranteed to be accurate up to the distance . We illustrate this process in Fig. 12.

Algorithms 1 and 2 describe how we use this method in an update loop for motion planning with SDF predictions generated in real-time.

0:  Continuously updates motion planner Initialisation :
2:   Execution :
3:  while not shutdown do
7:     for  in  do
11:     end for
13:  end while
Algorithm 1 Real-time Update Loop
2:  for  do
5:  end for
6:  return  
Algorithm 2

To evaluate the speed-up achieved using composite SDFs, we constructed a set of four dynamic datasets modelled on the ‘table and cabinet’ setup presented in [21]. Each of the environments contains a table and cabinet with the addition of: a) one small moving box, b) two small moving boxes, c) one moving pillar, d) two moving pillars. Figure 15 shows aerial and three-quarter perspective views of the ‘single moving pillar’ setup. We also include a simpler environment consisting of an empty workspace with a large moving block.

Fig. 15: ‘Single moving pillar’ environment - Aerial and three-quarter perspective views of an example environment in which a tall pillar traverses the floor. Example start and goal configurations are depicted using a sphere approximation of the WAM robot.

For each dynamic environment, we simulate 31 different time-steps in a workspace with resolution. At each time-step, we produce composite SDF predictions for the remaining time-steps. We perform the same simulation and benchmark against using exact SDF computations. As in GPMP2, exact SDF computations are performed using MATLAB’s bwdist function on the predicted occupancy grids.

Iii-C Dynamic GPMP2

In this section, we describe our approach to adapting GPMP2 for dynamic environments and an experiment to explore the effects of including predicted obstacle trajectories into motion planning.

GPMP2 formulates motion planning as probabilistic inference on a factor graph. Collision avoidance is applied via the use of ‘obstacle factors’, which evaluate the collision-free likelihood, using a hinge-loss obstacle cost for each time-step. The hinge-loss is calculated in both CHOMP and GPMP2, by using an approximate robot model, comprising of spheres, to query the signed-distance field [28, 20]. However, Mukadam et al. use a single pre-computed SDF of the surrounding environment that is shared by all of the obstacle factors [20] and so does not incorporate the dynamics of moving obstacles. Kolur et al. consider dynamic environments but update all of the obstacle factors to use the same updated SDF and then re-optimise to solve for an updated trajectory plan [12].

We adapt the original GPMP2 implementation to enable rapid updates of the SDF associated with each obstacle factor independently. The process of quickly updating obstacle factors facilitates a motion planner that can re-optimise and adapt planned trajectories to a changing environment.

To examine the effect of using independent SDFs for each obstacle factor, and to assess the importance of incorporating obstacle dynamics, we considered and compared three different scenarios. Each scenario considers a different degree of perception, in dynamic environments; they are as follows:

  1. Static - the motion planner assumes a static workspace and all obstacle factors will remain unchanged throughout the execution of the trajectory. The factor graph is thus optimised only once.

  2. Execute and update - obstacles factors are initialised with the starting SDF of the scene. During execution, at each time-step, all obstacle factors with a time-index greater than or equal to the current time are updated to use the current observation of the workspace. The trajectory is re-optimised and executed at each time-step in an iterative process.

  3. Full prior knowledge - each obstacle factor is provided with the known SDF of the environment that will occur at the associated time. The factor graph has maximum knowledge of the environment evolution and is thus only optimised once.

The factor graphs associated with each of the perception scenarios are illustrated in Fig. 16.

To test each scenario, we used a 7-DoF WAM robot arm in simulation to plan trajectories in the ‘single moving pillar’ setup shown in Fig. 15. Plans were conducted over seven different combinations of start and goal configuration, and a range of moving obstacle speeds,   to   . We recorded the resultant smoothness (GP) cost associated with each trajectory and the number of collisions that the robot made with the environment.

To formalise the task, we constructed a factor graph comprising of 31 time-indexed variable nodes, separated by intervals, to form a time horizon. Prior factors were added to specify the start and goal configurations, and Gaussian priors used to connect variable nodes, as in [20]. We assigned an obstacle factor to each variable node and interpolated obstacle factors between pairs of variable node, separated by (). Further parameters used were and . The environment was discretised at resolution in a workspace.

We performed another similar experiment on the experimental setup shown in Fig. 4, in which a Franka Emika Panda 7-DoF robotic arm was tasked with reaching across the gap between two tables to achieve a ‘pick-up’ goal configuration. During execution, a Toyota HSR [33] traverses the walkway between the tables, acting as a moving obstacle. For this setup, we used 42 different combinations of start and goal configuration. The moving obstacle speed was chosen to be in the range of   to   , most closely resembling walking speeds of a human. Parameters used were (), and . As before, the environment was discretised at resolution in a workspace.

Fig. 16: Factor graphs associated with three different feedback scenarios. Variable nodes are shown in white, prior factors in black, and coloured obstacle factors. Top - the obstacle factors share the same SDF and remain unchanged during execution. Middle - as the trajectory is executed, the current and all future obstacle factors are replaced with the latest SDF. Bottom - as the trajectory is executed, the current obstacle factor is updated to the latest observed SDF while all future obstacle factors receive updated SDF predictions.

Iii-D Closed-loop Implementation

We combined our predictive framework using composite signed-distance fields with our dynamic approach, applied within GPMP2, to solve planning problems in dynamic environments. The control loop is shown in Algorithm 1. In each iteration of the control loop, we observe the environment and update the workspace occupancy grid. We populate the occupancy grid by tracking the position of model states in the Gazebo simulation and inserting occupied regions into the occupancy grid at corresponding locations. We use this method because to calculate signed distance fields appropriate for motion planning, we require solid occupied regions, rather than the occupancy of object surfaces as provided by packages such as OctoMap.

Using the methods discussed in Section III-A, we segment and track obstacles online. A constant velocity model is applied to each of the segmented moving objects, enabling us to predict the future locations of each object and generate composite SDFs for all obstacle factors with time-index equal to or greater than the current time. A prior factor is also added for the current time-index to set the current configuration of the robot. The factor graph is then re-optimised and the new trajectory is merged with the current trajectory being executed on the robot [1].

We demonstrate our closed-loop implementation of the proposed framework on a Panda 7-DoF robotic arm in Gazebo [10], using the same task as described previously in which the Panda reaches across a gap to place the end-effector over a table, as in a pick-and-place task. We set the HSR base movement speed at to test against a speed comparable with the average walking pace [3, 23, 31].

Iv Evaluation

Workspace Size (Voxels Per Side)
64 96 128 160 192 224 256 288 320
Full Computation (ms)
Composite Initialisation (ms)
[Ours] Composite Prediction (ms)
Repeat Prediction Speed-Up 15.5x 28.0x 15.5x 16.4x 9.0x 8.0x 8.0x 8.0x 7.9x
TABLE I: Signed-Distance Field Timing Analysis

We conducted all experiments using an Intel Core i7-9700 CPU (8 cores, Cache, ) and RAM (DDR4, ). Signed-distance field computations were performed in series, as in the original GPMP2 implementation, using the bwdist function in MATLAB (R2020a) [16]. GTSAM was compiled with multi-threading enabled and is used when linearising the factor graph.

Iv-a Composite SDF Prediction

Computation times for composite signed-distance fields and full computation from occupancy grids are shown in Table I.

The composite initialisation time comprises of the time to calculate the SDFs associated with the static workspace, , and each of the moving obstacles in the scene, . Our results show that it is significantly faster to generate a composite SDF than perform a full SDF computation for further predictions; we see a   to   speed-up for subsequent predictions depending on the discretisation of the workspace. This has a significant impact in enabling SDF predictions to be generated in real-time—conducting our experiment for a discretisation of the workspace, as in our closed-loop implementation, gives a mean SDF generation rate of , compared with for the benchmark.

Iv-B Dynamic GPMP2

Fig. 21: Results for experiments conducted on the WAM arm in the ‘single moving pillar’ setup are shown in Figures LABEL:fig:wam_traj_costs and LABEL:fig:wam_traj_collisions. Results for the Panda arm avoiding a moving HSR are shown in LABEL:fig:hsr_traj_costs and LABEL:fig:hsr_traj_collisions. We see that in all cases, using full knowledge of obstacle trajectories provides at least an equal chance of finding a collision-free trajectory using GPMP2; for most obstacle speeds, the improvement is significant.

The performance of the three scenarios described in Section III-C, on the ‘single moving pillar’ dataset, are presented in Fig. 21, excluding results for which none of the planning scenarios found a collision-free trajectory. The figure shows an absence of data for the obstacle speed of ; at , the obstacle intersects the goal configurations at the end of the time horizon, resulting in inevitable collisions. Therefore, we exclude results for from our analysis.

Our results strongly support the incorporation of future obstacle motions into motion planning. Figure 21 shows that in most cases, including the obstacle trajectory motion into the planning significantly increases the likelihood of both finding a collision-free trajectory and reducing the jerk associated with the trajectory. Furthermore, the results motivate the use of our proposed framework in which we generate predicted ESDFs in real-time.

We note here that our implementation of a dynamic obstacle factor in GPMP2 incurs a cost of to be replaced by an obstacle factor with a different SDF.

Iv-C Closed-Loop Implementation

Our closed-loop implementation was successful in providing re-optimised trajectories fast enough for a Panda arm to avoid a moving HSR robot while reaching across a gap between two tables. Figure 4 illustrates the trajectory taken in each of the three cases. The ‘static’ case would result in a collision and the ‘execute and update’ would exhibit erratic movements. Figure 24 shows a comparison of the resultant joint position trajectories for the ‘execute and update’ against our composite prediction method. The results lend further support that by including predicted SDFs into motion planning, we can obtain smoother robot trajectories.

This experiment was conducted using a workspace with resolution. The parameters for the motion planner were , , . A time horizon of was given, with variable factors time-indexed in steps of , resulting in 31 support states. The first iteration of the control loop results in the slowest update frequency because more future factors need to be updated and more predictions are generated. Averaged over 100 runs, our implementation runs at

in the early iterations, with a standard deviation of

. In the later iterations, we achieve update frequency . We demonstrate the resultant trajectories for each of the three cases in the accompanying video.111

Fig. 24: Example trajectories for joint position and velocity. Our results highlight that the inclusion of environment prediction in motion planning can lead to smoother trajectories and exhibit less erratic movements.
Fig. 25: MATLAB code profiling for the first iteration of the update loop (averaged over ten trials). A significant amount of time is spent on publishing the resultant trajectory and re-formatting the signed-distance fields for use in the GPMP2 data structure; we believe that these time costs could be further optimised by moving to a ‘non-MATLAB’ implementation and initialising the SDF structure in parallel.

V Discussion

Our proposed framework gives compelling results in support of incorporating predicted obstacle trajectories into motion planning. In this work, we did not explore the avoidance of obstacles that follow curved trajectories and leave this for later work. However, we emphasise that while we implemented a constant velocity, one can replace this with a more complex model that takes into account any prior information we have on the obstacle’s trajectory, including its history.

A disadvantage of the proposed method is the potentially large memory requirements associated with assigning and storing different signed-distance fields for different time-indexed obstacle factors. For example, if a discretisation of the workspace is required for an application, our previous problem of a time horizon, discretised into 31 time-steps, will require RAM.222A double is stored using ytes. . We thus encounter a trade-off; minimising the workspace size results in quicker computation and lower memory requirements, however, to monitor a larger workspace requires the resolution to be decreased. We believe that this method could be adapted in future work to perform the superposition on a query basis, rather than computing the composite signed-distance field for the entire workspace.

Generating composite predictions for each time-step, as well as the updating of the obstacle factors, are operations which seem suitable for parallelisation. By updating factors in parallel, we believe that significantly higher update rates could be achieved. Although our implementation was effective in experiments, we believe that a substantial cost overhead was incurred by using MATLAB to interface between GPMP2 and ROS. We conducted code profiling over ten repeats of the Panda arm experiment and present the results in Fig. 25; a significant amount of time is spent on publishing the trajectory to the controller and copying SDF values between data structures.

As mentioned in Section III-D, in our implementation, we update the occupancy grid by ‘listening’ to the position of model states (the moving obstacle) in Gazebo. Our reasoning for is that to generate signed-distance fields suitable for motion planning, the occupancy grid from which they are constructed must comprise of solid objects rather than shells. Implementing an off-the-shelf mapping tool, such as OctoMap, would use the point cloud data to allocated occupied voxels at the surface boundaries of objects, but not the inside. In future work, we will further investigate solutions to this and extend our module to use live point-cloud data.

Vi Conclusion

This paper explored the application of composite signed-distance fields to motion planning in dynamic environments. We first exploited the speed of the min operation for composition of SDFs, and secondly, that signed-distance field representations must only be accurate up to a specified for motion planning problems. We show that composite SDFs can be used to provide significant speed-up for generating SDFs when accounting for moving obstacles. We investigated motion planning with a GPMP2 implementation which uses dynamic obstacles factors, enabling the planner to account for moving obstacles. Over a range of tasks and two different robot platforms, our results show that by incorporating predicted obstacle trajectories, we can significantly reduce the smoothness cost of trajectories and the rate of collisions. We leveraged composite SDFs and a dynamic GPMP2 implementation to present a novel framework which exploits the sparsity of the workspace and the compositional nature of signed-distance fields to generate real-time predictions of the workspace SDF, predicted signed-distance fields. We verified our approach on a 7-DoF Panda arm, demonstrating that it can plan trajectories in dynamic environments and successfully avoid moving obstacles.


  • [1] S. Chitta, E. Marder-Eppstein, W. Meeussen, V. Pradeep, A. Rodríguez Tsouroukdissian, J. Bohren, D. Coleman, B. Magyar, G. Raiola, M. Lüdtke, and E. Fernández Perdomo (2017-12) Ros_control: a generic and simple control framework for ROS.

    The Journal of Open Source Software

    2, pp. 456.
    External Links: Document, Link Cited by: §III-D.
  • [2] P. Hart, N. Nilsson, and B. Raphael (1968)

    A Formal Basis for the Heuristic Determination of Minimum Cost Paths

    IEEE Transactions on Systems Science and Cybernetics 4 (2), pp. 100–107. External Links: Document, ISSN 0536-1567, Link Cited by: §II.
  • [3] Y. Hermodsson, C. Ekdahl, B. M. Persson, and G. Roxendal (1994) Gait in male trans-tibial amputees: A comparative study with healthy subjects in relation to walking speed. Prosthetics and Orthotics International 18 (2), pp. 68–77. External Links: Document, Link Cited by: §III-D.
  • [4] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard (2013) OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees. Autonomous Robots. External Links: Document, Link Cited by: §II.
  • [5] D. Hsu, R. Kindel, J. C. Latombe, and S. Rock (2002) Randomized kinodynamic motion planning with moving obstacles. The International Journal of Robotics Research 21 (3), pp. 233–255. External Links: Document, ISSN 02783649, Link Cited by: §II.
  • [6] Jianbo Shi and Tomasi (1994) Good features to track. In Proc. IEEE CVPR, pp. 593–600. External Links: Document, ISBN 0-8186-5825-8, Link Cited by: §III-A.
  • [7] M.W. Jones, J.A. Baerentzen, and M. Sramek (2006-07) 3D distance fields: a survey of techniques and applications. IEEE Transactions on Visualization and Computer Graphics 12 (4), pp. 581–599. External Links: Document, ISSN 1077-2626, Link Cited by: §II.
  • [8] L.E. Kavraki, M.N. Kolountzakis, and J.-C. Latombe (1998-02) Analysis of probabilistic roadmaps for path planning. IEEE Transactions on Robotics and Automation 14 (1), pp. 166–171. External Links: Document, ISSN 1042-296X, Link Cited by: §II.
  • [9] L.E. Kavraki, P. Svestka, J.-C. Latombe, and M.H. Overmars (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation 12 (4), pp. 566–580. External Links: Document, ISSN 1042296X, Link Cited by: §II.
  • [10] N. Koenig and A. Howard (2004) Design and use paradigms for gazebo, an open-source multi-robot simulator. In Proc. IEEE/RSJ IROS, Vol. 3, pp. 2149–2154 vol.3. Cited by: §III-D.
  • [11] S. Koenig and M. Likhachev (2002) Improved fast replanning for robot navigation in unknown terrain. In Proc. IEEE ICRA, Vol. 1, pp. 968–975. External Links: Document, ISBN 0-7803-7272-7, Link Cited by: §II.
  • [12] K. Kolur, S. Chintalapudi, B. Boots, and M. Mukadam (2019-11) Online Motion Planning Over Multiple Homotopy Classes with Gaussian Process Inference. In Proc. IEEE/RSJ IROS, pp. 2358–2364. External Links: Document, 1908.00641v1, ISBN 978-1-7281-4004-9, Link Cited by: §II, §III-C.
  • [13] J.J. Kuffner and S.M. LaValle (2000) RRT-connect: An efficient approach to single-query path planning. In Proc. IEEE ICRA, Vol. 2, pp. 995–1001. External Links: Document, ISBN 0-7803-5886-4, Link Cited by: §II.
  • [14] S. M. LaValle (1998) Rapidly-exploring random trees : a new tool for path planning. Technical report Iowa State University. Cited by: §II.
  • [15] S. Li and J. A. Shah (2019-05) Safe and efficient high dimensional motion planning in space-time with time parameterized prediction. In Proc. IEEE ICRA, pp. 5012–5018. External Links: Document, ISBN 9781538660263, ISSN 10504729, Link Cited by: §II.
  • [16] MATLAB (2020) MATLAB version (r2020a). The MathWorks Inc., Natick, Massachusetts. Cited by: §IV.
  • [17] C. R. Maurer, R. Qi, and V. Raghavan (2003) A linear time algorithm for computing exact Euclidean distance transforms of binary images in arbitrary dimensions. IEEE Transactions on Pattern Analysis and Machine Intelligence 25 (2), pp. 265–270. External Links: Document, ISSN 01628828, Link Cited by: §II.
  • [18] A. Menon, B. Cohen, and M. Likhachev (2014-05) Motion planning for smooth pickup of moving objects. In Proc. IEEE ICRA, Vol. 25, pp. 453–460. External Links: Document, ISBN 978-1-4799-3685-4, ISSN 00207683, Link Cited by: §II.
  • [19] W. Merkt, V. Ivan, and S. Vijayakumar (2019-11) Continuous-Time Collision Avoidance for Trajectory Optimization in Dynamic Environments. In Proc. IEEE/RSJ IROS, pp. 7248–7255. External Links: Document, ISBN 978-1-7281-4004-9, Link Cited by: §II.
  • [20] M. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots (2018) Continuous-time Gaussian process motion planning via probabilistic inference. The International Journal of Robotics Research 37 (11), pp. 1319–1340. External Links: Document, Link Cited by: §I, §II, §II, §III-C, §III-C.
  • [21] M. Mukadam, X. Yan, and B. Boots (2016-05) Gaussian Process Motion planning. In Proc. IEEE ICRA, pp. 9–15. External Links: Document, ISBN 978-1-4673-8026-3, Link Cited by: §III-B.
  • [22] R. A. Newcombe, A. Fitzgibbon, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison, P. Kohi, J. Shotton, and S. Hodges (2011-10) KinectFusion: Real-time dense surface mapping and tracking. In Proc. IEEE ISMAR, pp. 127–136. External Links: Document, ISBN 978-1-4577-2183-0, Link Cited by: §II.
  • [23] C. M. Nolan, S. S. C. Kon, S. Patel, S. E. Jones, R. E. Barker, M. I. Polkey, M. Maddocks, and W. D. Man (2018) Gait speed and pedestrian crossings in COPD. Thorax 73 (2), pp. 191–192. External Links: Document, ISSN 0040-6376, Link Cited by: §III-D.
  • [24] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto (2017) Voxblox: Incremental 3D Euclidean Signed Distance Fields for on-board MAV planning. In Proc. IEEE/RSJ IROS, Vol. 2017-Sept, pp. 1366–1373. External Links: Document, 1611.03631, ISBN 9781538626825, ISSN 21530866, Link Cited by: §II.
  • [25] C. Park, J. Pan, and D. Manocha (2012) ITOMP: Incremental trajectory optimization for real-time replanning in dynamic environments. In Proc. Int. Conf. on Automation Planning and Scheduling, pp. 207–215. External Links: ISBN 9781577355625, Link Cited by: §I, §II, §II.
  • [26] J. S. Park, C. Park, and D. Manocha (2019-01) I-Planner: Intention-aware motion planning using learning-based human motion prediction. The International Journal of Robotics Research 38 (1), pp. 23–39. External Links: Document, ISBN 9780992374730, ISSN 0278-3649, Link Cited by: §II.
  • [27] A. Raj, A. Sivaraman, C. Bhowmick, and N. K. Verma (2016-12) Object tracking with movement prediction algorithms. In Proc. IEEE ICIIS, pp. 285–290. External Links: Document, ISBN 978-1-5090-3818-3, Link Cited by: §III-A.
  • [28] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa (2009-05) CHOMP: Gradient optimization techniques for efficient motion planning. In Proc. IEEE ICRA, pp. 489–494. External Links: Document, ISBN 978-1-4244-2788-8, Link Cited by: §II, §III-C.
  • [29] T. Reiner, G. Mückl, and C. Dachsbacher (2011) Interactive modeling of implicit surfaces using a direct visualization approach with signed distance functions. Technical report Technical Report 3, Vol. 35, Karlsruhe Institute of Technology. External Links: Document, ISSN 00978493, Link Cited by: §II.
  • [30] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. External Links: Document, Link Cited by: §II.
  • [31] Z. Wang, R. Liu, X. Wu, and Z. Liu (2019) Estimation Models for the Safety Level of Indoor Space Pedestrian Flows. pp. 9–18. External Links: Document Cited by: §III-D.
  • [32] T. Whelan, M. Kaess, and M. Fallon (2012) Kintinuous: Spatially extended kinectfusion. RSS Workshop on RGB-D: Advanced Reasoning with Depth Cameras, pp. 7. External Links: Link Cited by: §II.
  • [33] T. Yamamoto, T. Nishino, H. Kajima, M. Ohta, and K. Ikeda (2018) Human Support Robot (HSR). In Proc. ACM SIGGRAPH 2018 Emerging Technologies, SIGGRAPH ’18, New York, NY, USA. External Links: ISBN 9781450358101, Link, Document Cited by: §III-C.
  • [34] Yun Seok Nam, Bum Hee Lee, and Moon Sang Kim (1996) View-time based moving obstacle avoidance using stochastic prediction of obstacle motion. In Proc. IEEE ICRA, Vol. 2, pp. 1081–1086. External Links: Document, ISBN 0-7803-2988-0, Link Cited by: §II.
  • [35] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa (2013-08) CHOMP: Covariant Hamiltonian optimization for motion planning. The International Journal of Robotics Research 32 (9-10), pp. 1164–1193. External Links: Document, ISSN 0278-3649, Link Cited by: §II, §II.