Robust Whole-Body Motion Control of Legged Robots

TL;DR

This paper presents a robust whole-body motion control architecture for legged robots that ensures stability and performance despite model uncertainties and delays, verified on a quadruped robot.

Contribution

It introduces a robust contact force control method combined with task space decomposition for improved stability and performance in legged robot control.

Findings

Robust control guarantees stability despite model mismatches and delays.

Task space decomposition reduces coupling effects between controllers.

Experimental validation shows improved performance over standard inverse dynamics.

Abstract

We introduce a robust control architecture for the whole-body motion control of torque controlled robots with arms and legs. The method is based on the robust control of contact forces in order to track a planned Center of Mass trajectory. Its appeal lies in the ability to guarantee robust stability and performance despite rigid body model mismatch, actuator dynamics, delays, contact surface stiffness, and unobserved ground profiles. Furthermore, we introduce a task space decomposition approach which removes the coupling effects between contact force controller and the other non-contact controllers. Finally, we verify our control performance on a quadruped robot and compare its performance to a standard inverse dynamics approach on hardware.

Figures

Equations

$$\displaystyle\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}(\mathbf{q})=\mathbf{S}^{\top}\mbox{\boldmath$\tau$}+\mathbf{J}^{\top}_{c_{\bot}}\mbox{\boldmath$\lambda$}_{c_{\bot}}+\mathbf{J}^{\top}_{c_{\parallel}}\mbox{\boldmath$\lambda$}_{c_{\parallel}}$$

$$\displaystyle\mathbf{J}_{c_{\parallel}}\dot{\mathbf{q}}=0,\quad\mathbf{J}_{c_{\bot}}\dot{\mathbf{q}}=0,\quad\mbox{\boldmath$\lambda$}_{\bot}\geq 0$$

$$\mathbf{M}_{x}(\mathbf{q})\ddot{\mathbf{x}}+\mathbf{C}_{x}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{x}(\mathbf{q})=\mathbf{S}_{x}^{\top}\mbox{\boldmath$\tau$}+\mathbf{J}_{c,x}^{\top}\mbox{\boldmath$\lambda$},$$

$$\begin{array}{rl}& {\mathbf{M}}_x={\mathbf{J}}_x^{†\top }\mathbf{M}{\mathbf{J}}_x^{†}\\ & {\mathbf{C}}_x={\mathbf{J}}_x^{†\top }\mathbf{C}-{\mathbf{M}}_x{\dot{\mathbf{J}}}_x\dot{\mathbf{q}}\\ & {\mathbf{G}}_x={\mathbf{J}}_x^{†\top }\mathbf{G}\end{array}\begin{array}{rl}& {\mathbf{S}}_x=\mathbf{S}{\mathbf{J}}_x^{†}\\ & {\mathbf{J}}_{c,x}={\mathbf{J}}_c{\mathbf{J}}_x^{†}\\ & {\mathbf{J}}_x^{†}={\mathbf{M}}^{-1}{\mathbf{J}}_x^{\top }{\left({\mathbf{J}}_x{\mathbf{M}}^{-1}{\mathbf{J}}_x^{\top }\right)}^{-1}\end{array}$$

$$\mathbf{C}_{c}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{c}(\mathbf{q})=\mathbf{S}_{c}^{\top}\mbox{\boldmath$\tau$}+\mbox{\boldmath$\lambda$},\qquad\mbox{\boldmath$\lambda$}_{\bot}\geq 0,$$

$$\mathbf{C}_{c}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{c}(\mathbf{q})=\mbox{\boldmath$\tau$}_{c}+\mbox{\boldmath$\lambda$},\qquad\mbox{\boldmath$\lambda$}_{\bot}\geq 0$$

$$\mbox{\boldmath$\lambda$}=-\mathbf{F},\qquad\mbox{\boldmath$\lambda$}_{\bot}\geq 0.$$

$${\tilde{H}}_p(s)=\frac{{\lambda }_i}{u_i}=-\frac{e^{-\widetilde{{\eta }_d}s}}{\tilde{\eta }s+1},$$

$$\Pi =\left\{H_p(s)\mid \Vert \frac{H_p(j\omega )-{\tilde{H}}_p(j\omega )}{{\tilde{H}}_p(j\omega )}\Vert \le \overline{\text{l}}(\omega )\right\}$$

$$\min {\Vert e|}_2^2={\int }_0^{\infty }\Vert \lambda (t)-{\lambda }_r(t){\Vert }_2^{2}dt$$

$$f(s)=\frac{1}{{\eta }_{f}s+1}$$

$$\left|\overline{l}(\omega )f(i\omega )\right|+\left|\left(1-e^{-i{\eta }_d\omega }f(i\omega )\right)w\right|<1$$

$$\mathbf{M}_{com}(\mathbf{q})\ddot{\mathbf{x}}_{com}+\mathbf{C}_{com}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{com}(\mathbf{q})=\mathbf{J}_{c,com}^{\top}\mbox{\boldmath$\lambda$}$$

$$\mathbf{M}_{nc}(\mathbf{q})\ddot{\mathbf{x}}_{nc}+\mathbf{C}_{nc}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{nc}(\mathbf{q})=\tau_{nc}+\mathbf{J}_{c,nc}^{\top}\mbox{\boldmath$\lambda$},$$

$$\mbox{\boldmath$\tau$}=\mathbf{W}_{c}\mathbf{S}_{c}\Big{(}\mathbf{S}_{c}^{\top}\mathbf{W}_{1}\mathbf{S}_{c}\Big{)}^{-1}\!\mbox{\boldmath$\tau$}_{c}+\mathbf{W}_{nc}\mathbf{S}_{nc}\Big{(}\mathbf{S}_{nc}^{\top}\mathbf{W}_{2}\mathbf{S}_{nc}\Big{)}^{-1}\!\mbox{\boldmath$\tau$}_{nc}$$

$${\mathbf{W}}_c$$

$${\mathbf{W}}_{nc}$$

$$\mathbf{M}_{c}\ddot{\mathbf{q}}_{c}+\mathbf{C}_{c}+\mathbf{G}_{c}=\mathbf{S}_{c}^{\top}\mbox{\boldmath$\tau$}+\mathbf{J}_{c,c}^{\top}\mbox{\boldmath$\lambda$}$$

$$\displaystyle\mathbf{S}_{c}^{\top}\mbox{\boldmath$\tau$}$$

$$\displaystyle=\mbox{\boldmath$\tau$}_{c}+\mathbf{S}_{c}^{\top}\mathbf{W}_{nc}\mathbf{S}_{nc}\left(\mathbf{S}_{nc}^{\top}\mathbf{W}_{nc}\mathbf{S}_{nc}\right)^{-1}\!\mbox{\boldmath$\tau$}_{nc}$$

$$\mathbf{S}_{c}^{\top}\mbox{\boldmath$\tau$}=\mbox{\boldmath$\tau$}_{c}+(1-\alpha_{c})\mathbf{S}_{c}^{\top}\mathbf{W}\mathbf{S}_{nc}\left(\mathbf{S}_{nc}^{\top}\mathbf{W}\mathbf{S}_{nc}\right)^{-1}\!\mbox{\boldmath$\tau$}_{nc}$$

$$\mathbf{M}_{com}(\mathbf{q})\ddot{\mathbf{x}}_{corr}+\mathbf{C}_{com}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{com}(\mathbf{q})=\mathbf{J}_{c,com}^{\top}\mbox{\boldmath$\lambda$}_{r}.$$

$$\displaystyle\underset{\mbox{\boldmath$\lambda$}_{r}}{\text{minimize}}$$

$$\displaystyle\mbox{\boldmath$\lambda$}_{Z}\geq 0,\quad\Sigma\left[\mbox{\boldmath$\lambda$}\right]\geq 0.$$

$$\left|k-\tilde{k}\right|\le \Delta k$$

$$\left|\eta -\tilde{\eta }\right|\le \Delta \eta$$

Full text

I INTRODUCTION

The robustness of motion controllers for legged robots has recently attracted a significant research interest. Generally in any robotic application, a generic motion control requires three main components namely, (i) trajectory planning which is responsible for generating reference trajectories, (ii) a state observer to estimate the state of the robot and (iii) a stabilizing feedback controller which is responsible for tracking the reference trajectories and rejecting disturbances. For any successful hardware implementation, the robustness should be considered in designing each of these three modules. Here, we focus on the robustness of the stabilizing controller.

Task space inverse dynamics is a popular approach for whole–body control of legged robots given that the reference trajectory has already been calculated [1, 2]. Hierarchical inverse dynamics based on cascades of quadratic programs has been proposed for the control of legged robots and implemented on torque controlled legged robots [3, 4]. Inverse kinematics based approaches with constraints and hierarchical control approach such as [5] have also been used for the control of legged robots. All of these methods are in general a versatile tool for implementing already existing reference trajectories. However, the main limitation of these methods is their assumption and reliance on ideal force tracking actuators. Thus, they are often susceptible to the actuators dynamics, model uncertainties, and delays.

In order to increase the robustness of a motion controller, different schemes have been proposed. A robust inverse dynamics method is proposed by [6, 7] which makes the task constraints less susceptible to the additive uncertainty in joint torques. In [8, 9], an adaptive control method has been used to estimate or improve the rigid body model in order to adapt the controller. Task space robustness of manipulator’s motion has been considered by [10, 11] where the proposed method guarantees stability in the presence of rigid body model mismatch and actuator dynamics. While torque control algorithms with high-performance [12] can reinforce the perfect actuator assumption to a certain extent, their performance is still not ideal. In this paper, we propose a feedback control structure which can increase the robustness of the controller in face of actuator dynamics, finite contact surface stiffness, and unobserved ground profile. We assume that the objective of the whole-body stabilizing controller is to track planned trajectories of the Center of Mass (CoM). Considering that the CoM motion is only affected by the external forces, a robust implementation of the given plan can be achieved by robustly controlling the robot’s interaction ports (e.g. footsteps or contact forces) with the environment.

The main contributions of this work are as follows: First, we introduce an Internal Model Control (IMC) structure to feedback control the end-effector (e.g. stance leg) contact force by considering the actuator dynamics and delays. Then, we demonstrate that we can achieve the robust stability and performance of our IMC contact force controller with the proper tuning of IMC; the tuning boils down to the selection of a single parameter. Furthermore, in order to decouple the contact force controller and the non-contact end-effector controller (e.g. swing leg), we introduce a general approach for either hierarchical or co-equal task space decomposition into subsystems. While this approach is formulated for fully-actuated legged robots extension to under-actuated cases is conceivable. Finally, we discuss the experimental results on our quadrupedal robot, HyQ. and compare it to a standard whole-body control method of legged robot used in [2].

II Background

In this section, we briefly introduce the task space modeling approach and model-based feedback force control structure which we will use later in our proposed method.

II-A Task Space

Assuming rigid body modelling, we can write the equations of motion of a robot as

[TABLE]

where $\mathbf{M}$, $\mathbf{C}$, $\mathbf{G}$, and $\mathbf{S}$ are respectively the generalized inertia matrix, Coriolis-centrifugal forces, generalized gravity forces, and selection matrix. $\mathbf{q}$ is the generalized coordinate vector which is augmented by the floating base pose coordinate. \mbox{\boldmath\lambda}_{c_{\bot}} and \mbox{\boldmath\lambda}_{c_{\parallel}} are the orthogonal and tangential contact forces. Their corresponding Jacobian are $\mathbf{J}_{c_{\bot}}$ and $\mathbf{J}_{c_{\parallel}}$ respectively and the equality and the inequality equations are the constraints that are introduced through contacts. The task space dynamics can be obtained by mapping the whole-body model in Equation (1) to the range space of the task Jacobian matrix, $\mathbf{J}_{x}$, [13]. Thus, we can write

[TABLE]

where we have

[TABLE]

$\mathbf{J}_{x}^{\dagger}$ is the dynamically consistent right pseudo-inverse of the full row rank Jacobian matrix, $\mathbf{J}_{x}$. In the next section, we use this equation to derive motion dynamics of three task spaces defined in our subsystem decomposition approach.

II-B Internal Model Control (IMC)

The IMC structure is an alternative parametrization over the stable classical feedback controllers for linear systems. However, in contrast to the classical approach which uses the system’s measured outputs for feedback control, IMC directly utilizes the system dynamics model for calculating a feedback signal based on the difference between the measured outputs and the predicted outputs of its internal model (Fig. 2). Moreover, it uses an inverse model approach in order to improve its tracking and disturbance rejection. Another major advantage of IMC is that it allows a systematic tradeoff between performance and robustness [14] since the closed loop sensitivity function is linear with respect to its controller. In the next section, we explain the design process of a robust IMC structure in more details.

To the best of our knowledge, IMC has not been applied for task space force control. However, a closely related method known as disturbance observer (DOB) has been recently implemented for force control in low-level control of actuators [15, 16]. Strictly speaking, IMC and DOB are not the same. However, they both rely on the internal model principle. The only notable exception for DOB application in rigid body level is [17] in which DOB is used to achieve the precise tracking on a humanoid arm while being compliant.

III Approach

The main objective of this paper is to increase the robustness of the motion controller for tracking given CoM trajectories. Based on the Newton-Euler equation of motion, the CoM dynamics are only affected by external forces on the robot. Thus, a robust implementation of a given CoM plan can be achieved by robustly controlling the contact forces and contact point locations at the moments of touchdown. The robust and compliant control of the free end-effectors (e.g. swing legs and arms) has been extensively studied in the literature [17]. Therefore, we mainly focus on the robust control of the contact forces. First, we introduce a new contact force controller which performs robustly in face of rigid body model mismatch, actuator dynamics, delays, contact surface stiffness, and unobserved ground profiles. Then, to reduce the coupling effects between controllers, we propose a generalized approach for controlling two task spaces with similar precedence. Finally, we explain our control approach for CoM and non-contact degrees of freedom (DoF).

In our proposed method, the legged robot model is broken down into three subsystems, namely: contact subsystem, CoM subsystem, and non-contact subsystem as shown in Fig. 1 (the blue block). The contact subsystem includes DoF in system which directly manipulate the contact forces. The CoM subsystem is attributed to the whole-body CoM dynamics. At last, the non-contact subsystem comprises of any remaining DoF such as the swing legs.

III-A Contact Force Control at End–Effectors

The contact subsystem is defined in the range space of the end-effectors’ Jacobians $\mathbf{J}_{c}$ that are in contact with the environment (e.g. stance legs). The equation of motion for this subsystem has the same form as (2), thus we have

[TABLE]

where the end-effector acceleration, $\ddot{\mathbf{x}}_{c}$, is set to zero due to the equality constraints in Equation (2) for non-slipping contact points. Notice that the projected contact forces Jacobian in this equation is an identity matrix since $\mathbf{J}_{c,c}=\mathbf{J}_{c}\mathbf{J}_{c}^{\dagger}=\mathbf{I}$. The assumption that $\mathbf{J}_{c}$ is a full row rank matrix holds as long as there exist at least three independent DoF for each contact point. Furthermore, conveniently all of the contact related equality and inequality constraints are assimilated in Equation (4). Assuming that all of the joints are actuated, $\mathbf{S}_{c}^{\top}$ has the same range space as $\mathbf{J}_{c}$ which renders this subsystem as fully actuated. Therefore, we get

[TABLE]

where \mbox{\boldmath\tau}_{c}=\mathbf{S}_{c}^{\top}\mbox{\boldmath\tau}. Notice that the mapping from \mbox{\boldmath\tau}_{c} to $\tau$ is not unique since for any given \mbox{\boldmath\tau}_{c}, there exists a subspace of dimension $Dim(\textit{Ker}(\mathbf{J}_{c}))$ in the generalized force space that produces the same value. Later, we will use this characteristic to decouple the contact and non-contact subsystems.

In order to control this subsystem, we first feedback-linearize its dynamics by setting \mbox{\boldmath\tau}_{c}=\mathbf{F}+\mathbf{C}_{c}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{c}(\mathbf{q}). Then, we get

[TABLE]

According to this equation, the relation between contact forces and $\mathbf{F}$ is algebraic. However, this equation is based on the rigid body modeling and hard contact assumption and does not consider the actuator dynamics and softness of the contact surfaces ($\ddot{\mathbf{x}}_{c}\neq 0$). In order to design a controller which can deal with these issues, we propose a feedback structure known as IMC. To guarantee the robustness of this structure, we need to make a few assumptions on the system dynamics and the external signal type (such as references and disturbances). First, we assume that all of the actuators have the same nominal dynamics. This assumption causes the projected force \mbox{\boldmath\tau}_{c} and consequently $\mathbf{F}$ to have the same nominal dynamics as actuators. We use here a first order system with delay (first order deadtime system). Thus, for each component $i$, in the contact force vector, we have the following dynamics

[TABLE]

where $\tilde{H}_{p}(s)$ is the nominal model between the $i$th contact force component and the $i$th element of the projected feedback-linearized actuator command $u_{i}$. $\tilde{\eta_{d}}$ and $\tilde{\eta}$ are the nominal delay and the time constant. In this nominal model, we assume that the system does not have a steady state error to a constant input (the gain is one) which is the case for ideal actuators. Finally, for modeling uncertainties of this model, we assume that the actual transfer function of system, $H_{p}(s)$, belong to the family $\Pi$ defined as

[TABLE]

which means that relative error in each frequency is bounded by $\overline{\l}(\omega)$. A typical shape of $\overline{\l}(\omega)$ can be found in Fig. 3. In practice, we can also assume that $\overline{\l}(0)<1$ which is equivalent to say that the constant input to the actuators produces a steady state force output with less than $100\%$ relative tracking error. This is a reasonable assumption for many force controlled robots. The external signals such as the reference forces and disturbances are assumed to be step inputs 111Note that any arbitrary signal can be approximated by a finite set of shifted step functions. This class of signals can capture the sudden changes in the contact force reference; such changes appear when establishing/breaking contacts as well as disturbances such as slippage of the legs and push forces on the robot’s body.

The proposed IMC structure is depicted in Fig. 2. Since there is no interconnection between non-pair inputs and outputs, we use a single-input single-output (SISO) control structure for each input-output pairs. Our SISO IMC consists of two controllers, namely the disturbance rejection controller, $q_{d}(s)$, and the reference tracking controller, $q_{r}(s)$. The disturbance rejection controller uses the estimated disturbance signal $\tilde{\mathbf{d}}$ to reject disturbances, $\mathbf{d}$ and the reference tracking controller modulates the input signals in order to track the desired output (refer to Fig. 2).

Both controllers use a notion of the system inverse model ($\tilde{H}^{-1}_{p}(s)$) for improving the closed loop force control performance. In general, the design process of IMC consists of two steps: (i) design an optimal controller for the nominal plant and (ii) detune the nominal control to obtain robust performance. The first step of the design uses an $H_{2}$-optimal control method which minimizes the Integral of Square Error (ISE) criterion for the step type input. The latter step often uses a lowpass filter in order to guarantee robust stability and robust performance. The $H_{2}$-optimal controller has no tracking error since for ISE being finite, the reference tracking error should asymptotically approach zero. The ISE criterion is defined as:

[TABLE]

Based on $H_{2}$-optimal theorem [14] the optimal controller is $q(s)=(\eta s+1)f(s)$ for a first order deadtime system with step input which is obtained based on the nominal model inverse (${H}^{-1}_{p}(s)$). Here, $f(s)$ is a lowpass filter. For optimal solution $f(s)$ should be unity. However, there are two major issues with this choice: first, since the degree of the nominator is higher than the degree of the denominator, this controller will not be a causal system. Second, this system has a high amplitude in the high frequencies which makes it susceptible to sensory noise and model uncertainty such as non-stiff ground (which produces high frequency $\ddot{\mathbf{x}}_{c}$). To overcome these issues we use a lowpass filter with a proper structure. The following theorem states that there exists such a filter which can guarantee stability and zero tracking error for all the system in the family $\Pi$.

Theorem 1

Assume $\overline{\l}(\omega)$ is continuous and $\overline{\l}(0)<1$. Then there exists a lowpass filter $f(s)$ such that the closed loop system is robustly stable for family $\Pi$ and it tracks asymptotically error-free constant inputs.

Proof:

The proof is based on the corollary (4.3-2) and corollary (4.4-2) in [14] and the assumption $\overline{\l}(0)<1$. ∎

In practice, this filer should be designed by using the minimum order lowpass filter which results in a satisfactory performance. On the system used in this paper, a first order lowpass filter with one free parameter $\eta_{f}$ was enough for robust stability and performance.

[TABLE]

This parameter can be tuned directly on the hardware, by gradually increasing $\eta_{f}$ until a satisfactory performance is obtained. Since $\eta_{f}$ determines the response time of the controller, increasing $\eta_{f}$ reduces the performance of the closed loop system in favour of robustness. In order to analytically guarantee the robust performance of the controller for all systems in the family $\Pi$, we need to show that the following condition holds for all the frequencies

[TABLE]

where $w$ is a weighting used for determining the desired closed-loop system’s bandwidth. A typical shape of $w^{-1}$ can be found in [14]. In Section IV, we explain a process of determining $\eta_{f}$ in order to fulfil the robust performance criterion in Equation (10).

The last but not least advantage of using the IMC structure is that we can optimally and robustly incorporate the input/output constraints [18]. In the contact force control case, we mostly have output constraints such as the unilateral constraint of orthogonal forces to the contact surface and stiction/friction in the actuators. To this end, we have used a hinge-function block at the output of the nominal model in order to account for unilateral constraint for orthogonal contact surfaces. Moreover, to model the stiction/friction of the actuators, we use a deadzone block on the estimated disturbance $\tilde{d}$ signal. The latter can be interpreted as the case where the small difference between the nominal model’s output force and the plant’s output force is considered as the actuators’ stiction/friction resisting force. In the next section, we study the coupling effect between the contact subsystem and other remaining DoF and introduce a generalized method to prevent this cross coupling.

III-B Task Space Control Decoupling

Special care should be taken in mapping \mbox{\boldmath\tau}_{c}=\mathbf{S}_{c}^{\top}\mbox{\boldmath\tau}. This is because the generalized forces $\tau$ are not only manipulated by the contact subsystem’s controller, but also by the other controllers acting on remaining DoF. This can produce undesirable couplings between controllers. In this section, we show how to systematically decouple the controllers for different DoF. To this end, we break down the legged robot model into three subsystems: contact subsystem, CoM subsystem, and non-contact subsystem as shown in Fig. 1. Since the contact subsystem and its controller structure have already been introduced in the previous section we now briefly introduce the two other subsystems.

CoM subsystem

The CoM subsystem describes the equation of motion governing the CoM of the robot. As the Newton-Euler equations state the acceleration (angular and linear) of the CoM is proportional to the net external generalized forces (net torques and net forces). Therefore, the CoM subsystem is only affected by the external forces (such as contacts and disturbances) and is independent of the internal contact forces. Hence, the equation of motion is:

[TABLE]

where $\mathbf{M}_{com}$ consists of two 3-by-3 blocks on the main diagonal, where one block corresponds to the CoM angular momentum inertia and the other block to the total mass of robot multiplied by an identity matrix. The CoM subsystem has two distinct mechanisms for control. The first one is the contact forces of the stance feet in each of the phases of motion and the second one is the switching time between two consecutive motion phases and the contact point locations. Therefore, the CoM controller determines the desired contact forces and the switching times in order to track given trajectories. These desired forces are used as reference inputs to the contact subsystem controller and the switching times are used in the non-contact subsystem controller in order to establish/break contacts at given times and given locations.

Non-contact subsystem

The non-contacting subsystem includes all the remaining DoF after excluding the contact subsystem and CoM subsystem. The task space Jacobian matrix for this subsystem can be defined in a way that either it picks the remaining generalized coordinates or it chooses any Cartesian task-dependent coordinates. In general, the equation of motion for this task does not have any special structure.

[TABLE]

where $\ddot{\mathbf{x}}_{nc}$ is the non-contact subsystem acceleration and \tau_{nc}=\mathbf{S}_{nc}^{\top}\mbox{\boldmath\tau} is the projected torque vector.

As Equations (4), (11), and (12) shows the contact and non-contact subsystems are the only ones that are directly affected by the generalized forces, $\tau$. Therefore, in order to avoid undesirable coupling between controllers of the contact and non-contact subsystems, the control input of subsystems, \mbox{\boldmath\tau}_{c} and $\tau_{nc}$, should be decoupled.

In general, there are two strategies to deal with this coupling effect: using hierarchical tasks control approach or using complete decoupling approach; the latter we call: “coequal tasks control”. The hierarchical tasks control sets low-priority task in the null space of the high priority task. Thus, the top level task can be designed independently whereas the lower level task needs to be informed of the higher task command. On the contrary, the coequal tasks control approach sets each of the two tasks in the null space of the other task. Hence, the tasks are decoupled into two completely independent tasks which allows for designing of each controller without the knowledge of the other task’s control command. Theorem 2 introduces a generalized formulation for representing these two approaches.

Theorem 2

Assume that the contact subsystem and non-contact subsystem have full column rank input selection matrices $\mathbf{S}_{c}$ and $\mathbf{S}_{nc}$ respectively. The projected forces of subsystems, namely \mbox{\boldmath\tau}_{c} and \mbox{\boldmath\tau}_{nc}, can be mapped by the following transformations to the generalized forces $\tau$.

[TABLE]

with $\mathbf{W}_{c}$ and $\mathbf{W}_{nc}$ are defined as

[TABLE]

where $\mathbf{W}$ is an arbitrary full rank weighting matrix and $\alpha_{c}$, $\alpha_{nc}$ $\in\{0,1\}$. If both $\alpha$’s are $1$ then the tasks are coequal which means that one task control input does not directly affect the other subsystem’s state evolution. If only one of them is $1$ then the tasks have a hierarchy (task with $\alpha_{i}=1$ has precedence). The case where both $\alpha$’s are [math] does not have any specific structure.

Proof:

A simple way to prove this is to check the effect of one of the subsystem’s generalized control input on the other subsystem’s equation of motion. As an example, for the contact subsystem we have

[TABLE]

The generalized forces affect contact subsystem through the term \mathbf{S}_{c}^{\top}\mbox{\boldmath\tau}. For this term we can write

[TABLE]

Using the definition of $\mathbf{W}_{nc}$ in Equation (15), we obtain

[TABLE]

For $\alpha_{c}=1$, contact subsystem will not be affected by \mbox{\boldmath\tau}_{nc} and if $\alpha_{c}=0$ contact subsystem will be affected by the non-contact subsystem’s projected force vector. Therefore, for $\alpha_{c}=\alpha_{nc}=1$ the two subsystems are independent (coequal), while if only one of the $\alpha$’s is $1$, the subsystem with $\alpha$ equal to $1$ will have precedence over the other subsystem. ∎

You may notice that Theorem 2 requires the selection matrices to be full rank which is the case for fully actuated robots. However, for underactuated robots, this may not be the case. Under this circumstance, if we partition the selection matrix by unactuated and actuated parts, we can still use Theorem 2. Fig. 1 shows relationships between subsystems using the coequal task space decomposition.

III-C CoM Subsystem Controller

In this work, we assume that the constraint satisfactory reference CoM motion is given by an external planner. In addition to reference pose and velocity of CoM 222Note that the CoM orientation is not a measurable physical entity. Therefore, using its value in a feedback controller structure requires an estimation scheme through integrating CoM velocity which is prone to drift in the absence of direct measurements. There are two solutions to this problem (i) approximating the CoM orientation with the base orientation which we use in the case where the planner provides desired CoM acceleration. (ii) using the modeling approach introduced in [19]. This approach normally designs the desired contact forces directly., the planner should either provide the desired acceleration of the CoM or the contact forces which realize the desired CoM motion. In order to deal with the discrepancies between the model and hardware, we use a PD controller on the CoM desired trajectories. This PD controller provides correction to the CoM acceleration which is later mapped to an equivalent correction of the desired contact forces. First, we assume the case where the planner provides the desired acceleration of the CoM. The corrected CoM acceleration can be calculated as: $\ddot{\mathbf{x}}_{corr}=\ddot{\mathbf{x}}_{com,ref}+K_{d}(\dot{\mathbf{x}}_{com}-\dot{\mathbf{x}}_{com,ref})\ +K_{p}(\mathbf{x}_{com}-\mathbf{x}_{com,ref})$, where $\ddot{\mathbf{x}}_{corr}$ is the corrected acceleration. Based on the CoM subsystem equation in (11), the reference contact force should satisfy the following equation

[TABLE]

Depending on the number of the contact points and their configuration, the above linear matrix equation may have one, none, or multiple solutions. Furthermore, in order to produce valid desired contact forces, we need to respect the unilateral orthogonal contact force constraints and Coulomb friction limits. To this end, we have defined the following Quadratic Programming (QP) problem

[TABLE]

where $\mathbf{F}_{r}=\mathbf{M}_{com}(\mathbf{q})\ddot{\mathbf{x}}_{corr}+\mathbf{C}_{com}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{G}_{com}(\mathbf{q})$. The first set of inequality constraints are the unilateral constraint of the orthogonal forces and the second set represents the convex polyhedral approximation of the friction cones, $\Sigma\left[\cdot\right]$. In our implementation, we have approximated the friction cone with an inscribed pyramid whose base is a twelve sided polygon. To solve this problem, we use the freely available QP solver QuadProg++ [20] which uses a dual method to solve the QP problem. In our C++ implementation, we are able to solve this QP problem in each step of the CoM control loop in less than $100\mu s$. Finally, in the case where the planner directly provides the desired contact forces, we first map the desired contact forces to the net forces (\mathbf{J}_{c,com}^{\top}\mbox{\boldmath\lambda}_{ref}). Then, we add the PD controller correction force. Finally, we map the corrected net forces back to the contact forces while satisfy the contact constraints in Problem (17).

III-D Non-contact Subsystem Controller

Except for possible user defined task, the main control objective for the non-contact subsystem is to establish contact in the next motion phase at a desired location. The controller on this subsystem should control the end-effectors (e.g. feet) to touch the contact surface at the given switching times. Here, we use inverse dynamics and a PD correction to track the reference inputs. We tune the controller to the lowest possible impedance that still achieves satisfactory tracking.

IV Results

In order to calculate the optimally robust $\eta_{f}$ which satisfies the robust performance inequality in Equation (10), we need to determine an upper bound of the multiplicative uncertainty, $\overline{l}(\omega)$. We assume that the family $\Pi$ in Equation (8) includes first order deadtime systems. Since the delay in our system is fixed and only stems from known communication delay, we set $\eta_{d}=\tilde{\eta}_{d}=0.003$. Then, we have approximated the uncertainties of DC gain, $k$, and the time constant, $\eta$, of the actuators based on the real data where we got the following bounds

[TABLE]

Based on this estimation for $\Delta k$ and $\Delta\eta$, we obtain the upper bound $\overline{\l}(\omega)$ in Equation (8) which is shown in Fig. 3 for different frequencies. Our weighting function, $w(s)$, has a unity weight up to 50 [$rad/s$] (which is the controller’s bandwidth) then it rolls off with -20 [$dB/dec$]. Thus, $w(s)$ is equivalent to a first order filter with time constant 0.02.

Since the reference tracking controller in IMC structure, $q_{r}(s)$, does not affect the closed-loop robustness, its lowpass filter is tuned to achieve smooth changes in the realized contact forces. The robust performance criterion (Equation (10)) for the disturbance rejection controller $q_{d}(s)$ is demonstrated in Fig. 3. As it can be seen, for $\eta_{f}$ around $0.3$ this criteria is satisfied while decreasing $\eta_{f}$ causes the loss of guarantee on robust performance. For example for $\eta_{f}=0.01$, the red curve goes over $1$ in certain frequencies. Thus, we set $\eta_{f}=0.03$, in order to guarantee robust performance in disturbance rejection controller. In practice, it is better to add an extra fast pole to this filter (with around 10 times smaller response time) in order to filter out sensor noise.

We design a number of experiments to compare our approach of full body control against approaches that rely on inverse dynamics and PD control in the robot’s joints (e.g. [2]) for tracking the reference trajectory. We start by showing the results obtained in simulation using SL simulation package [21]. In Fig. 4, our approach is compared with the inverse dynamics based approach by showing the CoM tracking performance while executing the walking gait.

The plots shown in Fig. 4 are obtained by simulating relatively low ground stiffness (spring constant of $1.0\times 10^{4}N/m$). While both approaches produce comparable tracking performance on a stiff ground ( $6.0\times 10^{5}N/m$), our approach brings significant improvement for walking on soft ground. The walking experiment was also conducted on a real robot. To simulate soft ground conditions, we wrapped HyQ’s feet in foam and added soft rubbery ground for HyQ to walk on (see video 333The reader can find the videos of the following simulation results online: https://youtu.be/bE2_-lpZU7o ). The comparison of tracking performances is shown in Fig. 5. While both controllers achieve similar tracking performance in $y$ and $z$ directions, in $x$ direction the inverse dynamics based controller accumulates about 10 [$cm$] error in about 1 [$m$] of walking. On the other hand, our controller has less than 1 [$cm$] error in tracking of the $x$ direction.

To show that our whole-body control approach indeed actively controls end-effector contact forces without any auxiliary PD controllers in the joints, we conduct the seesaw and the plank experiments. In both experiments, the contact subsystem has no knowledge about changes in the robot’s foothold positions. For the first experiment, we put a plank under the robot’s front feet and lift it up while the robot’s hind legs are still standing on the floor. The plank is displaced along the vertical axis; however, we also introduce lateral and longitudinal displacement in order to demonstrate the robustness of our approach (see video). For the second experiment, HyQ is placed on a seesaw which we swing while the robot is maintaining the upright position of its base by actively controlling the feet’s contact forces. Snapshots of the plank and seesaw experiments are shown in Fig. 6 and 7.

Disturbance rejection performance for the plank experiment is shown in Fig. 8 and the contact force tracking performance is shown in Fig. 9. In both figures, the light grey rectangles denote the time intervals during which the longitudinal and the lateral disturbances have been applied to the plank (see video). We show the pitching angle $\beta$, the $z$ coordinate of the robot’s base, the $z$ coordinate of the left front (LF) and the left hind (LH) feet as well as the contact forces for LF and LH feet. Comparing the z coordinate of the feet with the base pitching angle and z coordinate, we see that the effect of disturbance on the base is attenuated. This shows the performance of the contact force controller which robustly tracks the the desired forces under disturbances such as moving contact points. A similar results can be seen for the seesaw experiment in Fig. 10 and 11. Like the plank experiment, the disturbance is considerably reduced.

V Conclusion and Outlook

In this paper, we presented a method for whole-body control of legged robots with the goal of tracking a planned trajectory for the CoM. The resulting full body controller has a feedback structure which increases the robustness in the presence of unmodeled actuator dynamics, changes in the ground stiffness and unobserved ground profile. The robust performance of our full body controller stems from the IMC control structure which we use to control the end-effector contact forces. The IMC structure allows us to incorporate model constraints specific for the contact subsystem, admits an easy and intuitive tuning process and provides theoretical guarantees on the robust performance of the resulting contact subsystem controller.

Our full body controller relies on decomposing system dynamics into three subsystems. These three subsystems are the contact subsystem, the CoM subsystem and the non-contact subsystem. This decomposition allows us to design controllers separately for each system in a way such that there is no direct interference between different subsystems, which results in the better trajectory tracking for the CoM. We have demonstrated the effectiveness of our approach in simulation and on the real robot. Moreover, we have verified robustness of our approach by comparing its performance to the existing approach for legged robot motion control that relies mainly on inverse dynamics based controller.

In future, we would like to have this motion controller working together with our dynamic programming motion planner, OCS2, [23] which designs contact forces references and switching time based on the first principle of optimality and test our controller for high dynamic movements such as trotting. Moreover, we like to improve our contact force controller by incorporating contact force sensors in the control loop where we currently use state estimation algorithm.

Acknowledgement

This research has been supported in part by a Max-Planck ETH Center for Learning Systems Ph.D. fellowship to Farbod Farshidian and a Swiss National Science Foundation Professorship Award to Jonas Buchli and the NCCR Robotics.