Dynamically-Consistent Trajectory Optimization for Legged Robots via Contact Point Decomposition

TL;DR

This paper introduces a dynamically consistent, phase-based trajectory optimization framework for legged robots that co-optimizes path, contact sequence, and phase durations. By decoupling translational dynamics across contact points and leveraging Bézier differentiation matrices, it derives an analytic link between position and force and enforces friction cone constraints via convex hull properties. Angular dynamics are handled on the rotation group with a tangent-space reparameterization, while dynamics are enforced across the trajectory with a single-shot translational formulation. The method demonstrates improved dynamic feasibility and tracking performance across diverse terrains compared with a baseline, at the cost of modestly higher computation time and occasional optimization failures. This approach provides a robust reference-motion generator for model-based control and learning-based planners in legged locomotion.

Abstract

To generate reliable motion for legged robots through trajectory optimization, it is crucial to simultaneously compute the robot's path and contact sequence, as well as accurately consider the dynamics in the problem formulation. In this paper, we present a phase-based trajectory optimization that ensures the feasibility of translational dynamics and friction cone constraints throughout the entire trajectory. Specifically, our approach leverages the superposition properties of linear differential equations to decouple the translational dynamics for each contact point, which operates under different phase sequences. Furthermore, we utilize the differentiation matrix of B{é}zier polynomials to derive an analytical relationship between the robot's position and force, thereby ensuring the consistent satisfaction of translational dynamics. Additionally, by exploiting the convex closure property of B{é}zier polynomials, our method ensures compliance with friction cone constraints. Using the aforementioned approach, the proposed trajectory optimization framework can generate dynamically reliable motions with various gait sequences for legged robots. We validate our framework using a quadruped robot model, focusing on the feasibility of dynamics and motion generation.

Figures (4)

• Figure 1: Overview of proposed method.
• Figure 2: The proposed TO framework generated motion on various terrains. (a) Plane is a flat surface, (b) Block represents a 0.5m step, (c) Stairs is a 2-step terrain, and (d) ChimneyLR represents continuous vertical walls inclined at 45 degrees. All robot visualizations were generated using RaiSimUnreal hwangbo2018per.
• Figure 3: The gravity-compensated linear momentum derivative and total GRF along the z-axis for trajectories generated by the proposed method and the baseline when the robot bounded 3 meters on a plane with $\tilde{\mu} = 1$. Red dots in (b) indicate the nodes where the translational dynamics are explicitly satisfied. In (a), we numerically calculated the linear momentum derivative by taking the second derivative of $m \mathbf{x}(t)$, demonstrating the feasibility of the continuity constraints. The initial and final base positions are $\mathbf{x}_{\text{init}} = [0 \text{m},\ 0 \text{m},\ 0.51 \text{m}]^{\top}$ and $\mathbf{x}_{\text{fin}} = [3 \text{m},\ 0 \text{m},\ 0.51 \text{m}]^{\top}$. The initial and final linear and angular base velocities are zero, and the base orientation is aligned with the world frame, i.e., the rotation matrix is identity at both ends.
• Figure 4: GRF profile projected onto the x-z plane (left) and x-directional force magnitude relative to the z-directional force over time (right) for the front right leg in the proposed method and the baseline. In the left graph, the black line represents the friction pyramid projected onto the x-z plane, while the red dots indicate the control points and the points where the friction cone constraints are applied in the proposed method and baseline.