## Abstract

The primary goal of this paper is to develop a formal foundation to design nonlinear feedback control algorithms that intrinsically couple legged robots with bio-inspired tails for robust locomotion in the presence of external disturbances. We present a hierarchical control scheme in which a high-level and real-time path planner, based on an event-based model predictive control (MPC), computes the optimal motion of the center of mass (COM) and tail trajectories. The MPC framework is developed for an innovative reduced-order linear inverted pendulum (LIP) model that is augmented with the tail dynamics. At the lower level of the control scheme, a nonlinear controller is implemented through the use of quadratic programming (QP) and virtual constraints to force the full-order dynamical model to track the prescribed optimal trajectories of the COM and tail while maintaining feasible ground reaction forces at the leg ends. The potential of the analytical results is numerically verified on a full-order simulation model of a quadrupedal robot augmented with a tail with a total of 20 degrees-of-freedom. The numerical studies demonstrate that the proposed control scheme coupled with the tail dynamics can significantly reduce the effect of external disturbances during quadrupedal locomotion.

## 1 Introduction

This paper presents a systematic approach to design a hierarchical control algorithm, based on reduced-order models, model predictive control (MPC), quadratic programming (QP), and virtual constraints, to intrinsically couple bio-inspired articulated tails with legged robots for robust locomotion. In the proposed approach, the high-level MPC computes an optimal center of mass (COM) trajectory for the body and an optimal tail trajectory in real-time. More specifically, we develop an MPC formulation for an innovative and augmented linear inverted pendulum (LIP) model which has been extended to consider the forces introduced by the tail dynamics. The high-level MPC is implemented in an event-based manner in the sense that it is solved at the beginning of each continuous-time domain subject to the predefined foot placements, the feasibility of the net ground reaction force (GRF), and the tail joint and actuation limits. In order to close the gap between the reduced-order model used by the high-level MPC and the full-order model of the robot augmented with a tail, a low-level nonlinear controller, based on convex QP and virtual constraints, is implemented to track the prescribed COM and tail trajectories while ensuring feasible GRFs at the contacting leg ends. The potential of the proposed control approach is numerically demonstrated through extensive full-order simulations of the Vision 60 quadruped augmented with a serpentine tail with a total of 20 degrees-of-freedom (DOFs) (see Fig. 1). It is shown that extending existing MPC frameworks for locomotion to consider additional dynamics induced by a tail can produce greater robustness to external disturbances during locomotion than a legged system without the addition of a tail like structure.

### 1.1 Background, Motivation, and Challenges

#### 1.1.1 Related Work on Legged Locomotion.

The most widely used control method for legged locomotion is based on a heuristic notion of gait stability, known as the zero moment point criterion [1,2]. This control methodology produces quasi-static gaits and assumes full-actuation. Alternatively, legged locomotion can be described by hybrid systems [3,4] with continuous-time domains (i.e., phases) to represent the Lagrangian dynamics and discrete-time transitions between continuous-time domains to represent the abrupt changes in the velocity components of the state vector during the infinitesimal period of the rigid impacts between the swing leg ends and the environment [5–29]. State-of-the-art nonlinear control algorithms that address the hybrid nature of legged locomotion to stabilize gait patterns include controlled symmetries [8], hybrid reduction [30,31], transverse linearization [10,32], and hybrid zero dynamics (HZD) [7,18,33,34]. Of these methods, only HZD and transverse linearization are capable of addressing the general case of underactuation inherent to dynamic locomotion. In the case of HZD, a set of kinematic constraints, referred to as virtual constraints [5,34], are enforced via input–output (I-O) feedback linearization [35] in order to impose coordinated motion among the limbs. Virtual constraint controllers have been validated both numerically and experimentally for the stable locomotion of bipedal robots [7,17,20,26,34,36–39], quadrupedal robots [40–42], lower extremity powered prostheses [43–45], and exoskeletons [46]. The gait planning for virtual constraint controllers is typically performed offline by solving a nonlinear programming problem. While algorithms exist in which agile gait planning can be performed efficiently [26,47], these methods *cannot* be used in an online manner, thereby restricting the use of HZD methods for navigating complex environments.

A variety of QP- and MPC-based approaches also exist for locomotion, including LIP-based MPC path planning [42,48–51], centroidal dynamics-based MPC [52,53], nonlinear MPC [54], policy-regularized MPC [55], and whole-body QP-based control [56,57], though LIP and centroidal methods have been studied most extensively. In LIP-based MPC, optimal COM and center of pressure (COP) trajectories are determined subject to the zero moment point conditions and feasible net GRF. This approach has the benefit of not requiring offline trajectory optimization and may be coupled with a number of different low-level control techniques to track the prescribed trajectories. While LIP-based MPC has primarily been applied to bipedal systems [48–50], it has recently been extended to quadrupedal locomotion as well [42,58]. Centroidal-based MPC techniques determine optimal GRFs using reduced-order dynamics and have the primary benefit of considering the feasibility of GRFs at each contact point. While these optimization methods have shown to produce robust locomotion, it is a significant computational burden to solve the MPC at every time-step even considering the problems are typically formulated as convex QPs. For this reason, event-based optimization is of great interest [42,59]. This is especially true as the number of decision variables and constraints increase, as is the case when augmented with a serpentine tail.

#### 1.1.2 Motivation and Related Work on Tails for Legged Locomotion.

Although the technology involved in the development of sophisticated quadrupedal machines is rapidly advancing, the knowledge regarding the synthesis of feedback control paradigms that intrinsically couple the motion of tails and legged robots for improved dynamic stability is lacking. The use of tails in nature has primarily been studied in terms of dynamic reorientation during leaps and falls [60,61], but there has been evidence that large cats use their tails for dynamic locomotion as well [62]. Similar to their biological counterpart, the use of biomimetic pendulum-like tails has been explored on bipedal [63–66], quadrupedal [67–69], and hexapedal [70] systems for stability. While these tails have been shown to negate external disturbances [67], perform aerial reorientation [63–65,69–71] in an effort to regain stability, and increase rapid accelerations [68], the dynamic stability of locomotion for quadrupeds augmented with biomimetic tails has been addressed in very few studies, see, e.g., Refs. [72] and [73]. These investigations primarily explore the use of tails during static stance [67], flight phase [63–65,69,70], and gait initiation [68], allowing for simplified models and controllers which *cannot* easily be generalized to varying single- and multicontact domains present during quadrupedal locomotion. Additionally, control of legged systems with the addition of serpentine appendages (as opposed to pendulum-like appendages)–which greatly increases the nonlinearities present–has been largely unexplored.

#### 1.1.3 Significant Challenges.

Existing nonlinear controller design approaches for legged locomotion are tailored specifically to systems without the addition of tails, and the integration of tails introduces significant overhead in terms of complexity and dimensionality of these already sophisticated machines. This, in turn, further complicates the design of robust nonlinear control algorithms. We aim to answer the following *questions*: (1) how can we extend the LIP dynamics to include external forces induced by a tail? (2) how can we systematically plan for the motion of the tail in real-time to ensure robust locomotion? and (3) how can we implement these reduced-order trajectories on a full-order model using nonlinear control approaches?

### 1.2 Goals, Objectives, and Contributions.

The *overarching goal* of this paper is to present a formal foundation to design real-time motion planning and nonlinear feedback control algorithms for robust locomotion of quadrupedal robots augmented with serpentine tails. The *specific objectives* and *key contributions* of the paper are as follows:

We present an innovative reduced-order model, referred to as the extended LIP dynamics, for the real-time planning of the COM and tail motions. In particular, the presented reduced-order model integrates the LIP dynamics with the nonlinear tail dynamics to enable us to formulate an online optimal control problem.

We present a hierarchical nonlinear control scheme for the motion control of quadrupedal robots with robotic tails. At the higher-level of the control scheme, we propose an MPC formulation for the real-time path planning of the reduced-order model to effectively generate the optimal trajectories for the robot's COM and tail joints. The MPC considers the feasibility of the tail motion, COP constraints, and the feasibility of the net GRF. As the proposed reduced-order model is nonlinear, we linearize the extended LIP model at the beginning of each continuous-time domain and then solve the MPC algorithm in an event-based manner to formulate a convex QP (see Fig. 2).

At the lower-level of the control scheme, a nonlinear controller, based on QP and virtual constraints, is presented for the motion control of the full-order dynamical system to track the prescribed motions of the COM and tail while imposing feasibility of the GRFs at all contact points.

We investigate the effect of robotic tails on quadrupedal locomotion intrinsically coupled with the proposed control algorithms. For this purpose, a series of extensive and full-order numerical simulations is presented to demonstrate the effectiveness and robustness of the proposed control approach for locomotion of a 20DOF advanced quadrupedal robot, Vision 60, augmented with a serpentine tail in the presence of external disturbances (see Fig. 1). We numerically show that the integration of the tail dynamics with the developed nonlinear feedback control algorithms can significantly reduce the effect of external disturbances on quadrupedal locomotion.

References [72] and [73] have provided an interesting approach to address the dynamic stability of legged robots augmented with a single-DOF pendulum-like tail, but in both cases the controller used is a central pattern generator, where the trajectory is calculated offline and the robot is controlled in an open-loop manner. Reference [72] provided an analysis regarding the general design of the tail using a spring loaded inverted pendulum model, though it is not used as part of any control algorithm and only encompasses movement in the sagittal plane. Our work differs completely in that (1) we consider the use of a serpentine tail, (2) we propose real-time planning algorithms for both locomotion and the tail by using an extended LIP model, and (3) we track the generated reduced-order trajectories in a closed-loop manner through a QP-based virtual constraint controller. The work presented in the current paper also differs from the previous work [42] in that Ref. [42] did *not* address locomotion with tail dynamics. Here, we derive and use a novel extended LIP model in which additional external forces may be addressed. Furthermore, the current work includes the dynamics of the appendage in the path planner in order to increase gait stability and robustness as opposed to treating the appendage as a disturbance. We also extend the event-based MPC formulation of Ref. [42] to the *nonlinear* reduced-order model, arising from the extended LIP dynamics and plan for the optimal COM motion of the robot as well as the tail trajectory.

The paper is organized as follows. The full-order nonlinear models for quadrupedal locomotion with robotic tails are presented in Sec. 2. Section 3 presents the extended reduced-order model for real-time planning. Section 4 addresses the optimal control problem and formulation of the higher-level MPC for the extended LIP model. The lower-level nonlinear controller, based on QP and virtual constraints, for the whole-body motion control is presented in Sec. 5. Section 6 presents the numerical simulations for quadrupedal locomotion with tails to verify the effectiveness of the proposed control algorithms. Finally, Secs. 7 and 8 present discussion and concluding remarks, respectively.

## 2 Full-Order Nonlinear Model of Locomotion With Tail

### 2.1 Robot and Tail Model.

In this paper, we consider the full-order dynamical model for locomotion of the quadrupedal robot Vision 60, made by Ghost Robotics,^{2} augmented with a serpentine tail (see Fig. 1). Vision 60 can be modeled with 18DOFs, 12 of which consist of leg DOFs while the other six represent the absolute position and orientation of the floating base in an inertial world frame. Each leg of the robot consists of an actuated 2DOF hip joint (roll and extension) plus a 1DOF knee joint, ending with a point foot. The tail design used in this paper has 2DOFs—one allows for a rolling motion, while the other allows for yaw motion (see Fig. 3). The yaw portion of the tail consists of eight holonomically constrained gears which are driven by a single actuator. The gear ratio is held constant from one link to the next; therefore, each link is constrained to rotate by the same amount as the previous link in order to obtain uniform deformation along the length of the tail. This design is partially inspired by the designs found in Refs. [74] and [71]. To keep the tail mechatronics light and simple, each actuated joint uses a single Hebi Robotics^{3} actuator which will provide sufficient torque to drive the tail. We note that the total mass of the tail is approximately 1.51 (kg), in which the base, roll link, and individual yaw links are approximately 0.4 (kg), 0.68 (kg), and 0.053 (kg), respectively. Throughout this paper, the tail mechanism is assumed to be mounted on a point, referred to as the base of the robot (see Fig. 1).

### 2.2 Nonlinear Dynamics.

*r*” stands for the robot, $pr\u2208\mathbb{R}3$ and $\alpha r\u2208\mathbb{R}3$ represent the absolute position and orientation of the body of the robot in the inertial world frame, respectively, and $qb\u2208\mathbb{R}12$ denotes the body angles (i.e., shape of the robot). The torque and force inputs applied to the robot are denoted by $ur\u2208\mathbb{R}12,\lambda c\u2208\mathbb{R}3nc$, and $\lambda t\u2208\mathbb{R}6$, which are the actuator torque inputs, the GRFs at the contacting leg ends, and the tail reaction wrench (forces and moments), respectively. In our notation, “col” represents the column vector and

*n*denotes the number of contacting legs with the ground. The evolution of the robot is then described by the Euler–Lagrange equations and principle of virtual work as follows:

_{c}where $Dr(qr)\u2208\mathbb{R}18\xd718$ is the symmetric and positive definite mass-inertia matrix, $Hr(qr,q\u02d9r)\u2208\mathbb{R}18$ denotes the Coriolis, centrifugal, and gravitational terms, and $Br\u2208\mathbb{R}18\xd712$ is the input distribution matrix. In addition, $Jc(qr)\u2208\mathbb{R}3nc\xd718$ is the ground contact Jacobian matrix, and $Jbr(qr):=\u2202pbr\u2202qr(qr)\u2208\mathbb{R}6\xd718$ is the Jacobian of the base of the robot on which the tail is connected.

*t*” stands for the tail, $pt\u2208\mathbb{R}3$ and $\alpha t\u2208\mathbb{R}3$ represent the absolute position and orientation of the base of the tail in the inertial world frame, respectively, and $\theta :=col(\theta roll,\theta yaw)\u2208\mathbb{R}2$ are the internal roll and yaw angles. Finally, $\tau t:=col(\tau roll,\tau yaw)\u2208\mathbb{R}2$ are the torque inputs to the tail roll and yaw actuators. The equations of motion can be described as follows:

*q*and

_{r}*q*coordinates, respectively. Differentiating the holonomic constraints twice yields

_{t}*λ*(i.e., internal wrench), the equations of motion in the nonlinear coupled dynamics (4) can be expressed as an input-affine system as follows:

_{t}where $z:=col(qr,qt,q\u02d9r,q\u02d9t)$ denotes the state vector and $\tau :=col(\tau r,\tau t)$ represents the control inputs.

## 3 Extended Linear Inverted Pendulum Model

The objective of this section is to derive a reduced-order model based on the LIP dynamics to study the equations of motion for the COM and the tail dynamics. The model, referred to as the extended LIP dynamics, will be utilized for the development of the real-time motion planning algorithm in Sec. 4.

*x*and

*y*directions, respectively, having been projected onto the

*xy*-plane. In addition, $zcom$ is the vertical height of the center of mass, which is assumed to remain constant throughout the duration of the gait,

*g*

_{0}is the gravitational constant, and $ucop:=col(xcop,ycop)\u2208\mathbb{R}2$ represents the Cartesian coordinates of the COP in the world frame. In order to include external forces in the LIP model, we consider the following dynamics:

*F*denotes the external force in the

_{x}*x*direction applied by the tail,

*F*represents the GRF in the

_{gx}*x*direction, $FMrx$ denotes the equivalent force in the

*x*direction due to an external roll moment, $FMpx$ represents the equivalent force in the

*x*direction due to an external pitch moment, and $FMyx$ denotes the equivalent force in the

*x*direction due to an external yaw moment (see Fig. 4). The notation is defined similarly for both the

*y*and

*z*directions. In order to solve for the net GRF, we consider the equation of motion in the

*z*direction as

and let $z\xa8com=0$ for the duration of the gait. For this purpose, we first study the components of the GRF and the equivalent forces due to moments according to the geometry of the model. In particular, it can be shown that $Fgx=Fgxcom\u2212xcopl,\u2009Fgy=Fgycom\u2212ycopl$, and $Fgz=Fgzcoml$, where *F _{g}* denotes the net GRF and $l:=(xcom\u2212xcop)2+(ycom\u2212ycop)2+(zcom)2$. In addition, we can easily translate the moments into equivalent forces as follows (see Fig. 4)

*xz*-plane, and similarly, $\u2113yz:=(ycom\u2212ycop)2+(zcom)2$ and $\u2113xy:=(xcom\u2212xcop)2+(ycom\u2212ycop)2$. These geometric relations together with Eq. (8) and $z\xa8com=0$ finally result in the following net GRF

*F*and the moments about the base of the tail (i.e.,

_{z}*M*,

_{r}*M*, and

_{p}*M*). Since the dominant external wrenches that are applied by the tail are assumed to be those involving

_{y}*F*and

_{x}*F*, the vertical force and the moments induced about the base of the tail are neglected for the purpose of planning, but all forces and moments are considered during the full-order controller and simulations (see Secs. 5 and 6). Furthermore, $\u2113xy$ goes to zero as the COM approaches the COP which causes $FMyx$ and $FMyy$ to become undefined due to division by zero. We therefore plan only for the standard COP inputs (i.e., $xcop$ and $ycop$) together with

_{y}*F*and

_{x}*F*which are to be applied by the tail. We can show that this assumption will reduce the nonlinear dynamics of the extended LIP model to the following linear dynamics

_{y}### 3.1 Addition of Tail Dynamics.

*B*and

_{t}*J*, the equations of motion in Eq. (2) can be rewritten more concisely as

_{b}where $\lambda t=col(Ft,Mt)$ with $Ft:=col(Fx,Fy,Fz)\u2208\mathbb{R}3$ being the forces at the base of the tail in the *x*, *y*, and *z* directions, and $Mt:=col(Mr,Mp,My)\u2208\mathbb{R}3$ being the moments at the base of the tail in the roll, pitch, and yaw directions.

*x*and

*y*directions, only the first two rows of Eq. (11) are used for the high-level MPC. We may then solve for the reaction forces caused by the tail in the

*x*and

*y*directions by noting the tail mass matrix,

*D*, and nonlinear vector,

_{t}*H*, may be decomposed into individual parts, denoted by $Dij$ for $1\u2264i,j\u22648$ and

_{t}*H*for $1\u2264i\u22648$, respectively. More specifically, we have

_{i}^{4}. Combining Eqs. (10) and (12), the LIP equations with external forces in the

*x*and

*y*directions may then be written as

where $x:=col(xcom,ycom,\theta roll,\theta yaw,x\u02d9com,y\u02d9com,\theta \u02d9roll,\theta \u02d9yaw)\u2208\mathbb{R}8$ and $u:=col(xcop,ycop,\tau roll,\tau yaw)\u2208\mathbb{R}4$ denote the state and control inputs, respectively. In addition, $W(x)\u2208\mathbb{R}8\xd78$ is a weighting matrix, $A\u2208\mathbb{R}8\xd78$ represents the state matrix, $B\u2208\mathbb{R}4\xd78$ denotes the input distribution matrix, and $N(x)\u2208\mathbb{R}8$ contains the remaining nonlinear terms.

### 3.2 Linearization of the Extended Linear Inverted Pendulum Model.

*W*matrix, the Taylor series expansion is applied to both the left- and right-hand sides of the equation separately yielding

*x*

_{0}, $x\u02d90$, and

*u*

_{0}are the operating points around which the system is linearized. Combining Eqs. (16) and (17) and solving for $x\u02d9$, we obtain the linearized equations of motion

where the subscript “$\u2113$” stand for the linearized dynamics.

where $Ad\u2208\mathbb{R}8\xd78$ and $Bd\u2208\mathbb{R}8\xd74$ are the discrete state and input distribution matrices, respectively, and $d\u2208\mathbb{R}8$ is a vector of constants. The aforementioned matrices and vector are constant for a given linearization/discretization, and will be reevaluated in an event-based manner for the real-time path planning problem (see Sec. 4).

## 4 High-Level Path Planner

The objective of this section is to formulate a real-time optimal control problem for the extended reduced-order model to compute an optimal trajectory for the COM and tail motions subject to the COP remaining within the support polygon created by the contacting leg ends, feasible net GRF, and joint and actuation limits of the tail. In particular, we formulate a high-level MPC problem to be solved in an event-based manner (i.e., beginning of each continuous-time domain) to drive the state of the extended reduced-order system from an arbitrary initial state, *x _{i}*, to some final state,

*x*, over a finite number of continuous time domains, $M\u22651$. We consider a general locomotion pattern with a graph $G(V,E)$ with the vertices set $V$ and the edges set $E\u2282V\xd7V$ (see Fig. 6). The vertices denote the continuous-time domains of locomotion including double-, triple-, and quadruple-contact phases, and edges represent the discrete-time transitions between the continuous-time domains. Each continuous-time domain is assumed to have $Nd\u22651$ evenly temporally spaced grid points over which the trajectory is optimized. In order to indicate the continuous-time domain for every time sample, we define the index function as $\zeta :Z\u22650\u2192{1,2,\u2026,M}$ by $\zeta [k]:=\u230akNd\u230b+1$ for all $0\u2264k<M\u2009Nd$ and $\zeta [k]:=M$ for $k\u2265M\u2009Nd$. In our notation, $\u230a.\u230b$ represents the floor function.

_{f}### 4.1 Model Predictive Control Formulation.

We formulate an event-based optimal control problem for the real-time planning of the extended reduced-order model. In particular, we first linearize the nonlinear extended LIP dynamics (15) about its current operating points at the event times (beginning of each continuous-time domain), that is, $k=r\u2009Nd$ for some $r\u2208Z\u22650$. We then setup an MPC problem using the previously-mentioned linearization over a finite control horizon $Nc=n\u2009Nd$ for some $n\u22651$. This reduces the optimal control problem of the nonlinear system into QP that can be solved in real-time. The optimal control inputs are then employed for *N _{d}* grid points of the current domain while we discard the remaining control inputs for the next domains. At the beginning of the next domain (i.e., new event), the Jacobian linearization is updated according to the current state of the nonlinear reduced-order model and a new MPC problem is solved. This iterative procedure continues for all future events.

*o*” stands for the original system. These matrices are then kept constant for the formulation of the optimal control problem. At the next event, the matrices will be reevaluated and will be utilized again for a new optimal control problem. We now consider the affine dynamics (20) with the initial condition $xk=xko$ for all future times. The optimal control inputs must be constrained such that $ukcop$ (i.e., COP components of the input) belongs to the convex hull of the contacting points with the ground, $U\zeta [k]\u2282\mathbb{R}2$, for all domains. That is, $ukcop\u2208U\zeta [k]$, ensuring that the COP is within the support polygon for all time samples. Additionally, we constrain the net GRF to remain within the friction cone. From the linearized extended LIP dynamics, the friction cone constraints can be given by

in which the subscript “$f$” stands for friction cone. More specifically, $Af\u2208\mathbb{R}2\xd78$ and $Bf\u2208\mathbb{R}2\xd74$ are the state and input matrices which are composed of the rows of $A\u2113$ and $B\u2113$ pertaining to the accelerations in the *x* and *y* directions. In a similar manner, $df\u2208\mathbb{R}2$ is composed of the appropriate rows of $d\u2113$. Finally, we define $\eta :=1g0\u2009\mu 2$ to represent the friction cone that the net GRF must stay within, where *μ* denotes the static friction coefficient, *g*_{0} represents the gravitational constant, and **1** is an appropriately sized vector of ones.

*k*, respectively. We also define $\tau tlb,\u2009\tau tub,\u2009\theta lb$, and $\theta ub$ to denote the lower and upper bounds of the tail torque inputs and joint positions. We are now in a position to setup the MPC problem to be solved in an event-based manner (i.e., beginning of each continuous-time domain) in order to create an optimal sequence of inputs (i.e., COP and

*τ*) which drives the system from

_{t}*x*

_{0}to

*x*over the control horizon. In particular, we consider the following optimal control problem at $k=r\u2009Nd$

_{f}where $Uk\u2192k+Nc\u22121|k:=col(uk|k,\u2026,uk+Nc\u22121|k)$ denotes the sequence of control inputs and $xk+i|k$ is the predicted state vector at time *k *+* i* computed at time *k* subject to the dynamics $xk+i+1|k=Ad\u2009xk+i|k+Bd\u2009uk+i|k+d$ with the initial condition $xk|k:=xk$. In a similar manner, we denote $uk+i|k$ to be the COP and tail torque inputs at time *k *+* i* computed at time *k*. The terminal cost is expressed as $p(xk+Nc|k):=||xk+Nc|k\u2212xk+Nc|kdes||P2$ for some symmetric positive definite matrix $P\u2208\mathbb{R}8\xd78$. In this notation $xk+i|kdes$ represents the desired state vector at time *k *+* i* which is a smooth trajectory beginning at *x _{k}* and ending at

*x*, and $||x||P2:=x\u22a4Px$. The stage cost is also defined as $L(xk+i|k,uk+i|k):=||xk+i|k\u2212xk+i|kdes||Q2+||uk+i|k||R2$ where $Q\u2208\mathbb{R}8\xd78$ and $R\u2208\mathbb{R}4\xd74$ are symmetric positive definite matrices.

_{f}*Remark 1*. The matrices *A _{d}* and

*B*together with the vector d are reevaluated at the beginning of each new continuous-time domain using the current state of the original system and the previous optimal input, and are held constant for the duration of the MPC. The same is true for the friction matrices

_{d}*A*and

_{f}*B*, and the friction vector

_{f}*d*. This reduces the MPC into QP that can be effectively solved in real-time (see Sec. 6 for more details).

_{f}*Remark 2*. The optimal COM and tail motions for the reduced-order model that are prescribed by the MPC for the current domain will be utilized as a reference trajectory for the full-order model in Sec. 5. In particular, the low-level nonlinear controller will impose the full-order dynamics to asymptotically track the optimal reduced-order trajectory while imposing all contact forces to remain within the friction cone.

## 5 Low-Level Nonlinear Controller

This section presents the low-level controller (i.e., whole-body motion controller), which is developed based on virtual constraints and QP to track the optimal reduced-order motions generated by the high-level path planner. Specifically, we derive a nonlinear control algorithm which tracks the COM and tail trajectories prescribed by the event-based MPC for the current domain. Here, we extend the low-level nonlinear control algorithm of Ref. [42] to locomotion with robotic tails. While the path planner directly provides torques for the tail, the torques are based upon the linearized model and are not likely to be accurate enough to use on the full-order system. In addition to tracking the trajectories of the COM and the tail, the low-level controller is responsible for coordinating the motion of the swing legs such that they arrive at the next desired foot placement while ensuring the feasibility of all GRFs at the contacting legs.

where $h0(q)$ represents a set of holonomic parameters to be controlled and $hd(s,\alpha )$ denotes the desired evolution of the holonomic outputs *h*_{0} on the gait. In particular, $hd(s,\alpha )$ is taken as a Bézier polynomial that passes through the discrete waypoints generated by the MPC path planner, in which *s* is a gait phasing variable taken as the normalized time and *α* represents the coefficients of the Bézier polynomial. The dimension of the outputs varies depending on the number of contact points with the ground. During quadruple-contact, *h*_{0} consists of 8 components including the COM position, body orientation (i.e., roll, pitch, and yaw), and two internal joints of the tail. The idea here is to impose the full-order system to track the prescribed COM and tail motions while regulating the Euler angles of the floating base. During triple- and double-contact domains, these controlled variables are augmented with the Cartesian coordinates of the swing legs ends for the foot placement. Here, we would like the swing leg ends follow a desired trajectory in the work space starting from the former foothold and ending at the next one. This would increase the dimension of the controlled variables to 11 and 14 during the triple- and double-contact domains, respectively.

*K*and

_{P}*K*are positive definite gain matrices. Furthermore, because the GRFs cannot be measured on the quadruped, we estimate the GRFs through the use of a rigid contact model. This assumes that the acceleration of the foot remains zero after contact which may be expressed as $p\xa8=Jc(qr)\u2009q\xa8r+\u2202\u2202qr(Jc(qr)\u2009q\u02d9r)q\u02d9r=0$. This in tandem with Eq. (5) yields

_{D}*τ*for the robot as well as the tail, while imposing the feasibility of GRFs at all contacting legs. More precisely, we consider the following real-time QP

where $FC$ denotes the friction cone, and $\tau min$ and $\tau max$ represent the lower and upper bounds for the torque inputs, respectively. By formulating the low-level controller in terms of a QP, the controller also becomes more robust to singularities that could arise from matrix inversions during I–O linearization. We remark that the low-level QP for nonlinear control is executed in real-time (e.g., 1 kHz) to compute the optimal torques for the full-order model of locomotion, while the higher-level MPC is solved at a slower rate (i.e., in an event-based manner) for trajectory replanning based on the extended LIP model.

## 6 Numerical Simulations

The objective of this section is to numerically verify the effectiveness of the proposed control algorithm for robust and stable quadrupedal locomotion with robotic tails. We present a multitude of full-order simulations to demonstrate the increased robustness that an articulated tail provides using the proposed control technique. In all cases, we consider a trot gait with start and stop conditions as shown in Fig. 6.

### 6.1 Controller Parameters and Nominal Closed-Loop Simulations.

The extended LIP model is discretized using a sampling time of *T _{s}* = 80 (ms). The high-level MPC computes the optimal reduced-order trajectory over a control horizon of

*n*=

*2 domains, where each domain consists of*

*N*= 4 grid points. We utilize the optimal solution for the current domain, meaning that only the first four optimal trajectory points from the high-level MPC–that correspond to the current domain– are used as inputs to the low-level nonlinear controller. The rest of the parameters for the MPC are taken as $P=diag(104,104,103,103,104,104,1,1),\u2009Q=diag(10,10,10,10,0.1,0.1,0.1,0.1)$, and $R=I4\xd74$, where

_{d}*I*represents the identity matrix and “$diag$” denotes the diagonalization operator. Conceptually, we aim to give the MPC the ability to deviate from the desired tail trajectory (which is always 0) and prioritize tracking the desired COM motion. Additionally, we attempt to drive the tail back to zero at the end of the path planner to allow the tail to begin in an uncompromised position at the start of the next domain. However, we put relatively little weight on the velocity of the tail at the end of each domain, thereby allowing the tail to keep momentum. We begin by simulating the original and extended LIP models integrated with the proposed MPC approach to show the effectiveness of the higher-level controller in rejecting pulse-like disturbances, see Fig. 7. Here, we consider a forward trot gait with 20 domains subject to a pulse disturbance with a magnitude of 30 (N) in the lateral direction (i.e.,

*y*-axis), which is applied for the duration of two grid points, i.e., $2Ts=160$ (ms), in domain 11. The reduced-order simulation is done in MATLAB and uses the Embedded Conoc Solver (ECOS) [75] in order to solve the QP arising from the event-based MPC. The stability of final target point for both the standard LIP (without tail) and the extended LIP (with tail) models subject to the event-based MPC is clear, but it is evident that the closed-loop system using the extended LIP model more adequately attenuates the disturbance. In particular, the system without the tail obtains a maximum displacement from the

*y*-axis of approximately 9.7 (cm) while the system with the tail reaches a maximum displacement of 8 (cm) and settles back to steady-state faster.

Full-order simulations are then done primarily in RaiSim [76], where we utilize qpSWIFT [77] to solve the high- and low-level QPs. Here, the MPC problem (23) is solved at the beginning of each continuous-time domain, that is approximately every $4Ts=320$ (ms), whereas the low-level nonlinear control QP is solved at 1 kHz. We can show that the QP for the higher-level MPC has 120, 112, and 128 decision variables for the first, middle, and last continuous-time domains, respectively. In addition, the typical computation time of the MPC problem on a desktop computer with an Intel^{(R)} Xeon^{(R)} W-2125 CPU at 4.00 GHz (4 cores) and 16 GB of RAM is 0.32 (ms) to 0.42 (ms). The lower-level QP has 40 decision variables for both the double- and quadruple-contact domains of the trot gait with a typical computation time of 0.19 (ms) to 0.3 (ms).

RaiSim uses a universal robot description file (URDF) representation of the system. This presents a hurdle in that URDFs cannot represent holonomically constrained systems. In order to get around this, a URDF is created assuming that each yaw link in the tail is actuated instead of being holonomically constrained. In order to get the same dynamical behavior of the holonomically constrained tail in Fig. 3, the Rigid Body Dynamics Library (RBDL) [78] is then employed to compute the equivalent torque at each yaw joint of the tail via inverse dynamics by using *τ _{t}* and

*λ*computed by the low-level QP.

_{t}We consider three nominal simulations, two of which are conducted in RaiSim, and the third in MATLAB. We consider multiple simulation environments in order to demonstrate the robustness of the closed-loop system against nonparametric uncertainties arising from different contact models. The robot hardware has a small compliant element at the toe, but the provided damping is assumed to be small and the rebound during impact is assumed to be zero. It is therefore reasonable to assume that the compliant dynamics at the toe are negligible when compared to the overall dynamics and impacts may be treated as rigid. For this reason, the majority of the simulations are conducted in RaiSim [76], where a rigid contact model between the leg ends and the ground is used, whereas few simulations are conducted in MATLAB, in which we utilize a LuGre model [79] to represent a compliant contact model. Figures 8(a) and 8(b) depict the output (i.e., virtual constraints) and torque profiles of the quadrupedal robot and tail for forward and diagonal trots in RaiSim. Figure 8(c) shows a full-order simulation of the system run in matlab/simulink subject to LuGre contact model. In addition, Fig. 8(c) illustrates the components of the ground reaction force experienced at one of the legs. For the purpose of this paper, we use a step length of 10 (cm) for the forward trot and step lengths of (7,4) (cm) in the *x* and *y* directions for the diagonal trot.

### 6.2 Robustness Analysis.

In order to study the robustness of the closed-loop system against external disturbances, we consider two different trot gaits, namely forward and diagonal trots. The robustness simulations fall under two different categories, the first being a pulse-like disturbance and the latter being a time-varying disturbance.

#### 6.2.1 Pulse Disturbance Rejection.

In order to analyze the closed-loop system's robustness, we investigate the effects of a pulse perturbation applied to the system for 200 (ms) at the COM. First we consider the forward trot gait with two different pulse disturbances—the first being 75 (N) in the lateral direction and the second being 100 (N) in the forward direction. The virtual constraints and joint-level torques for these simulations are depicted in Figs. 9(a) and 9(b). We also consider two additional simulations with the diagonal trot gait, where the first simulation is subject to a pulse disturbance directly opposing the direction of motion (i.e., along the unit vector $(\u22120.8682,\u22120.4961,0)$), and the second is subject to a pulse disturbance which is orthogonal to the direction of motion (i.e., along the unit vector $(0.4961,\u22120.8682,0)$). The pulse-like forces in the latter two simulations have magnitudes of 75 (N) and 110 (N), respectively, and the corresponding virtual constraints and torques are shown in Figs. 9(c) and 9(d). The same disturbances were then applied to the system without the addition of the tail and the corresponding plots are provided in Fig. 10 to allow direct comparison. Figure 11 furthermore provides snapshots for the simulations found in Figs. 9(a) and 10(a). It is clear that the quadruped robot with the tail and the proposed control algorithm can successfully reach the final target in the presence of the aforementioned disturbance, whereas its counterpart without the tail cannot reach the target destination.

It is important to note that the ability for the closed-loop system to reject a pulse disturbance is greatly influenced by when the disturbance is applied. Specifically, it depends on what point in the gait cycle the robot is currently in, and the state of the tail at that point. With this in mind, we aim to quantify the level of improvement the tail adds by considering a forward trot with a lateral pulse, where the pulse is applied at different times. In this regard, twelve different simulations were conducted, where in each simulation the pulse was applied 0.1 (s) later than the previous simulation. During each test, the largest magnitude pulse that the system was able to withstand while still reaching the target destination was recorded. On average, the system including the tail was able to sustain 61.7 (N) while still being able to reach the target destination which is an increase of 32.6% compared to its tailless counterpart. A similar procedure was done for the diagonal trot but was instead subject to a disturbance orthogonal to the direction of locomotion (i.e., along the unit vector $(0.4961,\u22120.8682,0)$). The system with the tail could reject a pulse of 83.4 (N) on average. This is a 25.6% increase in magnitude compared to the system without the tail.

#### 6.2.2 Time-Varying Disturbance Rejection.

In this section, we consider four additional simulations in which the system is subject to a persistent time-varying disturbance. Similar to the pulse disturbances, we first investigate two forward trots, and then two diagonal trots, each subject to a different persistent disturbance. For each simulation, we note the largest magnitude at which the system can walk for 80 continuous-time domains both with and without the tail in order to compare the results. In the first scenario, we consider a forward trot gait with a lateral disturbance of the form $\beta \u2009sin(4t)$, where the largest magnitude the system with the tail could withstand was $\beta =21.5$ (N), which is 35.2% larger than the system without the tail. We then consider a second simulation of the forward trot gait subject to a disturbance of the form $\beta (sin(4t),\u2009cos(4t))$ in the *x* and *y* directions. In this case, the system with the tail could reject a maximum of *β* = 11 (N) while still being able to walk for 80 domains, up from $\beta =7.4$ (N) without the tail. Figures 12(a) and 12(b) depict the closed-loop system's behavior for the forward trot gait subject to the aforementioned time-varying disturbances. Finally, we consider two different simulations with a diagonal trot, where each system is subject to a disturbance of the form $\beta \u2009sin(4t)$ but in different directions. During the first diagonal simulation, the disturbance acts along the direction of motion (i.e., along the unit vector $(0.8682,0.4961,0)$), and in the latter, the disturbance acts along the unit vector $(0.8682,\u22120.4961,0)$. During these simulations, the system could reject $\beta =14.3$ and *β* = 18 (N), which is an increase of 20.2% and 73% compared to the tailless system, respectively. Figures 12(c) and 12(d) illustrate the time profiles of the virtual constraints and joint torques for these simulations. In order to demonstrate the robustness of the closed-loop system with the tail mechanism, Fig. 13 contains the same simulations having been performed on the system without the tail. In addition, Figs. 14(a) and 14(b) provide snapshots of the simulations corresponding to Figs. 12(b) and 13(b), respectively. Animations of these simulations can be found online.^{5}

## 7 Discussion

The numerical studies and full-order simulations of this paper show significant improvements in rejecting both pulse-like and time-varying disturbances during quadrupedal locomotion integrated with robotic tails and the proposed control algorithms. The amount of improvements depends largely on the gait and the properties of the disturbance applied. The tail mechanism integrated with the proposed hierarchical control algorithm has been shown to allow the closed-loop system to withstand pulse-like disturbances up to 32.6% and 25.6% greater in magnitude during forward and diagonal trots when compared to a system without a tail. Furthermore, the tailed closed-loop system was able to withstand persistent perturbations up to 21.5% and 73% greater in magnitude during forward and diagonal trots, respectively.

It is noteworthy that the tail does not produce aggressive motions in order to reject disturbances. However, because of the nonlinearities of the tail dynamics, it only takes a small angular deflection at the base of the yaw links to deflect the end effector of the tail greatly. In addition, the gaits provided are quasi-static gaits where it stands to reason that the tail motions would be quasi-static as well. In a more aggressive gait, the tail would likely produce faster motions of larger amplitude. Furthermore, the roll of the tail remains relatively constant throughout the gait which indicates that the yaw portion of the tail is more important in maintaining robust and stable locomotion on flat ground.

While increasing the complexity of the reduced-order model to a nonlinear template model such as the inverted pendulum model [80] or spring-loaded inverted pendulum model [19] could potentially offer increased performance, it comes at the cost of increased computational demand. More specifically, these templates necessitate solving a nonlinear programming in real-time for path planning which significantly increases the computational demand compared to working with a linearized model. Furthermore, in the case of quasi-static quadrupedal locomotion, the assumption that the COM height remains constant is a very good approximation as shown by simulation results above, making the LIP model a suitable choice for the reduced-order model. During more agile locomotion where these assumptions might be violated, a more complex reduced-order model could play a larger role in increasing robustness. However, for the quasi-static case with multicontact domains considered here, the additional tail hardware can play a larger role in attenuating disturbance forces applied to the robot than increasing model complexity due to the tails ability to directly apply forces to the body of the robot to reject said disturbances.

The numerical results above have shown that the event-based framework coupled with the assumption that the roll, pitch, and yaw of the body are zero, along with the height of the COM being constant, leads to robust locomotion when maneuvering over flat, even terrain. In particular, under these conditions, linearizing the extended LIP model once per domain is sufficient due to the quasi-static nature of the gait and small deflections of the tail. However, the high-level path planner in its current form would likely need to be altered in order to address more complex environments wherein the terrain is unknown or unstructured. This is largely due to several reasons, the first being that in the current framework the desired foot locations are calculated offline, the second being that moving up or down slopes would inherently violate the assumptions regarding the COM height and body orientation, and finally, the event-based linearization/planning may not be sufficient to adequately plan for locomotion along unstructured terrain considering higher planning rates are typically required when the environment is not well known.

## 8 Conclusion

This paper presented a formal foundation to design hierarchical and nonlinear feedback control algorithms that effectively couple bio-inspired articulated tails with legged robots to produce robust locomotion patterns in the presence of external disturbances. The proposed control approach utilizes a high-level and real-time path planner, based on an event-based MPC, to compute the optimal robot's COM and tail trajectories for an innovative reduced-order model, referred to as the extended LIP dynamics. The MPC problem considers the feasibility of the net GRF and the tail's motion during the steering problem of the extended LIP model. A nonlinear whole-body motion controller, based on virtual constraints and QP, is then utilized in the lower-level of the control scheme to impose the full-order dynamical model of locomotion to track the optimal and reduced-order trajectories prescribed by the MPC, while ensuring the feasibility of the GRFs at the contacting legs. The potential of the proposed control approach was demonstrated through a variety of full-order simulations for an 18DOF quadruped robot augmented with a 2DOF serpentine tail. It is shown that the proposed control solution integrated with the tail dynamics can significantly reduce the effect of disturbances on quadrupedal locomotion.

For future research, we will extend the framework to more agile gaits wherein the tail could have an even greater impact on the overall stability of locomotion. We will also study alternative tail designs with different structures and DOF to further investigate the effects of tails on quadrupedal locomotion. Moreover, we will experimentally employ the developed control algorithms for locomotion of the Vision 60 platform coupled with a robotic tail. Finally, we will consider extending the proposed framework to uneven, unstructured, and sloped terrain to allow for greater versatility.

## Acknowledgment

This material is based upon work supported by the National Science Foundation (NSF) under Grant Numbers CMMI-1906727 and CMMI-1923216. The content is solely the responsibility of the authors and does not necessarily represent the official views of the NSF.

## Funding Data

National Science Foundation (NSF) (Grant Nos. CMMI-1906727 and CMMI-1923216; Funder ID: 10.13039/100000001).

## Nomenclature

- $B(.),\u2009Jb(.)$ =
input distribution matrix, Jacobian matrix at the base of the tail

- $D(.),\u2009H(.)$ =
mass inertia matrix, nonlinear vector (i.e. Coriolis, centrifugal, and gravitational terms)

- (
*f*,*g*,*w*) =vector fields of the affine continuous-time dynamics

*F*,_{t}*M*=_{t}forces at the base of the tail, moments at the base of the tail

*F*,_{x}*F*,_{y}*F*=_{z}forces in the

*x*,*y*, and*z*directions- $LgLf,LwLf,Lf2$ =
lie derivatives along the continuous-time dynamics

*M*,*n*=total number of continuous-time domains, number of continuous-time domains to plan over

*M*,_{r}*M*,_{p}*M*=_{y}moments in the roll, pitch, and yaw directions

*N*,_{d}*N*,_{c}*T*=_{s}number of grid points to plan over, control horizon, time-step between grid points

*q*,_{r}*q*,_{t}*z*=configuration and state variables for the robot, the tail, and the augmented system

*W*, $A(.)$*, N, d*=extended LIP weighting matrix, state matrix, nonlinear vector function, and vector of constants

*x*,*u*=state variables for the extended LIP model including tail dynamics, and inputs to the extended LIP model including tail dynamics

*x*_{0}, $x\u02d90$,*u*_{0}=operating points around which the augmented LIP model is linearized

- $xko,\u2009x\u02d9ko,\u2009uko$ =
state and input variables for the original and extended reduced-order system at time

*k* *y*,*h*_{0},*h*=_{d}holonomic output function, holonomic control variables, and desired evolution of controlled variables

- $\zeta [k],\u2009U\zeta [k]$ =
current domain function, convex hull formed by stance legs

- $\theta roll,\u2009\theta yaw$ =
internal roll and yaw angles of the tail

*λ*,_{c}*λ*=_{t}ground reaction forces, tail wrench

- $\tau roll,\u2009\tau yaw$ =
torques applied to the roll and yaw actuators of the tail

*τ*,_{r}*τ*,_{t}*τ*=inputs to the robot, the tail, and the augmented system

- $(.)com,\u2009(.)cop$ =
variables corresponding to the COM having been projected onto the

*xy*-plane, and variables corresponding to the center of pressure (COP) - $(.)r,\u2009(.)t$ =
variables corresponding to the robot, and variables corresponding to the tail

- $(.)\u2113,\u2009(.)d$ =
variables corresponding to the linearized system, and variables corresponding to the discretized system

## Footnotes

Due to the nature of the floating base coordinates, the upper-left 3 × 3 block of the *D* matrix is diagonalized.