Distributed Feedback Controllers for Stable Cooperative Locomotion of Quadrupedal Robots: A Virtual Constraint Approach

TL;DR

This paper develops distributed feedback control algorithms enabling stable cooperative locomotion of quadrupedal robots with holonomic constraints, using virtual constraints and nonlinear control, demonstrated through extensive numerical simulations.

Contribution

It introduces a novel approach to distributed control of cooperative legged robots using virtual constraints and quadratic programming, addressing complex hybrid dynamical models.

Findings

Successful stabilization of cooperative locomotion in simulations

Development of optimal distributed feedback controllers

Handling complex hybrid dynamical models with high-dimensional states

Abstract

This paper aims to develop distributed feedback control algorithms that allow cooperative locomotion of quadrupedal robots which are coupled to each other by holonomic constraints. These constraints can arise from collaborative manipulation of objects during locomotion. In addressing this problem, the complex hybrid dynamical models that describe collaborative legged locomotion are studied. The complex periodic orbits (i.e., gaits) of these sophisticated and high-dimensional hybrid systems are investigated. We consider a set of virtual constraints that stabilizes locomotion of a single agent. The paper then generates modified and local virtual constraints for each agent that allow stable collaborative locomotion. Optimal distributed feedback controllers, based on nonlinear control and quadratic programming, are developed to impose the local virtual constraints. To demonstrate the power of the analytical foundation, an extensive numerical simulation for cooperative locomotion of two quadrupedal robots with robotic manipulators is presented. The numerical complex hybrid model has 64 continuous-time domains, 192 discrete-time transitions, 96 state variables, and 36 control inputs.

Equations

$$D(q)\ddot{q}$$

$$J_v^c(q)\ddot{q}$$

$$D(q)\left({\dot{q}}^{+}-{\dot{q}}^{-}\right)=J_{\mu (v)}^{c\top }\delta {\lambda }^c,J_{\mu (v)}^c(q){\dot{q}}^{+}=0,$$

$${\Vert p^e(q_1)-p^e(q_2)\Vert }_2^2=\text{constant}.$$

$$D_1{\ddot{q}}_1+H_1=B{u}_1+J_{v,1}^{c\top }{\lambda }_1^c+J_1^{e\top }\left(p_1^e-p_2^e\right){\lambda }^e$$

$$D_2{\ddot{q}}_2+H_2=B{u}_2+J_{w,2}^{c\top }{\lambda }_2^c+J_2^{e\top }\left(p_2^e-p_1^e\right){\lambda }^e$$

$$J_{v,1}^c{\ddot{q}}_1+{\dot{J}}_{v,1}^c=0$$

$$J_{w,2}^c{\ddot{q}}_1+{\dot{J}}_{w,2}^c=0$$

$${\left(p_1^e-p_2^e\right)}^{\top }\left(J_1^e{\ddot{q}}_1-J_2^e{\ddot{q}}_2\right)+{\Vert {\dot{p}}_1^e-{\dot{p}}_2^e\Vert }^2=0,$$

$$D_1\left({\dot{q}}_1^{+}-{\dot{q}}_1^{-}\right)={\hat{J}}_{v\to v^{\prime },1}^{c\top }\delta {\lambda }_1^c+J_1^{e\top }\left(p_1^e-p_2^e\right)\delta {\lambda }^e$$

$$D_2\left({\dot{q}}_2^{+}-{\dot{q}}_2^{-}\right)={\hat{J}}_{w\to w^{\prime },2}^{c\top }\delta {\lambda }_2^c+J_2^{e\top }\left(p_2^e-p_1^e\right)\delta {\lambda }^e$$

$${\hat{J}}_{v\to v^{\prime },1}^c{\dot{q}}_1^{+}=0$$

$${\hat{J}}_{w\to w^{\prime },2}^c{\dot{q}}_2^{+}=0$$

$${\left(p_1^e-p_2^e\right)}^{\top }\left(J_1^e{\dot{q}}_1^{+}-J_2^e{\dot{q}}_2^{+}\right)=0,$$

$$u_i={\Gamma }_{v,w}^i\left(t,x_i,\Theta ,{\xi }_i\right),(v,w)\in {\mathcal{V}}^a$$

$${\Gamma }_{v,v}^i\left(t,x_i,\Theta ,{\xi }_i\right)={\Gamma }_v^{\text{nom}}\left(t,x_i\right),\forall x^a\in {\mathcal{O}}_d^a,\forall t\ge 0.$$

$$y_v(t,x):=\left[\begin{array}{c}{y}_v^{\text{nh}}(t,q,\dot{q})\\ y_v^{\text{h}}(t,q)\end{array}\right]:=\left[\begin{array}{c}s(q,\dot{q})-s^{\star }(\tau ,v)\\ C_v\left(q-q^{\star }(\tau ,v)\right)\end{array}\right],$$

$$\left[\begin{array}{c}{\dot{y}}_v^{\text{nh}}\\ {\ddot{y}}_v^{\text{h}}\end{array}\right]=\underset{=:A_v(t,x)}{\underbrace{\left[\begin{array}{c}{\text{L}}_{g_v}{y}_v^{\text{nh}}\\ {\text{L}}_{g_v}{\text{L}}_{f_v}{y}_v^{\text{h}}\end{array}\right]}}u+\underset{=:b_v(t,x)}{\underbrace{\left[\begin{array}{c}{\text{L}}_{f_v}{y}_v^{\text{nh}}\\ {\text{L}}_{f_v}^2{y}_v^v\end{array}\right]}}=-\underset{=:e_v(t,x)}{\underbrace{\left[\begin{array}{c}{K}_p{y}_v^{\text{nh}}\\ K_p{y}_v^{\text{h}}+K_d{\dot{y}}_v^{\text{h}}\end{array}\right]}},$$

$$u={\Gamma }_v^{\text{nom}}(t,x):=-A_v^{\top }{\left(A_v{A}_v^{\top }\right)}^{-1}\left(b_v+e_v\right)$$

$$\Theta :=\text{col}\left(s_1,s_2,q_1^{\text{roll}},q_2^{\text{roll}},q_1^{\text{pitch}},q_2^{\text{pitch}},{\dot{q}}_1^{\text{roll}},{\dot{q}}_2^{\text{roll}},{\dot{q}}_1^{\text{pitch}},{\dot{q}}_2^{\text{pitch}}\right)$$

$$y_{v,w}^i\left(t,x_i,\Theta ,{\xi }_i\right):$$

$$-\left[\begin{array}{c}{\alpha }_{v,w}\left(s_j-s^{\star }(\tau ,w\right)\\ C_{v,w}^{\text{roll}}\left(q_j^{\text{roll}}-q^{\star ,\text{roll}}\left(\tau ,w\right)\right)\end{array}\right]$$

$$-\left[\begin{array}{c}0\\ C_{v,w}^{\text{pitch}}\left(q_j^{\text{pitch}}-q^{\star ,\text{pitch}}\left(\tau ,w\right)\right)\end{array}\right],$$

$${\xi }_i:={\left\{{\alpha }_{v,w},C_{v,w}^{\text{roll}},C_{v,w}^{\text{pitch}}\right\}}_{(v,w)\in {\mathcal{V}}^a}.$$

$$\left[\begin{array}{cc}{D}_1& 0\\ 0& D_2\end{array}\right]\left[\begin{array}{c}{\ddot{q}}_1\\ {\ddot{q}}_2\end{array}\right]+\left[\begin{array}{c}{H}_{v,w,1}^a\\ H_{v,w,2}^a\end{array}\right]=\left[\begin{array}{cc}{B}_{v,w,11}^a& B_{v,w,12}^a\\ B_{v,w,21}^a& B_{v,w,22}^a\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right],$$

$$D_i{\ddot{q}}_i+{\hat{H}}_{v,w,i}^a={\hat{B}}_{v,w,ii}^a{u}_i+{\hat{B}}_{v,w,ij}^a{u}_j^{\star }(t,w),$$

$$A_{v,w}^i\left(t,x_i,\Theta ,{\xi }_i\right)u_i$$

$$=-e_{v,w}^i\left(t,x_i,\Theta ,{\xi }_i\right),$$

$$\underset{(u_i,\delta )}{\min }$$

$$A_{v,w}^i{u}_i+b_{v,w}^i+\delta =-e_{v,w}^i$$

$$u_{\min }\le u_i\le u_{\max },{\delta }_{\min }\le \delta \le {\delta }_{\max },$$

$$C_{v,w}^{\text{roll}}={\beta }_{v,w}{C}_v^{\text{roll}}\text{and}{C}_{v,w}^{\text{pitch}}={\gamma }_{v,w}{C}_v^{\text{pitch}},\text{vspace}-0.4em$$

Full text

I INTRODUCTION

This paper aims to develop a formal foundation, based on hybrid systems theory, nonlinear control, and quadratic programming (QP), to develop distributed feedback control algorithms that stabilize cooperative locomotion of quadrupedal robots while steering objects. Legged robots that are augmented with manipulators can form collaborative robot (co-robot) teams that assist humans in different aspects of their life such as labor-intensive tasks, construction, and manufacturing. Although important theoretical and technological advances have allowed the development of distributed controllers for motion control of complex robot systems, state-of-the-art approaches address the control of multiagent systems composed of collaborative robotic arms [1], multifingered robot hands [2], aerial vehicles [3, 4], and ground vehicles [5, 6, 7], but not cooperative legged agents. Legged robots are inherently unstable, as opposed to most of the systems where these algorithms have been deployed. Furthermore, the evolution of legged co-robot teams that cooperatively manipulate objects can be represented by high-dimensional and complex hybrid dynamical systems which complicate the design of distributed control algorithms.

I-A Related Work and Motivation

Hybrid systems theory has become a powerful tool to design nonlinear controllers for dynamic legged locomotion both in theory and practice [8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23]. Nonlinear controllers that address hybrid nature of legged locomotion has come out of hybrid reduction [24], controlled symmetries [19], transverse linearization [20], and hybrid zero dynamics (HZD) [9, 11]. HZD-based controllers have been numerically and experimentally evaluated for bipedal locomotion [13, 25, 26, 27, 11, 12, 20, 28], quadrupedal locomotion [29, 30], powered prosthetic legs [31], and exoskeletons [32].

Existing nonlinear control approaches for legged robots are tailored to the path planning and stabilization of dynamic gaits for one legged machine, but not complex hybrid dynamical models that describe the evolution of multiagent legged robotic systems. This is mainly due to the fact that state-of-the-art techniques for dynamic locomotion are centralized approaches that cannot be easily transferred to legged co-robot teams. A legged co-robot team that manipulates an object can be modeled as a set of legged robots which are coupled to each other and the object by a set of holonomic constraints. The question is how to construct controllers for such a complex robotic system that control locomotion with many DOFs and large amounts of sensory data? Computing the control torques for such a composite system in 1kHz is often impossible—this motivates the importance of developing distributed and decentralized controllers that address lower-dimensional subsystems (e.g., each agent).

I-B Objectives and Contributions

The objectives and contributions of this paper are as follows. We address complex hybrid models that describe cooperative locomotion of legged robotic systems. The properties as well as periodic solutions (i.e., complex gaits) of these sophisticated hybrid dynamical models are investigated. We develop a distributed feedback control algorithm, based on HZD and distributed QPs, to stabilize cooperative gaits. We study virtual constraints and nominal HZD-based controllers that stablize locomotion of one agent. We then modify the virtual constraint controllers for the cooperative locomotion of two agents. A QP formulation is set up to compute the optimal distributed controllers that are close to the nominal HZD controllers of each agent while imposing the modified virtual constraints. To demonstrate the power of the analytical foundation, an extensive numerical simulation of two quadrupedal agents (Vision 60 robots) with Kinova arms steering an object is presented (see Fig. 1). In this simulation, the complex hybrid model has 64 continuous-time domains, 192 discrete transitions, 96 state variables, and 36 control inputs. Our previous work in [33, 34] developed an optimization algorithm, based on Poincaré return maps and linear and bilinear matrix inequalities (LMIs and BMIs) to synthesize decentralized controllers for legged locomotion. However, the Jacobian linearization of the Poincaré map for the above-mentioned complex hybrid model with 64 continuous-time models and 96 state variables is computationally intensive. The current paper addresses this complexity with the proper modification of local virtual constraints and the QP formulation. The current work is different from [35] which addresses cooperative locomotion of guide robots and humans. In particular, [35] considers a leash structure with a “variable” and “controlled” length and angle that stabilize the cooperative locomotion of quadrupedal robots and humans. In the current study, the object is “passive” with a “fixed length”. The paper instead develops distributed controllers for each agent that stabilize the cooperative locomotion subject to holonomic constraints. The current work is also different from the study presented in [21] for locomotion adaptation of limit cycle bipedal walkers in leader/follower collaborative tasks. The current paper addresses complex models of two agents while designing local and optimal distributed controllers for stable collaborative locomotion. Reference [21] only considers the follower dynamics while designing a switching based controller for its adaptation to a persistent external force that represents the leader.

II MODELS OF COOPERATIVE LOCOMOTION

II-A Hybrid Model of Locomotion for One Agent

We consider a full-order dynamical model of Vision 60 that is augmented with a Kinova arm for the locomotion and manipulation purposes (see Fig. 1). Vision 60 is an autonomous quadrupedal robot that is designed and manufactured by Ghost Robotics111https://www.ghostrobotics.io/. Vision 60 has 18 DOFs of which 12 leg DOFs are actuated. In particular, each leg of the robot consists of a 1 DOF actuated knee joint with pitch motion and a 2 DOF actuated hip joint with pitch and roll motions. The remaining 6 DOFs are associated with the translational and rotational movements of the body. In this paper, we consider a 6 DOF Kinova arm that is affixed on Vision 60. The generalized coordinates vector of each agent is represented by $q:=\textrm{col}(p_{b},\phi_{b},q_{\textrm{body}})\in\mathcal{Q}\subset\mathbb{R}^{24}$, where $p_{b}\in\mathbb{R}^{3}$ and $\phi_{b}\in\mathbb{R}^{3}$ denote the absolute position and orientation (i.e., roll-pitch-yaw) of the robot with respect to a world frame, respectively. In addition, $q_{\textrm{body}}$ denotes a set of coordinates that describe the shape of Vision 60 together with the arm, and $\mathcal{Q}$ is the configuration space. The state and control inputs (i.e., joint torques) of the system are denoted by $x:=\textrm{col}(q,\dot{q})\in\mathcal{X}:=\textrm{T}\mathcal{Q}\subset\mathbb{R}^{48}$ and $u\in\mathcal{U}\subset\mathbb{R}^{18}$, respectively. In our notation, $\mathcal{X}$ and $\mathcal{U}$ represent the state manifold and set of admissible controls.

Throughout this paper, we will consider a hybrid systems formulation to describe multi-domain quadrupedal locomotion for each agent as $\Sigma\left(\mathcal{G},\mathcal{D},\mathcal{S},FG,\Delta\right)$, in which $\mathcal{G}=(\mathcal{V},\mathcal{E})$ denotes the directed cycle corresponding to the desired locomotion pattern with the vertices set $\mathcal{V}$ and the edges set $\mathcal{E}\subseteq\mathcal{V}\times\mathcal{V}$. In our formulation, the vertices represent the continuous-time domains of quadrupedal locomotion that are described by ordinary differential equations (ODEs) arising from the Lagrangian dynamics. The edges represent the discrete-time transitioning between continuous-time domains arising from the changes in physical constraints. The evolution of the mechanical system during the domain $v\in\mathcal{V}$ is described by the ODE $\dot{x}=f_{v}(x)+g_{v}(x)\,u$ for all $(x,u)\in\mathcal{D}$, where $\mathcal{D}$ represents the domains of admissibility. The set of control systems is given by $FG:=\{(f_{v},g_{v})\}_{v\in\mathcal{V}}$. The evolution of the system during the discrete-time transition $e\in\mathcal{E}$ is then described by the reset law (i.e., reinitialization rule) $x^{+}=\Delta_{e}(x^{-})$, where $x^{-}$ and $x^{+}$ represent the state of the system right before and after the transition, respectively. Moreover, $\Delta:=\{\Delta_{e}\}_{e\in\mathcal{E}}$ is the set of discrete-time dynamics. Finally, the guards of the hybrid system $\Sigma$ is given by the switching manifolds $\mathcal{S}:=\{\mathcal{S}_{e}\}_{e\in\mathcal{E}}$ on which the discrete-time transitions occur.

Continuous-Time Dynamics: Let us assume that $J_{v}^{c}(q)$ denotes the contact Jacobian matrix during the continuous-time domain $v\in\mathcal{V}$, where the superscript “$c$” stands for the contact. Then, the evolution of the mechanical system in domain $v$ can be described by the following ODE:

[TABLE]

where $D(q)$ is the mass-inertia matrix, $H(q,\dot{q})$ represents the Coriolis, centrifugal, and gravitational terms, $B$ is the input distribution matrix, $\lambda^{c}$ denotes the ground reaction forces (i.e., Lagrange multipliers), and $\dot{J}_{v}^{c}(q,\dot{q}):=\frac{\partial}{\partial q}(J_{v}^{c}(q)\,\dot{q})\,\dot{q}$. By eliminating $\lambda^{c}$, one can express (1) and (2) as the input-affine system $\dot{x}=f_{v}(x)+g_{v}(x)\,u$.

Discrete-Time Dynamics: The next-domain function $\mu:\mathcal{V}\rightarrow\mathcal{V}$ is defined as $\mu(v)=w$ if the vertices $v$ and $w$ are adjacent on $\mathcal{G}$, or equivalently, if $e=(v\rightarrow{}w)\in\mathcal{E}$. If during the discrete transition $e$, an existing contact breaks, we define the reset law $\Delta_{e}(x)$ as identity to preserve the continuity of position and velocity. However, if a new contact point is added to the existing set of contacts points with the ground, we make use of the rigid impact model as follows [36]

[TABLE]

to model the abrupt changes in the velocity components according to the impact. Here, $\delta\lambda^{c}$ represents the intensity of the impulsive ground reaction forces. From (3) and continuity of position, one can obtain the impact map as $x^{+}=\Delta_{e}(x^{-})$.

II-B Continuous-Time Models for Cooperative Locomotion

In this section, we will address models of continuous-time domains that describe cooperative locomotion of two agents while steering an object. In our notation, the subscript $i\in\{1,2\}$ represents the agent number. The state variables and control inputs for the agent $i$ are represented by $x_{i}:=\textrm{col}(q_{i},\dot{q}_{i})\in\mathcal{X}$ and $u_{i}\in\mathcal{U}$, respectively. To simplify the analysis, we will assume that the agents are identical. Let $p^{e}(q_{i})\in\mathbb{R}^{3}$ denote the Cartesian coordinates of the end effector (EE) with respect to the world frame. We consider a holonomic constraint between the EEs as follows (see Fig. 2a):

[TABLE]

Equation (4) states that the Euclidean distance between the agents’ EEs is constant. In particular, we assume that the agents are carrying a massless bar with a fixed length whose ends are connected to the agents’ EEs via socket (i.e., ball) joints. Differentiating (4) results in $(p_{1}^{e}-p_{2}^{e})^{\top}(J^{e}_{1}\,\dot{q}_{1}-J_{2}^{e}\,\dot{q}_{2})=0$ and $(p_{1}^{e}-p_{2}^{e})^{\top}(J^{e}_{1}\,\ddot{q}_{1}-J_{2}^{e}\,\ddot{q}_{2})+\|\dot{p}_{1}^{e}-\dot{p}_{2}^{e}\|^{2}=0$, where $J^{e}(q):=\frac{\partial p^{e}}{\partial q}(q)$, $p_{i}^{e}:=p^{e}(q_{i})$, $J_{i}^{e}:=J^{e}(q_{i})$, and $\dot{p}_{i}^{e}=J_{i}^{e}\,\dot{q}_{i}$ for $i\in\{1,2\}$. Let us assume that $\lambda^{e}\in\mathbb{R}$ denotes the Lagrange multiplier corresponding to the holonomic constraint (4). Suppose further that the agents 1 and 2 are in the continuous-time domains $v\in\mathcal{V}$ and $w\in\mathcal{V}$, respectively. Then, the equations of motions can be described by the following coupled and constrained dynamics:

[TABLE]

where $D_{i}:=D(q_{i})$, $H_{i}:=H(q_{i},\dot{q}_{i})$, $J_{v,i}^{c}:=J_{v}^{c}(q_{i})$, and $\dot{J}_{v,i}^{c}:=\dot{J}_{v}^{c}(q_{i},\dot{q}_{i})$ for $i\in\{1,2\}$. We remark that in (5) and (6), $(p_{1}^{e}-p_{2}^{e})\,\lambda^{e}\in\mathbb{R}^{3}$ and $(p_{2}^{e}-p_{1}^{e})\,\lambda^{e}\in\mathbb{R}^{3}$ represent the forces associated with the holonomic constraint (4) that are aligned with the bar and applied to the EEs of the agents 1 and 2, respectively. By eliminating the Lagrange multipliers $\lambda_{1}^{c}$, $\lambda_{2}^{c}$, and $\lambda^{e}$ from (5)-(9), one can express the evolution of the composite mechanical system by $\dot{x}^{a}=f_{v,w}^{a}\left(x^{a}\right)+g_{v,w}^{a}\left(x^{a}\right)u^{a}$, where the superscript “$a$” stands for the augmented system, and $x^{a}:=\textrm{col}(x_{1},x_{2})\in\mathcal{X}\times\mathcal{X}$ and $u^{a}:=\textrm{col}(u_{1},u_{2})\in\mathcal{U}\times\mathcal{U}$ denote the augmented state and control inputs, respectively.

II-C Discrete-Time Models for Cooperative Locomotion

This section addresses the discrete-time transition for the composite mechanical system. We consider a general case in which the agents 1 and 2 can switch from any continuous-time domains $v$ and $w$ to $v^{\prime}$ and $w^{\prime}$, respectively. This discrete-time transition is denoted by $(v,w)\rightarrow(v^{\prime},w^{\prime})$. We define the extended contact Jacobian matrix for the agent 1 as $\hat{J}_{v\rightarrow{v^{\prime}}}^{c}(q_{1})$ by $\hat{J}_{v\rightarrow{v^{\prime}}}^{c}(q_{1}):=J_{v}^{c}(q_{1})$ if $v^{\prime}=v$ and $\hat{J}_{v\rightarrow{v^{\prime}}}^{c}(q_{1}):=J_{\mu(v)}^{c}(q_{1})$ for $v^{\prime}\neq{v}$. An analogous extended contact Jacobian matrix $\hat{J}_{w\rightarrow{w^{\prime}}}^{c}(q_{2})$ can be defined for the agent 2. The evolution of the composite mechanical system over the infinitesimal period of the impact can be then described by the following coupled dynamics

[TABLE]

where $\delta\lambda^{c}_{1}$, $\delta\lambda^{c}_{2}$, and $\delta\lambda^{e}$ represent the intensity of the impulsive Lagrange multipliers at the leg ends and EEs. By eliminating the Lagrange multipliers from (10)-(14), one can obtain an augmented reset law as $x^{a+}=\Delta_{(v,w)\rightarrow(v^{\prime},w^{\prime})}^{a}(x^{a-})$.

II-D Complex Hybrid Model for Cooperative Locomotion

The complex hybrid model that describes the cooperative locomotion of two robots will have a complex graph that is taken as the strong product of graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$ with itself. In particular, this strong product is represented by $\mathcal{G}^{a}:=\mathcal{G}\boxtimes\mathcal{G}=(\mathcal{V}^{a},\mathcal{E}^{a})$ that has the vertices set $\mathcal{V}^{a}:=\mathcal{V}\times\mathcal{V}$. Furthermore to define the edges set $\mathcal{E}^{a}$, we remark that any $e=\left((v,w)\rightarrow(v^{\prime},w^{\prime})\right)\in\mathcal{E}^{a}$ if and only if 1) $v=v^{\prime}$ and $(w\rightarrow{}w^{\prime})$ is an edge in $\mathcal{E}$, or 2) $(v\rightarrow{}v^{\prime})$ is an edge in $\mathcal{E}$ and $w=w^{\prime}$, or 3) $(v\rightarrow{}v^{\prime})$ is an edge in $\mathcal{E}$ and $(w\rightarrow{}w^{\prime})$ is an edge in $\mathcal{E}$. Finally, the complex hybrid model is defined as $\Sigma^{a}\left(\mathcal{G}^{a},\mathcal{D}^{a},\mathcal{S}^{a},FG^{a},\Delta^{a}\right)$, in which $FG^{a}:=\{(f_{v,w}^{a},g_{v,w}^{a})\}_{(v,w)\in\mathcal{V}^{a}}$, $\Delta^{a}:=\{\Delta^{a}_{e}\}_{e\in\mathcal{E}^{a}}$, and $\mathcal{D}^{a}$ and $\mathcal{S}^{a}$ denote the augmented sets of admissibility and switching surfaces, respectively.

Example 1

In this paper, we will consider walking gait of Vision 60 whose graph $\mathcal{G}$ is assumed to have $8$ continuous-time domains and $8$ discrete-time domains. It can be shown that $\mathcal{G}^{a}:=\mathcal{G}\boxtimes\mathcal{G}$ has $64$ continuous-time domains and $192$ discrete-time domains (see 2a).

III DISTRIBUTED FEEDBACK CONTROLLERS

We will consider complex periodic locomotion of the composite robot. This section aims to present the structure of the proposed distributed feedback control algorithms that will stabilize complex dynamic gaits for cooperative locomotion of two legged robots. We will also investigate some properties of the complex hybrid model. Sections IV and V will synthesize distributed controllers for the cooperative locomotion. We define a periodic gait for one agent as follows.

Assumption 1

(Periodic Gait of One Agent): We suppose that there is a family of nominal state feedback laws $\Gamma^{\textrm{nom}}(t,x):=\{\Gamma_{v}^{\textrm{nom}}(t,x)\}_{v\in\mathcal{V}}$ that generates a periodic orbit (i.e., gait) for the hybrid model of one agent $\Sigma$. In particular, there is a periodic state trajectory (i.e., solution) $\varphi^{\star}:[0,\infty)\rightarrow\mathcal{X}$ for $\Sigma$ if we apply the nominal state feedback law $u=\Gamma_{v}^{\textrm{nom}}(t,x)$ during the continuous-time domain $v$ for all $v\in\mathcal{V}$. The corresponding periodic orbit is given by $\mathcal{O}:=\{x=\varphi^{\star}(t)\,|\,0\leq{t}<T\}$ for some fundamental period $T>0$.

We then present the concept of measurable global variables for both agents (i.e., subsystems) as follows.

Assumption 2

(Measurable Global Variables): There is a set of quantities $\Theta$ that (i) depend on the global state variables $x^{a}=\textrm{col}(x_{1},x_{2})$, i.e., $\Theta=\Theta(x^{a})$ and (ii) are measurable for both subsystems via sensors. These variables are referred to as the measurable global variables. We further assume that agents know the domain numbers of each other. In particular, $(v,w)\in\mathcal{V}^{a}$ is known for both agents.

Using this hypothesis, we propose a parameterized family of local controllers for the agent $i\in\{1,2\}$ as follows

[TABLE]

(see Fig. 2b). Here, $\Gamma^{i}$ is a local state feedback law that has access to 1) the local state variables of the agent $i$, i.e., $x_{i}$, 2) the measurable global variables $\Theta(x^{a})$, and 3) the domain number of the other agent that is denoted by $w$. Furthermore, this local feedback law is parameterized by a set of adjustable local controller parameters $\xi_{i}$ to achieve stability of the complex gait. More specifically, we will show that the stability of the cooperative gaits will depend on the proper selection of local controller parameters $\xi_{i}$, $i\in\{1,2\}$.

Before we present the complex periodic gaits for cooperative locomotion of two agents, we consider the following assumption on distributed feedback controllers.

Assumption 3

The family of local feedback laws $\Gamma^{i}_{v,w}(t,x_{i},\Theta,\xi_{i})$ for $i\in\{1,2\}$ do not depend on the horizontal displacements (i.e., Cartesian coordinates) of the robots on the walking surface. However, they are allowed to depend on the translational velocities of the robots.

Definition 1

(Translation Operator): We define the translation operator on $\mathcal{X}$ by $T_{d}(x)=T_{d}(q,\dot{q})$ which takes the state vector $x$ and adds the vector $d\in\mathbb{R}^{2}$ to its Cartesian positions in the horizontal plane. This corresponds to moving the robot on the walking surface by the vector $d$ while keeping the other state variables of the robot unchanged.

Now we are in a position to present the following result for complex gaits of cooperative locomotion.

Theorem 1

(Complex Periodic Orbits): Suppose that Assumptions 1-3 are satisfied and we employ the local feedback laws (15). Let $d\in\mathbb{R}^{2}-\{0\}$ be a vector and define the augmented trajectory as $\mathcal{O}_{d}^{a}:=\{x^{a}\,|\,x_{1}=\varphi^{\star}(t),\,x_{2}=T_{d}\left(\varphi^{\star}(t)\right),\,0\leq{t}<T\}$. Assume that the distributed feedback controllers $\Gamma^{i}_{v,w}(t,x_{i},\Theta,\xi_{i})$, when evaluated on the augmented orbit $\mathcal{O}^{a}_{d}$, are reduced to the nominal state feedback laws for each agent, that is, for every $i\in\{1,2\}$ and $v\in\mathcal{V}$,

[TABLE]

Then, $\mathcal{O}_{d}^{a}$ is a periodic orbit for the complex model $\Sigma^{a}$.

Proof:

Choose an arbitrary $x_{1}\in\mathcal{O}$ and let $v$ be the corresponding continuous-time domain vertex for the agent 1. Then, $x_{2}=T_{d}(x_{1})\in{}T_{d}(\mathcal{O})$ and $w=v$ is the domain vertex for the agent 2. For these state values, $p_{2}^{e}=p_{1}^{e}+\textrm{col}(d,0)$ and $\dot{p}_{2}^{e}=\dot{p}_{1}^{e}$. Consequently, the holonomic constraints (4) and its first-order time derivative are satisfied. We need to show that its second order time-derivative is also met. From (16), $u_{1}=\Gamma_{v,v}^{1}(t,x_{1},\Theta,\xi_{1})=\Gamma_{v}^{\textrm{nom}}(t,x_{1})$. This together with Assumption 3 implies that $u_{2}=\Gamma_{v}^{\textrm{nom}}(t,T_{d}(x_{1}))=\Gamma_{v}^{\textrm{nom}}(t,x_{1})=u_{1}$. Hence, the Lagrange multiplier $\lambda^{e}$ in (5) and (6) can be zero and thereby, $\ddot{p}_{2}^{e}=\ddot{p}_{1}^{e}$. This renders $\mathcal{O}_{d}^{a}$ invariant under the augmented continuous-time dynamics of the complex model. An analogous reasoning can be presented for the invariance under the augmented discrete-time dynamics. Consequently, $\mathcal{O}_{d}^{a}$ is an augmented periodic orbit for the system. ∎

IV HZD-BASED NOMINAL CONTROLLERS

This section presents the nominal controllers that generate the periodic locomotion $\mathcal{O}$ for each agent. The modification of these feedback laws to develop the distributed feedback controllers for cooperative locomotion of two agents will be presented in Section V. Since the agents are identical, we drop the subscript $i\in\{1,2\}$ to simplify the presentation. During the continuous-time domain $v\in\mathcal{V}$, we consider virtual constraints with nonholonomic (i.e., relative degree one) and holonomic (i.e., relative degree two) components. The nonholonomic component is used to regulate the speed of the robot, wheres the holonomic component is used for position control. We employ standard input-output (I-O) linearization [37] to asymptotically impose virtual constraints.

Consider an output function for the domain $v$ as $y_{v}(t,x)$ that can be decomposed as follows:

[TABLE]

in which the superscripts “nh” and “h” stand for the nonholonomic and holonomic components, respectively. In (17), $s(q,\dot{q})\in\mathbb{R}$ represents the forward speed of a point on the robot (i.e, head of the robot), $s^{\star}(\tau,v)$ denotes the desired evolution of the speed on the orbit $\mathcal{O}$ in terms of the gait timing variable and the continuous-time domain $v\in\mathcal{V}$, $\tau$ denotes the gait timing variable (i.e., phasing variable) that is taken as the scaled time for each domain, and $q^{\star}(\tau,v)$ represents the desired evolution of the configuration variables on $\mathcal{O}$ during the domain $v$. Finally, $C_{v}$ is an output matrix that affects the stability of the gait. In particular, our previous work has shown that the proper selection of the output matrix $C_{v}$ can change the stability behaviors of the gait [23, 34, 29]. Differentiating the virtual constraints (17) yileds

[TABLE]

where $K_{p}$ and $K_{d}$ are positive-definite gains. From (18), one can solve for the nominal state feedback law as follows:

[TABLE]

that will result in the asymptotic output tracking, i.e., $\lim_{t\rightarrow\infty}y_{v}(t)=0$. Here, we assume that the decoupling matrix $A_{v}$ is full-rank with a number of rows less than or equal to the number of actuators (i.e., columns).

V SYNTHESIS OF DISTRIBUTED CONTROLLERS

The objective of this section is to synthesize the distributed feedback controllers that satisfy the properties of Section III. We will make use of two local real-time QPs (one for each agent) to synthesize these controllers. For this purpose, we consider the following set of measurable global variables

[TABLE]

that are available for both subsystems. Here, $s_{i}$, $q_{i}^{\textrm{roll}}$, $q_{i}^{\textrm{pitch}}$, $\dot{q}_{i}^{\textrm{roll}}$, and $\dot{q}_{i}^{\textrm{pitch}}$ represent the forward speed, roll angle, pitch angle, roll angular velocity, and pitch angular velocity for the agent $i\in\{1,2\}$, respectively. Suppose further that the agents $i\in\{1,2\}$ and $j\neq{}i\in\{1,2\}$ are in the continuous-time domains $v$ and $w$, respectively. We would like to modify the virtual constraints for agents to allow stable cooperative locomotion. Let us assume that $y_{v,w}^{i}(t,x_{i},\Theta,\xi_{i})$ denotes the modified virtual constraints for the agent $i$. In our notation, the modified virtual constraints depend on 1) the time $t$, 2) the local state variables $x_{i}$, and 3) the measurable global variables $\Theta$. Furthermore, they are parameterized by a set of adjustable controller parameters, represented by $\xi_{i}$. One typical choice for the modified outputs can be as follows:

[TABLE]

where $\alpha_{v,w}$ as well as $C_{v,w}^{\textrm{roll}}$ and $C_{v,w}^{\textrm{pitch}}$ are scalars and vectors with proper dimensions to be determined. In (V), the nonholonomic output for the agent $i$ is corrected according to the additive term $-\alpha_{v,w}(s_{j}-s^{\star}(\tau,w))$ which takes into account the speed of the agent $j$. Note that the original nonholonomic term $y^{\textrm{nh}}_{v}(t,q_{i},\dot{q}_{i})$, defined in (17), vanishes on the continuous-time domain $v$ of the orbit $\mathcal{O}$. According to the construction procedure, the additive term is also zero on the domain $w$ of the same orbit. Hence, on the domain $(v,w)$ of the complex gait $\mathcal{O}_{d}^{a}$, the modified nonholonomic term is zero. The additive terms for the holonomic portion of the modified output include the roll and pitch measurements of the other agent that are given by the terms $-C_{v,w}^{\textrm{roll}}(q_{j}^{\textrm{roll}}-q^{\star,\textrm{roll}}(\tau,w))$ and $-C_{v,w}^{\textrm{pitch}}(q_{j}^{\textrm{pitch}}-q^{\star,\textrm{pitch}}(\tau,w))$, respectively. In an analogous manner, one can show that the modified holonomic output is zero on the continuous-time domain $(v,w)$ of the cooperative gait $\mathcal{O}_{d}^{a}$. In Section VI, we will show that the stability of the complex gait depends on the proper selection of the parameters $\alpha_{v,w}$, $C_{v,w}^{\textrm{roll}}$, and $C_{v,w}^{\textrm{pitch}}$. Let us define the adjustable controller parameters as follows:

[TABLE]

We remark that in general the controller parameters can be different for the agents. However, since the models of two agents are assumed to be identical, we assume that $\xi_{i}=\xi_{j}$ for $i,j\in\{1,2\}$.

We are now interested in regulating the modified outputs. Eliminating the Lagrange multipliers in (5)-(9) will result in the following coupled dynamics for the vertex $(v,w)$ of the complex graph

[TABLE]

where $D_{i}:=D(q_{i})$ for $i\in\{1,2\}$, and $H_{v,w,i}^{a}$ and $B_{v,w,ij}^{a}$ depend on the augmented state variables $x^{a}$ for $i,j\in\{1,2\}$. Since 1) the agent $i$ does not have access to all state variables of the agent $j$, and 2) the agent $i$ cannot make decision for the control action of the agent $j\neq{}i$ (i.e., $u_{j}$), we need to approximate the coupled dynamics (22) for the I-O linearization purpose of the modified output $y^{i}_{v,w}$. As the agent $i$ has access to its own local state variables $x_{i}$ as well as the measurable global variables $\Theta$, it is reasonable to assume that it can approximate the “remaining” portion of the augmented state variables by their desired evolution on the domain $w$ of the orbit $\mathcal{O}$ at any time $t$. Using this assumption, the agent $i$ can approximate its own coupled dynamics in (22) as follows:

[TABLE]

where $u_{j}^{\star}(t,w)$ represents the feedforward torques on domain $w$ of the orbit $\mathcal{O}$. Moreover, $\hat{H}_{v,w,i}^{a}$, $\hat{B}_{v,w,ii}^{a}$, and $\hat{B}_{v,w,ij}^{a}$ are approximations of $H_{v,w,i}^{a}$, $B_{v,w,ii}^{a}$, and $B_{v,w,ij}^{a}$, respectively, using the above-mentioned assumption.

Next, the I-O linearization along (23) yields

[TABLE]

where $A_{v,w}^{i}$, $b_{v,w}^{i}$, and $e_{v,w}^{i}$ are the extensions of the terms that were computed for the locomotion of a single agent in (18). Now we are in a position to present the local QP for each agent. The objective of the local QPs is to modify the control inputs for each agent to be close enough to its own nominal HZD-based control while imposing the modified virtual constraints for cooperative locomotion. More specifically, we consider the following local QP for the agent $i\in\{1,2\}$ at every time sample (e.g., $1$kHz) (see Fig. 2b)

[TABLE]

in which $w>0$ is a weighting factor, and $\delta$ is a defect variable to satisfy the equality constraint (26) in case (24) cannot be met. This may happen if the modified decoupling matrix $A_{v,w}^{i}$ is not full-rank. The cost function (25) tries to make the local controller $u_{i}$ close enough to the nominal controller while keeping the $2$-norm of the defect variable small. Inequality constraints (27) ensure the feasibility of the applied torques, where $u_{\min}$, $u_{\max}$, $\delta_{\min}$, and $\delta_{\max}$ denote the lower and upper bounds for the decision variables.

Remark 1

As the nominal HZD-based controllers stabilize the motion of each single, we would like to find an optimal solution that is close enough to the nominal controller while satisfying the modified virtual constraints. The solution of the QP will be then a combination of the nominal HZD-based controller and modified virtual constraint controllers, and this combination will be parameterized by the weighing factor $w$. We have observed that this combination is important in stabilizing cooperative gaits (see Section VI).

Theorem 2

(Properties of the Optimal Local Controllers): The solutions of the local QPs (25)-(27) satisfy the property (16).

Proof:

According to the construction procedure, for every $x^{a}\in\mathcal{O}_{d}^{a}$, $(v,w)\in\mathcal{V}^{a}$, $i\in\{1,2\}$, and any controller parameters $\xi_{i}$, the modified virtual constraints $y_{v,w}^{i}(t,x_{i},\Theta,\xi_{i})$ and $\frac{\textrm{d}}{\textrm{d}t}y_{v,w}^{i}(t,x_{i},\Theta,\xi_{i})$ (for holonomic portions) are zero. In addition, the approximate terms $\hat{H}_{v,w}^{a}$, $\hat{B}_{v,w,ii}^{a}$, and $\hat{B}_{v,w,ij}^{a}$ are equal to their actual values on the orbit $\mathcal{O}_{d}^{a}$. Hence, the approximate dynamics (23) as well as the approximate I-O linearization in (24) become the precise ones for the complex dynamics on the orbit $\mathcal{O}_{d}^{a}$. This together with the fact that the modified outputs are zero on $\mathcal{O}_{d}^{a}$ implies that the optimal solution of local QPs (25)-(27) on $\mathcal{O}_{d}^{a}$ is equal to the nominal HZD-based controllers which completes the proof. ∎

Corollary 1

(Complex Gait with Local QPs): Suppose that Assumptions 1-3 are satisfied and we employ the optimal local controllers in (25)-(27). Then, $\mathcal{O}_{d}^{a}$ is a periodic orbit for the complex hybrid model $\Sigma^{a}$.

Proof:

The proof is a result of Theorems 1 and 2. ∎

Remark 2

(Stability Modulo $d$): One immediate result from Corollary 1 and Assumption 3 is that the proposed local controllers can stabilize the cooperative locomotion by the proper selection of the controller parameters $\xi_{i},i\in\{1,2\}$, but cannot stabilize the location of the agents with respect to each other. In particular, $\mathcal{O}_{d}^{a}$ for any $d$ with the property $d\neq 0$ can be a periodic orbit. We refer to this stability, stability modulo $d$. Full-state stability requires the measurement of the absolute Cartesian positions of the agents which is not considered in this paper.

VI NUMERICAL RESULTS

The objective of this section is to numerically evaluate the effectiveness of the proposed distributed controllers for stabilizing cooperative locomotion of two Vision 60 robots augmented with 6 DOF Kinova arms. We consider a walking gait $\mathcal{O}$ for each agent with 8 continuous-time domains at the speed of $0.34$ (m/s) (see Fig. 2a). The gait is designed via FROST (Fast Robot Optimization and Simulation Toolkit) — an open-source toolkit for path planning of dynamic legged locomotion [38]. Our previous work in [23] has shown that the stability of gaits in the HZD approach depends on the proper selection of the virtual constraints that is equivalent to the proper selection of the output matrices $C_{v}$ in (17).

To exponentially stabilize the gait $\mathcal{O}$, we make use of the iterative optimization algorithm of [29, 34] that was developed based on LMIs and BMIs to look for stabilizing values of $C_{v}$. Figure 3a depicts the time profile of the regular virtual constraints for stable locomotion of one agent. Here, we employ the nominal HZD-based controllers that were developed in Section IV. Convergence to a stable periodic motion is clear. Although the nominal HZD-based controllers can stabilize the walking gait for a single agent, they cannot address the cooperative locomotion of two agents.

Figure 3b illustrates the original virtual constraints for the complex hybrid model of collaborative locomotion, where each agent only employs its own nominal controller. Here, we assume that the agents carry a massless bar with the length of $1$ (m) (we suppose that $d=\textrm{col}(0,1)$). The initial augmented state is taken off of the orbit $\mathcal{O}_{d}^{a}$. Divergence from the periodic gait is clear which indicates the instability. To stabilize the gait, we modify the virtual constraints for each agent as proposed in (V). In this paper, we choose the distributed controller parameters based on the output matrices $C_{v}$ that are optimized using the BMI algorithm. Let $C_{v}^{\textrm{roll}}$ and $C_{v}^{\textrm{pitch}}$ denote the columns of the optimized matrix $C_{v}$ that correspond to the roll and pitch motions, respectively. We then choose

[TABLE]

where $\beta_{v,w}$ and $\gamma_{v,w}$ are scalars to be determined. Equation (28) reduces the choice of the controllers parameters $\xi_{i}$ in (21) to that of the scalars $\{\alpha_{v,w},\beta_{v,w},\gamma_{v,w}\}$. In this paper, we heuristically take $\alpha_{v,w}=\beta_{v,w}=\gamma_{v,w}=0.5$. Figure 4 illustrates the phase portraits for the roll, pitch, and yaw motions of one of the agents during cooperative locomotion. Convergence to the periodic orbit is clear. Figure 3c depicts the time profile of modified virtual constraints. Snapshots of the cooperative locomotion are illustrated in Fig. 5. The animation of these simulations can be found at [39].

VII CONCLUSIONS

This paper presented an analytical approach to design distributed feedback controllers that stabilize cooperative locomotion of legged robots which are coupled to each other by holonomic constraints. We addressed complex hybrid dynamical models that represent collaborative locomotion of legged robots while steering objects with their arms. The paper studied properties and complex periodic orbits of these sophisticated and high-dimensional hybrid systems. Virtual constraints were devolved for stable locomotion of single agents and are imposed by nominal HZD-based controllers. The paper then modified the virtual constraints to achieve stable cooperative locomotion of two agents. The distributed controllers were implemented via local QPs to be close to the nominal HZD-based controllers while satisfying the modified virtual constraints. To demonstrate the power of the proposed approach, we employed the distributed controllers for an extensive numerical simulation that describes the complex hybrid model for cooperative locomotion of two Vision 60s with Kionva arms to steer an object. The complex model of locomotion has 64 continuous-time domains, 192 discrete-time transitions, 96 state variables, and 36 control inputs. For future work, we will experimentally investigate the distributed control algorithms on two Vision 60 robots with Kinova arms. We will also study safety-critical control algorithms that allow collaborative locomotion and obstacle avoidance in complex environments.