Online Gain Adaptation of Whole-Body Control for Legged Robots with Unknown Disturbances

1 Introduction

Stability analysis for efficient whole-body control (WBC) of humanoid robots is important to execute robustly multiple tasks in bipedal and humanoid robots. Most WBC approaches face difficulty ensuring the stability at the closed loop systems due to intricate control structures. For this reason, bipedal and humanoid robot stability is frequently studies in task-space, based on constant feedback gains. However, these feedback gains defined a priori might be inappropriate to track the planned motions robustly and stabilize the robot’s behaviors under unknown external disturbances. This paper proposes an online gain adaptation method based on stability analysis of the closed-loop robotic system via WBC. A basic assumption of our problem is that there are no force/torque sensors to measure both contact forces and external force disturbances. Our online gain adaptation approach utilizes three methods: 1) a WBC controller, dubbed Whole-Body Locomotion Controller (WBLC), 2) a Centroidal dynamic model, and 3) various approximation techniques.

1.1 Whole-Body Control

Legged humanoid robots need to deliver stable and robust legged manipulation (also referred to as loco-manipulation) behaviors while operating in unstructured environments. For this purpose, Whole-Body Control (WBC) enables the generation of control commands for tracking multiple task trajectories effectively Khatib et al. (2004). Most WBC frameworks rely on dynamic models of the robot and classical control laws such as proportional-derivative (PD) control Feng et al. (2015) or impedance control Albu-Schäffer et al. (2007). Based on these requirements, projection-based WBC approaches have been widely used due to their intuitive, concise, and computationally efficient form. However, more versatile algorithms are frequently needed to consider inequality and unilateral constraints Lee et al. (2012) such as Contact Wrench Cone (CWC) constraints Caron et al. (2015). Optimization-based approaches have been widely used to incorporate inequality constraints explicitly Escande et al. (2014); Hong et al. (2020); Feng et al. (2014); Wiedebach et al. (2016); Kim et al. (2020). However, there is a major challenge for these optimization-based WBC approaches: how can we ensure the desired task hierarchy and how can we guarantee system stability when there exist uncertain external forces?

A well-known method to enforce task priorities employs hierarchical quadratic programming (HQP) Escande et al. (2014); Hong et al. (2020). In Feng et al. (2014); Wiedebach et al. (2016), the authors employ a single quadratic program (QP) with different weighted cost terms to impose a task hierarchy. These types of controllers that employ a single QP process reduce the number of optimizations compared with HQP and enforce the task hierarchy in an implicit manner; however, the weights are heuristic, resulting in occasional violations of the priorities. Often, there is a discrepancy between the desired configuration and the result from the WBC optimization. The joint acceleration, which is one of the results from the WBC optimization process, can be numerically integrated to resolve this problem Kuindersma et al. (2016); Koolen et al. (2016); Ahn et al. (2021). Alternatively, this problem can be solved by directly re-optimizing the trajectories considering dynamic reachability Lee et al. (2020). Another approach is to have WBC incorporate both internal and external force/torque measurements via embedded sensors within the mechanical actuation/transmission system Nori et al. (2015). These sensors can be used to improve the stability of the robots via their WBC controllers.

In general, it is complex to analyze the stability of robots controlled by the WBC approaches. For instance, stability of a priority-based kinematic control approach is verified based on Lyapunov stability in Antonelli (2009). Also, it is shown that the robot controlled by the operational space control framework is asymptotically stable Dietrich et al. (2018). However, the above stability analysis studies do not consider floating-base dynamics of the legged robot and the presence of unknown external disturbances. Compared with the above stability analysis which are applied to projection-based approaches, the stability analysis of optimization-based WBC approaches is more complicated to perform when considering external perturbations. We previously proposed an optimization-based WBC method dubbed WBLC, that combines both a projection-based controller and an optimization technique to enforce task priorities while satisfying inequality constraints Kim et al. (2020). The WBLC framework combines a whole-body kinematic controller and a whole-body dynamic controller to compute joint position, velocity, acceleration, and torque references in real-time as shown in Figure 1. WBLC has successfully demonstrated dynamic locomotion of passive ankled robots. However, the robustness of WBLC regarding the external disturbances mostly relies on the experts’ parameter tuning in experiments. Also, we did not address the theoretic stability analysis in our previous work Kim et al. (2020). In this paper, we will analyze here our WBLC framework for ensuring stability and tracking performance under external forces. This is key since if WBLC is affected by unknown external forces, the robot could easily lose balance.

FIGURE 1

FIGURE 1. Block diagram of the proposed approach: The proposed approach includes various feedback loops, inside and outside the whole-body locomotion controller (WBLC) Kim et al. (2020). Our adaptive approach is tasked with modifying feedback gains inside WBLC.

1.2 Robots With External Disturbances

Compared to other methods which focus on push recovery, here we focus on adaptive control as a means to deal with external forces. Some push recovery methods employ linear MPC to obtain CoM and foot trajectories based on the Zero Moment Point (ZMP) dynamics to re-plan walking behaviors for disturbance recovery Mason et al. (2018). The desired recovery force associated with the ZMP condition is converted to CoM positions Wang et al. (2014). It has been widely investigated how to determine stepping motions for balance recovery such as Push Recovery Model Predictive Control (PR-MPC) Stephens and Atkeson (2010b) and Capture Point control Pratt et al. (2006). Although these strategies have accomplished successful results, they are based on motion generation instead of modifying feedback gains or impedances. However, the latter strategy considered in this paper can provide a layer of additional robustness. Indeed, it is not always needed to perform an additional stepping motion to recover from external forces. If the external or contact forces are not adequately compensated for or there exist unknown external forces, the controllers may be unstable regardless of the recovery strategy Nakanishi et al. (2008).

One solution to this problem is to compensate for external forces. This has been often studied as a compliant control problem for balancing despite the external forces Hyon et al. (2007); Stephens and Atkeson (2010a); Ott et al. (2011); Herzog et al. (2016). A key idea of the above researches is to obtain proper ground reaction forces keeping the balance against the external force then the forces are utilized to compute the control command torque Hyon et al. (2007); Ott et al. (2011). LQR is employed for better momentum control of a torque-controlled humanoid and they show robust performance on balancing in the face of unknown disturbances Herzog et al. (2016). A common point of the above researches is that they rely on constant gains or impedances. Although the above researches have accomplished practical balancing ability, the stability of the controlled robots are not proven at the actuation level yet when the external forces are applied to the robots. Furthermore, they also need to modify or replan the task specifications such CoM and angular momentum for keeping the robot’s balance. By contrast, our work does not require this replanning step.

Another solution to the above problem is to directly measure external forces/moments using an F/T sensor Käslin et al. (2018) or internal, tactile, and F/T sensors, simultaneously Ivaldi et al. (2011). However, it is normally challenging to attach F/T sensors everywhere in the robot’s body. An alternative method is to accurately estimate external forces and the corresponding ground reaction forces relying on the dynamic model of the robot Xin et al. (2018). For humanoid robotic applications, an external force/moment can be estimated based on an extended Kalman filter using whole-body dynamics, force sensor on the feet, and an IMU Benallegue et al. (2018). In some cases, external forces can be measured using force-sensing resistors Hawley et al. (2019) on the feet. We can also deploy adaptive control approaches for stabilizing the robots without direct estimation of the external forces. A robust and adaptive control approach was proposed without using the robot’s dynamic model to guarantee stable control performance under uncertain disturbances Lee et al. (2016). This approach shows the robustness of WBC with respect to impact disturbance at the task level.

1.3 Contributions

Overall, this paper proposes a unique gain adaptation approach for WBLC applied to legged robots under both contact and external forces. More specifically, we aim to enhance the robot’s balance and robust performance by guaranteeing feedback system stability under unknown external forces. To do that, we leverage the WBLC as proposed in Kim et al. (2020). Then, we approximately decouple the joint-space dynamics and analyze their closed-loop stability. Compared to other approaches such as Lee et al. (2016), we adapt the feedback gains to make the closed-loop system critically damped at the joint level. This results in four contributions.

• Firstly, the proposed adaptation approach is very intuitive and straightforward to be implemented. Our method does not require any modification of the task specifications defined a priori. Without any significant modifications to the controller and planners, it is also possible to employ classical WBC methods Kim et al. (2020) for controlling the robot in the presence of unknown external forces.

• Second, the proposed WBLC approach also improves task tracking performance with respect to external disturbances. This improvement results in the robot’s ability to improve its balance and provide more robust bipedal walking.

• Third, we provide a joint-level control stability study of robots perturbed by external forces. There exist a number of studies considering the stability of legged robots performed at the force or task/operational space level. These types of stability analysis cannot ensure whole-body stability involving actuation effects since those take place at the joint level.

• Lastly, the proposed online gain adaptation method is designed to be easy-to-use. We do not need to rely on F/T sensors and a complicated estimation process to stabilize the robot system due to our use of centroidal robot dynamics.

In this paper, our primary task is to control the position of the CoM, keeping balance in the double support phase and to stably walk against the external disturbances. Many studies on legged robots such as push recovery and reactive planning have addressed keeping balance while standing and walking. Regarding reactive planning processes, they do not change low-level controller feedback gains as we do and instead modify the desired task specifications or trajectories in the presence of the perturbations. Therefore, in such methods, it is hard to guarantee the closed-loop stability of the robot while tracking the desired task trajectories. By contrast, our method stabilizes the robot against the external disturbance by adapting the feedback gains without further modification of task trajectories. For this reason, we can ensure the closed-loop stability of the robot while the external disturbance is applied to the robot. Our proposed approach is therefore simpler and more intuitive than reactive planning approaches and it can also be used in combination with them to enhance their performance.

The remainder of this paper is organized as follows. Section 2 explains the preliminaries related to a QP-based WBC approach, e.g., WBLC and the Centroidal Dynamic model. The proposed approach is explained in Section 3. In Section 4, we apply the proposed adaptive approach to a legged robot Draco2 to show the effectiveness of the proposed method in numerical simulations and hardware experiments.

2 Preliminaries

In section 2.1, we first review our previous WBLC. Then, in section 2.2, we thoroughly analyze the closed-loop behavior of robots with no external perturbations aside from the ground forces needed to balance.

2.1 QP-Based Whole-Body Locomotion Control

We review here the structure of our previous WBC controller. Given a configuration space $\mathcal{Q}\subseteq {\mathbb{R}}^n$ and an input space $\mathcal{U}\subseteq {\mathbb{R}}^{n_u}$, the rigid-body dynamics of a robot is described as follows:

$\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{B}\left(\mathbf{q},\dot{\mathbf{q}}\right)={\mathbf{S}}^{\top }\mathbf{\Gamma }+{\mathbf{J}}_c^{\top }\left(\mathbf{q}\right){\mathbf{F}}_c(1)$

Where $\mathbf{q}\in \mathcal{Q}$, $\mathbf{M}(\mathbf{q})\in {\mathbb{S}}_{++}^n$, $\mathbf{B}(\mathbf{q},\dot{\mathbf{q}})\in {\mathbb{R}}^n$, and $\mathbf{\Gamma }\in \mathcal{U}$ denote the joint position vector, mass/inertia matrix, sum of Coriolis/Centrifugal and gravity forces, and torque command, respectively. Considering six DOFs virtual joints for the floating base, n_u = n − 6, we can define the selection matrix $\mathbf{S}=\left[\begin{array}{cc}\hfill {\mathbf{0}}_{n_u\times 6}\hfill & \hfill {\mathbf{I}}_{n_u\times n_u}\hfill \end{array}\right]\in {\mathbb{R}}^{n_u\times n}$. ${\mathbf{F}}_c\in {\mathbb{R}}^{6m}$ is a vertically concatenated contact force vector and the corresponding contact Jacobian is represented as ${\mathbf{J}}_c(\mathbf{q})\in {\mathbb{R}}^{6m\times n}$ where m is the number of contacts. A standard feedback control scheme can be designed in the joint space using a PD control law:

${\mathbf{\Gamma }}_{\text{cmd}}=\mathbf{S}\mathbf{M}\left(\mathbf{q}\right)\left({\mathbf{K}}_p{\mathbf{e}}_{\mathbf{q}}+{\mathbf{K}}_d{\dot{\mathbf{e}}}_{\mathbf{q}}\right)+{\widehat{\mathbf{\Gamma }}}^d(2)$

Where e_q = q^d − q, and ${\dot{\mathbf{e}}}_{\mathbf{q}}={\dot{\mathbf{q}}}^d-\dot{\mathbf{q}}$. Also, ${\mathbf{q}}^d\in \mathcal{Q}$, ${\dot{\mathbf{q}}}^d\in {\mathbb{R}}^n$, ${\mathbf{K}}_p=\mathrm{diag}(k_{p_1},\dots ,k_{p_n})$ and ${\mathbf{K}}_d=\mathrm{diag}(k_{d_1},\dots ,k_{d_n})$ denote desired joint position, desired joint velocity, proportional gain matrix and derivative gain matrix, respectively. ${\widehat{\mathbf{\Gamma }}}^d$ represents a feedforward torque command generated by the Dynamic-level WBC control block shown in Figure 1. Using a hierarchical inverse kinematic control scheme, we can compute q^d and ${\dot{\mathbf{q}}}^d$ given task specifications, x^d, ${\dot{\mathbf{x}}}^d$, and ${\ddot{\mathbf{x}}}^d$. Let us consider N hierarchical tasks in a lexicographic order. The desired joint position is obtained by q^d = q + Δq where $\mathrm{\Delta }\mathbf{q}={\sum }_{k=1}^N\mathrm{\Delta }{\mathbf{q}}_k$ and

$\begin{array}{cc}\hfill \mathrm{\Delta }{\mathbf{q}}_k& ={\mathbf{J}}_{k|k-1}{\left(\mathbf{q}\right)}^{+}\left({\mathbf{e}}_{{\mathbf{x}}_k}-{\mathbf{J}}_k\left(\mathbf{q}\right)\mathrm{\Delta }{\mathbf{q}}_{k-1}\right),\hfill \\ \hfill {\mathbf{N}}_k\left(\mathbf{q}\right)& ={\mathbf{N}}_{k-1}\left(\mathbf{q}\right)-{\mathbf{J}}_{k|k-1}{\left(\mathbf{q}\right)}^{+}{\mathbf{J}}_{k|k-1}\left(\mathbf{q}\right)\hfill \end{array}(3)$

Where J_k|k−1(q) = J_k(q)N_k−1(q), ${\mathbf{e}}_{{\mathbf{x}}_k}={\mathbf{x}}_k^d-{\mathbf{x}}_k$ and N₀(q) = I. In addition, ${\dot{\mathbf{q}}}^d$ and ${\ddot{\mathbf{q}}}^d$ can be simply computed as ${\dot{\mathbf{q}}}^d={\sum }_{k=1}^N{\dot{\mathbf{q}}}_k^d$ and ${\ddot{\mathbf{q}}}^d={\sum }_{k=1}^N{\ddot{\mathbf{q}}}_k^d$ where

$\begin{array}{cc}\hfill {\dot{\mathbf{q}}}_k^d& ={\mathbf{J}}_{k|k-1}{\left(\mathbf{q}\right)}^{+}\left({\dot{\mathbf{x}}}_k^d-{\mathbf{J}}_k\left(\mathbf{q}\right){\dot{\mathbf{q}}}_{k-1}^d\right),\hfill \\ \hfill {\ddot{\mathbf{q}}}_k^d& ={\mathbf{J}}_{k|k-1}{\left(\mathbf{q}\right)}^{+}\left({\ddot{\mathbf{x}}}_k^d-{\dot{\mathbf{J}}}_k\left(\mathbf{q},\dot{\mathbf{q}}\right)\dot{\mathbf{q}}-{\mathbf{J}}_k\left(\mathbf{q}\right){\ddot{\mathbf{q}}}_{k-1}^d\right),\hfill \end{array}(4)$

${\dot{\mathbf{q}}}_0^d=\mathbf{0}$, and ${\ddot{\mathbf{q}}}_0=\mathbf{0}$. Based on derivations of q^d, ${\dot{\mathbf{q}}}^d$, and ${\ddot{\mathbf{q}}}^d$, an additional optimization problem is formulated to handle full-body dynamics of the robot, contact wrench cone constraints, and torque limits as described in Kim et al. (2020), i.e.:

$\begin{array}{cc}\hfill \underset{{\mathbf{F}}_c,{\ddot{\mathbf{x}}}_c,\delta \ddot{\mathbf{q}}}{\min }& {\mathbf{F}}_c^{\top }{\mathbf{W}}_F{\mathbf{F}}_c+{\ddot{\mathbf{x}}}_c^{\top }{\mathbf{W}}_c{\ddot{\mathbf{x}}}_c+\delta {\ddot{\mathbf{q}}}^{\top }{\mathbf{W}}_{\ddot{q}}\delta \ddot{\mathbf{q}}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{B}\left(\mathbf{q},\dot{\mathbf{q}}\right)={\mathbf{S}}^{\top }\mathbf{\Gamma }+{\mathbf{J}}_c^{\top }\left(\mathbf{q}\right){\mathbf{F}}_c,\hfill \\ \hfill & {\ddot{\mathbf{x}}}_c={\mathbf{J}}_c\left(\mathbf{q}\right)\ddot{\mathbf{q}}+{\dot{\mathbf{J}}}_c\left(\mathbf{q},\dot{\mathbf{q}}\right)\dot{\mathbf{q}},\hfill \\ \hfill & \ddot{\mathbf{q}}={\ddot{\mathbf{q}}}^d+{\mathbf{K}}_p^h{\mathbf{e}}_{\mathbf{q}}+{\mathbf{K}}_d^h{\dot{\mathbf{e}}}_{\mathbf{q}}+\delta \ddot{\mathbf{q}},\hfill \\ \hfill & \mathbf{U}\left(\mathbf{q}\right){\mathbf{F}}_c\ge \mathbf{0},\hfill \\ \hfill & {\mathbf{\Gamma }}_{\min }\le \mathbf{\Gamma }\le {\mathbf{\Gamma }}_{\max }\hfill \end{array}(5)$

Where ${\mathbf{W}}_F\in {\mathbb{S}}_{+}^{6m}$, ${\mathbf{W}}_c\in {\mathbb{S}}_{+}^{6m}$, and ${\mathbf{W}}_{\ddot{q}}\in {\mathbb{S}}_{+}^n$ are weighting matrices for the objective function. ${\mathbf{K}}_p^h=\mathrm{diag}(k_{p_1}^h,\dots ,k_{p_n}^h)$ and ${\mathbf{K}}_d^h=\mathrm{diag}(k_{d_1}^h,\dots ,k_{d_n}^h)$ are feedback gains in the optimization problem. $\mathbf{U}(\mathbf{q})\in {\mathbb{R}}^{17}$ is a matrix for expressing the contact wrench cone constraints and upper bounds of the reaction forces for smooth contact changes. Γ_min and Γ_max are the minimum and maximum torques of the actuators. Based on the optimal decision variables ${\mathbf{F}}_c^{*}$ and $\delta {\ddot{\mathbf{q}}}^{\star }$, we can provide the feedforward torque command as follows:

${\widehat{\mathbf{\Gamma }}}^d=\mathbf{S}\left(\mathbf{M}\left(\mathbf{q}\right){\ddot{\mathbf{q}}}^{\star }+\mathbf{B}\left(\mathbf{q},\dot{\mathbf{q}}\right)-{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{\top }{\mathbf{F}}_c^{\star }\right)(6)$

Where ${\ddot{\mathbf{q}}}^{\star }={\ddot{\mathbf{q}}}^d+{\mathbf{K}}_p^h{\mathbf{e}}_{\mathbf{q}}+{\mathbf{K}}_d^h{\dot{\mathbf{e}}}_{\mathbf{q}}+\delta {\ddot{\mathbf{q}}}^{\star }$. Using the above desired torque command ${\widehat{\mathbf{\Gamma }}}^d$, we can compute the low-level joint torque command in (2).

2.2 Closed-Loop Analysis Without External Perturbations

We analyze the closed-loop behavior of the robotic system. Substituting (6) into (2) we get:

${\mathbf{\Gamma }}_{\text{cmd}}=\mathbf{S}\left(\mathbf{M}\left(\mathbf{q}\right)\widehat{\ddot{\mathbf{q}}}+\mathbf{B}\left(\mathbf{q},\dot{\mathbf{q}}\right)-{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{\top }{\mathbf{F}}_c^{\star }\right)(7)$

Where $\widehat{\ddot{\mathbf{q}}}={\ddot{\mathbf{q}}}^d+{\widehat{\mathbf{K}}}_p{\mathbf{e}}_{\mathbf{q}}+{\widehat{\mathbf{K}}}_d{\dot{\mathbf{e}}}_{\mathbf{q}}+\delta {\ddot{\mathbf{q}}}^{\star }$, ${\widehat{\mathbf{K}}}_p=({\mathbf{K}}_p^h+{\mathbf{K}}_p)$, and ${\widehat{\mathbf{K}}}_d=({\mathbf{K}}_d^h+{\mathbf{K}}_d)$. By substituting the command in (7) into the full-body dynamics in (1), the closed-loop dynamics become:

$\left[\begin{array}{cc}\hfill {\mathbf{M}}_{vv}\hfill & \hfill {\mathbf{M}}_{va}\hfill \\ \hfill {\mathbf{M}}_{va}^{\top }\hfill & \hfill {\mathbf{M}}_{aa}\hfill \end{array}\right]\left[\begin{array}{c}\hfill {\ddot{\mathbf{q}}}_v\hfill \\ \hfill {\ddot{\mathbf{q}}}_a\hfill \end{array}\right]=\left[\begin{array}{c}\hfill {\mathbf{d}}_v\hfill \\ \hfill {\mathbf{d}}_a\hfill \end{array}\right],(8)$

With

$\begin{array}{cc}\hfill {\mathbf{d}}_v=& {\mathbf{S}}_v{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{\top }{\mathbf{F}}_c-{\mathbf{B}}_v,\hfill \\ \hfill {\mathbf{d}}_a=& {\mathbf{M}}_{va}^{\top }{\widehat{\ddot{\mathbf{q}}}}_v+{\mathbf{M}}_{aa}{\widehat{\ddot{\mathbf{q}}}}_a+\mathbf{S}{\mathbf{J}}_c{\left(\mathbf{q}\right)}^{\top }\left({\mathbf{F}}_c-{\mathbf{F}}_c^{\star }\right)\hfill \end{array}(9)$

Where subscriptions ${(.)}_v\in {\mathbb{R}}^6$ and ${(.)}_a\in {\mathbb{R}}^{n_u}$ denote the properties for virtual and actuated joints, respectively. Virtual joints are those corresponding to degrees of freedom for the floating base whereas actuated joints correspond to motorized robot articulations. As such, we define a selection matrix for the virtual joints, ${\mathbf{S}}_v=\left[\begin{array}{cc}\hfill {\mathbf{I}}_{6\times 6}\hfill & \hfill {\mathbf{0}}_{6\times n_u}\hfill \end{array}\right]\in {\mathbb{R}}^{6\times n}$. We also introduce a matrix called the Schur Complement of M_aa in M(q) as ${\mathbf{M}}_{aa,s}={\mathbf{M}}_{vv}-{\mathbf{M}}_{va}{\mathbf{M}}_{aa}^{-1}{\mathbf{M}}_{va}^{\top }$. Since M(q) is positive definite and M_aa is invertible, the matrix M_aa,s is also invertible. The actuated joint acceleration can be expressed as follows:

$\begin{array}{cc}\hfill {\ddot{\mathbf{q}}}_a=& {\mathbf{M}}_{aa}^{-1}{\mathbf{d}}_a+{\mathbf{M}}_{aa}^{-1}{\mathbf{M}}_{va}^{\top }{\mathbf{M}}_{aa,s}^{-1}\left({\mathbf{M}}_{va}{\mathbf{M}}_{aa}^{-1}{\mathbf{d}}_a-{\mathbf{d}}_v\right)\hfill \\ \hfill =& {\widehat{\ddot{\mathbf{q}}}}_a+\mathrm{\Delta }\mathbf{c}\hfill \end{array}(10)$

Where $\mathrm{\Delta }\mathbf{c}={\mathbf{M}}_{aa}^{-1}\left({\mathbf{M}}_{va}^{\top }({\widehat{\ddot{\mathbf{q}}}}_v-{\ddot{\mathbf{q}}}_v)+\mathbf{S}{\mathbf{J}}_c{(\mathbf{q})}^{\top }({\mathbf{F}}_c-{\mathbf{F}}_c^{\star })\right)$. Assuming that the optimal decision variable satisfies ${\mathbf{F}}_c={\mathbf{F}}_c^{\star }$ and ${\widehat{\ddot{\mathbf{q}}}}_v={\ddot{\mathbf{q}}}_v$ with perfect compensation for the Coriolis/centrifugal and gravitational forces, we can decouple the joint space dynamics: Δc = 0. However, the optimal decision variable ${\mathbf{F}}_c^{\star }$ relies on the robot model and there is no direct feedback control-loop for regulating the contact force error ${\mathbf{F}}_c-{\mathbf{F}}_c^{\star }$. Also, Δc becomes significant when Centroidal dynamics are not precisely controlled due to external disturbances, since both ${\ddot{\mathbf{q}}}_v$ and F_c significantly affect the Centroidal dynamics of the robot. So the above assumptions are frequently invalid in the real world. This is the reason to introduce the error Δc in (10). Then, we can approximately decompose the closed-loop dynamics in the joint space as follows:

${\ddot{q}}_i={\ddot{q}}_i^f+{\widehat{k}}_{p_i}\left(q_i^d-q_i\right)+{\widehat{k}}_{d_i}\left({\dot{q}}_i^d-{\dot{q}}_i\right)+\mathrm{\Delta }{c}_i(11)$

Where ${\ddot{q}}_i^f={\ddot{q}}_i^d+\delta {\ddot{q}}_i^{\star }$, ${\widehat{k}}_{p_i}=k_{p_i}+k_{p_i}^h$, and ${\widehat{k}}_{d_i}=k_{d_i}+k_{d_i}^h$ for all i ∈ {7, …, n}. q_i, ${\dot{q}}_i$, and ${\ddot{q}}_i$ represent the joint position, velocity, and acceleration values for the ith joint, respectively. Also, Δc_i is the ith component of Δc. The relationships between the natural frequency, W_n, and the PD gains are defined as $W_n^2={\widehat{k}}_{p_i}$ and $2\zeta W_n={\widehat{k}}_d$, respectively, where ζ denotes the damping ratio. To achieve critical damping, the derivative gain must be, ${\widehat{k}}_{d_i}=2\sqrt{{\widehat{k}}_{p_i}}$. Based on this analysis, it is possible to obtain the gain matrices of the whole-body control to achieve stable closed-loop behavior.

3 Proposed Gain Adaptation Method

Our online gain adaption approach considers robots with unknown ground reaction forces on their feet as well as unknown external forces applied to their bodies. We use four steps to tune the feedback gains in WBC as shown in Figure 2. First, we obtain a solution for the ground reaction forces using the optimization problem (5) as shown in Figure 2A. Second, given current joint position and velocities, the centroidal dynamics are computed, discretized and approximated as shown in Figure 2B. In Figure 2C, we show the estimation of unknown external forces applied to the robot’s body using the previously computed ground contact forces and centroidal robot dynamics. Lastly, we test the stability of the robot considering the estimated forces and contacts. In turn the feedback gains are adapted to stabilize the robot under the external disturbance, as shown in Figure 2D. This section describes the detailed process of our approach.

FIGURE 2

FIGURE 2. Our approach consists of: (A) obtaining contact forces via WBLC. (B) Computing Centroidal dynamics using the current joint configuration and velocity. (C) Estimating unknown external forces with respect to the robot’s body frame. (D) Online gain adaptation with stability analysis.

3.1 Closed-Loop Behavior With External Forces

Next, we consider an additional unknown external force applied to the robot’s body, such as a force acting on the torso or pelvis. The optimization problem (5) cannot directly incorporate the unknown perturbation. Given such perturbations, the rigid-body dynamics of the robot changes as follows:

$\mathbf{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathbf{B}\left(\mathbf{q},\dot{\mathbf{q}}\right)={\mathbf{S}}^{\top }\mathbf{\Gamma }+{\mathbf{J}}_c^{\top }\left(\mathbf{q}\right){\mathbf{F}}_c+{\mathbf{J}}_t^{\top }\left(\mathbf{q}\right){\mathbf{F}}_t(12)$

Where ${\mathbf{F}}_t\in {\mathbb{R}}^6$ denotes the unknown external force acting on the robot’s body and ${\mathbf{J}}_t(\mathbf{q})\in {\mathbb{R}}^{6\times n}$ is the corresponding Jacobian. Based on Proposition 1, the closed-loop behavior of the system with the control torque input becomes:

$\begin{array}{cc}\hfill {\ddot{\mathbf{q}}}_a=& {\widehat{\ddot{\mathbf{q}}}}_a+\mathrm{\Delta }\mathbf{c}+{\mathbf{M}}_{aa}^{-1}\mathbf{S}{\mathbf{J}}_t{\left(\mathbf{q}\right)}^{\top }{\mathbf{F}}_t\hfill \\ \hfill =& {\widehat{\ddot{\mathbf{q}}}}_a+\underset{\mathbf{S}{\mathit{\psi }}_t}{\underbrace{\mathrm{\Delta }\mathbf{c}+\left(\mathbf{S}{+\mathcal{E}}_m\right)\mathbf{M}{\left(\mathbf{q}\right)}^{-1}{\mathbf{S}}^{\top }\mathbf{S}{\mathbf{J}}_t{\left(\mathbf{q}\right)}^{\top }{\mathbf{F}}_t}}\hfill \end{array}(13)$

Where ${\mathcal{E}}_m=\left[\begin{array}{cc}\hfill -{\mathbf{M}}_{aa,s}^{-1}{\mathbf{M}}_{va}^{\top }{\mathbf{M}}_{vv}^{-1}{\mathbf{M}}_{vv,s}\hfill & \hfill \mathbf{0}\hfill \end{array}\right]$. When the external disturbance is unknown, it is impossible to compute Δc and F_t, separately. Therefore, we consider the effect of the external disturbance using Sψ_t.

Proposition 1. Given a matrix $\mathbf{E}=\left[\begin{array}{cc}\hfill {\mathbf{E}}_1\hfill & \hfill {\mathbf{E}}_{12}\hfill \\ \hfill {\mathbf{E}}_{12}^{\top }\hfill & \hfill {\mathbf{E}}_2\hfill \end{array}\right]\in {\mathbb{S}}_{++}^n$ where ${\mathbf{E}}_1\in {\mathbb{R}}^{n_1\times n_1}$, ${\mathbf{E}}_2\in {\mathbb{R}}^{n_2\times n_2}$, and n = n₁ + n₂, ${\mathbf{E}}_2^{-1}$ can be expressed as $(\mathcal{E}+{\mathbf{S}}_2)\mathbf{G}{\mathbf{S}}_2^{\top }$ where G = E⁻¹ and ${\mathbf{S}}_2=\left[\begin{array}{cc}\hfill {\mathbf{0}}_{n_2\times n_1}\hfill & \hfill {\mathbf{I}}_{n_2\times n_2}\hfill \end{array}\right]\in {\mathbb{R}}^{n_2\times n}$.

The detailed proof of Proposition 1 is provided in Appendix. We introduce an estimated external force ${\widehat{\mathbf{F}}}_t={[{\widehat{\mathbf{f}}}_t^{\top },{\widehat{\mathit{\tau }}}_t^{\top }]}^{\top }$ satisfying the following equality: flushleft

$\mathbf{S}{\mathit{\psi }}_t=\mathbf{S}\mathbf{M}{\left(\mathbf{q}\right)}^{-1}{\mathbf{\Sigma }}_a{\mathbf{J}}_t{\left(\mathbf{q}\right)}^{\top }{\widehat{\mathbf{F}}}_t(14)$

Where ${\widehat{\mathbf{f}}}_t\in {\mathbb{R}}^3$ and ${\widehat{\mathit{\tau }}}_t\in {\mathbb{R}}^3$ denote the estimated force and torque of the unknown disturbance, respectively. Also, ${\mathbf{\Sigma }}_a={\mathbf{S}}^{\top }\mathbf{S}=\left[\begin{array}{cc}\hfill {\mathbf{0}}_{6\times 6}\hfill & \hfill {\mathbf{0}}_{6\times n_u}\hfill \\ \hfill {\mathbf{0}}_{n_u\times 6}\hfill & \hfill {\mathbf{I}}_{n_u\times n_u}\hfill \end{array}\right]$. Unfortunately, we cannot simply express Sψ_t in terms of q and $\dot{\mathbf{q}}$ since we need an additional explicit mapping between ${\widehat{\mathbf{F}}}_t$ and these joint state variables.

Our approach is to employ robot centroidal dynamics to convert the external forces into corresponding stiffness and damping forces in the joint space. We can approximate the CoM velocity, acceleration, and the time derivative of its angular momentum with a discrete time interval Δt Dai et al. (2014) as follows:

$\begin{array}{cc}\hfill \dot{\mathbf{p}}& \approx 2\mathrm{\Delta }{t}^{-1}\left(\mathbf{p}-{\mathbf{p}}^p\right)-{\mathbf{J}}_{\text{CoM}}\left({\mathbf{q}}^p\right){\dot{\mathbf{q}}}^p,\hfill \\ \hfill \ddot{\mathbf{p}}& \approx \mathrm{\Delta }{t}^{-1}\left(\dot{\mathbf{p}}-{\mathbf{J}}_{\text{CoM}}\left({\mathbf{q}}^p\right){\dot{\mathbf{q}}}^p\right),\hfill \\ \hfill \dot{\mathbf{h}}\left(\mathbf{q},\dot{\mathbf{q}}\right)& \approx \mathrm{\Delta }{t}^{-1}\left(\mathbf{h}\left(\mathbf{q},\dot{\mathbf{q}}\right)-\mathbf{h}\left({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p\right)\right)\hfill \end{array}(15)$

Where p, $\mathbf{h}(\mathbf{q},\dot{\mathbf{q}})$, J_CoM(q) denote the position of CoM, its angular momentum, and its Jacobian, respectively. q^p, ${\dot{\mathbf{q}}}^p$, p^p, and ${\dot{\mathbf{p}}}^p$ are the joint position, the joint velocity, the CoM position, and the CoM velocity in the previous time step, respectively. First, we estimate the linear force corresponding to the external disturbance, f_t, using the centroidal dynamics and CoM acceleration:

${\widehat{\mathbf{f}}}_t=M_r\left(\ddot{\mathbf{p}}-\mathbf{g}\right)-\sum _{\ell =1}^m{\mathbf{f}}_{\ell }^{\star }(16)$

Where ${\mathbf{f}}_{\ell }^{\star }\in {\mathbb{R}}^3$ represents the linear force part for the ℓ-th contact among ${\mathbf{F}}_c^{\star }$ from WBC. Also, M_r and g denote the total mass of the robot and the gravity vector, respectively. In addition, ${\widehat{\mathit{\tau }}}_t$ can be estimated by using the centroidal momentum equation and ${\widehat{\mathbf{f}}}_t$.

${\widehat{\mathit{\tau }}}_t=\dot{\mathbf{h}}\left(\mathbf{q},\dot{\mathbf{q}}\right)-\sum _{\ell =1}^m\left({\mathbf{r}}_{\ell }\times {\mathbf{f}}_{\ell }^{\star }+{\mathit{\tau }}_{\ell }^{\star }\right)-{\mathbf{r}}_t\times {\widehat{\mathbf{f}}}_t(17)$

Where ${\mathit{\tau }}_{\ell }^{\star }\in {\mathbb{R}}^3$ denotes the torque part for the ℓ-th contact among ${\mathbf{F}}_c^{\star }$. In turn, we can re-express the closed-loop dynamics (13) using the estimated external force ${\widehat{\mathbf{F}}}_t$. We cannot decouple the closed-loop dynamics due to the term ψ_t.

3.2 The Proposed Gain Adaptation Approach

Our proposed method consists of decoupling the closed-loop dynamics by approximating ψ_t. Our goal is to obtain a pair of PD gains to ensure that the approximated closed-loop dynamics are critically damped, i.e.

${\mathit{\psi }}_t\approx {\tilde{\mathbf{K}}}_p\mathbf{q}+{\tilde{\mathbf{K}}}_d\dot{\mathbf{q}}+{\mathbf{\Omega }}_t(18)$

Where ${\tilde{\mathbf{K}}}_p=\mathrm{diag}({\tilde{k}}_{p_1},\dots ,{\tilde{k}}_{p_n})$, ${\tilde{\mathbf{K}}}_d=\mathrm{diag}({\tilde{k}}_{d_1},\dots ,{\tilde{k}}_{d_n})$, and Ω_t are approximated PD gain matrices and a constant bias term, respectively. To obtain this approximation we have used the centroidal dynamics and the external force approximations specified further above. First, p is calculated using a first order approximation:

$\mathbf{p}\approx {\mathbf{p}}^p+{\mathbf{J}}_{\text{CoM}}\left({\mathbf{q}}^p\right)\left(\mathbf{q}-{\mathbf{q}}^p\right)(19)$

In turn the estimated external force ${\widehat{\mathbf{f}}}_t$ can be expressed in joint space as follows using (15) and (16):

$\begin{array}{cc}\hfill & {\widehat{\mathbf{f}}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)=M_r{\mathbf{J}}_{\text{CoM}}{\left({\mathbf{q}}^p\right)\mathcal{L}}_f\left(\mathbf{q},\dot{\mathbf{q}}\right)-M_r\mathbf{g}-\sum _{\ell =1}^m{\mathbf{f}}_{\ell }^{\star },\hfill \\ \hfill & {\mathcal{L}}_f\left(\mathbf{q},\dot{\mathbf{q}}\right)=\frac{2\left(\mathbf{q}-{\mathbf{q}}^p\right)}{\mathrm{\Delta }{t}^2}-\frac{2{\dot{\mathbf{q}}}^p}{\mathrm{\Delta }t}={\mathbf{K}}_p^f\mathbf{q}+{\mathbf{K}}_d^f\dot{\mathbf{q}}+{\mathit{ϵ}}_f\hfill \end{array}(20)$

Where both matrices, ${\mathbf{K}}_p^f=\mathrm{diag}(k_{p_1}^f,\dots ,k_{p_n}^f)$ and ${\mathbf{K}}_d^f=\mathrm{diag}(k_{d_1}^f,\dots ,k_{d_n}^f)$, represent gain matrices. Using established adaptive techniques we chose the proportional gain as $k_{p_i}^f={\eta }_i^f(q_i^d-q_i^p)$ and plugging it in the second equation shown in (20), we get $k_{d_i}^f={\xi }_i^f\mathrm{\Delta }t{k}_{p_i}^f$ where ${\eta }_i^f$ and ${\xi }_i^f$ represent positive coefficients. We also get ϵ_f as:

${\mathit{ϵ}}_f=\left(2\mathbf{I}-\mathrm{\Delta }t{\mathbf{K}}_{pd}^f\right){\ddot{\mathbf{q}}}^f-{\mathbf{K}}_{pd}^f{\dot{\mathbf{q}}}^p-{\mathbf{K}}_p^f{\mathbf{q}}^p(21)$

Where ${\mathbf{K}}_{pd}^f=\mathrm{\Delta }t{\mathbf{K}}_p^f+{\mathbf{K}}_d^f$. In turn Eq. 20 becomes:

${\widehat{\mathbf{f}}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)={\tilde{\mathbf{K}}}_p^f\mathbf{q}+{\tilde{\mathbf{K}}}_d^f\dot{\mathbf{q}}+{\mathit{\gamma }}_f(22)$

Where ${\tilde{\mathbf{K}}}_p^f=M_r{\mathbf{J}}_{\text{CoM}}({\mathbf{q}}^p){\mathbf{K}}_p^f$, ${\tilde{\mathbf{K}}}_d^f=M_r{\mathbf{J}}_{\text{CoM}}({\mathbf{q}}^p){\mathbf{K}}_d^f$, and ${\mathit{\gamma }}_f=M_r{\mathbf{J}}_{\text{CoM}}({\mathbf{q}}^p){\mathit{ϵ}}_f-M_r\mathbf{g}-{\sum }_{\ell =1}^m{\mathbf{f}}_{\ell }^{\star }$.

Secondly, we estimate (15) using the first order approximation:

$\begin{array}{cc}\hfill \mathbf{h}\left(\mathbf{q},\dot{\mathbf{q}}\right)\approx & \mathbf{h}\left({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p\right)+{\mathbf{H}}_G\left({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p\right)\left(\mathbf{q}-{\mathbf{q}}^p\right)+{\mathbf{A}}_G\left({\mathbf{q}}^p\right)\left(\dot{\mathbf{q}}-{\dot{\mathbf{q}}}^p\right),\hfill \\ \hfill {\mathbf{A}}_G\left(\mathbf{q}\right)=& \frac{\partial \mathbf{h}}{\partial \dot{\mathbf{q}}}\left(\mathbf{q},\dot{\mathbf{q}}\right),{\mathbf{H}}_G\left(\mathbf{q},\dot{\mathbf{q}}\right)=\sum _{k=1}^n\frac{\partial {\mathbf{A}}_G^k\left(\mathbf{q}\right)}{\partial \mathbf{q}}{\dot{q}}_k\hfill \end{array}(23)$

Where ${\mathbf{A}}_G^k(\mathbf{q})$ is the kth column vector of A_G(q). Based on the above approximations, the estimated external torque can be expressed as follows:

$\begin{array}{cc}\hfill {\widehat{\mathit{\tau }}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)=& {\mathbf{H}}_G{\left({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p\right)\mathcal{L}}_{\tau }\left(\mathbf{q},\dot{\mathbf{q}}\right)+{\mathbf{A}}_G\left({\mathbf{q}}^p\right){\ddot{\mathbf{q}}}^f-\sum _{\ell =1}^m\left({\mathbf{r}}_{\ell }\times {\mathbf{f}}_{\ell }^{\star }+{\mathit{\tau }}_{\ell }^{\star }\right)-{\mathbf{r}}_t\times {\widehat{\mathbf{f}}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right),\hfill \\ \hfill {\mathcal{L}}_{\tau }\left(\mathbf{q},\dot{\mathbf{q}}\right)=& {\mathbf{K}}_p^{\tau }\mathbf{q}+{\mathbf{K}}_d^{\tau }\dot{\mathbf{q}}+{\mathit{ϵ}}_{\tau }\hfill \end{array}(24)$

Where both matrices, ${\mathbf{K}}_p^{\tau }=\mathrm{diag}(k_{p_1}^{\tau },\dots ,k_{p_n}^{\tau })$ and ${\mathbf{K}}_d^{\tau }=\mathrm{diag}(k_{d_1}^{\tau },\dots ,k_{d_n}^{\tau })$, are gain matrices for estimating the external torque. More specifically, each component can be computed using adaptive control techniques in Åström and Wittenmark (2013) as: $k_{p_i}^{\tau }={\eta }_i^{\tau }(q_i^d-q_i^p)$ and $k_{d_i}^{\tau }={\xi }_i^{\tau }\mathrm{\Delta }t{k}_{p_i}^{\tau }$, where ${\eta }_i^{\tau }$ and ${\xi }_i^{\tau }$ represent positive coefficients. The term ϵ_τ is described as ${\mathit{ϵ}}_{\tau }=\left(\mathrm{\Delta }t\mathbf{I}-\mathrm{\Delta }t{\mathbf{K}}_{pd}^{\tau }\right){\ddot{\mathbf{q}}}^f+\left(\mathbf{I}-{\mathbf{K}}_{pd}^{\tau }\right){\dot{\mathbf{q}}}^p-{\mathbf{K}}_p^{\tau }{\mathbf{q}}^p$ where ${\mathbf{K}}_{pd}^{\tau }=\mathrm{\Delta }t{\mathbf{K}}_p^{\tau }+{\mathbf{K}}_d^{\tau }$. Suppose r_ℓ and r_t are constant then we can re-write the estimated ${\widehat{\mathit{\tau }}}_t$ as:

${\widehat{\mathit{\tau }}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)={\tilde{\mathbf{K}}}_p^{\tau }\mathbf{q}+{\tilde{\mathbf{K}}}_d^{\tau }\dot{\mathbf{q}}+{\mathit{\gamma }}_{\tau }(25)$

Where ${\tilde{\mathbf{K}}}_p^{\tau }={\mathbf{H}}_G({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p){\mathbf{K}}_p^{\tau }-\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}({\mathbf{r}}_t){\tilde{\mathbf{K}}}_p^f$ and ${\tilde{\mathbf{K}}}_d^{\tau }={\mathbf{H}}_G({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p){\mathbf{K}}_d^{\tau }-\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}({\mathbf{r}}_t){\tilde{\mathbf{K}}}_d^f$. The detailed γ_τ is

${\mathit{\gamma }}_{\tau }={\mathbf{H}}_G\left({\mathbf{q}}^p,{\dot{\mathbf{q}}}^p\right){\mathit{ϵ}}_{\tau }-{\mathbf{r}}_t\times {\mathit{\gamma }}_f-\sum _{\ell =1}^m\left({\mathbf{r}}_{\ell }\times {\mathbf{f}}_{\ell }^{\star }+{\mathit{\tau }}_{\ell }^{\star }\right)+{\mathbf{A}}_G\left({\mathbf{q}}^p\right){\ddot{\mathbf{q}}}^f.(26)$

We can construct ${\widehat{\mathbf{F}}}_t$ using selection matrices ${\mathbf{S}}_f={\left[{\mathbf{I}}_{3\times 3},{\mathbf{0}}_{3\times 3}\right]}^{\top }$ and ${\mathbf{S}}_{\tau }={\left[{\mathbf{0}}_{3\times 3},{\mathbf{I}}_{3\times 3}\right]}^{\top }$ as follows:

${\widehat{\mathbf{F}}}_t={\mathbf{S}}_f{\widehat{\mathbf{f}}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)+{\mathbf{S}}_{\tau }{\widehat{\mathit{\tau }}}_t\left(\mathbf{q},\dot{\mathbf{q}}\right)={\tilde{\mathbf{K}}}_p^s\mathbf{q}+{\tilde{\mathbf{K}}}_d^s\dot{\mathbf{q}}+{\mathit{\gamma }}^s(27)$

Where ${\tilde{\mathbf{K}}}_p^s={\mathbf{S}}_f{\tilde{\mathbf{K}}}_p^f+{\mathbf{S}}_{\tau }{\tilde{\mathbf{K}}}_p^{\tau }$, ${\tilde{\mathbf{K}}}_d^s={\mathbf{S}}_f{\tilde{\mathbf{K}}}_d^f+{\mathbf{S}}_{\tau }{\tilde{\mathbf{K}}}_d^{\tau }$, and γ^s = S_fγ_f + S_τγ_τ. Using the above estimated external force ${\widehat{\mathbf{F}}}_t$, we approximate ψ_t as follows:

$\begin{array}{cc}\hfill {\mathit{\psi }}_t\approx & \mathbf{M}{\left({\mathbf{q}}^p\right)}^{-1}{\mathbf{\Sigma }}_a{\mathbf{J}}_t\left({\mathbf{q}}^p\right)\left({\tilde{\mathbf{K}}}_p^s\mathbf{q}+{\tilde{\mathbf{K}}}_d^s\dot{\mathbf{q}}+{\mathit{\gamma }}^s\right)\hfill \\ \hfill \approx & {\widehat{\mathbf{Z}}}_p\mathbf{q}+{\widehat{\mathbf{Z}}}_d\dot{\mathbf{q}}+{\widehat{\mathbf{\Omega }}}_t\hfill \end{array}(28)$

Where ${\widehat{\mathbf{Z}}}_p$ and ${\widehat{\mathbf{Z}}}_d$ are diagonal matrices. We define ${\widehat{\mathbf{Z}}}_p=\mathrm{diag}(z_{p_1},\dots ,z_{p_n})$ and ${\widehat{\mathbf{Z}}}_d=\mathrm{diag}(z_{d_1},\dots ,z_{d_n})$ where $z_{p_i}$ and $z_{d_i}$ are the ith diagonal components of $\mathbf{M}{({\mathbf{q}}^p)}^{-1}{\mathbf{\Sigma }}_a{\mathbf{J}}_t({\mathbf{q}}^p){\tilde{\mathbf{K}}}_p^s$ and $\mathbf{M}{({\mathbf{q}}^p)}^{-1}{\mathbf{\Sigma }}_a{\mathbf{J}}_t({\mathbf{q}}^p){\tilde{\mathbf{K}}}_d^s$, respectively. The last term takes the form ${\widehat{\mathbf{\Omega }}}_t=\mathbf{M}{({\mathbf{q}}^s)}^{-1}{\mathbf{J}}_t{({\mathbf{q}}^p)}^{\top }{\mathit{\gamma }}^s$. Since ${\widehat{\mathbf{K}}}_p$, ${\widehat{\mathbf{K}}}_d$, ${\widehat{\mathbf{Z}}}_p$, and ${\widehat{\mathbf{Z}}}_d$ are diagonal matrices, the closed-loop dynamics in actuated joint level space becomes:

${\ddot{\mathbf{e}}}_a=\left({\widehat{\mathbf{K}}}_p^a-{\widehat{\mathbf{Z}}}_p^a\right){\mathbf{e}}_a+\left({\widehat{\mathbf{K}}}_d^a-{\widehat{\mathbf{Z}}}_d^a\right){\dot{\mathbf{e}}}_a+{\bar{\mathbf{\Omega }}}_{t,a}(29)$

Where ${\mathbf{e}}_{{\mathbf{q}}_a}={\mathbf{q}}_a^d-{\mathbf{q}}_a$, ${\dot{\mathbf{e}}}_a={\dot{\mathbf{q}}}_a^d-{\dot{\mathbf{q}}}_a$, and ${\ddot{\mathbf{e}}}_a=\mathbf{S}({\ddot{\mathbf{q}}}^d+\delta {\ddot{\mathbf{q}}}^{\star })-{\ddot{\mathbf{q}}}_a$. Also, we define ${\widehat{\mathbf{K}}}_p^a=\mathbf{S}{\widehat{\mathbf{K}}}_p=\mathrm{diag}({\widehat{k}}_{p_7},\dots ,{\widehat{k}}_{p_n})$, ${\widehat{\mathbf{K}}}_d^a=\mathbf{S}{\widehat{\mathbf{K}}}_d=\mathrm{diag}({\widehat{k}}_{d_7},\dots ,{\widehat{k}}_{d_n})$, ${\widehat{\mathbf{Z}}}_p^a=\mathrm{diag}(z_{p_7},\dots ,z_{p_n})$, and ${\widehat{\mathbf{Z}}}_d^a=\mathrm{diag}(z_{d_7},\dots ,z_{d_n})$, and ${\bar{\mathbf{\Omega }}}_{t,a}=\mathbf{S}({\widehat{\mathbf{\Omega }}}_t-{\widehat{\mathbf{Z}}}_p{\mathbf{q}}^d-{\widehat{\mathbf{Z}}}_d{\dot{\mathbf{q}}}^d)$.

We define the state $\mathit{\nu }={[{\mathbf{e}}_a^{\top },{\dot{\mathbf{e}}}_a^{\top }]}^{\top }$ and express the state-space model of the above dynamics equation as follows:

$\dot{\mathit{\nu }}=\underset{\mathbf{V}}{\underbrace{\left[\begin{array}{cc}\hfill {\mathbf{0}}_n\hfill & \hfill {\mathbf{I}}_n\hfill \\ \hfill -{\widehat{\mathbf{K}}}_p^a+{\widehat{\mathbf{Z}}}_p^a\hfill & \hfill -{\widehat{\mathbf{K}}}_d^a+{\widehat{\mathbf{Z}}}_d^a\hfill \end{array}\right]}}\mathit{\nu }+\left[\begin{array}{c}\hfill {\mathbf{0}}_{n\times 1}\hfill \\ \hfill {\bar{\mathbf{\Omega }}}_{t,a}\hfill \end{array}\right].(30)$

Properly choosing ${\eta }_i^f$, ${\xi }_i^f$, ${\eta }_i^{\tau }$, and ${\xi }_i^{\tau }$, we can make ${\bar{\mathbf{\Omega }}}_{t,a}$ negligible. Then, the matrix V should be Hurwitz to guarantee that the closed-loop state model is exponentially stable. To compute the characteristic equation of the matrix V, we express the determinant of the matrix V − λI_2n as follows:

$\begin{array}{cc}\hfill \det \left(\left[\begin{array}{cc}\hfill -\lambda {\mathbf{I}}_n\hfill & \hfill {\mathbf{I}}_n\hfill \\ \hfill -{\widehat{\mathbf{K}}}_p^a+{\widehat{\mathbf{Z}}}_p^a\hfill & \hfill -{\widehat{\mathbf{K}}}_d^a+{\widehat{\mathbf{Z}}}_d^a-\lambda {\mathbf{I}}_n\hfill \end{array}\right]\right)& ={\lambda }^{-1}\det \left({\widehat{\mathbf{K}}}_d-{\widehat{\mathbf{Z}}}_{\dot{q}}+{\lambda }^{-1}\left({\widehat{\mathbf{K}}}_p-{\widehat{\mathbf{Z}}}_q\right)+\lambda {\mathbf{I}}_{n\times n}\right)\hfill \\ \hfill & =\det {\lambda }^{-1}\underset{\mathbf{X}}{\underbrace{\left(\left({\widehat{\mathbf{K}}}_d^a-{\widehat{\mathbf{Z}}}_d^a\right)+{\lambda }^{-1}\left({\widehat{\mathbf{K}}}_p^a-{\widehat{\mathbf{Z}}}_p^a\right)\right)}}+{\mathbf{I}}_n,\hfill \end{array}$

Which means that   det(V − λI₂) =  det(λ⁻¹X + I_n). It is complicated to obtain analytic forms of the eigenvalues of V. For this reason, we can compute the determinant above using the Leibniz formula described in Bhatia (2013).

$\det \left({\lambda }^{-1}\mathbf{X}+{\mathbf{I}}_n\right)=1+{\lambda }^{-1}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left(\mathbf{X}\right)+\mathcal{O}\left({\left({\lambda }^{-1}\right)}^2\right)(31)$

Where $\mathcal{O}(.)$ represents the big-O notation. Using the above formulation, the characteristic equation can be re-written as:

$\det \left(\mathbf{V}-\lambda {\mathbf{I}}_{2n}\right)=1+{\lambda }^{-1}\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left(\mathbf{X}\right)+\mathcal{O}\left({\left({\lambda }^{-1}\right)}^2\right)=0.(32)$

All solutions of the above characteristic equation λ should be negative to guarantee that the matrix V is Hurwitz. Such analysis could be conducted via numerical computation given the gain matrices. However, in this paper, we aim at adaptive feedback gains of the controller so that we obtain ${\tilde{\mathbf{K}}}_p^a={\widehat{\mathbf{Z}}}_p^a$ and ${\tilde{\mathbf{K}}}_d^a={\widehat{\mathbf{Z}}}_d^a$. In turn the term $\mathcal{O}({({\lambda }^{-1})}^2)$ vanishes. We can obtain the analytical expression of the approximated characteristic equation as follows:

${\lambda }^2+\lambda \mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left({\widehat{\mathbf{K}}}_p^a-{\tilde{\mathbf{K}}}_p^a\right)+\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}\left({\widehat{\mathbf{K}}}_d^a-{\tilde{\mathbf{K}}}_d^a\right)=0.(33)$

This condition will allow us to make the closed-loop system stable.

Condition 1. The following condition makes the matrix V Hurwitz: ${\sum }_{i=7}^n{\widehat{k}}_{p_i}-z_{p_i}>0$ and ${\sum }_{i=7}^n{\widehat{k}}_{d_i}-z_{d_i}>0$. Without any adaptation, these condition should be fulfilled to make the closed-loop system asymptotically stable given desired feedback gains.

We can approximately decouple the dynamics of (29) in the joint space as follows:

${\ddot{q}}_i+\left({\widehat{k}}_{d_i}-z_{d_i}\right){\dot{q}}_i+\left({\widehat{k}}_{p_i}-z_{p_i}\right)q_i={\tilde{\mathcal{F}}}_i^{\star }(34)$

Where ${\tilde{\mathcal{F}}}_i^{\star }={\ddot{q}}_i^f+{\widehat{k}}_{p_i}{q}_i^d+{\widehat{k}}_{d_i}{\dot{q}}_i+{\widehat{\mathbf{\Omega }}}_t(i)$. We enforce that the joint space dynamics are critically damped given $z_{p_i}$ and $z_{d_i}$ for all i ∈ {7, …, n}. For the above system, the following relationships are obtained in terms of the natural frequency and the damping ratio: ${\widehat{k}}_{p_i}-z_{p_i}={\bar{W}}_n^2$ and ${\widehat{k}}_{d_i}-z_{d_i}=2\zeta {\bar{W}}_n$. Since we already know all gains and coefficients, ${\widehat{k}}_{p_i}$, ${\widehat{k}}_{d_i}$, $z_{p_i}$, and $z_{d_i}$, ζ can be determined a priori. We can compute adaptive variables ${\epsilon }_{p_i}$ and ${\epsilon }_{d_i}$ to make ${\bar{W}}_n=W_n^d$ and ζ = ζ^d.

$\begin{array}{cc}\hfill {\epsilon }_{p_i}& ={\left(W_n^d\right)}^2+z_{q_i}-{\widehat{k}}_{p_i},\hfill \\ \hfill {\epsilon }_{d_i}& =2{\zeta }^d\sqrt{{\widehat{k}}_{p_i}+{\epsilon }_{p_i}-z_{q_i}}+z_{d_i}-{\widehat{k}}_{d_i}.\hfill \end{array}(35)$

Condition 2. Since W_n is a positive real number, it is always true that ${\widehat{k}}_{p_i}+{\epsilon }_{p_i}-z_{p_i}>0$ and ${\widehat{k}}_{d_i}+{\epsilon }_{d_i}-z_{d_i}>0$ in the adaptive system. In such case: ${\sum }_{i=7}^n{\widehat{k}}_{p_i}+{\epsilon }_{p_i}-z_{p_i}>0$ and ${\sum }_{i=7}^n{\widehat{k}}_{d_i}+{\epsilon }_{d_i}-z_{d_i}>0$. However, if there are no ${\epsilon }_{p_i}$ and ${\epsilon }_{d_i}$ satisfying the equation above, the motion bandwidth cannot be stably achievable under the external force.

Based on the previous adaptation scheme, we replace the WBLC $k_{p_i}^h$, $k_{d_i}^h$ by ${\bar{k}}_{p_i}^h$, ${\bar{k}}_{d_i}^h$ in the optimization problem where ${\bar{k}}_{p_i}^h=k_{p_i}^h+{\epsilon }_{p_i}$ and ${\bar{k}}_{d_i}^h=k_{d_i}^h+{\epsilon }_{d_i}$. These feedback gains are limited in practice. Since relying on the contact forces, the adaptive gains might abruptly increase during contact transition or applying an impact force. For such reason, we consider the following additional condition:

Condition 3. We set the ranges of ${\epsilon }_p\in [{\epsilon }_p^{\min },{\epsilon }_p^{\max }]$ and ${\epsilon }_d\in [{\epsilon }_d^{\min },{\epsilon }_d^{\max }]$ for real implementation in robotic systems.

After testing all conditions, we update the gains in WBLC. Then, the control command torque is obtained without further modifications given the task specifications. Algorithm 1 summarizes the entire process of how to update the feedback gains in WBLC.

Algorithm 1. Proposed Gain Adaptation Algorithm

Data: q, $\dot{\mathbf{q}}$, q^p, ${\dot{\mathbf{q}}}^p$, ${\mathbf{x}}_k^d$, ${\dot{\mathbf{x}}}_k^d$, ${\ddot{\mathbf{x}}}_k^d\forall k\in \{1,\dots ,N\}$

Result: Γ_cmd

${\widehat{\mathbf{F}}}_c^{\star }\leftarrow$ WBLC in (5)

J_CoM ← with q^p

${\tilde{\mathbf{K}}}_p^f$, ${\tilde{\mathbf{K}}}_d^f$,γ_f ← (22)

h, H_G, A_G ← with q^p and ${\dot{\mathbf{q}}}^p$

${\tilde{\mathbf{K}}}_p^{\tau }$, ${\tilde{\mathbf{K}}}_d^{\tau }$, γ_τ ← (25)

${\tilde{\mathbf{K}}}_p^s$, ${\tilde{\mathbf{K}}}_d^s$, γ_s ← (27)

${\widehat{\mathbf{Z}}}_p$, ${\widehat{\mathbf{Z}}}_d\leftarrow$ (28)

${\epsilon }_{p_i}$, ${\epsilon }_{d_i}\forall i\in \{7,\dots ,n\}\leftarrow$ (35)

if Condition 1, 2, and 3 are satisfied then

${\bar{k}}_{p_i}^h=k_{p_i}^h+{\epsilon }_{p_i}$, ${\bar{k}}_{d_i}^h=k_{d_i}^h+{\epsilon }_{d_i}\forall i\in \{7,\dots ,n\}$

end

${\mathbf{K}}_p^h$ and ${\mathbf{K}}_d^h$ in (5) $\leftarrow {\bar{k}}_{p_i}^h$ and ${\bar{k}}_{d_i}^h$

Γ_cmd ← with the updated ${\mathbf{K}}_p^h$ and ${\mathbf{K}}_d^h$

4 Simulations and Experiments

In this section, our proposed adaptive approach is validated by demonstrating both simulations and experiments using a 10 DOF liquid-cooled bipedal robot called Draco2 and a full-body humanoid robot called Valkyrie. Since Draco2 is a line-feet biped robot, it is hard for it to maintain its balance during the double support phase robustly. So, in section 4.1, we focus on showing how to stabilize the robot’s body during the double support phase while an external force is applied to its pelvis, as shown in Figure 3. In turn we demonstrate experimentally that our online gain adaptation method can be applied in real-time to a physical platform as discussed in section 4.2. Using Valkyrie which uses flat feet for balancing, we implement walking simulations to verify the effectiveness of the proposed online gain adaptation approach when there exists long-term perturbations instead of fast impacts. This study is described in section 4.3. We employ our open-source software architecture called PnC¹ (Planning and Control).

FIGURE 3

FIGURE 3. (A) Snapshots of control simulations: the upper and lower figures show balancing behaviors with and without gain adaptation, respectively. A black star marks the edge point on the hip shell as a measurement reference. (B) Comparison between the positions of the robot’s CoM using standard WBLC versus adaptive WBLC, where the desired x and y positions of the CoM is the center position between two feet [3.80, ×, 10^–5, −0.167] m, and the desired height of the CoM is 0.7 m.

4.1 Draco2 Simulations

Using Draco2, we implement three simulations to show that the proposed approach makes the robot more stable: 1) pushing the pelvis in the double support phase, 2) pushing the pelvis while swinging CoM position, 3) applying an impact force to the pelvis in the double support. For whole-body control, we define these tasks: x_CoM, x_{base, o}, x_feet, and x_joint, which represent tasks for controlling the position of the robot’s CoM, the orientation of its floating base, the position and orientation of its feet, and the joint configuration, respectively.

4.1.1 Pushing Force Applied to the Pelvis

The first simulation aims to control the position of the robot’s CoM while applying a pushing force at the pelvis. We define the desired location of the CoM as [3.80, ×, 10^–5, −0.167, 0.7] m in the double support phase. The external force is applied to the y-direction of the pelvis with an amplitude of 12 N during the interval [10, 15] s. We design the appropriate bounds for the gain margins as ${\epsilon }_p^{\max }=350$, ${\epsilon }_p^{\min }=-100$, ${\epsilon }_d^{\max }=60$, and ${\epsilon }_d^{\min }=-10$.

As the snapshots show in Figure 3A, gain adaptation reduces the adverse effects of external forces on the robot’s behavior. Figure 3B shows the CoM positions when the robot is controlled with and without the proposed gain adaptation scheme. For a more precise analysis, we compare the maximum variations of the robot’s CoM position: max(x_CoM) − min(x_CoM). Without the proposed approach, [0.013 9, 0.081 0, 0.012 4] m is the maximum amplitude of the variation. When using our proposed adaptation approach the amplitude reduces to [0.029, 0.069 5, 0.009 8] m. In addition, we compare the maximum distances of the position of the CoM $d_{\max }^{\text{CoM}}=\Vert {\mathbf{x}}_{\text{CoM}}^d-{\mathbf{x}}_{\text{CoM}}^{\text{far}}\Vert$ where ${\mathbf{x}}_{\text{CoM}}^{\text{far}}$ denotes the farthest position from the desired position while the external force is acting on the robot. The values of $d_{\max }^{\text{CoM}}$ with and without the proposed adaptive approach are 0.070 9 m and 0.083 5 m, respectively. Therefore $d_{\max }^{\text{CoM}}$ with the proposed approach is 84.91% of that without our method. Although the difference is small, it is significant for legged robots having a small support polygon such as Draco2. The proposed approach results in the adaptation of the feedback gains in WBLC as shown in Figure 4A. Also, the configuration of the robot for each case is represented in Figure 4B. As shown in the results, the adapted gains contribute to stabilize the robot so that the configuration becomes less oscillatory. More specifically, the settling times of the simulations with and without the proposed approach are 18.16 and 10.2 s, respectively.

FIGURE 4

FIGURE 4. Simulation results of pushing the pelvis: (A) The adapted gains of the WBLC in the joint level. (B) The configuration of the robot controlled by WBLC.

4.1.2 Pushing Force While Swinging

In this second simulation, we implement a behavior consisting of swinging the position of the robot’s CoM in double support phase while an external force is acting on the pelvis. We generate a sinusoidal reference for the CoM with 0.03 m amplitude and 0.5 Hz frequency. Figures 5A,B show snapshots of the simulations. As shown in Figure 5A, the controller with fixed gains falls due to the effect of the external force. On the other hand, the adaptive gains prevent the robot from losing balance, as shown in Figure 5B. The positions of the CoM controlled by WBLC with and without adaptive gains are compared in Figure 5C. The robot with the adaptive gains can track the trajectory of the CoM while maintaining solid contacts with the ground. However, employing WBLC without adaptation fails to control the position of the CoM as shown in Figure 5C, and as a result the right foot starts moving away from contact around 14 s as shown in Figure 5D.

FIGURE 5

FIGURE 5. Simulation results of CoM swing with unknown external force: (A) Snapshots of the simulation swinging CoM without the proposed gain adaption. (B) Snapshots of the simulation with the proposed gain adaptation. (C) The Positions of the robot’s CoM while performing a swinging motion: x, y, and z position of the CoM, (D) The right foot position without the adaptive gains.

4.1.3 Impact Force

The last simulation compares the response of the robot due to an impact force with and without the proposed adaptive control approach. An impact force with an amplitude 700 N over 0.01 s from 10 s is applied. Figure 6 shows the position of the CoM in response to the impact force, and the impact timing is showing as marked lines. We compare the settling times of the y position of the CoM, which are 11.194 and 4.903 s without and with the proposed approach, respectively. Also, the peak of the y position of the CoM with adaptive gains, − 0.207 6 m, is smaller than that without the proposed approach, − 0.219 0 m. Based on these results, it is verified that the proposed approach makes the robot more robust to impact forces.

FIGURE 6

FIGURE 6. Simulation results of impact test: (A) the desired position of the CoM is the same as that for the pushing scenario. An impact force is applied to the robot with an amplitude of 700 N and duration 0.01 at 10 s. (B) The position of the robot’s CoM.

4.1.4 Addition Simulations

Here, we increase the amplitude of the external disturbances with respect to the previous simulations to analyze the effectiveness of our method. Firstly, we increase the external pushing force from −12 N to −18 N with respect to the forces used in Section 4.1.1. Without the proposed adaptation, the robot falls when applying −14 N due to the effect of the pushing force. However, the robot with the adapted gain scheme keeps its balance while tolerating external forces up to −17 N. The robot falls when applying −18 N. Secondly, we increase the pushing force from −10 N to −17 N while swinging the CoM position as shown in the simulation in Section 4.1.2. The robot with fixed gains loses its balance when the pushing force is −12 N. By contrast, our method enables the robot to swing the CoM position robustly while increasing the external force up to −16 N. These results indicate that the robot’s balance is more fragile when applying the external force during dynamic motions rather than static ones. Lastly, we repeat the impact test of Section 4.1.3 with different amplitudes of the impact forces. The robot without our method slips and falls when the amplitude of the impact is −1500 N. In contrast, our gain adaptation scheme keeps the robot standing up to −1900 N of impact force. Overall, these results confirm that our proposed gain adaptation method yields more robust balancing capabilities against external disturbances than using fixed gains.

For more detailed analysis of the results, we calculate the Integral Absolute Error (IAE) of the CoM task with respect to the external forces during the perturbation period:

$\mathrm{I}\mathrm{A}\mathrm{E}={\int }_{t_i}^{t_f}\Vert {\mathbf{x}}_{\text{com}}^d\left(t\right)-{\mathbf{x}}_{\text{com}}\left(t\right)\Vert dt(36)$

Where [t_i, t_f] denotes the time interval for the perturbation. Figure 7 shows the IAE values for all simulation results concerning the various amplitudes of the external disturbances. We repeat the same simulation scenarios shown in Sections 4.1.1 (push while standing: Figure 7A), 4.1.2 (push while swinging the CoM position: Figure 7B), and 4.1.3 (impact while standing: Figure 7C). As shown in Figure 7, our proposed method results in smaller IAE values compared to fixed-gain WBLC. The comparison of the IAE values shows that our proposed approach reduces the task tracking error and makes the robot more robust regarding disturbance rejection.

FIGURE 7

FIGURE 7. IAE values corresponding to simulation results with different levels of external disturbances: The simulation scenarios are those described in Sections 4.1.1 (A), 4.1.2 (B), and 4.1.3 (C). To calculate the IAE values of the impact simulation (C), we use the same time interval [10, 15] to cases (A,B).

Figures 8A,B represent the estimated disturbances ${\widehat{\mathbf{F}}}_t$ when applying the pushing force (Section 4.1.1) and the impact disturbance (Section 4.1.3), respectively. The mean of the estimated force ${\widehat{\mathbf{F}}}_t$ in Figure 8A is ${\widehat{\mathbf{F}}}_{t,\mathrm{m}\mathrm{e}\mathrm{a}\mathrm{n}}=[-1.966,-8.626,0.667,-2.981,0.483,0.297]$ N. The estimated amplitude is approximately 72% of the real external force. Therefore our estimate of the pushing force disturbance is a rough approximate, but it has the advantage that it does not require external F/T sensors to measure the contact forces. By contrast, a limitation of this estimation is that the impact disturbance cannot be precisely estimated as shown in Figure 8B, since we employ the centroidal dynamics and the WBLC formulas to estimate ${\widehat{\mathbf{F}}}_t$. The peak value of the estimated impact in the lateral direction ${\widehat{\mathbf{F}}}_{t,[y]}$ is only −5.2 N. Additional impact models and information such as impact time are required to compute the accurate impact between rigid-bodies Stewart (2000), however, this is out of the scope in this paper.

FIGURE 8

FIGURE 8. Estimation of unknown external forces using the contact force formula provided by WBLC: (A) Pushing force disturbance (− 12 N in the y direction), (B) Impact force disturbance (− 700 N in the y direction).

4.2 Draco2 Experiments

In this section, we verify the proposed approach using the real Draco2 robot. The main goal of these experimental evaluations is to show that the simulation results can be reproducible in real hardware. We select two experimental scenarios, which are to keep the balance under pushing and impact forces and evaluate them in the real hardware as shown in Figure 9. The robot is standing upright in the double support phase, and we push and hit the handle at the pelvis of the robot.

FIGURE 9

FIGURE 9. Experiment results with the same scenarios with the simulations in 4.1.1 and 4.1.3: pushing (A,B) and impacting (C,D) the pelvis of Draco2 in double support phase. The estimated external disturbance ${\widehat{\mathbf{F}}}_t$ is computed when pushing (E) and impacting (F) the robot.

The positions of the CoM are shown in Figure 9 for both experimental scenarios. As shown in Figure 9, the robot is standing up while tracking the predefined desired CoM position, which is identical to the desired CoM position described in Sections 4.1.1, 4.1.3. For the first scenario, we push the robot at 10.6 and 25.8 s twice, which is shown as marked zones in Figure 9B. Although the robot’s CoM position has fluctuated in the y-direction, the robot does not fall and keeps its balance. Figure 9D shows the position of the CoM for the second scenario. We hit the robot’s pelvis with the rubber hammer at 8.5 s, which is shown as marked lines in Figure 9D. Note that the proposed approach makes the robot stable and keeps its balance in double support against the external perturbations as anticipated. Without our adaptation approach, it is not possible to keep the robot balanced when disturbed as shown in our Supplementary Video. Since we could not directly measure the pushing and impact disturbances, we estimate ${\widehat{\mathbf{F}}}_t$ as shown in Figures 9E,F. More specifically, the peak and mean values of the estimated pushing external disturbances in two pushing time intervals shown in Figure 9E are ${\widehat{\mathbf{F}}}_{t,[y]}^{\text{peak}}=[27.14,22.41]$ N and ${\widehat{\mathbf{F}}}_{t,[y]}^{\text{mean}}=[10.8,8.6]$ N, respectively. Figure 9F shows the estimation of the external disturbance with a peak in the y direction of 6.8 N. The experimental results show that our approach is effective in the real hardware under similar conditions to the simulations.

4.3 Valkyrie Simulations

For simulated walking behaviors, we generate a reference trajectory for the robot’s CoM using the three-dimensional Divergent Component of Motion (DCM) Englsberger et al. (2013). In addition, we define multiple tasks to control the robot Valkyrie using WBLC: x_CoM, x_AM, x_{pelvis, o}, x_rf, and x_lf which represent the tasks for controlling the position of the CoM, the body’s angular momentum, the orientation of the pelvis, and the position and orientation of the right and left feet, respectively. Additionally, we assume that the desired angular momentum change is zero during the planning and control calculations.

4.3.1 DCM Planner and Contact Transition

This section briefly introduces a locomotion planner based on DCM and the parameters that we use. DCM is defined as a point that lies ahead of the position of the CoM:

$\mathit{\xi }={\mathbf{x}}_{\text{CoM}}+b{\dot{\mathbf{x}}}_{\text{CoM}}(37)$

Where b > 0 denotes the time constant of the DCM dynamics. We generate a desired DCM trajectory as discussed in Englsberger et al. (2013) and convert it to a CoM trajectory. Given walking parameters, the reference DCM can be computed at a time step t, ξ_ref(t), in turn we can obtain ${\dot{\mathbf{x}}}_{\text{CoM}}^d(t)$ and ${\mathbf{x}}_{\text{CoM}}^d(t)$ using Euler integration:

$\begin{array}{cc}\hfill {\dot{\mathbf{x}}}_{\text{CoM}}^d\left(t\right)=& -\frac{1}{b}\left({\mathbf{x}}_{\text{CoM}}^d\left(t-\mathrm{\Delta }t\right)-{\mathit{\xi }}_{\text{ref}}\left(t\right)\right),\hfill \\ \hfill {\mathbf{x}}_{\text{CoM}}^d\left(t\right)=& {\mathbf{x}}_{\text{CoM}}^d\left(t-\mathrm{\Delta }t\right)+{\dot{\mathbf{x}}}_{\text{CoM}}^d\left(t\right)\mathrm{\Delta }t\hfill \end{array}(38)$

Where Δt is the time increment. The reference DCM and feet trajectories are interpolated using a polynomial function and B-spline.

One of the advantages of WBLC is that we do not need to change the optimization problem to achieve smooth contact transitions. Using WBLC, it is possible to ensure smooth transitions between the support phases by changing the weighting matrices in the objective function in the optimization problem. The weighting matrix for the contacts W _c are interpolated as follows:

${\mathbf{W}}_c^{\text{update}}=\left(1-s\left(t\right)\right){\mathbf{W}}_c^{\text{old}}+s\left(t\right){\mathbf{W}}_c^{\text{new}}(39)$

Where ${\mathbf{W}}_c^{\text{old}}$ and ${\mathbf{W}}_c^{\text{new}}$ denote the weighting matrices before and after contact transition. $s:\mathbb{R}\mapsto [0,1]$ can be a monotonously increasing function of time from 0 to 1.

4.3.2 Pushing Force While Walking

We implement simulations of our method when walking with external pushing forces. Here Valkyrie walks five steps forward. An unknown lateral force is applied to the pelvis of the robot with an amplitude of 100 N during the interval [6.5, 8.5] s. For DCM planning, the nominal height of the CoM is 1.015 m. The single support swing time and the transition time are set to 0.75 and 0.45 s, respectively. The length of the subsequent steps is set to 0.2 m.

Figure 10 shows snapshots of walking simulations. Without our gain adaptation approach, the robot tilts and falls due to the disturbances, as shown in Figure 10A. In contrast, the robot is able to walk forward against the external forces when using our proposed method (see Figure 10B). The detailed simulation results are shown in Figure 11. In Figure 11A, we depict the positions of the CoM and both feet in Cartesian space. Figure 11A shows that the robot with gain adaptation can track the preplanned trajectories before and after the perturbation. The exact CoM position and foot locations are shown in Figures 11B,C. After standing upright, the initial CoM position of the robot is [0.521 2, 0.512 2, 1.020 7] m. With the adapted gains, the robot’s CoM reaches the goal destination [0.950 7, 0.763 2, 1.017 8] m. The center position of the feet also changes from [0.520 7, 0.512 1, 0] m to [0.954 2, 0.763 3, 0] m. However, without the proposed approach, the robot’s feet start to move slightly in the second swing phase of the right foot, as shown in Figure 11C. Also, the CoM position cannot be controlled from that moment. Then the right foot detaches from the ground at 8.2 s although it should remain on the floor. Note that these simulations show robust performance of the proposed underlying approach against unknown external force disturbances.

FIGURE 10

FIGURE 10. Snapshots of simulations for walking while an external force exists: (A) walking without the proposed gain adaptation, (B) walking with the online gain adaptation.

FIGURE 11

FIGURE 11. Simulation results of walking with an external force: (A) positions of CoM and feet in Cartesian coordinate, (B) the robot’s CoM position, (C) the robot’s feet positions.

5 Conclusion

This paper presents extensive analysis, algorithms, and experimentation for the robust and adaptive employment of whole-body controllers in legged robots. The proposed approach devises a strategy to adapt feedback control gains of WBLC in response to disturbances. When a pushing force or impact occurs, the desired feedback gains are computed given nominal natural frequency and damping ratio. Subsequently, we test the theory to achieve stability both in simulation and experimental evaluations. Thus, our proposed approach is validated in both simulation and experimentation during double support balancing under pushing and impacting forces. Also, we implement walking simulations using a DCM planner to validate our method. The simulation and experimental results show that the adaptive WBLC enables legged robots to keep their balance against unknown external disturbances. We summarize the results of Draco simulations to show the improved performance in Table 1. Although we previously addressed dynamic walking under pushing forces using WBLC, that work was not adaptive and in turn relied solely on replanning. In contrast, in this new work we have developed a framework for adaptive WBLC that may require less reliance on replanning and provide better stability and robustness. In the future, we will extend this work to loco-manipulation behaviors and under multiple external force disturbances.

TABLE 1

TABLE 1. Summary of Draco simulations.

Data Availability Statement

The original contributions presented in the study are included in the article/Supplementary Material, further inquiries can be directed to the corresponding authors.

Author Contributions

All authors contributed to the theory, implementation, and numerical/experimental validations of this study. More specifically, JL developed the theory, designed the algorithm, and conducted the simulations and real hardware experiments in this paper. JA and SB contributed to the experimental validations in this paper. DK formulated WBLC and partially contributed to the stability analysis. LS supervised this research and revised the submitted manuscript.

Funding

This work was supported by an NSF Grant No. 1724360 and partially supported by an ONR Grant No. N000141512507.

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.

Publisher’s Note

All claims expressed in this article are solely those of the authors and do not necessarily represent those of their affiliated organizations, or those of the publisher, the editors and the reviewers. Any product that may be evaluated in this article, or claim that may be made by its manufacturer, is not guaranteed or endorsed by the publisher.

Supplementary Material

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

Footnotes

¹ PnC package: https://github.com/junhyeokahn/PnC

References

Ahn, J., Jorgensen, S. J., Bang, S. H., and Sentis, L. (2021). Versatile Locomotion Planning and Control for Humanoid Robots. Front. Robotics AI, 257. doi:10.3389/frobt.2021.712239

CrossRef Full Text | Google Scholar

Albu-Schäffer, A., Ott, C., and Hirzinger, G. (2007). A Unified Passivity-Based Control Framework for Position, Torque and Impedance Control of Flexible Joint Robots. Int. J. Robotics Res. 26, 23–39. doi:10.1177/0278364907073776

CrossRef Full Text | Google Scholar

Antonelli, G. (2009). Stability Analysis for Prioritized Closed-Loop Inverse Kinematic Algorithms for Redundant Robotic Systems. IEEE Trans. Robot. 25, 985–994. doi:10.1109/tro.2009.2017135

CrossRef Full Text | Google Scholar

Åström, K. J., and Wittenmark, B. (2013). Adaptive Control. Chelmsford, MA, USA: Courier Corporation.

Google Scholar

Benallegue, M., Gergondet, P., Audrerr, H., Mifsud, A., Morisawa, M., Lamiraux, F., et al. (2018). Model-based External Force/moment Estimation for Humanoid Robots with No Torque Measurement. Proc. IEEE Int. Conf. Robotics Automation, 3122–3129. doi:10.1109/icra.2018.8460809

CrossRef Full Text | Google Scholar

Bhatia, R. (2013). Matrix Analysis, 169. Springer Science & Business Media.

Google Scholar

Caron, S., Pham, Q.-C., and Nakamura, Y. (2015). Stability of Surface Contacts for Humanoid Robots: Closed-form Formulae of the Contact Wrench Cone for Rectangular Support Areas. Proc. IEEE Int. Conf. Robotics Automation, 5107–5112. doi:10.1109/icra.2015.7139910

CrossRef Full Text | Google Scholar

Dai, H., Valenzuela, A., and Tedrake, R. (2014). Whole-body Motion Planning with Centroidal Dynamics and Full Kinematics. In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots. 295–302. doi:10.1109/humanoids.2014.7041375

CrossRef Full Text | Google Scholar

Dietrich, A., Ott, C., and Park, J. (2018). The Hierarchical Operational Space Formulation: Stability Analysis for the Regulation Case. IEEE Robot. Autom. Lett. 3, 1120–1127. doi:10.1109/lra.2018.2792154

CrossRef Full Text | Google Scholar

Englsberger, J., Ott, C., and Albu-Schäffer, A. (2013).Three-dimensional Bipedal Walking Control Using Divergent Component of Motion. In 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2600–2607. doi:10.1109/iros.2013.6696723

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. Robotics Res. 33, 1006–1028. doi:10.1177/0278364914521306

CrossRef Full Text | Google Scholar

Feng, S., Whitman, E., Xinjilefu, X., and Atkeson, C. G. (2014). Optimization Based Full Body Control for the Atlas Robot. In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots. 120–127. doi:10.1109/humanoids.2014.7041347

CrossRef Full Text | Google Scholar

Feng, S., Whitman, E., Xinjilefu, X., and Atkeson, C. G. (2015). Optimization-based Full Body Control for the Darpa Robotics challenge. J. Field Robotics 32, 293–312. doi:10.1002/rob.21559

CrossRef Full Text | Google Scholar

Hawley, L., Rahem, R., and Suleiman, W. (2019). External Force Observer for Small- and Medium-Sized Humanoid Robots. Int. J. Hum. Robot. 16, 1950030. doi:10.1142/s0219843619500300

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. Robot 40, 473–491. doi:10.1007/s10514-015-9476-6

CrossRef Full Text | Google Scholar

Hong, S., Jang, K., Kim, S., and Park, J. (2020). Regularized Hierarchical Quadratic Program for Real-Time Whole-Body Motion Generation. IEEE Trans. Mechatronics 26, 2115–2126. doi:10.1109/TMECH.2020.3032522

CrossRef Full Text | Google Scholar

Ivaldi, S., Fumagalli, M., Randazzo, M., Nori, F., Metta, G., and Sandini, G. (2011).Computing Robot Internal/external Wrenches by Means of Inertial, Tactile and F/t Sensors: Theory and Implementation on the Icub. In 11th IEEE-RAS International Conference on Humanoid Robots. IEEE, 521–528. doi:10.1109/humanoids.2011.6100813

CrossRef Full Text | Google Scholar

Käslin, R., Kolvenbach, H., Paez, L., Lika, K., and Hutter, M. (2018). Towards a Passive Adaptive Planar Foot with Ground Orientation and Contact Force Sensing for Legged Robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. 2707–2714. doi:10.1109/iros.2018.8593875

CrossRef Full Text | Google Scholar

Khatib, O., Sentis, L., Park, J., and Warren, J. (2004). Whole-body Dynamic Behavior and Control of Human-like Robots. Int. J. Hum. Robot. 01, 29–43. doi:10.1142/s0219843604000058

CrossRef Full Text | Google Scholar

Kim, D., Jorgensen, S. J., Lee, J., Ahn, J., Luo, J., and Sentis, L. (2020). Dynamic Locomotion for Passive-Ankle Biped Robots and Humanoids Using Whole-Body Locomotion Control. Int. J. Robotics Res. 39, 936–956. doi:10.1177/0278364920918014

CrossRef Full Text | Google Scholar

Koolen, T., Bertrand, S., Thomas, G., De Boer, T., Wu, T., Smith, J., et al. (2016). Design of a Momentum-Based Control Framework and Application to the Humanoid Robot Atlas. Int. J. Hum. Robot. 13, 1650007. doi:10.1142/s0219843616500079

CrossRef Full Text | Google Scholar

Kuindersma, S., Deits, R., Fallon, M., Valenzuela, A., Dai, H., Permenter, F., et al. (2016). Optimization-based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot. Auton. Robot 40, 429–455. doi:10.1007/s10514-015-9479-3

CrossRef Full Text | Google Scholar

Lee, J., Ahn, J., Bakolas, E., and Sentis, L. (2020).Reachability-based Trajectory Optimization for Robotic Systems Given Sequences of Rigid Contacts. In 2020 American Control Conference (ACC). IEEE, 2158–2165. doi:10.23919/acc45564.2020.9147926

CrossRef Full Text | Google Scholar

Lee, J., Dallali, H., Jin, M., Caldwell, D., and Tsagarakis, N. (2016). Robust and Adaptive Whole-Body Control for Humanoids with Multiple Tasks Under Uncertain Disturbances. Proc. IEEE Int. Conf. Robotics Automation, 5683–5689. doi:10.1109/icra.2016.7487790

CrossRef Full Text | Google Scholar

Lee, J., Mansard, N., and Park, J. (2012). Intermediate Desired Value Approach for Task Transition of Robots in Kinematic Control. IEEE Trans. Robot. 28, 1260–1277. doi:10.1109/tro.2012.2210293

CrossRef Full Text | Google Scholar

Mason, S., Rotella, N., Schaal, S., and Righetti, L. (2018). An Mpc Walking Framework with External Contact Forces. In Proceedings of the IEEE International Conference on Robotics and Automation. 1785–1790. doi:10.1109/icra.2018.8461236

CrossRef Full Text | Google Scholar

Nakanishi, J., Cory, R., Mistry, M., Peters, J., and Schaal, S. (2008). Operational Space Control: A Theoretical and Empirical Comparison. Int. J. Robotics Res. 27, 737–757. doi:10.1177/0278364908091463

CrossRef Full Text | Google Scholar

Nori, F., Traversaro, S., Eljaik, J., Romano, F., Del Prete, A., and Pucci, D. (2015). Icub Whole-Body Control Through Force Regulation on Rigid Non-coplanar Contacts. Front. Robot. AI 2, 6. doi:10.3389/frobt.2015.00006

CrossRef Full Text | Google Scholar

Ott, C., Roa, M. A., and Hirzinger, G. (2011). Posture and Balance Control for Biped Robots Based on Contact Force Optimization. In 2011 11th IEEE-RAS International Conference on Humanoid Robots. IEEE, 26–33. doi:10.1109/humanoids.2011.6100882

CrossRef Full Text | Google Scholar

Pratt, J., Carff, J., Drakunov, S., and Goswami, A. (2006). Capture Point: A Step Toward Humanoid Push Recovery. In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots. 200–207. doi:10.1109/ichr.2006.321385

CrossRef Full Text | Google Scholar

Sang-Ho Hyon, S.-H., Hale, J. G., and Cheng, G. (2007). Full-Body Compliant Human-Humanoid Interaction: Balancing in the Presence of Unknown External Forces. IEEE Trans. Robot. 23, 884–898. doi:10.1109/tro.2007.904896

CrossRef Full Text | Google Scholar

Stephens, B. J., and Atkeson, C. G. (2010a). Dynamic Balance Force Control for Compliant Humanoid Robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. 1248–1255. doi:10.1109/iros.2010.5648837

CrossRef Full Text | Google Scholar

Stephens, B. J., and Atkeson, C. G. (2010b). Push Recovery by Stepping for Humanoid Robots with Force Controlled Joints. In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots. 52–59. doi:10.1109/ichr.2010.5686288

CrossRef Full Text | Google Scholar

Stewart, D. E. (2000). Rigid-body Dynamics with Friction and Impact. SIAM Rev. 42, 3–39. doi:10.1137/s0036144599360110

CrossRef Full Text | Google Scholar

Wang, Y., Xiong, R., Zhu, Q., and Chu, J. (2014). Compliance Control for Standing Maintenance of Humanoid Robots under Unknown External Disturbances. In Proceedings of the IEEE International Conference on Robotics and Automation. 2297–2304. doi:10.1109/icra.2014.6907177

CrossRef Full Text | Google Scholar

Wiedebach, G., Bertrand, S., Wu, T., Fiorio, L., McCrory, S., Griffin, R., et al. (2016). Walking on Partial Footholds Including Line Contacts with the Humanoid Robot Atlas. In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots. 1312–1319. doi:10.1109/humanoids.2016.7803439

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 Proceedings of the IEEE International Conference on Robotics and Automation. 4375–4382. doi:10.1109/icra.2018.8461172

CrossRef Full Text | Google Scholar

Appendix

Proof of Proposition 1

Since E is positive definite, E ₁, E ₂, E _1,s , and E _2,s are invertible where E _1,s and E _2,s are Schur Complements of E ₁ and E ₂, respectively. It is possible to express the partitioned matrices of G as follows:

$\mathbf{G}=\left[\begin{array}{cc}\hfill {\mathbf{G}}_1\hfill & \hfill {\mathbf{G}}_{12}\hfill \\ \hfill {\mathbf{G}}_{12}^{\top }\hfill & \hfill {\mathbf{G}}_2\hfill \end{array}\right]=\left[\begin{array}{cc}\hfill {\mathbf{E}}_{2,s}^{-1}\hfill & \hfill -{\mathbf{E}}_1^{-1}{\mathbf{E}}_{12}{\mathbf{E}}_{1,s}^{-1}\hfill \\ \hfill -{\mathbf{E}}_{1,s}^{-1}{\mathbf{E}}_{12}^{\top }{\mathbf{E}}_1^{-1}\hfill & \hfill {\mathbf{E}}_{1,s}^{-1}\hfill \end{array}\right](40)$

Then, we get the following formula by comparing the expressions of E with each Schur Complements:

$\begin{array}{cc}\hfill {\mathbf{E}}_2^{-1}& ={\mathbf{E}}_{1,s}^{-1}-{\mathbf{E}}_2^{-1}{\mathbf{E}}_{12}^{\top }{\mathbf{E}}_{2,s}^{-1}{\mathbf{E}}_{12}{\mathbf{E}}_2^{-1}\hfill \\ \hfill & ={\mathbf{G}}_2-{\mathbf{E}}_{1,s}^{-1}{\mathbf{E}}_{12}^{\top }{\mathbf{E}}_1^{-1}{\mathbf{E}}_{2,s}{\mathbf{G}}_{12}\hfill \\ \hfill & =\left(\mathcal{E}+{\mathbf{S}}_2\right)\mathbf{G}{\mathbf{S}}_2^{\top }\hfill \end{array}(41)$

Where $\mathcal{E}=\left[\begin{array}{cc}\hfill -{\mathbf{E}}_{1,s}^{-1}{\mathbf{E}}_{12}^{\top }{\mathbf{E}}_1^{-1}{\mathbf{E}}_{2,s}\hfill & \hfill \mathbf{0}\hfill \end{array}\right]\in {\mathbb{R}}^{n_2\times n}$ .