Hierarchical and Safe Motion Control for Cooperative Locomotion of Robotic Guide Dogs and Humans: A Hybrid Systems Approach

TL;DR

This paper develops a hierarchical hybrid systems control framework for safe, cooperative locomotion of robotic guide dogs and visually impaired humans, integrating nonlinear control, safety guarantees, and real-time obstacle avoidance.

Contribution

It introduces a novel hybrid systems approach combining nonlinear control, safety-critical control barrier functions, and real-time quadratic programming for cooperative robot-human locomotion.

Findings

Successfully designed stable gaits using virtual constraints.

Implemented real-time obstacle avoidance with control barrier functions.

Validated the approach through extensive numerical simulation of a complex quadrupedal robot-human model.

Abstract

This paper presents a hierarchical control strategy based on hybrid systems theory, nonlinear control, and safety-critical systems to enable cooperative locomotion of robotic guide dogs and visually impaired people. We address high-dimensional and complex hybrid dynamical models that represent collaborative locomotion. At the high level of the control scheme, local and nonlinear baseline controllers, based on the virtual constraints approach, are designed to induce exponentially stable dynamic gaits. The baseline controller for the leash is assumed to be a nonlinear controller that keeps the human in a safe distance from the dog while following it. At the lower level, a real-time quadratic programming (QP) is solved for modifying the baseline controllers of the robot as well as the leash to avoid obstacles. In particular, the QP framework is set up based on control barrier functions (CBFs) to compute optimal control inputs that guarantee safety while being close to the baseline controllers. The stability of the complex periodic gaits is investigated through the Poincare return map. To demonstrate the power of the analytical foundation, the control algorithms are transferred into an extensive numerical simulation of a complex model that represents cooperative locomotion of a quadrupedal robot, referred to as Vision 60, and a human model. The complex model has 16 continuous-time domains with 60 state variables and 20 control inputs.

Equations

$$\Sigma \left(\mathcal{G},\mathcal{D},\mathcal{S},\Delta ,FG\right),$$

$$D(q)\ddot{q}+C\left(q,\dot{q}\right)\dot{q}+G(q)$$

$$J_v(q)\ddot{q}+\frac{\partial }{\partial q}\left(J_v(q)\dot{q}\right)\dot{q}$$

$$D(q)\ddot{q}+H_v\left(q,\dot{q}\right)=T_v(q)u,$$

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

$$D^d\left(q^d\right){\ddot{q}}^d$$

$$D^h\left(q^h\right){\ddot{q}}^h$$

$$x^{d+}={\Delta }_{v\to v^{\prime }}^d\left(x^{d-}\right),x^{h+}={\Delta }_{w\to w^{\prime }}^h\left(x^{h-}\right).$$

$${\mathcal{O}}_{\text{ul}}^i:=\left\{x^i=x_{\star }^i(t)|0\le t<T^i\right\},$$

$${\mathcal{O}}_{\text{ul}}^c:=\left\{x^c=\text{col}(x_{\star }^d(t),x_{\star }^h(t))|0\le t<N_{\min }^d{T}^d\right\}$$

$$y_v^d\left(x^d\right):=\left[\begin{array}{c}{y}_{1v}^d(q^d,{\dot{q}}^d)\\ y_{2v}^d(q^d)\end{array}\right],$$

$$\left[\begin{array}{c}{\dot{y}}_{1v}^d\\ {\ddot{y}}_{2v}^d\end{array}\right]=A_v^d\left(x^d\right)u^d+b_v^d\left(x^d,F\right),$$

$$\left[\begin{array}{c}{\dot{y}}_{1v}^d\\ {\ddot{y}}_{2v}^d\end{array}\right]=-{\ell }_v(x^d):=-\left[\begin{array}{c}{K}_P{y}_{1d}^v\\ K_D{\dot{y}}_{2v}^d+K_P{y}_{2v}^d\end{array}\right]$$

$${\Gamma }_v^d\left(x^d,F\right):=-A_v^{d\top }{\left(A_v^d{A}_v^{d\top }\right)}^{-1}\left(b_v^d+{\ell }_v\right)$$

$$\displaystyle\mathcal{C}:=\big{\{}x^{c}=\textrm{col}\left(x^{d},x^{h}\right)$$

$$\displaystyle\forall(\alpha,\beta,\gamma)\in\mathcal{I}^{o}\times\mathcal{I}^{d}\times\mathcal{I}^{h}\big{\}},$$

$${\mathcal{B}}_{\beta ,\alpha }^d\left(x^d\right)$$

$${\mathcal{B}}_{\gamma ,\alpha }^h\left(x^h\right)$$

$${\dot{\mathcal{B}}}_{\beta ,\alpha }^d\left(x^d,u^d,F\right)$$

$${\dot{\mathcal{B}}}_{\gamma ,\alpha }^h\left(x^h,F\right)$$

$${\ddot{h}}_{\beta ,\alpha }^d$$

$${\ddot{h}}_{\gamma ,\alpha }^h$$

$$A_{\beta ,\alpha }^d\left(x^d\right)\left[\begin{array}{c}{u}^d\\ F\end{array}\right]$$

$$A_{\gamma ,\alpha }^h\left(x^h\right)F$$

$$\underset{(u^d,F)}{\min }$$

$$A_{\beta ,\alpha }^d\left(x^d\right)\left[\begin{array}{c}{u}^d\\ F\end{array}\right]+b_{\beta ,\alpha }^d\left(x^d\right)\ge 0,\forall (\alpha ,\beta ,\gamma )\in \mathcal{I}$$

$$A_{\gamma ,\alpha }^h\left(x^h\right)F+b_{\gamma ,\alpha }^h\left(x^h\right)\ge 0,\forall (\alpha ,\beta ,\gamma )\in \mathcal{I}$$

$$u_{\min }\le u^d\le u_{\max }$$

$$F_{\min }\le F\le F_{\max },$$

Full text

I INTRODUCTION

This paper aims to develop an analytical foundation, based on hybrid systems theory, nonlinear control, quadratic programming, and safety-critical systems, to develop a hierarchical control algorithm that enables safe and stable cooperative locomotion of robotic guide dogs and visually impaired people (see Fig. 1). One of the most challenging problems in deploying autonomous guide robots is to enable ubiquitous mobility. More than half the Earth’s landmass is inaccessible to wheeled vehicles which motivates the deployment of intelligent and highly agile legged guide robots to access these environments. In particular, infrastructures for human-centered communities, including factories, offices, and homes, are developed for humans which are bipedal walkers capable of stepping over gaps and walking up/down stairs.

I-A Related Work

Although important theoretical and technological advances have occurred for the construction and control of guide robots, state-of-the-art approaches are mainly tailored to the deployment of wheeled vehicles and not legged guide robots (e.g., [2, 3, 4]). Unlike wheeled guide robots, legged robots are inherently unstable complex dynamical systems with hybrid nature and high degrees of freedom (DOF). This complicates the design of feedback control algorithms that ensure stable and safe cooperative locomotion of guide dogs and human. Hybrid systems theory has become a powerful approach for modeling and control of legged robots both in theory and practice [5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18]. Existing nonlinear control approaches that address the hybrid nature of legged locomotion models are developed based on hybrid reduction [19], controlled symmetries [16], transverse linearization [17], and hybrid zero dynamics (HZD) [6, 8]. State-of-the art nonlinear control approaches for dynamic legged locomotion have been tailored to stable locomotion of legged robots, but not stable and safe cooperative locomotion of legged guide robots and visually impaired people.

I-B Objectives and Contributions

The objectives and contributions of this paper are to present a formal foundation towards 1) developing complex hybrid models of cooperative locomotion of legged guide dogs and human, and 2) creating a hierarchical control algorithm, based on nonlinear control, quadratic programming, and control barrier functions (CBFs) [20, 21, 22], to ensure stability, safety, and obstacle avoidance. We address complex and high-dimensional models of cooperative legged locomotion via hybrid systems approach. An actuated leash structure is considered for the coordination of the dog and human locomotion while steering the human for safety and obstacle avoidance. At the higher level, the proposed hierarchical control algorithm employs local and nonlinear controllers, referred to as baseline controllers, that induce asymptotically stable unleashed locomotion patterns for the robotic dog and human. The baseline controllers are synthesized via the HZD approach and assumed to have access to the local state measurements as well as the force measurement applied by the leash structure. The leash baseline controller is then designed to keep the human in a safe distance from the robot while following it. The existence and stability of complex and leashed locomotion patterns for the coupled dynamics are addressed through the Poincaré return map. At the lower level of the control strategy, the baseline controllers for the dog and leash are modified by a real-time quadratic programming (QP) that includes CBF constraints to ensure safety and obstacle avoidance. The power of the anlytical results are demonstrated on an extensive numerical simulation of a complex hybrid model that represents cooperative locomotion of a quadrupedal robot, referred to as Vision 60 [1] (see Fig. 1), and a human model. The complex and full-order hybrid dynamical model has $60$ state variables and $20$ control inputs together with $16$ continuous-time domains to describe a trotting gait of the robot and a bipedal gait of the human. The performance of the closed-loop hybrid system in the presence of a discrete set of obstacles around the complex gait is investigated.

II HYBRID MODELS OF LOCOMOTION

Hybrid models of locomotion can be described by directed cycles. In this section, we will first present the hybrid models for the locomotion of each agent (i.e., robot and human). We will then address the complex hybrid model that describes the cooperative locomotion of agents.

II-A Directed Cycles

Throughout this paper, we shall consider multi-domain hybrid models described by the following tuple [23]

[TABLE]

where $\mathcal{G}:=(\mathcal{V},\mathcal{E})$ represents a direct cycle (i.e., graph) for the studied locomotion pattern (see Fig. 2a). In our formulation, the vertices $\mathcal{V}$ denote the continuous-time dynamics of legged locomotion, referred to as domains or phases. The edges $\mathcal{E}\subseteq\mathcal{V}\times\mathcal{V}$ represent the discrete-time transitions among continuous-time dynamics arising from changes in physical constraints (e.g., a new contact point is added to the set of existing contact points with the ground or an existing contact point leaves the ground). For every $v_{i},v_{j}\in\mathcal{V}$, $e=(v_{i}\rightarrow{}v_{j})\in\mathcal{E}$ if $v_{i}$ and $v_{j}$ are adjacent on $\mathcal{G}$. The state variables and control inputs of the hybrid system are shown by $x\in\mathcal{X}$ and $u\in\mathcal{U}$, respectively. The set of state manifolds and set of admissible controls are then denoted by $\mathcal{X}:=\{\mathcal{X}_{v}\}_{v\in\mathcal{V}}$ and $\mathcal{U}:=\{\mathcal{U}_{v}\}_{v\in\mathcal{V}}$, in which $\mathcal{X}_{v}$ and $\mathcal{U}_{v}$ are the state space and admissible controls for the domain $v\in\mathcal{V}$. The set of domains of admissibility are further represented by $\mathcal{D}:=\{\mathcal{D}_{v}\}_{v\in\mathcal{V}}$, where $\mathcal{D}_{v}\subseteq\mathcal{X}_{v}\times\mathcal{U}_{v}$ denotes the set of all points $(x,u)$ on which the unilateral constraints and friction cone conditions are satisfied (i.e., legs are above the walking surface and the foot slippage does not occur). The evolution of the hybrid system during the continuous-time domain $v\in\mathcal{V}$ is described by an ordinary differential equation (ODE) arising from the Euler-Lagrange equations as $\dot{x}=f_{v}(x)+g_{v}(x)\,u$ for all $(x,u)\in\mathcal{D}_{v}$. In addition, $FG:=\{(f_{v},g_{v})\}_{v\in\mathcal{V}}$ represents the set of control systems on $\mathcal{D}$. In order to simplify the presentation, we define the next domain function as $\mu:\mathcal{V}\rightarrow\mathcal{V}$ by $\mu(v_{i})=v_{j}$ if $e=(v_{i}\rightarrow{}v_{j})\in\mathcal{E}$. The evolution of the hybrid system during the discrete-time transition $e\in\mathcal{E}$ is further described by the instantaneous mapping $x^{+}=\Delta_{e}(x^{-})$, where $x^{-}(t):=\lim_{\tau\nearrow t}x(\tau)$ and $x^{+}(t):=\lim_{\tau\searrow t}x(\tau)$ represent the state of the system right before and after the discrete transition, respectively. $\Delta:=\{\Delta_{e}\}_{e\in\mathcal{E}}$ denotes the set of discrete-time dynamics. The guards of the hybrid system are finally given by $\mathcal{S}:=\{\mathcal{S}_{e}\}_{e\in\mathcal{E}}$, on which the state trajectories undergo an abrupt change according to the discrete-time dynamics $\Delta_{e}$ when the state and control trajectories $(x,u)$ hit the surface $\mathcal{S}_{e}$ in $\mathcal{D}$.

II-B Continuous-Time Dynamics

In this section, we consider the continuous-time dynamics for each agent. We assume that $q\in\mathcal{Q}\subset\mathbb{R}^{n}$ denotes the configuration variables for the robot and/or human. The configuration space is further represented by $\mathcal{Q}$. The state vector is taken as $x:=\textrm{col}(q,\dot{q})\in\textrm{T}\mathcal{Q}$, where $\textrm{T}\mathcal{Q}$ denotes the tangent bundle of $\mathcal{Q}$. We remark that the Vision 60 robot has $n=18$ DOFs. For the human model, we make use of an $n=12$ DOF tree structure with a torso and two identical legs consisting of a femur and tibia links. The control inputs $u\in\mathcal{U}\subset\mathbb{R}^{m}$ are finally taken as torques at the joint levels (i.e., $m=12$ for the dog robot and $m=6$ for the human model) (see Section VI for further details on the models).

It is supposed that $\eta_{v}(q)\equiv 0$ represents the holonomic constraints during the domain $v\in\mathcal{V}$ arising form the contact conditions between the leg ends and the ground. The equations of motion during the continuous-time domain $v$ are then described by the Euler-Lagrange equations and principle of virtual work as follows

[TABLE]

where $D(q)\in\mathbb{R}^{n\times{}n}$ denotes the positive definite mass-inertia matrix, $C(q,\dot{q})\,\dot{q}+G(q)\in\mathbb{R}^{n}$ represents the Coriolis, centrifugal, and gravitational terms, $B\in\mathbb{R}^{n\times{m}}$ denotes the input distribution matrix, $\lambda$ represents the Lagrange multipliers (i.e., ground reaction forces), and $J_{v}(q):=\frac{\partial\eta_{v}}{\partial q}(q)$ is the contact Jacobian matrix. If $J_{v}$ has full rank, one can eliminate the Lagrange multipliers to express (2) as

[TABLE]

in which $H_{v}:=\textrm{proj}_{v}\,H+J_{v}^{\top}\,(J_{v}\,D^{-1}\,J_{v}^{\top})^{-1}\frac{\partial}{\partial q}(J_{v}\,\dot{q})\dot{q}$, $H=C(q,\dot{q})\,\dot{q}+G(q)$, $T_{v}:=\textrm{proj}_{v}\,B$, and $\textrm{proj}_{v}:=I-J_{v}^{\top}(J_{v}\,D^{-1}\,J_{v}^{\top})^{-1}J_{v}\,D^{-1}$. We remark that (3) can be expressed as an input-affine system, i.e., $\dot{x}=f_{v}(x)+g_{v}(x)\,u$.

II-C Discrete-Time Dynamics

If a new contact point is added to the existing set of contact points with the ground, we employ a rigid impact model [24] to describe the abrupt changes in the velocity coordinates according to the impact. In particular, if $\delta\lambda$ represents the intensity of the impulsive ground reaction force on the contacting points, integrating (2) over the infinitesimal period of the impact (i.e., $[t^{-},t^{+}]$) yields

[TABLE]

in which $\dot{q}^{-}$ and $\dot{q}^{+}$ represent the generalized velocity vector right before and after the impact, respectively. From the continuity of position, we assume $q^{+}=q^{-}$ and then from (4), one can solve for $\dot{q}^{+}$ and $\delta\lambda$ in terms of $(q^{-},\dot{q}^{-})$ to have a closed-form expression as $x^{+}=\Delta_{e}(x^{-})$. Furthermore, if the leg leaves the ground, we take $\Delta_{e}$ as the identity map to preserve the continuity of position and velocity coordinates over the corresponding discrete transition.

II-D Complex Hybrid Models for Cooperative Locomotion

Throughout this paper, we shall assume that there is a rigid and massless leash model that connects a point on the dog (e.g., head) to a point on the human (e.g., hand or hip) via ball (i.e., socket) joints. The leash will further be assumed to be actuated to control its length and orientation so that the human can follow the dog in a safe manner. This will be clarified with more details in Section III-B. The state and control inputs for the robotic dog and human are shown by $x^{i}:=\textrm{col}(q^{i},\dot{q}^{i})$ and $u^{i}$, respectively, for $i\in\{d,h\}$, where the superscripts “$d$” and “$h$” stand for the dog and human.

Complex Graph: The complex hybrid model that describes the cooperative locomotion of the robot and human will have a complex graph that is taken as the strong product of graphs $\mathcal{G}^{d}=(\mathcal{V}^{d},\mathcal{E}^{d})$ and $\mathcal{G}^{h}=(\mathcal{V}^{h},\mathcal{E}^{h})$. The strong product is denoted by $\mathcal{G}^{c}:=\mathcal{G}^{d}\boxtimes\mathcal{G}^{h}$ that has the vertex set $\mathcal{V}^{c}:=\mathcal{V}^{d}\times\mathcal{V}^{h}$, and any two vertices $(v,w)$ and $(v^{\prime},w^{\prime})$ in $\mathcal{V}^{c}$ are adjacent if and only if 1) $v=v^{\prime}$ and $(w\rightarrow{}w^{\prime})$ is an edge in $\mathcal{E}^{h}$, or 2) $(v\rightarrow{}v^{\prime})$ is an edge in $\mathcal{E}^{d}$ and $w=w^{\prime}$, or 3) $(v\rightarrow{}v^{\prime})$ is an edge in $\mathcal{E}^{d}$ and $(w\rightarrow{}w^{\prime})$ is an edge in $\mathcal{E}^{h}$. In our notation, the superscript “$c$” represents the complex model. The augmented state and control inputs are further denoted by $x^{c}:=\textrm{col}(x^{d},x^{h})$ and $u^{c}:=\textrm{col}(u^{d},u^{h})$, respectively.

Complex Continuous-Time Dynamics: For every vertex $(v,w)\in\mathcal{V}^{c}$, the evolution of the composite mechanical system, consisting of the robot and human, can be described by the following nonlinear and coupled dynamics

[TABLE]

in which $J_{\textrm{head}}^{d}(q^{d})$ and $J_{\textrm{hand}}^{h}(q^{h})$ denote the Jacobian matrices for the end points of the leash at the dog and human sides, respectively, and $F\in\mathbb{R}^{3}$ represents the force applied by the leash to the human hand. We remark that the superscripts “$d$” and “$h$” represent the dynamic and kinematic terms for the dog and human models, respectively.

Complex Discrete-Time Dynamics: Since the leash model is assumed to be massless and cannot employ impulsive forces, the evolution of the composite mechanical system over the discrete transition $(v,w)\rightarrow(v^{\prime},w^{\prime})$ can be described by the following nonlinear and decoupled mappings

[TABLE]

We remark that if $v=v^{\prime}$ (resp. $w=w^{\prime}$) in (6), the mapping $\Delta_{v\rightarrow{}v^{\prime}}$ (resp. $\Delta_{w\rightarrow{}w^{\prime}}$) is simply taken as the identity.

Remark 1

In this paper, we shall consider a trotting gait for Vision 60 robot with $8$ continuous-time domains (see Fig. 2a for more details). The graph for the bipedal gait of the human model also has $2$ continuous-time domains that represent the right and left stance phases. Consequently, the complex hybrid model of locomotion would have $8\times 2=16$ continuous-time domains for which there are $2\times(n^{d}+n^{h})=2\times(18+12)=60$ state variables and $m^{d}+m^{h}+m^{l}=12+6+2=20$ control inputs. Here, $m^{l}=2$ represents the actuator numbers for the leash (see Section III-B).

III HIERARCHICAL CONTROL STRATEGY

In order to have stable and safe cooperative locomotion for the robot and human, we will present a two-level control strategy for the robotic dog and leash (see Fig. 2b). Since the mathematical models for the local controller of the human part are not known, we shall assume a nonlinear local controller for the human, but will not change that controller to address unforeseen events and obstacle avoidance. We will instead focus on the dog and leash hierarchical control strategy to ensure stability and safety. At the higher level of the control strategy, we will employ a local nonlinear controller for the robot that has access to its own state variables as well as the force employed by the leash (i.e., force measurement). This controller will be referred to as the robot baseline controller. The objective is to asymptotically derive some outputs to zero that encode the locomotion patterns for the guide robot (e.g., trot, amble, walk, or gallop gaits at desired speeds). The baseline controller for the dog will be designed via HZD and virtual constraints approach in Section IV. This controller exponentially stabilizes gaits for the hybrid model of the dog in the presence of the leash force. The baseline controller for the leash will be designed to ensure that 1) there is always a safe distance between the robot and human, and 2) human follows the robot (see Section III-B). At the lower level, we will solve a real-time QP optimization to ensure safety and obstacle avoidance. In particular, the QP optimization modifies the baseline controllers for the robot as well as the leash to keep the dog and human in a safe distance from obstacles. The QP framework will be set up based on CBFs in Section V.

III-A Local Baseline Controllers for the Agents

In this section, we consider the robot and human as two multi-body “agents” specified by the superscript $i\in\{d,h\}$.

Definition 1** (Local Baseline Controllers)**

We suppose that there are local and smooth feedback laws $\Gamma^{i}(x^{i},F):=\{\Gamma_{v}^{i}(x^{i},F)\}_{v\in\mathcal{V}^{i}}$ for the agent $i\in\{d,h\}$ to have stable locomotion patterns. In our notation, $\Gamma_{v}^{i}(x^{i},F)$ is a local and nonlinear feedback controller, referred to as baseline controller, that is employed during the continuous-time $v\in\mathcal{V}^{i}$ and assumed to have access to the state variables of the agent $i$ (i.e., $x^{i}$) as well as the force $F$.

Assumption 1** (Transvsersal Stable Periodic Orbits)**

By employing the local baseline controllers for the agent $i\in\{d,h\}$ in the unleashed case (i.e., $F\equiv 0$), we assume that there is a period-one orbit (i.e., gait) for the closed-loop hybrid model $\Sigma^{i}$, denoted by $\mathcal{O}^{i}_{\textrm{ul}}$, that is transversal to the guards $\mathcal{S}^{i}$. In our notation, the subscript “ul” stands for the unleashed gait. The orbit $\mathcal{O}^{i}_{\textrm{ul}}$ is further supposed to be locally exponentially stable.

For future purposes, the evolution of the state variables $x^{i}$ on the unleashed orbit $\mathcal{O}^{i}_{\textrm{ul}}$ is represented by $x_{\star}^{i}(t)$ for $t\geq 0$. The orbit $\mathcal{O}^{i}_{\textrm{ul}}$ can then be expressed as

[TABLE]

in which $T^{i}>0$ denotes the minimal period of $x_{\star}^{i}(t)$. Section IV will present a class of HZD-based local baseline controllers to exponentially stabilize the periodic gaits $\mathcal{O}^{i}_{\textrm{ul}}$.

Assumption 2** (Common Multiples of Gait Periods)**

We assume that there are common multiples for the periods of the dog and human unleashed gaits. More specifically, there are positive integers $N^{d}$ and $N^{h}$ such that $N^{d}\,T^{d}=N^{h}\,T^{h}$. For future purposes, we denote the minimum of these values by $N^{d}_{\min}$ and $N^{h}_{\min}$.

III-B Leash Baseline Controller

As mentioned previously, the leash structure is assumed to be rigid. We further suppose that its length and orientation can be independently controlled by two linear and rotational actuators. To make this notion more precise, let us denote the Cartesian coordinates of the dog head and the human hand by $p^{d}_{\textrm{head}}(q^{d})\in\mathbb{R}^{3}$ and $p^{h}_{\textrm{hand}}(q^{h})\in\mathbb{R}^{3}$, respectively. Next, consider the vector connecting $p^{h}_{\textrm{hand}}(q^{h})$ to $p^{d}_{\textrm{head}}(q^{d})$. The representation of this vector in the cylindrical coordinates can be given by $(r,\theta,z)$. We assume that there are sensors for the leash structure to measure $(r,\theta)$. The objective here is to design a local force feedback controller for the leash that has access to $(r,\theta)$ to keep the human in a safe distance from the robot dog while regulating the angle $\theta$. In particular, we are interested in (i) having $r\in[r_{\min},r_{\max}]$ for some $0<r_{\min}<r_{\max}$ and (ii) imposing $\theta\rightarrow 0$. This controller is referred to as the leash baseline controller. One possible way to design such a controller is to decompose the force $F$ into $(F_{r},F_{\theta},F_{z})$, in which $F_{r}(r,\dot{r})$ is the longitudinal force designed to be sufficiently differentiable while being zero over the safe zone $[r_{\min},r_{\max}].$ Moreover, $F_{\theta}(\theta,\dot{\theta})$ is a torsional force that can be taken as a simple PD controller to regulate $\theta$. For the purpose of this paper, $F_{z}$ is assumed to be zero. For future purposes, the leash baseline controller will be represented by $F_{b}(r,\dot{r},\theta,\dot{\theta},\kappa)\in\mathbb{R}^{3}$, where the subscript “$b$” stands for the baseline control and $\kappa$ represents some adjustable controller parameters, e.g., PD gains.

Assumption 3

We assume that $F_{b}$ is sufficiently differentiable with respect to its arguments $(r,\dot{r},\theta,\dot{\theta},\kappa)$. Furthermore, for $\kappa=0$, $F_{b}(r,\dot{r},\theta,\dot{\theta},\kappa)\equiv 0$.

Remark 2

Since the longitudinal force $F_{r}(r,\dot{r})$ is assumed to be zero over the safe zone $[r_{\min},r_{\max}]$, $F_{r}$ would have a deadzone structure over $[r_{\min},r_{\max}]$. Assumption 3 ensures that $F_{r}$ is designed to be differentiable at the corners $r_{\min}$ and $r_{\max}$ such that the stability analysis can be carried out via the Poincaré return map in Theorem 1.

III-C Stability Analysis of Complex Gaits

This section addresses the existence and stability of periodic orbits for the cooperative locomotion of the robot and human in the presence of leash. We make use of the Poincaré sections analysis and present the following theorem.

Theorem 1** (Stability of Complex Gaits with Leash)**

Under Assumptions 1-3, there is an open neighborhood of [math], denoted by $\mathcal{N}(0)$, such that for all gain values $\kappa\in\mathcal{N}(0)$, there is an exponentially stable complex gait for the leashed closed-loop hybrid system $\Sigma^{c}$.

Proof:

From Assumptions 1 and 2, the following augmented orbit

[TABLE]

is indeed a periodic orbit for the complex and unleashed hybrid system $\Sigma^{c}$. We next choose a Poincaré section transversal to this orbit, denoted by $\mathscr{S}$, and consider a Poincaré return map for $\Sigma^{c}$ from $\mathscr{S}$ back to $\mathscr{S}$ as $P^{c}(x^{c},\kappa)$. According to the construction procedure, there is a fixed point for the Poincaré map that corresponds to $\mathcal{O}^{c}_{\textrm{ul}}$, that is $P^{c}(x_{\star,\textrm{ul}}^{c},0)=x_{\star,\textrm{ul}}^{c}$, in which $x_{\star,\textrm{ul}}^{c}$ represents the fixed point. We next consider the algebraic equation $E(x^{c},\kappa):=P^{c}\left(x^{c},\kappa\right)-x^{c}=0$. Since $\mathcal{O}_{\textrm{ul}}^{c}$ is exponentially stable for the unleashed complex system, the Jacobian matrix $\frac{\partial E}{\partial x^{c}}(x_{\star,\textrm{ul}}^{c},0)=\frac{\partial P^{c}}{\partial x^{c}}(x_{\star,\textrm{ul}}^{c},0)-I$ is nonsingular. Hence, from the Implicit Function Theorem, there exists $\mathcal{N}(0)$ such that for all $\kappa\in\mathcal{N}(0)$, there is a fixed point for $P^{c}(x^{c},\kappa)$. Moreover, since the elements and eigenvalues of the Jacobian matrix $\frac{\partial P^{c}}{\partial x^{c}}(x^{c},\kappa)$ continuously depend on $\kappa$, one can choose $\mathcal{N}(0)$ sufficiently small such that the eigenvalues of the Jacobian matrix remain inside the unit circle. This completes the proof of exponential stability for leashed locomotion. ∎

IV LOCAL VIRTUAL CONSTRAINT CONTROLLERS WITH FORCE FEEDBACK

The objective of this section is to design the local baseline controller for the robotic dog. The controller is designed based on virtual constraints approach [5, 6] to ensure exponential stability of the gait for the unleashed case. Virtual constraints are defined as kinematic constraints (i.e., outputs) that encode the locomotion pattern. They are imposed through the action of the baseline controllers. The idea is to coordinate the motion of the links within domains. We make use of relative degree one and relative degree two virtual constraints (i.e., outputs). In particular, during the continuous-time domain $v\in\mathcal{V}^{d}$, we consider the following outputs to be regulated

[TABLE]

in which $y_{1v}^{d}(q^{d},\dot{q}^{d})$ represents relative degree one nonholonomic outputs for velocity regulation and and $y_{2v}^{d}(q^{d})$ denotes relative degree two holonomic outputs for position tracking. Using the nonlinear dynamics (5) and standard input-output linearization [25], one can obtain

[TABLE]

where $A_{v}^{d}(x)$ is a decoupling matrix and $b_{v}^{d}$ consists of Lie derivatives (see [23] for more details). Furthermore, we would like to solve for $u^{d}$ that results in the following output dynamics

[TABLE]

with $K_{P}$ and $K_{D}$ being positive PD gains. The local baseline controller for the dog is finally chosen as

[TABLE]

that 1) requires local state and force measurement and 2) exponentially stabilizes the equilibrium point $(y_{1v}^{d},y_{2v}^{d},\dot{y}_{2v}^{d})=(0,0,0)$ for the output dynamics (11) in the presence of the external force $F$, i.e., $\lim_{t\rightarrow\infty}y_{v}^{d}(t)=0$.

Remark 3** (Proper Selection of Virtual Constraints)**

The periodic orbit $\mathcal{O}^{d}_{\textrm{ul}}$ can be designed in an offline manner through direct collocation based trajectory optimization techniques [26, 9]. For a given periodic gait $\mathcal{O}_{\textrm{ul}}^{d}$, the output functions $y_{v}^{d}$ in (9) are chosen to vanish on the desired gait $\mathcal{O}_{\textrm{ul}}^{d}$. We have observed that the stability of gaits in the virtual constraint approach depends on the proper selection of the output functions $y_{v}^{d}$ to be regulated [27]. Our previous work [27, 28] has developed a recursive algorithm, based on semidefinite programming, to systematically design output functions for which the gaits are exponentially stable for the corresponding hybrid dynamics. The algorithm is offline and assumes a finite-dimensional parameterization of the output functions to be determined. Then it translates the exponential stabilization problem into a recursive optimization problem that is set up based on linear and bilinear matrix inequalities. Sufficient conditions for the convergence of the algorithm to a set of stabilizing parameters have been addressed in [28, 29].

Remark 4

Nonlinear local controllers for the human model are not know. However, for the purpose of this paper, we assume virtual constraint-based controllers, analogous to (12), for the human model. Furthermore, evidence suggests that the phase-dependent models can reasonably predict human joint behavior across perturbations [30].

V SAFETY-CRITICAL CONTROL AND QP OPTIMIZATION

This section aims to develop low-level safety-critical control algorithms that ensure obstacle avoidance while implementing the baseline controllers for the agents and leash in the hierarchical control structure. We will address safety critical conditions through set invariance and CBFs. In particular, a system being safe is commonly defined as the system never leaving the safety set [20, 21, 22]. For low-dimensional dynamical systems, analytical control strategies can be derived. However, finding such a control policy for high-DOF and complex models of cooperative legged locomotion of guide dogs and humans is a challenge. To tackle this problem, we make use of a real-time QP formulation to address safety specifications represented by CBFs [20]. To present the main idea, let us consider a discrete set of static and point obstacles $\mathscr{P}^{o}_{\alpha}$ for $\alpha\in\mathcal{I}^{o}$ whose Cartesian coordinates in the $xy$-planes are given by $r_{\alpha}^{o}:=\textrm{col}(x_{\alpha}^{o},y_{\alpha}^{o})$. Next we assume a set of critical points on the robot and human that are supposed to be in a safe distance from these obstacles. These points are denoted by $\mathscr{P}^{d}_{\beta}$ and $\mathscr{P}^{h}_{\gamma}$ for the dog and human, respectively, for some $\beta\in\mathcal{I}^{d}$ and $\gamma\in\mathcal{I}^{h}$. One typical example includes the hip points of the robot and human models. The Cartesian coordinates of $\mathscr{P}^{d}_{\beta}$ and $\mathscr{P}^{h}_{\gamma}$ in the $xy$-plane are further denoted by $r_{\beta}^{d}(q^{d})\in\mathbb{R}^{2}$ and $r_{\gamma}^{h}(q^{h})\in\mathbb{R}^{2}$. We formulate the safety set as

[TABLE]

where $h_{\beta,\alpha}^{d}(q^{d}):=\|r^{d}_{\beta}(q^{d})-r_{\alpha}^{o}\|_{2}^{2}-h_{\min}^{2}$ and $h_{\gamma,\alpha}^{h}(q^{h}):=\|r^{h}_{\gamma}(q^{h})-r_{\alpha}^{o}\|_{2}^{2}-h_{\min}^{2}$ for some safety distance $h_{\min}>0$. In addition, $\|.\|_{2}$ denotes the Euclidean norm. The safety constraints $h_{\beta,\alpha}^{d}(q^{d})\geq 0$ and $h_{\gamma,\alpha}^{h}(q^{h})\geq 0$ are relative degree two. Our objective is to modify the torques for the dog robot $u^{d}$ as well as the leash force $F$ to render the safety set $\mathcal{C}$ forward invariant under the flow of the closed-loop complex model. We remark that we are not allowed to change the human controller $u^{h}=\Gamma^{h}(x^{h},F)$ as the person can be visually impaired and cannot react properly. For this purpose, we make use of the concept of exponential CBFs (ECBFs) [31]. In particular, we define the ECBFs as follows

[TABLE]

for all $(\alpha,\beta,\gamma)\in\mathcal{I}^{o}\times\mathcal{I}^{d}\times\mathcal{I}^{h}=:\mathcal{I}$, where $\lambda>0$ is an adjustable parameter. The exponential CBF condition further implies that

[TABLE]

for all $(\alpha,\beta,\gamma)\in\mathcal{I}$ and some adjustable scalar $\omega>0$. Substituting (14) and (15) into (16) and (17) results in

[TABLE]

for every $(\alpha,\beta,\gamma)\in\mathcal{I}$. From (5), we remark that (18) and (19) can be expressed as affine inequalities in terms of the the robot torques and leash force $(u^{d},F)$. This can be expressed as follows

[TABLE]

for all $(\alpha,\beta,\gamma)\in\mathcal{I}$. Next, we set up the following real-time QP to ensure safety-critical constraints while being close to the baseline controllers

[TABLE]

where $u_{\min}$, $u_{\max}$, $F_{\min}$, and $F_{\max}$ denote the lower and upper bounds for the torques and forces. We remark that according to the construction procedure of the baseline controller in (10) and (12), $b^{d}_{v}(x^{d},F)$ and $\Gamma^{d}_{v}(x^{d},F)$ are affine in terms of the leash force $F$ for every $v\in\mathcal{V}^{d}$. Hence, the cost function in (22) is indeed quadratic in terms of $(u^{d},F)$. The output of the QP framework are eventually employed as the control inputs for the robotic dog and as well as the leash.

Remark 5

In the QP formulation (22), one would need to measure the human state variables $x^{h}$ to check for the inequality constraints (21). However, we do not modify the torques for the human model. This assumption is not restrictive as one can measure the human state variable via 1) a set of wearable inertial measurement units (IMUs) and 2) asymptotic observers. In particular, our previous work [32] has developed a systematic approach for asymptotic estimation of the state variables for human models via hybrid observers and IMUs. For the purpose of this paper, we hence assume that $x^{h}$ is available for the QP framework.

VI NUMERICAL SIMULATIONS AND RESULTS

The objective of this section is to numerically validate the theoretical results of the paper. For this purpose, we consider a complex and full-order hybrid dynamical model that describes the cooperative locomotion of Vision 60 and a human model.

Vision 60 Robot: Vision 60 is an autonomous quadrupedal robot manufactured by Ghost Robotics [1]. It weighs approximately 26 kg. Vision 60 has 18 DOFs of which 12 leg DOFs are actuated. More specifically, 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. In addition, 6 DOFs are associated with the translational and rotational motions of the torso.

Human Model: The human model consists of a rigid tree structure with a torso link, including hands and head, and two identical legs terminating at point feet (see [28]). Each leg of the robot includes 3 actuated joints: a 2 DOF hip (ball) joint with roll and pitch motions and a 1 DOF knee joint in the sagittal plane. The model has 12 DOFs: 6 DOF for the translational and rotational motions of the torso and 6 DOF for the internal shape variables. The kinematic and dynamic parameter values for the links are taken according to those reported in [33] from a human cadaver study.

Path Planning: We consider an unleashed trotting gait $\mathcal{O}_{\textrm{ul}}^{d}$ for the dog robot at the speed of $1.2$ (m/s). To generate the gait, we make use of FROST (Fast Robot Optimization and Simulation Toolkit) — an open-source toolkit for path planning of dynamic legged locomotion [26, 9]. FROST makes use of direct collocation based trajectory optimization. In particular, it utilizes the Hermite-Simpson collocation approach to translate the path planning problem into a finite-dimensional nonlinear programming (NLP) that can be effectively solved with state-of-the-art NLP tools such as IPOPT. A desired periodic bipedal gait $\mathcal{O}_{\textrm{ul}}^{h}$ is designed for the locomotion of the human model at the speed of $1.1$ (m/s). We intentionally design the human gait to be slower than that of the dog to show that using the proposed control strategy, there can be a common speed leashed gait.

Local Baseline Controllers: Using the semidefinite programming algorithm of [27, 28], we synthesize the virtual constraint controllers of (12) in an offline manner to exponentially stabilize the unleashed gaits for the dog and human models. In particular, the algorithm looks for the optimal outputs to be regulated such that the stability condition in Assumption 1 is satisfied. We further do not consider the full state stability for the human gait. Instead, we consider the stability modulo yaw [27, Section 6.5] to have a model of visually impaired people locomotion. We remark that the dog robot together with the leash structure will have the responsibility to stabilize the yaw motion for itself as well as the human. The leash baseline controller is further designed to keep the human in the safe zone of $[1.25,1.75]$ (m). Figures 3a and 3b depict the robot and human center of mass (COM) trajectories in the $xy$-plane without and with using the leash structure, respectively. We remark that without the leash, the human gait does not have the yaw stability (see Fig. 3a). However, utilizing the leash structure, the robot and human trajectories converge to a complex gait with the same speed while having the yaw stability (i.e., locomotion along the $x$-axis on which the yaw angle is zero) (see Fig. 3b).

Obstacle Avoidance: In order to demonstrate the power of the proposed hierarchical control algorithm, we consider a set of point obstacles $\mathscr{P}_{\alpha}^{o}$ for some $\alpha$ in the discrete set $\mathcal{I}^{o}$. The critical points on the robot and human (i.e., $\mathscr{P}^{d}_{\beta}$ and $\mathscr{P}_{\gamma}^{h}$) are then chosen as the hip points. In the first simulation, we consider $11$ obstacles around the steady-state trajectory of Fig. 3b. Without employing the real-time QP-based modification, the robot and human COM can hit the obstacles. In particular, Fig. 3b illustrates an undershoot around $-0.3$ (m) along the $y$-axis for the human COM that can easily collide with the obstacle located there in Fig. 3c. However, utilizing the hierarchical control algorithm with QP running at 1kHz, the robot and human trajectories are locally modified around the steady-state gait such that the safety critical conditions are satisfied (see Fig. 3c). The time profile for the robot’s yaw and roll motions to accommodate the obstacles is depicted in Figs. 4a and 4b. The performance of the closed-loop hybrid system in the presence of a more-dense set of obstacles is shown in 4c. Figure 5 illustrates the snapshots of the robot and human locomotion around the obstacles. Animations can be found online [34].

VII CONCLUSION

This paper presented a formal method towards 1) addressing complex hybrid dynamical models that describe cooperative locomotion of guide legged robots and humans and 2) systematically designing hierarchical control algorithms that enable stable and safe collaborative locomotion in the presence of discrete obstacles. At the higher level of the proposed control strategy, baseline controllers are assumed for the robotic dog and the leash structure. The robot baseline controller is developed based on HZD approach to asymptotically stabilize a pre-designed unleashed gait for the quadrupedal robot. The leash baseline controller is further developed to keep the human in a safe distance from the dog while following it. The existence and exponential stability of leashed gaits for the complex model are investigated via the Poincaré return map. At the lower level, a real-time QP is solved to modify the baseline controllers for the robot as well as the leash to ensure safety (i.e., obstacles avoidance) via CBFs. The power of the analytical approach is validated through extensive numerical simulations of a complex hybrid model with $60$ state variables and $20$ control inputs that represents the cooperative locomotion of Vision 60 and a human model. We considered an unleashed trotting gait for the dog and a bipedal gait for the human, where the dog gait is assumed to be faster. We further assumed that the human gait does not have yaw stability. It is shown that using the proposed control strategy, the dog and human can reach a common speed for the leashed motion. Moreover, we demonstrated that the robot can stabilize the yaw motion for the human model. The proposed approach can locally guarantee safety around pre-designed unleashed trajectories. The QP framework can significantly reduce the overshoot and undershoot in the human COM trajectories for following the guide robot. For future research, we will improve control algorithms to address sharp turns around corners and obstacles. We will extend the approach to consider dynamic obstacles. We will also investigate robust hierarchical approaches to address cooperative locomotion over uneven terrains.