A Data-Driven Approach to Synthesizing Dynamics-Aware Trajectories for Underactuated Robotic Systems

Anusha Srikanthan, Fengjun Yang, Igor Spasojevic, Dinesh Thakur, Vijay Kumar, Nikolai Matni

In the world of robotics, we often divide and conquer. We build a "stack" where a high-level planner dreams up a path, and a low-level controller tries its best to follow it. But there’s a catch: the planner usually works with a simplified model of the world, while the controller has to deal with the messy reality of physics—things like motor saturation and underactuation. When the planner asks for a maneuver the robot physically can't perform, the system fails.

A recent paper from researchers at the University of Pennsylvania proposes a elegant solution: instead of manually tuning simplified models, why not let the robot learn its own limitations and use that knowledge to plan better paths?

TL;DR

This paper introduces a data-driven framework that makes trajectory planners "dynamics-aware" by learning a tracking penalty regularizer from system rollouts. By approximating the cost-to-go of a low-level controller using a neural network, the planner can generate aggressive yet feasible trajectories that are significantly easier to track and faster to compute than traditional methods.

What Problem Does This Paper Address?

The standard "layered" control architecture relies on a fundamental mismatch. The top layer (planning) uses a reduced-order model to stay fast, while the bottom layer (tracking) uses a high-fidelity model to stay accurate. Bridging the gap between these two usually requires "engineering intuition"—essentially, humans manually tweaking parameters until the planner stops asking the robot to do the impossible.

For underactuated systems (like quadrotors, which have fewer control inputs than degrees of freedom), this mismatch is particularly punishing. If a planner ignores the robot's dynamic constraints, the resulting "optimal" path might be physically untrackable, leading to crashes or massive deviations.

Methods

The authors derive their approach by treating the layered architecture not just as a design choice, but as a mathematical relaxation of a global nonlinear optimal control problem.

  1. Augmented Lagrangian Relaxation: They take a global optimization problem and split it into two nested parts using a soft-penalty (the "tracking penalty").
  2. The Tracking Penalty as Cost-to-Go: Crucially, they identify that this penalty—which represents how much a controller will struggle to follow a specific path—can be interpreted as a "cost-to-go" function.
  3. Supervised Learning: Since this penalty rarely has a closed-form solution for complex robots, they use a Multi-Layer Perceptron (MLP) to learn it. They collect data by running a specific feedback controller (like a PID or SE(3) geometric controller) on various reference trajectories and recording the errors.
  4. Dynamics-Aware Planning: At inference time, the planner minimizes the original task cost plus the learned tracking penalty. This forces the planner to choose paths that it "knows" the low-level controller can actually handle.

Datasets

The researchers evaluated their method on two primary systems:

  • Unicycle Model: They generated a training set of 1,000 trajectories (500 "easy" ones from iLQR and 500 "hard" polynomial ones) to help the network learn the boundaries of feasibility.
  • Quadrotor (Sim & Hardware): They used 125 trajectories based on Lissajous curves to train the model.
  • Hardware Evaluation: Testing was performed on a Qualcomm Snapdragon-based quadrotor platform using the VOXL Flight - PX4 autonomy stack.

Key Insights and Ideas

  • Layering as Decomposition: The paper proves that the common "planner-tracker" split is a natural result of decomposing a global optimization problem, provided you include the right regularizer.
  • Learning the "Aha!" Moment: By training on both successful and "difficult" trajectories, the neural network learns the "landscape" of the robot's dynamics. It learns, for example, that a sharp turn at high speed has a high "cost" because the controller will inevitably drift.
  • Sim-to-Real Robustness: Because the tracking penalty captures the actual performance of the closed-loop system, it implicitly accounts for hardware-specific issues (like latency or motor limits) that are never present in a pure mathematical model.

Key Results

  • Speedup: In unicycle simulations, the dynamics-aware planner was nearly 2x faster than the standard iLQR algorithm (6.9s vs 11.4s average convergence).
  • Feasibility: The trajectories generated were significantly easier to track. For the quadrotor, the learned penalty resulted in a major reduction in tracking error compared to standard minimum-jerk or polynomial planners.
  • Hardware Success: The system handled the "sim-to-real gap" seamlessly. The quadrotor successfully flew complex trajectories on real hardware using a penalty learned entirely in simulation, without any additional fine-tuning.

Limitations and Future Work

  • Weight Sensitivity: The performance is sensitive to the relaxation weight ($\rho$). Finding the "sweet spot" between task accuracy and tracking feasibility still requires some tuning.
  • Optimization Landscape: The authors noted that MLPs don't guarantee convexity. They suggest that using Input-Convex Neural Networks (ICNNs) could make the planning optimization even more robust and faster.
  • Onboard Computation: While the trajectories were generated online, the current hardware tests involved a remote workstation. Future work aims to move the entire dynamics-aware stack onto the limited onboard compute of the drone itself.
  • Data Collection: The current method requires a pre-defined set of trajectories for training; a more "active" learning approach that explores the state space more systematically could improve the model.

Summary updated

Abstract

We consider joint trajectory generation and tracking control for under-actuated robotic systems. A common solution is to use a layered control architecture, where the top layer uses a simplified model of system dynamics for trajectory generation, and the low layer ensures approximate tracking of this trajectory via feedback control. While such layered control architectures are standard and work well in practice, selecting the simplified model used for trajectory generation typically relies on engineering intuition and experience. In this paper, we propose an alternative data-driven approach to dynamicsaware trajectory generation. We show that a suitable augmented Lagrangian reformulation of a global nonlinear optimal control problem results in a layered decomposition of the overall problem into trajectory planning and feedback control layers. Crucially, the resulting trajectory optimization is dynamicsaware, in that, it is modified with a tracking penalty regularizer encoding the dynamic feasibility of the generated trajectory. We show that this tracking penalty regularizer can be learned from system rollouts for independently-designed low layer feedback control policies, and instantiate our framework in the context of a unicycle and a quadrotor control problem in simulation. Further, we show that our approach handles the sim-to-real gap through experiments on the quadrotor hardware platform without any additional training. For both the synthetic unicycle example and the quadrotor system, our framework shows significant improvements in both computation time and dynamic feasibility in simulation and hardware experiments.
BibTeX
@inproceedings{srikanthan2023data,
  author = {Anusha Srikanthan and Fengjun Yang and Igor Spasojevic and Dinesh Thakur and Vijay Kumar and Nikolai Matni},
  title = {{A Data-Driven Approach to Synthesizing Dynamics-Aware Trajectories for Underactuated Robotic Systems}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2023},
  url = {https://openreview.net/forum?id=6ufRRRfTo5},
}

Loading related content...

Figure