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 dynamics-aware 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 dynamics-aware, 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.
Introduction
In the field of robotics, control systems are typically designed using a layered architecture. A high-level planner generates a trajectory based on a simplified model of the environment and robot, while a low-level controller attempts to track that path using the full system dynamics. However, this separation often creates a "feasibility gap." When a planner ignores physical constraints—such as motor saturation or the inherent limitations of underactuated systems—it may request maneuvers that the robot cannot physically execute, leading to system failure.
Bridging this gap traditionally requires significant manual tuning and "engineering intuition" to refine the simplified models used for planning. This paper proposes a data-driven alternative: a framework that allows a robot to learn its own dynamic limitations and incorporate that knowledge directly into the planning process.
Methodology: Dynamics-Aware Planning
The researchers derive their approach by treating the layered control architecture as a mathematical relaxation of a global nonlinear optimal control problem.
Augmented Lagrangian Reformulation
By applying an augmented Lagrangian reformulation to the global optimization problem, the authors decompose the task into two nested layers. This decomposition introduces a "tracking penalty" regularizer into the trajectory optimization layer. This penalty represents the "cost-to-go" of the low-level controller—essentially a measure of how much the controller will struggle to follow a specific reference path.
Learning the Tracking Penalty
Since the tracking penalty for complex robotic systems rarely has a closed-form solution, the framework utilizes a Multi-Layer Perceptron (MLP) to learn it from data:
- Data Collection: The system performs rollouts using a specific feedback controller (such as a PID or SE(3) geometric controller) across various reference trajectories.
- Supervised Learning: The resulting tracking errors are recorded to train the MLP, allowing it to map specific trajectory characteristics to predicted tracking costs.
- Dynamics-Aware Inference: During the planning phase, the optimizer minimizes the task cost plus the learned tracking penalty. This forces the planner to select trajectories that it "knows" the low-level controller can feasibly handle.
Experimental Results
The framework was evaluated using both simulation and physical hardware to test its efficiency and robustness.
Unicycle and Quadrotor Simulations
In unicycle simulations, the dynamics-aware planner demonstrated a significant speed advantage, converging nearly 2x faster than the standard iterative Linear Quadratic Regulator (iLQR) algorithm (6.9s vs 11.4s). For quadrotor simulations, the learned penalty significantly reduced tracking errors compared to traditional minimum-jerk or polynomial planners by ensuring the generated paths respected the vehicle's underactuated dynamics.
Sim-to-Real Performance
A critical challenge in robotics is the "sim-to-real gap," where models trained in simulation fail in the real world due to unmodeled physics. The researchers tested their quadrotor model on a Qualcomm Snapdragon-based hardware platform (VOXL Flight - PX4). The quadrotor successfully executed complex trajectories using a penalty learned entirely in simulation, without any additional fine-tuning. This suggests that the tracking penalty implicitly captures hardware-specific constraints like latency and motor limits.
Discussion and Future Work
While the proposed framework shows significant improvements in feasibility and computation time, the authors identified several areas for future research:
- Weight Sensitivity: The performance is sensitive to the relaxation weight ($\rho$), which balances task accuracy against tracking feasibility.
- Optimization Robustness: Future work may explore Input-Convex Neural Networks (ICNNs) to ensure the planning optimization remains convex and robust.
- Onboard Computation: While trajectories were generated online, the current hardware tests relied on a remote workstation. The team aims to migrate the full dynamics-aware stack to the limited onboard compute of the robotic platform.
Conclusion
By reformulating the control stack to include a learned tracking penalty, this research provides a principled method for generating trajectories that are inherently aware of a robot's dynamic constraints. This approach reduces the reliance on manual model tuning and enables more reliable, aggressive maneuvers in underactuated robotic systems.
Summary updated