Quadrupedal Robots Whole-Body Motion Control Based on Centroidal Momentum Dynamics

Abstract

In this paper, we demonstrate a method for quadruped dynamic locomotion based on centroidal momentum control. Our method relies on a quadratic program that solves an optimal control problem to track the reference rate of change of centroidal momentum as closely as possible while satisfying the dynamic, input, and contact constraints of the full quadruped robot dynamics. Given the desired footstep positions, the according reference rate of change of the centroidal momentum is formulated as a feedback control task derived from the CoM motions of a simplified model (linear inverted pendulum) based on Capture Point dynamics. The joint accelerations and the Ground Reaction Forces(GRFs) outputted from the quadratic program solver are used to calculate the desired joint torques using an inverse dynamics algorithm. The performance of the proposed method is tested in simulation and on real hardware.

1. Introduction

Legged robots are highly mobile and can walk over rough terrain with their discontinuous support motion. As a legged robot, quadruped robot, such as BigDog [1], Cheetah [2], Anymal [3] and HyQ [4], have a great potential for applications in complex environments.

Recently, agility and dynamic skills have been demonstrated by the Massachusetts Institute of Technology’s (MIT) Cheetah robots, which are actuated with a unique electric actuation system [2]. Cheetah 1 [5], a planar quadruped platform can run up to speeds at 3.2 m/s in plane experiments, using proprioceptive touchdown feedback and programmable leg compliance. Cheetah 2 is skilled enough to bound over obstacles using an MPC controller [6]. A time-switched impulse scaling principles, which generalize the control parameters, allowing the Cheetah 2 robot to run at speeds of up to 6.4 m/s [7,8]. The event-based framework based on the contact detection algorithm [9,10] allows the Cheetah 3 robot to leap and gallop across unstructured terrain with obstacles and climb a staircase littered with debris without relying on external sensing information of the environment. The IIT HyQ quadruped has shown a control architecture for quadrupedal quasi-static walking over challenging inclined terrain [11]. Dynamic trotting gaits have also been demonstrated in [12] using bio-inspired nonlinear oscillators.

Using inverse dynamics for a floating base multi-body systems (MBS) force control has become a popular topic. Khatib [13] was the first to propose this broad category. The advantage of this approach is that the control problem change under a transformation to coordinates of the task-space coordinates. It is effective for applications involving the end-effector at contact with a highly stiff surface. Joint torques are computed using the inertia matrix, which is required for pseudoinverse computations and projections. Taking the unilateral contact and friction into account and based on a convex optimization to solve an optimal control problem, the reference motion in the operational space of the robot can be tracked perfectly [14]. Although detailed formulations differ, numerous optimization problems are phrased as cascades of quadratic programming (QP) to solve the floating base inverse dynamics. Hierarchical operational-space inverse dynamics control framework rely on a least-squares method specifically considering the effects of contact interactions has been proposed and applied to StarlETH [15]. Ref. [16] demonstrates a control architecture rely on a whole-body control framework that allows ANYmal to execute dynamic gaits, such as pronking and running trot.

A successive approach for whole-body humanoid locomotion control based on centroidal momentum rise in popularity [17,18], because it is more robust to disturbances than inverse dynamics-based control strategies [19]. The momentum of a floating base MBS is comprised of its linear momentum and angular momentum. The centroidal momentum of a floating base MBS is added to its linked momentum, which is referred to as the robot centroid or Center of Mass (CoM). Kajita was the first to propose a approach to generate whole body motion of robot with given reference momentum, named this approach the Resolved Momentum Control [20]. The humanoid robot HRP-2 has demonstrated its application for balance maintenance and the HRP-2LR robot [21] achieved running gaits based on the Resolved Momentum Control. Orin and Goswami indicated that the centroidal momentum are linearly related to joint velocities [22]. They introduced the centroidal momentum matrix (CMM), which represents the linear sensitivity of centroidal momentum to the joint velocities. They recognized the fact that this matrix is a function of the inertia and Jacobian matrix. Specialized algorithms to compute the CMM were originally presented by Wensing and Orin in [23]. They noted that the CMM and the bias force can be computed though the mass matrix, Coriolis force and the kinematics of the floating-base. Based on this work, the humanoid robot achieves a dynamic kick and dynamic jump in the simulation.

The novel contributions of this paper include:

• The presentation of trotting gait with a quadruped robot based on centroidal momentum dynamics controller. While the robot is under-actuated, we sacrifice the lateral component of linear momentum to aid stability of Capture Point trajectory tracking. The controller is robust to errors in the inertial parameters (such as mass) of the robot.
• Using Capture Point methodology to generate the desired motion of the quadruped robot.
• We demonstrate a method to establish the dynamics equations of a floating base systems into a closed-form, which is convenient to compute and analyze the centroidal momentum dynamics.

The remainder of this paper is organized as follows. Section 2 demonstrates the framework of the centroidal momentum control application for quadrupedal robot dynamic motion. The method demonstrated in the previous sections are tested in simulations and on the physical prototype in Section 3. Finally, the conclusions of this paper are presented in Section 4.

2. Methods

The following subsection introduces our control framework. The dynamics equations of the quadruped robot are established in Section 2.1 and then the centroidal dynamics of the quadruped robot are obtained in Section 2.2. Section 2.3 demonstrates the framework of centroidal momentum control application for quadrupedal robot dynamic motion.

2.1. Dynamics Equation of Floating Base Systems

Dynamic-based approaches play an important role in legged robot dynamic locomotion because they have strong disturbance resisting ability. The modelling presents an inherent complexity, mainly due to the joint variables must satisfy several loop-closure constraint equations. Several algorithms have been used to the dynamic modelling of floating base systems. In this paper, we employ the Newton–Euler inverse dynamics algorithm organized into a closed-form [24] to establish the dynamics equation of the quadruped robot.

Consider the quadrupedal robot shown in Figure 1. For modelling purposes, we attach a base reference frame to the robot and assume that the base frame can freely move relative to a fixed inertial frame. Figure 1 illustrates this representation by showing the 6 virtual degrees of freedom (${\theta }_b\in {\mathbb{R}}^6$ is the position and orientation of the coordinate system attached to the robot base, measured with respect to the fixed inertial frame).

As shown in Figure 2. The system can be partitioned into four subsystems (the inertia tensor for the body is also divided into four parts). We can write the dynamic equation for the four chains as follows:

$$H_i\left({\theta }_i\right){\ddot{\theta }}_i+C({\theta }_i,{\dot{\theta }}_i)+G_i\left({\theta }_i\right)={S_i}^T{\tau }_i+{J_i}^T{f}_i,i=RF,LF,LH,RH$$

(1)

where $H_i\left({\theta }_i\right)\in {\mathbb{R}}^{9\times 9}$, $C_i({\theta }_i,{\dot{\theta }}_i)\in {\mathbb{R}}^9$, $G_i\left({\theta }_i\right)\in {\mathbb{R}}^9$, ${\theta }_i={\left[\begin{array}{ccc}{r_b}^T& {q_b}^T& {q_i}^T\end{array}\right]}^T\in {\mathbb{R}}^9$, $S_i=\left[0_{3\times 6}{I}_{3\times 3}\right]$ and ${\tau }_i\in {\mathbb{R}}^3$ denote the mass matrix, the force vector as the sum of the Coriolis and centrifugal forces, gravitational forces, the actuated joint selection matrix and the actuator torque, respectively. Limbs in contact with the ground are modelled as point contacts (with 3 forces). The Ground Reaction Forces (GRFs) at the contact points are mapped to the joint space torques through the Jacobian $J_{stance,i}=\left[J_{stance,i}^{body}{J}_{stance,i}^{leg}\right]\in {\mathbb{R}}^{3\times 9}$.

For floating-base systems, such as a quadruped robot, the dynamic in Equation (1) can be divided into floating-base and actuated components

$$\left[\begin{array}{cc}{H}_i^{11}\left({\theta }_i\right)& H_i^{1*}\left({\theta }_i\right)\\ H_i^{*1}\left({\theta }_i\right)& H_i^{**}\left({\theta }_i\right)\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_b\\ {\ddot{q}}_i\end{array}\right]+\left[\begin{array}{c}{C}_i^1({\theta }_i,{\dot{\theta }}_i)\\ C_i^{*}({\theta }_i,{\dot{\theta }}_i)\end{array}\right]+\left[\begin{array}{c}{G}_i^1\left({\theta }_i\right)\\ G_i^{*}\left({\theta }_i\right)\end{array}\right]=\left[\begin{array}{c}J{{}_{stance,i}^{body}}^T{f}_i\\ {\tau }_i+J{{}_{stance,i}^{leg}}^T{f}_i\end{array}\right]$$

(2)

where ${\theta }_b={\left[\begin{array}{cc}{r_b}^T& {q_b}^T\end{array}\right]}^T\in {\mathbb{R}}^6$. All the four subsystems are clustered together, and we obtain the dynamics equation of the quadruped robot in a closed form

$$H\left(\theta \right)\ddot{\theta }+C(\theta ,\dot{\theta })+G\left(\theta \right)=S^T\tau +J^{T}f$$

(3)

where

$$H\left(\theta \right)=\left[\begin{array}{ccccc}{\displaystyle \sum _{i=RF}^{RH}}{H}_i^{11}\left({\theta }_i\right)& H_{RF}^{1*}\left({\theta }_{RF}\right)& H_{LF}^{1*}\left({\theta }_{LF}\right)& H_{LB}^{1*}\left({\theta }_{LB}\right)& H_{RB}^{1*}\left({\theta }_{RB}\right)\\ H_{RF}^{*1}\left({\theta }_{RF}\right)& H_{RF}^{**}\left({\theta }_{RF}\right)& 0& \dots & 0\\ H_{LF}^{*1}\left({\theta }_{LF}\right)& 0& H_{LF}^{**}\left({\theta }_{LF}\right)& \dots & 0\\ H_{LB}^{*1}\left({\theta }_{LB}\right)& ⋮& ⋮& H_{LB}^{**}\left({\theta }_{LB}\right)& 0\\ H_{RB}^{*1}\left({\theta }_{RB}\right)& 0& 0& 0& H_{RB}^{**}\left({\theta }_{RB}\right)\end{array}\right]\in {\mathbb{R}}^{18\times 18}$$

(4)

$$C\left(\theta \right)=\left[\begin{array}{ccccc}{\displaystyle \sum _{i=RF}^{RH}}{C}_i^1({\theta }_i,{\dot{\theta }}_i),& C_{RF}^{*}({\theta }_{RF},{\dot{\theta }}_{RF}),& C_{LF}^{*}({\theta }_{LF},{\dot{\theta }}_{LF}),& C_{LB}^{*}({\theta }_{LB},{\dot{\theta }}_{LB}),& C_{RB}^{*}({\theta }_{RB},{\dot{\theta }}_{RB})\end{array}\right]\in {\mathbb{R}}^{18}$$

(5)

$$G\left(\theta \right)=\left[\begin{array}{ccccc}{\displaystyle \sum _{i=RF}^{RH}}{G}_i^1\left({\theta }_i\right),& G_{RF}^{*}\left({\theta }_{RF}\right),& G_{LF}^{*}\left({\theta }_{LF}\right),& G_{LB}^{*}\left({\theta }_{LB}\right),& G_{RB}^{*}\left({\theta }_{RB}\right)\end{array}\right]\in {\mathbb{R}}^{18}$$

(6)

$$\theta ={\left[\begin{array}{ccccc}{{\theta }_b}^T& {q_{RF}}^T& {q_{LF}}^T& {q_{LB}}^T& {q_{RB}}^T\end{array}\right]}^T\in {\mathbb{R}}^{18}$$

(7)

$$\tau ={\left[\begin{array}{cccc}{{\tau }_{RF}}^T& {{\tau }_{LF}}^T& {{\tau }_{LB}}^T& {{\tau }_{RB}}^T\end{array}\right]}^T\in {\mathbb{R}}^{12}$$

(8)

where m is the number of limbs in contact, and the Jacobian $J\in {\mathbb{R}}^{k\times 18}$ projects the Ground Reaction Forces (GRFs) $f\in {\mathbb{R}}^k(k=3m)$ at the contact points to the joint space torques. $S=\left[0_{12\times 6}{I}_{12\times 12}\right]$.

Equation (3) can be written explicitly as

$$\left[\begin{array}{cc}{H}^{11}\left(\theta \right)& H^{1*}\left(\theta \right)\\ H^{*1}\left(\theta \right)& H^{**}\left(\theta \right)\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_b\\ \ddot{q}\end{array}\right]+\left[\begin{array}{c}{C}^1(\theta ,\dot{\theta })\\ C^{*}(\theta ,\dot{\theta })\end{array}\right]+\left[\begin{array}{c}{G}^1\left(\theta \right)\\ G^{*}\left(\theta \right)\end{array}\right]=\left[\begin{array}{c}J{{}_{stance}^{body}}^{T}f\\ \tau +J{{}_{stance}^{leg}}^{T}f\end{array}\right]$$

(9)

2.2. Centroidal Dynamics

The centroidal momentum of the quadruped robot is the total momenta of its links, defined in centroidal frame. The centroidal frame locate in the instantaneous center of mass (COM) position and aligned with the world frame [18]. The centroidal momentum of the quadruped robot consists of its linear momentum $l\in {\mathbb{R}}^3$ and its angular momentum $k\in {\mathbb{R}}^3$

$$h=\left[\begin{array}{c}l\\ k\end{array}\right]=A\dot{\theta }\in {\mathbb{R}}^6$$

(10)

where A is called the CMM. The rate of the centroidal momentum is often required for dynamic whole-body controllers. The differentiation of (10) can be written in the following forms:

$$\dot{h}=A\ddot{\theta }+\dot{A}\dot{\theta }$$

(11)

These centroidal dynamics are in connection with the external wrench (force/torque) on the quadrupedal robot. The Newton’s and Euler’s laws states that $\dot{h}$ be equal to the net external wrench (force/torque about the instantaneous CoM) on the quadrupedal robot.

$$A\ddot{\theta }+\dot{A}\dot{\theta }=W_g+\sum _{i=1}{W}_{grd,i}$$

(12)

where $W_g={\left[\begin{array}{cc}m{g}^T& 0_{1\times 3}\end{array}\right]}^T$ is the wrench due to gravity, and $W_{grd,i}$ are the ground reaction wrenches exerted upon the limbs in contact with the ground. Note that the rate of the change of momentum is unaffected by forces or torques internal to the quadrupedal robot. The CMM A and bias $\dot{A}\dot{\theta }$ can be calculated, using the mass matrix $H\left(\theta \right)$ and the Coriolis term $C\left(\theta \right)$ through the spatial transform ${}^1{X}_G^T$, which was proposed by Wensing in [23]

$$A={}^1{X}_G^T\left[\begin{array}{ccccc}\sum _{i=RF}^{RH}{H}_i^{11}\left({\theta }_i\right)& H_{RF}^{1*}\left({\theta }_{RF}\right)& H_{LF}^{1*}\left({\theta }_{LF}\right)& H_{LB}^{1*}\left({\theta }_{LB}\right)& H_{RB}^{1*}\left({\theta }_{RB}\right)\end{array}\right]$$

(13)

$$\dot{A}\dot{\theta }={}^1{X}_G^T\left[\sum _{i=RF}^{RH}{C}_i^1({\theta }_i,{\dot{\theta }}_i)\right]$$

(14)

2.3. Application to Quadrupedal Robot Dynamic Motion

This section demonstrates the application of centroidal momentum control to the dynamic motion of a quadruped robot. Figure 3 shows the block diagram of our control framework. The set of future footprints are given from high level behaviour, and the reference walking pattern of the quadruped robot is generated based on the Capture Point dynamics in Section 2.3.1. The motion of the robot is projected according to the walking pattern. The according reference rate of change of the centroidal momentum is formulated as a feedback control task derived from the CoM motions of the quadrupedal robot in Section 2.3.2. We design a QP to solve an optimal control problem to track the reference rate of change of the centroidal momentum as closely as possible while satisfying the constraints in Section 2.3.3. The joint accelerations and the Ground Reaction Forces (GRFs) outputted from the quadratic programme solver are used to compute the reference joint control torques using an inverse dynamics algorithm. The application of the whole-body control system based on centroidal momentum to control a quadruped robot dynamic motion is shown in Section 2.3.4.

2.3.1. Walking Pattern based on Capture Point Dynamics

The walking pattern is defined as a set of time series of joint angles for the desired walking [25]. To ensure stable walking, a reduced model of the robot is indispensable. Recently, the linear inverted pendulum model (LIPM) required for the control of bipedal walking robots rise in popularity [26,27,28,29,30]. Once the walking pattern is using simple models, such as the LIPM, the motion of the robot can be equivalent to the motion of the centre of mass of the robot and the motion of the supporting foot. In general, the walking pattern was generated with a predetermined footstep position. If a set of time series of robot footstep positions is given, the set of time series of the CoM trajectory of the robot is the general walking pattern [31].

In the simplified model, when a quadruped robot is supporting its body on two legs, the behaviour of a pair of legs acting in unison can be represented by an equivalent virtual leg [32]. In this paper, we extend the concept of virtual leg. No matter how many legs in contact with the ground, it can be equivalent to a virtual leg, as depicted in Figure 4. GRFs $F_i$ at the contact point can be mapped to the CoM though the matrix B.

$$\underset{B}{\underbrace{\left[\begin{array}{ccc}{I}_3& \cdots & I_3\\ \left[P_1-r\right]\times & \cdots & \left[P_i-r\right]\times \end{array}\right]}}\left[\begin{array}{c}{F}_1\\ ⋮\\ F_i\end{array}\right]=\left[\begin{array}{c}F\\ \tau \end{array}\right]$$

(15)

where $F={\left[\begin{array}{ccc}{f}_x& f_y& f_z\end{array}\right]}^T$, $\tau ={\left[\begin{array}{ccc}{\tau }_x& {\tau }_y& {\tau }_z\end{array}\right]}^T$. We assume that the virtual foot is located at the midpoint between the two supporting foot, which strike the ground recnetly. Its dominant dynamics can be represented by a single inverted pendulum, which is comprised of a point mass and a massless telescopic leg in contact with the ground. According to LIPM, the relationship between the CoM and $P_{vl}$ can be expressed as follows:

$$\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right]=\frac{1}{\ddot{z}+g}\left[\begin{array}{c}x(\ddot{z}+g)+z\ddot{x}\\ y(\ddot{z}+g)+z\ddot{y}\end{array}\right]-\frac{1}{m\left(\ddot{z}+g\right)}\left[\begin{array}{c}{\tau }_y\\ {\tau }_x\end{array}\right]$$

(16)

where x, y and z denote the horizontal, lateral and vertical CoM position, $p_x$ and $p_y$ denote the horizontal and lateral position of the virtual leg, ${\tau }_y$ and ${\tau }_x$ denote the torque about the y axis and x axis of CoM, m is the total mass of the quadruped robot and g is the acceleration due to gravity.

The Linear Inverted Pendulum Model, described by Kajita et al. [26,27] assumes that the height of the CoM remains constant and the change in the rotational momentum about the CoM is zero. Based on these assumptions, the dynamic equations of the robot are linear and decoupled, and the relationship between the CoM and P can be written:

$$\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\end{array}\right]=w^2\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right]\right)$$

(17)

where $w=\sqrt{g/z_0}$ is the natural frequency of the pendulum.

Pratt et al. derive the unstable part of LIPM from its orbital energy and named it the Capture Point [33]. This is the point on the floor where the robot (modelled as an LIPM) has to step to come to a complete rest. Equivalent values of the Capture Point have been analysed by Hof as the ‘extrapolated center of mass’ in [34] and DCM by Takenaka et al. in [35]. The Capture Point is comprised of the horizontal CoM position, velocity, and the natural frequency of the pendulum. It can be expressed as:

$$\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right]=\left[\begin{array}{c}x\\ y\end{array}\right]+\frac{1}{w}\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]$$

(18)

The equation of the Capture Point can be rewritten as

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=-w\left(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right]\right)$$

(19)

We find that x and y has a stable first-order open loop dynamics with a time constant $\frac{1}{w}$.

Inserting Equations and (19) in Equation (17) then gives

$$\left[\begin{array}{c}{\dot{\xi }}_x\\ {\dot{\xi }}_y\end{array}\right]=w\left(\left[\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right]-\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right]\right)$$

(20)

The Capture Point $\xi ={\left[\begin{array}{cc}{\xi }_x& {\xi }_y\end{array}\right]}^T$ has an unstable first-order open loop dynamics. The CoM naturally follows the Capture Point, and the Capture Point dynamics is pushed away by the $P={\left[\begin{array}{cc}{p}_x& p_y\end{array}\right]}^T$ with an unstable first-order dynamics. If the Capture Point dynamics is controlled to be stable, the CoM dynamics are naturally stabilized. The equation of (20) can be solved into the time domain in the form of (21) to calculate the Capture Point value in the futures to produce a Capture Point trajectory, which corresponds to a constant position of the virtual foot of the pre-planned future foot positions $P_{d,i}$ by shifting the Capture Point during a step from one footprint to the next.

$$\xi \left(t\right)=P_{d,i}+e^{wt}\left({\xi }_{ini,i}-P_{d,i}\right)$$

(21)

The desired Capture Point locations at the beginning of each step ${\xi }_{d,ini,i}$ are found via recursion, which was proposed by Englsberger [28].

$${\xi }_{d,eos,i-1}={\xi }_{d,ini,i}=P_{d,i}+e^{-wt}\left({\xi }_{d,eos,i-1}-P_{d,i}\right)$$

(22)

Given a desired set of time series of quadruped robot footstep positions $P_d$ of a virtual leg, the Capture Point trajectory can be obtained from Equation (21). The desired velocity $v={\left[\begin{array}{cc}\dot{x}& \dot{y}\end{array}\right]}^T$ of the CoM can be obtained by Equation (19).

2.3.2. Desired Centroidal Momentum for Motion Control

The motion of the robot is determined by the desired linear and angular momentum rate change. For angular momentum, we employ the PD feedback control policy to regulate the CoM orientation. When the robot is walking in a straight line, $q_{d,b}$ is set to zero.

$${\dot{k}}_{G,d}={\overline{I}}_G\left[-K_{P,k}\left(q_b-q_{d,b}\right)-K_{D,k}{\dot{q}}_b\right]$$

(23)

To encode a whole-body rotation given the desired motion plan, ${\overline{I}}_G$ can be found from the mass matrix by computing the centroidal composite-rigid-body inertia of the system $I_G$ about the CoM.

$$\begin{array}{cc}\hfill I_G& ={}^1{X}_G^T\left[\sum _{i=RF}^{RH}{H}_i^{11}\left({\theta }_i\right)\right]{}^1{X}_G\hfill \\ & =\left[\begin{array}{cc}M& 0\\ 0& {\overline{I}}_G\end{array}\right]\hfill \end{array}$$

(24)

For linear momentum, a commanded ${\dot{l}}_{G,d}$ is formed from the PD control on the desired CoM

$${\dot{l}}_{G,d}=M{\left[-K_{D,l}^x\left({\dot{r}}_b^x-{\dot{r}}_{b,d}^x\right),-K_{D,l}^y\left({\dot{r}}_b^y-{\dot{r}}_{b,d}^y\right),-K_{P,l}^z\left(r_b^z-r_{b,d}^z\right)-K_{D,l}{\dot{r}}_b^z\right]}^T$$

(25)

The desired velocity of the CoM ${\dot{r}}_{b,d}^x$ and ${\dot{r}}_{b,d}^y$ is the output of the walking pattern, which is described in Section 2.3.1. The CoM height $r_{b,d}^z$ is often assumed to be constant.

2.3.3. QP Formulation

Given a admissible momentum rate change ${\dot{h}}_{G,d}={\left[\begin{array}{cc}{\dot{l}}_{G,d}^T& {\dot{k}}_{G,d}^T\end{array}\right]}^T$, we design a QP to solve for a vector ${\chi }_d={\left[\begin{array}{cc}{\ddot{\theta }}_d^T& f_d^T\end{array}\right]}^T\in {\mathbb{R}}^{18+k}$ of the desired joint accelerations and GRFs for the full robot dynamics that minimizes Equation (27), which is a quadratic motion cost to track the reference rate of change of the centroidal momentum as closely as possible while satisfying the dynamic, input, and contact constraints of the full quadruped robot dynamics.

$$\underset{E}{\underbrace{\left[\begin{array}{cc}A& 0\end{array}\right]}}\chi =\underset{b}{\underbrace{{\dot{h}}_{G,d}-\dot{A}\theta }}$$

(26)

Minimize

$${\left(E\chi -b\right)}^{T}Q\left(E\chi -b\right)+f^{T}Wf$$

(27)

Subject to

$$\left[\begin{array}{ccc}{H}_{11}& H_{1*}& -J_{body}^T\end{array}\right]{\chi }_d=C_1\dot{q}+G_1$$

(28)

$$\left[\begin{array}{cc}{J}_{stance}& 0_{K\times 24-K}\end{array}\right]{\chi }_d=-\left[\begin{array}{cc}{\dot{J}}_{stance}& 0_{K\times 12-K}\end{array}\right]\dot{\theta }-\alpha \left[\begin{array}{cc}{J}_{stance}& 0_{K\times 12-K}\end{array}\right]\dot{\theta }$$

(29)

$$\left[\begin{array}{cc}{J}_{swing}& 0_{18+K\times 18+K}\end{array}\right]{\chi }_d=-\left[\begin{array}{cc}{\dot{J}}_{swing}& 0_{K\times K}\end{array}\right]\dot{\theta }+p_{swing}$$

(30)

$$f_{i,d}^z\ge 0,\left|\mu f_{i,d}^z\right|\ge f_{i,d}^x,\left|\mu f_{i,d}^z\right|\ge f_{i,d}^y$$

(31)

$${\tau }_{min}-C_{*}\dot{q}-G_{*}\le \left[\begin{array}{ccc}{H}_{*1}& H_{**}& -J_{leg}^T\end{array}\right]{\chi }_d\le {\tau }_{max}-C_{*}\dot{q}-G_{*}$$

(32)

The matrices Q and W are weighting matrices. The relationship between the accelerations $\ddot{\theta }$ and the GRFs f can be found though the floating base system dynamics equation constraint (28). Since legs in contact with the ground are not allowed to move, Equation (29) is a no-slip constraint on the foot contacts, requiring that their acceleration be negatively proportional to the velocity. To improve the track of the desired motion determined by the high level controller, we constrain the joint accelerations and by implementing operational space controllers with feed-forward reference acceleration and a motion dependent feedback state in the constraint in Equation (30). Where

$${\ddot{p}}_{swing}={\ddot{p}}_{swing,d}+k_p(p_{swing,d}-p_{swing})+k_i\int (p_{swing,d}-p_{swing})dt+k_d({\dot{p}}_{swing,d}-{\dot{p}}_{swing})$$

(33)

To avoid slipping, the constraint ensures that contact forces should be constrained to lie in the friction cone, which is aligned normal to the contact surface, where $\mu$ is the friction coefficient. To write these as linear constraints, we approximate the friction cones with pyramids, which was proposed by Dario Bellicoso in [36]. Equation (32) ensure that the joint actuation torques cannot exceed the rated torques.

The formulation is solved based on Active-set method. This QP is solved for every controller time step for the joint acceleration and the GRFs. Given the desired joint acceleration and the GRFs, we compute the reference joint torques though

$$\tau =\left[\begin{array}{ccc}{H}_{*1}& H_{**}& -J_{leg}^T\end{array}\right]{\chi }_d+C_{*}\dot{q}+G_{*}$$

(34)

2.3.4. Whole Body Control System

A feedforward and feedback control system is designed to generate a robust walking gait in Figure 3. From a set of time series of quadruped robot footstep positions, a target Capture Point reference $\xi$ is calculated. The desired velocity of the CoM can be obtained by Equation (17) based on the current robot state x (Section 2.3.1). The desired momentum rate of change is provided by Equations (23) and (25) (Section 2.3.2). Meanwhile, the swing foot trajectory based on quintics order polynomials will be generated. Given a desired rate of change of the centroidal momentum, and taking the swing foot motion, floating base system dynamics equation, limitations on the GRF, joint torque limits and contact motion as constraints, we set up a QP (Section 2.3.3). The QP solver outputs the joint accelerations and GRFS, which are then used to calculate the desired joint torques using an inverse dynamics algorithm via Equation (34). While in simulations it is possible to directly apply the joint torques computed by the whole-body control system, the physical quadruped robot can only be controlled through motor current commands. Because of the errors between the multibody model and the real robot, during the swing motion we increase the PD gains of the swing leg joints to improve the tracking capabilities. Overall, we compute the desired torques ${\tau }_{swing,d}$ though:

$${\tau }_{swing,d}=K_{P,ff}{\tau }_{swingff}+{\tau }_{swingfd}$$

(35)

$${\tau }_{swingfd}=-K_{P,swing}({\theta }_{swing}-{\theta }_{swing,d})-K_{D,swing}({\dot{\theta }}_{swing}-{\dot{\theta }}_{swing,d})$$

(36)

where ${\tau }_{swingff}$ is the swing leg inverse dynamics feed-forward term computed using Equation (34) and ${\tau }_{swingfd}$ is the swing leg joints PD feed-back term computed via a standard PD controller using Equation (36), where $K_{P,ff}>0$, $K_{P,swing}>0$ and $K_{D,swing}>0$ are the feed-forward proportional gain, feed-back proportional gain and differential gain, respectively. The desired swing leg joint angle positions and velocities ${\theta }_{swing,d}$ and ${\dot{\theta }}_{swing,d}$ can be computed though inverse kinematics. This control law is a trade-off between the torque control and joint position control. The torque control provide compliant motions and robustness to external perturbations but is sensitive to dynamics model errors (usually uses a mix of CAD-based data and estimated parameters) and forces from joint friction, whereas the velocity control results in a high impedance behaviour that is insensitive to dynamics model errors. By the use of the inverse dynamics feed-forward term and the joint position control gains, we were able to strike a compromise between the tracking capabilities of the desired swing leg motion and being reasonably compliant to external disturbances that let the quadruped robot to move on a variety of terrains.

3. Experimental Results and Discussion

The method described in the previous sections was tested in simulations and on physical prototype. We demonstrate successful trotting in simulation and with a physical prototype.

A high level behaviour defines the sequence of the swing and stance modes for each leg with respect to time, such as a trotting gait, as demonstrated in Figure 5. The dark areas indicate the fraction of the stride when a leg is in the swing phase, which is characterized by the relative timing of the lift-off and touch-down events. A dark line indicates that a leg is in swing mode. It is interesting to note that a short with four contact points phase is planned to be allowed, since trotting in double support is truly under-actuated and over-constrained at the same time (rank (B) = 5) and five of the floating base’s six degrees of freedom can be controlled though the GRFS. An over-constrained situation is the result of a short four contact points support phase, which provides compliant motions and stable walking over rough terrain. When the robot is in over-constrained situation, there are many ways to adapt the robot motion behavior to track the motion task [37]. In this paper, with all kinds of constraints are satisfied, the highest priority was assigned to track the desired rate of change of centroidal momentum. We address the highest priority task and try to minimize the GRFs.

During all phases except the double support state, only the diagonal elements of the weighting matrices Q in the QP in Equation (27) are set to 1. During the double support state, the weighting matrices $Q=diag\left[1,0,1,1,1,1\right]$ so that the whole-body control system will not take the lateral component of linear momentum rate of change into account. The weighting matrices for the GRFs $W=0.01I$.

3.1. Simulation

The method described in the previous sections has been performed in simulation. The robot walks forward. The desired step length was set to 10cm and the desired time per step to 0.25 s. (blue curve depicted in Figure 6). The simulation results are displayed in Figure 6. The method described in this paper show a reasonably good tracking of the desired Capture Point trajectory (yellow curve in Figure 6). While walking steadily, the robot is able to robustly trotting with an average speed of 0.4 m/s, as shown in Figure 7. We note that while the robot trots in place the disturbance to its attitude is very small; the pitch, the roll and the yaw of the robot oscillate with an amplitude less than 0.01 rad in Figure 8.

Snapshots of the robot trotting in the simulation are shown in Figure 9. The joint torques of the left front leg of the quadruped robot during a forward velocity of 0.4 m/s are shown in Figure 10. According to Equation (9), ${\tau }_m={\tau }_{*}-J{{}_{stance}^{leg}}^{T}f$, where ${\tau }_{*}=\left[\begin{array}{cc}{H}^{*1}\left(\theta \right)& H^{**}\left(\theta \right)\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_b\\ \ddot{q}\end{array}\right]+C^{*}(\theta ,\dot{\theta })+G^{*}\left(\theta \right)$. When the left front leg is in the swing phase, ${\tau }_m={\tau }_{*}$.When the left front leg is in the stance phase, It is observed that ${\tau }_{*}$ provides a minority of the total torque whereas $-J{{}_{stance}^{leg}}^{T}f$ (torque against the GRFs at the foot) play a prominent role, indicating that the torque is insensitive to the dynamics model errors. This is why we chose to use a PD feed-back joint position control only for legs in swing mode.

Figure 11 show the phase portraits of the states. It is clear that the solution converges to a limit cycle.

In order to test the controller is robust to errors in the inertial parameters (such as mass) of the robot, we changed the mass of the robot and the controller remains the same. Figure 12 summarizes the results. The results show a very good tracking of the reference Capture Point trajectories which indicated that the centroidal momentum-based controller based on centroidal momentum is robust to the robot mass errors.

3.2. Experiment

The developed control algorithms were tested on a physical prototype of a quadruped robot.

Table 1. Physical Robot Parameters.

Parameter	Size	Mass
Body	0.5 × 0.37 × 0.24	12 Kg
Hip Length	0.07 m	1.5 Kg
Thigh Length	0.2 m	0.9 Kg
Shank Length	0.2 m	0.75 Kg

lists the detailed Physical Robot Parameters. The physical prototype has three degrees of freedom per leg actuated by electric motors. Each actuator can be controlled at 200 Hz by a torque control loop from a Linux host computer through EtherCAT communication. Four contact sensors (one on each foot), positioned in each foot, and an IMU is fixedly installed for measuring the body orientation and angular velocity. The CoM position and linear velocity is currently estimated by means of the robot’s kinematics.

The task for the robot was exactly the same as in the simulations: walking forward with a stride of 10 cm and a step time of 0.25 s. The results of the experiments are shown in Figure 13, Figure 14, Figure 15 and Figure 16.

Several limitations of the the physical prototype affect the performance of the control framework. Communication latency is one of the main sources of limitations, which allowed us to tune down the gains for controller. State estimation is another sources of limitations. Now, linear velocity is currently estimated by means of the robot’s kinematics. We use low-pass filters to process the sensor noise from the joint velocities, which lead to phase delay. In the future, we may employ a state estimator based on an extended Kalman filter.

4. Conclusions

This paper has demonstrated a whole-body control method based on centroidal momentum dynamics and its successful application to a quadruped robot for walking tasks. Our method explored using QP by controlling both the rates of change for the linear and angular momenta of the robot.

The characteristic features of the presented method are as follows:

• We use the concept of a virtual leg when the quadruped robot performs dynamic gaits, such as a trot. Therefore, one-leg algorithms can provide control. The desired motion of the quadruped robot from the walking pattern is based on Capture Point methodology, which describes the natural dynamics of a simplified dynamic model (LIPM).
• The controller based on centroidal momentum dynamics tracks the reference motion from the walking pattern. Our method relies on a QP that solves an optimal control problem to track the reference rate of change of the centroidal momentum as closely as possible while satisfying the dynamic, input, and contact constraints of the full quadruped robot dynamics.
• During the double support state, the weighting of the lateral component of the linear momentum rate of change is set to 0 so that the controller will not take the lateral CoM motion of the quadruped robot into account, since the quadruped robot in this state is underactuated.

We have demonstrated the execution of dynamic gaits, such as a trot, in simulation and in a physical prototype. The simulation and experiment results show that the quadruped robot trotted forward at a speed of ~0.4 m/s with a low amplitude oscillation.