PaperlayerView exact revision on arXiv

Source-linked AI summary

MATS: An Interpretable Trajectory Forecasting Representation for Planning and Control

Boris Ivanovic, Amine Elhafsi, Guy Rosman, Adrien Gaidon, Marco Pavone

arXiv:2009.07517v2cs.ROcs.HCcs.LGcs.SYeess.SY

TL;DR

Many planning and control algorithms reason about system dynamics, while trajectory forecasting commonly outputs tracklets, complicating integration. MATS represents forecasts as mixtures of affine time-varying systems and pairs them with multimodal planning. The authors report more efficient planning than a tracklet-based method, with a minor regression in raw prediction performance.

  • Problem

    Many planning and control algorithms reason about system dynamics rather than the agent tracklets commonly produced by trajectory forecasting, complicating integration.

    Source: §1 · p. 1

  • Method

    MATS represents forecasts as mixtures of affine time-varying systems, and its planner optimizes multiple control sequences across agent evolution modes.

    Source: §1 · p. 1, §5.2 · p. 5

  • Results

    MATS yielded plans more efficiently than a recent tracklet-based method on nuScenes, with a minor regression in raw prediction performance.

    Source: §1 · p. 1, §7 · p. 8

  • Takeaways & Limitations

    Because MATS predictions change with candidate ego controls, a planner can explore how those controls affect predicted agent responses without querying a tracklet predictor for every tentative plan.

    Source: §5.2 · p. 5

    The planner does not guarantee safe behavior under uncertain predictions, and finite-horizon planning does not guarantee feasibility.

    Source: §5.2 · p. 5

Abstract

from arXiv · show

Reasoning about human motion is a core component of modern human-robot interactive systems. In particular, one of the main uses of behavior prediction in autonomous systems is to inform robot motion planning and control. However, a majority of planning and control algorithms reason about system dynamics rather than the predicted agent tracklets (i.e., ordered sets of waypoints) that are commonly output by trajectory forecasting methods, which can hinder their integration. Towards this end, we propose Mixtures of Affine Time-varying Systems (MATS) as an output representation for trajectory forecasting that is more amenable to downstream planning and control use. Our approach leverages successful ideas from probabilistic trajectory forecasting works to learn dynamical system representations that are well-studied in the planning and control literature. We integrate our predictions with a proposed multimodal planning methodology and demonstrate significant computational efficiency improvements on a large-scale autonomous driving dataset.

1 Introduction

Trajectory forecasting commonly outputs agent tracklets, while many planning and control algorithms reason about system dynamics, complicating integration. MATS is proposed as a forecasting representation that improves planning and control efficiency with a minor regression in raw prediction performance.

1 Introduction

  • Many forecasting methods output trajectories or distributions of agent tracklets, whereas many planning and control algorithms reason about system dynamics.The authors note that sampling trajectory distributions for rare events can be too expensive in fixed-computation, safety-critical settings such as autonomous driving.Source: §1 · p. 1
  • MATS represents trajectory forecasts with a linear-affine dynamical structure intended to integrate more directly with downstream planning and control.The authors report reduced planning and control complexity, including with multimodal predictions, at the cost of a minor regression in raw prediction performance.Source: §1 · p. 1

2 Related Work

Prior forecasting and planning approaches often provide tracklets or cost volumes, which do not directly capture how other agents respond to the ego-vehicle’s future actions. MATS instead provides an explicit dynamical representation for planning and control.

2 Related Work

  • NMP uses a cost volume with a sampling-based planner, but its predictions do not account for the environment’s dynamic response to ego-vehicle actions.MATS is presented as enabling direct optimization of ego controls while predicting trajectories that account for ego motion and agent-agent interactions.Source: §2 · p. 2
  • Many forecasting methods produce tracklets, and multimodal prediction distributions can be complex to incorporate into downstream planning.The authors note that these integration complexities have limited the wider robotics community’s use of state-of-the-art trajectory forecasting methods.Source: §2 · p. 2
  • The proposed planner optimizes ego controls using future-conditional MATS predictions, following the control-space planning approach described in prior work [20].Position-space planning that avoids predicted agent locations is another approach identified in the passage.Source: §2 · p. 2
  • Unlike latent-dynamics approaches such as Neural Relational Inference [24], MATS produces an explicit physical dynamical model in the original state space.The authors describe this representation as more interpretable and easier to integrate with downstream planning and control algorithms.Source: §2 · p. 2

3 Problem Formulation

The problem is to predict future states jointly for a time-varying set of interacting agents from their recent histories. The prediction is also conditioned on the ego-agent’s future motion plan for integration with planning and control.

3 Problem Formulation

  • The scene may contain a time-varying number N(t) of interacting agents, each with a semantic class and state.Examples of semantic classes include cars, buses, and pedestrians.Source: §3 · p. 2
  • The formulation seeks a distribution over all agents’ future states for the next T timesteps, given their state histories over the previous H timesteps.The passage defines the joint state as the concatenation of all agent states, with full dimension F equal to the sum of their individual dimensions.Source: §3 · p. 2
  • The prediction is conditioned on the ego-agent’s future motion plan, yielding p(y | x, uR) for use in evaluating motion primitives or optimizing a plan.The text uses ego-agent, ego-robot, and ego-vehicle to refer to the robot being controlled.Source: §3 · p. 3

4 Trajectory Forecasting with MATS

MATS predicts with mixtures of affine time-varying systems, combining learned agent-interaction components with known dynamics. Its graph-based recurrent model conditions on agent histories and the ego-robot’s future plan to produce multimodal, interaction-aware forecasts.

4 Trajectory Forecasting with MATS

  • Affine time-varying systems provide a planning- and control-friendly representation that can capture nonlinear effects through time-varying matrices and an affine term.The text identifies straightforward integration with planning and control as a reason for choosing this representation.Source: §4 · p. 3
  • The model represents agents and interactions as a directed complete spatiotemporal graph, with each edge encoder-decoder producing system submatrices that are stacked into an overall system.Edge encoders use the connected agents’ state histories and the ego-robot’s future motion plan as inputs.Source: §4 · p. 3
  • A bidirectional LSTM encodes the ego-agent’s future control plan so predictions can reflect candidate ego motion.The passage describes this capability as useful for evaluating candidate plans while accounting for potential responses from other agents.Source: §4 · p. 3
  • A categorical latent variable selects a scene-level behavior mode, and the model combines the mode-conditioned system distributions according to their likelihoods.Using a shared mode for the predicted system is intended to yield self-consistent predictions across agents.Source: §4 · p. 3
  • Each mode-conditioned component is linear Gaussian, with uncertainty supplied by w(t), so its future-state distribution can be obtained by iteratively evaluating the system.The passage states that w(t) follows a standard normal distribution.Source: §4 · p. 3
  • Known dynamics fill agent-specific diagonal blocks and affine terms, while learned off-diagonal blocks capture inter-agent effects and learned control blocks model the ego’s influence on others.The ego-agent’s own dynamics and control matrix are determined by the system structure rather than learned.Source: §4 · pp. 3–4
  • The model supports a time-varying agent count, recreating the graph at each timestep while assuming agents present now remain through the prediction horizon.Past trajectories of varying lengths can be encoded with LSTMs.Source: §4 · p. 4
  • Training maximizes a discrete InfoVAE objective, with a training-only posterior encoder for the latent variable and weight sharing to limit parameters.The posterior encoder is a bidirectional LSTM with 32 hidden dimensions; state-history LSTMs share weights within agent types.Source: §4 · p. 4

5 Incorporating MATS Predictions in Motion Planning

The section extends MPC to use multimodal MATS predictions for interaction-aware motion planning. It coordinates mode-specific controls over a shared near-term horizon while allowing distinct futures afterward.

5 Incorporating MATS Predictions in Motion Planning

  • The section motivates using ATV dynamics because they fit existing planning algorithms while representing agent-agent interactions.The proposed method extends MPC to use multimodal MATS predictions for interaction-aware robot motion planning.Source: §5 · p. 4

5.1 Model Predictive Control

  • MPC optimizes a finite-horizon control sequence to minimize a designer-specified objective while satisfying system dynamics and control constraints.It is applied in receding-horizon fashion: compute an open-loop sequence, apply its first action, then re-optimize from the new state.Source: §5.1 · p. 4

5.2 Planning with MATS

  • The planner optimizes parallel control sequences for different agent-evolution modes, constraining them to agree over a consensus horizon so the output is actionable.This shared near-term action is intended to accommodate any predicted future mode while allowing mode-specific plans afterward.Source: §5.2 · p. 5
  • The planning formulation includes all agents’ states and mode-indexed dynamics, while the robot’s controls are the only controls used to act on the system.States, controls, and constraints are indexed by the latent mode z.Source: §5.2 · p. 5
  • MATS-based planning lets the optimizer explore robot controls and their effects on other agents’ responses, unlike static tracklet predictions that cannot change post-hoc.Tracklet planners may need to query the prediction model for every tentative robot control, which may be infeasible for real-time use.Source: §5.2 · p. 5
  • The current formulation weights prediction modes equally; using mode likelihoods or state uncertainty in planning is left for future work.The authors describe likelihood-weighted planning costs and uncertainty-informed collision-avoidance bounds such as chance constraints as possible extensions.Source: §5.2 · p. 5
  • The planner does not guarantee safe behavior under uncertain predictions or feasibility over its finite horizon; lower-level reachability control could add hard safety constraints.The MPC structure permits including lower-level reachability terms or constraints.Source: §5.2 · p. 5

6 Experimental Results

The evaluations pair a controlled nonlinear two-particle system with nuScenes forecasting and an MPC intersection planner, testing MATS for modeling and downstream planning use. They report competitive forecasting results and substantially more efficient planning than a tracklet-based method.

6.1 Two Particle System

  • In the nonlinear Social Forces test, the ego-to-agent A block activates during interaction and dissipates afterward, encoding velocity changes that move the agent away.The evaluation used two double-integrator agents interacting through a force proportional to inverse-square distance.Source: §6.1 · p. 6

6.2 nuScenes Dataset

  • On nuScenes, MATS performs similarly to Trajectron++ for pedestrians and better than most methods at early vehicle timesteps, with vehicle performance dropping at later timesteps.The comparison uses Final Displacement Error; the table reports results for vehicles and pedestrians across time.Source: Table 1 · p. 7, §6.2 · p. 7
  • Ego influence on other vehicles peaks mid-intersection, as indicated by B-matrix block norms; pedestrian influence remains equal through time.The norms correspond to the most-likely mode.Source: §6.2 · p. 7, Figure 4 · p. 8

6.3 Interaction-Aware Motion Planning

  • The MPC planner safely and smoothly crossed an intersection; QP solves averaged 50 ms, while trajectory forecasting averaged 200 ms on a 2.9 GHz dual-core Intel i5 CPU.The planner used three parallel predicted-mode horizons.Source: §6.3 · pp. 7–8
  • In 0.25 s, MATS planned for 24 interacting agents and 552 directed interactions, while TBPP planned for one agent and generated nearly 100,000 tracklets.TBPP used two rounds of inference and beam search; MATS queried its prediction model once per planning step on a CPU.Source: §6.3 · p. 8

7 Conclusion

The conclusion presents MATS as a dynamic multimodal forecasting representation paired with a planner for efficient interaction-aware motion planning. Its nuScenes demonstration motivates future work on environment-aware forecasts, richer probabilistic reasoning, safety, and hardware validation.

7 Conclusion

  • Unlike static tracklet distributions, dynamic MATS predictions let a planner evaluate the effects of ego-control sequences after prediction.The paper reports demonstrating prediction and planning efficacy on nuScenes, with plans produced more efficiently than by a recent tracklet-based method.Source: §7 · p. 8
  • Future work includes environment-aware MATS, more sophisticated probabilistic reasoning and safety considerations, and experimental verification on real-world hardware.Source: §7 · p. 8

A Additional Training Information

The appendix gives training settings for the particle-system experiment and broader optimizer choices.

A Additional Training Information

  • Both datasets used Adam with exponential learning-rate decay to 10^-5; β was sigmoid-annealed, and α was set to 1.0.The decay factor was γ = 0.9999 per optimizer step.Source: A Additional Training Information · p. 11

A.1 Two Particle System

  • The particle-system model was trained for 100 epochs with an initial learning rate of 0.001.Source: §A.1 · p. 11

A.2 nuScenes Dataset

The nuScenes appendix describes the prediction window, data splits, and scene-rotation augmentation used for training.

A.2 nuScenes Dataset

  • The model predicted six timesteps (3 s) from eight observed timesteps (4 s), using nuScenes challenge splits for training, tuning, and validation evaluation.Training used 10 epochs and an initial learning rate of 0.002.Source: §A.2 · p. 11
  • To reduce overfitting to environment-specific characteristics, each scene’s trajectories were rotated around the origin from 0° to 345° in 15° intervals.Source: §A.2 · p. 11

B Motion Planner Implementation Details

The planner uses an MPC optimization to track the ego-vehicle’s reference path while avoiding predicted agents, incorporating vehicle dynamics and predictions from the three most probable MATS modes.

Tracking Error

  • The planner tracks a reference path using contouring and lag errors, with their nonlinear forms linearized at each sampling time for a quadratic objective.Initial nominal points are evenly spaced along the path at constant velocity; later iterations use the previous solution.Source: B Motion Planner Implementation Details · p. 11

Ego-Vehicle Dynamics

  • The ego-vehicle is modeled as a dynamically extended unicycle with position, heading, and velocity states, and heading-rate and acceleration controls.The discrete-time dynamics assume piecewise-constant controls over each sampling interval.Source: B Motion Planner Implementation Details · p. 12

Reference Path

  • Path dynamics approximate the ego-vehicle’s arc-length parameter with first-order integration, treating path progress as a state and projected path velocity as a control input.The reference path is parameterized by arc length and fitted as a third-order spline to the lane centerline.Source: B Motion Planner Implementation Details · pp. 11–12

Collision Avoidance

  • Obstacle avoidance uses half-plane constraints based on each agent’s predicted mean position and the normal direction from that agent to the ego-vehicle.Constraints apply to every agent and sampling time in the planning horizon, with a distance margin.Source: B Motion Planner Implementation Details · p. 12

Planning Problem

  • The MPC plans ego-vehicle trajectories to follow the current lane while avoiding agents predicted under the three most probable modes.Its objective includes a reward for progress along the path and a penalty on control smoothness.Source: B Motion Planner Implementation Details · pp. 12–13

Planning Details

  • Planning uses a 3-second horizon with 0.25-second sampling, a 1-second consensus horizon, and bounded steering, acceleration, and velocity.The stated bounds are −0.7 to 0.7 rad/s for steering rate, −5 to 4 m/s2 for acceleration, and 0.05 to 12 m/s for velocity.Source: B Motion Planner Implementation Details · p. 13

C Additional Frobenius Norm Visualizations

In the illustrated intersection scene, the most likely mode shows time-varying interaction patterns for five vehicles, while pedestrians interact weakly with the ego-vehicle.

Additional Frobenius Norm Visualizations

  • Five vehicle submatrices show temporal patterns as the ego-vehicle crosses the intersection, whereas pedestrian agents interact weakly and act independently of its motion.The pedestrian examples are walking on the sidewalk or waiting at a crosswalk.Source: C Additional Frobenius Norm… · p. 13
  • Figure 6 shows B(t)z matrices over time for the most likely mode, with separate blocks for the ego-robot and each modeled agent.The right panel shows each non-robot agent block’s Frobenius norm; the matrix visualization is normalized so the largest norm is 1.Source: Figure 6 · p. 14, C Additional Frobenius Norm… · p. 14
Loading 2009.07517v2…