Locomotion generation for quadruped robots on challenging terrains via quadratic programming

Abstract

This paper proposes a locomotion generation method for quadruped robots, which first computes an optimal trajectory of the robot’s center of mass (CoM) and then its whole-body motion through inverse kinematics. As the core component, the computing of the CoM trajectory, which is parameterized as polynomials, is based on the robot’s centroidal dynamics and it is observed that several terms in the centroidal dynamics are minor and can be omitted in the locomotion generation. Then, as a basic form of the proposed method, the CoM trajectory optimization is written as a quadratic programming (QP) problem for the case of given step sequences, timings and footholds. Furthermore, the uncertainty of the robot’s CoM, which is described as a convex polyhedron around a nominal CoM position, and the reachability of the robot’s feet, which is approximated as another convex polyhedron, can be added to the QP problem as linear inequality constraints. Ultimately, the planning of step sequences, timings, and footholds is all incorporated, leading to a single mixed-integer quadratic programming problem. Numerical and hardware experiments have been conducted and show that the proposed method can generate various walking motions for a quadruped robot to travel over challenging terrains.

Appendix—motion controller

Similar to many existing motion controllers for legged robots (Gehring et al., 2013; Park et al., 2015; Focchi et al., 2017; Bledt et al., 2018; Di Carlo et al., 2018; Feng et al., 2015; Caron et al., 2015a; Kuindersma et al., 2016; Zheng et al., 2019), our controller computes the required contact forces to determine the feedforward joint torques and then the final joint torques by adding the feedback joint torques, as depicted by the green block in Fig. 2.

1.1 Computing of contact forces

The first step is aimed at computing the required contact forces for the robot to produce the reference CoM trajectory and base’s angular motion, which can be written as the following QP problem:

$$\begin{aligned} \left\{ \begin{array}{cl} \min \limits _{\varvec{f}_1,\varvec{f}_2,\dotsc ,\varvec{f}_{N_C}} &{} J_w+J_{\tau }+J_f \\ \mathrm {subject~to} &{} \varvec{N}^T_i \varvec{f}_i \le \varvec{0} \\ &{} \varvec{n}^T_i \varvec{f}_i \le f^U_i \\ &{} i=1,2,\dotsc ,N_C. \end{array} \right. \end{aligned}$$

(30)

The above objective function consists of three terms to integrate different control goals. The purpose of the first term $J_w$ is to realize the reference CoM trajectory and angular motion of the robot’s base. From (4), it is written as

$$\begin{aligned} J_w = \left( \varvec{w}^{\mathrm {des}} - \varvec{G}\varvec{f} \right) ^T \varvec{W}_w \left( \varvec{w}^{\mathrm {des}} - \varvec{G}\varvec{f} \right) , \end{aligned}$$

(31)

where $\varvec{W}_w \in {\mathbb {R}}^{6 \times 6}$ is a positive-definite diagonal weight matrix and $\varvec{w}^{\mathrm {des}}$ is the desired wrench given by

$$\begin{aligned} \varvec{w}^{\mathrm {des}} = \begin{bmatrix} m(\ddot{\varvec{p}}^{\mathrm {des}}_G -\varvec{g}) \\ m\hat{\varvec{p}}_G (\ddot{\varvec{p}}^{\mathrm {des}}_G -\varvec{g}) + \dot{\varvec{{\mathcal {L}}}}^{\mathrm {des}} \end{bmatrix}. \end{aligned}$$

(32)

Here, $\ddot{\varvec{p}}^{\mathrm {des}}_G$ is the desired CoM acceleration derived from the reference acceleration $\ddot{\varvec{p}}^{\mathrm {ref}}_G$, velocity $\dot{\varvec{p}}^{\mathrm {ref}}_G$, and position $\varvec{p}^{\mathrm {ref}}_G$ together with the actual velocity $\dot{\varvec{p}}_G$ and position $\varvec{p}_G$ of the CoM using the proportional-derivative (PD) control law:

$$\begin{aligned} \ddot{\varvec{p}}^{\mathrm {des}}_G = \ddot{\varvec{p}}^{\mathrm {ref}}_G + \varvec{K}^G_d \left( \dot{\varvec{p}}^{\mathrm {ref}}_G-\dot{\varvec{p}}_G \right) + \varvec{K}^G_p \left( \varvec{p}^{\mathrm {ref}}_G-\varvec{p}_G \right) , \end{aligned}$$

(33)

where $\varvec{K}^G_p$ and $\varvec{K}^G_d$ are PD gains. From (5), we write the desired time derivative of angular momentum as

$$\begin{aligned} \dot{\varvec{{\mathcal {L}}}}^{\mathrm {des}} = \varvec{{\mathcal {I}}}^S_0 \dot{\varvec{\omega }}^{\mathrm {des}}_0 - (\varvec{{\mathcal {I}}}^S_0 \varvec{\omega }^S_0) \times \varvec{\omega }^S_0, \end{aligned}$$

(34)

where $\dot{\varvec{\omega }}^{\mathrm {des}}_0$ is the desired angular acceleration as

$$\begin{aligned} \dot{\varvec{\omega }}^{\mathrm {des}}_0 = \dot{\varvec{\omega }}^{\mathrm {ref}}_0 + \varvec{K}^{\omega }_d \left( \varvec{\omega }^{\mathrm {ref}}_0-\varvec{\omega }_0 \right) + \varvec{K}^{\omega }_p \left( \varvec{R}^{\mathrm {ref}}_0\varvec{R}^T_0 \right) ^{\vee }, \end{aligned}$$

(35)

where $\varvec{R}^{\mathrm {ref}}_0,\varvec{R}_0 \in SO(3)$ represent the reference and actual orientations of the robot’s base with respect to the global frame, respectively, and $\left( \varvec{R}^{\mathrm {ref}}_0\varvec{R}^T_0 \right) ^{\vee }$ is the associated vector of rotation to change $\varvec{R}_0$ to $\varvec{R}^{\mathrm {ref}}_0$.

The second term $J_{\tau }$ is used to penalize high joint toques and written as

$$\begin{aligned} J_{\tau } = \varvec{f}^T \varvec{J}_{\tau } \varvec{W}_{\tau } \varvec{J}^T_{\tau } \varvec{f}, \end{aligned}$$

(36)

where $\varvec{W}_{\tau } \in {\mathbb {R}}^{N_J \times N_J}$ is a positive-definite diagonal weight matrix and $\varvec{J}^T_{\tau } \in {\mathbb {R}}^{N_J \times 3N_C}$ is the transposed Jacobian matrix, comprising the last $N_J$ rows of $\varvec{J}^T$ in (1) and converting contact forces to joint torques.

The last term $J_f$ regulates the contact forces, i.e.,

$$\begin{aligned} J_f = \left( \varvec{f}^{\mathrm {des}} - \varvec{f} \right) ^T \varvec{W}_f \left( \varvec{f}^{\mathrm {des}} - \varvec{f} \right) , \end{aligned}$$

(37)

where $\varvec{W}_f \in {\mathbb {R}}^{3N_C \times 3N_C}$ is a weight matrix and $\varvec{f}^{\mathrm {des}}$ comprises the desired contact forces. When a foot is contacting and expected to remain on the ground, we take its desired contact force to be the optimal value obtained by solving the QP problem (30) in the previous control cycle. For a foot that is in the air but expected to contact or is contacting but expected to leave the ground, we simply set its desired contact force to zero.

1.2 Computing of joint torques

The second step of the motion controller is to compute the joint torques for controlling the robot. Since the leg’s mass of a quadruped robot is often negligible, we can skip the robot’s whole-body dynamics and simply compute the joint torques for controlling the robot as (Focchi et al., 2017; Mastalli et al., 2017)

$$\begin{aligned} \varvec{\tau } = -\varvec{J}^T_{\tau } \varvec{f}^{\mathrm {opt}} + \tilde{\varvec{K}}^q_d \left( \dot{\varvec{q}}^{\mathrm {ref}}-\dot{\varvec{q}} \right) + \tilde{\varvec{K}}^q_p \left( \varvec{q}^{\mathrm {ref}}-\varvec{q} \right) , \end{aligned}$$

(38)

where $\dot{\varvec{q}}$ and $\varvec{q}$ with or without the superscript “ref” are the reference or current joint velocities and angles, respectively, and $\tilde{\varvec{K}}^q_p$ and $\tilde{\varvec{K}}^q_d$ are the PD gains mapping the errors in joint angles and velocities directly into the joint torques for joint trajectory tracking. Here, the reference joint trajectories $\varvec{q}^{\mathrm {ref}}$ are calculated through inverse kinematics with the reference footholds and body trajectories determined by the motion generator, as illustrated in Fig. 2 and discussed in this paper.