Source-linked AI summary
An Efficient Optimal Planning and Control Framework For Quadrupedal Locomotion
Farbod Farshidian, Michael Neunert, Alexander W. Winkler, Gonzalo Rey, Jonas Buchli
TL;DR
The paper addresses efficient optimal planning and control for legged robots, where hybrid dynamics, constraints, and feedback requirements make computation difficult. It reformulates locomotion as switched-system optimal control, alternates optimization of switching times and continuous inputs, and introduces a continuous-time constrained SLQ method. The framework is evaluated on quadrupedal locomotion tasks including gap crossing, walking, and trotting.
Problem
Optimal planning and control for legged robots is difficult because locomotion is hybrid and nonlinear, while efficient feedback planners often struggle with high dimensionality and constraints.
Method
The framework reformulates legged locomotion as constrained switched-system optimal control, alternates optimization of switching times and continuous inputs, and uses continuous-time constrained SLQ.
Results
The framework is evaluated on a quadruped using centroidal dynamics and full kinematics for gap crossing, walking, and trotting tasks.
Takeaways & Limitations
The switched-system formulation and multi-level optimization provide an efficient framework for optimizing contact-dependent locomotion and feedback control.
Abstract
from arXiv · showhide
In this paper, we present an efficient Dynamic Programing framework for optimal planning and control of legged robots. First we formulate this problem as an optimal control problem for switched systems. Then we propose a multi--level optimization approach to find the optimal switching times and the optimal continuous control inputs. Through this scheme, the decomposed optimization can potentially be done more efficiently than the combined approach. Finally, we present a continuous-time constrained LQR algorithm which simultaneously optimizes the feedforward and feedback controller with $O(n)$ time-complexity. In order to validate our approach, we show the performance of our framework on a quadrupedal robot. We choose the Center of Mass dynamics and the full kinematic formulation as the switched system model where the switching times as well as the contact forces and the joint velocities are optimized for different locomotion tasks such as gap crossing, walking and trotting.
I. INTRODUCTION
Legged-robot motion control is difficult because locomotion is hybrid and nonlinear, while practical systems must handle uncertainty and environmental changes. The paper motivates efficient feedback planning and addresses constraints that limit existing DP- and TO-based approaches.
- Motivation: Legged locomotion makes optimal planning and control challenging because the system is hybrid, nonlinear, and subject to environmental uncertainty.Practical motion-control frameworks must also respond to dynamic changes in the environment.
- Feedback Planning: Feedback planners jointly design a stabilizing controller with the feedforward plan, which is especially important when MPC runs slower than the controller rate.
- Feedback Planning: Dynamic Programming provides a basis for feedback planning but traditionally suffers from the curse of dimensionality on high-dimensional systems.
- SLQ Methods: SLQ methods reduce this burden through local quadratic value-function approximations and forward integration, but often cannot efficiently handle equality or inequality constraints.
- Related Approaches: Trajectory Optimization can plan contact-rich legged motions, yet its nonlinear-program formulation may converge slowly and usually requires tracking controllers for hardware execution.
B. Contributions
The paper reformulates legged-robot motion planning as constrained switched-system optimal control and solves it with alternating optimization over switching times and continuous inputs. This formulation enables efficient constrained feedback planning for contact-dependent locomotion.
- Optimization Framework: A multi-level optimization alternates between optimizing switching times and optimizing continuous control inputs.
- Switched System Formulation: The model uses generalized coordinates, torques, and contact forces to describe each locomotion mode.The formulation includes the inertia matrix, Coriolis-gravity forces, and contact Jacobians.
- Switched System Formulation: The framework assumes a predefined gait sequence and represents legged locomotion as a switched system with contact-induced equality constraints.Mode transitions such as touchdown can cause state discontinuities, which the formulation assumes negligible and leaves to feedback control.
- Switched System Formulation: Switching times are introduced as additional optimization variables, converting hybrid-system optimization into switched-system optimization with additional inequality constraints.
- Optimization Framework: The switched-system formulation permits use of efficient optimal-control algorithms developed for switched systems instead of hybrid-system methods.
B. Optimal Control for Switched System
The paper formulates legged-robot planning as switched-system optimal control and decomposes it into switching-time and continuous-control optimization. It then uses constrained continuous-time SLQ to efficiently update feedforward and feedback controllers.
- Problem formulation: The switched-system problem optimizes switching times and continuous-time controls under dynamics, state-input, and pure-state constraints.The switching times determine mode intervals, while each mode contributes intermediate and terminal costs.
- Multi-level optimization: Multi-level optimization separates finding optimal switching times from optimizing continuous controls, using alternating procedures for the two subtasks.The top level estimates switching-time gradients, while the bottom level solves the continuous-time control problem with fixed switching times.
- Constrained SLQ: The constrained continuous-time SLQ algorithm handles state and input equality constraints while retaining O(n) computational complexity over the optimization horizon.This is contrasted with the O(n^3) complexity of a common SQP solver and a prior discrete-time constrained SLQ method.
- Constrained SLQ: Each SLQ iteration forward-integrates the dynamics, forms local quadratic cost and linearized dynamics and constraints, then computes a controller update using constrained LQR.The update combines feedback gains with feedforward terms for cost reduction and constraint correction.
D. Remarks
The proposed decomposition treats switching times as optimization variables alongside continuous controller parameters. By alternating these optimizations, it enables the continuous-control subproblem to use an efficient SLQ solver and does not require a constraint-satisfactory initial solution.
- Remarks: Switching times are represented as an extended state and estimated together with continuous controller parameters through alternating optimization.The paper connects this structure to alternating estimation of latent variables and model parameters in approximate inference.
- Remarks: Alternating optimization transforms continuous-input optimization into a conventional optimal control problem that can use SLQ, potentially improving efficiency.The fixed contact sequence and switching-time variables are distinguished from prior work by the optimization procedure used here.
- Remarks: The initial solution need not satisfy the constraints, so switching-time changes can violate constraints without interfering with the inner-loop algorithm.This property is stated as important when the top-level optimizer changes switching times.
III. GENERALIZED CONSTRAINED LQR
The paper derives a generalized constrained LQR procedure for the local time-varying quadratic problems arising in SLQ. It uses Lagrange multipliers for state-input constraints and a penalty-based treatment for pure-state constraints while preserving linear-time complexity.
- Constrained LQR formulation: The constrained time-varying LQR problem is formulated with a Lagrangian that incorporates system dynamics and state-input constraints through multipliers.The Hamiltonian formulation leads to the necessary optimality conditions used to derive the controller.
- Constrained LQR formulation: The controller update is obtained by combining the resulting optimality equations with transversality conditions for the constrained linear-quadratic problem.The paper identifies this derivation with the Pontryagin Minimum Principle for the stated problem.
- Controller structure: The constrained feedforward-feedback controller projects the unconstrained SLQ controller into the constraint null space and adds a term that drives constraint violations toward zero.The second term lies in the constraint range space through right-pseudo-inverse multiplication.
- Pure-state constraints: Pure-state constraints are handled by penalizing squared constraint violations and adding normal-equation terms, avoiding the O(n^3) cost associated with a direct multiplier approach.Increasing the penalty multiplier over iterations enforces the pure-state constraints to the desired accuracy.
- Optimality conditions: The quadratic value function satisfies the Hamilton-Jacobi-Bellman equation under the derived sufficient conditions.The value function is represented using a time-varying quadratic form in the state.
IV. MOTION CONTROL FOR THE COM AND THE FULL KINEMATICS
The robot model combines Center of Mass dynamics with full kinematics in a switched formulation for quadrupedal motion. Its controls include switching times, leg contact forces, and joint angular velocities, with swing trajectories and contact constraints enforced during optimization.
- Robot model: The HyQ quadruped is modeled with Center of Mass dynamics and full kinematics, using three degrees of freedom per leg.The generalized torque is set to zero because the effective Center of Mass forces are external forces.
- Robot model: The optimization controls are switching times, contact forces for the legs, and joint angular velocities.The contact-force vectors and foot kinematic quantities are defined for each leg.
- Walking task: Walking-task integration and Riccati-equation step sizes are higher during the initial four-legged support phase and the second-last phase.These phases are also identified as the longest phases in the walking sequence, where larger average step sizes can save computation relative to fixed-step implementations.
- Contact handling: A predefined swing-leg trajectory enforces touchdown according to the switching schedule and makes the swing-foot velocity zero immediately before contact.Unilateral contact constraints are enforced by imposing zero contact force for legs that violate the constraint during the SLQ forward pass.
- State representation: The model uses base orientation rather than Center of Mass orientation because Center of Mass orientation is not directly measurable and integrated estimates can drift.This choice is motivated by the requirements of a feedback-controller structure.
V. RESULTS
The framework is validated across walking, gap-crossing, and trotting tasks with different contact patterns. Contact-switching times are initialized uniformly and then optimized for each task.
- The evaluation covers walking in a free environment, gap crossing with restricted footholds, and trotting in different directions.
A. Walking Task
The walking task uses a lateral sequence in which three feet remain on the ground simultaneously. Its longest phases, especially early four-legged support and the second-last phase, require comparatively large SLQ step sizes.
- The lateral walking sequence keeps three feet on the ground at the same time.
- SLQ step sizes are significantly larger than the approximately 0.002 typically used in discrete-time SLQ.The increase is especially prominent during the initial four-legged support phase and the second-last phase.
B. Gap Crossing Task
The optimized framework produces walking and jumping behaviors that cross 1 m and 2 m gaps while respecting leg reach and foothold constraints. It also yields smooth support forces and task-adapted switching times.
- The 1 m gap is encoded as a state constraint, and optimization finds a gait that avoids stepping into it.The constraint is enforced at touchdown through the terminal cost of each swing-leg mode.
- HyQ yaws into a slight diagonal configuration to increase hip mobility when crossing the wide 1 m gap.This behavior helps avoid the robot’s kinematic limits by using the hip degrees of freedom for increased mobility.
- The 2 m gap requires a trotting jump with an optimized flight phase because the gap exceeds the reach of HyQ’s leg.The optimizer places the flight phase over the gap and shortens it, reducing flight time and jump height to a lower optimum.
- Contact forces remain relatively smooth during support phases, with no distinct spikes at touchdown or lift-off.The resulting force profiles provide smooth references for hardware tracking controllers and reflect the stepping sequence.
C. Forward and Side Trotting Task
The framework is applied to statically unstable trotting with diagonal leg pairs alternating in contact. Altering the cost function produces forward and lateral target-directed trots.
- Trotting maintains contact through one diagonal leg pair at a time without an intervening four-leg support phase.
- Changing the cost function sets different target points, enabling forward and lateral trots.The forward trot exhibits an even step length in the reported foot-position results.
D. Convergence and Constraint Violation
The two optimization levels converge rapidly while progressively reducing constraint violations. Switching-time optimization further lowers costs across tasks, and the framework demonstrates constrained SLQ with O(n) complexity.
- Inner optimization: After one or two iterations, costs are significantly reduced, but this initial improvement increases constraint violation.The inner optimization then reduces constraint violation while costs remain nearly constant.
- Constraint satisfaction: Forward trotting maintains even x-direction step lengths except during acceleration and deceleration, with no significant side stepping or ground penetration.
- Inner optimization: After about 10 to 15 iterations, the constraint-violation ISE reaches a very low range, where physical inaccuracies may matter more during execution.
- Top-level optimization: 15 to 40% of normalized cost is reduced by optimizing switching times across the evaluated tasks.
- Top-level optimization: Top-level optimization usually achieves this cost reduction in 2-3 iterations, supporting rapid convergence of the outer level.The figure caption describes the optimization as both effective and efficient.
- Framework: The framework combines switched-system modeling, multi-level optimization, and a constrained SLQ algorithm with O(n) time-complexity.The conclusion identifies these as the approach's main contributions.