A Robust Disturbance Rejection Whole-Body Control Framework for Bipedal Robots Using a Momentum-Based Observer

Abstract

This paper presents a complete planner and controller scheme for bipedal robots, designed to enhance robustness against external disturbances. The high-level planner utilizes model predictive control (MPC) to optimize both the foothold location and step duration based on the divergent component of motion (DCM) to increase the robustness of generated gaits. For low-level control, we employ a momentum-based observer capable of estimating external forces acting on both stance and swing legs. The full-body dynamics, incorporating estimated disturbances, are integrated into a weighted whole-body control (WBC) to obtain more accurate ground reaction forces needed by the momentum-based observer. This approach eliminates the dependency on foot-mounted sensors for ground reaction force measurement, distinguishing our method from other disturbance estimation methods that rely on direct sensor measurements. Additionally, the controller incorporates trajectory compensation mechanisms to mitigate the effects of external disturbances. The effectiveness of the proposed framework is validated through comprehensive simulations and experimental evaluations conducted on BRUCE, a miniature bipedal robot developed by Westwood Robotics (Los Angeles, CA, USA). These tests include walking under swing leg disturbances, traversing uneven terrain, and simultaneously resisting upper-body pushes.

1. Introduction

Legged robots, owing to their exceptional adaptability to complex terrains, demonstrate significant potential for diverse practical applications. In recent years, legged robots like quadrupeds and bipeds have received considerable research attention. Quadrupedal robots, for instance, Boston Dynamics’ Spot and Unitree’s B2, have successfully been deployed in industrial environments. However, the practical implementation of bipedal robots remains limited. During real-world operations, bipedal robots are inevitably subjected to various external disturbances, including impulsive forces, environmental collisions, and dynamic load variations. These disturbances may lead to trajectory deviations and, in severe cases, loss of stability. As a result, the development of robust disturbance rejection capabilities is crucial for ensuring stable bipedal locomotion in practical applications.

To date, most model-based locomotion control strategies are implemented by solving an optimal control problem [1]. However, because the bipedal system is high-dimensional, hybrid, nonlinear, and highly constrained, solving this problem in real time remains computationally intractable. Consequently, most approaches break down the problem into multiple stages, employing either abstract models or full dynamic models at each stage. While this hierarchical approach sacrifices global optimality, it enables real-time implementation [1,2,3]. A widely adopted framework employs a two-level hierarchical structure, where the high-level plans the footsteps and the low level optimizes the contact forces [4,5,6].

The high-level planner aims to generate trajectories in real-time, usually adopting abstract models such as the linear inverted pendulum model (LIPM) [7,8]. Kajita et al. [9] developed a preview control approach to generate center of mass (CoM) trajectory. Their method solved a discrete-time, infinite-horizon linear quadratic regulator (LQR) problem based on LIPM. Wieber [10] enhanced the preview control by using a linear MPC scheme, which is capable of handling inequality constraints, including stability bounds and reachability limits. The MPC and LIPM frameworks can be extended to generate other trajectories, like footstep [11] and CoM height trajectories [12]. Further developments revealed that the LIPM dynamics can be divided into stable and unstable components. Therefore, the controller can only consider the unstable parts, known as DCM [13]. Additionally, researchers recognized the potential benefits of variable step durations. Khadiv et al. [14,15] demonstrated this through their work on simultaneous optimization of landing positions and step durations using the DCM model, showing that timing adjustment can enhance robustness.

For the low-level controller, there has been a convergence among many researchers working with torque-controlled bipeds, formulating the controller as a quadratic program (QP) [3,4,16,17]. This QP formulation optimizes joint torques, joint accelerations, contact wrenches, and angular momentum simultaneously, enabling the execution of multiple tasks while satisfying system dynamics and contact force constraints. The key advantage of this method is that it provides a strict hierarchy, which guarantees each tasks resolved in order. Depending on how the task hierarchy is managed, QP-based WBC can be classified into two main categories. The weighted WBC scheme sets all operational tasks as objectives of a single QP with priorities implicitly being enforced with weights [3]. The null space projection WBC method achieves a strict task hierarchy by projecting the lower priority tasks into the null space of higher priority tasks [18].

Robustness against external disturbances in legged robots has been extensively studied in recent years. Current approaches can be broadly categorized into two main types. The first category relies on state feedback to adjust control strategies, including joint-specific torque compensation [19,20], foot force planning [21,22], and support configuration adjustment through step-taking [23,24]. However, these methods lack precise external disturbance estimation; therefore, their effectiveness in resisting larger external disturbances is limited [25]. The second category employs momentum-based observer to estimate external disturbance. For example, Englsberger et al. [26] developed a momentum-based disturbance observer, enabling the TORO robot to withstand external disturbances equivalent to 15% of its body weight in simulations. Focchi et al. [27] further extended this algorithm to angular cases by estimating the complete wrench at CoM and implemented it on the HyQ quadruped robot. Nevertheless, these momentum-based methods typically require additional force sensors mounted on the robot’s feet, making them incompatible with many robotic platforms such as BRUCE.

With respect to the literature review depicted above, this paper aims to proposes a novel control framework that addresses two critical limitations in existing approaches: (1) the dependency on force sensors for external disturbance resistance and (2) the lack of integration between robust planning and control methods. The proposed framework comprises three key components. First, a high-level planner utilizes MPC to optimize both the foothold location and step duration based on the DCM to enhance the robustness of generated gaits. Second, a momentum-based observer estimates disturbances acting on both stance and swing legs. Third, a weighted WBC incorporates full-body dynamics with the estimated disturbances to compute more accurate ground reaction forces required for the observer. Moreover, WBC compensates for the estimated disturbance effects on each task.

Zhu et al. [25] also employed a disturbance observer to estimate external disturbance without requiring additional force sensors on the robot’s feet and performed online disturbance compensation within the WBC. However, their method only estimates the external disturbance on CoM, which is similar to estimating disturbances on the stance leg [28]. While estimating external wrenches acting solely on the CoM can improve locomotion robustness on uneven terrain, it fails to prevent the robot from falling after severe impact to the swing leg. Morlando et al. [28] estimated disturbances for both stance and swing legs and compensated for external forces acting on both legs. However, their method requires force sensors on the robot’s feet. In addition, we adopt a different planning method, which use MPC to plan both step locations and durations, further enhancing robustness.

This paper is organized as follows. Section 2 presents the mathematical derivation of the momentum-based observer. Section 3 introduces whole control scheme, including the footstep planner and weighted WBC method. Section 4 presents the simulation and experimental results of the control scheme applied on BRUCE. In the end, conclusions and discussion are provided in Section 5.

2. Momentum-Based Observer for Disturbance Estimation

In this section, the dynamics model of a biped robot is first presented for momentum-based observer design purposes. Afterward, the momentum-based observer, which estimates disturbances acting on both the stance and swing legs, is introduced.

2.1. Dynamic Model

To effectively demonstrate the impact of external forces perceived by the momentum-based observer, we establish a floating base dynamics model for bipedal robots. This model typically consists of a free-floating base with two legs, which effectively captures the influence of external forces on both the support leg and the swing leg. Let $\left\{C\right\}$ denote the frame attached to the robot’s CoM. The free-floating base is connected to the fixed world frame $\left\{W\right\}$ through six virtual joints, resulting in six degrees of freedom. Virtual joints, unlike the joints in the legs, do not physically exist and have no actual rotational axes. They are introduced to describe the position and posture of the robot’s upper body, allowing the joint values to serve as generalized coordinates for describing the robot’s upper body configuration. A frame $\left\{B\right\}$ is attached to the floating base to represent its position and orientation. Additionally, two legs are connected to the floating base, adding $2n$ degrees of freedom to the bipedal robot, where $n>0$ represents the number of joints in each leg. Figure 1 illustrates the biped robot and its corresponding coordinate frames.

Let ${\mathit{x}}_b={[x_b,y_b,z_b]}^T\in {\mathbb{R}}^3$, ${\dot{\mathit{x}}}_b,{\ddot{\mathit{x}}}_b\in {\mathbb{R}}^3$ denote the position, velocity, and acceleration of the origin of frame $\left\{B\right\}$ relative to the frame $\left\{W\right\}$, respectively. Similarly, ${\mathit{\omega }}_b,{\dot{\mathit{\omega }}}_b\in {\mathbb{R}}^3$ represent the angular velocity and angular acceleration of frame $\left\{B\right\}$ relative to frame $\left\{W\right\}$, respectively. Additionally, variables are defined to describe the position of CoM: ${\mathit{x}}_c={[x_c,y_c,z_c]}^T\in {\mathbb{R}}^3$, ${\dot{\mathit{x}}}_c,{\ddot{\mathit{x}}}_c\in {\mathbb{R}}^3$ denote the position, velocity, and acceleration of the origin of frame $\left\{C\right\}$ relative to $\left\{W\right\}$, respectively. ${\mathit{\omega }}_c,{\dot{\mathit{\omega }}}_c\in {\mathbb{R}}^3$ are the angular velocity and angular acceleration of frame $\left\{C\right\}$ relative to $\left\{W\right\}$, respectively. The orientation of frame $\left\{B\right\}$ relative to $\left\{W\right\}$ is described by the rotation matrix ${\mathit{R}}_b\in \mathrm{SO}\left(3\right)$, from which the ZYX Euler angles $\mathit{\varphi }\in {\mathbb{R}}^3$ can be derived.

When the floating base frame $\left\{B\right\}$’s coordinates and joint space coordinates are utilized as generalized coordinates, the dynamics of the robot can be expressed as follows:

$$\overline{\mathit{M}}\left(\begin{array}{c}{\ddot{\mathit{x}}}_b\\ {\dot{\mathit{\omega }}}_b\\ \ddot{\mathit{q}}\end{array}\right)+\overline{\mathit{C}}\left(\begin{array}{c}{\dot{\mathit{x}}}_b\\ {\mathit{\omega }}_b\\ \dot{\mathit{q}}\end{array}\right)+\overline{\mathit{G}}=\left(\begin{array}{c}\mathbf{0}\\ \mathbf{0}\\ {\mathit{\tau }}_{\mathrm{joint}}\end{array}\right)+{\mathit{\tau }}_{\mathrm{ee}}+{\mathit{\tau }}_{\mathrm{ext}}$$

(1)

where $\overline{\mathit{M}}$, $\overline{\mathit{C}}$, and $\overline{\mathit{G}}$ are the inertia matrix, Coriolis and centrifugal force matrix, and gravity matrix, respectively, all expressed in frame $\left\{B\right\}$. $\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}}\in {\mathbb{R}}^{2n}$ represent the joint positions, velocities, and accelerations of the legs, respectively. ${\mathit{\tau }}_{\mathrm{joint}}\in {\mathbb{R}}^{2n}$ denotes the joint torques of the legs, ${\mathit{\tau }}_{\mathrm{ee}}$ represents the generalized external forces due to ground reaction forces on the support leg, and ${\mathit{\tau }}_{\mathrm{ext}}$ represents the generalized external forces due to external disturbances.

For ${\mathit{\tau }}_{\mathrm{ee}}$, it can be expressed as follows:

$${\mathit{\tau }}_{\mathrm{ee}}=\sum _{i=1}^{n_{ee}}\left[\begin{array}{c}\left[\begin{array}{cc}\mathit{I}& \mathbf{0}\\ {\widehat{\mathit{x}}}_{bi}& \mathit{I}\end{array}\right]\\ {\overline{\mathit{J}}}_i^T\end{array}\right]{\mathit{F}}_i$$

(2)

where $n_{ee}$ is the number of contact end-effectors. The bipedal robot has two feet as contact end-effectors, so $n_{ee}=2$. ${\overline{\mathit{J}}}_i$ is the Jacobian matrix of the contact end-effector with dimensions ${\mathbb{R}}^{6\times (2n+6)}$. ${\mathit{x}}_{bi}$ is the offset of the end-effector frame ${\left\{T\right\}}_i$ relative to frame $\left\{B\right\}$, given by ${\mathit{x}}_{bi}={\mathit{x}}_i-{\mathit{x}}_b$. The hat operator can turn a vector $\mathit{a}\in {\mathbb{R}}^3$ into a $3\times 3$ skew symmetric matrix such that $\widehat{\mathit{a}}\mathit{b}=\mathit{a}\times \mathit{b}\forall \mathit{b}\in {\mathbb{R}}^3$. ${\mathit{F}}_i$ is the wrench applied to the contact end-effector.

According to [29], using the position and orientation of the CoM frame as generalized coordinates, as opposed to those of the base frame, simplifies the dynamics model of the floating base robot and facilitates the direct control of the CoM. The transformation between the generalized coordinates using the CoM frame’s coordinates and those using the floating base frame’s coordinates is given by the following:

$$\left(\begin{array}{c}{\dot{\mathit{x}}}_c\\ {\mathit{\omega }}_c\\ \dot{\mathit{q}}\end{array}\right)=\left[\begin{array}{ccc}\mathit{I}& -{\widehat{\mathit{x}}}_{bc}& {\mathit{J}}_{bc}\\ \mathbf{0}& \mathit{I}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathit{I}\end{array}\right]\left(\begin{array}{c}{\dot{\mathit{x}}}_b\\ {\mathit{\omega }}_b\\ \dot{\mathit{q}}\end{array}\right)=\overline{\mathit{T}}\left(\begin{array}{c}{\dot{\mathit{x}}}_b\\ {\mathit{\omega }}_b\\ \dot{\mathit{q}}\end{array}\right)$$

(3)

where ${\mathit{x}}_{bc}$ is the relative position deviation of the CoM with respect to the frame $\left\{B\right\}$ expressed in frame $\left\{W\right\}$, and ${\mathit{J}}_{bc}$ is the Jacobian matrix of the position deviation ${\mathit{x}}_{bc}$.

Thus, the dynamics equation expressed with the CoM frame’s coordinates, as in [30], is obtained as follows:

$$\underset{\mathit{M}}{\underbrace{\left[\begin{array}{ccc}m\mathit{I}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{M}}_{11}& {\mathit{M}}_{12}\\ \mathbf{0}& {\mathit{M}}_{21}& {\mathit{M}}_{22}\end{array}\right]}}\underset{\dot{\mathit{\nu }}}{\underbrace{\left(\begin{array}{c}{\ddot{\mathit{x}}}_c\\ {\dot{\mathit{\omega }}}_c\\ \ddot{\mathit{q}}\end{array}\right)}}+\underset{\mathit{C}}{\underbrace{\left[\begin{array}{c}{\mathit{C}}_1\\ {\mathit{C}}_2\\ {\mathit{C}}_3\end{array}\right]}}\underset{\mathit{\nu }}{\underbrace{\left(\begin{array}{c}{\dot{\mathit{x}}}_c\\ {\mathit{\omega }}_c\\ \dot{\mathit{q}}\end{array}\right)}}+\underset{\mathit{G}}{\underbrace{\left[\begin{array}{c}m\mathit{g}\\ \mathbf{0}\\ \mathbf{0}\end{array}\right]}}=\left(\begin{array}{c}\mathbf{0}\\ \mathbf{0}\\ {\mathit{\tau }}_{\mathrm{joint}}\end{array}\right)+{\overline{\mathit{T}}}^{-\mathrm{T}}({\mathit{\tau }}_{\mathrm{ee}}+{\mathit{\tau }}_{\mathrm{ext}})$$

(4)

The transformed inertia matrix and Coriolis matrix are given by $\mathit{M}={\overline{\mathit{T}}}^{-\mathrm{T}}\overline{\mathit{M}}{\overline{\mathit{T}}}^{-1}$ and $\mathit{C}={\overline{\mathit{T}}}^{-\mathrm{T}}\overline{\mathit{C}}{\overline{\mathit{T}}}^{-1}+{\overline{\mathit{T}}}^{-\mathrm{T}}\overline{\mathit{M}}{\dot{\overline{\mathit{T}}}}^{-1}$, respectively. The scalar m represents the total mass of the bipedal robot, and $\mathit{g}\in {\mathbb{R}}^3$ is the gravitational acceleration vector. The first row of the equation corresponds to Newton’s law of motion, which governs the translational dynamics of the system’s CoM. The second row describes the rotational dynamics of the CoM frame, establishing the relationship between its angular acceleration and external forces. The third row models the internal motion of the multi-body system, specifically formulating the dynamics of the leg joint accelerations $\ddot{\mathit{q}}$ [30]. When the leg inertia is small, ${\mathit{M}}_{12}$, ${\mathit{M}}_{21}$, ${\mathit{C}}_1$, and ${\mathit{C}}_2$ can be approximated as zero, meaning the Coriolis and centrifugal forces, due to the coupling between the leg motion and the CoM, can be neglected [28].

The simplified dynamics equation is expressed as follows:

$$\left[\begin{array}{cc}{\mathit{M}}_{\mathrm{com}}& {\mathbf{0}}_{6\times 2n}\\ {\mathbf{0}}_{2n\times 6}& {\mathit{M}}_q\end{array}\right]\left(\begin{array}{c}{\ddot{\mathit{x}}}_c\\ {\dot{\mathit{\omega }}}_c\\ \ddot{\mathit{q}}\end{array}\right)+\left[\begin{array}{c}\mathbf{0}\\ \mathbf{0}\\ {\mathit{C}}_q\end{array}\right]+\left[\begin{array}{c}m\mathit{g}\\ \mathbf{0}\\ \mathbf{0}\end{array}\right]={\mathit{S}}^T\mathit{\tau }+{\mathit{J}}_{\mathrm{st}}^T{\mathit{f}}_{\mathrm{gr}}+{\mathit{J}}_{\mathrm{ext}}^T{\mathit{f}}_{\mathrm{ext}}$$

(5)

where $\mathit{S}=\left[{\mathbf{0}}_{2n\times 6}{\mathit{I}}_{2n}\right]$ is the selection matrix for joint torques. ${\mathit{J}}_{\mathrm{st}}\left(\mathit{q}\right)=\left[{\mathit{J}}_{\mathrm{st},\mathrm{com}}\left(\mathit{q}\right){\mathit{J}}_{\mathrm{st},\mathrm{j}}\left(\mathit{q}\right)\right]\in {\mathbb{R}}^{(3\times n_c)\times (6+2n)}$ is the Jacobian matrix of the ground reaction force application points, mapping the ground reaction forces to the accelerations of the CoM and leg joints. Here, $0\le n_c\le 4$ is the number of contact points. For the BRUCE robot, each foot has two contact points (toe and heel), resulting in a maximum of four contact points. ${\mathit{f}}_{\mathrm{gr}}$ represents the ground reaction forces. ${\mathit{J}}_{\mathrm{ext}}\left(\mathit{q}\right)=\left[{\mathit{J}}_{\mathrm{ext},\mathrm{com}}\left(\mathit{q}\right){\mathit{J}}_{\mathrm{ext},\mathrm{j}}\left(\mathit{q}\right)\right]\in {\mathbb{R}}^{(3\times 4)\times (6+2n)}$ is the Jacobian matrix of the external disturbance application points, mapping the external disturbances to the accelerations of the CoM and leg joints. ${\mathit{f}}_{\mathrm{ext}}={[{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}^{\mathrm{T}}, {\mathit{f}}_{\mathrm{st},\mathrm{ext}}^{\mathrm{T}}]}^{\mathrm{T}}$ represents the external disturbances, including those acting on the support leg and swing leg. The last term can be expressed as ${\mathit{J}}_{\mathrm{ext}}^T{\mathit{f}}_{\mathrm{ext}}={\left[{\mathit{J}}_{\mathrm{sw}}^{\mathrm{T}}\left(\mathit{q}\right){\mathit{J}}_{\mathrm{st}}^{\mathrm{T}}\left(\mathit{q}\right)\right]}^{\mathrm{T}}{\mathit{f}}_{\mathrm{ext}}$.

2.2. Momentum-Based Observer Design

The proposed observer utilizes the momentum of both the support leg and the swing leg to estimate external disturbance on both legs. Our method is different from [27], which used only the momentum at the CoM to estimate external disturbances acting on the CoM. While estimating external wrenches acting only on the CoM can enhance locomotion robustness on uneven terrain, it fails to prevent the robot from falling after severe impact to the swing leg. Such impacts can cause the swing foot to deviate from its desired trajectory, potentially leading to missed footholds or even complete failure to touch the ground, resulting in a fall. Thus, our observer addresses a broader range of scenarios compared to methods focused solely on CoM disturbances.

During the dynamic modeling process, the dynamics equations (Equation (5)) of the robot’s CoM and leg joints have been decoupled, allowing the generalized momentum of the legs to be separately derived as follows:

$$\mathit{k}={\mathit{M}}_q\dot{\mathit{q}}$$

(6)

Based on the fundamental properties of the robot’s inertia matrix [31], we have the following:

$${\dot{\mathit{M}}}_q={\mathit{C}}_q^T+{\mathit{C}}_q$$

(7)

Differentiating Equation (6) and combining it with Equation (7), then substituting into Equation (5), yields the time derivative of the generalized momentum as follows:

$$\dot{\mathit{k}}={\mathit{C}}_q^T\dot{\mathit{q}}+\mathit{\tau }+{\mathit{J}}_{\mathrm{st},\mathrm{j}}^T{\mathit{f}}_{\mathrm{gr}}+{\mathit{J}}_{\mathrm{ext},\mathrm{j}}^T{\mathit{f}}_{\mathrm{ext}}$$

(8)

Let $\widehat{\mathit{f}}$ represent the estimate of ${\mathit{f}}_{\mathrm{ext}}$. Then, $\widehat{\mathit{F}}={\mathit{J}}_{\mathrm{ext},\mathrm{j}}^T\widehat{\mathit{f}}$ and ${\mathit{F}}_{\mathrm{ext}}={\mathit{J}}_{\mathrm{ext},\mathrm{j}}^T{\mathit{f}}_{\mathrm{ext}}$ denote the estimated and actual generalized joint torques due to external disturbances, respectively. The estimated generalized momentum is given by the following:

$$\widehat{\mathit{k}}\left(t\right)={\int }_0^t\underset{\dot{\widehat{\mathit{k}}}}{\underbrace{{\mathit{C}}_q^T\dot{\mathit{q}}+\mathit{\tau }+{\mathit{J}}_{\mathrm{st},\mathrm{j}}^T{\mathit{f}}_{\mathrm{gr}}+\widehat{\mathit{F}}\left(\sigma \right)}}d\sigma$$

(9)

Here, the initial estimated generalized momentum is assumed to be zero. Then, the estimated generalized external force is derived as follows:

$$\widehat{\mathit{F}}={\mathit{K}}_1\left(\mathit{k}\left(t\right)-{\mathit{\int }}_0^t\dot{\widehat{\mathit{k}}}\left(\sigma \right)d\sigma \right)$$

(10)

where ${\mathit{K}}_1$ is a positive definite diagonal matrix containing the gains. Combining Equations (8) and (10), the observer dynamics can be expressed as follows:

$$\dot{\widehat{\mathit{F}}}={\mathit{K}}_1\left({\mathit{F}}_{\mathrm{ext}}-\widehat{\mathit{F}}\right)$$

(11)

According to [32], a second-order dynamic model can better attenuate the effects of high-frequency noise (e.g., introduced by both the IMU sensor and the joint velocity measurements), thereby improving the observer’s estimation accuracy. To achieve more accurate external force estimation, we extended this first-order dynamic model to an arbitrary-order observer dynamics model through the following iterative equation:

$${\mathit{\gamma }}_i\left(t\right)={\mathit{K}}_i{\mathit{\int }}_0^t\left(-\widehat{\mathit{F}}\left(\sigma \right)+{\mathit{\gamma }}_{i-1}\left(\sigma \right)\right)d\sigma ,i=2,\dots ,r$$

(12)

Here, $\widehat{\mathit{F}}={\mathit{\gamma }}_r$, where $r>0$ is the order of the estimator. ${\mathit{\gamma }}_1\left(t\right)$ is obtained from Equation (10). When ${\mathit{F}}_{\mathrm{ext}}$, $\widehat{\mathit{F}}$, and ${\mathit{\gamma }}_i\left(t\right)$ satisfy the Laplace transform conditions, the Laplace-transformed forms of Equations (11) and (12) are expressed as follows:

$$\begin{array}{cc}\hfill s{\mathit{\gamma }}_1\left(s\right)& ={\mathit{K}}_1\left({\mathit{F}}_{\mathrm{ext}}\left(s\right)-\widehat{\mathit{F}}\left(s\right)\right)\hfill \\ \hfill s{\mathit{\gamma }}_i\left(s\right)& ={\mathit{K}}_i\left(-\widehat{\mathit{F}}\left(s\right)+{\mathit{\gamma }}_{i-1}\left(s\right)\right),i=2,\dots ,r\hfill \end{array}$$

(13)

where s is the complex variable in the Laplace domain. The yields are rearranged as follows:

$$\widehat{\mathit{F}}\left(s\right)={\mathit{\gamma }}_r\left(s\right)={\displaystyle \frac{{\prod }_{i=1}^r{\mathit{K}}_i}{{\sum }_{i=0}^r{s}^i{\prod }_{j=i+1}^r{\mathit{K}}_j}}{\mathit{F}}_{\mathrm{ext}}\left(s\right)$$

(14)

Here, matrix division refers to the element-wise division of corresponding diagonal elements. Thus, the estimated generalized external force is linearly related to the actual generalized external force in the Laplace domain, with the following transfer function:

$$\mathit{G}\left(s\right)={\displaystyle \frac{{\prod }_{i=1}^r{\mathit{K}}_i}{{\sum }_{i=0}^r{s}^i{\prod }_{j=i+1}^r{\mathit{K}}_j}}={\displaystyle \frac{{\mathit{K}}_1{\mathit{K}}_2\cdots {\mathit{K}}_r}{s^r+{\mathit{K}}_r{s}^{r-1}+\cdots +{\mathit{K}}_r\cdots {\mathit{K}}_{2}s+{\mathit{K}}_r\cdots {\mathit{K}}_1}}$$

(15)

where $\mathit{G}\left(s\right)\in {\mathbb{C}}^{2n\times 2n}$ is a diagonal matrix. The i-th diagonal element of $\mathit{G}\left(s\right)$ is expressed as follows:

$$G_i\left(s\right)={\displaystyle \frac{k_{1i}{k}_{2i}\cdots k_{ri}}{s^r+k_{ri}{s}^{r-1}+\cdots +k_{ri}\cdots k_{2i}s+k_{ri}\cdots k_{1i}}}$$

(16)

where $k_{ji},j=1\cdots r$, is the i-th diagonal element of matrix ${\mathit{K}}_j$. $k_{1i}{k}_{2i}\cdots k_{ri}>0$ represents the gain of the observer, and when ${\prod }_j^r{k}_{ji},j=1\cdots r$ are coefficients of a Hurwitz polynomial, the stability of the estimator is guaranteed. The selection of r involves a trade-off between ideal estimation error and computational time, as a higher value of r results in increased computation time. The selection of ${\mathit{K}}_i$ is determined by two factors: ensuring the stability of Equation (16) and optimizing performance metrics such as overshoot, rise time, observation error, and other effects.

In practical implementation, the integrals in Equations (10) and (12) are discretized to enable real-time updates of $\widehat{\mathit{F}}$. Based on the estimated generalized external force, the estimated external force acting on the feet can be derived as follows:

$$\widehat{\mathit{f}}={\mathit{J}}_{\mathrm{ext},\mathrm{j}}^{T^{†}}\widehat{\mathit{F}}$$

(17)

where ${\mathit{J}}_{\mathrm{ext},\mathrm{j}}^{{\mathrm{T}}^{†}}$ is the pseudo-inverse of ${\mathit{J}}_{\mathrm{ext},\mathrm{j}}^{\mathrm{T}}$.

3. Control Framework

This section introduces the robust disturbance rejection control framework for bipedal robots. The control framework consists of an high-level planner and a lower-level controller. The high-level planner utilizes the planner in our previous work [33]. The lower-level controller employs a weighted WBC scheme, which derives ground reaction forces for the momentum-based observer and compensates for the estimated disturbance effects on each task. The overall control scheme proposed in this paper is illustrated in Figure 2.

In our previous work [33], we proposed a balance and walking control scheme for bipedal robots, which utilized the same high-level planner as in this study but a different low-level controller. The low-level controller in the previous work employed a method that did not require solving inverse dynamics, enabling high control frequencies. This approach eliminated the dependency on a precise dynamic model and demonstrated rapid push recovery capabilities. In contrast, the current study differs in two key aspects: First, the focus has shifted to disturbance rejection, whereas the previous work primarily addressed basic standing and walking balance without considering disturbance rejection. Second, the low-level controller in this study adopts a whole-body control approach, which relies on a precise dynamic model and incorporates a momentum-based observer to enhance disturbance rejection performance. In comparison, the previous method did not utilize the robot’s precise dynamic model.

3.1. Footstep Planner

The high-level planner utilizes the MPC approach, employing a DCM-based LIPM for dynamics. The DCM-based LIPM dynamics simplify the original linear inverted pendulum model by introducing the divergent component of motion variable. Specifically, while the original LIPM dynamics are described by a second-order differential equation, the DCM-based formulation reduces them to two first-order differential equations. This simplification reveals that the DCM variable is inherently divergent in this model, necessitating its control to ensure system stability. In our paper, the planner simultaneously determines both the footstep location and the step duration to enhance walking robustness.

The equations of motion for the LIPM are given by the following:

$$\ddot{\mathit{x}}={\omega }^2(\mathit{x}-\mathit{p})$$

(18)

where $\mathit{x}$ and $\ddot{\mathit{x}}$ are 2D vectors representing the horizontal position and acceleration of the CoM, respectively. The parameter $\omega =\sqrt{{\displaystyle \frac{g}{z_{\mathrm{com}}}}}$ is the reciprocal of the time constant of the LIPM, where g denotes gravitational acceleration and $z_{\mathrm{com}}$ is the fixed vertical position of the CoM. The variable $\mathit{p}$ represents the horizontal position of the center of pressure (CoP), which is assumed to remain on the ground (i.e., its height is zero).

The DCM is defined as a linear combination of the CoM position and velocity:

$$\mathit{\xi }:=\mathit{x}+{\displaystyle \frac{\dot{\mathit{x}}}{\omega }}$$

(19)

By introducing DCM, the LIPM dynamics can be rewritten as two first-order linear differential equations:

$$\begin{array}{ccc}\dot{\mathit{x}}& =& \omega (\mathit{\xi }-\mathit{x})\\ \dot{\mathit{\xi }}& =& \omega (\mathit{\xi }-\mathit{p})\end{array}$$

(20)

The first equation in Equation (20) represents the stable component of the LIPM, where $\mathit{x}$ converges to $\mathit{\xi }$. The second equation describes the unstable component, where $\mathit{\xi }$ diverges along the line between $\mathit{\xi }$ and $\mathit{p}$. Therefore, stabilizing $\mathit{\xi }$ is necessary for achieving stable walking. Since this equation is relatively simple, we can derive its analytical solution:

$$\mathit{\xi }\left(t\right)={\mathit{p}}_0+({\mathit{\xi }}_0-{\mathit{p}}_0)e^{\omega t}$$

(21)

where ${\mathit{\xi }}_0$ and $\mathit{\xi }\left(t\right)$ represent the DCM position at initial and at time t, respectively. Here, ${\mathit{p}}_0$ is assumed to be fixed during this period. With this solution, we can predict the future evolution of the DCM over multiple steps, which is essential for the MPC method.

The DCM offset is then defined as $\mathit{b}=\mathit{\xi }-\mathit{p}$, which represents the direction of the DCM velocity. The periodicity of the gait requires this offset to remain constant at the start and end of each step:

$$\mathit{b}={\mathit{\xi }}_0-{\mathit{p}}_0={\mathit{\xi }}_T-{\mathit{p}}_T$$

(22)

where T denotes the step duration.

Following the method introduced in [14], we can specify nominal step values, including nominal step length ($L_{nom}$), width ($W_{nom}$), duration ($T_{nom}$), and DCM offset (${\mathit{b}}_{nom}={[b_{x,nom},b_{y,nom}]}^T$), which are related to desired average walking velocity. To eliminate the nonlinearity introduced by the step duration, we introduce a new decision variable $\eta =e^{\omega T}$ instead of directly using T.

The MPC method is employed to consider the changes in system variables over a future horizon, enabling the optimization of control effects over multiple steps. In this paper, our primary goal is to stabilize the DCM for the next few steps. Therefore, the cost function includes the error between the DCM offset and the nominal DCM offset at each step, the error between the step length and the nominal step length at each step, and the error between the step duration and the nominal step duration. Additionally, the solution must satisfy the DCM dynamics, including both intra-step and inter-step DCM dynamics, the kinematic constraints of the step length, and the constraints on the step duration. Ultimately, this forms a QP problem to solve for the optimal footstep positions for the next few steps and the optimal footstep timing for the current step. The decision variables of the QP problem include the footstep location, DCM offset of the next few steps, and the current footstep duration. Here, we set the foothold location as the CoP position:

$$\begin{array}{cccc}\hfill & \underset{{\mathit{p}}_k,{\mathit{b}}_k,\eta }{\min }\hfill & \hfill & \sum _{k=1}^{N_s}\parallel {\mathit{b}}_k-\mathit{R}{\mathit{b}}_{nom}{\parallel }_{{\mathit{W}}_b}^2+\parallel {\mathit{l}}_k-\mathit{R}{\mathit{l}}_{nom}{\parallel }_{{\mathit{W}}_l}^2+w_{\eta }{(\eta -e^{\omega T_{nom}})}^2\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & {\mathit{\xi }}_1={\mathit{p}}_0+{\mathit{b}}_0{e}^{-\omega t_0}\eta \hfill \\ \hfill & \hfill & & {\mathit{\xi }}_k={\mathit{p}}_{k-1}+{\mathit{b}}_{k-1}{e}^{\omega T_{nom}},k=2,\dots ,N_s\hfill \\ \hfill & \hfill & & {\mathit{l}}_{min}\le {\mathit{R}}^T{\mathit{l}}_k\le {\mathit{l}}_{max},k=1,\dots ,N_s\hfill \\ \hfill & \hfill & & e^{\omega T_{min}}\le \eta \le e^{\omega T_{max}}\hfill \end{array}$$

(23)

where ${\mathit{l}}_{nom}={[L_{nom},W_{nom}]}^T$ is the nominal step length in both the sagittal and coronal planes. The matrix $\mathit{R}\in SO\left(2\right)$ accounts for foot yaw rotation. The parameter $t_0$ represents the elapsed time in the current step. The horizon length is denoted as $N_s$. ${\mathit{l}}_{min},{\mathit{l}}_{max}$ define the kinematic limits based on the robot’s range of motion. The parameters $T_{min}$ and $T_{max}$ limit the step duration and need to be tuned empirically. The matrices ${\mathit{W}}_b,{\mathit{W}}_l$, and scalar weight $w_{\eta }$ are positive definite and serve as weighting factors for the DCM offset, step length, and step duration deviations in the cost function, respectively.

The main parameters of the MPC problem include the prediction horizon, the weights of the different components of the cost function, and the minimum and maximum step duration. Through extensive testing, we found that as the prediction horizon increased from one step to three steps, the walking stability improved. However, increasing it further to five steps did not yield additional benefits. At five steps, the computation time approaches the set frequency of 500 Hz. For the cost function weights, the DCM offset is prioritized with the largest weight, as it is the most critical for stability. Next is the nominal gait, and the smallest weight is assigned to the step duration. The setting of the maximum and minimum step duration is crucial for the walking performance of the BRUCE robot. Since the BRUCE robot is small (0.3 m in CoM height), as shown in Equation (21), its DCM diverges relatively quickly, requiring a shorter step duration. Thus, the maximum step duration is set to be smaller. At the same time, an excessively small step duration would cause the swing leg acceleration to become too large, which the robot’s motors cannot handle. Therefore, the maximum and minimum step durations are carefully adjusted based on the robustness of the bipedal walking performance.

After computing the optimal footstep location ${\mathit{p}}^{*}$ and timing $T^{*}$ through the above QP problem, the swing foot trajectory must be carefully designed to accommodate changing footstep location and timing. To ensure continuity up to the acceleration level, we employ a fifth-order polynomial as the reference trajectory:

$${\mathit{p}}^{ref}\left(t\right)=\sum _{i=0}^5{c}_i{t}^i,t_0\le t\le T^{*}$$

(24)

where $t_0$ represents the current time, measured from the beginning of the step.

3.2. Disturbance Rejection Whole-Body Control

This section employs the weighted WBC method from the literature [3], combined with the momentum-based observer from the previous section, to enhance its disturbance rejection capabilities. The weighted WBC does not require strict prioritization but instead allocates the importance of different tasks through weights in the cost function. By constructing a QP problem, the desired ground reaction forces and joint accelerations are solved. Subsequently, the feedforward torque is obtained through the whole-body dynamics, combined with joint-level Proportional and Derivative (PD) control to yield the final joint torques. Unlike other WBC methods based on full-body dynamics that do not consider disturbances, our approach integrates the full-body dynamics with estimated disturbances to compute more accurate ground reaction forces required for the observer, eliminating the dependency on foot-mounted sensors for ground reaction force measurement, which is a limitation of many momentum-based observer methods.

Recall that the external force estimated by the observer in previous section is $\widehat{\mathit{f}}$, which can be divided into ${\widehat{\mathit{f}}}_{\mathrm{st}}$ (estimated external force on the stance leg) and ${\widehat{\mathit{f}}}_{\mathrm{sw}}$ (estimated external force on the swing leg). The external force on the stance leg primarily affects the control performance of the CoM, while the external force on the swing leg mainly impacts the tracking performance of the swing leg trajectory. Therefore, this paper incorporates the estimated external forces into the affected tasks for compensation, thereby achieving better task tracking performance under external disturbances.

The decision variables selected for the QP optimization in the whole-body control are $\mathit{\zeta }={\left[{\ddot{\mathit{r}}}_c^T{\ddot{\mathit{q}}}^T{\mathit{f}}_{\mathrm{gr}}^T\right]}^T$, where ${\ddot{\mathit{r}}}_c={\left[{\ddot{\mathit{x}}}_c^T{\dot{\mathit{\omega }}}_c^T\right]}^T$. The cost function involves a weighted combination of multiple tracking task objectives. The main tasks are as follows:

3.2.1. CoM Trajectory Tracking Task

First, the WBC method needs to track the planned CoM trajectory, which is derived from user input. When the desired CoM state, read from the user input, is ${\mathit{r}}_{\mathrm{c},\mathrm{ref}}$, ${\dot{\mathit{r}}}_{\mathrm{c},\mathrm{ref}}$, and ${\ddot{\mathit{r}}}_{\mathrm{c},\mathrm{ref}}$, the desired wrench at the CoM, ${\mathit{w}}_{\mathrm{com},\mathrm{des}}\in {\mathbb{R}}^6$, can be computed using feedback and feedforward terms:

$${\mathit{w}}_{\mathrm{com},\mathrm{des}}={\mathit{K}}_{\mathrm{p},\mathrm{com}}({\mathit{r}}_{\mathrm{c},\mathrm{ref}}-{\mathit{r}}_c)+{\mathit{K}}_{\mathrm{d},\mathrm{com}}({\dot{\mathit{r}}}_{\mathrm{c},\mathrm{ref}}-{\dot{\mathit{r}}}_c)+m\mathit{g}+{\mathit{M}}_{\mathrm{com}}\left(\mathit{q}\right){\ddot{\mathit{r}}}_{\mathrm{c},\mathrm{ref}}$$

(25)

where ${\mathit{K}}_{\mathrm{p},\mathrm{com}},{\mathit{K}}_{\mathrm{d},\mathrm{com}}\in {\mathbb{R}}^{6\times 6}$ are the diagonal matrices containing PD gains for the CoM tracking task.

To achieve the desired wrench on CoM, we consider the first six rows of the whole-body dynamics equation (Equation (5)), which represent the dynamics of the CoM:

$${\mathit{w}}_{\mathrm{com}}={\mathit{M}}_{\mathrm{com}}\left(\mathit{q}\right)\left(\begin{array}{c}{\ddot{\mathit{x}}}_c\\ {\dot{\mathit{\omega }}}_c\end{array}\right)+\left[\begin{array}{c}m\mathit{g}\\ \mathbf{0}\end{array}\right]={\mathit{J}}_{\mathrm{st},\mathrm{com}}^T\left(\mathit{q}\right){\mathit{f}}_{\mathrm{gr}}+{\mathit{J}}_{\mathrm{ext},\mathrm{com}}^T\left(\mathit{q}\right){\mathit{f}}_{\mathrm{ext}}$$

(26)

where ${\mathit{w}}_{\mathrm{com}}\in {\mathbb{R}}^6$ is the wrench at the robot’s CoM, including inertial and gravitational terms. From this equation, it is evident that the external force on the stance leg affects the control performance of the CoM trajectory. Therefore, the estimated external force on the support leg is incorporated into Equation (26), and the ground reaction force need to satisfy this equation. Thus, the cost function for the CoM tracking task is defined as follows:

$${\mathcal{L}}_{\mathrm{com}}={∥{\mathit{J}}_{\mathrm{st},\mathrm{com}}{\mathit{f}}_{\mathrm{gr}}+{\mathit{J}}_{\mathrm{st},\mathrm{com}}{\widehat{\mathit{f}}}_{\mathrm{st}}-{\mathit{w}}_{\mathrm{com},\mathrm{des}}∥}_{{\mathit{W}}_{\mathrm{com}}}^2$$

(27)

where ${\mathit{W}}_{\mathrm{com}}$ is the weight matrix for the CoM trajectory tracking task, which can adjust the weights of the six degrees of freedom of the CoM to prioritize tracking in specific directions.

3.2.2. Swing Foot Trajectory Tracking Task

Second, the WBC method need to track the planner swing foot trajectory to achieve stable walking. Given the planned swing foot position, velocity, and acceleration from the footstep planner, ${\mathit{p}}^{ref}$, ${\dot{\mathit{p}}}^{ref}$, and ${\ddot{\mathit{p}}}^{ref}$, respectively, the desired swing foot acceleration ${\ddot{\mathit{p}}}^{cmd}$ without considering external disturbances can be calculated using feedforward and feedback terms:

$${\ddot{\mathit{p}}}^{\mathrm{cmd}}={\ddot{\mathit{p}}}^{\mathrm{ref}}+{\mathit{K}}_{p,\mathrm{sw}}({\mathit{p}}^{\mathrm{ref}}-{\mathit{x}}_{\mathrm{sw}})+{\mathit{K}}_{d,\mathrm{sw}}({\dot{\mathit{p}}}_{\mathrm{ref}}-{\dot{\mathit{x}}}_{\mathrm{sw}})$$

(28)

where ${\mathit{K}}_{p,\mathrm{sw}},{\mathit{K}}_{d,\mathrm{sw}}$ are the PD parameter matrices for the swing foot. ${\mathit{x}}_{\mathrm{sw}},{\dot{\mathit{x}}}_{\mathrm{sw}},$ denote swing foot position and velocity, respectively.

However, external disturbances on the swing leg affect the swing foot trajectory tracking performance. Therefore, the estimated external force on the swing leg ${\widehat{\mathit{f}}}_{\mathrm{sw}}$ is used to compensate for the disturbance. To achieve this, the relationship between the swing foot acceleration and the external force must be established. Considering the whole-body dynamics, the orthogonal null space projector $\mathit{P}\in {\mathbb{R}}^{6+2n\times 6+2n}$, which satisfies $\mathit{P}{\mathit{J}}_{\mathrm{st}}^T=0$ and ${\mathit{P}}^2={\mathit{P}}^T=\mathit{P}$, is used. The projection matrix $\mathit{P}$ eliminates the contact constraint-related term ${\mathit{J}}_{\mathrm{st}}^T{\mathit{f}}_{\mathrm{gr}}$ in the dynamics equation [34]. Following the method in [35,36], $\mathit{P}$ is selected as $\mathit{P}={\mathit{I}}_{6+2n}-{\mathit{J}}_{\mathrm{st}}^{+}{\mathit{J}}_{\mathrm{st}}$. Multiplying both sides of Equation (5) by $\mathit{P}$ yields the following:

$$\mathit{P}\left(\mathit{M}\dot{\mathit{v}}+\mathit{h}\right)=\mathit{P}{\mathit{S}}^{\mathrm{T}}\mathit{\tau }+\mathit{P}{\left[\begin{array}{c}{\mathit{J}}_{\mathrm{sw}}\\ {\mathit{J}}_{\mathrm{st}}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}\\ {\mathit{f}}_{\mathrm{st},\mathrm{ext}}\end{array}\right]=\mathit{P}{\mathit{S}}^{\mathrm{T}}\mathit{\tau }+\mathit{P}{\mathit{J}}_{\mathrm{sw}}^{\mathrm{T}}{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}$$

(29)

where $\mathit{h}$ is the sum of Coriolis and gravity force. Since $\mathit{P}{\mathit{J}}_{\mathrm{st}}^T=0$, the only term related to external forces is $\mathit{P}{\mathit{J}}_{\mathrm{sw}}^T{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}$, which is associated with the external force on the swing leg. Following [36], the equation can be transformed into the following:

$${\mathit{M}}_c\dot{\mathit{v}}+\mathit{P}\mathit{h}-\mathit{C}\mathit{v}=\mathit{P}{\mathit{S}}^{\mathrm{T}}\mathit{\tau }+\mathit{P}{\mathit{J}}_{\mathrm{sw}}^{\mathrm{T}}{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}$$

(30)

where ${\mathit{M}}_c=\mathit{P}\mathit{M}+{\mathit{I}}_{6+2n}-\mathit{P}$ and $\mathit{C}=-{\mathit{J}}_{\mathrm{st}}^{+}{\mathit{J}}_{\mathrm{st}}$. As long as $\mathit{M}$ is invertible, ${\mathit{M}}_c$ is also invertible.

Based on the floating base kinematics, the relationship between the generalized coordinates and the swing foot’s velocity and acceleration is as follows:

$$\begin{array}{cc}\hfill {\dot{\mathit{x}}}_{\mathrm{sw}}& ={\mathit{J}}_{\mathrm{sw}}\mathit{v}\hfill \\ \hfill {\ddot{\mathit{x}}}_{\mathrm{sw}}& ={\mathit{J}}_{\mathrm{sw}}\dot{\mathit{v}}+{\dot{\mathit{J}}}_{\mathrm{sw}}\mathit{v}\hfill \end{array}$$

(31)

Multiplying both sides of Equation (30) by ${\mathit{M}}_c^{-1}$ yields the relationship for $\dot{\mathit{v}}$, which is substituted into Equation (31) to obtain the relationship between the swing foot acceleration and the external force:

$${\ddot{\mathit{x}}}_{\mathrm{sw}}={\mathit{J}}_{\mathrm{sw}}{\mathit{M}}_c^{-1}\mathit{P}{\mathit{S}}^T\mathit{\tau }+{\mathit{J}}_{\mathrm{sw}}{\mathit{M}}_c^{-1}\mathit{P}{\mathit{J}}_{\mathrm{sw}}^T{\mathit{f}}_{\mathrm{sw},\mathrm{ext}}+{\dot{\mathit{J}}}_{\mathrm{sw}}\mathit{v}-{\mathit{J}}_{\mathrm{sw}}{\mathit{M}}_c^{-1}(\mathit{P}\mathit{h}-\mathit{C}\mathit{v})$$

(32)

To mitigate the disturbance on swing leg, we can compensate for the effect of the external force on the swing foot acceleration, i.e., the second term in the equation above. Therefore, the desired swing foot acceleration after compensation becomes the following:

$${\ddot{\mathit{p}}}^{\mathrm{des}}={\ddot{\mathit{p}}}^{\mathrm{cmd}}-{\mathit{J}}_{\mathrm{sw}}{\mathit{M}}_c^{-1}\mathit{P}{\mathit{J}}_{\mathrm{sw}}^T{\widehat{\mathit{f}}}_{\mathrm{sw}}$$

(33)

The desired swing foot acceleration is realized through the kinematic relationship in Equation (31), resulting in the cost function for the swing foot trajectory tracking task with disturbance compensation:

$${\mathcal{L}}_{\mathrm{sw}}={∥{\mathit{J}}_{\mathrm{sw}}\dot{\mathit{v}}+{\dot{\mathit{J}}}_{\mathrm{sw}}\mathit{v}-{\ddot{\mathit{p}}}^{\mathrm{des}}∥}_{{\mathit{W}}_{sw}}^2$$

(34)

where ${\mathit{W}}_{sw}$ is the weight matrix for the swing foot trajectory tracking task.

3.2.3. Stance Foot Contact Unmoving Task

This task ensures that the stance foot remains fixed at its contact point with the ground, preventing any relative motion such as slipping or lifting. To achieve this, we enforce a zero relative velocity constraint at the contact point, ensuring that it remains stationary with respect to the ground. This condition is formulated similarly to the kinematic equation for the swing foot position (31), leading to the following:

$$\begin{array}{cc}\hfill {\mathbf{0}}_{3n_c}& ={\mathit{J}}_{\mathrm{st},\mathrm{com}}\left(\mathit{q}\right){\dot{\mathit{r}}}_c+{\mathit{J}}_{\mathrm{st},\mathrm{j}}\left(\mathit{q}\right)\dot{\mathit{q}}\hfill \\ \hfill {\mathbf{0}}_{3n_c}& ={\mathit{J}}_{\mathrm{st},\mathrm{com}}\left(\mathit{q}\right){\ddot{\mathit{r}}}_c+{\dot{\mathit{J}}}_{\mathrm{st},\mathrm{com}}(\mathit{q},\dot{\mathit{q}}){\dot{\mathit{r}}}_c+{\mathit{J}}_{\mathrm{st},\mathrm{j}}\left(\mathit{q}\right)\ddot{\mathit{q}}+{\dot{\mathit{J}}}_{\mathrm{st},\mathrm{j}}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}\hfill \end{array}$$

(35)

Based on the above equation, the cost function for the stance foot contact unmoving task is as follows:

$${\mathcal{L}}_{\mathrm{st}}={∥{\mathit{J}}_{\mathrm{st},\mathrm{com}}\left(\mathit{q}\right){\ddot{\mathit{r}}}_c+{\dot{\mathit{J}}}_{\mathrm{st},\mathrm{com}}(\mathit{q},\dot{\mathit{q}}){\dot{\mathit{r}}}_c+{\mathit{J}}_{\mathrm{st},\mathrm{j}}\left(\mathit{q}\right)\ddot{\mathit{q}}+{\dot{\mathit{J}}}_{\mathrm{st},\mathrm{j}}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}-\mathbf{0}∥}_{{\mathit{W}}_{\mathrm{st}}}^2$$

(36)

where ${\mathit{W}}_{st}$ is the weight matrix for the stance foot contact unmoving task. Maintaining contact point unmoving is typically a constraint, but in this paper, it is treated as a task-space objective, i.e., a soft constraint. This approach often speeds up the QP and provides better numerical stability [37]. With sufficient task weights, the effect of null space projection can be achieved, ensuring that other tasks comply with the non-slip contact condition.

In addition to the task-related cost functions, the optimization also minimizes the generalized coordinate acceleration and ground contact force, both of which are included in the decision variables. The corresponding cost function is ${\parallel \mathit{\zeta }\parallel }_{\mathit{R}}^2$.

3.2.4. Constraints

For the constraint equations, the first is the motion equation, which restricts the motion to satisfy the first six rows of the floating base dynamics equation (Equation (5)):

$$[{\mathit{M}}_{\mathrm{com}}\left(\mathit{q}\right){\mathbf{0}}_{6\times 2n}-{\mathit{J}}_{\mathrm{st},\mathrm{com}}^T\left(\mathit{q}\right)]\mathit{\zeta }=\left[\begin{array}{c}-m\mathit{g}\\ \mathbf{0}\end{array}\right]+{\mathit{J}}_{\mathrm{ext},\mathrm{com}}^T\left(\mathit{q}\right){\mathit{f}}_{\mathrm{ext}}$$

(37)

The second constraint ensures that the ground reaction forces satisfy the friction cone constraints to avoid slipping. To avoid the nonlinearity of this constraint, the friction cone is approximated as a square pyramid, resulting in linear constraints in the optimization problem. The ground reaction force ${\mathit{f}}_{\mathrm{gr},i}\in {\mathbb{R}}^3$, $i=1,\dots ,n_{\mathrm{c}}$, satisfies the contact constraint with friction coefficient $\mu$ as follows:

$$\underset{\mathit{U}}{\underbrace{\left[\begin{array}{ccc}\pm 1& 0& -\mu \\ 0& \pm 1& -\mu \\ 0& 0& -1\\ 0& 0& 1\end{array}\right]}}{\mathit{f}}_{\mathrm{gr},i}\le \underset{\mathit{b}}{\underbrace{\left[\begin{array}{c}0\\ 0\\ -f_{z,min}\\ f_{z,max}\end{array}\right]}}$$

(38)

where $f_{z,min},f_{z,max}$ are the minimum and maximum normal forces to constrain the magnitude of the contact force. Note that a positive minimum value prevents contact detachment.

The final QP formulation of our disturbance rejection WBC method is as follows:

$$\begin{array}{cc}\hfill \underset{\mathit{\zeta }=\left[{\ddot{\mathit{r}}}_c\ddot{\mathit{q}}{\mathit{f}}_{\mathrm{gr}}\right]}{\mathrm{minimize}}& {\mathcal{L}}_{\mathrm{com}}+{\mathcal{L}}_{\mathrm{sw}}+{\mathcal{L}}_{\mathrm{st}}+{\parallel \mathit{\zeta }\parallel }_{\mathit{R}}^2\hfill \\ \hfill \mathrm{subject}\mathrm{to}& [{\mathit{M}}_{\mathrm{com}}\left(\mathit{q}\right){\mathbf{0}}_{6\times 2n}-{\mathit{J}}_{\mathrm{st},\mathrm{com}}^T\left(\mathit{q}\right)]\mathit{\zeta }=\left[\begin{array}{c}-m\mathit{g}\\ \mathbf{0}\end{array}\right]+{\mathit{J}}_{\mathrm{ext},\mathrm{com}}^T\left(\mathit{q}\right){\mathit{f}}_{\mathrm{ext}}\hfill \\ \hfill & \mathit{U}{\mathit{f}}_{\mathrm{gr},i}\le \mathit{b}i=1,\dots ,n_{\mathrm{c}}\hfill \end{array}$$

(39)

The main parameters of our WBC method include the PD parameters for the CoM trajectory and swing leg trajectory tracking tasks, the weight matrix for each task and the minimum and maximum normal forces. For the PD gains, the tuning process relies on trial and error, guided by tracking performance metrics such as the CoM velocity tracking error and swing leg tracking error. For the weight matrix, the highest weight is assigned to the stance foot contact unmoving task, as it serves as a hard constraint. The CoM task is assigned the next highest weight, with maintaining the CoM height being the most critical among its directional components. The lowest weight is assigned to the swing foot trajectory tracking task. For the minimum and maximum normal forces, a small minimum normal force is set to prevent contact detachment. The maximum normal force is chosen to avoid generating ground reaction forces that exceed the motor’s capabilities.

After obtaining the optimal ${\mathit{\zeta }}^{*}=\left[{\ddot{\mathit{r}}}_c^{*}{\ddot{\mathit{q}}}^{*}{\mathit{f}}_{\mathrm{gr}}^{*}\right]$, the feedforward torque ${\mathit{\tau }}_{\mathrm{ff}}$ is obtained by substituting into the second row of the Equation (5):

$${\mathit{\tau }}_{\mathrm{ff}}={\mathit{M}}_q{\ddot{\mathit{q}}}^{*}+\mathit{C}(\mathit{q},\mathit{v})\dot{\mathit{q}}-{\mathit{J}}_{st,j}^T\left(\mathit{q}\right){\mathit{f}}_{\mathrm{gr}}^{*}-{\mathit{J}}_{\mathrm{ext},\mathrm{j}}^T{\mathit{f}}_{\mathrm{ext}}$$

(40)

The joint torque command is obtained by adding feedforward and joint position feedback ${\mathit{\tau }}_{\mathrm{fd}}$ together as follows:

$${\mathit{\tau }}_{cmd}={\mathit{\tau }}_{\mathrm{ff}}+{\mathit{k}}_p({\mathit{q}}^{\mathrm{des}}-\mathit{q})+{\mathit{k}}_d({\dot{\mathit{q}}}^{\mathrm{des}}-\dot{\mathit{q}}),{\mathit{\tau }}_{\min }\le {\mathit{\tau }}_{\mathrm{cmd}}\le {\mathit{\tau }}_{\max }$$

(41)

where ${\mathit{k}}_p,{\mathit{k}}_d$ are positive-definite diagonal matrices containing PD feedback gains. ${\mathit{q}}^{\mathrm{des}},{\dot{\mathit{q}}}^{\mathrm{des}}$ are the desired joint position and velocity obtained from inverse kinematics. ${\mathit{\tau }}_{\min },{\mathit{\tau }}_{\max }$ are the minimum and maximum joint torques, respectively.

4. Simulations and Experiments

In this section, a series of simulations and real-world experiments were conducted to validate the effectiveness of our controller. Both the simulations using MuJoCo (Version 3.1.4) [38] and experiments were tested with a miniature bipedal robot, BRUCE. Each leg of BRUCE has five degrees of freedom, including a spherical hip joint, a knee joint, and an ankle joint. The robot’s state estimation is supported by an onboard IMU, joint encoders, and contact-sensing feet. A mini PC with an Intel Core i5-7260U Dual-Core CPU at 2.2 GHz was utilized as the onboard computing resource.

The 3D model of the BRUCE robot employed in our simulations is based on the model made available by the official developers [39], with the necessary sensors added to the original model files. The QP problems in both the footstep planner and the controller were solved using the OSQP solver (Version 0.6.5) [40]. With the onboard PC and OSQP, both our control scheme and momentum-based observer operate at an update frequency of 500 Hz.

The software architecture is designed to operate in a multithreaded environment, enabling the simultaneous execution of all modules in the control block diagram depicted in Figure 2. The architecture comprises six primary threads: a motor command thread operating at 1000 Hz, a state estimation thread at 500 Hz, a user input thread at 100 Hz, a momentum-based observer thread at 500 Hz, a high-level planner thread at 500 Hz, and a low-level whole-body control thread at 500 Hz. Data communication utilizes a shared memory based on Python module posix_ipc [41]. The program is developed in Python 3.10, with Numba [42] employed for pre-compilation to optimize the computational speed of state estimation, kinematics, and dynamics calculations. The data curves are generated by recording the necessary data using variables during the software execution, saving it into a CSV file, and then processing and plotting the data using MATLAB R2024a.

4.1. Simulations

To achieve better walking disturbance rejection, the control parameters were tuned through many attempts. The final parameters used in the simulation are as follows: In the high-level planner, the weight matrices are set to ${\mathit{W}}_b=\mathrm{diag}(100,100)$, ${\mathit{W}}_l=\mathrm{diag}(1,1)$, $w_{\eta }=0.1$, and the prediction horizon $N_s$ is 3. The maximum and minimum step durations are set to $T_{max}=0.3$ and $T_{min}=0.18$, respectively. For the CoM trajectory tracking task in the WBC, the gain parameters for the desired wrench are ${\mathit{K}}_{p,com}=\mathrm{diag}(100,100,900,250,250,100)$ and ${\mathit{K}}_{d,com}=\mathrm{diag}(50,50,120,100,100,50)$. For the swing foot trajectory tracking task, the desired acceleration gains are ${\mathit{K}}_{p,\mathrm{sw}}=100{\mathit{I}}_5$ and ${\mathit{K}}_{d,\mathrm{sw}}=25{\mathit{I}}_5$. Since the swing leg has only five degrees of freedom, it is not possible to control the 3D positions of both the toe and heel of the swing foot simultaneously. Therefore, the control of the x-direction of the heel is omitted. The weights in the QP problem are set to ${\mathit{W}}_{\mathrm{com}}=\mathrm{diag}(5,5,100,5,5,5)$, ${\mathit{W}}_{\mathrm{sw}}=10{\mathit{I}}_5$, ${\mathit{W}}_{\mathrm{st}}={10}^3{\mathit{I}}_6$, and $\mathit{R}=\mathit{I}$.

An analysis of the observer order was conducted to compare the estimation errors of observers with different orders. Figure 3 illustrates the estimation errors over time when a constant external force is applied to the robot. The first-order observer exhibits a higher overshoot in error compared to the second-order observer. As the order increases, the overshoot decreases. However, the overshoot increases again at the fourth order. If the order is further increased, the computation time becomes excessively long and there would be too many parameters, so no further comparisons were made. Among these four orders, the third-order observer performs the best. Therefore, in subsequent experiments, the order r was set to 3, and the coefficients ${\mathit{K}}_1$, ${\mathit{K}}_2$, and ${\mathit{K}}_3$ were chosen as $0.02{\mathit{I}}_{10}$, $0.42{\mathit{I}}_{10}$, and $19{\mathit{I}}_{10}$, respectively.

4.1.1. Stable Walking Under External Disturbance on the Swing Leg

To verify the improvement in disturbance rejection performance of our WBC method, we first tested the standard WBC approach without the momentum-based observer [3], which serves as the baseline controller provided by the developers of the BRUCE robot. During the test, the robot was subjected to external disturbances applied to the swing leg. As shown in Figure 4, when a forward external force of 5 N is applied to the swing leg during walking, the swing leg deviates from the planned foothold, causing the robot to lose stability after two or three steps.

When the proposed disturbance rejection WBC method is employed, the observer provides an estimate of the external force under the same conditions, as shown in Figure 5. It can be seen from the figure that the observer can quickly estimate the magnitude of the external force, and the estimation speed meets the requirements of the control method. With the momentum-based observer, the deviation of the swing leg caused by the external force is quickly compensated, allowing it to return to the planned foothold. The 3D spatial position error curves between the desired and actual positions of the right foot are shown in Figure 6. It can be observed that the swing foot is subjected to an external force at around 3 s, as shown in Figure 5. At this point, the swing foot error begins to increase, reaching a maximum deviation of 2 cm. As the estimation of the external force becomes more accurate, the disturbance is compensated, causing the error to decrease rapidly and allowing the swing foot to realign with the planned trajectory. This simulation verifies that under the same external disturbance, the standard WBC approach loses stability, while the proposed disturbance rejection WBC method maintains stable walking.

4.1.2. Stable Walking on Uneven Terrain

To further validate the performance of the proposed control scheme, we tested its effectiveness in controlling the BRUCE robot walking on uneven terrain. Comparisons were made with the standard WBC method and the CoM-based disturbance rejection WBC method from the literature [25], which only compensates for disturbances acting on the CoM. The uneven terrain consists of multiple 1 cm high wooden boards irregularly distributed across the area. Screenshots of the simulations for the three control methods are shown in Figure 7. Both the proposed disturbance rejection WBC method, which considers disturbances on both legs, and the CoM-based disturbance rejection WBC method enabled the robot to traverse the entire uneven terrain, whereas the standard WBC method failed.

The CoM trajectories of the robot walking on uneven terrain using CoM-based disturbance rejection WBC method and the proposed method are shown in Figure 8. From the CoM curve in the x-direction, it can be seen that the proposed method maintains a consistent slope during walking, with smaller velocity fluctuations. In the y-direction, although there is no walking velocity command in the y-direction, both methods inevitably experience deviations in the y-direction because of uneven terrain. However, compared to the CoM-based method, the proposed method exhibits smaller deviations. The simulation results demonstrate that the proposed disturbance rejection WBC method, compared to the CoM-based method, results in smaller CoM deviation errors and better trajectory tracking performance under such disturbances. Moreover, the proposed method enhances the robot’s robustness, enabling it to traverse such uneven terrain, unlike the standard WBC method.

4.2. Experiment

To evaluate the practicality of the proposed control scheme in real world environment, we conducted experimental validation on the biped robot BRUCE. The parameters used in the planning method for the experiments are same as those used in the simulations. For the CoM trajectory tracking task in the WBC, the gain parameters for the desired wrench are ${\mathit{K}}_{p,com}=\mathrm{diag}(100,100,800,200,200,100)$ and ${\mathit{K}}_{d,com}=\mathrm{diag}(50,50,150,100,100,50)$. For the swing foot trajectory tracking task, the desired acceleration gains are ${\mathit{K}}_{p,\mathrm{sw}}=100{\mathit{I}}_5$ and ${\mathit{K}}_{d,\mathrm{sw}}=10{\mathit{I}}_5$.

4.2.1. Walking Experiment Under External Swing Leg Disturbance

In this experiment, an external disturbance was applied to the robot’s swing leg using a tether attached to the right leg. The other end of the tether was connected to a force sensor to measure the magnitude and duration of the applied force. Figure 9 shows snapshots of the experiment. Initially, the tether was slack, and no external force was applied to the swing leg. When the tether was tightened, the swing leg began to experience an external force. Once the swing leg touched the ground, the external force was removed, and the tether returned to a slack state. From the figure, it can be seen that the robot maintains stable walking even when the swing leg is subjected to an external disturbance. The experiment demonstrated that the robot could withstand a maximum external force of 5 N while maintaining stability, with the disturbance lasting approximately 0.2 s.

Figure 10 depicts the deviation curve between the actual and desired positions of the swing foot during the experiment. Here, the deviation is the 3D spatial distance deviation e, calculated as the square root of the sum of the squared deviations in the three spatial directions $e_x$, $e_y$, and $e_z$, i.e., $e=\sqrt{e_x^2+e_y^2+e_z^2}$. As shown in the figure, when no external force was applied, the deviation between the actual and target positions remained within 2 cm. At approximately 1.1 s, the swing foot began to experience an external force, causing the position deviation to increase to 3.5 cm within 0.1 s. Due to the observer’s estimation and compensation of the external force in the proposed control method, the deviation decreased to 1.6 cm within 0.2 s, returning to the range observed in the absence of disturbance. This reduction minimized the impact of the external force on the swing foot’s landing position. Furthermore, the subsequent deviation remained within the 2 cm range, indicating that the effect of the external disturbance has been compensated and did not produce any further impact. The experimental results demonstrate that the proposed disturbance rejection control method effectively reduces the position deviation of the swing foot, ensuring stable walking of the robot when swing leg is under disturbance.

4.2.2. Walking Experiment Under Complex Disturbance

To further evaluate the disturbance rejection performance of the proposed method, an experiment was conducted to simultaneously resist multiple types of disturbances, including those from uneven terrain and external forces. Uneven terrain was created by placing wooden boards of varying heights (5 mm and 10 mm) on the ground. External forces were applied to the robot’s upper body using a stick with a force sensor at its tip. Figure 11 shows snapshots of this experiment. From the third subfigure, it can be seen that the BRUCE robot’s left foot is stepping on a 10 mm wooden board, while the right foot is on flat ground, and the upper body is subjected to an external force. The results demonstrate that the robot maintained stable walking even under these complex disturbances. During the experiment, the external force applied to the upper body was 10 N, lasting approximately 0.7 s.

Figure 12 illustrates the trajectories of the CoM, left foot, right foot, and divergent component of motion in the x-direction during the experiment. It can be observed that at the beginning of the experiment, the robot walked on uneven terrain at a speed of approximately 0.2 m/s. At around 1 m (approximately 3.2 s), the upper body began to experience an external force. Under the influence of this force, the CoM position stopped moving forward. To maintain stability, the right foot moved backward by about 10 cm. After the external force was removed, the robot quickly recovered to a walking speed of 0.2 m/s. The experimental results demonstrate that the proposed disturbance rejection control method enables the robot to maintain stable walking while simultaneously resisting disturbances from uneven terrain and external forces.

5. Discussion and Conclusions

In this paper, we presented a complete disturbance rejection control scheme for bipedal robots. The high-level DCM-based planner selects the desired foothold position and step duration using the MPC method, enhancing gait robustness through adaptive step duration adjustments. The low-level control method integrates a momentum-based observer, capable of estimating disturbances on both stance and swing legs, and a weighted WBC method based on full-body dynamics with estimated disturbances. Our method compensates for disturbance on both stance leg and swing leg, enabling the robot to adapt to complex situations. Notably, our method does not require foot-mounted sensors to measure ground reaction forces, making it applicable to a wider range of bipedal robots. We conducted a series of simulations and experiments to demonstrate the effectiveness of our control scheme. Compared to the standard WBC method, our disturbance rejection WBC method effectively handles disturbances on the swing leg. In comparison to the CoM-based disturbance rejection WBC method, our method results in smaller CoM deviation errors and better trajectory tracking performance when controlling the robot’s walking on uneven terrain.