An Optimization-Based Locomotion Controller for Quadruped Robots Leveraging Cartesian Impedance Control

1. Introduction

Recent improvements in mobile robotics indicate that it is possible to achieve autonomous inspection and maintenance of critical industrial infrastructure in extreme environments (e.g., off-shore plants and nuclear sites) within the next decade. The automation of such processes will reduce their economic cost and increase the life quality of the operators that can be moved away from operational hazards. Legged robots are the most promising among ground robots for locomotion flexibility, being able to surpass a varied set of obstacles. However, this comes at the expense of reduced stability compared to wheeled robots. Quadruped robots offer a balance between efficiency and stability. Algorithms that can generate precise but compliant motions while satisfying physical feasibility are required to control quadruped robots with a large number of degrees of freedom. On the other hand, a trade-off strategy is also required to coordinate multiple tasks and constraints.

A good example for legged locomotion is the management of the contact forces that need to provide sufficient support to the body while not violating the non-slipping conditions determined by the friction cones. An intuitive control technique that can deal with these problems is the Virtual Model Controller (VMC) (Pratt et al., 2001), which has been applied to state of the art legged prototypes (Boaventura et al., 2012; Gehring et al., 2013). The VMC determines the contact forces based on the centroidal momentum model according to desired motions, and then maps these forces into joint torques. Meanwhile, the swing legs are controlled using joint PD controllers tracking a desired reference trajectory. Therefore, VMC controls swing legs and supporting legs separately and is not a whole-body controller.

Another widely used control framework is the fully optimization-based control (De Lasa et al., 2010; Saab et al., 2013). This framework relies on solving an optimization problem (usually quadratic) to minimize a cost function. The dynamic model and desired end-effector motions are introduced as equality constraints, while friction cones, torques, and joint limitations are inequality constraints. The main benefit of the optimization-based control is the integration of the operational space tasks and the joint space dynamics in a single problem. Furthermore, its formulation can be extended to solve multiple tasks, which can be implemented using either weighted optimization or strictly hierarchical optimization (Hutter et al., 2014). Two popular implementations are the Hierarchical Quadratic Programming (HQP) and the Weighted Quadratic Programming (WQP). HQP uses a strictly prioritized control framework based on null-space projectors; meanwhile, WQP relies on tunable weight matrices. Despite the fact that HQP solves two or more QPs, it is normally preferred to WQP because it does not require tuning.

The QP-schemes outlined above optimize for joint accelerations, contact forces, and joint torques, but only joint torques are eventually used for control. Since only torques are needed to control the robot, the original optimization formulation can be simplified by reducing optimization variables by considering reducing computation complexity as well. In Bellicoso et al. (2016), the QR decomposition proposed in Mistry et al. (2010) is employed to eliminate contact forces from dynamic equations so that optimization variables are reduced to two vectors. In Mansard (2012), a projector considering the consistency of the inertia matrix is proposed to separate the dynamic equations, and the optimization variables are finally reduced to two vectors. A straightforward decomposition is presented in Herzog et al. (2016) that directly splits six rows corresponding to six dimensional floating bases from the whole dynamic equation, thus removing actuation torques from optimization variables. By contrast, joint torques are the decision variables for the proposed control technique in this paper, and only one QP needs to be solved.

For optimization-based approaches, operational tasks are bounded to joint space dynamics by optimization constraints. Consequently, tuning parameters, such as impedance gains require several trials. A seminal control technique called operational space control (OSC), presented first in Khatib (1987), provides us with the analytical solution to derive joint torques from desired operational tasks. OSC has been extended to hierarchical OSC by iterative null-space projection for legged robots (Sentis and Khatib, 2006; Sentis et al., 2010; Lee et al., 2016). In Mistry and Righetti (2012), the orthogonal projector proposed by Aghili (2005) is applied to OSC with the consideration of reducing complexity. Subsequently, in Lin et al. (2018) and Xin et al. (2018), we extend that approach to involve the analytical Cartesian impedance controller proposed in Albu-Schaffer et al. (2003) and QP optimization. This paper is a further extension of our previous works under the framework of projected inverse dynamics.

The formulation proposed in this manuscript is a synergistic integration between the analytical Cartesian impedance controller and the QP. The method enables compensation of model errors while operating a trade-off strategy between multiple constraints, thus improving the locomotion capability traversing steep and slippery terrains. Specifically, the contributions of this paper come from combining the following components in a single control pipeline:

• Analytical Cartesian impedance control, based on the orthogonal projected inverse-dynamics, which allows us to analyze a legged robot using a mass-spring-damper model against disturbances.

• Perturbation estimation using the impedance behavior to evaluate disturbances including external forces and model errors.

• QP optimization that performs a trade-off strategy to relax trajectory tracking when feasibility constraints are close to violation, which is critical when walking in extreme environments.

Finally, the proposed pipeline and controller are extensively validated on the torque controllable quadruped robot ANYmal (see Figure 1) in various scenarios.

FIGURE 1

Figure 1. A quadruped robot, ANYmal, with potential external disturbances F_x and contact forces λ_c within friction cones during static walking.

2. Problem Formulation

In this section we consider the general problem of constrained inverse-dynamics for quadruped robots, including a discussion of the controllability of quadruped robots. Then we present the projected inverse-dynamics to deal with constrained dynamics, which we will use to derive our locomotion controller for quadruped robots.

2.1. Dynamic Model of Quadruped Robots

The rigid-body dynamics equation of a floating base system under contact constraints is

$\begin{array}{ll}\text{M}\ddot{\text{q}}+\text{h}=\text{B}\tau +{\text{J}}_c^{\top }\lambda & (1)\end{array}$

where $\text{q}={\begin{array}{cc}[{\text{x}}_b^{\top }& {\theta }^{\top }]\end{array}}^{\top }$ denotes the vector of actuated joint positions (θ ∈ ℝⁿ) and floating base position and orientation (x_b ∈ SE(3)), M ∈ ℝ^{(n+6) × (n+6)} is the inertia matrix, h ∈ ℝⁿ⁺⁶ is the generalized vector containing Coriolis, centrifugal and gravitational effects, τ ∈ ℝⁿ⁺⁶ is the vector of torques, ${\text{J}}_c\in {ℝ}^{k\times (n+6)}$ is the constraint Jacobian that describes k linearly independent constraints, λ ∈ ℝ^k is the generalized constraint force vector that enforces the condition of ${\text{J}}_c\ddot{\text{q}}+{\stackrel{\bullet }{\text{J}}}_c\stackrel{\bullet }{\text{q}}=0$ (or ${\text{J}}_c\stackrel{\bullet }{\text{q}}=0$), and

$\begin{array}{ll}\text{B}=\left[\begin{array}{cc}{0}_6& 0\\ 0& {\text{I}}_n\end{array}\right]& (2)\end{array}$

is the projector into actuated joint space with n dimensional identity matrix I_n.

It should be noted that J_c = [J_cb J_cs], whereby ${\text{J}}_{c_b}\in {ℝ}^{k\times 6}$ and ${\text{J}}_{c_s}\in {ℝ}^{k\times n}$. J_cb encodes the effect of contact constraints on the floating base, with its rank providing direct insights into the controllability properties of the robots (Mistry et al., 2008). For quadruped robots, with the assumption of point contacts, there is k = 3 × n_c where n_c denotes the number of legs in stance. The case of rank(J_cb) < 6 (such as during a trotting gait with only two stance legs, and static walking with three stance feet in a line) is called truly underactuated (Hutter et al., 2014) which means there are not enough contact constraints to independently control the six unactuated base coordinates x_b. The case of rank(J_c) − rank(J_cb) > 0 is called overconstrained which means we can use redundancy to control the constraint/internal forces. Static walking (except with co-linear feet) is a typical overconstrained case, as rank(J_c) − rank(J_cb) = 9 − 6 = 3 with one swing leg. The proposed controller incorporating QP aims to control the internal forces to satisfy contact constraints. Moreover, one should note that we cannot independently generate the contact forces for both static walking and trotting gait because of rank(J_c) − rank(J_cb) < rank(J_c).

2.2. Projected Inverse Dynamics

The dynamics Equation (1) can be divided into two subspaces: the motion space where the acceleration can be chosen freely, and the constraint space where the acceleration is fixed and constraint forces can be chosen. Working with the two subspaces allows us to satisfy contact constraints independently of joint motion. Paper (Aghili, 2005) proposes an orthogonal projector derived only from kinematic parameters without consideration of constraint dynamic consistency, leading to a much simplified solution compared to Sentis et al. (2010). The employment of this orthogonal projector is the key for analytical Cartesian impedance control and optimization variable reduction.

The constraint equation ${\text{J}}_c\stackrel{\bullet }{\text{q}}=\text{0}$ implies that any admissible generalized velocity must lie in the null space of matrix J_c, i.e., $\stackrel{\bullet }{\text{q}}\in {N}({\text{J}}_c)$. We can use the pseudoinverse of J_c to compute the orthogonal null space projector P as $\text{P}=\text{I}-{\text{J}}_c^{+}{\text{J}}_c$, such that ${R}(\text{P})={N}({\text{J}}_c)$, $\text{P}\stackrel{\bullet }{\text{q}}=\stackrel{\bullet }{\text{q}}$, $\text{P}{\text{J}}_c^{\top }=\text{0}$, and P² = P^⊤ = P. Since the vectors lying in ${N}({\text{J}}_c)$ denote the motions of torso and any end-effector in the air, we call ${N}({\text{J}}_c)$ the motion space. It should noted that any vector could be projected into motion space by pre-multiplying with the projection matrix P. Then the complimentary projection I − P projects vectors into the constraint space ${N}{({\text{J}}_c)}^{\perp }={R}({\text{J}}_c^{\top })$. Subsequently, we can split Equation (1) into two equations of two orthogonal spaces (motion space and constraint space) using the above two projectors:

$\begin{array}{ll}\text{P}\text{M}\ddot{\text{q}}+\text{P}\text{h}=\text{P}\text{B}\tau ={\tau }_m& (3)\end{array}$

$\begin{array}{ll}(\text{I }-\text{ P})(\text{M}\ddot{\text{q}}+\text{h})=(\text{I }-\text{ P})\text{B}\tau +{\text{J}}_c^{\top }\lambda ={\tau }_c+{\text{J}}_c^{\top }\lambda & (4)\end{array}$

and then torque command τ = τ_m + τ_c. Constraint torques τ_c in Equation (4) do not contribute to the motion of the system, but system motion will affect contact forces due to $\ddot{\text{q}}$ in Equation (4). We wish to invert PM in Equation (3) to solve $\ddot{\text{q}}$, and then substitute that result into (4) to determine contact forces. However, PM is not invertible due to the rank deficiency of P. The contact constraint dictates that $(\text{I }-\text{ P})\stackrel{\bullet }{\text{q}}=0$, and its derivative

$\begin{array}{ll}(\text{I }-\text{ P})\ddot{\text{q}}=\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}}& (5)\end{array}$

Substituting Equation (5) into Equation (3) yields

$\begin{array}{ll}{\text{M}}_c\ddot{\text{q}}+\text{Ph}-\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}}={\tau }_m& (6)\end{array}$

where M_c = PM + I − P is invertible and named as constraint inertia matrix. Then we get

$\begin{array}{ll}\ddot{\text{q}}={\text{M}}_c^{-1}({\tau }_m-\text{Ph}+\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}})& (7)\end{array}$

Substituting Equation (7) into Equation (4) yields the equation that can determine the contact forces

$\begin{array}{ll}{\text{J}}_c^{\top }\lambda =(\text{I }-\text{ P})\left[\text{M}{\text{M}}_c^{-1}({\tau }_m-\text{Ph}+\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}})+\text{h}\right]-{\tau }_c& (8)\end{array}$

By replacing τ_m and τ_c with their original definitions in terms of τ, Equation (8) becomes

$\begin{array}{ll}{\text{J}}_c^{\top }\lambda =(\text{I }-\text{ P})\left[-(\text{I }-\text{ M}{\text{M}}_c^{-1}\text{P})(\text{B}\tau -\text{h})+\text{M}{\text{M}}_c^{-1}\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}}\right]& (9)\end{array}$

It should be noted that the utility of Equation (7) is key for the derivation of the analytical impedance control law in section 3 and the reduction of optimization parameters in section 4 to τ instead of ${\begin{array}{cc}[{\ddot{\text{q}}}^{\top }& {\tau }^{\top }]\end{array}}^{\top }$.

3. Cartesian Impedance Control in Motion Space

3.1. Applying Cartesian Impedance Control to Underactuated Systems

Since contact forces have been removed from the projected inverse dynamics of Equation (3), we can design a Cartesian impedance controller to achieve a desired motion performance in motion space without considering contact constraints. First, for the four stance legs case, only the torso requires desired motion, the torso is therefore considered an end-effector. The objective of Cartesian impedance control is to dictate the disturbance response of the robot. If a given Cartesian location x of the end-effector is subject to an external disturbance F_x we would like to prescribe the motion at x as

$\begin{array}{ll}{\Lambda }_d\ddot{\text{e}}+{\text{D}}_d\stackrel{\bullet }{\text{e}}+{\text{K}}_d\text{e}={\text{F}}_x& (10)\end{array}$

where e = x − x_d is the motion error between the current pose and the desired pose, Λ_d, D_d and K_d are the desired Cartesian inertia, damping, and stiffness matrices that define the Cartesian impedance behavior. On the other hand, adding the external disturbance into the general dynamic equation Results in

$\begin{array}{ll}\text{M}\ddot{\text{q}}+\text{h}=\text{B}\tau +{\text{J}}_c^{\top }\lambda +{\text{J}}_x^{\top }{\text{F}}_x& (11)\end{array}$

where J_x is the Jacobian matrix mapping external forces/torques to joint space. Project Equation (11) into the motion space

$\begin{array}{ll}\text{P}\text{M}\ddot{\text{q}}+\text{P}\text{h}=\text{P}\text{B}\tau +\text{P}{\text{J}}_x^{\top }{\text{F}}_x& (12)\end{array}$

Next, we employ M_c to transform Equation (12) to operational space. Equation (12) can be rewritten as

$\begin{array}{ll}{\text{M}}_c\ddot{\text{q}}+\text{Ph}-\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}}=\text{PB}\tau +\text{P}{\text{J}}_x^{\top }{\text{F}}_x& (13)\end{array}$

Pre-multiplying by ${\text{J}}_x{\text{M}}_c^{-1}$ gives

$\begin{array}{ll}{\text{J}}_x\ddot{\text{q}}+{\text{J}}_x{\text{M}}_c^{-1}(\text{Ph}-\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}})={\text{J}}_x{\text{M}}_c^{-1}\text{PB}\tau +{\text{J}}_x{\text{M}}_c^{-1}\text{P}{\text{J}}_x^{\top }{\text{F}}_x& (14)\end{array}$

We replace ${\text{J}}_x\ddot{\text{q}}$ with $\ddot{\text{x}}-{\stackrel{\bullet }{\text{J}}}_x\stackrel{\bullet }{\text{q}}$ and multiply by ${\Lambda }_c={({\text{J}}_x{\text{M}}_c^{-1}\text{P}{\text{J}}_x^{\top })}^{-1}$

$\begin{array}{ll}{\Lambda }_c\ddot{\text{x}}+{\Lambda }_c{\text{J}}_x{\text{M}}_c^{-1}(\text{Ph}-\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}})-{\Lambda }_c{\stackrel{\bullet }{\text{J}}}_x\stackrel{\bullet }{\text{q}}={\Lambda }_c{\text{J}}_x{\text{M}}_c^{-1}\text{PB}\tau +{\text{F}}_x& (15)\end{array}$

where Λ_c is named as the constraint-consistent operational space inertia matrix. For the moment, we will assume full actuation B = I. In the latter, we will deal with the underactuation constraint separately. Then, if we replace all non-linear terms with h_c and τ with ${\text{J}}_x^{\top }\text{F}$, we can compactly write the above equation as

$\begin{array}{ll}{\Lambda }_c\ddot{\text{x}}+{\text{h}}_c=\text{F}+{\text{F}}_x& (16)\end{array}$

where F is the operational forces due to actuation. In order to achieve the impedance response of Equation (10), we can define the control law as

$\begin{array}{ll}\text{F}={\text{h}}_c+{\Lambda }_c{\ddot{\text{x}}}_d-{\Lambda }_c{\Lambda }_d^{-1}({\text{D}}_d\stackrel{\bullet }{\text{e}}+{\text{K}}_d\text{e})+({\Lambda }_c{\Lambda }_d^{-1}-\text{I}){\text{F}}_x& (17)\end{array}$

We notice that if we sacrifice the ability to shape the inertia matrix by specifying Λ_d = Λ_c our control law simplifies to

$\begin{array}{ll}\text{F}={\text{h}}_c+{\Lambda }_c{\ddot{\text{x}}}_d-{\text{D}}_d\stackrel{\bullet }{\text{e}}-{\text{K}}_d\text{e}& (18)\end{array}$

The advantage of the above control law is that we do not need to measure the external disturbances F_x (e.g., with a force/torque sensor) to realize the impedance response.

On the other hand, by assigning Λ_d = Λ_c, the impedance gains of Equation (10) can still be tuned by analytical techniques based on different configurations (see in Angelini et al., 2019), whereas HQP controllers rely on manual trial and error tuning.

Remark. It should be noted that if J_x is rank deficient, ${\text{J}}_x{\text{M}}_c^{-1}\text{P}{\text{J}}_x^{\top }$ is not invertible, and we cannot determine ${\Lambda }_c={({\text{J}}_x{\text{M}}_c^{-1}\text{P}{\text{J}}_x^{\top })}^{-1}$. This may happen, for example, for the case when the robot is trotting (only two point feet on the ground) or for a swing leg when the leg is in singularity configuration. In these cases we can approximate Λ_c by computing a singularity robust inverse of ${\text{J}}_x{\text{M}}_c^{-1}\text{P}{\text{J}}_x^{\top }$, for example, by Singular Value Decomposition (SVD) with zero or near-zero eigenvalues removed.

3.2. Imposing Underactuation Constraints

The previous derivation of the control law (Equation 18) for Cartesian impedance control is based on an assumption of B = I. In fact, a quadruped robot is a typical underactuated system, i.e., the definition of B in Equation (2) is not an identity matrix. We need to satisfy the additional underactuation constraint

$\begin{array}{ll}{\tau }_m=\text{B}{\tau }_m& (19)\end{array}$

Our control law of Equation (18) is defined in operational space. Although in general ${\text{J}}_x^{\top }\text{F}\ne \text{B}{\text{J}}_x^{\top }\text{F}$, we may still be able to satisfy (19) by adding constraint forces in constraint space

$\begin{array}{ll}{\tau }_m={\text{J}}_x^{\top }\text{F}+(\text{I }-\text{ P}){\tau }_0=\text{B}{\text{J}}_x^{\top }\text{F}+\text{B}(\text{I }-\text{ P}){\tau }_0& (20)\end{array}$

and we can solve for τ₀

$\begin{array}{ll}(\text{I }-\text{ B}){\text{J}}_x^{\top }\text{F}=-(\text{I }-\text{ B})(\text{I }-\text{ P}){\tau }_0& (21)\end{array}$

$\begin{array}{ll}{\tau }_0=-{\left[(\text{I }-\text{ B})(\text{I }-\text{ P})\right]}^{+}(\text{I }-\text{ B}){\text{J}}_x^{\top }\text{F}& (22)\end{array}$

Subsequently, the Cartesian impedance control law in motion space with consideration of underactuation becomes

$\begin{array}{ll}{\tau }_m={\text{J}}_x^{\top }\text{F}-(\text{I }-\text{ P}){\left[(\text{I }-\text{ B})(\text{I }-\text{ P})\right]}^{+}(\text{I }-\text{ B}){\text{J}}_x^{\top }\text{F}& (23)\end{array}$

Furthermore, as described in Mistry and Righetti (2012), it could be simplified to

$\begin{array}{ll}{\tau }_m={(\text{PB})}^{+}{\text{J}}_x^{\top }\text{F}& (24)\end{array}$

where F is defined in Equation (18).

The above Cartesian impedance controller could also be applied to swing legs by replacing J_x with corresponding Jacobian matrix of a swing foot. If we define the Jacobian matrix of torso and one swing leg as J_b and J_s, respectively, the only difference is ${\text{J}}_s\in {ℝ}^{3\times (n+6)}$ due to the point foot while ${\text{J}}_b\in {ℝ}^{6\times (n+6)}$. Furthermore, let ${\text{F}}_b\in {ℝ}^6$ and ${\text{F}}_s\in {ℝ}^3$ represent the control law of Equation (18) for torso and swing feet, resulting in the torque command of motion space as follows

$\begin{array}{ll}{\tau }_m={(\text{PB})}^{+}({\text{J}}_b^{\top }{\text{F}}_b+{\displaystyle \sum _{i=0}^{n_{legs}}}\omega {\text{J}}_{s,i}^{\top }{\text{F}}_{s,i})& (25)\end{array}$

where weight ω is 1 when leg i is in swing, and 0 otherwise. Figure 2 depicts the case of two Cartesian impedance controllers applied to base and one foot, respectively.

FIGURE 2

Figure 2. The base pose and the swing foot position are controlled by two Cartesian impedance controllers.

3.3. Model Error Estimation and Compensation

One of the benefits of Cartesian impedance control is that the impedance response of Equation (10) provides us with a way to estimate the disturbances using motion errors

$\begin{array}{ll}{\widehat{\text{F}}}_x={\Lambda }_d\ddot{\text{e}}+{\text{D}}_d\stackrel{\bullet }{\text{e}}+{\text{K}}_d\text{e}& (26)\end{array}$

It should be noted that ${\widehat{\text{F}}}_x$ contains external disturbances as well as model errors. Considering the existing model error, the control law of Equation (18) becomes

$\begin{array}{ll}\text{F}={\widehat{\text{h}}}_c+{\widehat{\Lambda }}_c{\ddot{\text{x}}}_d-{\text{D}}_d\stackrel{\bullet }{\text{e}}-{\text{K}}_d\text{e}& (27)\end{array}$

where ${\widehat{\text{h}}}_c$ and ${\widehat{\Lambda }}_c$ denote the approximate items in computation. Substituting Equation (27) into operational space dynamics of Equation (16) yields

$\begin{array}{ll}{\widehat{\Lambda }}_c\ddot{\text{e}}+{\text{D}}_d\stackrel{\bullet }{\text{e}}+{\text{K}}_d\text{e}=({\text{h}}_c-{\widehat{\text{h}}}_c)+({\Lambda }_c-{\widehat{\Lambda }}_c)\ddot{\text{x}}+{\text{F}}_x& (28)\end{array}$

Further, the above equation implies ${\widehat{\text{F}}}_x=({\text{h}}_c-{\widehat{\text{h}}}_c)+({\Lambda }_c-{\widehat{\Lambda }}_c)\ddot{\text{x}}+{\text{F}}_x$ where not only external disturbances are involved, but also model errors. In the case where small external disturbances exist, Equation (28) can be used to estimate model errors.

This feature is useful for legged robots to carry unknown objects. If we feed the model error (e.g., caused by adding an object of unknown mass onto the robot) back into the control law of Equation (18), the position errors due to model errors will be removed. Therefore, the robot may carry unknown objects by compensating for estimated model errors. The control law in motion space with model-error compensation becomes

$\begin{array}{ll}\text{F}={\widehat{\text{h}}}_c+{\widehat{\Lambda }}_c{\ddot{\text{x}}}_d-{\text{D}}_d\stackrel{\bullet }{\text{e}}-{\text{K}}_d\text{e}+{\widehat{\text{F}}}_x& (29)\end{array}$

where ${\widehat{\text{F}}}_x$ is constant by computing Equation (28) once at the beginning of switching on model error compensation.

By contrast, HQP controllers cannot use motion errors to estimate disturbances because the gains of HQP controllers have different scaling. We will verify this point in the experimental section.

4. Quadratic Programming

The control scheme outlined above does not yet impose physical feasibility on the contact forces and joint torques. In this section, these constraints are introduced, and we present our quadratic programming approach to handle them.

4.1. Physical Constraints

The contact forces at the feet should be sufficient to prevent the separation or sliding of the contact. Additionally, motor torques typically have saturation values. To formalize the contact force constraint, the force at each foot is divided into tangential components λ_x and λ_y, and normal component λ_z. This leads to the following constraints:

• Unilateral constraint to avoid loss of contact

$\begin{array}{ll}{\lambda }_z\ge 0& (30)\end{array}$

• Friction cone constraint to avoid slipping

$\begin{array}{ll}\mu {\lambda }_z\ge \sqrt{{\lambda }_x^2+{\lambda }_y^2}& (31)\end{array}$

where μ is the friction coefficient at the contact point.

The saturation of torque command gives.

$\begin{array}{ll}{\tau }_{\text{min}}\le \tau \le {\tau }_{\text{max}}& (32)\end{array}$

4.2. Optimization Formulation

Our previous work (Xin et al., 2018) minimized a quadratic cost function of the torque commands, thereby minimizing the torques.

$\begin{array}{ll}{\displaystyle \underset{\tau }{\text{minimize}}}\frac{1}{2}\left|\right|\tau |{|}_2^2& (33)\end{array}$

where τ = τ_m + τ_c. To track trajectories and to deal with external disturbances, the Cartesian impedance controller (25) defines the motion space torque command τ_m. This partial command is included in the optimization using the linear equality constraint:

$\begin{array}{ll}\text{PB}\tau ={\tau }_m& (34)\end{array}$

The friction forces are computed from the optimization variable τ based on (9) and simplified to be

$\begin{array}{ll}\lambda =\eta +\rho & (35)\end{array}$

where $\eta =-{({\text{J}}_c^{\top })}^{+}(\text{I }-\text{ P})(\text{I }-\text{ M}{\text{M}}_c^{-1}\text{P})\text{B}\tau$ and $\rho ={({\text{J}}_c^{\top })}^{+}(\text{I }-\text{ P})\left[(\text{I }-\text{ M}{\text{M}}_c^{-1}\text{P})\text{h}+\text{M}{\text{M}}_c^{-1}\stackrel{\bullet }{\text{P}}\stackrel{\bullet }{\text{q}}\right]$. Then we can transform the unilateral constraint and friction cone constraint to be written in terms of decision variable τ. The quadratic programming problem is now formulated as:

$\begin{array}{ll}\begin{array}{l}{\displaystyle \underset{\tau }{\text{minimize}}}\frac{1}{2}\left|\right|\tau |{|}_2^2\\ \text{subject to}\text{ PB}\tau ={\tau }_m\\ {\eta }_z^i+{\rho }_z^i\ge 0\\ \mu ({\eta }_z^i+{\rho }_z^i)\ge \sqrt{{({\eta }_x^i+{\rho }_x^i)}^2+{({\eta }_y^i+{\rho }_y^i)}^2}\\ {\tau }_{\text{min}}\le \tau \le {\tau }_{\text{max}}\end{array}& (36)\end{array}$

where the index i signifies the ith stance leg.

4.3. Adding Trade-Off Between Multiple Constraints

In practice, there may not be a physically feasible solution that achieves both the desired motion and impedance behavior. For example, tangential contact forces may increase due to the desired acceleration for walking forward, causing contact forces to violate the friction cone. To avoid a need for replanning the desired motion, we propose a trade-off between the constraints. The inequality constraints cannot be traded-off because they enforce physical limitations and stability of the system. On the other hand, the equality constraint enforces the operational space tasks. We will sacrifice motion performance to ensure feasibility of the physical (inequality) constraints when there is no possibility of finding a solution that satisfies both types of constraints. Here, we employ a trade-off of the trajectory tracking control to seek a compromise in low level control. The trade-off is implemented by moving the equality constraint into the cost function to compose a least squares optimization.

$\begin{array}{ll}\begin{array}{l}{\displaystyle \underset{\tau }{\text{minimize}}}\frac{1}{2}\left|\right|\text{PB}\tau -{\tau }_m|{|}_2^2\\ \text{subject to}{\eta }_z^i+{\rho }_z^i\ge 0\\ \mu ({\eta }_z^i+{\rho }_z^i)\ge \sqrt{{({\eta }_x^i+{\rho }_x^i)}^2+{({\eta }_y^i+{\rho }_y^i)}^2}\\ {\tau }_{\text{min}}\le \tau \le {\tau }_{\text{max}}\end{array}& (37)\end{array}$

The above formulation means that the QP optimization will try to find the admissible torques that are as close as possible to the torque command of motion space required for the desired motion performance. It should be noted that if the QP needs to trade-off the motion performance it will change the swing trajectory more than the torso trajectory, as the torso contributes more to contact forces than the swing leg. Alternatively, we could add a weighting matrix to balance the modification of different parts if it is necessary. However, we would then need to tune the weighting matrix in practice. Therefore, we recommend using a null space projection of the swing legs to project the cost function, in order to enforce accurate swing leg performance. After the QP provides the optimized torque command based on the torso motion, we will add the dynamic consistent torques for the swing legs. The QP in the null space of the swing legs becomes

$\begin{array}{ll}\begin{array}{l}{\displaystyle \underset{\tau }{\text{minimize}}}\frac{1}{2}\left|\right|{\text{N}}_s(\text{PB}\tau -{\tau }_m)|{|}_2^2\\ \text{subject to}{\eta }_z^i+{\rho }_z^i\ge 0\\ \mu ({\eta }_z^i+{\rho }_z^i)\ge \sqrt{{({\eta }_x^i+{\rho }_x^i)}^2+{({\eta }_y^i+{\rho }_y^i)}^2}\\ {\tau }_{\text{min}}\le \tau \le {\tau }_{\text{max}}\end{array}& (38)\end{array}$

where ${\text{N}}_s=\text{I}-{\text{J}}_s^{+}{\text{J}}_s$, J_s = J_{s, 1} for static walking gait, ${\text{J}}_s=\left(\begin{array}{c}{\text{J}}_{s,1}\\ {\text{J}}_{s,2}\end{array}\right)$ for the gaits with only two supporting legs, such as trotting gait, and J_s = 0 for four stance legs case. The final torque command will be

$\begin{array}{ll}\tau ={\tau }^{*}+(\text{I}-{\text{N}}_s){\tau }_m& (39)\end{array}$

The controller proposed above is represented as a block diagram in Figure 3.

FIGURE 3

Figure 3. An overview of the proposed locomotion controller with Cartesian impedance control and quadratic programming subject to physical constraints.

Remark. One notices that the friction cone constraints are quadratic constraints. Therefore, the quadratic programming of Equation (38) is quadratically constrained quadratic programming. In order to use standard active set methods, we approximate the friction cone with a linearized 4-edge pyramid. This results in a linearly constrained quadratic programming problem, which is readily solved (Goldfarb and Idnani, 1983).

5. Experiments and Discussion

To validate our proposed controller, three sets of experiments were performed: a comparison the HQP controller, model error compensation, and walking on challenging terrains. This section treats each of the experiments in turn, and provides a setup description, results, and discussion for all of them.

The experiments have been performed using ANYmal, a torque-controlled quadruped robot made by ANYbotics. The robot weights ~35 kg and has 12 joints actuated by the Series Elastic Actuator (SEA). The control cycle runs at 400Hz. The on-board computer running controllers has an Intel 4th generation (Haswell ULT) i7-4600U (1.4–2.1GHz) processor and two HX316LS9IBK2/16 DDR3L memory cards. Experiment videos can be found in the Supplementary Video 1.

5.1. Comparison With HQP Controller

To show the efficacy of our proposed controller, we compare it with an HQP-computer. This comparison considers both the computational efficiency and the capacity to estimate external disturbances. Our implementation of the HQP controller follows the original framework from De Lasa et al. (2010), adding a dimension-reduction to that framework in order to enhance computational efficiency. This dimension-reduction, proposed in Herzog et al. (2016) removes the motor-torque from the decision variables by using the equations of motion of the robot. The remaining decision variables are the torso and joint accelerations and the contact forces. An active-set QP solver was employed to solve QPs in both controllers.

5.1.1. Computational Efficiency

To evaluate the computational efficiency of the proposed whole-body controller against the HQP-controller, we compare their execution time on the robot hardware. A comparison of average computation times while executing a fixed static walking gait is provided in Table 1.

TABLE 1

Table 1. The computation efficiency comparison between proposed controller (P.C.) and HQP controller during static walking.

The computation time analysis shows that our controller is faster than the HQP controller. This is expected since our controller only requires one QP although it computes a few pseudo-inverses of matrices. By contrast, HQP needs to compute more QPs and also needs to compute null-space projectors which include pseudo-inverses as well. In fact, HQP needs to solve at least two QPs for dynamic feasibility and trajectory tracking tasks. In order to save energy, a third QP has to be added to minimize torque commands. Usually, those are the three fundamental QPs required by HQP controllers (Bellicoso et al., 2019). For walking gaits, if we separate swing legs and torso into different prioritized tasks, one more QP has to be solved. In this experiment, the HQP controller solved three QPs for the four-legged stance phase and four QPs during a one leg swing. In Herzog et al. (2016), the first QP is solved by pseudo-inverse when moving friction cone constraints into a lower priority, which aims to save computation time. But, based on Table 1, the computation time of HQP is still greater than the proposed controller, even omitting the computation time of QP₁. Furthermore, Herzog et al. (2016) also suggests parallelizing the computations of null-space projectors and QPs. In that case, the total computation time of HQP will be the sum of solving QPs, which is close to our controller's according to Table 1. What's more, as pointed out in Escande et al. (2014) and Herzog et al. (2016), the size of decision variables can be reduced over the levels of hierarchies by additional SVD, which could potentially improve the computation efficiency of HQP even further. Readers should keep in mind that we did not employ that technique in the comparison of Table 1.

5.1.2. External Disturbance Estimation

The ability of the two controllers to estimate external disturbances is assessed on the robot by comparing their response to similar force perturbations while standing still. The operator pushed the torso along the vertical axis using about 30N force, and the recorded position displacements are used to estimate the external perturbation received by the robot. As would happen in practice, we only use the multiplication of K_d and displacement errors as the estimated disturbance instead of the entire Equation (28) since accelerations and velocities are noisy.

Figure 4 shows the measured pushing forces and torso heights, which have been obtained using the gains reported in Table 2. The position errors caused by pushing forces are similar between the two controllers, which means they produce approximately the same effective stiffness. The force estimated by the proposed controller is K_d,ze_z ≈ 35N. This estimation is close to the measured pushing force 30N. However, the relationship between the effective stiffness and the controller gains is not straightforward for the HQP-controller. For this controller, the gains map from an error to a desired acceleration, not to a desired force, as is the case for our proposed controller. As a result, the K_d gains of the HQP controller are much smaller than the ones of the proposed controller, meaning the HQP estimated force is 1.6N which is much lower than the received external perturbation. A conversion factor could be found, either based on the equations of motion or using experiments, such as this one. However, the conversion factor would depend on the robot configuration, which means the HQP-controller would require extensive tuning to be able to perform disturbance estimation. Therefore, the proposed controller is more suitable for performing the forces estimation for the compensation of constant external perturbations, such as the additional load carried by the robot.

FIGURE 4

Figure 4. The pushing forces and torso positions when pushing the robot down from standing still. (A) Measured and estimated pushing forces when running HQP controller. (B) Torso height during pushing with HQP running. (C) Measured and estimated pushing forces when running the proposed controller. (D) Torso height during pushing with the proposed controller running. The similarity in the response of both controllers shows that the effective torso stiffness is roughly equal, despite the large difference in gains used. HQP cannot estimate disturbance forces whereas the estimated force of our controller is roughly close to the measured force.

TABLE 2

Table 2. Gains for HQP controller and the proposed controller used in the stiffness comparison experiment.

5.2. Model Error Compensation

Legged robots may be deployed to move small loads which may not always be known; thus, it is important to adapt the robot behavior to maintain stable and efficient locomotion. Equation (29) describes how the proposed controller may be modified to use the external perturbation estimation to perform a model error compensation. For the experiment, the robot was loaded with a 10 kg mass and walked with and without activating the model compensation, which has been enabled after about 23 s. Table 3 contains the gains of the proposed controller in the experiment. The recorded video for this experiment can be found in the Supplementary Material.

TABLE 3

Table 3. Gains of the proposed controller during carrying weight experiment.

Figure 5 shows the snapshots from the experiments of adding weight to walking with it. Figure 6 is the recorded position error along the z axis during the experiment. After enabling the compensation, the torso immediately rose as shown in Figure 6 and continued its locomotion with a reduced position error. It should be noted that the estimated model error is about 65N. A bias of 35N has been added in this experiment to account for friction and other uncertainties of a real robot. The results demonstrate that the position error can be reduced by model error compensation when carrying unknown objects, which is useful for deployment of legged robots in th real world. However, an important upgrade required to deploy this feature in a real scenario is continuous real-time model adaptation, which has currently been left as future work to develop.

FIGURE 5

Figure 5. Snapshots of carrying 10 kg weight with model error compensation. (A) Before adding the weight. (B) After adding the weight, the torso went down. (C) The torso went up after enabling model error compensation. (D) Walking with the object with model error compensation.

FIGURE 6

Figure 6. Measured torso height during carrying 10 kg weight. At 10 s, the weight is slowly lowered onto the robot, resulting in an increasing position error. At 23 s the force compensation is enabled, after which the robot takes two steps. The small difference between the initial and final position error shows the efficacy of the disturbance compensation.

5.3. Walking on Challenging Terrains

We tested the proposed controller for its ability to tackle challenging terrains. We tested walking on a low friction surface (PTFE Sheets, fiction coefficient of 0.22 as shown in Figure 7), and the feasibly of climbing a significant incline (30° slope in Figure 8). As perception is not the focus of this work, the friction coefficient and slope angle parameters, to be used by the controller in the optimization stage, are set manually. Locomotion performance for these tasks may be seen in the accompanying video.

FIGURE 7

Figure 7. ANYmal walking on slippery synthetic ice. The estimated friction coefficient between ANYmal's feet and the synthetic ice is 0.22. The friction coefficient in our controller needs to be ≤0.2 in order to avoid slipping on the synthetic ice.

FIGURE 8

Figure 8. ANYmal climbing up on a 30° slope.

We did not alter the step planning in our experiment, but rather used the default ANYbotics static gait planner. This means the planned locomotion trajectory was not optimized for our particular tasks or evaluated for feasibility. Thus, during execution, if the controller does not trade-off trajectory tracking and constraint satisfaction, the robot may slip due to the inability of the QP finding a feasible solution. We further evaluate this trade-off by analyzing trajectory tracking errors when statically walking on flat terrain, while varying the friction coefficient parameter used by the controller.

As shown in Figure 9, the trajectory tracking error along the z direction has an occasional sharp rise particularly when the friction coefficient μ is equal to 0.1 and 0.08. This indicates that trajectory tracking performance is sacrificed in order to maintain the friction cone constraint. We also see that the cost function value of QP increases, due to the increase of trajectory tracking error. In theory, the value of cost function should be zero if the trade-off is not triggered. The model error explains why the deviation between PBτ and τ_m is not equal to zero even when μ = 0.5. The controller in our previous work (Xin et al., 2018) does not have this trade-off feature, and this is an improvement over our previous work.

FIGURE 9

Figure 9. Tracking performance when walking with a static gait on high-traction flat terrain for different values of the friction coefficient used by the controller. (A) Position tracking error of x direction. (B) Position tracking error of y direction. (C) Position tracking error of z direction. (D) Deviation between PBτ and τ_m. The increasing errors for small friction coefficients show the trade-off in tracking performance made to guarantee feasibility of the friction cone constraint. In particular, a behavior emerges in which the controller deviates from the target motion in z direction, in order to increase normal contact forces when walking with a small friction coefficient. The trade-off between tracking error and constraint satisfaction is also visible in the increasing deviation between PBτ and τ_m.

6. Conclusions

A semi-analytical locomotion controller incorporating an analytical Cartesian impedance controller and a QP optimization for quadruped robots is presented in the paper. Cartesian impedance controllers can track desired end-effector trajectories and can also be used to estimate external disturbances. The disturbance estimation is applied to model error compensation in the case of making a robot carry unknown objects. The QP optimization guarantees physical feasibility by sacrificing trajectory tracking if the torque command required to track the desired trajectory does not satisfy physical constraints. The benefit of this trade-off strategy enables the robot to walk on synthetic ice and a 30° ramp. Besides, the computation time is verified to be less than other fully optimization-based whole-body controllers since we only need to solve one QP with less decision variables compared to other controllers. Future work will focus on applying this control framework to dynamic gaits, such as trotting.

Data Availability Statement

All datasets generated for this study are included in the article/Supplementary Material.

Author Contributions

GX, H-CL, and MM significantly contributed to the development of the presented approach. GX, WW, and CT added the least-square optimization to the framework. All authors have contributed to the writing and they have reviewed the submitted version.

Funding

This work has been supported by the following grants: EPSRC UK RAI Hub NCNR (EPR02572X/1) and ORCA (EP/R026173/1), THING project in the EU Horizon 2020 (ICT-2017-1).

Conflict of Interest

The authors declare that the research was conducted in the absence of any commercial or financial relationships that could be construed as a potential conflict of interest.

Supplementary Material

The Supplementary Material for this article can be found online at: https://www.frontiersin.org/articles/10.3389/frobt.2020.00048/full#supplementary-material

Supplementary Video 1. Model error compensation and walking on challenging terrains of a quadruped robot.

References

Aghili, F. (2005). A unified approach for inverse and direct dynamics of constrained multibody systems based on linear projection operator: applications to control and simulation. IEEE Trans. Robot. 21, 834–849. doi: 10.1109/TRO.2005.851380

CrossRef Full Text | Google Scholar

Albu-Schaffer, A., Ott, C., Frese, U., and Hirzinger, G. (2003). Cartesian impedance control of redundant robots: recent results with the DLR-light-weight-arms. IEEE Int. Conf. Robot. Autom. 3, 3704–3709. doi: 10.1109/ROBOT.2003.1242165

CrossRef Full Text | Google Scholar

Angelini, F., Xin, G., Wolfslag, W. J., Tiseo, C., Mistry, M., Garabini, M., et al. (2019). “Online optimal impedance planning for legged robots,” in IEEE International Conference on Intelligent Robots and Systems (Macau). doi: 10.1109/IROS40897.2019.8967696

CrossRef Full Text | Google Scholar

Bellicoso, C. D., Gehring, C., Hwangbo, J., Fankhauser, P., and Hutter, M. (2016). “Perception-less terrain adaptation through whole body control and hierarchical optimization,” in IEEE-RAS International Conference on Humanoid Robots, (Cancun) 558–564. doi: 10.1109/HUMANOIDS.2016.7803330

CrossRef Full Text | Google Scholar

Bellicoso, C. D., Kramer, K., Stäuble, M., Sako, D., Jenelten, F., Bjelonic, F., et al. (2019). “Alma-articulated locomotion and manipulation for a torque-controllable robot,” in International Conference on Robotics and Automation (ICRA 2019) (Montreal). doi: 10.1109/ICRA.2019.8794273

CrossRef Full Text | Google Scholar

Boaventura, T., Semini, C., Buchli, J., Frigerio, M., Focchi, M., and Caldwell, D. G. (2012). “Dynamic torque control of a hydraulic quadruped robot,” in 2012 IEEE International Conference on Robotics and Automation (Saint Paul, MN: IEEE), 1889–1894. doi: 10.1109/ICRA.2012.6224628

CrossRef Full Text | Google Scholar

De Lasa, M., Mordatch, I., and Hertzmann, A. (2010). Feature-based locomotion controllers. ACM Trans. Graph. 29:131. doi: 10.1145/1778765.1781157

CrossRef Full Text | Google Scholar

Escande, A., Mansard, N., and Wieber, P.-B. (2014). Hierarchical quadratic programming: fast online humanoid-robot motion generation. Int. J. Robot. Res. 33, 1006–1028. doi: 10.1177/0278364914521306

CrossRef Full Text | Google Scholar

Gehring, C., Coros, S., Hutter, M., Bloesch, M., Hoepflinger, M. A., and Siegwart, R. (2013). “Control of dynamic gaits for a quadrupedal robot,” in 2013 IEEE International Conference on Robotics and Automation (Karlsruhe: IEEE), 3287–3292. doi: 10.1109/ICRA.2013.6631035

CrossRef Full Text | Google Scholar

Goldfarb, D., and Idnani, A. (1983). A numerically stable dual method for solving strictly convex quadratic programs. Math. Program. 27, 1–33. doi: 10.1007/BF02591962

CrossRef Full Text | Google Scholar

Herzog, A., Rotella, N., Mason, S., Grimminger, F., Schaal, S., and Righetti, L. (2016). Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid. Auton. Robots 40, 473–491. doi: 10.1007/s10514-015-9476-6

CrossRef Full Text | Google Scholar

Hutter, M., Sommer, H., Gehring, C., Hoepflinger, M., Bloesch, M., and Siegwart, R. (2014). Quadrupedal locomotion using hierarchical operational space control. Int. J. Robot. Res. 33, 1047–1062. doi: 10.1177/0278364913519834

CrossRef Full Text | Google Scholar

Khatib, O. (1987). A unified approach for motion and force control of robot manipulators: the operational space formulation. IEEE J. Robot. Autom. 3, 43–53. doi: 10.1109/JRA.1987.1087068

CrossRef Full Text | Google Scholar

Lee, Y., Hwang, S., and Park, J. (2016). Balancing of humanoid robot using contact force/moment control by task-oriented whole body control framework. Auton. Robots 40, 457–472. doi: 10.1007/s10514-015-9509-1

CrossRef Full Text | Google Scholar

Lin, H.-C., Smith, J., Babarahmati, K. K., Dehio, N., and Mistry, M. (2018). “A projected inverse dynamics approach for multi-arm cartesian impedance control,” in IEEE International Conference on Robotics and Automation (Brisbane). doi: 10.1109/ICRA.2018.8461202

CrossRef Full Text | Google Scholar

Mansard, N. (2012). “A dedicated solver for fast operational-space inverse dynamics,” in 2012 IEEE International Conference on Robotics and Automation (ICRA) (Saint Paul, MN: IEEE), 4943–4949. doi: 10.1109/ICRA.2012.6224851

CrossRef Full Text | Google Scholar

Mistry, M., Buchli, J., and Schaal, S. (2010). “Inverse dynamics control of floating base systems using orthogonal decomposition,” in 2010 IEEE International Conference on Robotics and Automation (ICRA) (Anchorage, AK: Citeseer), 3406–3412. doi: 10.1109/ROBOT.2010.5509646

CrossRef Full Text | Google Scholar

Mistry, M., Nakanishi, J., Cheng, G., and Schaal, S. (2008). “Inverse kinematics with floating base and constraints for full body humanoid robot control,” in Humanoids 2008-8th IEEE-RAS International Conference on Humanoid Robots (Daejeon: IEEE), 22–27. doi: 10.1109/ICHR.2008.4755926

CrossRef Full Text | Google Scholar

Mistry, M., and Righetti, L. (2012). “Operational space control of constrained and underactuated systems,” in Robotics: Science and Systems VII (Sydney), 225–232. doi: 10.15607/RSS.2011.VII.031

CrossRef Full Text | Google Scholar

Pratt, J., Chew, C.-M., Torres, A., Dilworth, P., and Pratt, G. (2001). Virtual model control: an intuitive approach for bipedal locomotion. Int. J. Robot. Res. 20, 129–143. doi: 10.1177/02783640122067309

CrossRef Full Text | Google Scholar

Saab, L., Ramos, O. E., Keith, F., Mansard, N., Soueres, P., and Fourquet, J. Y. (2013). Dynamic whole-body motion generation under rigid contacts and other unilateral constraints. IEEE Trans. Robot. 29, 346–362. doi: 10.1109/TRO.2012.2234351

CrossRef Full Text | Google Scholar

Sentis, L., and Khatib, O. (2006). “A whole-body control framework for humanoids operating in human environments,” in ICRA 2006. Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006 (Orlando, FL: IEEE), 2641–2648. doi: 10.1109/ROBOT.2006.1642100

CrossRef Full Text | Google Scholar

Sentis, L., Park, J., and Khatib, O. (2010). Compliant control of multicontact and center-of-mass behaviors in humanoid robots. IEEE Trans. Robot. 26, 483–501. doi: 10.1109/TRO.2010.2043757

CrossRef Full Text | Google Scholar

Xin, G., Lin, H.-C., Smith, J., Cebe, O., and Mistry, M. (2018). “A model-based hierarchical controller for legged systems subject to external disturbances,” in IEEE/RSJ International Conference on Robotics and Automation (Brisbane), 4375–4382. doi: 10.1109/ICRA.2018.8461172

CrossRef Full Text | Google Scholar