Multi-Objective Optimal Torque Control with Simultaneous Motion and Force Tracking for Hydraulic Quadruped Robots

Abstract

Model-based force control for motion and force tracking faces significant challenges on real quadruped platforms due to the apparent model inaccuracies. In this paper, we present a multi-objective optimal torque control for hydraulic quadruped robots under significant model errors, such as non-modelable hydraulic components, linearization, disturbances, etc. More specifically, the centroidal dynamics are first modeled to project the dynamics of the floating-based whole-body behaviors to the centroidal frame. Model error compensation mechanisms are subsequently developed to track the reference motion of the CoM, torso, and foot-end trajectories, which are mapped into the joint space. Furthermore, a multi-objective optimal torque control scheme is formulated using quadratic programming (QP) to coordinate follow the reference motion and ground reaction forces simultaneously while satisfying all constraints. Finally, we present a series of simulations as well as experiments on a real hydraulic quadruped platform, EHbot. The results demonstrate that the proposed torque control scheme is robust to large model inaccuracies and improves the performance of the overall system.

1. Introduction

As an important branch of the field robots, legged robots, especially hydraulic quadruped robots, have superior abilities over wheeled and tracked counterparts regarding to their dynamic balance, load capacity, and unstructured terrain adaptation [1,2,3]. Those distinct characteristics take their first steps outside the laboratories and into the real world. Accordingly, legged robots possess profound potential in applications in unknown and high-risk prospects such as hazardous environment reconnaissance, fire rescuing, military missions, and nuclear radiation scenarios [4,5]. The specific challenges for these scenario applications are the demands for rapid response to external disturbances and adaptation to unknown environmental changes. Due to the high dimensional degrees of freedom (DoFs), position controllers make complex behaviors kinematically feasible. However, they are inevitably restricted to quasi-static regions [6].

In recent years, researchers have explored control systems shifting away from position control paradigms to force control to deal with these challenges [7]. Generally speaking, the solution of force control, especially model-based torque control, can be further divided into joint space and operational space controllers. The former is a classical approach, widely applied to fixed-based manipulators. In the field of legged robots, it is extended and used to track the joint trajectories via inverse kinematics by feed-forward torque command calculating [8]. Thereby, the legged robots can achieve the desired motion [9]. The latter provides an elegant solution to generate whole-body behaviors in intuitive motion spaces, which is more suitable for legged locomotion [10]. One of the most well-known methods is the one proposed by Khatib [11,12]. Specifically, by defining the task priorities in a hierarchical manner, operational space control enables various dynamic behaviors while maintaining fundamental constraints, such as contacts, joint limits, and dynamic balance, simultaneously. The hierarchically prioritized operational space control allows multiple tasks to be independent controlled and executed using dynamically-consistent null-space projections [13,14].

In view of the diversity of legged robot configurations and the complexity of whole-body dynamics models, researchers have been devoted to leveraging the advantages of legged robots to make them more flexible [1,15]. Whole-body control (WBC) provides a unified framework that significantly develops and utilizes the redundant DoFs of high-dimensional floating-base system kinematics and the richness of multi-rigid-body (MRB) dynamics [16,17]. Due to the dynamically consistent formulation and general framework, WBC has the ability to guarantee the accurate tracking of robot trajectory and the flexible interaction with the time-varying contact of the environment through optimization approaches [18,19,20].

Apparently, dynamics modeling is a preliminary process for realizing to implement model-based force control. However, the equation of motion in legged robots is complex, with multiple DoFs. Orin et al. [21,22] proposed the concept of centroidal dynamics, which is the derivative of the centroidal momentum. The centroidal momentum provides a simplified formulation that each individual link momentum is gathered at the center of mass (CoM), where the robot’s aggregated linear and angular momentum are naturally defined, and the resultant gravity is acting on [23,24].

Centroidal dynamics has recently been a popular simplification in the robotics community for bipedal and quadruped locomotion. Inspired by this approach, Dai et al. [25] proposed a whole-body motion planning for legged locomotion with its full kinematics and simplified centroidal dynamics. The approach has been further validated on Atlas and LittleDog. With the concept of virtual leg and linear inverted pendulum model for quadruped robots, a whole-body control approach based on the centroidal dynamics was presented in [26]. Given unexpected external forces during walking, recently, [27] proposed a stabilization strategy using impedance control and optimization-based whole-body control with centroidal dynamics.

Unfortunately, for the hydraulic quadruped platform, as shown in Figure 1 [28], its hydraulic hoses and hydraulic cylinders cannot be accurately modeled into the MRB floating-based dynamics formula [29]. In addition, in conjunction with the inherent strong nonlinear characteristics of hydraulic systems, it is difficult to use whole-body dynamics for the control of hydraulic quadruped robot prototypes with the large model error. Kundesma, Atlas team leader of Boston Dynamics, mentioned that by leveraging the flexibility and richness of the MRB Dynamics, the Atlas prototype could achieved a series of extraordinary actions, such as leaps, bounds, and backflips, with the help of centroidal dynamics [30]. However, details of the latest work have not been released.

Based on the above, this paper presents an optimal torque control scheme gathering centroidal dynamics and model error compensation (MEC) for a real hydraulic quadruped platform, EHbot. Firstly, the leg link with a hydraulic actuator is considered as a single rigid body. A centroidal dynamics model is derived for the quadruped robot. Based on that, an intrinsic connection between the centroidal coordinates and the whole-body behavior in generalized coordinate space can be formulated and provide a dynamics model for torque control. To avoid the problem of poor robustness of model-based torque control caused by model inaccuracies, such as non-modelable hydraulic components, linearization, disturbances, etc., a MEC approach based on a low-gain PD feedback controller is introduced and projected to configuration space. Combining desired motion and force trajectories generated by a high-level planner, a torque control generated by the multi-objective quadratic programming (MQP) optimization allows robustness tracking target motion (CoM, torso, and foot-end trajectories) and ground reaction forces (GRFs) simultaneously under large model errors. Simulations and experiments were carried out on the EHbot. Finally, the results validate the effectiveness of the proposed torque control algorithm.

The contributions of this work are organized as follows: (i) a centroidal dynamics model is derived for quadruped robots via the change rate of the centroidal momentum, which is linearly associated with the generalized coordinates of the quadruped robot. (ii) We formulate a MEC to robustly address model errors caused by non-modelable hydraulic components, linearization, unexpected disturbances, etc. (iii) Gathering the centroidal dynamics and MEC for trajectory tracking described by generalized coordinates, an MQP torque control scheme is developed for motion and force tracking simultaneously, which can be successfully applied to the real quadruped platform, EHbot. Overall, the proposed optimal torque control scheme enables the quadruped robots to improve the tracking performance and be robust against model inaccuracies.

The remaining sections are outlined as follows. Section 2 introduces the centroidal dynamics model for quadruped robots. In Section 3, the details of the proposed MEC algorithms are presented. On this basis, we design an MQP-based torque controller for both motion and force tracking in Section 4. Several tests are carried out in simulations and on the EHbot, and the results are discussed in Section 5. Finally, the paper is summarized in Section 6.

2. Centroidal Dynamics Formulation

As depicted in Figure 2, the MRB dynamics expressed at the CoM has a distinct advantage that it matches the simplified inverted pendulum model, which is a commonly used template model serving the motion generators.

For each single-rigid-body (SRB) link on a quadruped robot, the linear momentum can be expressed as:

$${\mathit{h}}_i^{lin}=m_i{\mathit{v}}_i^c,$$

(1)

where ${\mathit{h}}_i^{lin}$ denotes the linear momentum of link i. $m_i$ and ${\mathit{v}}_i^c$ represent the mass and velocity of link i, respectively.

To correlate with the Denavit–Hartenberg (D-H) coordinate system and the generalized coordinates of the quadruped robot, it is convenient to assign a fixed frame numbered 0 and non-inertial frames numbered from 1 to N that are attached to each link of the tree-structure topology of the quadruped robot. On the basis of recursive computation, the CoM velocity of link i can be rewritten as:

$${\mathit{v}}_i^c={\mathit{v}}_i+{\mathit{\omega }}_i\times {\mathit{r}}_i,$$

(2)

where ${\mathit{v}}_i$ is the linear velocity of the origin of frame i, ${\mathit{\omega }}_i$ represents the angular velocity of the link i with respect to frame i. ${\mathit{r}}_i$ denotes the position vector connecting the origin of frame i to the CoM of link i.

Similarly, the angular momentum of link i can be expressed as:

$${\mathit{h}}_i^{ang}={\overline{\mathit{I}}}_i{\mathit{\omega }}_i+{\mathit{r}}_i\times m_i{\mathit{v}}_i,$$

(3)

where ${\mathit{h}}_i^{ang}$ and ${\overline{\mathit{I}}}_i$ represent the angular momentum and inertia tensor of link i.

Using spatial notation, the angular momentum and linear momentum of link i can be expressed as:

$${\mathit{h}}_i=\left[\begin{array}{c}{\mathit{h}}_i^{ang}\\ {\mathit{h}}_i^{lin}\end{array}\right]=\left[\begin{array}{cc}{\overline{\mathit{I}}}_i& m_i\mathit{S}\left({\mathit{r}}_i\right)\\ m_i\mathit{S}{\left(\mathit{r}{}_i\right)}^T& m_i{\mathbf{1}}_3\end{array}\right]\left[\begin{array}{c}{\mathit{\omega }}_i\\ {\mathit{v}}_i\end{array}\right]={\widehat{\mathit{I}}}_i{\widehat{\mathit{v}}}_i,$$

(4)

with

$$\mathit{S}\left(\mathit{r}\right)=\left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right],$$

(5)

where ${\mathit{h}}_i\in {\mathbb{R}}^6$ and ${\widehat{\mathit{I}}}_i\in {\mathbb{R}}^{6\times 6}$ represent the spatial momentum and spatial inertia of link i, $\mathit{S}\left(\mathit{r}\right)$ is the skew-symmetric cross-product matrix. ${\mathbf{1}}_3$ is $3\times 3$ unit diagonal matrix.

The generalized coordinates and generalized velocities of quadruped robots are usually described in configuration space, which facilitates the signal collection and dynamics analysis. Based on the generalized velocity $\dot{\mathit{q}}\in {\mathbb{R}}^{(n+6)}$ of the quadruped robot, the SRB spatial velocity can be further rewritten as:

$${\widehat{\mathit{v}}}_i={\widehat{\mathit{J}}}_i\dot{\mathit{q}},$$

(6)

where Jacobian matrix ${\widehat{\mathit{J}}}_i\in {\mathbb{R}}^{6\times (n+6)}$ represents a linear mapping of the generalized velocity into the spatial velocity of link i.

Therefore, the whole-body centroidal momentum of the quadruped robot aggregates the momentum of all the links projected at the CoM, which is expressed as:

$${\mathit{h}}_G=\sum _{i=1}^N{}^i\mathit{X}{}_G^T{\mathit{h}}_i=\sum _{i=1}^N{}^i\mathit{X}{}_G^T{\widehat{\mathit{I}}}_i{\widehat{\mathit{J}}}_i\dot{\mathit{q}}={\mathit{A}}_G\dot{\mathit{q}},$$

(7)

with

$${}^i\mathit{X}{}_G^T=\left[\begin{array}{cc}{}^G\mathit{R}{}_i& {}^G\mathit{R}{}_i\mathit{S}{\left({}^i\mathit{p}{}_G\right)}^T\\ 0& {}^G\mathit{R}{}_i\end{array}\right]\in {\mathbb{R}}^{6\times 6}.$$

(8)

where ${\mathit{h}}_G$ represents the centroidal momentum of the system. ${}^i\mathit{X}{}_G^T$ represents a spatial transformation matrix from link i to centroidal frame G, ${\mathit{A}}_G\in {\mathbb{R}}^{6\times n+6}$ is the centroidal momentum matrix (CMM). $\mathit{q}\in {\mathbb{R}}^{(n+6)}$ denotes generalized coordinate vector.

The dynamics equation of the system can be expressed as:

$$\begin{array}{c}\hfill {\mathit{f}}_G={\dot{\mathit{h}}}_G={\mathit{A}}_G\ddot{\mathit{q}}+{\dot{\mathit{A}}}_G\dot{\mathit{q}}\end{array},$$

(9)

where ${\mathit{f}}_G$ is the net external force vector acting on the centroidal point.

Equation (9) projects the whole-body dynamics of a tree-structure topology of the quadruped robot onto its CoM, thus capturing the relationship between the net external forces and the generalized coordinate states. Therefore, it provides a simplified whole-body dynamics model for force control of quadruped robots.

Generally, the equations of motion of quadruped robots can be expressed in terms of the generalized coordinates, velocities, and accelerations:

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{G}\left(\mathit{q}\right)={\mathit{S}}^T\mathit{\tau }+{\mathit{J}}_{\lambda }^T\mathit{\lambda },$$

(10)

where $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{(n+6)\times (n+6)}$ represents the generalized inertia matrix. $\mathit{C}\in {\mathbb{R}}^{(n+6)\times (n+6)}$ denotes the centrifugal and Coriolis forces. $\mathit{G}\in {\mathbb{R}}^{(n+6)}$ is the vectors of gravitational force. The generalized constraint forces $\mathit{\lambda }$ is mapped to the joint actuated torques $\mathit{\tau }\in {\mathbb{R}}^{(n+6)}$ through the constraint Jacobian $\mathit{J}\in {\mathbb{R}}^{3n_c\times (n+6)}$ that describes $3n_c$ constraints, $n_c$ denotes the number of legs in contact. $\mathit{S}$ is a selection matrix that selects which DoFs are actuated, and it is expressed as:

$$\mathit{S}=\left[\begin{array}{cc}{\mathbf{0}}_{6\times 6}& {\mathbf{0}}_{6\times n}\\ {\mathbf{0}}_{n\times 6}& {\mathbf{1}}_{n\times n}\end{array}\right].$$

(11)

Decoupling the dynamics into floating-based and actuated components. Equation (10) can be rewritten hereafter as:

$$\left[\begin{array}{cc}{\mathit{M}}_{11}& {\mathit{M}}_{12}\\ {\mathit{M}}_{21}& {\mathit{M}}_{22}\end{array}\right]\left[\begin{array}{c}{\ddot{\mathit{q}}}_{tor}\\ {\ddot{\mathit{q}}}_{act}\end{array}\right]+\left[\begin{array}{c}{\mathit{C}}_1\dot{\mathit{q}}+{\mathit{G}}_1\\ {\mathit{C}}_2\dot{\mathit{q}}+{\mathit{G}}_2\end{array}\right]=\left[\begin{array}{c}{\mathit{\tau }}_1\\ {\mathit{\tau }}_2\end{array}\right].$$

(12)

The first row of the Equation (12) is the force balance equation considering only the floating-based torso of the quadruped robot. Assuming there is no gravity and the floating-based torso remains actuated, the net external wrench acting on the torso can be expressed as [31]:

$${\mathit{U}}_1(\mathit{M}\ddot{\mathit{q}}+\mathit{C}\dot{\mathit{q}})={\mathit{\tau }}_1={\mathit{\varphi }}_1^T{\mathit{f}}_1,$$

(13)

where ${\mathit{U}}_1=\left[{\mathbf{1}}_{6\times 6}{\mathbf{0}}_{6\times n}\right]$ is used to select the top portion of the full Equation (12). The net external force ${\mathit{f}}_1$ acting on the floating torso, which is equivalent to the virtual wrench (force and moment) generated by a virtual model attached on the torso. ${\mathit{\varphi }}_1\in {\mathbb{R}}^{6\times 6}$ denotes a mapping of the generalized velocity of the torso and the spatial velocity described in the fixed frame.

Let ${\mathit{\psi }}_1={\mathit{\varphi }}_1^{-1}$, and:

$${\mathit{f}}_1={\mathit{\psi }}_1^T{\mathit{U}}_1(\mathit{M}\ddot{\mathit{q}}+\mathit{C}\dot{\mathit{q}}).$$

(14)

Combined with (9), the net external wrench acting on the centroidal frame G can be expressed as:

$$\begin{array}{cc}\hfill {\mathit{f}}_G=& {}^1\mathit{X}{}_G^T{\mathit{f}}_1\hfill \\ \hfill =& {}^1\mathit{X}{}_G^T{\mathit{\psi }}_1^T{\mathit{U}}_1(\mathit{M}\ddot{\mathit{q}}+\mathit{C}\dot{\mathit{q}})\hfill \\ \hfill =& {\mathit{A}}_G\ddot{\mathit{q}}+{\dot{\mathit{A}}}_G\dot{\mathit{q}},\hfill \end{array}$$

(15)

where ${}^1\mathit{X}{}_G^T$ denotes a spatial projection transform for the net external wrench, from torso frame to centroidal frame.

Therefore, the following equation holds:

$$\begin{array}{cc}\hfill {\mathit{A}}_G=& {}^1\mathit{X}{}_G^T{\mathit{\psi }}_1^T{\mathit{U}}_1\mathit{M}\hfill \\ \hfill {\dot{\mathit{A}}}_G\dot{\mathit{q}}=& {}^1\mathit{X}{}_G^T{\mathit{\psi }}_1^T{\mathit{U}}_1\mathit{C}\dot{\mathit{q}},\hfill \end{array}$$

(16)

Substituting (16) into (9), the whole-body centroidal dynamics of the quadruped robot can be calculated. Figure 3 shows the transformation diagram of the whole-body centroidal dynamics formulation for quadruped robots.

Based on the conservation of momentum, the linear and angular momentum of each link in the quadruped robot is mapped to the centroidal frame. With the relationship between momentum and force, the derivative of the momentum is used to formulate the whole-body centroidal dynamics, which provides a simplified model for quadruped torque control.

3. Model Error Compensation

Simplified models, such as linear inverted pendulum, single rigid body, etc., are used as template models for motion planning of the legged robots. However, there are some significant model errors, including: (i) inertia matrix errors aggregated from inaccurate measurements of each link, (ii) non-modelable hydraulic components, such as hydraulic hoses, cylinders and transmission parts, (iii) noises of position sensors and revolute joints with radial clearance for a physical platform. In this section, three PD compensations for CoM, torso, and foot-end trajectory tracking under model errors are introduced in details.

3.1. CoM Trajectory Compensation

Based on the axis of the centroidal frame parallel to that of the inertial frame, the net external wrench acting on the centroidal frame of the quadruped robot can be directly described by the operational space wrench of the system:

$${\mathit{f}}_G^{*}=\left[\begin{array}{c}{\dot{\mathit{h}}}_G^{ang}\\ {\dot{\mathit{h}}}_G^{lin}\end{array}\right]=\left[\begin{array}{c}{\dot{\mathit{h}}}_G^{ang*}\\ m_G{\ddot{\mathit{c}}}^{*}\end{array}\right].$$

(17)

To prevent the robot from being unstable due to the accumulation of model errors in centroidal motion tracking, a PD feedback compensation mechanism is introduced into (17) for disturbance rejection, which can be rewritten as:

$${\mathit{f}}_G^{*}=\left[\begin{array}{c}{\dot{\mathit{h}}}_G^{ang}+{\mathit{k}}_{p,\theta }({\mathit{h}}_G^{ang}-{\mathit{h}}_G^{ang}{}^{\prime })+{\mathit{k}}_{d,\theta }({\dot{\mathit{h}}}_G^{ang}-{\dot{\mathit{h}}}_G^{ang}{}^{\prime })\\ m_G[\ddot{\mathit{c}}+{\mathit{k}}_{p,c}(\mathit{c}-{\mathit{c}}^{{}^{\prime }})+{\mathit{k}}_{d,c}(\dot{\mathit{c}}-{\dot{\mathit{c}}}^{{}^{\prime }})]\end{array}\right],$$

(18)

where ${\mathit{k}}_{p,\theta }$ and ${\mathit{k}}_{p,c}$ represent the proportional gains for angular and linear trajectory tracking, respectively. ${\mathit{k}}_{d,\theta }$ and ${\mathit{k}}_{d,c}$ represent the derivative gains for angular and linear trajectory tracking in operational space, respectively. c, $\dot{c}$ and $\ddot{c}$ are the reference position velocity and acceleration of the centroidal point specified by a high-level motion generator. Superscript ${}^{{}^{\prime }}$ denotes the measured states.

Therefore, the desired change rate of the centroidal momentum and a PD feedback compensation are combined for centroidal momentum commands calculation.

3.2. Torso Trajectory Compensation

The desired torso posture is commonly described in operational space. An explicit mapping from operational space to the executed joint space is required to guarantee the precise control of the torque in executed joint space to the torso trajectories in operational space. Naturally, the Jacobian matrix defines a transformation mapping between the velocity described in operational space and that described in joint space.

$$\mathit{J}=\frac{\partial \mathit{x}}{\partial \mathit{q}}\Rightarrow \dot{\mathit{x}}=\mathit{J}\dot{\mathit{q}}.$$

(19)

The acceleration in operational space can be then expressed as:

$$\ddot{\mathit{x}}=\mathit{J}\ddot{\mathit{q}}+\dot{\mathit{J}}\dot{\mathit{q}}.$$

(20)

Therefore, the relationship between the torso acceleration in operational space and the generalized velocity and acceleration in joint space is:

$${\ddot{\mathit{x}}}_b^{*}={\mathit{J}}_b\ddot{\mathit{q}}+{\dot{\mathit{J}}}_b\dot{\mathit{q}}.$$

(21)

where ${\mathit{x}}_b^{*}\in {\mathbb{R}}^6$ represents the desired acceleration of the torso posture in operational space. ${\mathit{J}}_b\in {\mathbb{R}}^{6\times (n+6)}$ is a constant matrix, which is equal to $\left[1_{6\times 6}{0}_{6\times 12}\right]$. So, ${\dot{J}}_b=0$.

Specified in a form similar to the PD compensation of the centroidal motion, the acceleration of the torso trajectory can be expressed as:

$${\ddot{\mathit{x}}}_b^{*}={\ddot{\mathit{x}}}_b+{\mathit{k}}_{p,b}({\mathit{x}}_b-{\mathit{x}}_b^{{}^{\prime }})+{\mathit{k}}_{d,b}({\dot{\mathit{x}}}_b-{\dot{\mathit{x}}}_b^{{}^{\prime }}),$$

(22)

where ${\mathit{k}}_{p,b}$ and ${\mathit{k}}_{d,b}$ represent the proportional and derivative gains for torso trajectory tracking in operational space, respectively.

3.3. Foot-End Trajectory Compensation

The foot-end trajectory is planned in the operational space as well. Jacobian mapping is necessary for the foot-end trajectory planned in operational space and the generalized coordinates in joint space. In terms of MEC for foot-end trajectory, a similar PD feedback formulation for torso trajectory tracking is introduced. The desired foot-end acceleration is hereafter expressed as:

$$\begin{array}{cc}\hfill {\ddot{\mathit{P}}}^{*}& =\ddot{\mathit{P}}+{\mathit{k}}_{p,P}(\mathit{P}-{\mathit{P}}^{{}^{\prime }})+{\mathit{k}}_{d,P}(\dot{\mathit{P}}-{\dot{\mathit{P}}}^{{}^{\prime }})\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\ddot{\mathit{P}}}^{*}& ={\mathit{J}}_P\ddot{\mathit{q}}+{\dot{\mathit{J}}}_P\dot{\mathit{q}},\hfill \end{array}$$

(24)

where $\mathit{P}\in {\mathbb{R}}^{12}$ represents the foot-end trajectory in operational space. ${\mathit{J}}_P$ denotes the Jacobian matrix of the foot-end. ${\mathit{k}}_{p,b}$ and ${\mathit{k}}_{d,b}$ represent the proportional and derivative gains for foot-end trajectory in operational space, respectively.

It should be emphasized that the interaction between the foot-end and terrain is regarded as point-contact support. Unlike the six-dimensional elements consisting of linear and angular trajectory tracking of the torso, there are only linear elements in the foot-end trajectory. Therefore, the Jacobian of each foot-end is just the linear mapping portion. Formulation of the four legs of the quadruped robot in a unified manner, the Jacobian matrix of the foot-end is ${\mathit{J}}_P\in {\mathbb{R}}^{12\times (n+6)}$.

4. MQP Torque Control

In order to achieve the whole-body dynamic behaviors and track the spatial motion of the CoM, torso, foot-end, as well as the GRFs, a multi-objective optimization algorithm is necessary for the calculation of joint torque. The torque control can be optimized by solving a standard QP problem with a general form as follows:

$$\begin{array}{cc}\hfill \underset{\mathcal{X}}{min}& \frac{1}{2}{\mathcal{X}}^{\mathrm{T}}\mathit{H}\mathcal{X}+{\mathit{g}}^{\mathrm{T}}\mathcal{X}\hfill \\ \hfill s.t.& \mathit{CE}\mathcal{X}+\mathit{ce}=0\hfill \\ \hfill & \mathit{CI}\mathcal{X}+\mathit{ci}\ge 0,\hfill \end{array}$$

(25)

where the Hessian matrix $\mathit{H}$ and gradient vector $\mathit{g}$ are used to define any quadratic objective functions on the vector of decision variables $\mathcal{X}$. The matrix-vector couples $(\mathit{CE},\mathit{ce})$ and $(\mathit{CI},\mathit{ci})$ define inequality and equality constraints, respectively.

The generalized accelerations, joint torques, and GRFs are coupled to each other. In this work, the decision variables are expressed as:

$$\mathcal{X}={\left[{\ddot{\mathit{q}}}^{\mathrm{T}}{\mathit{\tau }}^{\mathrm{T}}{\mathit{\lambda }}^{\mathrm{T}}\right]}^{\mathrm{T}}\in {\mathbb{R}}^{2n+18}.$$

(26)

Without loss of generality, the objective expression is expressed in the least squares optimization form:

$$\underset{\mathcal{X}}{min}\frac{1}{2}{\parallel \mathcal{A}\mathcal{X}-\mathcal{R}\parallel }_2^2.$$

(27)

Back to the standard form, the Hessian matrix and gradient term of the objective function can be expressed as $\mathit{H}={\mathcal{A}}^{\mathrm{T}}\mathcal{A}$, $\mathit{g}=-{\mathcal{A}}^{\mathrm{T}}\mathcal{R}$. With

$$\mathcal{A}=\left[\begin{array}{c}{\omega }_0{\mathcal{A}}_0\\ {\omega }_1{\mathcal{A}}_1\\ ⋮\\ {\omega }_n{\mathcal{A}}_n\end{array}\right],\mathcal{R}=\left[\begin{array}{c}{\omega }_0{\mathcal{R}}_0\\ {\omega }_1{\mathcal{R}}_1\\ ⋮\\ {\omega }_n{\mathcal{R}}_n\end{array}\right],$$

(28)

where ${\omega }_i$ denotes the weights, expressing the relative importance among several certain desired behaviors.

4.1. Objective Functions

With the introductions of MEC for CoM, torso and foot-end trajectories and the reference GRFs tracking, the multiple objectives that can be plugged into (28) are listed as follows:

(1) Centroidal motion

$$\begin{array}{cc}\hfill {\mathcal{A}}_G& ={\mathit{A}}_G\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\mathcal{R}}_G& ={\mathit{f}}_G^{*}-{\dot{\mathit{A}}}_G\dot{\mathit{q}}.\hfill \end{array}$$

(30)

(2) Torso motion

$$\begin{array}{cc}\hfill {\mathcal{A}}_b& ={\mathit{J}}_b\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill {\mathcal{R}}_b& ={\ddot{\mathit{x}}}_b+{\mathit{k}}_{p,b}({\mathit{x}}_b-{\mathit{x}}_b^{{}^{\prime }})+{\mathit{k}}_{d,b}({\dot{\mathit{x}}}_b-{\dot{\mathit{x}}}_b^{{}^{\prime }})-{\dot{\mathit{J}}}_b\dot{\mathit{q}}.\hfill \end{array}$$

(32)

(3) Foot-end motion

$$\begin{array}{cc}\hfill {\mathcal{A}}_P& ={\mathit{J}}_P\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {\mathcal{R}}_P& =\ddot{\mathit{P}}+{\mathit{k}}_{p,P}(\mathit{P}-{\mathit{P}}^{{}^{\prime }})+{\mathit{k}}_{d,P}(\dot{\mathit{P}}-\dot{{\mathit{P}}^{{}^{\prime }}})-{\dot{\mathit{J}}}_P\dot{\mathit{q}}.\hfill \end{array}$$

(34)

(4) Directly GRFs tracking

To ensure the whole-body dynamics behaviors of quadruped robots in real-time, the reference GRFs described here are specified by analytical [32] or optimized approaches [33]. The objective function that penalizes deviation from the reference GRFs is expressed as:

$$\begin{array}{cc}\hfill {\mathcal{A}}_F& =\left[\mathbf{0}\mathbf{0}{\mathbf{1}}_{12}\right]\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\mathcal{R}}_F& ={\mathit{F}}^{*}.\hfill \end{array}$$

(36)

(5) Change in torques

In order to smooth the commands of torques and GRFs and avoid high-frequency oscillations in the commands, we penalize changes in torques $\mathit{\tau }$ and GRFs $\mathit{\lambda }$.

$$\begin{array}{cc}\hfill {\mathcal{A}}_{d\tau }& =\left[0{\mathbf{1}}_{12}0\right)\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\mathcal{R}}_{d\tau }& ={\tau }_{last}.\hfill \end{array}$$

(38)

(6) Change in GRFs

$$\begin{array}{cc}\hfill {\mathcal{A}}_{d\lambda }& =\left[00{\mathbf{1}}_{12}\right)\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill {\mathcal{R}}_{d\lambda }& ={\mathit{\lambda }}_{last}.\hfill \end{array}$$

(40)

It should be noted that the weight should be specified for the desired behavior detailed above.

4.2. Constraints

(1) Friction cone and unilateral limits

There are foot-ground friction cone and unilateral contact constraints for each leg (the normal GRF is not less than 0). The GRFs constraints for each leg can be expressed as:

$$\begin{array}{cc}\hfill {\mathit{CI}}_{fcon}^i& =\left[\begin{array}{ccc}1& 0& \frac{\mu }{\sqrt{2}}\\ -1& 0& \frac{\mu }{\sqrt{2}}\\ 0& 1& \frac{\mu }{\sqrt{2}}\\ 0& -1& \frac{\mu }{\sqrt{2}}\\ 0& 0& 1\end{array}\right]\hfill \\ \hfill {\mathit{ci}}_{fcon}^i& =\mathbf{0},\hfill \end{array}$$

(41)

where the top four rows of the matrix ${\mathit{CI}}_{fcon}^i$ represent the convex polyhedral approximation of the friction cone constraints of contact foot i. In this work, friction cones have been approximated as pyramids, which allows us to formulate them as a QP optimization. The last set is a unilateral constraint of the normal forces.

Noting that the reference GRFs optimized by foot force distributions have taken the friction constraints into account. The reference GRFs and the unknown GRFs in the decision variables are not completely equivalent. Therefore, it is necessary to consider the anti-slip friction cone constraints for the torque optimization.

(2) Joint torque limits

$${\mathit{CI}}_{\tau }=\left[\begin{array}{ccc}\mathbf{0}& {\mathbf{1}}_{12}& \mathbf{0}\\ \mathbf{0}& -{\mathbf{1}}_{12}& \mathbf{0}\end{array}\right],{\mathit{ci}}_{\tau }=\left[\begin{array}{c}{\mathit{\tau }}_{min}\\ {\mathit{\tau }}_{max}\end{array}\right].$$

(42)

(3) Floating-based inverse dynamics

$$\begin{array}{c}\hfill {\mathit{CE}}_{id}=\left[\begin{array}{ccc}\mathit{M}& -{\mathit{S}}^T& -{\mathit{J}}_{\mathit{\lambda }}^T\end{array}\right],{\mathit{ce}}_{id}=-(\mathit{C}\dot{\mathit{q}}+\mathit{G}).\end{array}$$

(43)

(4) Swing-phase GRFs

Ignoring the inertia force wrapped around the foot force sensor parts, the GRF of each leg equals zero when it is in the air.

$$\begin{array}{c}\hfill {\mathit{CE}}_{sw}^i={\mathbf{1}}_3,{\mathit{ce}}_{sw}^i=\mathbf{0}.\end{array}$$

(44)

Substituting the above objective functions and constraints into the QP paradigm in (25), the joint torque is regarded as torque feed-forward ${\tau }_{ff}$, which can be optimized using open-source QP solvers, such as IPopt [34], QPOASES [35], and QuadProg++ libraries [36].

The optimal torque control scheme is depicted in Figure 4. We have noticed that the motion trajectory generated by a motion generator and the reference GRFs obtained through a foot force distribution are exerted as the target for the joint torque control algorithms. Combined with joint space constraint equations, and finally, the feed-forward solution of joint torque has been realized. In addition, a low-gain joint-space PD feedback controller is introduced to further compensating for the model inaccuracies. Therefore, the total joint torque commands can be obtained:

$$\begin{array}{cc}\hfill {\mathit{\tau }}_{cmd}& ={\mathit{\tau }}_{ff}+{\mathit{\tau }}_{fd}\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill {\mathit{\tau }}_{fd}& ={\mathit{k}}_{p,q}({\mathit{q}}_d-{\mathit{q}}^{{}^{\prime }})+{\mathit{k}}_{d,q}({\dot{\mathit{q}}}_d-{\dot{\mathit{q}}}^{{}^{\prime }}).\hfill \end{array}$$

(46)

It is worth noting that the coupled ${\left[\ddot{\mathit{q}}\mathit{\tau }\mathit{\lambda }\right]}^{\mathrm{T}}$ in the optimal torque control algorithm is defined as the decision variables. However, only the joint torque is taken as the output commands, joint accelerations, and generalized constraint forces are not the input for actuation. The purpose of this method is to establish a unified mechanical mapping relationship between the strongly coupled generalized accelerations, GRFs and joint torques, so as to ensure that the joint torque can better track the motion and force at the same time.

5. Results

5.1. Experimental Setup

A hydraulically driven quadruped robot, EHbot, was used in our experiment. As shown in Figure 5, EHbot weights approximately 150 kg, is fully-torque controlled and equipped with the position sensors, force sensors, and an inertial measurement unit (IMU). An on-board hydraulic oil source system was customized for the hydraulic system. DoFs with three activities on each leg are named as hip abduction/adduction (HAA), knee flexion/extension(KFE) and ankle flexion/extension(AFE). The foot-end can be therefore placed in a three-dimensional space. In total, 12 active DoFs are driven by the customed hydraulic drive units (HDUs) [37].

In order to validate the effectiveness of the proposed optimal torque control algorithm, we have carried out the preliminary simulation on the physical simulator robot operating system (ROS) and Gazebo [38]. The simulation is implemented on a computer with 2.3 GHz quadcore Intel Core i5-6300HQ processor. The open source qpOASES-3.2.0 library [35] is used for optimization.

5.2. Simulations and Experiments

We have demonstrated various tests in simulations and then conducted experiments on a hydraulic quadruped platform, EHbot, to validate the advantages in two perspectives of the multi-objective optimal torque control scheme we proposed. A video of the results of the simulations and experiments can be found in https://www.youtube.com/watch?v=BbdXTgwV1L0&ab_channel=YapengShi (accessed on 29 January 2022).

Firstly, the tests were carried out using three controllers: (i) Joint high-gain PD control with refined parameters tuning, (ii) joint low-gain PD + MEC, and (iii) joint low-gain PD + MEC+ centroidal dynamics-based CoM trajectory tracking (CD). It should be noted that controller (iii) is the optimal torque control scheme that we proposed. The low-gain PD parameters of controllers (ii) and (iii) are the same. The high-gain PD parameters of controller (i) are carefully tuned for walking purposes. The block diagram of the proposed three different controllers is depicted in Figure 6.

Controllers (i), (ii), and (iii) are encapsulated by blue, green, and red dotted areas, respectively. We assume that the torso position and CoM trajectory are treated equivalently in the following cases, considering the deviations are caused by the inaccurate model.

Under the same reference CoM trajectory generated by an MPC motion generator [39], Figure 7 demonstrates the quadruped-CoM trajectory tracking results under the three different controllers.

In addition, the corresponding quantitative comparison of the quadruped-CoM tracking error under the three controllers is further discussed. The maximum deviation, endpoint value deviation, and root mean square error (RMSE) are illustrated in

Table 1. Quantitative comparison of quadruped-CoM tracking with three controllers.

Controller	Maximum Deviation [m]	Endpoint Value Deviation [m]	RMSE [m]
PD	0.1381	0.095	0.0501
PD + MEC	0.0602	0.020	0.0220
PD + MEC + CD	0.0214	0.002	0.0081

.

It can be seen from the simulation results that quadruped-CoM trajectory tracking has similar performance under the joint PD controller and the PD + MEC controller implemented at the beginning. However, due to the influence of model inaccuracies and foot-end slippage, the maximum deviation between the actual trajectory (blue line) and reference value (black line) reaches 0.1381 m. Additionally, the endpoint deviation reaches 0.095 m as time goes on. This is reasonable for a joint PD controller since not only is there no quadruped-CoM trajectory tracking feed-forward introduced, but also no error compensation term for the CoM trajectory. In the PD + MEC controller, the quadruped-CoM trajectory tracking of error compensation is introduced with torso position and CoM trajectory treated equivalently.

However, due to the large inertia of the quadruped robot system, it is difficult to eliminate the influence of inertial forces of the system by the torso trajectory tracking with Jacobian quasi-static mapping. Thereby, the CoM trajectory (green line) oscillates significantly and the RMSE reaches 0.022 m. With the CoM trajectory tracking on the basis of the centroidal dynamics, multi-objective torque controller (iii) is formulated by embedding centroidal dynamics and model error compensation terms. This allows for better tracking performance of the actual CoM trajectory (red line). The corresponding deviation is an order of magnitude lower than that of the joint PD controller.

To further validate the performance of our multi-objective torque control algorithm, the proposed three different controllers are implemented on the real quadruped platform, EHbot. Snapshots of experimental trials with three controllers are demonstrated in Figure 8.

Correspondingly, the comparison results of roll angles of the torso are depicted in Figure 9.

In the case of joint PD controller implementation only, the amplitude of the roll angle profile (black solid line) of the quadruped platform gradually increases during walking due to the contact impacts and model inaccuracies. The roll angle reaches a maximum of 0.12 rad at about 12.3 s (amplitude fluctuation is higher than 0.2 rad) and then cannot maintain stability. The reason for the subsequent decrease in roll angle fluctuations in the figure is that it is artificially stopped to prevent the robot from falling. After the introduction of the MEC algorithm, the large fluctuation of the roll angle caused by foot-ground impacts and model inaccuracies disappeared. However, without the tracking for the reference CoM trajectory, the torso tracking based on quasi-static Jacobian mapping cannot eliminate the effect of the large inertial forces of the system. As a result, the amplitude of the roll angle fluctuates above 0.13 rad.

The proposed multi-objective optimal torque control scheme establishes the mathematical relationship between the whole-body dynamics and the CoM trajectory by centroidal dynamics. The effect of the inertia forces of each link of the system is considered in the tracking of the CoM trajectory. The problem of the inertia forces is solved. As can be seen from the figure, the amplitude of roll angle fluctuation is controlled within 0.11 rad. The results of the comparison tests demonstrate that the proposed controllers can effectively improve the stability performance for real hydraulic quadruped platforms under large model inaccuracies.

The second validation is the simulation of the quadruped robot traversing over complex terrain to further demonstrate the effectiveness of the proposed scheme. Three different terrains are set in the simulation: (i) movable planks with a thickness of 3 cm randomly stacked, (ii) uneven stone road with a height of about 5 cm, and (iii) ${20}^{\circ }$ slope. The snapshots of the quadruped robot walking over the complex terrain at 0.3 m/s are demonstrated in Figure 10.

It should be highlighted that the quadruped robot is walking blindly without prior environment information input. The quadruped robot can traverse over planks and uneven stones, climb up the slope, and then reach your destination steadily. The torso angles of the quadruped robot while walking are depicted in Figure 10. We noticed that the angle of the robot fluctuates in a narrow range. The maximum fluctuation of pitch angle (red solid line) and roll angle (blue solid line) over rough terrain is less than 0.09 rad and 0.07 rad, respectively. During climbing, the pitch angle reaches about 0.24 rad because of slope, but the fluctuation range still remains small during locomotion.

Figure 11 shows the tracking results of the fore-left (FL) normal force during 36∽40 s. Bar patterns indicate the stance phase of each leg. HR, HL, and FR represent the hind-right, hind-left, and fore-right leg, respectively. We show the results for the FL leg only. Because the results for the other legs are qualitatively the same. When the actual normal force deviates significantly from the reference one, the torso posture fluctuates dramatically thereafter. It can be seen that the multi-objective optimal torque scheme we proposed has the ability to reduce the force deviation and restore the torso posture within one step.

In the third validation, the proposed optimal torque scheme was implemented on a real hydraulic quadruped platform, EHbot, with the environment of external disturbances and a 2 cm plank on the ground. The plank was taken and placed under the foot-end artificially to validate the dynamic stability of the quadruped robot. The pitch and roll angle profiles and experimental snapshots of the quadruped robot during walking are shown in Figure 12.

In stage 1 of the upper figure, unexpected external disturbances were exerted by kicking the torso laterally to test the performance of the push recovery of the quadruped robot. In stage 2, a plank with a height of 2 cm was repeatedly taken and placed on the FR foot-end of the quadruped platform. In stage 3 and 4, the quadruped platform walking over the plank (yellow shaded area in the lower Figure 12) was carried out. It can be seen that the quadruped robot has the ability to remain robust under both lateral external and terrain disturbances with the proposed torque control scheme. From the figure profiles, During the entire experimental trial, the maximum amplitudes of the pitch and roll angle profiles are about 0.05 rad and 0.12 rad, respectively.

Through the series of tests discussed above, comparison and rough terrain disturbances were implemented in simulations as well as experiments on a real hydraulic quadruped platform, EHbot. The results demonstrate that a similar performance can be observed in simulations and real quadruped experiments. These tests validate the proposed torque control scheme is robust to large model inaccuracies and improves the performance of the overall system.

6. Conclusions

In this paper, we present a multi-objective optimal torque control scheme for hydraulic quadruped robots. The scheme uses both centroidal dynamics and MEC algorithms. Centroidal dynamics modelled projects the dynamics behaviors to centroidal frame to follow the centroidal motion. In addition, MEC algorithm is developed to deal with model inaccuracies caused by non-modelable elements, linearization, and disturbances. The motion of the CoM, torso, and foot-end trajectories are further projected in joint space. Hence, a multi-objective QP problem is formulated to coordinate track the motion and GRFs simultaneously. With the satisfaction of all constraints, the optimal torque control scheme is proposed. Finally, a series of simulations and experimental trials were conducted on a hydraulic quadruped robot through comparison and disturbances tests. The results demonstrate that the torque control scheme we proposed can effectively improve the stability and robustness to model inaccuracies.

This work mainly focus on the application of QP-based torque control scheme to the hydraulic physical system with large inaccuracies. In the near future, we hope to incorporate hydraulic actuators and a hydraulic control system (e.g., force command tracking and synchronization control of the hydraulic actuators) for better performance.