1 of 23

Quadruped Locomotion through Nonlinear Model Predictive Control

24-785 Engineering Optimization, Fall 2022

Zixin Zhang, Khai Nguyen, Fukang Liu, Tianyu Ren, Saurabh Borse

2 of 23

Outline

Background
Problem Statement
Numerical Optimization

Discretization
High-level SQP structure and QP solver
Quadratic Approximation
Constraint Projection
Line-Search

Implementation
Results
Summary

2

3 of 23

Background

Legged robots

Legs can help the robot cope with uneven and unstructured terrain

Static Gaits vs. Dynamic Gaits

Dynamic gaits are more efficient when moving at high speeds

Control of Dynamic Legged Locomotion is Challenging

Legged robots are under-actuated systems
Ground reaction forces are constrained
Hardware has limited computational performance

Model Predictive Control (MPC)

Have preview capabilities
Can handle Multi-Input Multi-Output (MIMO) system
Can handle constraints

3

4 of 23

Problem Statement - Objective

Track a reference trajectory of state and control input:

4

State

Control Input

Running Cost

5 of 23

Problem Statement - Constraints

5

State

Input

Dynamics

Joint Limit

Joint Vel. Limit

Torque Limit

Friction Cone

Swing Foot

Stance Foot

Swing Trajectory

6 of 23

Numerical Optimization

6

Algorithm Overview [1]

[1] Ruben et al. Perceptive Locomotion through Nonlinear Model Predictive Control. 2022

7 of 23

Numerical Optimization - Discretization

Solve ODE using explicit Runge–Kutta 4 (RK4)

7

8 of 23

Numerical Optimization - Discretized Problem (NLP)

8

Decision Variables

Joint Limits

Friction Cone

Initial Condition

Discret Robot Dynamics

State-Input Equality Constraints

9 of 23

Numerical Optimization - Inequality Constraints

Relaxed barrier function:

New objective function:

9

10 of 23

Numerical Optimization - SQP

General equality-constrained NLP:

Lagrangian:

10

11 of 23

Numerical Optimization - SQP

QP subproblem:

11

12 of 23

Numerical Optimization - Quadratic Approximation

As we seek to deploy MPC on dynamic robotic platforms, we want the problem can be solved efficiently by the numerical solver. The calculation of full-Hessian can be challenging at some time.
With generalized Gauss-Newton method, while maintaining the convexity of the problem, a positive semi-definite Hessian can be a good estimation of full-Hessian.
The effectiveness of such an estimation has been proved in many papers.

12

13 of 23

Numerical Optimization - Quadratic Approximation

For tracking cost: ( is the tracking error)

Similarly, exploit the convexity for penalty function:

13

14 of 23

Numerical Optimization - Constraint Projection

The state input equality constraints were chosen to have full row rank w.r.t. the control inputs.
Equality constraints can be eliminated before solving the QP through a change of variables.

14

Robot Dynamics

Cost

Where the linear transformation satisfies,

15 of 23

Numerical Optimization - Line-search

Each update either improves the constraint satisfaction or the cost function

15

Constraint satisfaction:

High constraint violation

Low constraint behavior

Armijo condition

Primary acceptance condition

16 of 23

Implementation

OCS2 Toolbox

A C++ toolbox tailored for Optimal Control for Switched Systems (OCS2)
Why use OCS2?

Is designed for optimal control problems for switched systems
Provide an efficient implementation of SQP
Provide many useful functions for legged locomotion controllers
Provide interface for ROS communication (critical for simulation)

16

17 of 23

Results

17

18 of 23

Results

So, let’s analyze the result. We will mainly focus on the stance mode.

Did we find a local minimum? We think so, because for this solver, if the change of the Lagrangian is small enough, the solver would stop. In that case, the KKT condition is satisfied within the specified tolerance.

But did we find the global minimum? Actually we are not sure. However, the solution we found should be very close to the global minimum based on simple physics. Ideally, the control input should be this vector, which means the ground reaction force on each foot should be a quarter of the robot’s weight, be perpendicular to the ground, and all the foot have zero velocities. The figure below shows the solution in stance mode, we can see that the ground reaction forces on each foot are exactly the same. The velocity of each foot is not exactly zero, but is very close to that.

19 of 23

Results

Use good initial guess (tested in stance mode):

State trajectory guess: Default robot state of stance mode
Control input trajectory guess: GRFs on each foot: [0, 0, 0.25mg]. All foot velocities are 0
Termination: the change in Lagrangian is smaller than the tolerance (1e-4)

20 of 23

Results

Use bad initial guess (tested in stance mode):

Set all decision variables to 0: Use 5 iterations to solve
Set all decision variables to 1: Failed

When line search try to make the step size smaller, the equality constraints violation becomes bigger. Finally the solver stopped when the step size became too small

21 of 23

Results

21

22 of 23

Results

22

		0
Number of Iterations until Convergence	Step size below minimum	5	2	6	Step size below minimum

23 of 23

Summary

We formulate a trajectory optimization problem (model predictive control) to find the proper control inputs for a walking quadruped robot.
The original continuous-time problem is discretized to an NLP, then solved through SQP.
Quadratic approximation is used to get positive semi-definite Hessian, so that the QP in each SQP iteration is Convex. Constraint projection is used to eliminate equality constraints and make solving faster.
The solver is sensitive to the initial guess of states
The solver found the local minimum, and should be very close to the global minimum

23