PaperlayerView exact revision on arXiv

Source-linked AI summary

Real-Time Motion Planning of Legged Robots: A Model Predictive Control Approach

Farbod Farshidian, Edo Jelavić, Asutosh Satapathy, Markus Giftthaler, Jonas Buchli

arXiv:1710.04029v1cs.RO

TL;DR

Real-time planning for legged robots must adapt plans to current state measurements, while nonlinear optimal-control optimization can be computationally expensive. This paper applies constrained nonlinear MPC using SLQ with a parallelized backward pass. It reports optimized trajectories within milliseconds and performance at least one order of magnitude beyond the state of the art.

  • Problem

    Real-time legged-robot planning must adjust plans to current state measurements, but nonlinear optimal-control optimization can be computationally expensive.

    Source: §I · p. 1

  • Method

    The paper uses constrained nonlinear MPC with an SLQ variant that parallelizes value-function estimation in the backward pass.

    Source: Contributions · p. 1

  • Results

    The MPC generates optimized trajectories within milliseconds and reports performance at least one order of magnitude beyond the state of the art.

    Source: Contributions · p. 1

  • Takeaways & Limitations

    The approach is demonstrated for trotting, with hardware and simulation tests showing goal tracking and disturbance rejection.

    Source: Contributions · p. 1, §V · p. 8

Abstract

from arXiv · show

We introduce a real-time, constrained, nonlinear Model Predictive Control for the motion planning of legged robots. The proposed approach uses a constrained optimal control algorithm known as SLQ. We improve the efficiency of this algorithm by introducing a multi-processing scheme for estimating value function in its backward pass. This pass has been often calculated as a single process. This parallel SLQ algorithm can optimize longer time horizons without proportional increase in its computation time. Thus, our MPC algorithm can generate optimized trajectories for the next few phases of the motion within only a few milliseconds. This outperforms the state of the art by at least one order of magnitude. The performance of the approach is validated on a quadruped robot for generating dynamic gaits such as trotting.

I. INTRODUCTION

The paper addresses the real-time computational demands of legged-robot motion planning with constrained nonlinear MPC and a parallelized SLQ backward pass. It describes the SLQ optimization process and its use for planning dynamic gaits.

I. INTRODUCTION

  • Real-world planning must adjust to current state measurements in real time, while task decomposition can limit maneuverability and the reachable solution set.Hand-designing plans for complex legged systems is often impractical, and nonlinear optimal-control optimization can be computationally expensive.Source: §I · p. 1
  • The proposed constrained nonlinear MPC re-optimizes trajectories within milliseconds and reports performance at least one order of magnitude beyond the state of the art.Its SLQ variant parallelizes value-function estimation in the backward pass.Source: Contributions · p. 1
  • The approach is demonstrated for dynamic gaits such as trotting, with disturbances inserted during execution to test robustness and real-time planning.The authors describe this as the first whole-body nonlinear MPC for periodic gait generation in legged systems.Source: Contributions · p. 1

II. MODEL PREDICTIVE CONTROL APPROACH

  • The MPC uses constrained SLQ, which iteratively solves local extremal problems around estimated trajectories and updates the control policy.The method uses a generalized constrained LQR update based on the local problem.Source: §II · p. 1, A. An Overview of the SLQ Algorithm · p. 2

A. An Overview of the SLQ Algorithm

  • Continuous-time SLQ uses variable-step ODE solvers to discretize the problem according to a desired accuracy, reducing average discretization points relative to discrete-time SLQ.Fewer points can reduce calculations for expensive dynamics and constraint linearizations.Source: A. An Overview of the SLQ Algorithm · p. 2
  • The formulation handles switched systems with transitions at predefined times in a predefined subsystem sequence.SLQ uses a Lagrangian method for state-input equality constraints and a penalty method for state-only constraints.Source: A. An Overview of the SLQ Algorithm · p. 2

B. FASTSLQ Algorithm

FASTSLQ parallelizes SLQ’s computationally expensive backward pass by partitioning the horizon and using prior-iteration value-function estimates to initialize partitions. It corrects those estimates at current nominal trajectories and adds a line-search condition to keep trajectory changes small enough for the approximation to remain valid.

B. FASTSLQ Algorithm

  • FASTSLQ parallelizes the computationally expensive backward pass, which otherwise is often implemented as a single process.Each SLQ iteration also includes forward integration and construction of the LQ approximation.Source: B. FASTSLQ Algorithm · p. 2
  • For the switched-system formulation, the horizon is partitioned at switching moments so each interval’s Riccati-like equations can be solved from an estimated final value.The passage notes that other partitioning approaches can also be used.Source: B. FASTSLQ Algorithm · pp. 2–3
  • Each partition uses the following partition’s previous-iteration value-function approximation rather than waiting for its current backward solution.Because nominal trajectories change between iterations, the Riccati-like equations also change.Source: B. FASTSLQ Algorithm · p. 3
  • The previous iteration’s value-function approximation is reevaluated at the current nominal trajectory, correcting first-order terms while retaining its second-order term.The approximation supplies the value and first derivative at the partition’s final time; the second-order term is reused directly.Source: B. FASTSLQ Algorithm · p. 3, Figure 1 · p. 4
  • FASTSLQ adds a line-search condition requiring updated cost to remain close to the value-function approximation’s predicted cost.This is especially necessary in initial iterations, when the solution is far from optimal.Source: B. FASTSLQ Algorithm · p. 3
  • The note says the approximation issue is not a major concern in real-time-iteration MPC, where successive optimal solutions are nearby.FASTSLQ’s parallel sweep is described as analogous at a high level to asynchronous dynamic programming, where states may be swept in arbitrary order if all are visited.Source: B. FASTSLQ Algorithm · p. 3

C. Dealing with State-Input Inequality Constraints

State-input inequality constraints are handled by projecting the control when a constraint is violated during the forward pass.

C. Dealing with State-Input Inequality Constraints

  • When a forward-pass control violates a linearized state-input inequality constraint, it is projected onto that constraint’s plane.The approach ensures constraint-satisfactory trajectories but does not necessarily produce an optimal solution.Source: C. Dealing with State-Input Inequality… · p. 4

D. FASTSLQ-MPC with Real-Time Iteration Scheme

FASTSLQ-MPC uses one SLQ iteration per updated measurement and adjusts the current policy using local feedback when disturbances shift the state. It also shifts the terminal time across complete switching modes and uses an LQR value function at the horizon end.

D. FASTSLQ-MPC with Real-Time Iteration Scheme

  • Real-time iteration takes one solver iteration toward the current MPC problem before proceeding to the next measurement-triggered problem.The solver does not iterate each problem to convergence.Source: D. FASTSLQ-MPC with Real-Time… · p. 4
  • With disturbances or a receding horizon, transitions between successive optimization problems must be carefully designed because state deviations can impair convergence.FASTSLQ-MPC adjusts the current solution using its locally linear feedback policy, extending the open-loop plan to a vicinity of the optimal trajectories while respecting constraints.Source: D. FASTSLQ-MPC with Real-Time… · p. 4
  • The terminal time is set at a future switching time and the optimization horizon includes exactly n complete modes, with an LQR value function added as terminal cost.For n = 2, the horizon varies between 2Δt and 3Δt when average subsystem activation time is Δt; the quasi-infinite-horizon stability guarantee requires controllability at the terminal time.Source: D. FASTSLQ-MPC with Real-Time… · p. 4

A. Platform Description

The MPC approach is evaluated on HyQ, a hydraulically actuated quadruped with torque-controlled joints and sensing for joint position and torque. The setup uses a dedicated computer for MPC and separate midlevel and lowlevel control loops.

A. Platform Description

  • HyQ is a hydraulically actuated, fully torque-controlled quadruped with three joints per leg.Absolute and relative encoders measure joint positions, while load cells measure joint torque and estimate contact forces.Source: A. Platform Description · p. 4
  • A dedicated Intel Core i7-4790 computer runs the MPC loop, while the midlevel tracking controller runs at 250 Hz and the lowlevel torque controller at 1 kHz.The midlevel controller receives state estimates and sends desired torques to the lowlevel controller.Source: A. Platform Description · pp. 4–5
  • The setup estimates base state with a Kalman filter and approximates the ground plane using stance-foot positions.The plane is fitted through stance feet in two consecutive phases.Source: A. Platform Description · p. 5

B. Modeling Framework

The framework models legged-robot motion with whole-body CoM dynamics and full kinematics, represented as a switched system with phase-specific constraints. It combines that model with a motion-control structure that tracks end-effector forces and stabilizes the CoM.

B. Modeling Framework

  • For HyQ, the CoM-dynamics-plus-full-kinematics model has 24 states and 24 inputs, including contact forces and end-effector joint velocities.The model describes CoM motion and robot joint positions.Source: B. Modeling Framework · p. 5
  • The switched formulation assumes no state jump at phase transitions and a predefined phase sequence, with swing-foot trajectories designed for zero touchdown approach velocity.The predefined trajectory also aligns touchdown with switching times.Source: B. Modeling Framework · p. 5
  • The model equations describe base and CoM motion using foot contact forces, while stance and swing phases impose different foot-velocity and force constraints.The text defines the foot position, velocity, and contact-force variables; swing feet follow a prescribed normal-direction trajectory and have zero contact force.Source: B. Modeling Framework · p. 5
  • The controller transforms joint coordinates to end-effector coordinates, tracks end-effector forces, and maps CoM-acceleration corrections to the MPC reference force.End-effector tracking uses an impedance controller; the CoM controller stabilizes the CoM.Source: Figure 2 · p. 5

C. Model Comparison

The section compares a switched model with a rigid-body model using soft contacts, noting differences in dimension, constraint handling, and swing-leg velocity continuity.

C. Model Comparison

  • The switched model has a combined state-and-input dimension of 48, comparable in computational complexity to a soft-contact rigid-body model with 36 states and 12 torque inputs.The soft-contact model computes contact forces using nonlinear springs and dampers.Source: C. Model Comparison · p. 5
  • The switched model enforces contact unilateral constraints directly, whereas the soft-contact model uses penalties that may leave constraints unsatisfied.The text says soft-contact models require care on hardware and can make good go-to tasks difficult.Source: C. Model Comparison · p. 5
  • Soft-contact swing-leg velocities are continuous, but switched-model velocities are piecewise continuous and therefore need pre-filtering before hardware use.SLQ complexity scales cubically with the sum of state and input dimensions.Source: C. Model Comparison · p. 5

D. Motion Control Structure

HyQ experiments demonstrate in-place trotting, a 1 m go-to task, and responses to external disturbances. The motion-control structure tracks planned contact forces, swing-leg motion, and CoM trajectories.

D. Motion Control Structure

  • The controller tracks contact forces with a robust internal-model controller, swing-leg trajectories with inverse dynamics plus PD, and CoM motion with PD feedback.The CoM controller corrects acceleration and maps that correction to planned contact forces because the MPC loop runs at 60 Hz, below the cited 250 Hz example; contact forces are controlled directly using the approach in.Source: D. Motion Control Structure · pp. 5–6, A. Motion Planning for HyQ through… · p. 6

IV. RESULTS

A. Motion Planning for HyQ through FASTSLQ-MPC

B. FASTSLQ Benchmarking

FASTSLQ uses extra processing threads to reduce the computational burden of longer MPC horizons. Its parallel backward pass targets the main SLQ computation bottleneck.

B. FASTSLQ Benchmarking

  • When the time horizon doubles, MPC loop frequency does not fall by half, as FASTSLQ benefits from extra threads.The table compares loop frequency across subsystem counts and thread counts.Source: B. FASTSLQ Benchmarking · p. 7
  • SLQ's backward pass accounts for almost 65% of total computation; FASTSLQ leverages extra threads to reduce its computation load.Fig. 8 compares average CPU time for the three main operations of SLQ and FASTSLQ.Source: B. FASTSLQ Benchmarking · p. 7

V. CONCLUSION

The paper presents FASTSLQ-MPC for real-time legged-robot planning, using parallel computation in SLQ’s backward pass. It reports order-of-magnitude state-of-the-art rates and demonstrates trotting and disturbance rejection.

V. CONCLUSION

  • During trotting, HyQ side-steps about 40 cm after a lateral pull and eventually returns to its initial position.Source: Figure 7 · p. 8
  • Fig. 8 compares CPU time for three SLQ operations using 1 thread with FASTSLQ using 4 threads, averaged over 1,500 MPC-loop iterations.Source: Figure 8 · p. 8
  • Parallelizing SLQ’s backward pass reduces its computational complexity and improves the MPC loop frequency.The approach generates optimized trajectories for the next few motion phases within a few milliseconds.Source: §V · p. 8
  • FASTSLQ-MPC exceeds state-of-the-art rates by an order of magnitude; with 2 subsystems, the MPC loop runs at about 60 Hz.Hardware and simulation tests cover trotting, user-defined goal tracking, and disturbance rejection.Source: §V · p. 8
Loading 1710.04029v1…