Balance and Walking Control for Biped Robot Based on Divergent Component of Motion and Contact Force Optimization

Abstract

This paper presents a complete planner and controller scheme to achieve balance and walking for a biped robot, which does not need to distinguish the robot’s dynamic model parameters. The high-level planner utilizes model predictive control to optimize both the foothold location and step duration based on the Divergent Component of Motion (DCM) model to enhance the robustness of generated gaits. For low-level control, we use quadratic programming (QP) to optimize the contact force distribution under the contact constraints to achieve the virtual wrench exerted on the base of the robot. Then, the joint torques sent to the robot are derived from three parts: first, the torques mapped from the contact force; second, the swing leg tracking; and third, the stance foot stabilization. The simulation and experiment on BRUCE, a miniature bipedal robot from Westwood Robotics (Los Angeles, CA, USA), testify to the performance of the control scheme, including push recovery, Center of Mass (CoM) tracking, and omnidirectional walking.

1. Introduction

Legged robots hold great potential in numerous practical applications due to their adaptability to complex terrains. In recent years, legged robots like quadrupeds and bipeds have received considerable research attention. Quadrupedal robots, for instance, Spot from Boston Dynamics, have evolved rapidly and have been deployed in real industrial scenarios. In comparison, bipedal robots are still not ready for field deployment. For control, there are challenges like high-dimensional hybrid dynamics, heavy constraints, and strict real-time demands. Early on, bipeds were often position-controlled, which means that the joint input command type is position [1]. Position-controlled bipeds like Honda ASIMO [2], UBTECH Walker [3], and AIST HRP-5P [4] are typically quite stiff and lack compliance [5], appearing less dynamic and robust. In the last couple of years, torque-controlled bipeds, such as Boston Dynamics Atlas [6], Agility Robotics Digit [7], Bruce [8], and Unitree H1 [9], have become mainstream. Torque-controlled robots can directly handle forces, allowing for increased control frequency and typically requiring lower gains to track joint trajectories since they use feed-forward torque to compensate for the dynamics of the robot.

To date, most research communities converge on the idea of generating locomotion control strategies by solving an optimal control problem [10]. Since the biped system is hybrid, nonlinear, and highly constrained, solving this problem online remains intractable. Thus, most approaches break the problem down into multiple stages and utilize either an abstract model or the whole dynamic model in each stage, sacrificing global optimality while achieving real-time execution [10,11,12]. A major paradigm is to have a two-level structure, where the high level plans the footsteps and the low level optimizes the contact forces [6,13,14].

The high-level planner aims to generate the trajectory. The trajectory needs to stabilize the robot, and it is generally generated online. Online trajectory generation usually adopts an abstract model, e.g., the Linear Inverted Pendulum Model (LIPM) [15,16]. Kajita et al. [17] computed CoM trajectories using a preview control approach where a discrete-time, infinite-horizon Linear Quadratic Regulator (LQR) problem was solved based on LIPM. Wieber [18] enhanced the preview control by using a linear Model Predictive Control (MPC) scheme that handles inequality constraints, such as stability bounds and reachability limits. Then, MPC and LIPM can be extended to generate other trajectories, like footstep [19] and CoM height trajectories [20]. Later, researchers found that the dynamics of the LIPM can be divided into stable and unstable components. Thus, the controller can only consider the unstable parts, known as the DCM [21] or Instantaneous Capture Point. Also, researchers noticed that step duration can be unfixed. Khadiv et al. [22,23] optimized both the landing point and step duration based on the DCM model and showed that timing adjustment can enhance robustness.

For the low-level controller, Mistry et al. [24] used inverse dynamics to compute feed-forward torques, which increase movement accuracy and enhance compliance and robustness in the face ofperturbations. However, this method considers trajectories in joint space, resulting in the requirement of many motion details, which becomes impractical for robots with many degrees of freedom [25]. Sentis et al. [26] utilized task-space control to avoid this problem by considering trajectories in the operational space. During the last decade, optimization, usually QP, has been leveraged to compute inverse dynamics [6,27], which are able to account for both task space and joint space dynamics while satisfying various constraints concurrently. All the above methods need to compute inverse dynamics, which distinguishing the robot dynamic model parameters, i.e., the inertial of each link, is required. Also, inverse dynamics need to compute the inertia matrix [28] in each control loop. This increases the computational burden, as the inertia matrix is a large matrix. In this paper, we presented a controller that does not require computing inverse dynamics. The controller first computes a virtual wrench required to stabilize the upper body. Then, ground reaction forces (GRFs) at predefined contact points are optimized under unilateral and friction cone constraints. Finally, the torque command is derived by mapping GRF to joint space under a quasi-static assumption. Simultaneously, swing leg torque and stance foot stabilization are considered.

Lee and Kim [29] also controlled contact force without using inverse dynamics. They optimized the vertical force and roll/pitch moments at a single point on the foot to minimize the error of the Zero Moment Point (ZMP) and the height of the CoM. In contrast, our method directly controlled the contact force without needing to consider the ZMP, as our method implicitly controlled the ZMP to remain within the foot sole. Additionally, we optimized forces at two points on each foot during the single-support phase. This allowed our method to generate six parameters, while their method optimized only three. As a result, our method could handle more complex situations. Unlike their method, which used force/torque sensors to improve the accuracy of foot wrench control, we avoided using these sensors to prevent adding more inertia to the leg, which would make the robot deviate from the LIPM. Ott et al. [30] also directly controlled the contact force to achieve the desired wrench needed to recover posture, similar to our approach. However, they did not consider maintaining the foot position and orientation. For walking tasks, maintaining the foot position is crucial. Therefore, we proposed a foot stabilization wrench to address this issue and ensure stability during walking.

In this paper, we optimized both step location and timing in the high-level planner and combined it with the above controller to control the robot. The whole control scheme is shown in Figure 1. In this figure, ${\mathit{x}}_c^{des}$, ${\dot{\mathit{x}}}_c^{des}$, ${\mathit{R}}_b^{des}$, and ${\mathit{\omega }}_b^{des}$ are the desired values of the CoM position, velocity, base orientation, and angular velocity, respectively. The footstep planner derives the optimal foothold position ${\mathit{p}}^{*}$ and step duration $T^{*}$ based on the desired CoM velocity and feedback information, including the actual CoM position ${\mathit{x}}_c$, velocity ${\dot{\mathit{x}}}_c$, and the stance foot rotation matrix $\mathit{R}$. Then, the swing leg foot trajectories ${\mathit{p}}^{ref}$ and ${\dot{\mathit{p}}}^{ref}$ are calculated. These trajectories are the input for inverse kinematics to compute joint references ${\mathit{q}}^{ref}$ and ${\dot{\mathit{q}}}^{ref}$. A Proportional–Integral–Derivative (PID) controller is used to generate the corresponding joint torque ${\mathit{\tau }}^{sw}$, driving the actual joint position $\mathit{q}$ and velocity $\dot{\mathit{q}}$ to follow the joint references. For the low-level controller, the virtual wrench ${\mathit{W}}^d$ is generated based on the error between the desired CoM and base values and the actual CoM position, velocity, base orientation ${\mathit{R}}_b$, and angular velocity ${\mathit{\omega }}_b$. The contact force distribution optimizes the contact force ${\mathit{F}}_c$ at each predefined contact point. This also adds the contact foot stabilizing wrench ${\mathit{W}}_{c_i}^{stabil}$, which is based on the stance foot velocity ${\dot{\mathit{r}}}_{c{c}_i}$ and angular velocity ${}^b{\mathit{\omega }}_{b{c}_i}$. Finally, the contact force and stabilizing wrench are mapped to derive the joint torque ${\mathit{\tau }}_F$, which is combined with ${\mathit{\tau }}^{sw}$ and sent to the robot.

This paper is organized in accordance with the structure of the control scheme. Section 2 provides details about the DCM-based high-level footstep planner, deciding both the step location and duration. Section 3 describes in detail the design of the controller based on contact force optimization. Section 4 presents the simulation and experimental results of the controller applied on BRUCE. In the end, conclusions and a discussion are presented in Section 5.

2. Footstep Planner

The bipedal robot is a hybrid dynamic and highly nonlinear system, making it challenging to use the full-order dynamic model to optimize the trajectory of all bodies online. Thus, researchers use reduced-order models like LIPM to optimize the trajectory of the most salient parts of the system, e.g., CoM and Center of Pressure (CoP). Notably, the LIPM dynamics can be decomposed into two parts, one stable and the other unstable, which is called DCM. Only controlling the DCM is sufficient to generate stable walking. Here, we employ DCM to optimize both the footstep position and timing.

2.1. MPC Formulation

The equations of motion for the LIPM can be written as follows:

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

(1)

where $\mathit{x},\ddot{\mathit{x}}$ are 2D vectors representing the horizontal position and acceleration of the CoM, $\omega =\sqrt{\frac{g}{z_{\mathrm{com}}}}$ is the reciprocal of the time constant for LIPM, g is the gravitational acceleration, and $z_{\mathrm{com}}$ is a fixed parameter representing the vertical position of the CoM. $\mathit{p}$ is the horizontal position of the CoP, which is assumed to be on the ground, i.e., its height is 0.

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

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

(2)

Then, the LIPM dynamics can be split into 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}$$

(3)

The first equation of Equation (3) is the stable part of LIPM, i.e., $\mathit{x}$ converges to $\mathit{\xi }$, whereas the second equation is unstable, i.e., $\mathit{\xi }$ diverges along the line between $\mathit{\xi }$ and $\mathit{p}$. Therefore, we need to control $\mathit{\xi }$ to achieve stable walking. Since this differential equation is simple, we can obtain its analytical solution:

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

(4)

where ${\mathit{\xi }}_0$ and $\mathit{\xi }\left(t\right)$ denote the position of the DCM at the initial time and time t, respectively. Here, we assume ${\mathit{p}}_0$ is fixed during this period. With this solution, we can predict the future DCM in a few steps, which is needed by the MPC method.

Then, the DCM offset is defined as $\mathit{b}=\mathit{\xi }-\mathit{p}$, which is the direction of the DCM velocity, and periodicity requires this DCM offset to be the same at the start and end of the step:

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

(5)

where T is the step duration.

Using the method [22], we can specify nominal step values, including the nominal step length ($L_{nom}$), nominal width ($W_{nom}$), nominal duration ($T_{nom}$), and nominal DCM offset (${\mathit{b}}_{nom}={[b_{x,nom},b_{y,nom}]}^T$), which are related to the desired average walking velocity. Furthermore, to avoid the nonlinearity induced by step duration, a new decision variable $\eta =e^{\omega T}$ is applied instead of the step duration.

In order to make step values approach the above nominal values as closely as possible, we formalized a QP to minimize the error between predicted step values and nominal values, taking the foothold location and DCM offset of the next few steps and current footstep duration as decision variables. Note here that we directly take 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,\cdots ,N_s\hfill \\ \hfill & \hfill & & {\mathit{l}}_{min}\le {\mathit{R}}^T{\mathit{l}}_k\le {\mathit{l}}_{max},k=1,\cdots ,N_s\hfill \\ \hfill & \hfill & & e^{\omega T_{min}}\le \eta \le e^{\omega T_{max}}\hfill \end{array}$$

(6)

where ${\mathit{l}}_{nom}={[L_{nom},W_{nom}]}^T$ is the nominal step length in both the sagittal and the coronal planes. $\mathit{R}\in SO\left(2\right)$ is a planar rotation matrix considering the yaw orientation of the foot. $t_0$ is the elapsed time in this step. $N_s$ is the step horizon. ${\mathit{l}}_{min},{\mathit{l}}_{max}$ are the kinematically reachable constraints, which are determined by the range of motion of the robot. $T_{min},T_{max}$ are the parameters to limit the step duration, which need to be tuned by trial and error. Since BRUCE is a miniature biped robot, its Divergent Component of Motion (DCM) diverges faster. Therefore, a shorter step duration would be better. However, too short a step duration would result in a large swing foot acceleration. Thus, the minimum step duration parameter should be carefully chosen. The maximum duration parameter prevents slow stepping, which rarely occurs according to our experience.

2.2. Online Foot Trajectory Generation

After deriving the step location ${\mathit{p}}^{*}$ and timing $T^{*}$, the swing foot trajectory needs to be carefully designed to achieve the changing footstep location and timing. In order to ensure continuity at the order of acceleration, a fifth-order polynomial is applied as the reference trajectory:

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

(7)

where $t_0$ is the current time calculated from the start of this step. Note that this ${\mathit{p}}^{ref}$ only includes the horizontal trajectory. The boundary conditions are given as follows:

$$\begin{array}{cc}\hfill {\mathit{p}}^{ref}\left(t_0\right)=\mathit{p}\left(t_0\right)& {\mathit{p}}^{ref}\left(T^{*}\right)={\mathit{p}}^{*}\hfill \\ \hfill {\dot{\mathit{p}}}^{ref}\left(t_0\right)=\dot{\mathit{p}}\left(t_0\right)& {\dot{\mathit{p}}}^{\mathit{ref}}\left(T^{*}\right)=\mathbf{0}\hfill \\ \hfill {\ddot{\mathit{p}}}^{ref}\left(t_0\right)=\ddot{\mathit{p}}\left(t_0\right)& {\ddot{\mathit{p}}}^{\mathit{ref}}\left(T^{*}\right)=\mathbf{0}\hfill \end{array}$$

(8)

As for the trajectory in the vertical direction, we use two fifth-order polynomials: one for the rising phase, during which the height increases from the ground to a fixed apex, and another for the landing phase.

3. Controller Based on Contact Force Optimization

One of the main tasks of the controller is to control the position and orientation of the base. Compared to optimization-based inverse dynamics control, our approach is more intuitive; that is, we apply a virtual Cartesian wrench ${\mathit{W}}^d$ to control the CoM position and the orientation of the base, which simplifies the dynamic model compared to controlling the base position directly [31].

$${\mathit{W}}^d=\left[\begin{array}{c}{\mathit{W}}_{lin}^d\\ {\mathit{W}}_{ang}^d\end{array}\right]=\left[\begin{array}{c}{\mathit{k}}_{lin}({\mathit{x}}_c^{des}-{\mathit{x}}_c)+{\mathit{d}}_{lin}({\dot{\mathit{x}}}_c^{des}-{\dot{\mathit{x}}}_c)+{\mathit{i}}_{lin}\sum ({\mathit{x}}_c^{des}-{\mathit{x}}_c)\\ {\mathit{k}}_{ang}\mathrm{Log}\left({\mathit{R}}_b^T{\mathit{R}}_b^{des}\right)+{\mathit{d}}_{ang}({\mathit{\omega }}_b^{des}-{\mathit{\omega }}_b)+{\mathit{i}}_{ang}\sum \mathrm{Log}\left({\mathit{R}}_b^T{\mathit{R}}_b^{des}\right)\end{array}\right]$$

(9)

where ${\mathit{W}}_{lin}^d$ and ${\mathit{W}}_{ang}^d$ are the desired force and torque components, respectively. ${\mathit{k}}_{lin}$, ${\mathit{d}}_{lin}$, ${\mathit{i}}_{lin}$, ${\mathit{k}}_{ang}$, ${\mathit{d}}_{ang}$, and ${\mathit{i}}_{ang}$ are the PID gain matrices for force and torque. ${\mathit{x}}_c$, ${\mathit{R}}_b$, and ${\mathit{\omega }}_b$ represent the CoM position, base orientation, and angular velocity. The superscript “des” indicates their desired values. The following is an analysis of the relationship between the GRF and the desired wrench.

3.1. Dynamic Model

Here, we derive the floating base dynamics using the Newton–Euler method, as it provides an intuitive way to express the control method. The free-floating biped robot is described by the base position ${\mathit{r}}_b\in {\mathbb{R}}^3$ (we use the presuperscript to represent which frame it is denoted in. Mostly, it would be the world frame $\left\{w\right\}$, so w will always be omitted for brevity) and orientation ${\mathit{R}}_b\in SO\left(3\right)$, and $n_j$ actuated joint coordinates $\mathit{q}\in {\mathbb{R}}^{n_j}$. The generalized velocity is $\mathit{u}={[{\mathit{v}}_b,{}^b{\mathit{\omega }}_b,\dot{\mathit{q}}]}^T$, where ${\mathit{v}}_b,{}^b{\mathit{\omega }}_b$ are the linear and angular velocities of the base, respectively. According to the kinematics of the floating base system, the position ${\mathit{r}}_{p_i}$ and velocity ${\dot{\mathit{r}}}_{p_i}$ of an arbitrary point $p_i$ with respect to the world frame are as follows:

$$\begin{array}{c}{\mathit{r}}_{p_i}={\mathit{r}}_b+{\mathit{R}}_b{}^b{\mathit{r}}_{b{p}_i}\hfill \\ {\dot{\mathit{r}}}_{p_i}=[{\mathbf{1}}_{3\times 3}-{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{b{p}_i}{\mathit{J}}_{P_{p_i}}]\mathit{u}\hfill \end{array}$$

(10)

where ${}^b{\mathit{r}}_{b{p}_i}$ denotes the position of $p_i$ with respect to the base frame expressed in frame $\left\{b\right\}$. ${\mathbf{1}}_{3\times 3}$ represents the $3\times 3$ identity matrix and ${}^b{\widehat{\mathit{r}}}_{b{p}_i}$ is the hat operator $\widehat{(·)}$ of ${}^b{\mathit{r}}_{b{p}_i}$. 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{J}}_{P_{p_i}}=\partial {\mathit{r}}_{b{p}_i}/\partial \mathit{q}$ is a Jacobian matrix.

A frame $\left\{p_i\right\}$ is attached at position $p_i$ to represent its orientation ${\mathit{R}}_{b{p}_i}$ and angular velocity ${}^b{\mathit{\omega }}_{b{p}_i}$ with respect to the base frame:

$${}^b{\mathit{\omega }}_{p_i}=\left[{\mathbf{0}}_{3\times 3}{\mathbf{1}}_{3\times 3}{\mathit{J}}_{R_{b{p}_i}}\right]\mathit{u}$$

(11)

where ${\mathit{J}}_{R_{b{p}_i}}$ is calculated by ${\mathit{J}}_{R_{b{p}_i}}=\partial {\mathit{\phi }}_{b{p}_i}/\partial \mathit{q}$ and ${\mathit{\phi }}_{b{p}_i}\in {\mathbb{R}}^3$ is a rotation vector corresponding to ${\mathit{R}}_{b{p}_i}$. Our controller aims to control the position of the CoM instead of the base; thus, we replace the base velocity in $\mathit{u}$ with the velocity of the CoM ${\dot{\mathit{x}}}_c$. Equation (10) is used to calculate the CoM velocity:

$${\dot{\mathit{x}}}_c=[{\mathbf{1}}_{3\times 3}-{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{bc}{\mathit{J}}_{P_c}]\mathit{u}$$

(12)

where ${}^b{\mathit{r}}_{bc}$ denotes the position of the CoM with respect to the body frame expressed in frame $\left\{b\right\}$, and ${\mathit{J}}_{P_c}$ is calculated by ${\mathit{J}}_{P_c}=\partial {\mathit{r}}_{bc}/\partial \mathit{q}$. With the new generalized velocity $\overline{\mathit{u}}={[{\dot{\mathit{r}}}_c,{}^b{\mathit{\omega }}_b,\dot{\mathit{q}}]}^T$, ${\dot{\mathit{r}}}_{p_i}$ in Equation (10) can be written as follows:

$${\dot{\mathit{r}}}_{p_i}=[{\mathbf{1}}_{3\times 3}-{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{c{p}_i}^T{\mathit{J}}_{P_{c{p}_i}}]\overline{\mathit{u}}$$

(13)

where ${}^b{\mathit{r}}_{c{p}_i}={}^b{\mathit{r}}_{b{p}_i}-{}^b{\mathit{r}}_{bc}$ denotes the position of $p_i$ with respect to the CoM expressed in frame $\left\{b\right\}$ and ${\mathit{J}}_{P_{c{p}_i}}$ can be derived by ${\mathit{J}}_{P_c}={\mathit{J}}_{P_{p_i}}-{\mathit{J}}_{P_c}$.

Then, the virtual displacement $\delta {\mathit{r}}_{p_i}$ of $p_i$ and virtual rotation ${\delta }^b{\mathit{\phi }}_{p_i}$ of frame $\left\{p_i\right\}$ can be described by the variation $\delta \overline{\mathit{u}}={[\delta {\mathit{r}}_c,{\delta }^b{\mathit{\phi }}_b,\delta \mathit{q}]}^T$:

$$\left[\begin{array}{c}\delta {\mathit{r}}_{p_i}\\ {\delta }^b{\mathit{\phi }}_{p_i}\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{1}}_{3\times 3}& -{\mathit{R}}_b{{}^b{\widehat{\mathit{r}}}_{c{p}_i}}^T& {\mathit{J}}_{P_{c{p}_i}}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{1}}_{3\times 3}& {\mathit{J}}_{R_{b{p}_i}}\end{array}\right]\left[\begin{array}{c}\delta {\mathit{r}}_c\\ {\delta }^b{\mathit{\phi }}_b\\ \delta \mathit{q}\end{array}\right]$$

(14)

Note that we use $\delta {\mathit{\phi }}_b$ to denote the variation in the rotation of the base with respect to the world frame.

The free-body diagram of the biped robot is illustrated in Figure 2. We applied the principle of virtual work to derive the relationships between all the $n_{ext}$ applied wrenches under the assumption of quasi-static equilibrium:

$$\begin{array}{cc}\hfill \delta W& =\sum _{i=0}^{n_{ext}}\left[\begin{array}{cc}\delta {\mathit{r}}_{p_i}^T& {\delta }^b{\mathit{\phi }}_{p_i}^T\end{array}\right]{\mathit{W}}_{ext,i}\hfill \\ \hfill & =\left[\begin{array}{c}\delta {\mathit{r}}_c^T\\ {\delta }^b{\mathit{\phi }}_b^T\\ \delta {\mathit{q}}^T\end{array}\right]\left[\begin{array}{cc}{\mathbf{1}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ -{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{c{p}_i}^T& {\mathbf{1}}_{3\times 3}\\ {\mathit{J}}_{P_{c{p}_i}}^T& {\mathit{J}}_{R_{b{p}_i}}^T\end{array}\right]\left[\begin{array}{c}{\mathit{F}}_{ext,i}\\ {\mathit{T}}_{ext,i}\end{array}\right]=\mathbf{0}\forall \left(\begin{array}{c}\delta {\mathit{r}}_c^T\\ {\delta }^b{\mathit{\phi }}_b^T\\ \delta {\mathit{q}}^T\end{array}\right)\hfill \end{array}$$

(15)

The applied wrenches include a virtual Cartesian wrench ${\mathit{W}}^d$, gravitational wrench ${\mathit{W}}_g={[m\mathit{g},\mathbf{0}]}^T$, contact wrench ${\sum }_{i=1}^{n_c}{\mathit{W}}_{c_i}={\sum }_{i=1}^{n_c}{[{\mathit{F}}_{c_i},\mathbf{0}]}^T$, and actuation wrench ${\sum }_{i=1}^{n_j}{\mathit{W}}_{a_i}={\sum }_{i=1}^{n_j}{[\mathbf{0},{\mathit{\tau }}_i]}^T$. Here, $n_c$ is the number of contact points $c_i$. For simplicity, we consider the frictional point contact without ground reaction moment, since its magnitude is small [32]. Then, the relationships between all these forces are as follows:

$$\begin{array}{c}\hfill {\mathit{W}}_{lin}^d+m\mathit{g}+\sum _{i=1}^{n_c}{\mathit{F}}_{c_i}=\mathbf{0}\\ \hfill {\mathit{W}}_{ang}^d+\sum _{i=1}^{n_c}{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{c{c}_i}^T{\mathit{F}}_{c_i}=\mathbf{0}\\ \hfill \mathit{\tau }+\sum _{i=1}^{n_c}{\mathit{J}}_{P_{c{c}_i}}^T{\mathit{F}}_{c_i}=\mathbf{0}\end{array}$$

(16)

From this result, we can intuitively conclude that, by controlling the contact force distribution, the desired wrench can be achieved, and the mapping from contact forces to joint torques is also derived.

3.2. Computation of Contact Force and Torque

In this section, we aim to attain the desired contact force distribution with predefined contact points. Contact forces need to satisfy the first two equations of Equation (16), as well as unilateral and friction cone constraints. Thus, we cast our problem as an optimization problem in a QP form:

$$\begin{array}{cccc}\hfill & \underset{{\mathit{F}}_{c_1},\cdots ,{\mathit{F}}_{c_{n_c}}}{\min }\hfill & \hfill & \parallel \sum _{i=1}^{n_c}{\mathit{F}}_{c_i}+{\mathit{W}}_{lin}^d+m\mathit{g}{\parallel }_{{\mathit{W}}_F}^2+\parallel \sum _{i=1}^{n_c}{\mathit{R}}_b{}^b{\widehat{\mathit{r}}}_{c{c}_i}^T{\mathit{F}}_{c_i}+{\mathit{W}}_{ang}^d{\parallel }_{{\mathit{W}}_T}^2+\sum _{i=1}^{n_c}{\parallel {\mathit{F}}_{c_i}\parallel }_{{\mathit{W}}_c}^2\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.\hfill & \hfill & \left[\begin{array}{ccc}\pm 1& 0& -\mu \\ 0& \pm 1& -\mu \\ 0& 0& -1\\ 0& 0& 1\end{array}\right]{\mathit{F}}_{c_i}\le \left[\begin{array}{c}0\\ 0\\ -f_{z,min}\\ f_{z,max}\end{array}\right],i=1,\cdots n_c\hfill \end{array}$$

(17)

where ${\mathit{W}}_F,{\mathit{W}}_T,{\mathit{W}}_c$ are all $3\times 3$ diagonal matrices, denoting the weight for each task. The third term in the cost function makes the cost positive definite; without it, there are many solutions to this optimization problem. Since the friction cone constraint is nonlinear, we use a square pyramid to approximate it. $\mu$ is the friction coefficient, and $f_{z,min},f_{z,max}$ are positive scalar values designed to ensure contact stability and unilateral constraints. Finally, the joint torque $\tau$ is calculated with the third equation of Equation (16):

$$\mathit{\tau }=-\sum _{i=1}^{n_c}{\mathit{J}}_{P_{c{c}_i}}^T{\mathit{F}}_{c_i}$$

(18)

3.3. Swing Leg Tracking and Stance Foot Stabilization

The joint torques derived above are only able to achieve the balance task, while for walking, the above joint torques mostly only affect the stance leg joints. In addition, we need the swing leg to touch the desired foothold position, which we derived in the footstep planner. Thus, joint position PID control is applied because it does not require a dynamic model and allows for a high updating frequency. Therefore, besides the joint torques coming from the contact force, we take the joint position and velocity references into consideration, which are solved by inverse kinematics:

$$\mathit{\tau }=-\sum _{i=1}^{n_c}{\mathit{J}}_{P_{c{c}_i}}^T{\mathit{F}}_{c_i}+{\mathit{k}}_p({\mathit{q}}^{ref}-\mathit{q})+{\mathit{k}}_d({\dot{\mathit{q}}}^{ref}-\dot{\mathit{q}})+{\mathit{k}}_i\sum ({\mathit{q}}^{ref}-\mathit{q})$$

(19)

where ${\mathit{k}}_p$, ${\mathit{k}}_d$, and ${\mathit{k}}_i$ are $n_j\times n_j$ diagonal matrices combining PID gains for each joint.

A biped robot is a floating base system that needs to satisfy kinematic constraints. These kinematics constraints ensure that the contact points cannot move, which the above procedure does not consider. To maintain stance foot position and orientation, we add a damping wrench ${\mathit{W}}_{c_i}^{stabil}=-{\mathit{d}}_{stabil}{\dot{\mathit{r}}}_{c{c}_i}$ to the above contact wrench when deriving joint torques, where ${\mathit{d}}_{stabil}$ is a positive diagonal damping matrix. The final joint torques are as follows:

$$\mathit{\tau }=-\sum _{i=1}^{n_c}({\mathit{J}}_{P_{c{c}_i}}^T{\mathit{F}}_{c_i}+\left[{\mathit{J}}_{P_{c{c}_i}}^T{\mathit{J}}_{R_{b{c}_i}}^T\right]{\mathit{W}}_{c_i}^{stabil})+{\mathit{k}}_p({\mathit{q}}^{ref}-\mathit{q})+{\mathit{k}}_d({\dot{\mathit{q}}}^{ref}-\dot{\mathit{q}})+{\mathit{k}}_i\sum ({\mathit{q}}^{ref}-\mathit{q})$$

(20)

4. Simulations and Experiments

In this section, a series of simulations and real-world experiments were conducted to validate the capability of our controller. Both the simulations using MuJoCo (Version 3.1.4) [33] 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 onboard IMU, joint encoders, and the sensing feet (detecting contact) are used to estimate the robot’s state. A mini PC with an Intel Core i5-7260U Dual-Core CPU at 2.2 GHz was utilized as the onboard computing resource. The QPs in the footstep planner and controller were both solved using the off-the-shelf QP solver OSQP (Version 0.6.5) [34]. Based on the onboard PC and OSQP, our control scheme could achieve a 1 kHz updating frequency, while whole-body control [12], which considers whole dynamics, could only achieve 500 Hz. A Supplementary Video recorded all the simulations and experiments.

4.1. Simulations

4.1.1. Push Recovery

The balance task is the most fundamental task for a bipedal system; thus, we first tested the capability of the controller to maintain balance under external disturbances. In the first simulation, the desired linear and angular velocities in Equation (9) were set to $\mathbf{0}$, and an impulsive force was then exerted at the base in the positive x direction (7 N), followed by in the positive y direction (14 N). Figure 3 displays snapshots of the simulation and the results are shown in Figure 4. The push force is the maximum force in each direction with a fixed duration (0.1 s), and we can see that the robot could recover to its initial position within one second. The reason why the y direction can resist larger forces is that the support polygon has a larger width than length because of the short feet.

We compared our method with model-based whole-body control [12] through push recovery in the simulation. The result is shown in Figure 5. The figure shows that our method deviates less in both the x and y directions under the same push forces compared to the dynamic model-based method. This is because our method does not need to compute dynamic information in the controller, allowing our control frequency to achieve 1 kHz, which is twice that of their method (500 Hz). This high control frequency enables the controller to respond to push forces quickly and thus reduces the CoM deviation error.

4.1.2. CoM Tracking

In this simulation, BRUCE was commanded to track the desired CoM trajectory in both the sagittal and lateral directions. Like the first simulation, we first made the robot track the reference CoM trajectory in the sagittal direction, then in the lateral direction. The reference trajectory is a sinusoidal curve with a two-second period. From the results illustrated in Figure 6, we can see that the controller was able to track the reference with minor lags. A snapshot of each task is only shown in the simulation or experiment.

4.1.3. Omnidirectional Walking

This simulation was applied to validate the whole control scheme, including the DCM-based footstep planner and contact force distributed controller. Bruce was initially stepping in a fixed position, then commanded to walk in the positive x direction with a velocity of 0.05 m/s. After a while, the command switches to walk in the positive y direction with a speed of 0.05 m/s, and finally returns to walking in the positive x direction with the same velocity. The result is shown in Figure 7, and we can see that the robot was able to walk omnidirectionally. From the inset in the bottom graph of Figure 7, we can see that the duration of the stance phase was different, since we optimized the step duration in our planner.

We also compared the CoM tracking performance during walking with model-based whole-body control, the same method used in the push recovery comparison. As shown in Figure 8, at the start, the velocity command increased from 0 to 0.05 m/s. Our method responded faster to the change due to our higher control frequency. Since our method does not use dynamic information, it has a slight drift error, which is acceptable to us. In conclusion, our method benefits from a quick response to the velocity command.

4.2. Hardware Experiments

We also applied the above tasks to hardware to demonstrate the effectiveness of the control scheme in the real world, which is more challenging. The parameters of the planner and controller used in the experiments are presented in

Table 1. Parameters of planner and controller.

Parameter	Value
$N_s$	3
${\mathit{W}}_b$	(100, 100)
${\mathit{W}}_l$	(1, 1)
$w_{\eta }$	0.1
${\mathit{k}}_{lin}$	(90, 90, 900)
${\mathit{d}}_{lin}$	(30, 30, 120)
${\mathit{i}}_{lin}$	(8, 8, 20)
${\mathit{k}}_{ang}$	(45, 45, 90)
${\mathit{d}}_{ang}$	(20, 20, 25)
${\mathit{i}}_{ang}$	(4, 4, 9)
${\mathit{W}}_F$	(10, 10, 100)
${\mathit{W}}_T$	(10, 10, 10)
${\mathit{W}}_c$	(1, 1, 1)
${\mathit{d}}_{stabil}$	(60, 60, 60, 100, 100)

.

4.2.1. Push Recovery

In this experiment, we directly applied impulsive forces on the base by hand while the robot maintained its balance. This is similar to the first simulation, except we could not measure the magnitude and direction of the impulsive force. Figure 9 shows the CoM error. The result of contact force distribution is shown in Figure 10. When the CoM was pushed in the x direction, the vertical force on the toes of both the right and left legs increased to support the weight of the robot. At the same time, a force in the x direction generated a counteracting force to return the CoM to its initial position. The same procedure occurred when the CoM was pushed in the y direction.

4.2.2. CoM Tracking

In this experiment, BRUCE was commanded to track the desired CoM trajectory in both sagittal and lateral directions. The reference trajectory is a sinusoidal curve with a two-second period, like in the simulation, but with a different magnitude. A snapshot of this experiment is shown in Figure 11, and the trajectory of the CoM is illustrated in Figure 12. The contact force distribution for this experiment is depicted in Figure 13.

4.2.3. Omnidirectional Walking

This experiment aims to demonstrate the capability of the control scheme in real-world robot walking. Figure 14 and Figure 15 show the results when the robot is commanded to go left and then go forward. From the figures, we can see that our control scheme can maintain DCM stability and the optimized foothold position can keep the robot balanced.

5. Conclusions

In this paper, we presented a complete control scheme, including a planner and a controller, for the balance and walking of a biped robot. Our controller did not need to solve inverse dynamics. The high-level DCM-based planner was applied to choose the desired foothold position and step duration, considering future steps using the MPC method. We found that adapting the step duration in addition to the footstep location can help the robot recover better to the nominal gait pattern. Additionally, since BRUCE is a miniature biped robot, its DCM diverges faster, so a shorter step duration is preferred. However, too short a step duration would result in a large swing foot acceleration. Therefore, the step duration parameter $T_{min}$ should be carefully chosen. The controller based on contact force optimization was explained in detail. It first applied a virtual wrench on the CoM and base. Then, contact force distribution and corresponding joint torques were inferred based on the quasi-static assumption. The joint torques for swing leg tracking and stance foot stabilization were also considered in the controller. Lastly, we conducted a series of simulations and experiments to demonstrate the effectiveness of our control scheme. By comparing with the dynamic model-based method in simulation, we found that our method can respond to push forces quickly and reduce the CoM deviation error, as our method has a higher control frequency. This high control frequency also allows our method to more quickly follow changes in the velocity command. We also observed that our contact foot stabilization wrench can compensate for the absence of a dynamic model.

Future work will focus on decreasing velocity drift during walking and on enabling walking on uneven terrain. We will also consider using newer methods for modeling and system identification, as well as new control methods, such as control Lyapunov functions, to improve control performance.